0
$\begingroup$

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.

$\endgroup$

1 Answer 1

0
$\begingroup$

If the planned path could reach the same position twice, there is no unique "minimum distance" you could use to get the path parameter, whatever distance-function you will choose.

In this case you have to remember the path parameter for the old path at the time of stopping. In the case of the trajectory (which is path + time parameterization), the time-from-start is what you need.

I'm not familiar with noetic, so I can't tell you how exactly to implement this.

$\endgroup$
3
  • $\begingroup$ 1. I definitely agree with you that if a trajectory is repetitive (mopping floor, washing window, installing screws) then it will fail to find a unique shortest distance. So in such scenarios this approach will fail. 2. I also came up with the exact same idea that time-from-start will provide a good reference for the progress. However this only works in a perfect world. In my experiments with real robots, network overhead and mechanical delays are noisy enough to disturb the true measurement. Especially when the trajectory only takes small time to execute. Thank you for the comment!! $\endgroup$
    – L.Towson
    Commented Sep 15, 2023 at 13:50
  • $\begingroup$ You could use the time-from-start as initial value to search for a local minimum distance. This should work then even with repetitive paths. $\endgroup$ Commented Sep 16, 2023 at 10:05
  • $\begingroup$ That's a good idea. Let me put that into experiment. $\endgroup$
    – L.Towson
    Commented Sep 17, 2023 at 9:39

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.