Added test simulink model for Jacobians. Something's wrong with either the Jacobian or the forward kinematics
Debugging right hand trajectory task