Add motion_primitives_forward_controller for interfacing motion primitive messages with hardware interfaces

.pre-commit-config.yaml

@@ -113,7 +113,11 @@ repos:
     rev: v2.0.0
     hooks:
       - id: doc8
-        args: ['--max-line-length=100', '--ignore=D001']
+        args: [
+          '--max-line-length=100',
+          '--ignore=D001',
+          '--ignore-path=motion_primitives_forward_controller/userdoc.rst' # D000 fails with myst_parser
+          ]
         exclude: CHANGELOG\.rst$
 
   - repo: https://github.com/pre-commit/pygrep-hooks

doc/controllers_index.rst

@@ -58,6 +58,7 @@ The controllers are using `common hardware interface definitions`_, and may use
    Position Controllers <../position_controllers/doc/userdoc.rst>
    Velocity Controllers <../velocity_controllers/doc/userdoc.rst>
    Gpio Command Controller <../gpio_controllers/doc/userdoc.rst>
+   Motion Primitive Controller <../motion_primitives_forward_controller/userdoc.rst>
 
 
 Broadcasters

doc/release_notes.rst

@@ -31,3 +31,7 @@ pid_controller
   * The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy.
   * A new ``error_deadband`` parameter stops integration when the error is within a specified range.
 * PID state publisher can be turned off or on by using  ``activate_state_publisher`` parameter. (`#1823 <https://github.com/ros-controls/ros2_controllers/pull/1823>`_).
+
+motion_primitives_forward_controller
+*******************************************
+* 🚀 The motion_primitives_forward_controller was added 🎉 (`#1636 <https://github.com/ros-controls/ros2_controllers/pull/1636>`_).

motion_primitives_forward_controller/CMakeLists.txt

@@ -0,0 +1,110 @@
+cmake_minimum_required(VERSION 3.10)
+project(motion_primitives_forward_controller)
+
+find_package(ros2_control_cmake REQUIRED)
+set_compiler_options()
+export_windows_symbols()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
+  controller_interface
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  realtime_tools
+  std_srvs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(motion_primitives_forward_controller_parameters
+  src/motion_primitives_forward_controller_parameter.yaml
+)
+add_library(
+  motion_primitives_forward_controller
+  SHARED
+  src/motion_primitives_forward_controller.cpp
+)
+target_include_directories(motion_primitives_forward_controller PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+)
+target_link_libraries(motion_primitives_forward_controller PUBLIC
+  motion_primitives_forward_controller_parameters
+  controller_interface::controller_interface
+  hardware_interface::hardware_interface
+  pluginlib::pluginlib
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+  realtime_tools::realtime_tools
+  ${control_msgs_TARGETS}
+  ${std_srvs_TARGETS}
+)
+
+target_compile_definitions(motion_primitives_forward_controller PRIVATE "MOTION_PRIMITIVES_FORWARD_CONTROLLER_BUILDING_DLL")
+
+pluginlib_export_plugin_description_file(
+  controller_interface motion_primitives_forward_controller.xml)
+
+install(
+  TARGETS
+    motion_primitives_forward_controller
+    motion_primitives_forward_controller_parameters
+  EXPORT motion_primitives_forward_controller_targets
+  RUNTIME DESTINATION bin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+)
+
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+ament_export_targets(motion_primitives_forward_controller_targets HAS_LIBRARY_TARGET)
+
+ament_export_include_directories(
+  include
+)
+ament_export_dependencies(
+  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+)
+ament_export_libraries(
+  motion_primitives_forward_controller
+)
+
+ament_package()
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gmock REQUIRED)
+  find_package(controller_manager REQUIRED)
+  find_package(hardware_interface REQUIRED)
+  find_package(ros2_control_test_assets REQUIRED)
+
+  ament_add_gmock(
+    test_load_motion_primitives_forward_controller
+    test/test_load_motion_primitives_forward_controller.cpp
+  )
+  target_link_libraries(test_load_motion_primitives_forward_controller
+    motion_primitives_forward_controller
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  add_rostest_with_parameters_gmock(
+    test_motion_primitives_forward_controller
+    test/test_motion_primitives_forward_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/motion_primitives_forward_controller_params.yaml
+  )
+  target_link_libraries(test_motion_primitives_forward_controller
+    motion_primitives_forward_controller
+    controller_interface::controller_interface
+    hardware_interface::hardware_interface
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+endif()

motion_primitives_forward_controller/README.md

@@ -0,0 +1,68 @@
+motion_primitive_controllers
+==========================================
+
+Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC)
+
+# Description
+This project provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotionPrimitiveSequence.action` action from [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action). The controller receives the primitives via the action interface and forwards them through command interfaces to the robot-specific hardware interface. Currently, hardware interfaces for [Universal Robots](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and [KUKA Robots](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver) are implemented.
+
+- Supported motion primitives:
+  - `LINEAR_JOINT`
+  - `LINEAR_CARTESIAN`
+  - `CIRCULAR_CARTESIAN`
+
+If multiple motion primitives are passed to the controller via the action, the controller forwards them to the hardware interface as a sequence. To do this, it first sends `MOTION_SEQUENCE_START`, followed by each individual primitive, and finally `MOTION_SEQUENCE_END`. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives.
+
+The action interface also allows stopping the current execution of motion primitives. When a stop request is received, the controller sends `STOP_MOTION` to the hardware interface, which then halts the robot's movement. Once the controller receives confirmation that the robot has stopped, it sends `RESET_STOP` to the hardware interface. After that, new commands can be sent.
+
+![Action Image](doc/Moprim_Controller_ExecuteMotion_Action.drawio.png)
+
+This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/examples/send_dummy_motion_primitives_ur10e.py) in the `Universal_Robots_ROS2_Driver` package.
+
+## Command and State Interfaces
+To transmit the motion primitives, the following command and state interfaces are required. All interfaces use the naming scheme `tf_prefix_ + "motion_primitive/<interface name>"` where the `tf_prefix` is provided to the controller as a parameter.
+
+### Command Interfaces
+These interfaces are used to send motion primitive data to the hardware interface:
+- `motion_type`: Type of motion primitive (LINEAR_JOINT, LINEAR_CARTESIAN, CIRCULAR_CARTESIAN)
+- `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 (For LINEAR_JOINT and LINEAR_CARTESIAN. If move_time > 0, velocity and acceleration are ignored)
+
+### State Interfaces
+These interfaces are used to communicate the internal status of the hardware interface back to the `motion_primitives_forward_controller`.
+- `execution_status`: Indicates the current execution state of the primitive. Possible values are:
+  - `IDLE`: No motion in progress
+  - `EXECUTING`: Currently executing a primitive
+  - `SUCCESS`: Last command finished successfully
+  - `ERROR`: An error occurred during execution
+  - `STOPPING`: The hardware interface has received the `STOP_MOTION` command, but the robot has not yet come to a stop.
+  - `STOPPED`: The robot was stopped using the `STOP_MOTION` command and must be reset with the `RESET_STOP` command before executing new commands.
+- `ready_for_new_primitive`: Boolean flag indicating whether the interface is ready to receive a new motion primitive
+
+
+ ## Parameters
+ This controller uses the [`generate_parameter_library`](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. The parameter [definition file located in the src folder](https://github.com/ros-controls/ros2_controllers/blob/master/motion_primitives_forward_controller/src/motion_primitives_forward_controller_parameter.yaml) contains descriptions for all the parameters used by the controller.
+ An example parameter file for this controller can be found in [the test directory](https://github.com/ros-controls/ros2_controllers/blob/master/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml).
+
+
+# Architecture Overview
+Architecture for a UR robot with [`Universal_Robots_ROS2_Driver` in motion primitives mode](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver).
+
+![UR Robot Architecture](doc/ros2_control_motion_primitives_ur.drawio.png)
+
+Architecture for a KUKA robot with [`kuka_eki_motion_primitives_hw_interface`](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver/kuka_eki_motion_primitives_hw_interface).
+
+![KUKA Robot Architecture](doc/ros2_control_motion_primitives_kuka.drawio.png)
+
+# Demo-Video with UR10e
+[![UR demo video](doc/moprim_forward_controller_ur_demo_thumbnail.png)](https://youtu.be/SKz6LFvJmhQ)
+
+# Demo-Video with KR3
+[![KUKA demo video](doc/moprim_forward_controller_kuka_demo_thumbnail.png)](https://youtu.be/_BWCO36j9bg)

motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio

@@ -0,0 +1,99 @@
+<mxfile host="embed.diagrams.net" agent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:139.0) Gecko/20100101 Firefox/139.0" version="27.1.4">
+  <diagram name="Page-1" id="9u_5SAsCUr1ULjLXrb9y">
+    <mxGraphModel grid="1" page="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0" adaptiveColors="none">
+      <root>
+        <mxCell id="0" />
+        <mxCell id="1" parent="0" />
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-1" value="Python Script" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" vertex="1" parent="1">
+          <mxGeometry x="40" y="280" width="120" height="280" as="geometry" />
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-2" value="Motion Primitives Forward Controller" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" vertex="1" parent="1">
+          <mxGeometry x="440" y="280" width="120" height="280" as="geometry" />
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-4" value="&lt;div&gt;Action&lt;/div&gt;&lt;div&gt;Server&lt;/div&gt;" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=middle;fillColor=#C3C3C3;" vertex="1" parent="1">
+          <mxGeometry x="450" y="320" width="100" height="230" as="geometry" />
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-5" value="&lt;div&gt;Action&lt;/div&gt;&lt;div&gt;Client&lt;/div&gt;" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=middle;fillColor=#C3C3C3;" vertex="1" parent="1">
+          <mxGeometry x="50" y="320" width="100" height="230" as="geometry" />
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-6" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.009;entryY=0.086;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#0000FF;" edge="1" parent="1" target="sG7xvZkgZCj6ZZEBmGvM-4">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="150" y="340" as="sourcePoint" />
+            <mxPoint x="340" y="430" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-7" value="Send Goal (Motion Primitives Sequence)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-6">
+          <mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
+            <mxPoint x="17" as="offset" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-8" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.976;entryY=0.178;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#0000FF;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="450" y="356" as="sourcePoint" />
+            <mxPoint x="147.5999999999999" y="356.93999999999994" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-9" value="Goal accepted or rejected" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-8">
+          <mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
+            <mxPoint x="-16" y="-3" as="offset" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-10" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.009;entryY=0.086;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#CC0000;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="150" y="410" as="sourcePoint" />
+            <mxPoint x="450" y="409.5" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-11" value="Cancle Goal request" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-10">
+          <mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
+            <mxPoint x="17" as="offset" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-12" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.976;entryY=0.178;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#CC0000;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="449" y="425.5" as="sourcePoint" />
+            <mxPoint x="147" y="426.5" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-13" value="Response code" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-12">
+          <mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
+            <mxPoint x="-16" y="-2" as="offset" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-14" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.009;entryY=0.086;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#009900;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="150" y="520" as="sourcePoint" />
+            <mxPoint x="452" y="520" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-15" value="Request result" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-14">
+          <mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
+            <mxPoint x="17" as="offset" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-16" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.976;entryY=0.178;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#009900;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="451" y="536" as="sourcePoint" />
+            <mxPoint x="149" y="537" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-17" value="Result" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-16">
+          <mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
+            <mxPoint x="-16" y="-3" as="offset" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-18" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.976;entryY=0.178;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#7F7F7F;" edge="1" parent="1">
+          <mxGeometry width="50" height="50" relative="1" as="geometry">
+            <mxPoint x="450" y="474" as="sourcePoint" />
+            <mxPoint x="148" y="475" as="targetPoint" />
+          </mxGeometry>
+        </mxCell>
+        <mxCell id="sG7xvZkgZCj6ZZEBmGvM-19" value="Feedback" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;fontColor=#7F7F7F;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-18">
+          <mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
+            <mxPoint x="-16" y="-3" as="offset" />
+          </mxGeometry>
+        </mxCell>
+      </root>
+    </mxGraphModel>
+  </diagram>
+</mxfile>