I think I should redo this too. So that there is only one bodyKin in RigidBodyManipulator.cpp and using NULL pointers as arguments for J and P when they are not requested.