Rewrote GetLinkForceTorque to use only KinBody functions
Removed unnecessary rotation in InitKinBody