Now computing Vq (mapping from qdot to v) in doKinematics. The implementation is currently kind of dumb; ideally this would be a sparse matrix. Updated forwardKin so that it now maps qdot to xdot instead of v to xdot.