ros-controls · mathias31415 · Apr 8, 2025 · Apr 8, 2025 · Apr 8, 2025 · Apr 9, 2025
diff --git a/motion_primitives_controllers/CMakeLists.txt b/motion_primitives_controllers/CMakeLists.txt
@@ -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,25 +29,37 @@ 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(
   ${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(${PROJECT_NAME} PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
 )
 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}
 )
 
@@ -58,6 +74,14 @@ install(
   RUNTIME DESTINATION bin
 )
 
+install(
+  TARGETS motion_primitives_from_trajectory_controller_parameters
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
 install(TARGETS ${PROJECT_NAME}
   EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib

diff --git a/motion_primitives_controllers/README.md b/motion_primitives_controllers/README.md
@@ -3,24 +3,17 @@ 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.
+> [!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.
 
-- 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.
+**This package contains two controllers:**
+1. [motion_primitives_forward_controller](#moprim_forward_controller)
+2. [motion_primitives_from_trajectory_controller](#moprim_from_traj_controller)
 
-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.
+<a name="moprim_forward_controller"/>
 
 ## 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.
+Both 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:
@@ -46,7 +39,23 @@ These interfaces are used to communicate the internal status of the hardware int
   - `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
 
-# Architecture Overview
+# 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)
@@ -55,8 +64,49 @@ Architecture for a KUKA robot with [`kuka_eki_motion_primitives_hw_interface`](h
 
 ![KUKA Robot Architecture](doc/ros2_control_motion_primitives_kuka.drawio.png)
 
-# Demo-Video with UR10e
+## Demo-Video with UR10e
 [![UR demo video](doc/moprim_forward_controller_ur_demo_thumbnail.png)](https://youtu.be/SKz6LFvJmhQ)
 
-# Demo-Video with KR3
+## 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)
diff --git a/motion_primitives_controllers/controller_plugins.xml b/motion_primitives_controllers/controller_plugins.xml
@@ -5,4 +5,10 @@
     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>
diff --git a/..._primitives_controllers/doc/moprim_from_traj_controller_kuka_demo_thumbnail.png b/..._primitives_controllers/doc/moprim_from_traj_controller_kuka_demo_thumbnail.png
diff --git a/...on_primitives_controllers/doc/moprim_from_traj_controller_ur_demo_thumbnail.png b/...on_primitives_controllers/doc/moprim_from_traj_controller_ur_demo_thumbnail.png
diff --git a/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_kuka.drawio b/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_kuka.drawio
diff --git a/...itives_controllers/doc/ros2_control_motion_primitives_from_traj_kuka.drawio.png b/...itives_controllers/doc/ros2_control_motion_primitives_from_traj_kuka.drawio.png
diff --git a/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_ur.drawio b/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_ur.drawio
diff --git a/...imitives_controllers/doc/ros2_control_motion_primitives_from_traj_ur.drawio.png b/...imitives_controllers/doc/ros2_control_motion_primitives_from_traj_ur.drawio.png
diff --git a/...imitives_controllers/include/motion_primitives_controllers/approx_primitives_with_rdp.hpp b/...imitives_controllers/include/motion_primitives_controllers/approx_primitives_with_rdp.hpp
@@ -0,0 +1,59 @@
+// Copyright (c) 2025, b»robotized
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Mathias Fuhrer
+
+#ifndef MOTION_PRIMITIVES_CONTROLLERS__APPROX_PRIMITIVES_WITH_RDP_HPP_
+#define MOTION_PRIMITIVES_CONTROLLERS__APPROX_PRIMITIVES_WITH_RDP_HPP_
+
+#include <string>
+#include <vector>
+#include "control_msgs/msg/motion_argument.hpp"
+#include "control_msgs/msg/motion_primitive.hpp"
+#include "control_msgs/msg/motion_primitive_sequence.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "motion_primitives_controllers/rdp.hpp"
+
+using MotionSequence = control_msgs::msg::MotionPrimitiveSequence;
+
+namespace approx_primitives_with_rdp
+{
+
+struct PlannedTrajectoryPoint
+{
+  double time_from_start;
+  std::vector<double> joint_positions;
+  geometry_msgs::msg::Pose pose;
+};
+
+// Approximate with LIN Primitives in Cartesian Space
+MotionSequence approxLinPrimitivesWithRDP(
+  const std::vector<PlannedTrajectoryPoint> & trajectory, double epsilon_position,
+  double epsilon_angle, double cart_vel, double cart_acc, bool use_time_not_vel_and_acc = false,
+  double blend_overwrite = -1.0, double blend_scale = 0.1, double blend_lower_limit = 0.0,
+  double blend_upper_limit = 1.0);
+
+// Approximate with PTP Primitives in Joint Space
+MotionSequence approxPtpPrimitivesWithRDP(
+  const std::vector<PlannedTrajectoryPoint> & trajectory, double epsilon, double joint_vel,
+  double joint_acc, bool use_time_not_vel_and_acc = false, double blend_overwrite = -1.0,
+  double blend_scale = 0.1, double blend_lower_limit = 0.0, double blend_upper_limit = 1.0);
+
+double calculateBlendRadius(
+  const rdp::Point & previous_point, const rdp::Point & current_point,
+  const rdp::Point & next_point, double blend_scale, double blend_lower_limit,
+  double blend_upper_limit);
+}  // namespace approx_primitives_with_rdp
+
+#endif  // MOTION_PRIMITIVES_CONTROLLERS__APPROX_PRIMITIVES_WITH_RDP_HPP_
diff --git a/motion_primitives_controllers/include/motion_primitives_controllers/fk_client.hpp b/motion_primitives_controllers/include/motion_primitives_controllers/fk_client.hpp
@@ -0,0 +1,41 @@
+// Copyright (c) 2025, b»robotized
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// Authors: Mathias Fuhrer
+
+#ifndef MOTION_PRIMITIVES_CONTROLLERS__FK_CLIENT_HPP_
+#define MOTION_PRIMITIVES_CONTROLLERS__FK_CLIENT_HPP_
+
+#include <string>
+#include <vector>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <moveit_msgs/srv/get_position_fk.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+
+class FKClient : public rclcpp::Node
+{
+public:
+  explicit FKClient(const std::string & node_name = "fk_client");
+
+  geometry_msgs::msg::Pose computeFK(
+    const std::vector<std::string> & joint_names, const std::vector<double> & joint_positions,
+    const std::string & from_frame, const std::string & to_link);
+
+private:
+  rclcpp::Client<moveit_msgs::srv::GetPositionFK>::SharedPtr fk_client_;
+};
+
+#endif  // MOTION_PRIMITIVES_CONTROLLERS__FK_CLIENT_HPP_