Allow setting minimum timestamp delta between trajectory points in Iterative Spline Parameterization

[schornakj]

I ran into a situation where I needed to enforce that the differences between timestamps for consecutive trajectory points were all greater than some minimum threshold. Briefly, this is because some robot controllers can't execute trajectories with arbitrarily-fine time resolution and need to have (for example) at least a 1ms difference between each trajectory point.

One way to resolve this while ensuring that the trajectory is still smooth is to enforce this time delta threshold during the time parameterization stage. This lets us stretch out the trajectory between two waypoints if needed while ensuring that the robot's path for this trajectory segment follows the same positions but just at a slower speed.

I'm using ISP specifically so I only implemented this change there. I did add this parameter to the time parameterizer base class to allow implementing it elsewhere as well. Let me know if there's a better way to approach this, though.

I have to test this more thoroughly on my system and then update the branch to the latest state of main, so I'll keep this PR as a draft until I do both those things.

[rjoomen]

Looking at MoveIt, I noticed that the TOTG algorithm nowadays supports spacing waypoints equally in time. As the TOTG algorithm of Tesseract is forked from MoveIt, I think the best solution would be to bring Tesseract TOTG up to date with the MoveIt(1)/MoveIt2 version. This was on my wishlist anyway, as this would bring in some improvements/fixes as well (see here and here), so I'm willing to help.

Also, TOTG is proven to be superior to ISP, hence MoveIt2 dropped ISP, another reason for switching.

[gavanderhoorn]

@schornakj: is this related to Yaskawa-Global/motoros2#255?

[schornakj]

@schornakj: is this related to Yaskawa-Global/motoros2#255?

@gavanderhoorn Yes, this PR is where I ended up after digging into my motion planning stack to diagnose the issue I described in my comment in motoros2. I'll keep discussion about that primarily here so I don't just totally hijack the other issue

Looking at MoveIt, I noticed that the TOTG algorithm nowadays supports spacing waypoints equally in time. As the TOTG algorithm of Tesseract is forked from MoveIt, I think the best solution would be to bring Tesseract TOTG up to date with the MoveIt(1)/MoveIt2 version. This was on my wishlist anyway, as this would bring in some improvements/fixes as well (see here and here), so I'm willing to help.

Also, TOTG is proven to be superior to ISP, hence MoveIt2 dropped ISP, another reason for switching.

@rjoomen I agree that ultimately it's a good idea to move over to TOTG. The MoveIt folks decided that it's a better use of maintainer effort to just have one time parameterization approach that works well, so they removed the alternatives. It's probably a good idea to do the same thing in Tesseract for the same reasons.

Aside from that, my main concern would be that enforcing equal time spacing of trajectory points is different from enforcing a minimum time spacing between points. To ensure that TOTG never outputs a trajectory with points spaced by less than 1ms I'd need to calculate the equal sampling parameters for each trajectory given the trajectory's total duration. I'd need to dig into MoveIt's TOTG implementation in more detail to know for sure but my suspicion would be that these are two related but unique problems which need different solutions.

@Levi-Armstrong do you think this is a useful setting to expose within the time parameterization profiles? As long as there's a way to make it internally default to epsilon it's essentially an optional setting that wouldn't require changes to user code.

[Levi-Armstrong]

@marip8 What are your thoughts on this?

[schornakj]

@Levi-Armstrong @marip8 alrighty, this is ready for review.

I've been running this branch on my robot (a Yaskawa GP180-120 with a YRC1000 controller) and it resolves the issue I described in my comment in Yaskawa-Global/motoros2#255. I added some tests to show that the new parameter does have an effect on the trajectory timestamps too.

MotoROS2 cannot execute trajectories with time deltas less than 1ms, so a change like this one is required to prevent the time parameterizer from generating a trajectory that the robot cannot execute.