traj warping func implement in SM and Left arm follower