Ruckig-smoothed trajectories could be faster if waypoint durations were extended individually

[AndyZe]

The Ruckig trajectory smoothing plugin extends the duration of all waypoints if smoothing isn't successful. It's an iterative process until success is achieved. Attempt smoothing --> extend the duration --> attempt smoothing again.

The code is here:

https://github.com/ros-planning/moveit2/blob/cc635471aadfb9446398ece319ae31c6b72bec86/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp#L357

        for (size_t time_stretch_idx = 1; time_stretch_idx < num_waypoints; ++time_stretch_idx)
        {

The resulting trajectory could be faster if only the failing waypoint durations were extended. There's no reason to extend all of them at every iteration.

[ibrahiminfinite]

Hi @AndyZe

I would like to take this up, if not already done.

We could do this by initializing the waypoint counter outside the for loop here.
And then, instead of extending all waypoint durations , we can set time_stretch_idx to be waypoint_idx+1

This would mean we only update for the current segment that failed, and can continue with original durations for the rest.
Does the changes in the PR come close to what you had in mind ?

Thanks.