Add motion_primitives_from_trajectory_controller with trajectory approximation using PTP/LIN primitives

[mathias31415]

This PR adds the motion_primitives_from_trajectory_controller. To make reviewing easier, another PR #1857 was created first to move shared functionality into a base controller.

This PR replaces PR #1837.

This PR is marked as a draft because PR #1636 and PR #1857 must be merged first.

This PR complements motion_primitives_forward_controller PR #1636 .
Instead of sending hardcoded motion primitives to the motion_primitives_forward_controller, this controller (motion_primitives_from_trajectory_controller) approximates a JointTrajectory using either PTP or LIN motion primitives.

This allows, for example, collision-free paths planned with MoveIt to be executed as motion primitives. The approximation is performed using the Ramer-Douglas-Peucker (RDP) algorithm:

• For PTP primitives in joint space
• For LIN primitives in Cartesian space

[mergify]

This pull request is in conflict. Could you fix it @mathias31415?

[codecov]

Codecov Report

❌ Patch coverage is 10.05747% with 626 lines in your changes missing coverage. Please review.
✅ Project coverage is 81.52%. Comparing base (5ed4fa5) to head (751cbcb).

Files with missing lines	Patch %	Lines
...c/motion_primitives_from_trajectory_controller.cpp	0.00%	309 Missing ⚠️
...ves_controllers/src/approx_primitives_with_rdp.cpp	0.00%	159 Missing ⚠️
motion_primitives_controllers/src/rdp.cpp	0.00%	92 Missing ⚠️
...trollers/src/motion_primitives_base_controller.cpp	65.59%	27 Missing and 5 partials ⚠️
motion_primitives_controllers/src/fk_client.cpp	0.00%	20 Missing ⚠️
...s/motion_primitives_from_trajectory_controller.hpp	0.00%	14 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1858      +/-   ##
==========================================
- Coverage   85.12%   81.52%   -3.61%     
==========================================
  Files         138      145       +7     
  Lines       13547    14154     +607     
  Branches     1188     1273      +85     
==========================================
+ Hits        11532    11539       +7     
- Misses       1620     2220     +600     
  Partials      395      395              

Flag	Coverage Δ
unittests	81.52% <10.05%> (-3.61%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
..._controllers/motion_primitives_base_controller.hpp	100.00% <ø> (ø)
...ntrollers/motion_primitives_forward_controller.hpp	100.00% <100.00%> (ø)
...llers/src/motion_primitives_forward_controller.cpp	32.25% <100.00%> (ø)
...test_load_motion_primitives_forward_controller.cpp	100.00% <100.00%> (ø)
...test/test_motion_primitives_forward_controller.cpp	97.36% <100.00%> (ø)
...test/test_motion_primitives_forward_controller.hpp	89.06% <100.00%> (ø)
...s/motion_primitives_from_trajectory_controller.hpp	0.00% <0.00%> (ø)
motion_primitives_controllers/src/fk_client.cpp	0.00% <0.00%> (ø)
...trollers/src/motion_primitives_base_controller.cpp	65.59% <65.59%> (ø)
motion_primitives_controllers/src/rdp.cpp	0.00% <0.00%> (ø)
... and 2 more

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.