Ruckig smoothing cleanup

[AndyZe]

Description

Fixes #767

This fixes some bugs in the Ruckig smoothing plugin and simplifies it. I tried to comment it better and simplify so it will be self-explanatory.

Testing

You can test this easily with the MoveIt tutorials.

• Add Ruckig to the list of OMPL adapters. In panda_moveit_config/config/ompl_planning.yaml, add AddRuckigTrajectorySmoothing to the top of the list like this:

request_adapters: >-
  default_planner_request_adapters/AddRuckigTrajectorySmoothing

• Then launch the Panda with move_group: ros2 launch moveit_resources_panda_moveit_config demo.launch.py

• You will know the Ruckig plugin loaded if you see this in terminal:

Using planning request adapter 'Add Ruckig trajectory smoothing.

Data plots

Representative data from some very large trajectories (1000+ waypoints). We feed 10 waypoints at a time into this smoothing algorithm. Notice jerk decreased from 4000 rad/s^3 to 10 rad/s^3.

[codecov]

Codecov Report

Merging #1111 (4a510cc) into main (0f3f882) will increase coverage by 2.77%.
The diff coverage is 94.88%.

@@            Coverage Diff             @@
##             main    #1111      +/-   ##
==========================================
+ Coverage   58.54%   61.31%   +2.77%     
==========================================
  Files         203      274      +71     
  Lines       22453    24755    +2302     
==========================================
+ Hits        13143    15176    +2033     
- Misses       9310     9579     +269     

Impacted Files	Coverage Δ
...rajectory_processing/src/ruckig_traj_smoothing.cpp	91.67% <94.88%> (+9.60%)	⬆️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	75.36% <0.00%> (-1.07%)	⬇️
...z_industrial_motion_planner/planning_context_ptp.h	100.00% <0.00%> (ø)
...z_industrial_motion_planner/cartesian_trajectory.h	100.00% <0.00%> (ø)
...lz_industrial_motion_planner/planning_exceptions.h	100.00% <0.00%> (ø)
...z_industrial_motion_planner/trajectory_generator.h	100.00% <0.00%> (ø)
...otion_planner/src/planning_context_loader_circ.cpp	100.00% <0.00%> (ø)
...tion_planner_testutils/src/xml_testdata_loader.cpp	69.07% <0.00%> (ø)
.../pilz_industrial_motion_planner/limits_container.h	100.00% <0.00%> (ø)
...otion_planner_testutils/src/robotconfiguration.cpp	50.00% <0.00%> (ø)
... and 65 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 0f3f882...4a510cc. Read the comment docs.

[AndyZe]

I had an epiphany - I don't actually need to use Ruckig to smooth the waypoints in this application. I can use it as a pass/fail check for whether the trajectory duration is acceptable, and extend the duration if needed. This is a lot simpler and works great for a small number of waypoints.

For a large number of waypoints, this algorithm can be called repeatedly with small chunks of waypoints.

[henningkayser]

Changes look reasonable to me in general. Please consider switching back to west const for consistency, otherwise just minor comments.

[henningkayser]

wouldn't it make sense to only stretch until the current waypoint_idx?

[AndyZe]

I think that's good logic and it should work but it complicates the code for a couple reasons:

• You have to worry about the index zero edge case. (waypoint_idx == 0)
• The tracking of duration_extension_factor gets tricky

Overall I don't think it's worth the complication. Usually MoveIt trajectories are pretty small, like 100 waypoints or less. If you're working with very large trajectories like the ~7,000 waypoints I showed above, you should feed 10 at a time or so into this algorithm.

Your suggestion is even a pessimization w.r.t. run time for most cases. Increasing the timestep for every waypoint one at a time is computationally expensive.

Anyway, I did try this replacement but it often fails. Likely due to duration_extension_factor exceeding the limit but I didn't dig into it too much.

        // Reset the trajectory
        trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */);

        // If failure occurred at index 0, stretch up to index 1.
        // (Stretching index 0 is meaningless)
        // Else, stretch up to (current waypoint)
        size_t stretch_to_idx = (waypoint_idx == 0) ? 1 : waypoint_idx + 1;

        for (size_t time_stretch_idx = 1; time_stretch_idx <= stretch_to_idx; ++time_stretch_idx)
        {
          // Do stretching here...

[AndyZe]

The CI failure is unrelated

[JafarAbdi]

What do you think about having it inherited from TimeParameterization similar to the other paramterization algorithms? not required here we could add it as a first-time issue

[AndyZe]

What do you think about having it inherited from TimeParameterization similar to the other paramterization algorithms? not required here we could add it as a first-time issue

I'm not sure about that. It's not really a time-parameterization algorithm. It just does smoothing.