diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos index 9ab8ff404..684626b92 100644 --- a/Universal_Robots_ROS2_Driver.rolling.repos +++ b/Universal_Robots_ROS2_Driver.rolling.repos @@ -1,8 +1,10 @@ repositories: Universal_Robots_Client_Library: type: git - url: https://github.com/UniversalRobots/Universal_Robots_Client_Library.git - version: master + url: https://github.com/urfeex/Universal_Robots_Client_Library.git + version: callback_lists + # url: https://github.com/UniversalRobots/Universal_Robots_Client_Library.git + # version: master Universal_Robots_ROS2_Description: type: git url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git @@ -17,16 +19,20 @@ repositories: version: master ros2_controllers: type: git - url: https://github.com/ros-controls/ros2_controllers - version: master + url: https://github.com/b-robotized-forks/ros2_controllers + version: motion_primitive_forward_controller + # url: https://github.com/ros-controls/ros2_controllers + # version: master kinematics_interface: type: git url: https://github.com/ros-controls/kinematics_interface.git version: master control_msgs: type: git - url: https://github.com/ros-controls/control_msgs.git - version: master + url: https://github.com/urfeex/control_msgs.git + version: motion_primitives + # url: https://github.com/ros-controls/control_msgs.git + # version: master control_toolbox: type: git url: https://github.com/ros-controls/control_toolbox.git diff --git a/ur_calibration/include/ur_calibration/calibration.hpp b/ur_calibration/include/ur_calibration/calibration.hpp index 6cfcc3f6b..08ea84528 100644 --- a/ur_calibration/include/ur_calibration/calibration.hpp +++ b/ur_calibration/include/ur_calibration/calibration.hpp @@ -44,8 +44,8 @@ #ifndef UR_CALIBRATION__CALIBRATION_HPP_ #define UR_CALIBRATION__CALIBRATION_HPP_ -#include #include +#include #include #include #include diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 1a76a5cab..15c810594 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -38,6 +38,8 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(ur_client_library REQUIRED) find_package(ur_dashboard_msgs REQUIRED) find_package(ur_msgs REQUIRED) +find_package(motion_primitives_forward_controller REQUIRED) +find_package(control_msgs REQUIRED) include_directories(include) @@ -55,12 +57,14 @@ target_link_libraries(ur_robot_driver_plugin PUBLIC ${tf2_geometry_msgs_TARGETS} ${ur_dashboard_msgs_TARGETS} ${ur_msgs_TARGETS} + ${control_msgs_TARGETS} controller_manager::controller_manager hardware_interface::hardware_interface pluginlib::pluginlib rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ur_client_library::urcl + motion_primitives_forward_controller::motion_primitives_forward_controller ) target_include_directories( ur_robot_driver_plugin @@ -176,10 +180,6 @@ install(DIRECTORY resources ) -install(DIRECTORY include/ - DESTINATION include -) - ament_export_dependencies( hardware_interface pluginlib @@ -188,10 +188,6 @@ ament_export_dependencies( controller_manager controller_manager_msgs geometry_msgs - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle std_msgs std_srvs tf2_geometry_msgs @@ -211,6 +207,7 @@ install(PROGRAMS scripts/start_ursim.sh examples/examples.py examples/force_mode.py + examples/send_dummy_motion_primitives_ur10e.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/ur_robot_driver/README_MotionPrimitive.md b/ur_robot_driver/README_MotionPrimitive.md new file mode 100644 index 000000000..9fe6fa973 --- /dev/null +++ b/ur_robot_driver/README_MotionPrimitive.md @@ -0,0 +1,145 @@ +hardware_interface in motion primitives mode +========================================== + +Hardware interface for executing motion primitives on a UR robot using the ROS 2 control framework. It allows the controller to execute linear (LINEAR_CARTESIAN/ LIN/ MOVEL), circular (CIRCULAR_CARTESIAN/ CIRC/ MOVEC), and joint-based (LINEAR_JOINT/ PTP/ MOVEJ) motion commands asynchronously and supports motion sequences for smooth trajectory execution. + +![Licence](https://img.shields.io/badge/License-BSD-3-Clause-blue.svg) + +# Demo Video with URSim +[![Play Video](doc/motion_primitive_ur_driver/motion_primitive_demo_video_preview.png)](https://www.youtube.com/watch?v=htUJtfkgr6Q) + +# Related packages/ repos +- [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action) +- [ros2_controllers with motion_primitives_forward_controller](https://github.com/b-robotized-forks/ros2_controllers/tree/motion_primitive_forward_controller/motion_primitives_forward_controller) +- [Universal_Robots_ROS2_Driver with motion_primitive_ur_driver](https://github.com/b-robotized-forks/Universal_Robots_ROS2_Driver_MotionPrimitive) +- [Universal_Robots_Client_Library](https://github.com/UniversalRobots/Universal_Robots_Client_Library) + + +# Architecture + +![Architecture Overview](doc/motion_primitive_ur_driver/ros2_control_motion_primitives_ur.drawio.png) + +# Command and State Interfaces + +In motion primitives mode, the following state and command interfaces are used to enable communication between the controller and the hardware interface. + +## 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`](https://github.com/b-robotized-forks/ros2_controllers/tree/motion_primitive_forward_controller/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 + - `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 + +In addition to these, the driver also provides all standard state interfaces from the original UR hardware interface (e.g., joint positions, velocities). These are used by components like the `joint_state_broadcaster` and allow tools like RViz to visualize the current robot state. + + +# Supported Motion Primitives + +- Support for basic motion primitives: + - `LINEAR_JOINT` + - `LINEAR_CARTESIAN` + - `CIRCULAR_CARTESIAN` +- Additional helper types: + - `STOP_MOTION`: Immediately stops the current robot motion and clears all pending primitives in the controller's queue. + - `RESET_STOP`: After `RESET_STOP`, new commands can get handled. + - `MOTION_SEQUENCE_START` / `MOTION_SEQUENCE_END`: Define a motion sequence block. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives. + +![MotionPrimitiveExecutionWithHelperTypes](doc/motion_primitive_ur_driver/MotionPrimitiveExecutionWithHelperTypes_UR.drawio.png) + +# Overview + +In contrast to the standard UR hardware interface, this driver does not compute or execute trajectories on the ROS 2 side. Instead, it passes high-level motion primitives directly to the robot controller, which then computes and executes the trajectory internally. + +This approach offers two key advantages: + +- **Reduced real-time requirements** on the ROS 2 side, since trajectory planning and execution are offloaded to the robot. +- **Improved motion quality**, as the robot controller has better knowledge of the robot's kinematics and dynamics, leading to more optimized and accurate motion execution. + + +## write() Method + +In motion primitives mode, the `write()` method checks whether a new motion primitive command has been received from the controller via the command interfaces. If a new command is present: + +1. If the command is `STOP_MOTION`, a flag is set which leads to interrupting the current motion inside the `asyncStopMotionThread()`. If the command is `RESET_STOP`, the flag is reset, and new motion primitives can be received and executed. +2. For other commands, they are passed to the `asyncCommandThread()` and executed asynchronously. Individual primitives are executed directly via the Instruction Executor. +If a `MOTION_SEQUENCE_START` command is received, all subsequent primitives are added to a motion sequence. Once `MOTION_SEQUENCE_END` is received, the entire sequence is executed via the Instruction Executor. + +Threading is required since calls to the Instruction Executor are blocking. Offloading these to separate threads ensures the control loop remains responsive during motion execution. The stopping functionality is also threaded to allow interrupting a primitive even during execution or in a motion sequence. + +## read() Method + +The `read()` method: + +- Publishes the `execution_status` over a state interface with possible values: `IDLE`, `EXECUTING`, `SUCCESS`, `ERROR`, `STOPPED`. +- Publishes `ready_for_new_primitive` over a state interface to signal whether the interface is ready to receive a new primitive. +- Handles additional state interfaces from the UR driver, such as joint states, enabling RViz to visualize the current robot pose. + + +# Example usage notes with UR10e +## (optional) URSim +Start UR-Sim according to the [Universal Robots ROS 2 Driver Documentation](https://docs.universal-robots.com/Universal_Robots_ROS2_Documentation/doc/ur_client_library/doc/setup/ursim_docker.html) or the [Documentation for universalrobots/ursim_e-series docker container](https://hub.docker.com/r/universalrobots/ursim_e-series) +``` +ros2 run ur_client_library start_ursim.sh -m ur10e +``` +Remote control needs to be enabled: +https://robodk.com/doc/en/Robots-Universal-Robots-How-enable-Remote-Control-URe.html + +## Launch hardware_interface with motion_primitives_ur_driver +With URSim: +``` +ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=192.168.56.101 launch_rviz:=true headless_mode:=true initial_joint_controller:=motion_primitive_forward_controller +``` +With H-KA UR10e: +``` +ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=192.168.1.102 launch_rviz:=true headless_mode:=true initial_joint_controller:=motion_primitive_forward_controller +``` +## (optional) switching control mode +``` +ros2 control switch_controllers --activate motion_primitive_forward_controller --deactivate scaled_joint_trajectory_controller +``` +## Send motion primitives +> [!WARNING] +> Ensure that the robot in your configuration is able to execute these motion primitives without any risk of collision. +``` +ros2 run ur_robot_driver send_dummy_motion_primitives_ur10e.py +``` +During the execution of the motion primitives, the movement can be stopped by pressing the Enter key in the terminal. + +# TODOs/ Improvements +- if trajectory is finished while `instruction_executer->cancelMotion()` is called --> returns with execution_status ERROR --> no new command can be sent to hw-interface --> need to call `instruction_executer->cancelMotion()` a second time +- The default `hardware_interface` implementation and the `InstructionExecutor` used to execute motion primitives both rely on a callback function that is triggered when a trajectory is completed. In the current implementation, the callback function of the `ur_driver` is overwritten, meaning that only one of the callback functions can be active at a time. This issue has been addressed by registering the `InstructionExecutor`'s callback when motion primitives mode is activated, and restoring the `hardware_interface`'s callback when the mode is deactivated. To enable this, a method `registerTrajDoneCallback()` was added to the `InstructionExecutor` in the `ur_client_library`: +```cpp + void urcl::InstructionExecutor::registerTrajDoneCallback() + { + driver_->registerTrajectoryDoneCallback( + std::bind(&InstructionExecutor::trajDoneCallback, this, std::placeholders::_1)); + } +``` +--> Is there a better solution for this? + +# Useful sources +- https://docs.universal-robots.com/Universal_Robots_ROS_Documentation/doc/ur_client_library/doc/architecture/instruction_executor.html +- https://docs.universal-robots.com/Universal_Robots_ROS_Documentation/doc/ur_client_library/doc/examples/instruction_executor.html +- https://rtw.b-robotized.com/master/use-cases/ros_workspaces/aliases.html +- https://control.ros.org/master/doc/ros2_control/ros2controlcli/doc/userdoc.html +- ... diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index cf3a9984c..227ef52f4 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -50,6 +50,9 @@ controller_manager: # true. enforce_command_limits: false + motion_primitive_forward_controller: + type: motion_primitives_forward_controller/MotionPrimitivesForwardController + speed_scaling_state_broadcaster: ros__parameters: state_publish_rate: 100.0 @@ -187,3 +190,36 @@ tcp_pose_broadcaster: tool_contact_controller: ros__parameters: tf_prefix: "$(var tf_prefix)" + + +motion_primitive_forward_controller: + ros__parameters: + command_interfaces: + - motion_type + - q1 + - q2 + - q3 + - q4 + - q5 + - q6 + - pos_x + - pos_y + - pos_z + - pos_qx + - pos_qy + - pos_qz + - pos_qw + - pos_via_x + - pos_via_y + - pos_via_z + - pos_via_qx + - pos_via_qy + - pos_via_qz + - pos_via_qw + - blend_radius + - velocity + - acceleration + - move_time + state_interfaces: + - execution_status + - ready_for_new_primitive diff --git a/ur_robot_driver/doc/motion_primitive_ur_driver/MotionPrimitiveExecutionWithHelperTypes_UR.drawio.png b/ur_robot_driver/doc/motion_primitive_ur_driver/MotionPrimitiveExecutionWithHelperTypes_UR.drawio.png new file mode 100644 index 000000000..44cc008fd Binary files /dev/null and b/ur_robot_driver/doc/motion_primitive_ur_driver/MotionPrimitiveExecutionWithHelperTypes_UR.drawio.png differ diff --git a/ur_robot_driver/doc/motion_primitive_ur_driver/motion_primitive_demo_video_preview.png b/ur_robot_driver/doc/motion_primitive_ur_driver/motion_primitive_demo_video_preview.png new file mode 100644 index 000000000..3be26fee0 Binary files /dev/null and b/ur_robot_driver/doc/motion_primitive_ur_driver/motion_primitive_demo_video_preview.png differ diff --git a/ur_robot_driver/doc/motion_primitive_ur_driver/ros2_control_motion_primitives_ur.drawio.png b/ur_robot_driver/doc/motion_primitive_ur_driver/ros2_control_motion_primitives_ur.drawio.png new file mode 100644 index 000000000..0c7962c05 Binary files /dev/null and b/ur_robot_driver/doc/motion_primitive_ur_driver/ros2_control_motion_primitives_ur.drawio.png differ diff --git a/ur_robot_driver/examples/send_dummy_motion_primitives_ur10e.py b/ur_robot_driver/examples/send_dummy_motion_primitives_ur10e.py new file mode 100755 index 000000000..81ff23065 --- /dev/null +++ b/ur_robot_driver/examples/send_dummy_motion_primitives_ur10e.py @@ -0,0 +1,331 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2025, b»robotized +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Authors: Mathias Fuhrer + +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from geometry_msgs.msg import PoseStamped +from control_msgs.msg import MotionPrimitive, MotionArgument, MotionPrimitiveSequence +from control_msgs.action import ExecuteMotionPrimitiveSequence +from action_msgs.srv import CancelGoal +from action_msgs.msg import GoalStatus +import threading +import sys + +joint_velocity = 1.0 +joint_acceleration = 1.0 +cart_velocity = 0.5 +cart_acceleration = 0.5 +move_time = 0.0 # if move_time=0 vel and acc are used, otherwise move_time is used + +# Joint movement to home position +moveJ_1 = MotionPrimitive() +moveJ_1.type = MotionPrimitive.LINEAR_JOINT +moveJ_1.joint_positions = [1.57, -1.57, 1.57, -1.57, -1.57, -1.57] +moveJ_1.blend_radius = 0.1 +moveJ_1.additional_arguments = [ + MotionArgument(argument_name="velocity", argument_value=joint_velocity), + MotionArgument(argument_name="acceleration", argument_value=joint_acceleration), + MotionArgument(argument_name="move_time", argument_value=move_time), +] +# Linear movement down +moveL_1 = MotionPrimitive() +moveL_1.type = MotionPrimitive.LINEAR_CARTESIAN +moveL_1.blend_radius = 0.05 +moveL_1.additional_arguments = [ + MotionArgument(argument_name="velocity", argument_value=cart_velocity), + MotionArgument(argument_name="acceleration", argument_value=cart_acceleration), +] +pose_L1 = PoseStamped() +pose_L1.pose.position.x = 0.174 +pose_L1.pose.position.y = -0.692 +pose_L1.pose.position.z = 0.3 +pose_L1.pose.orientation.x = 1.0 +pose_L1.pose.orientation.y = 0.0 +pose_L1.pose.orientation.z = 0.0 +pose_L1.pose.orientation.w = 0.0 +moveL_1.poses = [pose_L1] + +# Linear movement up +moveL_2 = MotionPrimitive() +moveL_2.type = MotionPrimitive.LINEAR_CARTESIAN +moveL_2.blend_radius = 0.05 +moveL_2.additional_arguments = moveL_1.additional_arguments.copy() +pose_L2 = PoseStamped() +pose_L2.pose.position.x = 0.174 +pose_L2.pose.position.y = -0.692 +pose_L2.pose.position.z = 0.7 +pose_L2.pose.orientation.x = 1.0 +pose_L2.pose.orientation.y = 0.0 +pose_L2.pose.orientation.z = 0.0 +pose_L2.pose.orientation.w = 0.0 +moveL_2.poses = [pose_L2] + +# Joint movement +moveJ_2 = MotionPrimitive() +moveJ_2.type = MotionPrimitive.LINEAR_JOINT +moveJ_2.blend_radius = 0.1 +moveJ_2.joint_positions = [0.9, -1.57, 1.57, -1.57, -1.57, -1.57] +moveJ_2.additional_arguments = moveJ_1.additional_arguments.copy() + +# Circular movement +moveC_1 = MotionPrimitive() +moveC_1.type = MotionPrimitive.CIRCULAR_CARTESIAN +moveC_1.blend_radius = 0.0 +moveC_1.additional_arguments = moveL_1.additional_arguments.copy() +pose_C1_via = PoseStamped() +pose_C1_via.pose.position.x = 0.174 +pose_C1_via.pose.position.y = -0.9 +pose_C1_via.pose.position.z = 0.7 +pose_C1_via.pose.orientation.x = 1.0 +pose_C1_via.pose.orientation.y = 0.0 +pose_C1_via.pose.orientation.z = 0.0 +pose_C1_via.pose.orientation.w = 0.0 +pose_C1_goal = PoseStamped() +pose_C1_goal.pose.position.x = 0.5 +pose_C1_goal.pose.position.y = -0.692 +pose_C1_goal.pose.position.z = 0.7 +pose_C1_goal.pose.orientation.x = 1.0 +pose_C1_goal.pose.orientation.y = 0.0 +pose_C1_goal.pose.orientation.z = 0.0 +pose_C1_goal.pose.orientation.w = 0.0 +moveC_1.poses = [pose_C1_goal, pose_C1_via] # first pose is goal, second is via point + +# Motions to compare moprim and jtc mode (send_joint_positions.py for jtc mode) +eval_blend_radius = 0.0 +eval_move_time = 1.0 + +moveJ_eval_0 = MotionPrimitive() +moveJ_eval_0.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_0.joint_positions = [1.57, -1.57, 1.57, -1.57, -1.57, -1.57] +moveJ_eval_0.blend_radius = eval_blend_radius +moveJ_eval_0.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] +moveJ_eval_1 = MotionPrimitive() +moveJ_eval_1.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_1.joint_positions = [1.57, -1.1, 1.0, -1.57, -1.57, -1.57] +moveJ_eval_1.blend_radius = eval_blend_radius +moveJ_eval_1.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] +moveJ_eval_2 = MotionPrimitive() +moveJ_eval_2.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_2.joint_positions = [2.0, -0.9, 0.7, -1.57, -1.57, -1.57] +moveJ_eval_2.blend_radius = eval_blend_radius +moveJ_eval_2.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] +moveJ_eval_3 = MotionPrimitive() +moveJ_eval_3.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_3.joint_positions = [2.4, -1.57, 1.57, -1.57, -1.57, -1.57] +moveJ_eval_3.blend_radius = eval_blend_radius +moveJ_eval_3.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] +moveJ_eval_4 = MotionPrimitive() +moveJ_eval_4.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_4.joint_positions = [1.57, -1.57, 1.57, -1.57, -1.57, -1.57] +moveJ_eval_4.blend_radius = eval_blend_radius +moveJ_eval_4.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] +moveJ_eval_5 = MotionPrimitive() +moveJ_eval_5.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_5.joint_positions = [1.57, -1.1, 1.0, -1.57, -1.57, -1.57] +moveJ_eval_5.blend_radius = eval_blend_radius +moveJ_eval_5.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] +moveJ_eval_6 = MotionPrimitive() +moveJ_eval_6.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_6.joint_positions = [1.1, -0.9, 0.7, -1.57, -1.57, -1.57] +moveJ_eval_6.blend_radius = eval_blend_radius +moveJ_eval_6.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] +moveJ_eval_7 = MotionPrimitive() +moveJ_eval_7.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_7.joint_positions = [0.7, -1.57, 1.57, -1.57, -1.57, -1.57] +moveJ_eval_7.blend_radius = eval_blend_radius +moveJ_eval_7.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] +moveJ_eval_8 = MotionPrimitive() +moveJ_eval_8.type = MotionPrimitive.LINEAR_JOINT +moveJ_eval_8.joint_positions = [1.57, -1.57, 1.57, -1.57, -1.57, -1.57] +moveJ_eval_8.blend_radius = eval_blend_radius +moveJ_eval_8.additional_arguments = [ + MotionArgument(argument_name="move_time", argument_value=eval_move_time), +] + + +class ExecuteMotionClient(Node): + def __init__(self): + super().__init__("motion_sequence_client") + + # Initialize action client for ExecuteMotionPrimitiveSequence action + self._client = ActionClient( + self, + ExecuteMotionPrimitiveSequence, + "/motion_primitive_forward_controller/motion_sequence", + ) + + # Initialize client for cancel_goal service + self._cancel_client = self.create_client( + CancelGoal, "/motion_primitive_forward_controller/motion_sequence/_action/cancel_goal" + ) + + self._goal_id = None # To store the goal ID for cancellation + + # Send the motion goal + self._send_goal() + + # Start a thread to listen for ENTER key press to cancel the goal + thread = threading.Thread(target=self._wait_for_keypress, daemon=True) + thread.start() + + def _send_goal(self): + """Send the motion sequence goal to the action server.""" + self.get_logger().info("Waiting for action server...") + self._client.wait_for_server() + + goal_msg = ExecuteMotionPrimitiveSequence.Goal() + goal_msg.trajectory = MotionPrimitiveSequence() + + # "pick" sequence with moveC in the end + goal_msg.trajectory.motions = [moveJ_1, moveL_1, moveL_2, moveJ_2, moveC_1] + + # evaluation sequence with moveJ movements + # goal_msg.trajectory.motions = [ + # moveJ_eval_0, + # moveJ_eval_1, + # moveJ_eval_2, + # moveJ_eval_3, + # moveJ_eval_4, + # moveJ_eval_5, + # moveJ_eval_6, + # moveJ_eval_7, + # moveJ_eval_8, + # ] + + self.get_logger().info( + f"Sending {len(goal_msg.trajectory.motions)} motion primitives as a sequence..." + ) + send_goal_future = self._client.send_goal_async( + goal_msg, feedback_callback=self.feedback_callback + ) + send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + """Callback called when the action server accepts or rejects the goal.""" + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().error("Goal rejected") + return + + self.get_logger().info("Goal accepted") + self._goal_id = goal_handle.goal_id # Store goal ID for cancellation + + # Wait for result asynchronously + get_result_future = goal_handle.get_result_async() + get_result_future.add_done_callback(self.result_callback) + + def feedback_callback(self, feedback_msg): + """Receive feedback about the current motion primitive being executed.""" + current_index = feedback_msg.feedback.current_primitive_index + self.get_logger().info(f"Executing primitive index: {current_index}") + + def result_callback(self, future): + """Handle the result from the action server after goal finishes or is canceled.""" + result = future.result() + status = result.status + + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info("Motion sequence executed successfully!") + elif status == GoalStatus.STATUS_CANCELED: + self.get_logger().info("Motion sequence was canceled.") + elif status == GoalStatus.STATUS_ABORTED: + self.get_logger().error("Motion sequence execution failed.") + else: + self.get_logger().error(f"Execution ended with status: {status}") + + rclpy.shutdown() + + def _wait_for_keypress(self): + """Wait for the user to press ENTER key to cancel the motion sequence.""" + print("Press ENTER to cancel the motion sequence...") + while True: + input_str = sys.stdin.readline().strip() + if input_str == "": + self.get_logger().info("ENTER key pressed: sending cancel request.") + self.cancel_goal() + break + + def cancel_goal(self): + """Send a cancel request for the currently running goal.""" + if self._goal_id is None: + self.get_logger().warn("No goal to cancel (goal_id not set yet).") + return + + if not self._cancel_client.wait_for_service(timeout_sec=2.0): + self.get_logger().error("Cancel service is not available.") + return + + request = CancelGoal.Request() + request.goal_info.goal_id = self._goal_id + + future = self._cancel_client.call_async(request) + future.add_done_callback(self.cancel_response_callback) + + def cancel_response_callback(self, future): + """Handle the response from the cancel service call.""" + try: + response = future.result() + if response.return_code == 0: + self.get_logger().info("Cancel request accepted.") + elif response.return_code == 1: + self.get_logger().warn("Cancel request rejected.") + else: + self.get_logger().warn(f"Cancel returned code: {response.return_code}") + except Exception as e: + self.get_logger().error(f"Failed to call cancel service: {e}") + + +def main(args=None): + rclpy.init(args=args) + node = ExecuteMotionClient() + rclpy.spin(node) + + +if __name__ == "__main__": + main() diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index e6cc00c86..f4e2c1d09 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -34,6 +34,9 @@ * \author Marvin Große Besselmann grosse@fzi.de * \date 2019-04-11 * + * \author Mathias Fuhrer mathias.fuhrer@b-robotized.de + * \date 2025-05-28 – Added support for usage with motion_primitives_forward_controller + * */ //---------------------------------------------------------------------- #ifndef UR_ROBOT_DRIVER__HARDWARE_INTERFACE_HPP_ @@ -53,6 +56,7 @@ // UR stuff #include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/ur/instruction_executor.h" #include "ur_client_library/ur/robot_receive_timeout.h" #include "ur_robot_driver/dashboard_client_ros.hpp" #include "ur_dashboard_msgs/msg/robot_mode.hpp" @@ -64,6 +68,13 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +// Motion primitives controller +#include "motion_primitives_forward_controller/motion_primitives_forward_controller.hpp" +#include "control_msgs/msg/motion_primitive.hpp" + +using MoprimMotionType = control_msgs::msg::MotionPrimitive; +using MoprimMotionHelperType = motion_primitives_forward_controller::MotionHelperType; + namespace ur_robot_driver { enum class PausingState @@ -82,6 +93,7 @@ enum StoppingInterface STOP_FORCE_MODE, STOP_FREEDRIVE, STOP_TOOL_CONTACT, + STOP_MOTION_PRIMITIVES, }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -274,6 +286,52 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double force_mode_damping_; double force_mode_gain_scaling_; + //*************** Motion primitives stuff *************** + std::shared_ptr instruction_executor_; + + // Async thread handling + std::shared_ptr async_moprim_cmd_thread_; + std::shared_ptr async_moprim_stop_thread_; + std::atomic_bool async_moprim_thread_shutdown_; + std::mutex moprim_cmd_mutex_; + std::mutex moprim_stop_mutex_; + + // Command buffer for thread-safe communication + std::vector pending_moprim_cmd_; + std::atomic_bool new_moprim_cmd_available_; + std::atomic_bool new_moprim_stop_available_; + std::atomic_bool new_moprim_reset_available_; + + // Status for communication with controller + bool motion_primitives_forward_controller_running_; + using MoprimExecutionState = motion_primitives_forward_controller::ExecutionState; + MoprimExecutionState current_moprim_execution_status_; + std::atomic_bool ready_for_new_moprim_; + + // Command and state interfaces for the motion primitives + std::vector hw_moprim_commands_; + std::vector hw_moprim_states_; + + // flag to put all following primitives into a motion sequence instead of sending single primitives + std::atomic_bool build_moprim_sequence_{ false }; + std::vector> moprim_sequence_; + + void handleMoprimCommands(); + void resetMoprimCmdInterfaces(); + void asyncMoprimCmdThread(); + void asyncMoprimStopThread(); + void processMoprimMotionCmd(const std::vector& command); + void processMoprimStopCmd(); + void processMoprimResetCmd(); + bool getMoprimTimeOrVelAndAcc(const std::vector& command, double& velocity, double& acceleration, + double& move_time); + bool getMoprimVelAndAcc(const std::vector& command, double& velocity, double& acceleration, + double& move_time); + void quaternionToRotVec(double qx, double qy, double qz, double qw, double& rx, double& ry, double& rz); + + const std::string HW_IF_MOTION_PRIMITIVES = "motion_primitive"; + //*************** End Motion primitives stuff *************** + // copy of non double values std::array actual_dig_out_bits_copy_; std::array actual_dig_in_bits_copy_; @@ -307,7 +365,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool velocity_controller_running_; bool force_mode_controller_running_ = false; - std::unique_ptr ur_driver_; + std::shared_ptr ur_driver_; // changed to shared_ptr for instruction_executer std::shared_ptr async_thread_; std::atomic_bool rtde_comm_has_been_started_ = false; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 528da2ee7..71763f50b 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -28,6 +28,8 @@ # # Author: Denis Stogl +# +# Author modifications: Mathias Fuhrer from launch import LaunchDescription from launch.actions import ( @@ -191,6 +193,7 @@ def controller_spawner(controllers, active=True): "passthrough_trajectory_controller", "freedrive_mode_controller", "tool_contact_controller", + "motion_primitive_forward_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) @@ -323,7 +326,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "headless_mode", - default_value="false", + default_value="true", description="Enable headless mode for robot control", ) ) @@ -345,6 +348,7 @@ def generate_launch_description(): "forward_position_controller", "freedrive_mode_controller", "passthrough_trajectory_controller", + "motion_primitive_forward_controller", ], description="Initially loaded robot controller.", ) diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml index 57f60170c..860047cf4 100644 --- a/ur_robot_driver/package.xml +++ b/ur_robot_driver/package.xml @@ -25,6 +25,8 @@ Felix Exner Lea Steffen Tristan Schnell + Mathias Fuhrer + ament_cmake ament_cmake_python @@ -46,6 +48,8 @@ ur_dashboard_msgs ur_description ur_msgs + motion_primitives_forward_controller + control_msgs force_torque_sensor_broadcaster joint_state_broadcaster diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 5ed01ac7e..13e520569 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -34,6 +34,9 @@ * \author Marvin Große Besselmann grosse@fzi.de * \date 2020-11-9 * + * \author Mathias Fuhrer mathias.fuhrer@b-robotized.de + * \date 2025-05-28 – Added support for usage with motion_primitives_forward_controller + * */ //---------------------------------------------------------------------- #include @@ -142,6 +145,20 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys trajectory_joint_velocities_.reserve(32768); trajectory_joint_accelerations_.reserve(32768); + // Motion primitives stuff + async_moprim_thread_shutdown_ = false; + new_moprim_cmd_available_ = false; + new_moprim_stop_available_ = false; + new_moprim_reset_available_ = false; + current_moprim_execution_status_ = MoprimExecutionState::IDLE; + ready_for_new_moprim_ = false; + motion_primitives_forward_controller_running_ = false; + // 2 States: execution_status, ready_for_new_primitive + hw_moprim_states_.resize(2, std::numeric_limits::quiet_NaN()); + // 25 Commands: // motion_type + 6 joints + 2*7 positions (goal and via) + blend_radius + velocity + acceleration + + // move_time + hw_moprim_commands_.resize(25, std::numeric_limits::quiet_NaN()); + for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), @@ -316,6 +333,12 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_state", &tool_contact_state_)); + // Motion primitives stuff + state_interfaces.emplace_back( + hardware_interface::StateInterface(HW_IF_MOTION_PRIMITIVES, "execution_status", &hw_moprim_states_[0])); + state_interfaces.emplace_back( + hardware_interface::StateInterface(HW_IF_MOTION_PRIMITIVES, "ready_for_new_primitive", &hw_moprim_states_[1])); + return state_interfaces; } @@ -456,6 +479,63 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + TOOL_CONTACT_GPIO, "tool_contact_set_state", &tool_contact_set_state_)); + // Motion primitives stuff + // Command for motion type (motion_type) + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "motion_type", &hw_moprim_commands_[0])); + // Joint position commands (q1, q2, ..., q6) + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "q1", &hw_moprim_commands_[1])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "q2", &hw_moprim_commands_[2])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "q3", &hw_moprim_commands_[3])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "q4", &hw_moprim_commands_[4])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "q5", &hw_moprim_commands_[5])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "q6", &hw_moprim_commands_[6])); + // Position commands (pos_x, pos_y, pos_z, pos_qx, pos_qy, pos_qz, pos_qz) + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_x", &hw_moprim_commands_[7])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_y", &hw_moprim_commands_[8])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_z", &hw_moprim_commands_[9])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_qx", &hw_moprim_commands_[10])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_qy", &hw_moprim_commands_[11])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_qz", &hw_moprim_commands_[12])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_qw", &hw_moprim_commands_[13])); + // Via Position commands for circula motion + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_via_x", &hw_moprim_commands_[14])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_via_y", &hw_moprim_commands_[15])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_via_z", &hw_moprim_commands_[16])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_via_qx", &hw_moprim_commands_[17])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_via_qy", &hw_moprim_commands_[18])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_via_qz", &hw_moprim_commands_[19])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "pos_via_qw", &hw_moprim_commands_[20])); + // Other command parameters (blend_radius, velocity, acceleration, move_time) + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "blend_radius", &hw_moprim_commands_[21])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "velocity", &hw_moprim_commands_[22])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "acceleration", &hw_moprim_commands_[23])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(HW_IF_MOTION_PRIMITIVES, "move_time", &hw_moprim_commands_[24])); + return command_interfaces; } @@ -596,7 +676,7 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou driver_config.tool_comm_setup = std::move(tool_comm_setup); driver_config.handle_program_state = std::bind(&URPositionHardwareInterface::handleRobotProgramState, this, std::placeholders::_1); - ur_driver_ = std::make_unique(driver_config); + ur_driver_ = std::make_shared(driver_config); if (ur_driver_->getControlFrequency() != info_.rw_rate) { ur_driver_->resetRTDEClient(output_recipe_filename, input_recipe_filename, info_.rw_rate); } @@ -635,8 +715,15 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou get_robot_software_version_build_ = version_info.build; get_robot_software_version_bugfix_ = version_info.bugfix; + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initializing InstructionExecutor"); + instruction_executor_ = std::make_shared(ur_driver_); + async_thread_ = std::make_shared(&URPositionHardwareInterface::asyncThread, this); + // Start async thread for sending motion primitives + async_moprim_cmd_thread_ = std::make_shared(&URPositionHardwareInterface::asyncMoprimCmdThread, this); + async_moprim_stop_thread_ = std::make_shared(&URPositionHardwareInterface::asyncMoprimStopThread, this); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!"); ur_driver_->registerTrajectoryDoneCallback( @@ -645,6 +732,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou ur_driver_->registerToolContactResultCallback( std::bind(&URPositionHardwareInterface::tool_contact_callback, this, std::placeholders::_1)); + ur_driver_->registerToolContactResultCallback( + std::bind(&URPositionHardwareInterface::tool_contact_callback, this, std::placeholders::_1)); + return hardware_interface::CallbackReturn::SUCCESS; } @@ -686,6 +776,16 @@ hardware_interface::CallbackReturn URPositionHardwareInterface::stop() async_thread_->join(); async_thread_.reset(); } + if (async_moprim_cmd_thread_) { + async_moprim_thread_shutdown_ = true; + async_moprim_cmd_thread_->join(); + async_moprim_cmd_thread_.reset(); + } + if (async_moprim_stop_thread_) { + async_moprim_thread_shutdown_ = true; + async_moprim_stop_thread_->join(); + async_moprim_stop_thread_.reset(); + } ur_driver_.reset(); @@ -822,6 +922,10 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp:: updateNonDoubleValues(); + // Motion primitives stuff + hw_moprim_states_[0] = static_cast(current_moprim_execution_status_); + hw_moprim_states_[1] = static_cast(ready_for_new_moprim_); + return hardware_interface::return_type::OK; } if (!non_blocking_read_) @@ -853,6 +957,10 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, 0, urcl::RobotReceiveTimeout::millisec(1000 * 5.0 / static_cast(info_.rw_rate))); check_passthrough_trajectory_controller(); + + } else if (motion_primitives_forward_controller_running_) { + handleMoprimCommands(); + } else { ur_driver_->writeKeepalive(); } @@ -1136,6 +1244,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (tool_contact_controller_running_) { control_modes[i].push_back(TOOL_CONTACT_GPIO); } + if (motion_primitives_forward_controller_running_) { + control_modes[i].push_back(HW_IF_MOTION_PRIMITIVES); + } } auto is_mode_compatible = [this](const std::string& mode, const std::vector& other_modes) { @@ -1162,7 +1273,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod { tf_prefix + FORCE_MODE_GPIO + "/type", FORCE_MODE_GPIO }, { tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i), PASSTHROUGH_GPIO }, { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO }, - { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO } + { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO }, + { HW_IF_MOTION_PRIMITIVES + "/motion_type", HW_IF_MOTION_PRIMITIVES }, }; for (auto& item : start_modes_to_check) { @@ -1197,7 +1309,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod StoppingInterface::STOP_PASSTHROUGH }, { tf_prefix + FREEDRIVE_MODE_GPIO + "/async_success", FREEDRIVE_MODE_GPIO, StoppingInterface::STOP_FREEDRIVE }, { tf_prefix + TOOL_CONTACT_GPIO + "/tool_contact_set_state", TOOL_CONTACT_GPIO, - StoppingInterface::STOP_TOOL_CONTACT } + StoppingInterface::STOP_TOOL_CONTACT }, + { tf_prefix + HW_IF_MOTION_PRIMITIVES + "/motion_type", HW_IF_MOTION_PRIMITIVES, + StoppingInterface::STOP_MOTION_PRIMITIVES }, }; for (auto& item : stop_modes_to_check) { if (key == std::get<0>(item)) { @@ -1257,6 +1371,15 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod freedrive_activated_ = false; freedrive_mode_abort_ = 1.0; } + if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), + StoppingInterface::STOP_MOTION_PRIMITIVES) != stop_modes_[0].end()) { + motion_primitives_forward_controller_running_ = false; + resetMoprimCmdInterfaces(); + current_moprim_execution_status_ = MoprimExecutionState::IDLE; + ready_for_new_moprim_ = false; + + RCLCPP_INFO(get_logger(), "Motion primitives mode stopped."); + } if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(), StoppingInterface::STOP_TOOL_CONTACT) != stop_modes_[0].end()) { tool_contact_controller_running_ = false; @@ -1268,6 +1391,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod hardware_interface::HW_IF_POSITION) != start_modes_[0].end()) { velocity_controller_running_ = false; passthrough_trajectory_controller_running_ = false; + motion_primitives_forward_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; position_controller_running_ = true; @@ -1275,17 +1399,20 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod hardware_interface::HW_IF_VELOCITY) != start_modes_[0].end()) { position_controller_running_ = false; passthrough_trajectory_controller_running_ = false; + motion_primitives_forward_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; } if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), FORCE_MODE_GPIO) != start_modes_[0].end()) { + motion_primitives_forward_controller_running_ = false; force_mode_controller_running_ = true; } if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), PASSTHROUGH_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; + motion_primitives_forward_controller_running_ = false; passthrough_trajectory_controller_running_ = true; passthrough_trajectory_abort_ = 0.0; } @@ -1293,9 +1420,25 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(start_modes_[0].begin(), start_modes_[0].end(), FREEDRIVE_MODE_GPIO) != start_modes_[0].end()) { velocity_controller_running_ = false; position_controller_running_ = false; + motion_primitives_forward_controller_running_ = false; freedrive_mode_controller_running_ = true; freedrive_activated_ = false; } + if (start_modes_[0].size() != 0 && + std::find(start_modes_[0].begin(), start_modes_[0].end(), HW_IF_MOTION_PRIMITIVES) != start_modes_[0].end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + freedrive_mode_controller_running_ = false; + passthrough_trajectory_controller_running_ = false; + force_mode_controller_running_ = false; + + resetMoprimCmdInterfaces(); + current_moprim_execution_status_ = MoprimExecutionState::IDLE; + ready_for_new_moprim_ = true; + motion_primitives_forward_controller_running_ = true; + + RCLCPP_INFO(get_logger(), "Motion primitives mode started."); + } if (start_modes_[0].size() != 0 && std::find(start_modes_[0].begin(), start_modes_[0].end(), TOOL_CONTACT_GPIO) != start_modes_[0].end()) { tool_contact_controller_running_ = true; @@ -1464,6 +1607,413 @@ bool URPositionHardwareInterface::is_valid_joint_information(std::vector 0 && !std::isnan(data[0][0])); } +void URPositionHardwareInterface::handleMoprimCommands() +{ + // Check if we have a new command + if (!std::isnan(hw_moprim_commands_[0])) { + ready_for_new_moprim_ = false; // set to false to indicate that the driver is busy handling a command + // set state interface immediately + // --> if waiting for next read() cycle it happens sometimes that a command is overwritten + hw_moprim_states_[1] = static_cast(ready_for_new_moprim_); + + switch (static_cast(hw_moprim_commands_[0])) { + case static_cast(MoprimMotionHelperType::STOP_MOTION): + { + std::lock_guard guard(moprim_stop_mutex_); + if (!new_moprim_stop_available_) { + new_moprim_stop_available_ = true; + resetMoprimCmdInterfaces(); + } + break; + } + case static_cast(MoprimMotionHelperType::RESET_STOP): + { + std::lock_guard guard(moprim_stop_mutex_); + if (!new_moprim_reset_available_) { + new_moprim_reset_available_ = true; + resetMoprimCmdInterfaces(); + } + break; + } + default: + { + std::lock_guard guard(moprim_cmd_mutex_); + if (!new_moprim_cmd_available_) { + // Copy command to thread-safe buffer + pending_moprim_cmd_ = hw_moprim_commands_; + new_moprim_cmd_available_ = true; + resetMoprimCmdInterfaces(); + } + break; + } + } + } + // Send keepalive if current_moprim_execution_status_ is not EXECUTING + if (ur_driver_ && current_moprim_execution_status_ != MoprimExecutionState::EXECUTING) { + ur_driver_->writeKeepalive(); + } +} + +void URPositionHardwareInterface::resetMoprimCmdInterfaces() +{ + std::fill(hw_moprim_commands_.begin(), hw_moprim_commands_.end(), std::numeric_limits::quiet_NaN()); +} + +void URPositionHardwareInterface::asyncMoprimStopThread() +{ + while (!async_moprim_thread_shutdown_) { + if (new_moprim_stop_available_) { + std::lock_guard guard(moprim_stop_mutex_); + new_moprim_stop_available_ = false; + processMoprimStopCmd(); + } else if (new_moprim_reset_available_) { + std::lock_guard guard(moprim_stop_mutex_); + new_moprim_reset_available_ = false; + processMoprimResetCmd(); + } + // Small sleep to prevent busy waiting + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "[asyncMoprimStopThread] Exiting"); +} + +void URPositionHardwareInterface::processMoprimStopCmd() +{ + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Received Motion Primitives STOP command"); + // delete motion sequence + build_moprim_sequence_ = false; + moprim_sequence_.clear(); + + if (instruction_executor_->isTrajectoryRunning()) { + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Stopping motion ..."); + if (!instruction_executor_->cancelMotion()) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Failed to stop motion"); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + } else { + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Motion stopped successfully"); + current_moprim_execution_status_ = MoprimExecutionState::STOPPED; + ready_for_new_moprim_ = false; + } + } else { + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "No motion to stop"); + current_moprim_execution_status_ = MoprimExecutionState::STOPPED; + ready_for_new_moprim_ = false; + } + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + " [processMoprimStopCmd] After executing stop: current_moprim_execution_status_ = %d", + static_cast(current_moprim_execution_status_)); +} + +void URPositionHardwareInterface::processMoprimResetCmd() +{ + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Received RESET_STOP command"); + current_moprim_execution_status_ = MoprimExecutionState::IDLE; + ready_for_new_moprim_ = true; // set to true to allow sending new commands +} + +void URPositionHardwareInterface::asyncMoprimCmdThread() +{ + while (!async_moprim_thread_shutdown_) { + // Check for new commands + if (new_moprim_cmd_available_) { + // RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "[asyncMoprimCmdThread] New command available"); + std::vector current_command; + { + std::lock_guard guard(moprim_cmd_mutex_); + current_command = pending_moprim_cmd_; + new_moprim_cmd_available_ = false; + } + + // Process the command + processMoprimMotionCmd(current_command); + } + + // Small sleep to prevent busy waiting + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "[asyncMoprimCmdThread] Exiting"); +} + +void URPositionHardwareInterface::processMoprimMotionCmd(const std::vector& command) +{ + if (command.empty() || std::isnan(command[0])) { + return; + } + double velocity, acceleration, move_time; + double motion_type = command[0]; + double blend_radius = command[21]; + + try { + switch (static_cast(motion_type)) { + case static_cast(MoprimMotionHelperType::MOTION_SEQUENCE_START): + { + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Received MOTION_SEQUENCE_START: add all " + "following " + "commands to the motion sequence."); + build_moprim_sequence_ = true; // set flag to put all following commands into the motion sequence + moprim_sequence_.clear(); + ready_for_new_moprim_ = true; // set to true to allow sending new commands + return; + } + + case static_cast(MoprimMotionHelperType::MOTION_SEQUENCE_END): + { + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + "Received MOTION_SEQUENCE_END: executing motion sequence with %zu motion primitives", + moprim_sequence_.size()); + build_moprim_sequence_ = false; + current_moprim_execution_status_ = MoprimExecutionState::EXECUTING; + bool success = instruction_executor_->executeMotion(moprim_sequence_); + current_moprim_execution_status_ = success ? MoprimExecutionState::SUCCESS : MoprimExecutionState::ERROR; + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + " [processMoprimMotionCmd] After executing motion sequence: current_moprim_execution_status_ = %d", + static_cast(current_moprim_execution_status_)); + moprim_sequence_.clear(); + if (success) { + ready_for_new_moprim_ = true; // set to true to allow sending new commands + } + return; + } + + case MoprimMotionType::LINEAR_JOINT: + { // moveJ + // Check if joint positions are valid + for (int i = 1; i <= 6; ++i) { + if (std::isnan(command[i])) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Invalid motion command: joint positions " + "contain NaN values"); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + return; + } + } + urcl::vector6d_t joint_positions = { command[1], command[2], command[3], command[4], command[5], command[6] }; + + // Get move_time OR (velocity AND acceleration) + if (!getMoprimTimeOrVelAndAcc(command, velocity, acceleration, move_time)) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Invalid move_time, velocity or acceleration " + "values"); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + return; + } + + // Check if the command is part of a motion sequence or a single command + if (build_moprim_sequence_) { // Add command to motion sequence + moprim_sequence_.emplace_back(std::make_shared( + joint_positions, blend_radius, std::chrono::milliseconds(static_cast(move_time * 1000)), + acceleration, velocity)); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + "Added moveJ to motion sequence with joint positions: [%f, %f, %f, %f, %f, %f], " + "velocity: %f, acceleration: %f, move_time: %f, blend_radius: %f", + joint_positions[0], joint_positions[1], joint_positions[2], joint_positions[3], + joint_positions[4], joint_positions[5], velocity, acceleration, move_time, blend_radius); + ready_for_new_moprim_ = true; // set to true to allow sending new commands + return; + } else { // execute single primitive directly + current_moprim_execution_status_ = MoprimExecutionState::EXECUTING; + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + "Executing moveJ with joint positions: [%f, %f, %f, %f, %f, %f], " + "velocity: %f, acceleration: %f, move_time: %f, blend_radius: %f", + joint_positions[0], joint_positions[1], joint_positions[2], joint_positions[3], + joint_positions[4], joint_positions[5], velocity, acceleration, move_time, blend_radius); + bool success = instruction_executor_->moveJ(joint_positions, acceleration, velocity, move_time, blend_radius); + current_moprim_execution_status_ = success ? MoprimExecutionState::SUCCESS : MoprimExecutionState::ERROR; + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + " [processMoprimMotionCmd] After executing moveJ: current_moprim_execution_status_ = %d", + static_cast(current_moprim_execution_status_)); + if (success) { + ready_for_new_moprim_ = true; // set to true to allow sending new commands + } + return; + } + break; + } + + case MoprimMotionType::LINEAR_CARTESIAN: + { // moveL + // Check if pose values (position and quaternion) are valid + for (int i = 7; i <= 13; ++i) { + if (std::isnan(command[i])) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Invalid motion command: pose contains NaN " + "values"); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + return; + } + } + double rx, ry, rz; + quaternionToRotVec(command[10], command[11], command[12], command[13], rx, ry, rz); + urcl::Pose pose = { command[7], command[8], command[9], rx, ry, rz }; + + // Get move_time OR (velocity AND acceleration) + if (!getMoprimTimeOrVelAndAcc(command, velocity, acceleration, move_time)) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Invalid move_time, velocity or acceleration " + "values"); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + return; + } + + // Check if the command is part of a motion sequence or a single command + if (build_moprim_sequence_) { // Add command to motion sequence + moprim_sequence_.emplace_back(std::make_shared( + pose, blend_radius, std::chrono::milliseconds(static_cast(move_time * 1000)), acceleration, + velocity)); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + "Added moveL to motion sequence with pose: [%f, %f, %f, %f, %f, %f], " + "velocity: %f, acceleration: %f, move_time: %f, blend_radius: %f", + pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz, velocity, acceleration, move_time, + blend_radius); + ready_for_new_moprim_ = true; // set to true to allow sending new commands + return; + } else { // execute single primitive directly + current_moprim_execution_status_ = MoprimExecutionState::EXECUTING; + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + "Executing moveL with pose: [%f, %f, %f, %f, %f, %f], " + "velocity: %f, acceleration: %f, move_time: %f, blend_radius: %f", + pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz, velocity, acceleration, move_time, + blend_radius); + bool success = instruction_executor_->moveL(pose, acceleration, velocity, move_time, blend_radius); + current_moprim_execution_status_ = success ? MoprimExecutionState::SUCCESS : MoprimExecutionState::ERROR; + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + " [processMoprimMotionCmd] After executing moveL: current_moprim_execution_status_ = %d", + static_cast(current_moprim_execution_status_)); + if (success) { + ready_for_new_moprim_ = true; // set to true to allow sending new commands + } + return; + } + break; + } + + case MoprimMotionType::CIRCULAR_CARTESIAN: + { // CIRC + // Check if pose values (position and quaternion) are valid + for (int i = 7; i <= 20; ++i) { + if (std::isnan(command[i])) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Invalid motion command: pose contains NaN " + "values"); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + return; + } + } + + // 0: Unconstrained mode, 1: Fixed mode + // (https://tools.pages.cba.mit.edu/Universal_Robotics_UR10_Robot_Arm/scriptManual-3.5.4.pdf) + int32_t mode = 0; + + // Get velocity and acceleration) + if (!getMoprimVelAndAcc(command, velocity, acceleration, move_time)) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Invalid velocity or acceleration values"); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + return; + } + + double via_rx, via_ry, via_rz; + quaternionToRotVec(command[17], command[18], command[19], command[20], via_rx, via_ry, via_rz); + urcl::Pose via_pose = { command[14], command[15], command[16], via_rx, via_ry, via_rz }; + + double goal_rx, goal_ry, goal_rz; + quaternionToRotVec(command[10], command[11], command[12], command[13], goal_rx, goal_ry, goal_rz); + urcl::Pose goal_pose = { command[7], command[8], command[9], goal_rx, goal_ry, goal_rz }; + + // Check if the command is part of a motion sequence or a single command + if (build_moprim_sequence_) { // Add command to motion sequence + moprim_sequence_.emplace_back(std::make_shared( + via_pose, goal_pose, blend_radius, acceleration, velocity, mode)); + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + "Added moveC to motion sequence with via_pose: [%f, %f, %f, %f, %f, %f], " + "goal_pose: [%f, %f, %f, %f, %f, %f], velocity: %f," + "acceleration: %f, blend_radius: %f, mode: %d", + via_pose.x, via_pose.y, via_pose.z, via_pose.rx, via_pose.ry, via_pose.rz, goal_pose.x, + goal_pose.y, goal_pose.z, goal_pose.rx, goal_pose.ry, goal_pose.rz, velocity, acceleration, + blend_radius, mode); + ready_for_new_moprim_ = true; // set to true to allow sending new commands + return; + } else { // execute single primitive directly + current_moprim_execution_status_ = MoprimExecutionState::EXECUTING; + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + "Executing moveC with via_pose: [%f, %f, %f, %f, %f, %f], " + "goal_pose: [%f, %f, %f, %f, %f, %f], velocity: %f," + "acceleration: %f, blend_radius: %f, mode: %d", + via_pose.x, via_pose.y, via_pose.z, via_pose.rx, via_pose.ry, via_pose.rz, goal_pose.x, + goal_pose.y, goal_pose.z, goal_pose.rx, goal_pose.ry, goal_pose.rz, velocity, acceleration, + blend_radius, mode); + bool success = instruction_executor_->moveC(via_pose, goal_pose, acceleration, velocity, blend_radius, mode); + current_moprim_execution_status_ = success ? MoprimExecutionState::SUCCESS : MoprimExecutionState::ERROR; + RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), + " [processMoprimMotionCmd] After executing moveC: current_moprim_execution_status_ = %d", + static_cast(current_moprim_execution_status_)); + if (success) { + ready_for_new_moprim_ = true; // set to true to allow sending new commands + } + return; + } + break; + } + + default: + { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), + "Invalid motion command: motion type %f is not supported", motion_type); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + return; + } + } + } catch (const std::exception& e) { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "Failed to execute motion command: %s", e.what()); + current_moprim_execution_status_ = MoprimExecutionState::ERROR; + } +} + +void URPositionHardwareInterface::quaternionToRotVec(double qx, double qy, double qz, double qw, double& rx, double& ry, + double& rz) +{ + tf2::Quaternion q(qx, qy, qz, qw); + const double angle = q.getAngle(); + const auto axis = q.getAxis(); + rx = axis.x() * angle; // rx + ry = axis.y() * angle; // ry + rz = axis.z() * angle; // rz +} + +bool URPositionHardwareInterface::getMoprimTimeOrVelAndAcc(const std::vector& command, double& velocity, + double& acceleration, double& move_time) +{ + // Check if move_time is valid + if (!std::isnan(command[24]) && command[24] > 0.0) { + move_time = command[24]; + // If move_time is valid, velocity and acceleration are ignored in moveJ and moveL, but must be > 0.0 + velocity = 1.0; + acceleration = 1.0; + return true; + } else if (!std::isnan(command[22]) && command[22] > 0.0 && !std::isnan(command[23]) && command[23] > 0.0) { + // If no valid move_time, check if velocity and acceleration are valid + velocity = command[22]; + acceleration = command[23]; + move_time = 0.0; + return true; + } else { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), + "Invalid motion parameters: move_time = %.3f, velocity = %.3f, acceleration = %.3f", command[24], + command[22], command[23]); + + return false; + } +} + +bool URPositionHardwareInterface::getMoprimVelAndAcc(const std::vector& command, double& velocity, + double& acceleration, double& move_time) +{ + // Check if velocity and acceleration are valid + if (!std::isnan(command[22]) && command[22] > 0.0 && !std::isnan(command[23]) && command[23] > 0.0) { + velocity = command[22]; + acceleration = command[23]; + move_time = 0.0; + return true; + } else { + RCLCPP_ERROR(rclcpp::get_logger("URPositionHardwareInterface"), "velocity or acceleration is invalid"); + return false; + } +} + } // namespace ur_robot_driver #include "pluginlib/class_list_macros.hpp" diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index f32805273..1c2937ff4 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -305,6 +305,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +