fix bug when sending 14dof traj to PR2