I'm trying to implement a function to pause the execution of a trajectory, and then re-plan a trajectory following the remaining path. The framework I'm using is moveit noetic.
moveit_msgs::RobotTrajectory contains waypoints that are supposedly "intermediate joint positions" with velocity and acceleration. If I stop the execution of a trajectory at an arbitrary moment and take the current joint positions after the pause, it won't match any of the waypoints in the trajectory.
This is expected, since waypoints have its resolution and it won't cover all possible joint positions in between. My question is, how to find the closest waypoint, given the current joint position? As I'm trying to replan a new trajectory with the "remainder" of waypoints, it's critical to find the right (index) of the waypoint to "resume" from.
I find the smallest Cartesian distance between the current joint position and all waypoints from the original trajectory, and the waypoint who has the shortest distance from current position is the chosen waypoint to start replanning from. This approach kinda works, but sometimes it's not accurate enough, and the replanned trajectory will have a drastic move between the current position to the recovered waypoint.
In general, how to find the right waypoint to resume from? How to replan a smooth trajectory that follows the same path from the remainder of the original trajectory? Any advice or suggestion is greatly appreciated.