Add `motion_primitives_forward_controller` for interfacing motion primitive messages with hardware interfaces #1636

mathias31415 · 2025-04-11T12:21:12Z

This PR adds the motion_primitives_forward_controller, a controller for forwarding motion primitive commands to motion-primitive-capable hardware interfaces.
It was developed alongside the motion_primitive_ur_driver and is intended to support future hardware interfaces for additional robot manufacturers.
The corresponding PR for integrating the motion_primitive_ur_driver into the official Universal Robots ROS 2 driver can be found here: UniversalRobots/Universal_Robots_ROS2_Driver#1341.

The controller subscribes to MotionPrimitive.msg from the industrial_robot_motion_interfaces package. The MotionPrimitive.msg has been extended with additional helper types:

STOP_MOTION: Immediately interrupts execution and clears all queued primitives.
MOTION_SEQUENCE_START and MOTION_SEQUENCE_END: Define a sequence of primitives to be executed as a blended motion block.

The controller uses the following interfaces:

Command Interfaces

motion_type: Type of motion primitive (e.g., LINEAR_JOINT, LINEAR_CARTESIAN, CIRCULAR_CARTESIAN, etc.)
q1 – q6: Target joint positions for joint-based motion
pos_x, pos_y, pos_z: Target Cartesian position
pos_qx, pos_qy, pos_qz, pos_qw: Orientation quaternion of the target pose
pos_via_x, pos_via_y, pos_via_z: Intermediate via-point position for circular motion
pos_via_qx, pos_via_qy, pos_via_qz, pos_via_qw: Orientation quaternion of via-point
blend_radius: Blending radius for smooth transitions
velocity: Desired motion velocity
acceleration: Desired motion acceleration
move_time: Optional duration for time-based execution

State Interfaces

execution_status: Indicates the current execution state of the primitive.
ready_for_new_primitive: Boolean flag indicating whether the interface is ready to receive a new motion primitive

I'd appreciate any feedback or suggestions – thanks in advance!

…time --> added gitignore, to not push it to github --> remove it after everything is done

…hias31415/ros2_motion_primitives_controller_pkg

…_controller

… the hardware interface can receive a new motion primitive — replaces the previous, more complex handling via execution_status

…sting)

bmagyar

There is a lot of stuff coming from templates in this PR and you either used some AI tool for coding or you are very good at writing AI-style comments.

Please try removing all commented code or fix them and remove all comments that add nothing to the code itself (which in this code is almost all comments...)

bmagyar · 2025-04-17T09:44:38Z

...mitives_forward_controller/include/motion_primitives_forward_controller/visibility_control.h

+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_EXPORT __attribute__((dllexport))
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_IMPORT __attribute__((dllimport))
+#else
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_EXPORT __declspec(dllexport)
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_IMPORT __declspec(dllimport)
+#endif
+#ifdef MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_BUILDING_DLL
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_PUBLIC MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_EXPORT
+#else
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_PUBLIC MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_IMPORT
+#endif
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_PUBLIC_TYPE MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_PUBLIC
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_LOCAL
+#else
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_EXPORT __attribute__((visibility("default")))
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_IMPORT
+#if __GNUC__ >= 4
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_PUBLIC __attribute__((visibility("default")))
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
+#else
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_PUBLIC
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_LOCAL
+#endif
+#define MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_PUBLIC_TYPE
+#endif


we don't do this sort of stuff anymore, please take a look at how other controllers do it

bmagyar · 2025-04-17T09:44:45Z

motion_primitives_forward_controller/package.xml

+  <description>Package to control robots using motion primitives like PTP, LIN and CIRC</description>
+  <maintainer email="[email protected]">todo</maintainer>
+  <author>Mathias Fuhrer</author>
+  <license>BSD-3-Clause</license>


It would be ideal to update this license to Apache 2 if possible.

bmagyar · 2025-04-17T09:44:55Z

motion_primitives_forward_controller/motion_primitives_forward_controller.rolling.repos

@@ -0,0 +1,6 @@
+repositories:


these repos files are not needed

bmagyar · 2025-04-17T09:45:35Z

motion_primitives_forward_controller/package.xml

+  <version>0.0.0</version>
+  <description>Package to control robots using motion primitives like PTP, LIN and CIRC</description>
+  <maintainer email="[email protected]">todo</maintainer>
+  <author>Mathias Fuhrer</author>


email for you could be defined, the release version could be updated to match the rest of the repo and please copy the maintainer fields from other controllers in t his repo

bmagyar · 2025-04-17T09:45:46Z

motion_primitives_forward_controller/package.xml

+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>generate_parameter_library</build_depend>
+
+  <depend>control_msgs</depend>
+
+  <depend>controller_interface</depend>
+
+  <depend>hardware_interface</depend>
+
+  <depend>pluginlib</depend>
+
+  <depend>industrial_robot_motion_interfaces</depend>
+
+  <depend>rclcpp</depend>
+
+  <depend>rclcpp_lifecycle</depend>
+
+  <depend>realtime_tools</depend>
+
+  <depend>std_srvs</depend>
+


that's a lot of whitespace that could be removed ;)

bmagyar · 2025-04-17T09:49:26Z

motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp

+// int main(int argc, char ** argv)
+// {
+//   ::testing::InitGoogleTest(&argc, argv);
+//   rclcpp::init(argc, argv);
+//   int result = RUN_ALL_TESTS();
+//   rclcpp::shutdown();
+//   return result;
+// }


2 problems:

it's all commented out :D

it's always better to use gmock, not gtest. You get much better prints.

bmagyar · 2025-04-17T09:50:13Z

.gitignore

@@ -0,0 +1 @@
+**/COLCON_IGNORE


I'm afraid it won't be this PR pioneering on the .gitignore file ;) please keep this to your local workspace

bmagyar · 2025-04-17T09:50:25Z

motion_primitives_forward_controller/CMakeLists.txt

+# install(
+#   DIRECTORY include/
+#   DESTINATION include/${PROJECT_NAME}
+# )


bmagyar · 2025-04-17T09:51:01Z

motion_primitives_forward_controller/README.md

+
+
+1. **Command Reception**  
+   Python scripts can publish motion primitives to the `~/reference` topic. These messages are received by the `reference_callback()` method in the controller.


Do they have to be Python scripts? CLI, C++ or Rust are disqualified? ;)

bmagyar · 2025-04-17T09:51:19Z

...roller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp

+  // callback for topic interface
+  MOTION_PRIMITIVES_CONTTROLLER_PKG__VISIBILITY_LOCAL
+  void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg); // callback for reference message
+  // std::atomic<bool> new_msg_available_ = false; // flag to indicate if new message is available


…ls according to other controllers in the repo

mathias31415 added 12 commits April 8, 2025 14:35

added COLCON_IGNORE to every controller folder to not build it every …

98cd2e3

…time --> added gitignore, to not push it to github --> remove it after everything is done

added motion_primitive_forward_controller from https://github.com/mat…

783d129

…hias31415/ros2_motion_primitives_controller_pkg

renamed motion_primitives_controller_pkg to motion_primitives_forward…

6ed1181

…_controller

added functionallity to execute multiple primitives as a motion sequence

9074829

Added new state interface ready_for_new_primitive to indicate whether…

2cf7b09

… the hardware interface can receive a new motion primitive — replaces the previous, more complex handling via execution_status

removed case 33 block with hardcoded motion sequence (was only for te…

c09d1fd

…sting)

removed mail

f4abcbd

added command_mutex_

79e47be

edited readme

316ad54

changed image in readme to white background

28adf54

edited Related packages/ repos in readme

e28554d

added/ modified copyright headers

c451fb4

github-actions bot requested review from ARK3r, bmagyar, christophfroehlich, fmauch, mcbed, peterdavidfagan and VX792 April 11, 2025 12:21

mathias31415 mentioned this pull request Apr 11, 2025

Hardware interface for sending motion primitives to the robot via the Instruction Executor instead of trajectories UniversalRobots/Universal_Robots_ROS2_Driver#1341

Open

bmagyar requested changes Apr 17, 2025

View reviewed changes

mathias31415 added 7 commits April 24, 2025 10:33

removed LICENSE file

188f8b5

removed repos files

93cc787

changed license from BSD-3-Clause to Apache License 2.0

1122a31

added email to package.xml

af63eab

changed version to fit the rest of the repo, added maintainers and ur…

3f3881f

…ls according to other controllers in the repo

removed .gitignore (was not supposed to be pushed to the repo)

ce8b937

removed visibility control

92ee810

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add `motion_primitives_forward_controller` for interfacing motion primitive messages with hardware interfaces #1636

Add `motion_primitives_forward_controller` for interfacing motion primitive messages with hardware interfaces #1636

mathias31415 commented Apr 11, 2025

bmagyar left a comment

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025

bmagyar Apr 17, 2025



		1. Command Reception
		Python scripts can publish motion primitives to the `~/reference` topic. These messages are received by the `reference_callback()` method in the controller.

		@@ -0,0 +1 @@
		**/COLCON_IGNORE

Add motion_primitives_forward_controller for interfacing motion primitive messages with hardware interfaces #1636

Are you sure you want to change the base?

Add motion_primitives_forward_controller for interfacing motion primitive messages with hardware interfaces #1636

Conversation

mathias31415 commented Apr 11, 2025

bmagyar left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Add `motion_primitives_forward_controller` for interfacing motion primitive messages with hardware interfaces #1636

Add `motion_primitives_forward_controller` for interfacing motion primitive messages with hardware interfaces #1636