I spent some time testing some stuff about trajectory replacement, and just found a much simpler way to do that (I don’t know how I didn’t think of that before):
–> Here in this file, comment line 282 to 288 (starting with “if self.current_goal_handle is not None:”)
You can now send a new trajectory on the “/niryo_one/commander/robot_action”, this will send a new planned trajectory to ros_control, which will replace the current traj with the new one (and try to smooth the transition).
I also experienced some jerk when changing trajectory, but maybe not much as what you had. Please let me know if this change in the code makes things better for you.
The jerk comes mainly from the NiryoSteppers, but for that I don’t think we can do more than what we have now.