Ruckig trajectory smoothing improvements

[AndyZe]

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. In panda_moveit_config/demo.launch.py add the Ruckig plugin to the list of trajectory "request adapters". Ruckig should be first in the list and AddTimeOptimalParameterization 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

[codecov]

Codecov Report

Merging #712 (0e68a7f) into main (7e3fdcf) will increase coverage by 0.36%.
The diff coverage is 80.00%.

@@            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     

Impacted Files	Coverage Δ
...rajectory_processing/src/ruckig_traj_smoothing.cpp	84.70% <80.00%> (+84.70%)	⬆️
...include/moveit/robot_trajectory/robot_trajectory.h	86.05% <0.00%> (-1.45%)	⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	74.82% <0.00%> (-1.11%)	⬇️
...eit_core/robot_trajectory/src/robot_trajectory.cpp	24.68% <0.00%> (+0.99%)	⬆️

---

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 7e3fdcf...0e68a7f. Read the comment docs.

[henningkayser]

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.

[tylerjw]

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?

[tylerjw]

Would you mind using make_unique here too?

[AndyZe]

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.)

[tylerjw]

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.

[tylerjw]

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?

[AndyZe]

Is this something that could be tested in some small way?

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.

[tylerjw]

sweet, thank you!