Now storing qdotTov and vToqdot gradients in kinsol as well.
Gradients for forwardKinV. WIP.
Reshaped normalizeVec gradient to match geval output. Changed RigidBodyManipulator.qdotToV to match.
setSubMatrixGradient and getSubMatrixGradient can now handle case where rows and cols corresponding to submatrix are not in order.
forwardKinV Jacobian gradient now correct for 1 random point, doesn't work for more than 1 yet.
Fixed forwardKinV gradient for multiple points (without rotation output).
Removed minor code duplication in forwardKinV.
Added gradient output to rotmat2quat (taken from the old forwardKin). Added test.
Quat output forwardKin J gradients.
rotmat2rpy gradients.
RPY output forwardKin J gradients. Also decided to change angularvel2rpydotMatrix so that it only outputs the matrix, not its time derivative. Added gradient and second derivative outputs to angularvel2rpydotMatrix and added test.
Jdot_times_v gradients for no rotation and rpy outputs correct if there is only one point.
J gradients correct for any number of points and any output orientation. Jdot_times_v gradients correct for one point and any output orientation.
Jdot_times_v gradients correct for any number of points and any output orientation. Infrequent test failures in compareToNumerical (rpy) though.