C++ version of traj_moving, added gripper function