Blending Planned Trajectories

[peterdavidfagan]

Is there a generalized pattern using the C++ API or a package for blending trajectories in MoveIt2? I have seen the following tutorial which relates to blending trajectories with the pilz industrial motion planner but I believe this is just for ROS and not ROS 2.

I am considering writing my own code to blend planned robot trajectories but I wished to understand if the MoveIt community had any advice on this front as I don't want to reinvent the wheel. I hope to support this functionality within the Python API.

For free space motions I have started to use moveit_servo with pose targets and it works well.

[gavanderhoorn]

Not an answer, but your post reminded me of galou/blender_ws.

[peterdavidfagan]

Thanks for linking this @gavanderhoorn, this serves as good inspiration for methods that could be used in the Python API for blending.

[AndyZe]

You should poke around in this folder: https://github.com/ros-planning/moveit2/tree/main/moveit_core/trajectory_processing/include/moveit/trajectory_processing

They're plugins so the API is standardized I believe.

A lot of the time, what we'll do to blend trajectories is:

• Take Traj A, take Traj B. Concatenate the waypoints to get Traj C.

• Run time_optimal_trajectory_generation on Traj C.

I think this is only C++, though.

[peterdavidfagan]

Thanks @AndyZe, I was considering the above but great to get confirmation on this point. I'll look to include bindings for this in the Python library.

[gavanderhoorn]

But this is only 'blending' in the sense of concatenating two (or N) vectors of waypoints and then overlaying a velocity profile over the result.

It doesn't actually smoothly blend (ie: position-wise) trajectories together.

[AndyZe]

Well, you could follow up by running it through ruckig_traj_smoothing which will eliminate discontinuities incl. position.

[peterdavidfagan]

Related fuctionalities ported by @JensVanhooydonck 🚀: #2411