Fix the motion to traj functions