Added dAdH_times_X + test
Discovered matGradMultMat. Now using it in dAdH_times_X.
Rename (in two steps because case was wrong).
Rename.
Added dSdq.
* switched to computing dHdq based on motion subspace in body frame
* dHdq computation in doKinematics now passing tests
* fixed mistake in motionSubspace gradient for rpy parameterization. Added test for motionSubspace
* obvious optimization in dHdq
* got dJdq tests in testRigidBodyManipulatorGradients to pass.
Got geometric Jacobian gradient tests to pass.