Consider adding blending to Cartesian planning

[sea-bass]

Instead of having to stop exactly at the intermediate points, there are common approaches to blending Cartesian paths.

This is one resource I like: https://www.diag.uniroma1.it/~deluca/rob1_en/14_TrajectoryPlanningCartesian.pdf

Whether just blending on position is enough, or we also need to find ways to do the same with rotation e.g., here remains TBD.

[muhidabid]

Hi @sea-bass,

I learned the math in slides 6-13 from the resource you have shared above.

I also understood the implementation of examples/cartesian_path.py and src/pyroboplan/planning/cartesian_planner.py.

I have a few questions before attempting to implement blending:

1. When ‎using trapezoidal scaling, how to evaluate v1 and v2 which are constant speeds on the two linear paths to be concatenated?
2. Three approaches are mentioned for blending in the slides above: assign transition time, assign distance from via point, and impose acceleration. Can I use any one of these techniques for the implementation?

[sea-bass]

I think there should be a maximum velocity magnitude v assigned throughout. So at the start of the trajectory, the robot accelerates from 0 to v, at the end it decelerates, and at intermediate waypoints you want v1 = v2 = v, magnitude-wise, but their directions may be different.

As for how to parameterize, I think the best bet is to set a distance from the via point and then let the timing be dictated by max accelerations. This can be referred to as a "blending radius".

But before all this... @eholum you said you were potentially looking at this same task?

[eholum]

Not yet! I had started writing a Cartesian interpolator but I won't get back to it for at least 2 weeks. Happy to help review anything, though, @muhidabid.

[muhidabid]

I thought nobody was looking into this issue. If you have started working on it @eholum, please continue!

I was planning on working on it at a slow speed anyway since I have my university enrollment in a few days and I am preparing to travel.

I will look into other issues no problem