Small fix in doKinematics.
Added assertions to getSubMatrixGradient, setSubMatrixGradient.
forwardKinV code formatting.
geometricJacobianDotV gradients + test.
* implemented dVq, dVqInv in RigidBodyManipulator
* improved get/set SubMatrixGradient methods
* gradients for quat2rotmat, angularvel2quatdotMatrix, quatdot2angularvelMatrix.