Ruckig trajectory_processing plugin

[AndyZe]

This adds a Ruckig plugin that's similar to TOTG with these differences:

• Allows nonzero initial/final velocities and accelerations
• Allows jerk limits
• Does not compute timestamps like TOTG does. It's meant to run after TOTG

Occasionally Ruckig fails to solve so I made an issue there. Let's not be too negative about this, though... Realistically it is already better than the IterativeParabolic and IterativeSpline plugins that MoveIt was using previously.

My vision is that Servo can start using this same plugin.

Test with ros2 launch moveit_resources_panda_moveit_config demo.launch.py. It requires this branch of moveit_resources to change one config file.

Post-merge edit: TOTG provides a good first guess at the trajectory but Ruckig might require more time when joint limits are taken into account. It needs that extra time to catch up. For now, this generally only works at low speeds, like (accel scaling = 1.00, vel scaling = 0.05)

ruckig-2021-08-01_11.04.36.mp4

[codecov]

Codecov Report

Merging #571 (5adc02b) into main (61487d5) will decrease coverage by 0.23%.
The diff coverage is 0.00%.

@@            Coverage Diff             @@
##             main     #571      +/-   ##
==========================================
- Coverage   54.38%   54.15%   -0.22%     
==========================================
  Files         191      192       +1     
  Lines       20100    20184      +84     
==========================================
  Hits        10929    10929              
- Misses       9171     9255      +84     

Impacted Files	Coverage Δ
...rajectory_processing/src/ruckig_traj_smoothing.cpp	0.00% <0.00%> (ø)
...include/moveit/robot_trajectory/robot_trajectory.h	85.72% <0.00%> (-5.19%)	⬇️

---

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 61487d5...5adc02b. Read the comment docs.

[AndyZe]

I'd say this is at the point of being useful for some people. It's not perfect, but it is optional. Taking it out of WIP status.

[davetcoleman]

This is an exciting work, but I think is too experimental to be merged in here. I chatted with @tylerjw and he's going to propose a better location for it.

[henningkayser]

This looks really good to me so far. I think you can ignore my concerns about the non-active group variables for now, the other plugins do just the same. I'd still like to verify at some point if we should take care of this. Regarding the ruckig dependency: IMO there is nothing wrong with maintaining plugins like these in a separate repo for a while (like STOMP for example). Honestly, I would even suggest moving all the plugins out of moveit_core/moveit_ros since they are no build dependency for downstream packages.

On servo: Do you have any thoughts how planner adapters could be used for smoothing servo commands? I guess it would require a different form of plugin structure. To me it would not be such a bad idea to use the same (or related) plugin structure for that, possibly by providing an alternative interface. That way we could keep related algorithms in one place.

[AndyZe]

I figured out some extra logic to eliminate the backward motion that sometimes occurs. Looking much better now. I'll now move on to address the rest of the code reviews here.

ruckig-2021-08-01_11.04.36.mp4

[AndyZe]

A joint angle plot while running Ruckig.

[pantor]

Hi, author of Ruckig here. If there is anything I can do to bring this closer to merging, I'll be happy to help!

The recent video with the fix for the backward motion looks great! Regarding the goal pose: Isn't it possible to set the goal pose as the last target pose for Ruckig (when updating the input parameters the final time), so that the goal pose is always reached exactly?

With the recent commits of Ruckig, the numeric stability in particular for high limits (tested with |x| < 1e6) should have improved significantly. If you get any more errors, please file an issue with the input parameters so I can check what's going on exactly. By the way, I've added a ruckig::DynamicDOFs constant to replace the <0> for dynamic DoFs to improve the readability of the code.

[AndyZe]

Hey @pantor, how would you feel about creating a ROS2 release? That will make it a lot easier for us to use your library.

Without a ROS release, we will probably move this plugin to a different repository because we don't want to (essentially) copy/paste your library into moveit.

There are instructions here. I would probably target Rolling Ridley (Ubuntu 20.04). Does that sound right, @tylerjw?

[tylerjw]

If your library is released for both rolling and galactic we can then depend on it in our main branch. If you also release for foxy we can backport this to foxy.

[pantor]

I've now created a ROS2 release for Ruckig (with PRs for rolling and galactic). I'll add foxy later.

[gavanderhoorn]

I've now created a ROS2 release for Ruckig (with PRs for rolling and galactic). I'll add foxy later.

One for Melodic & Noetic would then also be appreciated.

[tylerjw]

Please add some basic unit tests to make sure your use of arrays in index-based for loops doesn't use memory past the end of the vectors under normal operation. You can do this by changing you array access to using at and then writing a test that will exercise the code in a normal way. That way the test will pass assuming it doesn't throw an exception.

[pantor]

Ruckig is now released as a ROS package for galactic, foxy, rolling, noetic, and melodic.

[tylerjw]

I recognize that some of the things I asked for changes to might require changes to the plugin interface. If that is the case please point that out and I'll make a separate PR to make that change (this being targeted towards rolling/galactic we should be able to make interface changes where they make sense).

[tylerjw]

Assuming this passes CI I'll merge this. Then we should backport it to Foxy, Noetic, and Melodic if possible.

[tylerjw]

I just saw your message about the warnings. I'll wait on merging this until you get a chance to clean that up. Thank you.

[henningkayser]

@AndyZe really almost there! Please address the minor comments on the CMakeLists, then you get a +1 from me

[personal-fork-robot]

@AndyZe Thanks very much for your contribution! The above mentioned, "Does not compute timestamps as TOTG does. It's meant to run after TOTG", but how to use it makes me confused. It would be better if a demo could be given:) Thx!

[AndyZe]

@personal-fork-robot it's very easy. You only need to add the plugin to demo.launch.py (or whatever launch file you're using.) See the example here. Ruckig should be the first plugin in the list, which makes it the last one to be applied.

AndyZe/moveit_resources@40dabca#diff-6393b58b4a7a0e7352f796c30f408dac2b85d7cd4b4293aaedc6c479754c8973R71

[AndyZe]

OK, the correct Ruckig Debian has finally been released. I tested it locally and things look fine. If it passes CI, let's get this merged

[AndyZe]

I've been testing this for a couple hours and tweaking the kinematics calculations. I find that it succeeds practically 100% of the time when (accel scaling = 1.00, vel scaling = 0.05). It often fails if (accel scaling = 1.00, vel scaling = 1.00). Which makes sense, because TOTG runs first and it isn't aware of jerk limits. So TOTG is making requests that just aren't possible when jerk limits are taken into account.

I think we can improve this for high speeds (later) by stretching out the timesteps from TOTG and re-running it, but it's not something I want to dive into in this PR.

doing_it-2021-09-15_16.41.59.mp4

[tylerjw]

I looked through your latest changes and this looks good.

[gavanderhoorn]

I think we can improve this for high speeds (later) by stretching out the timesteps from TOTG and re-running it

Starts to sound a bit like the IPTP works, IIRC?