diff --git a/motion_primitives_controllers/CMakeLists.txt b/motion_primitives_controllers/CMakeLists.txt index ec325fd2ee..2b2737cf0f 100644 --- a/motion_primitives_controllers/CMakeLists.txt +++ b/motion_primitives_controllers/CMakeLists.txt @@ -14,6 +14,10 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle realtime_tools std_srvs + moveit_msgs + geometry_msgs + tf2 + tf2_geometry_msgs ) find_package(ament_cmake REQUIRED) @@ -25,11 +29,18 @@ endforeach() generate_parameter_library(motion_primitives_forward_controller_parameters src/motion_primitives_forward_controller_parameter.yaml ) +generate_parameter_library(motion_primitives_from_trajectory_controller_parameters + src/motion_primitives_from_trajectory_controller_parameter.yaml +) add_library( ${PROJECT_NAME} SHARED src/motion_primitives_base_controller.cpp src/motion_primitives_forward_controller.cpp + src/motion_primitives_from_trajectory_controller.cpp + src/fk_client.cpp + src/rdp.cpp + src/approx_primitives_with_rdp.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ @@ -37,13 +48,18 @@ target_include_directories(${PROJECT_NAME} 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} ) @@ -58,6 +74,14 @@ install( RUNTIME DESTINATION bin ) +install( + TARGETS motion_primitives_from_trajectory_controller_parameters + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib diff --git a/motion_primitives_controllers/README.md b/motion_primitives_controllers/README.md index 60d5df25eb..f13e27cf01 100644 --- a/motion_primitives_controllers/README.md +++ b/motion_primitives_controllers/README.md @@ -3,24 +3,17 @@ motion_primitive_controllers Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC) -# Description -This project provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotionPrimitiveSequence.action` action from [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action). The controller receives the primitives via the action interface and forwards them through command interfaces to the robot-specific hardware interface. Currently, hardware interfaces for [Universal Robots](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and [KUKA Robots](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver) are implemented. +> [!WARNING] +> There is no guarantee that the motion defined by the motion primitive will actually be executed exactly as planned. In particular, for motions in Cartesian space such as LIN primitives, it is not necessarily ensured that the robot will execute the motion exactly in that way, since the inverse kinematics is not always unique. -- Supported motion primitives: - - `LINEAR_JOINT` - - `LINEAR_CARTESIAN` - - `CIRCULAR_CARTESIAN` - -If multiple motion primitives are passed to the controller via the action, the controller forwards them to the hardware interface as a sequence. To do this, it first sends `MOTION_SEQUENCE_START`, followed by each individual primitive, and finally `MOTION_SEQUENCE_END`. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives. +**This package contains two controllers:** +1. [motion_primitives_forward_controller](#moprim_forward_controller) +2. [motion_primitives_from_trajectory_controller](#moprim_from_traj_controller) -The action interface also allows stopping the current execution of motion primitives. When a stop request is received, the controller sends `STOP_MOTION` to the hardware interface, which then halts the robot's movement. Once the controller receives confirmation that the robot has stopped, it sends `RESET_STOP` to the hardware interface. After that, new commands can be sent. - -![Action Image](doc/Moprim_Controller_ExecuteMotion_Action.drawio.png) - -This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/examples/send_dummy_motion_primitives_ur10e.py) in the `Universal_Robots_ROS2_Driver` package. + ## 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. +Both 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: @@ -46,7 +39,23 @@ These interfaces are used to communicate the internal status of the hardware int - `STOPPED`: The robot was stopped using the `STOP_MOTION` command and must be reset with the `RESET_STOP` command before executing new commands. - `ready_for_new_primitive`: Boolean flag indicating whether the interface is ready to receive a new motion primitive -# Architecture Overview +# motion_primitives_forward_controller +This controller provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotionPrimitiveSequence.action` action from [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action). The controller receives the primitives via the action interface and forwards them through command interfaces to the robot-specific hardware interface. Currently, hardware interfaces for [Universal Robots](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and [KUKA Robots](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver) are implemented. + +- Supported motion primitives: + - `LINEAR_JOINT` + - `LINEAR_CARTESIAN` + - `CIRCULAR_CARTESIAN` + +If multiple motion primitives are passed to the controller via the action, the controller forwards them to the hardware interface as a sequence. To do this, it first sends `MOTION_SEQUENCE_START`, followed by each individual primitive, and finally `MOTION_SEQUENCE_END`. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives. + +The action interface also allows stopping the current execution of motion primitives. When a stop request is received, the controller sends `STOP_MOTION` to the hardware interface, which then halts the robot's movement. Once the controller receives confirmation that the robot has stopped, it sends `RESET_STOP` to the hardware interface. After that, new commands can be sent. + +![Action Image](doc/Moprim_Controller_ExecuteMotion_Action.drawio.png) + +This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/examples/send_dummy_motion_primitives_ur10e.py) in the `Universal_Robots_ROS2_Driver` package. + +## Architecture Overview Architecture for a UR robot with [`Universal_Robots_ROS2_Driver` in motion primitives mode](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver). ![UR Robot Architecture](doc/ros2_control_motion_primitives_ur.drawio.png) @@ -55,8 +64,49 @@ Architecture for a KUKA robot with [`kuka_eki_motion_primitives_hw_interface`](h ![KUKA Robot Architecture](doc/ros2_control_motion_primitives_kuka.drawio.png) -# Demo-Video with UR10e +## Demo-Video with UR10e [![UR demo video](doc/moprim_forward_controller_ur_demo_thumbnail.png)](https://youtu.be/SKz6LFvJmhQ) -# Demo-Video with KR3 +## Demo-Video with KR3 [![KUKA demo video](doc/moprim_forward_controller_kuka_demo_thumbnail.png)](https://youtu.be/_BWCO36j9bg) + + + +# 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 index b2e8b80816..455692c24d 100644 --- a/motion_primitives_controllers/controller_plugins.xml +++ b/motion_primitives_controllers/controller_plugins.xml @@ -5,4 +5,10 @@ MotionPrimitivesForwardController ros2_control controller. + + + MotionPrimitivesFromTrajectoryController ros2_control controller. + + 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_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_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_controllers/package.xml b/motion_primitives_controllers/package.xml index 716f4aa842..ec6dccfebb 100644 --- a/motion_primitives_controllers/package.xml +++ b/motion_primitives_controllers/package.xml @@ -31,6 +31,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_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_controllers/userdoc.rst b/motion_primitives_controllers/userdoc.rst index e199afa449..aa826e33f7 100644 --- a/motion_primitives_controllers/userdoc.rst +++ b/motion_primitives_controllers/userdoc.rst @@ -6,10 +6,16 @@ .. include:: README.md :parser: myst_parser.sphinx_ + Parameters ,,,,,,,,,,, -This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. +These controllers use the `generate_parameter_library `_ to handle their parameters. + +motion_primitives_forward_controller +------------------------------------ + +The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by this controller. .. generate_parameter_library_details:: src/motion_primitives_forward_controller_parameter.yaml @@ -17,3 +23,16 @@ An example parameter file for this controller can be found in `the test director .. literalinclude:: test/motion_primitives_forward_controller_params.yaml :language: yaml + + +motion_primitives_from_trajectory_controller +-------------------------------------------- + +The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by this controller. + +.. generate_parameter_library_details:: src/motion_primitives_from_trajectory_controller_parameter.yaml + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: test/motion_primitives_from_trajectory_controller_params.yaml + :language: yaml