diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ca2404daf3..bea0b0fa74 100644 --- a/.pre-commit-config.yaml +++ b/.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$ diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 1acb19f7ce..4bbe6eb673 100644 --- a/doc/controllers_index.rst +++ b/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 ********************** diff --git a/motion_primitives_forward_controller/CMakeLists.txt b/motion_primitives_controllers/CMakeLists.txt similarity index 57% rename from motion_primitives_forward_controller/CMakeLists.txt rename to motion_primitives_controllers/CMakeLists.txt index 35736c9bfd..0e6b110c55 100644 --- a/motion_primitives_forward_controller/CMakeLists.txt +++ b/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 $ $ ) -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 diff --git a/motion_primitives_controllers/README.md b/motion_primitives_controllers/README.md new file mode 100644 index 0000000000..ada0033ef0 --- /dev/null +++ b/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) + + + +## 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/"` 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) + + + +# 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 new file mode 100644 index 0000000000..455692c24d --- /dev/null +++ b/motion_primitives_controllers/controller_plugins.xml @@ -0,0 +1,14 @@ + + + + MotionPrimitivesForwardController ros2_control controller. + + + + + MotionPrimitivesFromTrajectoryController ros2_control controller. + + + diff --git a/motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio b/motion_primitives_controllers/doc/Moprim_Controller_ExecuteMotion_Action copy.drawio similarity index 100% rename from motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio rename to motion_primitives_controllers/doc/Moprim_Controller_ExecuteMotion_Action copy.drawio diff --git a/motion_primitives_controllers/doc/Moprim_Controller_ExecuteMotion_Action.drawio b/motion_primitives_controllers/doc/Moprim_Controller_ExecuteMotion_Action.drawio new file mode 100644 index 0000000000..dbbde2e7b2 --- /dev/null +++ b/motion_primitives_controllers/doc/Moprim_Controller_ExecuteMotion_Action.drawio @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio.png b/motion_primitives_controllers/doc/Moprim_Controller_ExecuteMotion_Action.drawio.png similarity index 100% rename from motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio.png rename to motion_primitives_controllers/doc/Moprim_Controller_ExecuteMotion_Action.drawio.png diff --git a/motion_primitives_forward_controller/doc/moprim_forward_controller_kuka_demo_thumbnail.png b/motion_primitives_controllers/doc/moprim_forward_controller_kuka_demo_thumbnail.png similarity index 100% rename from motion_primitives_forward_controller/doc/moprim_forward_controller_kuka_demo_thumbnail.png rename to motion_primitives_controllers/doc/moprim_forward_controller_kuka_demo_thumbnail.png diff --git a/motion_primitives_forward_controller/doc/moprim_forward_controller_ur_demo_thumbnail.png b/motion_primitives_controllers/doc/moprim_forward_controller_ur_demo_thumbnail.png similarity index 100% rename from motion_primitives_forward_controller/doc/moprim_forward_controller_ur_demo_thumbnail.png rename to motion_primitives_controllers/doc/moprim_forward_controller_ur_demo_thumbnail.png diff --git a/motion_primitives_controllers/doc/moprim_from_traj_controller_kuka_demo_thumbnail.png b/motion_primitives_controllers/doc/moprim_from_traj_controller_kuka_demo_thumbnail.png new file mode 100644 index 0000000000..5e975fd33c Binary files /dev/null and b/motion_primitives_controllers/doc/moprim_from_traj_controller_kuka_demo_thumbnail.png differ diff --git a/motion_primitives_controllers/doc/moprim_from_traj_controller_ur_demo_thumbnail.png b/motion_primitives_controllers/doc/moprim_from_traj_controller_ur_demo_thumbnail.png new file mode 100644 index 0000000000..7efc38852d Binary files /dev/null and b/motion_primitives_controllers/doc/moprim_from_traj_controller_ur_demo_thumbnail.png differ 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 new file mode 100644 index 0000000000..02b15ffe9b --- /dev/null +++ b/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_kuka.drawio @@ -0,0 +1,258 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_kuka.drawio.png b/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_kuka.drawio.png new file mode 100644 index 0000000000..97074bde0c Binary files /dev/null and b/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_kuka.drawio.png differ 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 new file mode 100644 index 0000000000..c51cdee792 --- /dev/null +++ b/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_ur.drawio @@ -0,0 +1,372 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_ur.drawio.png b/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_ur.drawio.png new file mode 100644 index 0000000000..fc003d31ae Binary files /dev/null and b/motion_primitives_controllers/doc/ros2_control_motion_primitives_from_traj_ur.drawio.png differ diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio b/motion_primitives_controllers/doc/ros2_control_motion_primitives_kuka.drawio similarity index 100% rename from motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio rename to motion_primitives_controllers/doc/ros2_control_motion_primitives_kuka.drawio diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio.png b/motion_primitives_controllers/doc/ros2_control_motion_primitives_kuka.drawio.png similarity index 100% rename from motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio.png rename to motion_primitives_controllers/doc/ros2_control_motion_primitives_kuka.drawio.png diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.drawio b/motion_primitives_controllers/doc/ros2_control_motion_primitives_ur.drawio similarity index 100% rename from motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.drawio rename to motion_primitives_controllers/doc/ros2_control_motion_primitives_ur.drawio diff --git a/motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.drawio.png b/motion_primitives_controllers/doc/ros2_control_motion_primitives_ur.drawio.png similarity index 100% rename from motion_primitives_forward_controller/doc/ros2_control_motion_primitives_ur.drawio.png rename to motion_primitives_controllers/doc/ros2_control_motion_primitives_ur.drawio.png diff --git a/motion_primitives_controllers/include/motion_primitives_controllers/approx_primitives_with_rdp.hpp b/motion_primitives_controllers/include/motion_primitives_controllers/approx_primitives_with_rdp.hpp new file mode 100644 index 0000000000..77cff60e6d --- /dev/null +++ b/motion_primitives_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 +#include +#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 joint_positions; + geometry_msgs::msg::Pose pose; +}; + +// Approximate with LIN Primitives in Cartesian Space +MotionSequence approxLinPrimitivesWithRDP( + const std::vector & 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 & 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 new file mode 100644 index 0000000000..54b68a8d8c --- /dev/null +++ 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 +#include + +#include +#include +#include +#include + +class FKClient : public rclcpp::Node +{ +public: + explicit FKClient(const std::string & node_name = "fk_client"); + + geometry_msgs::msg::Pose computeFK( + const std::vector & joint_names, const std::vector & joint_positions, + const std::string & from_frame, const std::string & to_link); + +private: + rclcpp::Client::SharedPtr fk_client_; +}; + +#endif // MOTION_PRIMITIVES_CONTROLLERS__FK_CLIENT_HPP_ diff --git a/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp b/motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_base_controller.hpp similarity index 68% rename from motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp rename to motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_base_controller.hpp index 476d383040..d17ab07e08 100644 --- a/motion_primitives_forward_controller/include/motion_primitives_forward_controller/motion_primitives_forward_controller.hpp +++ b/motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_base_controller.hpp @@ -14,15 +14,14 @@ // // Authors: Mathias Fuhrer -#ifndef MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ -#define MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ +#ifndef MOTION_PRIMITIVES_CONTROLLERS__MOTION_PRIMITIVES_BASE_CONTROLLER_HPP_ +#define MOTION_PRIMITIVES_CONTROLLERS__MOTION_PRIMITIVES_BASE_CONTROLLER_HPP_ #include #include #include #include -#include #include #include #include "controller_interface/controller_interface.hpp" @@ -36,7 +35,7 @@ #include "control_msgs/msg/motion_primitive.hpp" #include "rclcpp_action/rclcpp_action.hpp" -namespace motion_primitives_forward_controller +namespace motion_primitives_controllers { enum class ExecutionState : uint8_t { @@ -64,10 +63,10 @@ enum class ReadyForNewPrimitive : uint8_t READY = 1 }; -class MotionPrimitivesForwardController : public controller_interface::ControllerInterface +class MotionPrimitivesBaseController : public controller_interface::ControllerInterface { public: - MotionPrimitivesForwardController(); + MotionPrimitivesBaseController(); controller_interface::CallbackReturn on_init() override; @@ -88,23 +87,11 @@ class MotionPrimitivesForwardController : public controller_interface::Controlle const rclcpp::Time & time, const rclcpp::Duration & period) override; protected: - std::shared_ptr param_listener_; - motion_primitives_forward_controller::Params params_; std::string tf_prefix_; using MotionPrimitive = control_msgs::msg::MotionPrimitive; realtime_tools::LockFreeSPSCQueue moprim_queue_; - using ExecuteMotionAction = control_msgs::action::ExecuteMotionPrimitiveSequence; - rclcpp_action::Server::SharedPtr action_server_; - rclcpp_action::GoalResponse goal_received_callback( - const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); - rclcpp_action::CancelResponse goal_cancelled_callback( - const std::shared_ptr> goal_handle); - void goal_accepted_callback( - std::shared_ptr> goal_handle); - using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; - realtime_tools::RealtimeThreadSafeBox> rt_goal_handle_; std::atomic has_active_goal_ = false; rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(std::chrono::milliseconds(20)); @@ -123,6 +110,6 @@ class MotionPrimitivesForwardController : public controller_interface::Controlle MotionPrimitive current_moprim_; }; -} // namespace motion_primitives_forward_controller +} // namespace motion_primitives_controllers -#endif // MOTION_PRIMITIVES_FORWARD_CONTROLLER__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ +#endif // MOTION_PRIMITIVES_CONTROLLERS__MOTION_PRIMITIVES_BASE_CONTROLLER_HPP_ diff --git a/motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_forward_controller.hpp b/motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_forward_controller.hpp new file mode 100644 index 0000000000..c31cf65618 --- /dev/null +++ b/motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_forward_controller.hpp @@ -0,0 +1,65 @@ +// 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__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ +#define MOTION_PRIMITIVES_CONTROLLERS__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ + +#include +#include +#include "motion_primitives_controllers/motion_primitives_base_controller.hpp" + +namespace motion_primitives_controllers +{ + +class MotionPrimitivesForwardController : public MotionPrimitivesBaseController +{ +public: + MotionPrimitivesForwardController() = default; + ~MotionPrimitivesForwardController() override = default; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::shared_ptr param_listener_; + motion_primitives_forward_controller::Params params_; + + using ExecuteMotionAction = control_msgs::action::ExecuteMotionPrimitiveSequence; + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::GoalResponse goal_received_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + void goal_accepted_callback( + std::shared_ptr> goal_handle); + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + realtime_tools::RealtimeThreadSafeBox> rt_goal_handle_; +}; + +} // namespace motion_primitives_controllers + +#endif // MOTION_PRIMITIVES_CONTROLLERS__MOTION_PRIMITIVES_FORWARD_CONTROLLER_HPP_ diff --git a/motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_from_trajectory_controller.hpp b/motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_from_trajectory_controller.hpp new file mode 100644 index 0000000000..f6f14f7876 --- /dev/null +++ b/motion_primitives_controllers/include/motion_primitives_controllers/motion_primitives_from_trajectory_controller.hpp @@ -0,0 +1,148 @@ +// 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__MOTION_PRIMITIVES_FROM_TRAJECTORY_CONTROLLER_HPP_ +#define MOTION_PRIMITIVES_CONTROLLERS__MOTION_PRIMITIVES_FROM_TRAJECTORY_CONTROLLER_HPP_ + +#include +#include + +#include +#include "motion_primitives_controllers/motion_primitives_base_controller.hpp" + +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "geometry_msgs/msg/pose_array.hpp" + +#include "motion_primitives_controllers/approx_primitives_with_rdp.hpp" +#include "motion_primitives_controllers/fk_client.hpp" + +namespace motion_primitives_controllers +{ +enum class ApproxMode +{ + RDP_PTP, + RDP_LIN +}; + +class MotionPrimitivesFromTrajectoryController : public MotionPrimitivesBaseController +{ +public: + MotionPrimitivesFromTrajectoryController() = default; + ~MotionPrimitivesFromTrajectoryController() override = default; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +protected: + std::shared_ptr param_listener_; + motion_primitives_from_trajectory_controller::Params params_; + + using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::GoalResponse goal_received_callback( + const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal); + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + void goal_accepted_callback( + std::shared_ptr> goal_handle); + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + realtime_tools::RealtimeThreadSafeBox> rt_goal_handle_; + + std::shared_ptr fk_client_; + + ApproxMode approx_mode_; + bool use_time_not_vel_and_acc_; + double epsilon_joint_angle_; + double epsilon_cart_position_; + double epsilon_cart_angle_; + + double blend_radius_percentage_; + double blend_radius_lower_limit_; + double blend_radius_upper_limit_; + double blend_radius_overwrite_; + + double joint_vel_overwrite_; + double joint_acc_overwrite_; + double max_traj_joint_vel_; + double max_traj_joint_acc_; + void get_max_traj_joint_vel_and_acc( + const std::shared_ptr & trajectory_msg, + double & max_traj_joint_vel, double & max_traj_joint_acc); + double cart_vel_overwrite_; + double cart_acc_overwrite_; + double max_traj_cart_vel_; + double max_traj_cart_acc_; + void get_max_traj_cart_vel_and_acc( + const geometry_msgs::msg::PoseArray & planned_poses_msg, + const std::vector & time_from_start, double & max_vel, double & max_acc); + + rclcpp::Publisher::SharedPtr planned_trajectory_publisher_; + rclcpp::Publisher::SharedPtr planned_poses_publisher_; + rclcpp::Publisher::SharedPtr + motion_primitive_publisher_; + + // ############ Function copied from JointTrajectoryController ############ + void sort_to_local_joint_order( + std::shared_ptr trajectory_msg) const; +}; + +// ############ Function copied from JointTrajectoryController ############ +/** + * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 + * indices. If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated + * mapping vector is "{2, 1}". return empty vector if \p t1 is not a subset of \p t2. + */ +template +std::vector mapping(const T & t1, const T & t2) +{ + // t1 must be a subset of t2 + if (t1.size() > t2.size()) + { + return std::vector(); + } + + std::vector mapping_vector(t1.size()); // Return value + for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it) + { + auto t2_it = std::find(t2.begin(), t2.end(), *t1_it); + if (t2.end() == t2_it) + { + return std::vector(); + } + else + { + const size_t t1_dist = static_cast(std::distance(t1.begin(), t1_it)); + const size_t t2_dist = static_cast(std::distance(t2.begin(), t2_it)); + mapping_vector[t1_dist] = t2_dist; + } + } + return mapping_vector; +} + +} // namespace motion_primitives_controllers + +#endif // MOTION_PRIMITIVES_CONTROLLERS__MOTION_PRIMITIVES_FROM_TRAJECTORY_CONTROLLER_HPP_ diff --git a/motion_primitives_controllers/include/motion_primitives_controllers/rdp.hpp b/motion_primitives_controllers/include/motion_primitives_controllers/rdp.hpp new file mode 100644 index 0000000000..3ce9bfb93c --- /dev/null +++ b/motion_primitives_controllers/include/motion_primitives_controllers/rdp.hpp @@ -0,0 +1,80 @@ +// 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__RDP_HPP_ +#define MOTION_PRIMITIVES_CONTROLLERS__RDP_HPP_ + +#include +#include + +#include +#include + +namespace rdp +{ + +using Point = std::vector; +using PointList = std::vector; + +/** + * Computes the shortest distance from a point to a line in N-dimensional space. + * + * @param point The point to measure from. + * @param start The start of the line segment. + * @param end The end of the line segment. + * @return Distance from the point to the line segment. + */ +double pointLineDistanceND(const Point & point, const Point & start, const Point & end); + +/** + * Recursive implementation of the Ramer-Douglas-Peucker algorithm + * for simplifying a series of N-dimensional points. + * + * @param points The list of input points. + * @param epsilon Tolerance value: points closer than this distance to the line will be removed. + * @param offset Offset for original indices (used in recursion). + * @return A pair consisting of: + * - The simplified list of points. + * - The list of indices (from the original input) corresponding to the retained points. + */ +std::pair> rdpRecursive( + const PointList & points, double epsilon, std::size_t offset = 0); + +/** + * Computes the angular distance (in radians) between two quaternions. + * + * @param q1 First quaternion. + * @param q2 Second quaternion. + * @return Angular difference in radians. + */ +double quaternionAngularDistance( + const geometry_msgs::msg::Quaternion & q1, const geometry_msgs::msg::Quaternion & q2); + +/** + * Recursive RDP for quaternions based on angular deviation. + * + * @param quaternions List of quaternions. + * @param epsilon_angle Angular threshold (in radians). + * @param offset Index offset for tracking original indices. + * @return Pair of simplified quaternions and their original indices. + */ +std::pair, std::vector> rdpRecursiveQuaternion( + const std::vector & quaternions, double epsilon_angle_rad, + size_t offset); + +} // namespace rdp + +#endif // MOTION_PRIMITIVES_CONTROLLERS__RDP_HPP_ diff --git a/motion_primitives_forward_controller/package.xml b/motion_primitives_controllers/package.xml similarity index 90% rename from motion_primitives_forward_controller/package.xml rename to motion_primitives_controllers/package.xml index 768ab930cc..a7e13ef165 100644 --- a/motion_primitives_forward_controller/package.xml +++ b/motion_primitives_controllers/package.xml @@ -1,7 +1,7 @@ - motion_primitives_forward_controller + motion_primitives_controllers 5.5.0 Package to control robots using motion primitives like PTP, LIN and CIRC @@ -30,6 +30,10 @@ rclcpp_lifecycle realtime_tools std_srvs + geometry_msgs + moveit_msgs + tf2 + tf2_geometry_msgs ament_cmake_gmock controller_manager diff --git a/motion_primitives_controllers/src/approx_primitives_with_rdp.cpp b/motion_primitives_controllers/src/approx_primitives_with_rdp.cpp new file mode 100644 index 0000000000..9a59d28410 --- /dev/null +++ b/motion_primitives_controllers/src/approx_primitives_with_rdp.cpp @@ -0,0 +1,360 @@ +// 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 + +#include "motion_primitives_controllers/approx_primitives_with_rdp.hpp" +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +using control_msgs::msg::MotionArgument; +using control_msgs::msg::MotionPrimitive; +using control_msgs::msg::MotionPrimitiveSequence; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; + +namespace approx_primitives_with_rdp +{ + +MotionSequence approxLinPrimitivesWithRDP( + const std::vector & trajectory, + double epsilon_position, double epsilon_angle, double cart_vel, double cart_acc, + bool use_time_not_vel_and_acc, double blend_overwrite, double blend_scale, + double blend_lower_limit, double blend_upper_limit) +{ + MotionSequence motion_sequence; + std::vector motion_primitives; + + if (trajectory.empty()) + { + RCLCPP_WARN( + rclcpp::get_logger("approx_primitives_with_rdp"), + "[approxLinPrimitivesWithRDP] Warning: trajectory is empty."); + return motion_sequence; + } + + // Reduce positions using RDP + rdp::PointList cartesian_points; + for (const auto & point : trajectory) + { + cartesian_points.push_back( + {point.pose.position.x, point.pose.position.y, point.pose.position.z}); + } + + auto [reduced_points, reduced_indices] = rdp::rdpRecursive(cartesian_points, epsilon_position); + + RCLCPP_INFO( + rclcpp::get_logger("approx_primitives_with_rdp"), + "[approxLinPrimitivesWithRDP] Added %zu points due to position change (epsilon_position = " + "%.4f).", + reduced_indices.size(), epsilon_position); + + std::set final_indices(reduced_indices.begin(), reduced_indices.end()); + std::set position_indices(reduced_indices.begin(), reduced_indices.end()); + std::set orientation_indices; + + // Enrich reduced_indices by checking quaternion changes in each segment + for (size_t i = 1; i < reduced_indices.size(); ++i) + { + size_t start_index = reduced_indices[i - 1]; + size_t end_index = reduced_indices[i]; + + if (end_index - start_index <= 1) continue; // nothing in between + + // Extract quaternion segment for RDP + std::vector quats; + for (size_t j = start_index; j <= end_index; ++j) + { + quats.push_back(trajectory[j].pose.orientation); + } + + auto [reduced_quats, reduced_quat_indices] = + rdp::rdpRecursiveQuaternion(quats, epsilon_angle, start_index); + + // Add new orientation indices to final set + for (size_t idx : reduced_quat_indices) + { + if (final_indices.insert(idx).second) // true if inserted (i.e., newly added) + { + orientation_indices.insert(idx); + } + } + } + + // Sort final indices for ordered primitive generation + std::vector sorted_final_indices(final_indices.begin(), final_indices.end()); + std::sort(sorted_final_indices.begin(), sorted_final_indices.end()); + + RCLCPP_INFO( + rclcpp::get_logger("approx_primitives_with_rdp"), + "[approxLinPrimitivesWithRDP] Added %zu additional intermediate points due to orientation " + "angle change (epsilon_angle = %.4f).", + sorted_final_indices.size() - reduced_indices.size(), epsilon_angle); + + // Generate motion primitives between reduced points + for (size_t i = 1; i < sorted_final_indices.size(); ++i) + { + size_t start_index = sorted_final_indices[i - 1]; + size_t end_index = sorted_final_indices[i]; + + MotionPrimitive primitive; + primitive.type = MotionPrimitive::LINEAR_CARTESIAN; + + // Calculate blend radius based on the distance to the next and previous point + if (i == sorted_final_indices.size() - 1) + { + primitive.blend_radius = 0.0; // Last point has no blend radius + } + else + { + if (blend_overwrite > 0.0) + { + primitive.blend_radius = blend_overwrite; + } + else + { + const auto & p0 = trajectory[start_index].pose.position; + const auto & p1 = trajectory[end_index].pose.position; + const auto & p2 = trajectory[sorted_final_indices[i + 1]].pose.position; + primitive.blend_radius = calculateBlendRadius( + {p0.x, p0.y, p0.z}, {p1.x, p1.y, p1.z}, {p2.x, p2.y, p2.z}, blend_scale, + blend_lower_limit, blend_upper_limit); + } + } + + double velocity = -1.0; + double acceleration = -1.0; + double move_time = -1.0; + + // Use time or velocity+acceleration based on use_time_not_vel_and_acc + if (use_time_not_vel_and_acc) + { + move_time = trajectory[end_index].time_from_start - trajectory[start_index].time_from_start; + MotionArgument arg_time; + arg_time.name = "move_time"; + arg_time.value = move_time; + primitive.additional_arguments.push_back(arg_time); + } + else + { + velocity = cart_vel; + acceleration = cart_acc; + MotionArgument arg_vel; + arg_vel.name = "velocity"; + arg_vel.value = velocity; + primitive.additional_arguments.push_back(arg_vel); + + MotionArgument arg_acc; + arg_acc.name = "acceleration"; + arg_acc.value = acceleration; + primitive.additional_arguments.push_back(arg_acc); + } + + // Add pose to primitive + PoseStamped pose_stamped; + pose_stamped.pose = trajectory[end_index].pose; + primitive.poses.push_back(pose_stamped); + motion_primitives.push_back(primitive); + + // Determine reason for addition + std::string reason; + bool pos = position_indices.count(end_index); + bool ori = orientation_indices.count(end_index); + if (pos && ori) + reason = "position+orientation"; + else if (pos) + reason = "position"; + else if (ori) + reason = "orientation"; + else + reason = "unknown"; + + RCLCPP_DEBUG( + rclcpp::get_logger("approx_primitives_with_rdp"), + "Added LIN Primitive [%zu] (%s): (x,y,z,qx,qy,qz,qw) = (%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, " + "%.4f), blend_radius = %.4f, move_time = %.4f, velocity = %.4f, acceleration = %.4f", + i, reason.c_str(), pose_stamped.pose.position.x, pose_stamped.pose.position.y, + pose_stamped.pose.position.z, pose_stamped.pose.orientation.x, + pose_stamped.pose.orientation.y, pose_stamped.pose.orientation.z, + pose_stamped.pose.orientation.w, primitive.blend_radius, move_time, velocity, acceleration); + } + + motion_sequence.motions = motion_primitives; + + return motion_sequence; +} + +MotionSequence approxPtpPrimitivesWithRDP( + const std::vector & trajectory, + double epsilon, double joint_vel, double joint_acc, bool use_time_not_vel_and_acc, + double blend_overwrite, double blend_scale, double blend_lower_limit, double blend_upper_limit) +{ + MotionSequence motion_sequence; + std::vector motion_primitives; + + if (trajectory.empty()) + { + RCLCPP_WARN( + rclcpp::get_logger("approx_primitives_with_rdp"), + "[approxPtpPrimitivesWithRDP] Warning: trajectory is empty."); + return motion_sequence; + } + + // Collect joint positions for RDP + rdp::PointList points; + for (const auto & pt : trajectory) + { + points.push_back(pt.joint_positions); + } + + auto [reduced_points, reduced_indices] = rdp::rdpRecursive(points, epsilon); + + // Generate motion primitives between reduced joint points + for (size_t i = 1; i < reduced_points.size(); ++i) + { + MotionPrimitive primitive; + primitive.type = MotionPrimitive::LINEAR_JOINT; + + // Calculate blend radius based on the distance to the next and previous point + if (i == reduced_points.size() - 1) + { + primitive.blend_radius = 0.0; // Last point has no blend radius + } + else + { + if (blend_overwrite > 0.0) + { + primitive.blend_radius = blend_overwrite; + } + else + { + size_t prev_index = reduced_indices[i - 1]; + size_t curr_index = reduced_indices[i]; + size_t next_index = reduced_indices[i + 1]; + + rdp::Point prev_xyz = { + trajectory[prev_index].pose.position.x, trajectory[prev_index].pose.position.y, + trajectory[prev_index].pose.position.z}; + rdp::Point curr_xyz = { + trajectory[curr_index].pose.position.x, trajectory[curr_index].pose.position.y, + trajectory[curr_index].pose.position.z}; + rdp::Point next_xyz = { + trajectory[next_index].pose.position.x, trajectory[next_index].pose.position.y, + trajectory[next_index].pose.position.z}; + primitive.blend_radius = calculateBlendRadius( + prev_xyz, curr_xyz, next_xyz, blend_scale, blend_lower_limit, blend_upper_limit); + } + } + + double velocity = -1.0; + double acceleration = -1.0; + double move_time = -1.0; + + // Use time or velocity+acceleration based on use_time_not_vel_and_acc + if (use_time_not_vel_and_acc) + { + size_t start_index = reduced_indices[i - 1]; + size_t end_index = reduced_indices[i]; + double prev_time = trajectory[start_index].time_from_start; + double curr_time = trajectory[end_index].time_from_start; + move_time = curr_time - prev_time; + + MotionArgument arg_time; + arg_time.name = "move_time"; + arg_time.value = move_time; + primitive.additional_arguments.push_back(arg_time); + } + else + { + velocity = joint_vel; + acceleration = joint_acc; + + MotionArgument arg_vel; + arg_vel.name = "velocity"; + arg_vel.value = velocity; + primitive.additional_arguments.push_back(arg_vel); + + MotionArgument arg_acc; + arg_acc.name = "acceleration"; + arg_acc.value = acceleration; + primitive.additional_arguments.push_back(arg_acc); + } + + primitive.joint_positions = reduced_points[i]; + motion_primitives.push_back(primitive); + + std::ostringstream joint_stream; + joint_stream << "Added PTP Primitive [" << i << "]: joints = ("; + for (size_t j = 0; j < reduced_points[i].size(); ++j) + { + joint_stream << reduced_points[i][j]; + if (j + 1 < reduced_points[i].size()) joint_stream << ", "; + } + joint_stream << "), blend_radius = " << primitive.blend_radius << ", move_time = " << move_time + << ", velocity = " << velocity << ", acceleration = " << acceleration; + + RCLCPP_DEBUG( + rclcpp::get_logger("approx_primitives_with_rdp"), "%s", joint_stream.str().c_str()); + } + + motion_sequence.motions = motion_primitives; + + RCLCPP_INFO( + rclcpp::get_logger("approx_primitives_with_rdp"), + "Reduced %zu joint points to %zu PTP primitives with epsilon=%.4f", points.size(), + reduced_points.size() - 1, epsilon); + + return motion_sequence; +} + +double calculateBlendRadius( + const rdp::Point & previous_point, const rdp::Point & current_point, + const rdp::Point & next_point, const double blend_scale, const double blend_lower_limit, + const double blend_upper_limit) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("approx_primitives_with_rdp"), + "[calculateBlendRadius] Calculating blend radius with scale %.2f, lower limit %.2f, upper " + "limit " + "%.2f", + blend_scale, blend_lower_limit, blend_upper_limit); + + double dist_prev = std::sqrt( + std::pow(current_point[0] - previous_point[0], 2) + + std::pow(current_point[1] - previous_point[1], 2) + + std::pow(current_point[2] - previous_point[2], 2)); + + double dist_next = std::sqrt( + std::pow(next_point[0] - current_point[0], 2) + std::pow(next_point[1] - current_point[1], 2) + + std::pow(next_point[2] - current_point[2], 2)); + + double min_dist = std::min(dist_prev, dist_next); + double blend = blend_scale * min_dist; + + // Clamp blend radius to [blend_lower_limit, blend_upper_limit] + if (blend < blend_lower_limit) + { + blend = blend_lower_limit; + } + else if (blend > blend_upper_limit) + { + blend = blend_upper_limit; + } + + return blend; +} + +} // namespace approx_primitives_with_rdp diff --git a/motion_primitives_controllers/src/fk_client.cpp b/motion_primitives_controllers/src/fk_client.cpp new file mode 100644 index 0000000000..ec3df44ff3 --- /dev/null +++ b/motion_primitives_controllers/src/fk_client.cpp @@ -0,0 +1,62 @@ +// 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 + +#include "motion_primitives_controllers/fk_client.hpp" + +FKClient::FKClient(const std::string & node_name) : Node(node_name) +{ + fk_client_ = this->create_client("/compute_fk"); + + while (!fk_client_->wait_for_service(std::chrono::seconds(1))) + { + RCLCPP_INFO(this->get_logger(), "Waiting for /compute_fk service..."); + } +} + +geometry_msgs::msg::Pose FKClient::computeFK( + const std::vector & joint_names, const std::vector & joint_positions, + const std::string & from_frame, const std::string & to_link) +{ + auto request = std::make_shared(); + request->fk_link_names.push_back(to_link); + + sensor_msgs::msg::JointState joint_state; + joint_state.name = joint_names; + joint_state.position = joint_positions; + request->robot_state.joint_state = joint_state; + request->header.frame_id = from_frame; + + auto future = fk_client_->async_send_request(request); + + if ( + rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == + rclcpp::FutureReturnCode::SUCCESS) + { + auto response = future.get(); + if (!response->pose_stamped.empty()) + { + return response->pose_stamped.front().pose; + } + else + { + throw std::runtime_error("Empty response received from FK service."); + } + } + else + { + throw std::runtime_error("Error calling FK service."); + } +} diff --git a/motion_primitives_controllers/src/motion_primitives_base_controller.cpp b/motion_primitives_controllers/src/motion_primitives_base_controller.cpp new file mode 100644 index 0000000000..ef763f731e --- /dev/null +++ b/motion_primitives_controllers/src/motion_primitives_base_controller.cpp @@ -0,0 +1,207 @@ +// 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 + +#include "motion_primitives_controllers/motion_primitives_base_controller.hpp" +#include +#include +#include +#include +#include "controller_interface/helpers.hpp" + +namespace motion_primitives_controllers +{ +MotionPrimitivesBaseController::MotionPrimitivesBaseController() +: controller_interface::ControllerInterface() +{ +} + +controller_interface::CallbackReturn MotionPrimitivesBaseController::on_init() +{ + RCLCPP_DEBUG(get_node()->get_logger(), "Initializing Motion Primitives Base Controller"); + // needs to be implemented in derived classes + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MotionPrimitivesBaseController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_DEBUG(get_node()->get_logger(), "Configuring Motion Primitives Base Controller"); + // needs to be implemented in derived classes + RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +MotionPrimitivesBaseController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.reserve(25); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/motion_type"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q1"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q2"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q3"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q4"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q5"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q6"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_x"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_y"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_z"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qx"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qy"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qz"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qw"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_x"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_y"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_z"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qx"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qy"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qz"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qw"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/blend_radius"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/velocity"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/acceleration"); + command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/move_time"); + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +MotionPrimitivesBaseController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names.reserve(2); + state_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/execution_status"); + state_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/ready_for_new_primitive"); + return state_interfaces_config; +} + +controller_interface::CallbackReturn MotionPrimitivesBaseController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reset_command_interfaces(); + RCLCPP_DEBUG(get_node()->get_logger(), "Controller activated"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MotionPrimitivesBaseController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + reset_command_interfaces(); + RCLCPP_DEBUG(get_node()->get_logger(), "Controller deactivated"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type MotionPrimitivesBaseController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // This method should be implemented in derived classes + RCLCPP_ERROR(get_node()->get_logger(), "Update method not implemented in derived class"); + return controller_interface::return_type::ERROR; +} + +// Reset Command-Interfaces to nan +void MotionPrimitivesBaseController::reset_command_interfaces() +{ + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN())) + { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to reset command interface %ld", i); + } + } +} + +// Set command interfaces from the message, gets called in the update function +bool MotionPrimitivesBaseController::set_command_interfaces() +{ + // Get the oldest message from the queue + if (!moprim_queue_.pop(current_moprim_)) + { + RCLCPP_WARN(get_node()->get_logger(), "Failed to pop motion primitive from queue."); + return false; + } + + // Set the motion_type + (void)command_interfaces_[0].set_value(static_cast(current_moprim_.type)); + + // Process joint positions if available + if (!current_moprim_.joint_positions.empty()) + { + for (size_t i = 0; i < current_moprim_.joint_positions.size(); ++i) + { + (void)command_interfaces_[i + 1].set_value(current_moprim_.joint_positions[i]); // q1 to q6 + } + } + + // Process Cartesian poses if available + if (!current_moprim_.poses.empty()) + { + const auto & goal_pose = current_moprim_.poses[0].pose; // goal pose + (void)command_interfaces_[7].set_value(goal_pose.position.x); // pos_x + (void)command_interfaces_[8].set_value(goal_pose.position.y); // pos_y + (void)command_interfaces_[9].set_value(goal_pose.position.z); // pos_z + (void)command_interfaces_[10].set_value(goal_pose.orientation.x); // pos_qx + (void)command_interfaces_[11].set_value(goal_pose.orientation.y); // pos_qy + (void)command_interfaces_[12].set_value(goal_pose.orientation.z); // pos_qz + (void)command_interfaces_[13].set_value(goal_pose.orientation.w); // pos_qw + + // Process via poses if available (only for circular motion) + if (current_moprim_.type == MotionType::CIRCULAR_CARTESIAN && current_moprim_.poses.size() == 2) + { + const auto & via_pose = current_moprim_.poses[1].pose; // via pose + (void)command_interfaces_[14].set_value(via_pose.position.x); // pos_via_x + (void)command_interfaces_[15].set_value(via_pose.position.y); // pos_via_y + (void)command_interfaces_[16].set_value(via_pose.position.z); // pos_via_z + (void)command_interfaces_[17].set_value(via_pose.orientation.x); // pos_via_qx + (void)command_interfaces_[18].set_value(via_pose.orientation.y); // pos_via_qy + (void)command_interfaces_[19].set_value(via_pose.orientation.z); // pos_via_qz + (void)command_interfaces_[20].set_value(via_pose.orientation.w); // pos_via_qw + } + } + + (void)command_interfaces_[21].set_value(current_moprim_.blend_radius); // blend_radius + + // Read additional arguments + for (const auto & arg : current_moprim_.additional_arguments) + { + if (arg.name == "velocity") + { + (void)command_interfaces_[22].set_value(arg.value); + } + else if (arg.name == "acceleration") + { + (void)command_interfaces_[23].set_value(arg.value); + } + else if (arg.name == "move_time") + { + (void)command_interfaces_[24].set_value(arg.value); + } + else + { + RCLCPP_WARN(get_node()->get_logger(), "Unknown additional argument: %s", arg.name.c_str()); + } + } + return true; +} + +} // namespace motion_primitives_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + motion_primitives_controllers::MotionPrimitivesBaseController, + controller_interface::ControllerInterface) diff --git a/motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp b/motion_primitives_controllers/src/motion_primitives_forward_controller.cpp similarity index 63% rename from motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp rename to motion_primitives_controllers/src/motion_primitives_forward_controller.cpp index 480144441a..6c2b67468b 100644 --- a/motion_primitives_forward_controller/src/motion_primitives_forward_controller.cpp +++ b/motion_primitives_controllers/src/motion_primitives_forward_controller.cpp @@ -14,21 +14,11 @@ // // Authors: Mathias Fuhrer -#include "motion_primitives_forward_controller/motion_primitives_forward_controller.hpp" -#include -#include -#include -#include -#include "controller_interface/helpers.hpp" +#include "motion_primitives_controllers/motion_primitives_forward_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" -namespace motion_primitives_forward_controller +namespace motion_primitives_controllers { -MotionPrimitivesForwardController::MotionPrimitivesForwardController() -: controller_interface::ControllerInterface() -{ -} - controller_interface::CallbackReturn MotionPrimitivesForwardController::on_init() { RCLCPP_DEBUG(get_node()->get_logger(), "Initializing Motion Primitives Forward Controller"); @@ -47,9 +37,8 @@ controller_interface::CallbackReturn MotionPrimitivesForwardController::on_init( return controller_interface::CallbackReturn::SUCCESS; } - controller_interface::CallbackReturn MotionPrimitivesForwardController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State & previous_state) { RCLCPP_DEBUG(get_node()->get_logger(), "Configuring Motion Primitives Forward Controller"); @@ -65,69 +54,19 @@ controller_interface::CallbackReturn MotionPrimitivesForwardController::on_confi std::bind(&MotionPrimitivesForwardController::goal_cancelled_callback, this, _1), std::bind(&MotionPrimitivesForwardController::goal_accepted_callback, this, _1)); - RCLCPP_DEBUG(get_node()->get_logger(), "configure successful"); - return controller_interface::CallbackReturn::SUCCESS; -} - -controller_interface::InterfaceConfiguration -MotionPrimitivesForwardController::command_interface_configuration() const -{ - controller_interface::InterfaceConfiguration command_interfaces_config; - command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(25); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/motion_type"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q1"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q2"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q3"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q4"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q5"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/q6"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_x"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_y"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_z"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qx"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qy"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qz"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_qw"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_x"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_y"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_z"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qx"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qy"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qz"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/pos_via_qw"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/blend_radius"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/velocity"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/acceleration"); - command_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/move_time"); - return command_interfaces_config; -} - -controller_interface::InterfaceConfiguration -MotionPrimitivesForwardController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration state_interfaces_config; - state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(2); - state_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/execution_status"); - state_interfaces_config.names.push_back(tf_prefix_ + "motion_primitive/ready_for_new_primitive"); - return state_interfaces_config; + return MotionPrimitivesBaseController::on_configure(previous_state); } controller_interface::CallbackReturn MotionPrimitivesForwardController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State & previous_state) { - reset_command_interfaces(); - RCLCPP_DEBUG(get_node()->get_logger(), "Controller activated"); - return controller_interface::CallbackReturn::SUCCESS; + return MotionPrimitivesBaseController::on_activate(previous_state); } controller_interface::CallbackReturn MotionPrimitivesForwardController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State & previous_state) { - reset_command_interfaces(); - RCLCPP_DEBUG(get_node()->get_logger(), "Controller deactivated"); - return controller_interface::CallbackReturn::SUCCESS; + return MotionPrimitivesBaseController::on_deactivate(previous_state); } controller_interface::return_type MotionPrimitivesForwardController::update( @@ -159,7 +98,7 @@ controller_interface::return_type MotionPrimitivesForwardController::update( { case ExecutionState::IDLE: print_error_once_ = true; - was_executing_ = false; + // was_executing_ = false; break; case ExecutionState::EXECUTING: if (!was_executing_) @@ -280,91 +219,6 @@ controller_interface::return_type MotionPrimitivesForwardController::update( return controller_interface::return_type::OK; } -// Reset Command-Interfaces to nan -void MotionPrimitivesForwardController::reset_command_interfaces() -{ - for (size_t i = 0; i < command_interfaces_.size(); ++i) - { - if (!command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN())) - { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to reset command interface %ld", i); - } - } -} - -// Set command interfaces from the message, gets called in the update function -bool MotionPrimitivesForwardController::set_command_interfaces() -{ - // Get the oldest message from the queue - if (!moprim_queue_.pop(current_moprim_)) - { - RCLCPP_WARN(get_node()->get_logger(), "Failed to pop motion primitive from queue."); - return false; - } - - // Set the motion_type - (void)command_interfaces_[0].set_value(static_cast(current_moprim_.type)); - - // Process joint positions if available - if (!current_moprim_.joint_positions.empty()) - { - for (size_t i = 0; i < current_moprim_.joint_positions.size(); ++i) - { - (void)command_interfaces_[i + 1].set_value(current_moprim_.joint_positions[i]); // q1 to q6 - } - } - - // Process Cartesian poses if available - if (!current_moprim_.poses.empty()) - { - const auto & goal_pose = current_moprim_.poses[0].pose; // goal pose - (void)command_interfaces_[7].set_value(goal_pose.position.x); // pos_x - (void)command_interfaces_[8].set_value(goal_pose.position.y); // pos_y - (void)command_interfaces_[9].set_value(goal_pose.position.z); // pos_z - (void)command_interfaces_[10].set_value(goal_pose.orientation.x); // pos_qx - (void)command_interfaces_[11].set_value(goal_pose.orientation.y); // pos_qy - (void)command_interfaces_[12].set_value(goal_pose.orientation.z); // pos_qz - (void)command_interfaces_[13].set_value(goal_pose.orientation.w); // pos_qw - - // Process via poses if available (only for circular motion) - if (current_moprim_.type == MotionType::CIRCULAR_CARTESIAN && current_moprim_.poses.size() == 2) - { - const auto & via_pose = current_moprim_.poses[1].pose; // via pose - (void)command_interfaces_[14].set_value(via_pose.position.x); // pos_via_x - (void)command_interfaces_[15].set_value(via_pose.position.y); // pos_via_y - (void)command_interfaces_[16].set_value(via_pose.position.z); // pos_via_z - (void)command_interfaces_[17].set_value(via_pose.orientation.x); // pos_via_qx - (void)command_interfaces_[18].set_value(via_pose.orientation.y); // pos_via_qy - (void)command_interfaces_[19].set_value(via_pose.orientation.z); // pos_via_qz - (void)command_interfaces_[20].set_value(via_pose.orientation.w); // pos_via_qw - } - } - - (void)command_interfaces_[21].set_value(current_moprim_.blend_radius); // blend_radius - - // Read additional arguments - for (const auto & arg : current_moprim_.additional_arguments) - { - if (arg.name == "velocity") - { - (void)command_interfaces_[22].set_value(arg.value); - } - else if (arg.name == "acceleration") - { - (void)command_interfaces_[23].set_value(arg.value); - } - else if (arg.name == "move_time") - { - (void)command_interfaces_[24].set_value(arg.value); - } - else - { - RCLCPP_WARN(get_node()->get_logger(), "Unknown additional argument: %s", arg.name.c_str()); - } - } - return true; -} - rclcpp_action::GoalResponse MotionPrimitivesForwardController::goal_received_callback( const rclcpp_action::GoalUUID &, std::shared_ptr goal) { @@ -536,10 +390,10 @@ void MotionPrimitivesForwardController::goal_accepted_callback( get_node()->get_logger(), "Accepted goal with %zu motion primitives.", primitives.size()); } -} // namespace motion_primitives_forward_controller +} // namespace motion_primitives_controllers #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - motion_primitives_forward_controller::MotionPrimitivesForwardController, + motion_primitives_controllers::MotionPrimitivesForwardController, controller_interface::ControllerInterface) diff --git a/motion_primitives_forward_controller/src/motion_primitives_forward_controller_parameter.yaml b/motion_primitives_controllers/src/motion_primitives_forward_controller_parameter.yaml similarity index 100% rename from motion_primitives_forward_controller/src/motion_primitives_forward_controller_parameter.yaml rename to motion_primitives_controllers/src/motion_primitives_forward_controller_parameter.yaml diff --git a/motion_primitives_controllers/src/motion_primitives_from_trajectory_controller.cpp b/motion_primitives_controllers/src/motion_primitives_from_trajectory_controller.cpp new file mode 100644 index 0000000000..9cc7f96d12 --- /dev/null +++ b/motion_primitives_controllers/src/motion_primitives_from_trajectory_controller.cpp @@ -0,0 +1,642 @@ +// 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 + +#include "motion_primitives_controllers/motion_primitives_from_trajectory_controller.hpp" +#include +#include +#include +#include +#include "controller_interface/helpers.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +namespace motion_primitives_controllers +{ +controller_interface::CallbackReturn MotionPrimitivesFromTrajectoryController::on_init() +{ + RCLCPP_DEBUG( + get_node()->get_logger(), "Initializing Motion Primitives From Trajectory Controller"); + try + { + param_listener_ = + std::make_shared(get_node()); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Exception thrown during controller's init with message: %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn MotionPrimitivesFromTrajectoryController::on_configure( + const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_DEBUG( + get_node()->get_logger(), "Configuring Motion Primitives From Trajectory Controller"); + + params_ = param_listener_->get_params(); + tf_prefix_ = params_.tf_prefix; + + if (params_.local_joint_order.size() != 6) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Error: Exactly 6 joints must be provided in local_joint_order!"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.approximate_mode == "RDP_PTP") + { + approx_mode_ = ApproxMode::RDP_PTP; + } + else if (params_.approximate_mode == "RDP_LIN") + { + approx_mode_ = ApproxMode::RDP_LIN; + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), "Error: Unknown approximate mode '%s'", + params_.approximate_mode.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + use_time_not_vel_and_acc_ = params_.use_time_not_vel_and_acc; + epsilon_joint_angle_ = params_.epsilon_joint_angle; + epsilon_cart_position_ = params_.epsilon_cart_position; + epsilon_cart_angle_ = params_.epsilon_cart_angle; + blend_radius_percentage_ = params_.blend_radius_percentage; + blend_radius_lower_limit_ = params_.blend_radius_lower_limit; + blend_radius_upper_limit_ = params_.blend_radius_upper_limit; + joint_vel_overwrite_ = params_.joint_vel_overwrite; + joint_acc_overwrite_ = params_.joint_acc_overwrite; + cart_vel_overwrite_ = params_.cart_vel_overwrite; + cart_acc_overwrite_ = params_.cart_acc_overwrite; + blend_radius_overwrite_ = params_.blend_radius_overwrite; + + action_server_ = rclcpp_action::create_server( + get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(), + get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(), + std::string(get_node()->get_name()) + "/follow_joint_trajectory", + std::bind( + &MotionPrimitivesFromTrajectoryController::goal_received_callback, this, + std::placeholders::_1, std::placeholders::_2), + std::bind( + &MotionPrimitivesFromTrajectoryController::goal_cancelled_callback, this, + std::placeholders::_1), + std::bind( + &MotionPrimitivesFromTrajectoryController::goal_accepted_callback, this, + std::placeholders::_1)); + + planned_trajectory_publisher_ = + get_node()->create_publisher( + "~/planned_trajectory", rclcpp::QoS(1)); + planned_poses_publisher_ = + get_node()->create_publisher("~/planned_poses", rclcpp::QoS(1)); + motion_primitive_publisher_ = + get_node()->create_publisher( + "~/approximated_motion_primitives", rclcpp::QoS(1)); + + return MotionPrimitivesBaseController::on_configure(previous_state); +} + +controller_interface::CallbackReturn MotionPrimitivesFromTrajectoryController::on_activate( + const rclcpp_lifecycle::State & previous_state) +{ + fk_client_ = std::make_shared(); + + return MotionPrimitivesBaseController::on_activate(previous_state); +} + +controller_interface::CallbackReturn MotionPrimitivesFromTrajectoryController::on_deactivate( + const rclcpp_lifecycle::State & previous_state) +{ + action_server_.reset(); + fk_client_.reset(); + + return MotionPrimitivesBaseController::on_deactivate(previous_state); +} + +controller_interface::return_type MotionPrimitivesFromTrajectoryController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + if (cancel_requested_) + { + RCLCPP_INFO(get_node()->get_logger(), "Cancel requested, stopping execution."); + cancel_requested_ = false; + reset_command_interfaces(); + // send stop command immediately to the hw-interface + (void)command_interfaces_[0].set_value(static_cast(MotionHelperType::STOP_MOTION)); + // clear the queue (ignore return value) + static_cast(moprim_queue_.get_latest(current_moprim_)); + robot_stop_requested_ = true; + } + + // read the status from the state interface + auto opt_value_execution = state_interfaces_[0].get_optional(); + if (!opt_value_execution.has_value()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "State interface 0 (execution_state) returned no value."); + return controller_interface::return_type::ERROR; + } + execution_status_ = + static_cast(static_cast(std::round(opt_value_execution.value()))); + switch (execution_status_) + { + case ExecutionState::IDLE: + print_error_once_ = true; + break; + case ExecutionState::EXECUTING: + if (!was_executing_) + { + was_executing_ = true; + } + print_error_once_ = true; + break; + + case ExecutionState::SUCCESS: + print_error_once_ = true; + if (has_active_goal_ && was_executing_) + { + rt_goal_handle_.try_get( + [&](const std::shared_ptr & goal_handle) + { + was_executing_ = false; + auto result = std::make_shared(); + result->error_code = FollowJTrajAction::Result::SUCCESSFUL; + result->error_string = "Motion primitives executed successfully"; + goal_handle->setSucceeded(result); + has_active_goal_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Motion primitives executed successfully."); + }); + } + break; + + case ExecutionState::STOPPING: + RCLCPP_DEBUG(get_node()->get_logger(), "Execution state: STOPPING"); + print_error_once_ = true; + was_executing_ = false; + break; + + case ExecutionState::STOPPED: + print_error_once_ = true; + was_executing_ = false; + + if (has_active_goal_) + { + rt_goal_handle_.try_get( + [&](const std::shared_ptr & goal_handle) + { + auto result = std::make_shared(); + goal_handle->setCanceled(result); + has_active_goal_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Motion primitives execution canceled."); + }); + } + + if (robot_stop_requested_) + { + // If the robot was stopped by a stop command, reset the command interfaces + // to allow new motion primitives to be sent. + reset_command_interfaces(); + (void)command_interfaces_[0].set_value(static_cast(MotionHelperType::RESET_STOP)); + robot_stop_requested_ = false; + RCLCPP_INFO(get_node()->get_logger(), "Robot stopped, ready for new motion primitives."); + } + + break; + + case ExecutionState::ERROR: + was_executing_ = false; + if (print_error_once_) + { + RCLCPP_ERROR(get_node()->get_logger(), "Execution state: ERROR"); + print_error_once_ = false; + } + break; + + default: + RCLCPP_ERROR( + get_node()->get_logger(), "Error: Unknown execution status: %d", + static_cast(execution_status_)); + return controller_interface::return_type::ERROR; + } + + auto opt_value_ready = state_interfaces_[1].get_optional(); + if (!opt_value_ready.has_value()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "State interface 1 (ready_for_new_primitive) returned no value."); + return controller_interface::return_type::ERROR; + } + ready_for_new_primitive_ = + static_cast(static_cast(std::round(opt_value_ready.value()))); + + if (!cancel_requested_) + { + switch (ready_for_new_primitive_) + { + case ReadyForNewPrimitive::NOT_READY: + { + return controller_interface::return_type::OK; + } + case ReadyForNewPrimitive::READY: + { + if (moprim_queue_.empty()) // check if new command is available + { + return controller_interface::return_type::OK; + } + else + { + if (!set_command_interfaces()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Error: set_command_interfaces() failed"); + return controller_interface::return_type::ERROR; + } + return controller_interface::return_type::OK; + } + } + default: + RCLCPP_ERROR( + get_node()->get_logger(), "Error: Unknown state for ready_for_new_primitive: %d", + static_cast(ready_for_new_primitive_)); + return controller_interface::return_type::ERROR; + } + } + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse MotionPrimitivesFromTrajectoryController::goal_received_callback( + const rclcpp_action::GoalUUID &, std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received new action goal"); + + // Precondition: Running controller + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Can't accept new trajectories. Controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (has_active_goal_) + { + RCLCPP_WARN( + get_node()->get_logger(), "Already has an active goal, rejecting new goal request."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (robot_stop_requested_) + { + RCLCPP_WARN(get_node()->get_logger(), "Robot requested to stop. Discarding the new command."); + return rclcpp_action::GoalResponse::REJECT; + } + + // TODO(mathias31415): Check if queue has enough space? Number of primitives not known here? + + if (goal->trajectory.points.empty()) + { + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return rclcpp_action::GoalResponse::REJECT; + } + + RCLCPP_INFO(get_node()->get_logger(), "Accepted new action goal"); + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse MotionPrimitivesFromTrajectoryController::goal_cancelled_callback( + const std::shared_ptr>) +{ + RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal"); + cancel_requested_ = true; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void MotionPrimitivesFromTrajectoryController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Processing accepted goal ..."); + + auto trajectory_msg = + std::make_shared(goal_handle->get_goal()->trajectory); + + sort_to_local_joint_order(trajectory_msg); + + // Publish planned trajectory + planned_trajectory_publisher_->publish(*trajectory_msg); + + RCLCPP_INFO( + get_node()->get_logger(), "Received trajectory with %zu points.", + trajectory_msg->points.size()); + + const auto & joint_names = trajectory_msg->joint_names; + + geometry_msgs::msg::PoseArray planned_poses_msg; + planned_poses_msg.header.stamp = get_node()->now(); + planned_poses_msg.header.frame_id = "base"; + + std::vector planned_trajectory_data; + planned_trajectory_data.reserve(trajectory_msg->points.size()); + std::vector time_from_start; + time_from_start.reserve(planned_trajectory_data.size()); + for (const auto & point : trajectory_msg->points) + { + approx_primitives_with_rdp::PlannedTrajectoryPoint pt; + pt.time_from_start = point.time_from_start.sec + point.time_from_start.nanosec * 1e-9; + time_from_start.push_back(pt.time_from_start); + pt.joint_positions = point.positions; + try + { + pt.pose = fk_client_->computeFK(joint_names, point.positions, "base", "tool0"); + planned_poses_msg.poses.push_back(pt.pose); + RCLCPP_DEBUG( + get_node()->get_logger(), + "Tool0 pose: position (%.3f, %.3f, %.3f), orientation [%.3f, %.3f, %.3f, %.3f]", + pt.pose.position.x, pt.pose.position.y, pt.pose.position.z, pt.pose.orientation.x, + pt.pose.orientation.y, pt.pose.orientation.z, pt.pose.orientation.w); + } + catch (const std::exception & e) + { + RCLCPP_ERROR(get_node()->get_logger(), "FK-Error: %s", e.what()); + } + planned_trajectory_data.push_back(pt); + } + // Publish planned poses + planned_poses_publisher_->publish(planned_poses_msg); + + control_msgs::msg::MotionPrimitiveSequence motion_sequence; + switch (approx_mode_) + { + case ApproxMode::RDP_PTP: + { + RCLCPP_INFO( + get_node()->get_logger(), "Approximating motion primitives with PTP motion primitives."); + get_max_traj_joint_vel_and_acc(trajectory_msg, max_traj_joint_vel_, max_traj_joint_acc_); + RCLCPP_DEBUG( + get_node()->get_logger(), + "Max trajectory joint velocity: %.3f, Max trajectory joint acceleration: %.3f", + max_traj_joint_vel_, max_traj_joint_acc_); + RCLCPP_DEBUG( + get_node()->get_logger(), + "Joint velocity override: %.3f, Joint acceleration override: %.3f", joint_vel_overwrite_, + joint_acc_overwrite_); + if (joint_vel_overwrite_ > 0.0) + { + max_traj_joint_vel_ = joint_vel_overwrite_; + } + if (joint_acc_overwrite_ > 0.0) + { + max_traj_joint_acc_ = joint_acc_overwrite_; + } + motion_sequence = approxPtpPrimitivesWithRDP( + planned_trajectory_data, epsilon_joint_angle_, max_traj_joint_vel_, max_traj_joint_acc_, + use_time_not_vel_and_acc_, blend_radius_overwrite_, blend_radius_percentage_, + blend_radius_lower_limit_, blend_radius_upper_limit_); + break; + } + case ApproxMode::RDP_LIN: + { + RCLCPP_INFO( + get_node()->get_logger(), "Approximating motion primitives with LIN motion primitives."); + get_max_traj_cart_vel_and_acc( + planned_poses_msg, time_from_start, max_traj_cart_vel_, max_traj_cart_acc_); + RCLCPP_DEBUG( + get_node()->get_logger(), + "Max trajectory cartesian velocity: %.3f, Max trajectory cartesian acceleration: %.3f", + max_traj_cart_vel_, max_traj_cart_acc_); + RCLCPP_DEBUG( + get_node()->get_logger(), + "Cartesian velocity override: %.3f, Cartesian acceleration override: %.3f", + cart_vel_overwrite_, cart_acc_overwrite_); + if (cart_vel_overwrite_ > 0.0) + { + max_traj_cart_vel_ = cart_vel_overwrite_; + } + if (cart_acc_overwrite_ > 0.0) + { + max_traj_cart_acc_ = cart_acc_overwrite_; + } + motion_sequence = approxLinPrimitivesWithRDP( + planned_trajectory_data, epsilon_cart_position_, epsilon_cart_angle_, max_traj_cart_vel_, + max_traj_cart_acc_, use_time_not_vel_and_acc_, blend_radius_overwrite_, + blend_radius_percentage_, blend_radius_lower_limit_, blend_radius_upper_limit_); + break; + } + default: + RCLCPP_WARN(get_node()->get_logger(), "Unknown motion type."); + break; + } + // Publish approximated motion primitives + motion_primitive_publisher_->publish(motion_sequence); + + auto add_motions = [this](const control_msgs::msg::MotionPrimitiveSequence & moprim_sequence) + { + for (const auto & primitive : moprim_sequence.motions) + { + if (!moprim_queue_.push(primitive)) + { + RCLCPP_WARN(get_node()->get_logger(), "Failed to push motion primitive to queue."); + } + } + }; + + if (motion_sequence.motions.size() > 1) + { + MotionPrimitive start_marker; + start_marker.type = static_cast(MotionHelperType::MOTION_SEQUENCE_START); + if (!moprim_queue_.push(start_marker)) + { + RCLCPP_WARN( + get_node()->get_logger(), "Failed to push motion sequence start marker to queue."); + } + + add_motions(motion_sequence); + + MotionPrimitive end_marker; + end_marker.type = static_cast(MotionHelperType::MOTION_SEQUENCE_END); + if (!moprim_queue_.push(end_marker)) + { + RCLCPP_WARN(get_node()->get_logger(), "Failed to push motion sequence end marker to queue."); + } + } + else + { + add_motions(motion_sequence); + } + + auto rt_goal = std::make_shared(goal_handle); + rt_goal_handle_.set( + [&](auto & handle) + { + handle = rt_goal; + has_active_goal_ = true; + }); + + rt_goal->execute(); + + goal_handle_timer_.reset(); + goal_handle_timer_ = get_node()->create_wall_timer( + action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); + + RCLCPP_INFO( + get_node()->get_logger(), + "Reduced planned joint trajectory from %zu points to %zu motion primitives.", + trajectory_msg->points.size(), motion_sequence.motions.size()); +} + +void MotionPrimitivesFromTrajectoryController::get_max_traj_joint_vel_and_acc( + const std::shared_ptr & trajectory_msg, double & max_vel, + double & max_acc) +{ + max_vel = 0.0; + max_acc = 0.0; + + if (!trajectory_msg) + { + RCLCPP_WARN( + rclcpp::get_logger("MotionPrimitivesFromTrajectoryController"), + "Received null trajectory pointer in get_max_traj_joint_vel_and_acc()"); + return; + } + + for (const auto & point : trajectory_msg->points) + { + for (const auto & vel : point.velocities) + { + max_vel = std::max(max_vel, std::abs(vel)); + } + + for (const auto & acc : point.accelerations) + { + max_acc = std::max(max_acc, std::abs(acc)); + } + } +} + +void MotionPrimitivesFromTrajectoryController::get_max_traj_cart_vel_and_acc( + const geometry_msgs::msg::PoseArray & planned_poses_msg, + const std::vector & time_from_start, double & max_vel, double & max_acc) +{ + max_vel = 0.0; + max_acc = 0.0; + + const auto & poses = planned_poses_msg.poses; + size_t n = poses.size(); + + if (n < 2 || time_from_start.size() != n) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Invalid input: expected at least 2 poses and time_from_start of same size, got %zu poses " + "and %zu time values.", + n, time_from_start.size()); + return; + } + + std::vector translational_velocities; + + for (size_t i = 1; i < n; ++i) + { + const auto & p1 = poses[i - 1].position; + const auto & p2 = poses[i].position; + + double dt = time_from_start[i] - time_from_start[i - 1]; + if (dt <= 0.0) + { + RCLCPP_WARN(get_node()->get_logger(), "Invalid time difference between poses: %f", dt); + continue; + } + + double dx = p2.x - p1.x; + double dy = p2.y - p1.y; + double dz = p2.z - p1.z; + double dist = std::sqrt(dx * dx + dy * dy + dz * dz); + + double vel = dist / dt; + translational_velocities.push_back(vel); + + max_vel = std::max(max_vel, vel); + } + + for (size_t i = 1; i < translational_velocities.size(); ++i) + { + double dv = translational_velocities[i] - translational_velocities[i - 1]; + double dt = time_from_start[i + 1] - time_from_start[i]; + if (dt <= 0.0) continue; + + double acc = std::abs(dv / dt); + max_acc = std::max(max_acc, acc); + } +} + +void MotionPrimitivesFromTrajectoryController::sort_to_local_joint_order( + std::shared_ptr trajectory_msg) const +{ + std::vector mapping_vector = + mapping(trajectory_msg->joint_names, params_.local_joint_order); + + auto remap = [this]( + const std::vector & to_remap, + const std::vector & map_indices) -> std::vector + { + if (to_remap.empty()) return to_remap; + + if (to_remap.size() != map_indices.size()) + { + RCLCPP_WARN( + get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); + return to_remap; + } + + std::vector output(map_indices.size(), 0.0); + + for (size_t index = 0; index < map_indices.size(); ++index) + { + size_t map_index = map_indices[index]; + if (map_index < to_remap.size()) + { + output[index] = to_remap[map_index]; + } + } + + return output; + }; + + for (auto & point : trajectory_msg->points) + { + point.positions = remap(point.positions, mapping_vector); + + if (!point.velocities.empty()) point.velocities = remap(point.velocities, mapping_vector); + + if (!point.accelerations.empty()) + point.accelerations = remap(point.accelerations, mapping_vector); + + if (!point.effort.empty()) point.effort = remap(point.effort, mapping_vector); + } + + trajectory_msg->joint_names = params_.local_joint_order; +} + +} // namespace motion_primitives_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + motion_primitives_controllers::MotionPrimitivesFromTrajectoryController, + controller_interface::ControllerInterface) diff --git a/motion_primitives_controllers/src/motion_primitives_from_trajectory_controller_parameter.yaml b/motion_primitives_controllers/src/motion_primitives_from_trajectory_controller_parameter.yaml new file mode 100644 index 0000000000..0ce63ea1dd --- /dev/null +++ b/motion_primitives_controllers/src/motion_primitives_from_trajectory_controller_parameter.yaml @@ -0,0 +1,98 @@ +motion_primitives_from_trajectory_controller: + tf_prefix: { + type: string, + default_value: "", + description: "tf_prefix", + read_only: true, + } + local_joint_order: { + type: string_array, + default_value: [], + description: "Local joint order used internally by the controller.", + read_only: false, + validation: { + unique<>: null, + not_empty<>: null, + } + } + approximate_mode: { + type: string, + default_value: "RDP_PTP", + description: "Approximation mode for motion primitives: 'RDP_PTP' or 'RDP_LIN'.", + read_only: false, + validation: { + not_empty<>: [] + } + } + use_time_not_vel_and_acc: { + type: bool, + default_value: false, + description: "If true, uses time for motion primitives (e.g., UR); if false, uses velocity and acceleration (e.g., UR, KUKA).", + read_only: false + } + epsilon_joint_angle: { + type: double, + default_value: 0.0, + description: "Epsilon for RDP simplification in joint space (angle difference threshold).", + read_only: false + } + epsilon_cart_position: { + type: double, + default_value: 0.0, + description: "Epsilon for RDP simplification in Cartesian space (position threshold).", + read_only: false + } + epsilon_cart_angle: { + type: double, + default_value: 0.0, + description: "Epsilon for RDP simplification in Cartesian space (orientation angle threshold).", + read_only: false + } + blend_radius_percentage: { + type: double, + default_value: 0.1, + description: "Relative blend radius (percentage of the smaller distance to the previous or next point).", + read_only: false + } + blend_radius_lower_limit: { + type: double, + default_value: 0.01, + description: "Lower limit for the blend radius in meters.", + read_only: false + } + blend_radius_upper_limit: { + type: double, + default_value: 0.1, + description: "Upper limit for the blend radius in meters.", + read_only: false + } + blend_radius_overwrite: { + type: double, + default_value: -1.0, + description: "Blend radius overwrite for motion primitives.", + read_only: false + } + joint_vel_overwrite: { + type: double, + default_value: -1.0, + description: "Joint velocity overwrite for motion primitives.", + read_only: false + } + joint_acc_overwrite: { + type: double, + default_value: -1.0, + description: "Joint acceleration overwrite for motion primitives.", + read_only: false + } + cart_vel_overwrite: { + type: double, + default_value: -1.0, + description: "Cartesian velocity overwrite for motion primitives.", + read_only: false + } + cart_acc_overwrite: { + type: double, + default_value: -1.0, + description: "Cartesian acceleration overwrite for motion primitives.", + read_only: false + } diff --git a/motion_primitives_controllers/src/rdp.cpp b/motion_primitives_controllers/src/rdp.cpp new file mode 100644 index 0000000000..4f1f0c6274 --- /dev/null +++ b/motion_primitives_controllers/src/rdp.cpp @@ -0,0 +1,214 @@ +// 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 + +#include "motion_primitives_controllers/rdp.hpp" +#include +#include + +namespace rdp +{ + +// Computes the dot product of two vectors +double dot(const Point & a, const Point & b) +{ + double result = 0.0; + for (std::size_t i = 0; i < a.size(); ++i) result += a[i] * b[i]; + return result; +} + +// Computes the Euclidean norm (length) of a vector +double norm(const Point & v) { return std::sqrt(dot(v, v)); } + +// Subtracts two vectors +Point operator-(const Point & a, const Point & b) +{ + Point result(a.size()); + for (std::size_t i = 0; i < a.size(); ++i) result[i] = a[i] - b[i]; + return result; +} + +// Adds two vectors +Point operator+(const Point & a, const Point & b) +{ + Point result(a.size()); + for (std::size_t i = 0; i < a.size(); ++i) result[i] = a[i] + b[i]; + return result; +} + +// Multiplies a vector by a scalar +Point operator*(double scalar, const Point & v) +{ + Point result(v.size()); + for (std::size_t i = 0; i < v.size(); ++i) result[i] = scalar * v[i]; + return result; +} + +// Computes the perpendicular distance from a point to a line in N-dimensional space +double pointLineDistanceND(const Point & point, const Point & start, const Point & end) +{ + Point lineVec = end - start; + Point pointVec = point - start; + double len = norm(lineVec); + + if (len == 0.0) return norm(point - start); + + double projection = dot(pointVec, lineVec) / len; + Point closest = start + (projection / len) * lineVec; + return norm(point - closest); +} + +// Recursive implementation of the Ramer-Douglas-Peucker algorithm +std::pair> rdpRecursive( + const PointList & points, double epsilon, std::size_t offset) +{ + if (points.size() < 2) + { + std::vector indices; + for (std::size_t i = 0; i < points.size(); ++i) + { + indices.push_back(offset + i); + } + return {points, indices}; + } + + double dmax = 0.0; + std::size_t index = 0; + + // Find the point with the maximum distance from the line segment + for (std::size_t i = 1; i < points.size() - 1; ++i) + { + double d = pointLineDistanceND(points[i], points.front(), points.back()); + if (d > dmax) + { + index = i; + dmax = d; + } + } + + // If the max distance is greater than epsilon, recursively simplify + if (dmax > epsilon) + { + auto rec1 = + rdpRecursive(PointList(points.begin(), points.begin() + index + 1), epsilon, offset); + auto rec2 = + rdpRecursive(PointList(points.begin() + index, points.end()), epsilon, offset + index); + + // Remove duplicate in res1 + rec1.first.pop_back(); + rec1.second.pop_back(); + + // Merge + rec1.first.insert(rec1.first.end(), rec2.first.begin(), rec2.first.end()); + rec1.second.insert(rec1.second.end(), rec2.second.begin(), rec2.second.end()); + return rec1; + } + else + { + return {{points.front(), points.back()}, {offset, offset + points.size() - 1}}; + } +} + +// Compute angular difference (in radians) between two quaternions +double quaternionAngularDistance( + const geometry_msgs::msg::Quaternion & q1, const geometry_msgs::msg::Quaternion & q2) +{ + tf2::Quaternion tf_q1, tf_q2; + tf2::fromMsg(q1, tf_q1); + tf2::fromMsg(q2, tf_q2); + + tf_q1.normalize(); + tf_q2.normalize(); + + double dot = tf_q1.dot(tf_q2); + dot = std::clamp(dot, -1.0, 1.0); // numerical safety + + return 2.0 * std::acos(std::abs(dot)); // shortest angle between unit quaternions +} + +// Recursively apply the Ramer-Douglas-Peucker algorithm to a list of quaternions. +// Returns a simplified list of quaternions and their corresponding original indices. +std::pair, std::vector> rdpRecursiveQuaternion( + const std::vector & quaternions, double epsilon_angle_rad, + size_t offset) +{ + if (quaternions.size() < 2) + { + std::vector indices; + for (size_t i = 0; i < quaternions.size(); ++i) indices.push_back(offset + i); + return {quaternions, indices}; + } + + double max_angle = 0.0; + size_t max_index = 0; + + const geometry_msgs::msg::Quaternion & q_start = quaternions.front(); + const geometry_msgs::msg::Quaternion & q_end = quaternions.back(); + + // Check intermediate quaternions for deviation from SLERP curve + for (size_t i = 1; i < quaternions.size() - 1; ++i) + { + double t = static_cast(i) / static_cast(quaternions.size() - 1); + + // Interpolate on the SLERP curve between q_start and q_end + tf2::Quaternion tf_q_start, tf_q_end; + tf2::fromMsg(q_start, tf_q_start); + tf2::fromMsg(q_end, tf_q_end); + + tf_q_start.normalize(); + tf_q_end.normalize(); + + tf2::Quaternion tf_q_interp = tf_q_start.slerp(tf_q_end, t); + tf_q_interp.normalize(); + geometry_msgs::msg::Quaternion q_interp = tf2::toMsg(tf_q_interp); + + // Calculate angular distance to actual intermediate quaternion + double angle_diff = quaternionAngularDistance(q_interp, quaternions[i]); + + if (angle_diff > max_angle) + { + max_angle = angle_diff; + max_index = i; + } + } + + if (max_angle > epsilon_angle_rad) + { + auto left = rdpRecursiveQuaternion( + std::vector( + quaternions.begin(), quaternions.begin() + max_index + 1), + epsilon_angle_rad, offset); + + auto right = rdpRecursiveQuaternion( + std::vector( + quaternions.begin() + max_index, quaternions.end()), + epsilon_angle_rad, offset + max_index); + + // Avoid duplicate point + left.first.pop_back(); + left.second.pop_back(); + + // Merge results + left.first.insert(left.first.end(), right.first.begin(), right.first.end()); + left.second.insert(left.second.end(), right.second.begin(), right.second.end()); + return left; + } + else + { + return {{quaternions.front(), quaternions.back()}, {offset, offset + quaternions.size() - 1}}; + } +} + +} // namespace rdp diff --git a/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml b/motion_primitives_controllers/test/motion_primitives_forward_controller_params.yaml similarity index 100% rename from motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml rename to motion_primitives_controllers/test/motion_primitives_forward_controller_params.yaml diff --git a/motion_primitives_forward_controller/test/test_load_motion_primitives_forward_controller.cpp b/motion_primitives_controllers/test/test_load_motion_primitives_forward_controller.cpp similarity index 94% rename from motion_primitives_forward_controller/test/test_load_motion_primitives_forward_controller.cpp rename to motion_primitives_controllers/test/test_load_motion_primitives_forward_controller.cpp index 5024c67cad..29e87f6858 100644 --- a/motion_primitives_forward_controller/test/test_load_motion_primitives_forward_controller.cpp +++ b/motion_primitives_controllers/test/test_load_motion_primitives_forward_controller.cpp @@ -34,7 +34,7 @@ TEST(TestLoadMotionPrimitivesForwardController, load_controller) ASSERT_NO_THROW(cm.load_controller( "test_motion_primitives_forward_controller", - "motion_primitives_forward_controller/MotionPrimitivesForwardController")); + "motion_primitives_controllers/MotionPrimitivesForwardController")); rclcpp::shutdown(); } diff --git a/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp b/motion_primitives_controllers/test/test_motion_primitives_forward_controller.cpp similarity index 97% rename from motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp rename to motion_primitives_controllers/test/test_motion_primitives_forward_controller.cpp index e7729711fa..ee580ede32 100644 --- a/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.cpp +++ b/motion_primitives_controllers/test/test_motion_primitives_forward_controller.cpp @@ -141,9 +141,8 @@ TEST_F(MotionPrimitivesForwardControllerTest, receive_single_action_goal) EXPECT_EQ( command_values_[0], - static_cast( - motion_primitives_forward_controller::MotionType::LINEAR_JOINT)); // motion type - EXPECT_EQ(command_values_[1], 0.1); // q1 - q6 + static_cast(motion_primitives_controllers::MotionType::LINEAR_JOINT)); // motion type + EXPECT_EQ(command_values_[1], 0.1); // q1 - q6 EXPECT_EQ(command_values_[2], 0.2); EXPECT_EQ(command_values_[3], 0.3); EXPECT_EQ(command_values_[4], 0.4); diff --git a/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp b/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp similarity index 93% rename from motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp rename to motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp index 0f857b561d..eb66f636a6 100644 --- a/motion_primitives_forward_controller/test/test_motion_primitives_forward_controller.hpp +++ b/motion_primitives_controllers/test/test_motion_primitives_forward_controller.hpp @@ -32,7 +32,7 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "motion_primitives_forward_controller/motion_primitives_forward_controller.hpp" +#include "motion_primitives_controllers/motion_primitives_forward_controller.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" @@ -55,7 +55,7 @@ constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; // subclassing and friending so we can access member variables class TestableMotionPrimitivesForwardController -: public motion_primitives_forward_controller::MotionPrimitivesForwardController +: public motion_primitives_controllers::MotionPrimitivesForwardController { FRIEND_TEST(MotionPrimitivesForwardControllerTest, all_parameters_set_configure_success); FRIEND_TEST(MotionPrimitivesForwardControllerTest, activate_success); @@ -66,7 +66,7 @@ class TestableMotionPrimitivesForwardController controller_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override { - return motion_primitives_forward_controller::MotionPrimitivesForwardController::on_configure( + return motion_primitives_controllers::MotionPrimitivesForwardController::on_configure( previous_state); } }; @@ -157,8 +157,7 @@ class MotionPrimitivesForwardControllerFixture : public ::testing::Test auto goal_msg = ExecuteMotion::Goal(); MotionPrimitive primitive; - primitive.type = - static_cast(motion_primitives_forward_controller::MotionType::LINEAR_JOINT); + primitive.type = static_cast(motion_primitives_controllers::MotionType::LINEAR_JOINT); primitive.joint_positions = joint_positions; primitive.blend_radius = blend_radius; @@ -199,8 +198,8 @@ class MotionPrimitivesForwardControllerFixture : public ::testing::Test std::string interface_namespace_ = "motion_primitive"; std::array state_values_ = { - {static_cast(motion_primitives_forward_controller::ExecutionState::IDLE), - static_cast(motion_primitives_forward_controller::ReadyForNewPrimitive::READY)}}; + {static_cast(motion_primitives_controllers::ExecutionState::IDLE), + static_cast(motion_primitives_controllers::ReadyForNewPrimitive::READY)}}; std::array command_values_ = { {101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, 101.101, diff --git a/motion_primitives_controllers/userdoc.rst b/motion_primitives_controllers/userdoc.rst new file mode 100644 index 0000000000..b34999204e --- /dev/null +++ b/motion_primitives_controllers/userdoc.rst @@ -0,0 +1,7 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/motion_primitives_controllers/userdoc.rst + +.. _motion_primitives_controllers_userdoc: + + +.. include:: ../README.md + :parser: myst_parser.sphinx_ diff --git a/motion_primitives_forward_controller/README.md b/motion_primitives_forward_controller/README.md deleted file mode 100644 index e5e24bcbae..0000000000 --- a/motion_primitives_forward_controller/README.md +++ /dev/null @@ -1,68 +0,0 @@ -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/"` 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) diff --git a/motion_primitives_forward_controller/motion_primitives_forward_controller.xml b/motion_primitives_forward_controller/motion_primitives_forward_controller.xml deleted file mode 100644 index eab3a4113f..0000000000 --- a/motion_primitives_forward_controller/motion_primitives_forward_controller.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - MotionPrimitivesForwardController ros2_control controller. - - - diff --git a/motion_primitives_forward_controller/userdoc.rst b/motion_primitives_forward_controller/userdoc.rst deleted file mode 100644 index 23e4ff4e91..0000000000 --- a/motion_primitives_forward_controller/userdoc.rst +++ /dev/null @@ -1,7 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/motion_primitives_forward_controller/doc/userdoc.rst - -.. _motion_primitives_forward_controller_userdoc: - - -.. include:: ../README.md - :parser: myst_parser.sphinx_