add use_new_kinsol.
new doKinematics if use_new_kinsol.
new geometricJacobian
c++ new doKinematics
progress in c++ code for doKinematics, geometricJacobian.
testGeometricJacobian now passes when RigidBodyManipulator.use_new_kinsol is either true or false
Merge pull request #612 from tkoolen/newKinsol

New doKinematics, geometricJacobian