Manip_to_joint_pose quasi-final version