-
Notifications
You must be signed in to change notification settings - Fork 508
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 smoothing improvements #712
Conversation
Codecov Report
@@ Coverage Diff @@
## main #712 +/- ##
==========================================
+ Coverage 54.52% 54.87% +0.36%
==========================================
Files 196 196
Lines 21340 21358 +18
==========================================
+ Hits 11634 11719 +85
+ Misses 9706 9639 -67
Continue to review full report at Codecov.
|
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.
There is a lot going on here so I will take me another review to fully understand what's going on here. If possible, we should try to reduce the nested loops and maybe make use of vectors/matrices for representing the input and outputs for efficiency and readability. The change itself looks reasonable to me, only suggestion would be to use a MAX_DURATION_EXTENSION_FRACTION
instead of allowing exponential growth by the number of attempts.
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/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
Show resolved
Hide resolved
moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
Outdated
Show resolved
Hide resolved
3835bcc
to
f49549d
Compare
0f59a69
to
a5756f6
Compare
a5756f6
to
830b536
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.
Just a few small nits and comments. I don't really understand how this works so my comments are mostly on a few small style things I noticed.
Is this something that could be tested in some small way?
moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h
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
} | ||
|
||
timestep = trajectory.getAverageSegmentDuration(); | ||
ruckig_ptr.reset(new ruckig::Ruckig<0>{ num_dof, timestep }); |
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.
Would you mind using make_unique here too?
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.
Why? I think it's more semantically clear to do a reset()
here since the pointer was initialized to something else before. (Unlike your previous comment, where the pointer hadn't been initialized yet.)
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 the primary reason I generally encourage the make functions over reset and new with the constructor: https://stackoverflow.com/a/41001856
It seems that as of C++17 that reason doesn't matter much.
The second reason is that when you call make functions it only does one dynamic memory allocation (control block + new object). When you call new in a smart pointers constructor it does two memory allocations (your call to new, then inside the constructor did the smart pointer a second one for the control block). When you call reset a similar thing happens because it can't reuse the control block it has because someone else might also hold a reference to it. The short answer is that using reset or the constructor with new is a pessimization (double the amount of calls to malloc and more fragmented memory) vs using the make functions.
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
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.
Just a few small nits and comments. I don't really understand how this works so my comments are mostly on a few small style things I noticed.
Is this something that could be tested in some small way?
6083fdd
to
fb303e6
Compare
18aa636
to
648f741
Compare
If you mean how can YOU test it, there are instructions at the top of the PR. I'll see how hard it would be to write a little unit test. |
7996b08
to
e6036df
Compare
82729e4
to
a20286f
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.
sweet, thank you!
21a5121
to
5a8e61e
Compare
This adds some follow-up improvements to #571. #571 was rather limited -- it only worked well with a large acceleration limit, small velocity limit. This PR fixes that.
If the first pass of a trajectory fails, increase the duration and attempt smoothing again (up to MAX_DURATION_EXTENSION_ATTEMPTS)
Ignore consecutive, identical waypoints.
You can easily test with
ros2 launch moveit_resources_panda_moveit_config demo.launch.py
. Inpanda_moveit_config/demo.launch.py
add the Ruckig plugin to the list of trajectory "request adapters". Ruckig should be first in the list andAddTimeOptimalParameterization
should be the next one. This ensures TOTG runs before Ruckig."request_adapters": """default_planner_request_adapters/AddRuckigTrajectorySmoothing default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
1-2021-09-29_09.45.23.mp4