-
Notifications
You must be signed in to change notification settings - Fork 501
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Ruckig trajectory_processing plugin #571
Conversation
moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
Outdated
Show resolved
Hide resolved
Codecov Report
@@ 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
Continue to review full report at Codecov.
|
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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
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 |
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 |
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? |
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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/planning/planning_request_adapter_plugins/src/add_ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
Ruckig is now released as a ROS package for galactic, foxy, rolling, noetic, and melodic. |
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). |
652c8f4
to
83d8e26
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Assuming this passes CI I'll merge this. Then we should backport it to Foxy, Noetic, and Melodic if possible.
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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@AndyZe really almost there! Please address the minor comments on the CMakeLists, then you get a +1 from me
moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
Outdated
Show resolved
Hide resolved
@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! |
@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. |
591b89a
to
db1c370
Compare
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 |
e1f95ef
to
9c76110
Compare
6206238
to
5adc02b
Compare
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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I looked through your latest changes and this looks good.
Starts to sound a bit like the IPTP works, IIRC? |
Added a missing image and additional images of expected outputs.
This adds a Ruckig plugin that's similar to TOTG with these differences:
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