progress in c++ code for doKinematics, geometricJacobian.