Add motion_primitives_from_trajectory_controller with trajectory approximation using PTP/LIN primitives#1858
Draft
mathias31415 wants to merge 144 commits intoros-controls:masterfrom
Conversation
…time --> added gitignore, to not push it to github --> remove it after everything is done
… the hardware interface can receive a new motion primitive — replaces the previous, more complex handling via execution_status
…ls according to other controllers in the repo
… rtw template to fit the controller implementation
…tation from rtw template to fit the controller implementation
…ontroller namespace
christophfroehlich
requested changes
Feb 8, 2026
Member
christophfroehlich
left a comment
christophfroehlich
left a comment
There was a problem hiding this comment.
Thanks. But there are some build failures now:
From the docs
/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst:11: WARNING: Duplicate explicit target name: "definition file located in the src folder".
/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst:11: WARNING: Duplicate explicit target name: "the test directory".
/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst:37: WARNING: Include file '/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/test/motion_primitives_from_trajectory_controller_params.yaml' not found or reading it failed
/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/README.md:96: WARNING: duplicate label doc/ros2_controllers/motion_primitives_controllers/userdoc:architecture overview, other instance in /home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst
/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/README.md:105: WARNING: duplicate label doc/ros2_controllers/motion_primitives_controllers/userdoc:demo-video with ur10e, other instance in /home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst
/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/README.md:109: WARNING: duplicate label doc/ros2_controllers/motion_primitives_controllers/userdoc:demo-video with kr3, other instance in /home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst
/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst:16: WARNING: duplicate label doc/ros2_controllers/motion_primitives_controllers/userdoc:motion_primitives_forward_controller, other instance in /home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst
/home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst:29: WARNING: duplicate label doc/ros2_controllers/motion_primitives_controllers/userdoc:motion_primitives_from_trajectory_controller, other instance in /home/runner/work/ros2_controllers/ros2_controllers/doc/ros2_controllers/motion_primitives_controllers/userdoc.rst
from clang
2026-02-08T11:51:18.5191489Z --- stderr: motion_primitives_controllers
2026-02-08T11:51:18.5195110Z �[1m/home/runner/work/ros2_controllers/ros2_controllers/.work/target_ws/src/ros2_controllers/motion_primitives_controllers/src/rdp.cpp:105:63: �[0m�[0;1;31merror: �[0m�[1mimplicit conversion changes signedness: 'std::size_t' (aka 'unsigned long') to 'difference_type' (aka 'long') [-Werror,-Wsign-conversion]�[0m
2026-02-08T11:51:18.5197861Z 105 | rdpRecursive(PointList(points.begin(), points.begin() + index + 1), epsilon, offset);�[0m
2026-02-08T11:51:18.5198861Z | �[0;1;32m ~ ^~~~~
2026-02-08T11:51:18.5201308Z �[0m�[1m/home/runner/work/ros2_controllers/ros2_controllers/.work/target_ws/src/ros2_controllers/motion_primitives_controllers/src/rdp.cpp:107:47: �[0m�[0;1;31merror: �[0m�[1mimplicit conversion changes signedness: 'std::size_t' (aka 'unsigned long') to 'difference_type' (aka 'long') [-Werror,-Wsign-conversion]�[0m
2026-02-08T11:51:18.5203821Z 107 | rdpRecursive(PointList(points.begin() + index, points.end()), epsilon, offset + index);�[0m
2026-02-08T11:51:18.5204985Z | �[0;1;32m ~ ^~~~~
2026-02-08T11:51:18.5207487Z �[0m�[1m/home/runner/work/ros2_controllers/ros2_controllers/.work/target_ws/src/ros2_controllers/motion_primitives_controllers/src/rdp.cpp:191:52: �[0m�[0;1;31merror: �[0m�[1mimplicit conversion changes signedness: 'size_t' (aka 'unsigned long') to 'difference_type' (aka 'long') [-Werror,-Wsign-conversion]�[0m
2026-02-08T11:51:18.5209890Z 191 | quaternions.begin(), quaternions.begin() + max_index + 1),�[0m
2026-02-08T11:51:18.5210709Z | �[0;1;32m ~ ^~~~~~~~~
2026-02-08T11:51:18.5212956Z �[0m�[1m/home/runner/work/ros2_controllers/ros2_controllers/.work/target_ws/src/ros2_controllers/motion_primitives_controllers/src/rdp.cpp:196:31: �[0m�[0;1;31merror: �[0m�[1mimplicit conversion changes signedness: 'size_t' (aka 'unsigned long') to 'difference_type' (aka 'long') [-Werror,-Wsign-conversion]�[0m
2026-02-08T11:51:18.5215067Z 196 | quaternions.begin() + max_index, quaternions.end()),�[0m
2026-02-08T11:51:18.5215694Z | �[0;1;32m
…ith_base_controller
christophfroehlich
pushed a commit
that referenced
this pull request
Feb 22, 2026
Contributor
Author
|
@christophfroehlich sorry for the late response. Unfortunately, I’m a bit short on time for ROS at the moment. I hope this works for now. From what I can see, the only issue left in the docs is the link to the parameter file, as this file isn’t in the final repository yet. |
christophfroehlich
requested changes
Feb 23, 2026
Member
christophfroehlich
left a comment
christophfroehlich
left a comment
There was a problem hiding this comment.
Debian build fails because we'd need to add moveit_msgs there, compatibility builds are broken currently and under investigation.
christophfroehlich
requested changes
Feb 24, 2026
Member
christophfroehlich
left a comment
christophfroehlich
left a comment
There was a problem hiding this comment.
I see that we don't have a single test for the new controller, have you forgotten to push something? You are also referring to a non-existent parameter file.
Btw: I fixed the TOC structure with the last commit:
christophfroehlich
requested changes
Feb 24, 2026
Member
christophfroehlich
left a comment
christophfroehlich
left a comment
There was a problem hiding this comment.
I fixed the docs and converted everything to rst files. (thanks to copilot)
Before I start to integrate moveit_msgs into our debian and windows CI jobs:
I have objections against the use of /compute_fk service, maybe @mathias31415 or @destogl can comment on that.
| pt.joint_positions = point.positions; | ||
| try | ||
| { | ||
| pt.pose = fk_client_->computeFK(joint_names, point.positions, "base", "tool0"); |
Member
There was a problem hiding this comment.
Do we really need this dependency on moveit's move group service?
Why not using the kinematics_interface plugins we have? the robot_description is available in every controller, wouldn't this be more appropriate?
And why are the frames here hardcoded?
Member
There was a problem hiding this comment.
Just confirmed in todays PMC meeting: we should use kinematics_interface FK instead.
christophfroehlich
added
backport-jazzy
8 tasks
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
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 aJointTrajectoryusing eitherPTPorLINmotion 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:
PTPprimitives in joint spaceLINprimitives in Cartesian space