Add motion_primitives_from_trajectory_controller with trajectory approximation using PTP/LIN primitives

.pre-commit-config.yaml

@@ -116,7 +116,7 @@ repos:
         args: [
           '--max-line-length=100',
           '--ignore=D001',
-          '--ignore-path=motion_primitives_forward_controller/userdoc.rst' # D000 fails with myst_parser
+          '--ignore-path=motion_primitives_controllers/userdoc.rst' # D000 fails with myst_parser
           ]
         exclude: CHANGELOG\.rst$
 

doc/controllers_index.rst

@@ -58,8 +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>
-
+   Motion Primitive Controller <../motion_primitives_controllers/userdoc.rst>
 
 Broadcasters
 **********************

motion_primitives_forward_controller/CMakeLists.txt → motion_primitives_controllers/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10)
-project(motion_primitives_forward_controller)
+project(motion_primitives_controllers)
 
 find_package(ros2_control_cmake REQUIRED)
 set_compiler_options()
@@ -14,6 +14,10 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rclcpp_lifecycle
   realtime_tools
   std_srvs
+  moveit_msgs
+  geometry_msgs
+  tf2
+  tf2_geometry_msgs
 )
 
 find_package(ament_cmake REQUIRED)
@@ -25,59 +29,85 @@ endforeach()
 generate_parameter_library(motion_primitives_forward_controller_parameters
   src/motion_primitives_forward_controller_parameter.yaml
 )
+generate_parameter_library(motion_primitives_from_trajectory_controller_parameters
+  src/motion_primitives_from_trajectory_controller_parameter.yaml
+)
 add_library(
-  motion_primitives_forward_controller
+  ${PROJECT_NAME}
   SHARED
+  src/motion_primitives_base_controller.cpp
   src/motion_primitives_forward_controller.cpp
+  src/motion_primitives_from_trajectory_controller.cpp
+  src/fk_client.cpp
+  src/rdp.cpp
+  src/approx_primitives_with_rdp.cpp
 )
-target_include_directories(motion_primitives_forward_controller PUBLIC
+target_include_directories(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
 )
-target_link_libraries(motion_primitives_forward_controller PUBLIC
+target_link_libraries(${PROJECT_NAME} PUBLIC
   motion_primitives_forward_controller_parameters
+  motion_primitives_from_trajectory_controller_parameters
   controller_interface::controller_interface
   hardware_interface::hardware_interface
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
   realtime_tools::realtime_tools
+  tf2::tf2
+  tf2_geometry_msgs::tf2_geometry_msgs
   ${control_msgs_TARGETS}
+  ${geometry_msgs_TARGETS}
+  ${moveit_msgs_TARGETS}
   ${std_srvs_TARGETS}
 )
 
-target_compile_definitions(motion_primitives_forward_controller PRIVATE "MOTION_PRIMITIVES_FORWARD_CONTROLLER_BUILDING_DLL")
+target_compile_definitions(${PROJECT_NAME} PRIVATE "MOTION_PRIMITIVES_CONTROLLERS_BUILDING_DLL")
 
 pluginlib_export_plugin_description_file(
-  controller_interface motion_primitives_forward_controller.xml)
+  controller_interface controller_plugins.xml)
 
 install(
-  TARGETS
-    motion_primitives_forward_controller
-    motion_primitives_forward_controller_parameters
-  EXPORT motion_primitives_forward_controller_targets
-  RUNTIME DESTINATION bin
+  TARGETS motion_primitives_forward_controller_parameters
+  EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
-
 install(
-  DIRECTORY include/
-  DESTINATION include/${PROJECT_NAME}
+  TARGETS motion_primitives_from_trajectory_controller_parameters
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
-ament_export_targets(motion_primitives_forward_controller_targets HAS_LIBRARY_TARGET)
 
-ament_export_include_directories(
-  include
+install(TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
-ament_export_dependencies(
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
+
+install(DIRECTORY include/
+  DESTINATION include
 )
+
+install(FILES controller_plugins.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+
 ament_export_libraries(
-  motion_primitives_forward_controller
+  ${PROJECT_NAME}
 )
 
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 ament_package()
 
 if(BUILD_TESTING)
@@ -91,7 +121,7 @@ if(BUILD_TESTING)
     test/test_load_motion_primitives_forward_controller.cpp
   )
   target_link_libraries(test_load_motion_primitives_forward_controller
-    motion_primitives_forward_controller
+    ${PROJECT_NAME}
     controller_manager::controller_manager
     ros2_control_test_assets::ros2_control_test_assets
   )
@@ -102,7 +132,7 @@ if(BUILD_TESTING)
     ${CMAKE_CURRENT_SOURCE_DIR}/test/motion_primitives_forward_controller_params.yaml
   )
   target_link_libraries(test_motion_primitives_forward_controller
-    motion_primitives_forward_controller
+    ${PROJECT_NAME}
     controller_interface::controller_interface
     hardware_interface::hardware_interface
     ros2_control_test_assets::ros2_control_test_assets

motion_primitives_controllers/README.md

@@ -0,0 +1,121 @@
+motion_primitive_controllers
+==========================================
+
+Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC)
+
+> [!WARNING]
+> There is no guarantee that the motion defined by the motion primitive will actually be executed exactly as planned. In particular, for motions in Cartesian space such as LIN primitives, it is not necessarily ensured that the robot will execute the motion exactly in that way, since the inverse kinematics is not always unique.
+
+**This package contains two controllers:**
+1. [motion_primitives_forward_controller](#moprim_forward_controller)
+2. [motion_primitives_from_trajectory_controller](#moprim_from_traj_controller)
+
+<a name="moprim_forward_controller"/>
+
+## Command and State Interfaces
+TBoth controllers use the following command and state interfaces to transmit the motion primitives. 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
+These controllers use the [`generate_parameter_library`](https://github.com/PickNikRobotics/generate_parameter_library) to handle their parameters.
+
+For the **motion_primitives_forward_controller**, 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 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).
+
+For the **motion_primitives_from_trajectory_controller**, the [parameter definition file located in the src folder](https://github.com/ros-controls/ros2_controllers/blob/master/motion_primitives_forward_controller_parameter/src/motion_primitives_from_trajectory_controller_parameter.yaml) contains descriptions for all the parameters used by the controller. An example parameter file can be found in [the test directory](https://github.com/ros-controls/ros2_controllers/blob/master/motion_primitives_forward_controller_parameter/test/motion_primitives_from_trajectory_controller_params.yaml).
+
+
+# motion_primitives_forward_controller
+This controller 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.
+
+## 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)
+
+<a name="moprim_from_traj_controller"/>
+
+# motion_primitives_from_trajectory_controller
+
+The `motion_primitives_from_trajectory_controller` builds on the same architecture as the `motion_primitives_forward_controller` and uses the same command and state interfaces, making it compatible with the same hardware interfaces. However, instead of receiving motion primitives directly, it takes a `FollowJointTrajectory` action as input and approximates the trajectory using either `PTP` (`LINEAR_JOINT`) or `LIN` (`LINEAR_CARTESIAN`) motion primitives.
+
+The approximation mode can be selected via the `approximate_mode` parameter, with options `"RDP_PTP"` or `"RDP_LIN"`, using the Ramer-Douglas-Peucker algorithm to reduce the trajectory points. Tolerances for the approximation are defined by:
+- `epsilon_joint_angle` for PTP (in radians)
+- `epsilon_cart_position` (in meters) and `epsilon_cart_angle` (in radians) for LIN
+
+The `use_time_not_vel_and_acc` parameter determines whether motion duration is calculated based on time stamps or if velocity and acceleration values are used instead. For PTP primitives, joint velocity and acceleration are taken as the maximum values from the original trajectory. For LIN primitives, Cartesian velocity and acceleration are estimated numerically from the end-effector path.
+
+The blend radius is automatically computed based on the smaller of the distances to the previous and next target points, scaled by a user-configurable blend_radius_percentage. The resulting value is then clamped between a lower and upper limit, specified by blend_radius_lower_clamp and blend_radius_upper_clamp.
+All three parameters can be configured by the user:
+- blend_radius_percentage (default: 0.1) – relative scale factor applied to the smaller neighboring distance
+- blend_radius_lower_clamp (default: 0.01 m) – minimum allowable blend radius
+- blend_radius_upper_clamp (default: 0.1 m) – maximum allowable blend radius
+
+Alternatively, users can override velocity, acceleration, and blend radius values with the following parameters:
+- PTP: `joint_vel_overwrite`, `joint_acc_overwrite`
+- LIN: `cart_vel_overwrite`, `cart_acc_overwrite`
+- Blend radius: `blend_radius_overwrite`
+
+This controller enables executing collision-free trajectories planned with MoveIt using approximated motion primitives.
+
+## 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_from_traj_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_from_traj_kuka.drawio.png)
+
+## Demo-Video with UR10e
+[![UR demo video](doc/moprim_from_traj_controller_ur_demo_thumbnail.png)](https://youtu.be/Z_NCaSyE-KA)
+
+
+## Demo-Video with KR3
+[![KUKA demo video](doc/moprim_from_traj_controller_kuka_demo_thumbnail.png)](https://youtu.be/SvUU6PM1qRk)

motion_primitives_controllers/controller_plugins.xml

@@ -0,0 +1,14 @@
+<library path="motion_primitives_controllers">
+  <class name="motion_primitives_controllers/MotionPrimitivesForwardController"
+         type="motion_primitives_controllers::MotionPrimitivesForwardController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    MotionPrimitivesForwardController ros2_control controller.
+  </description>
+  </class>
+  <class name="motion_primitives_controllers/MotionPrimitivesFromTrajectoryController"
+         type="motion_primitives_controllers::MotionPrimitivesFromTrajectoryController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    MotionPrimitivesFromTrajectoryController ros2_control controller.
+  </description>
+  </class>
+</library>