From 7bc1ddb4a8de310fc2a2b1383c3b8f34eafa6d9e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 5 May 2025 09:13:28 +0200 Subject: [PATCH] Make check_starting_point of test_move launch files configurable This should allow to use them in situations, where the robot is not at the preconfigured pose, but users understand the risks and want to circumvent this. Also, I updated the documentation around this. --- .../config/test_goal_publishers_config.yaml | 2 -- ur_robot_driver/doc/usage/move.rst | 15 +++++++++++ ...test_joint_trajectory_controller.launch.py | 25 ++++++++++++++++--- ...aled_joint_trajectory_controller.launch.py | 25 ++++++++++++++++--- 4 files changed, 57 insertions(+), 10 deletions(-) diff --git a/ur_robot_driver/config/test_goal_publishers_config.yaml b/ur_robot_driver/config/test_goal_publishers_config.yaml index 79388fcd6..c0c9d89d3 100644 --- a/ur_robot_driver/config/test_goal_publishers_config.yaml +++ b/ur_robot_driver/config/test_goal_publishers_config.yaml @@ -22,7 +22,6 @@ publisher_scaled_joint_trajectory_controller: - wrist_2_joint - wrist_3_joint - check_starting_point: true starting_point_limits: shoulder_pan_joint: [-0.1,0.1] shoulder_lift_joint: [-1.6,-1.5] @@ -55,7 +54,6 @@ publisher_joint_trajectory_controller: - wrist_2_joint - wrist_3_joint - check_starting_point: true starting_point_limits: shoulder_pan_joint: [-0.1,0.1] shoulder_lift_joint: [-1.6,-1.5] diff --git a/ur_robot_driver/doc/usage/move.rst b/ur_robot_driver/doc/usage/move.rst index 714ccc8d6..2a9aa0725 100644 --- a/ur_robot_driver/doc/usage/move.rst +++ b/ur_robot_driver/doc/usage/move.rst @@ -16,6 +16,21 @@ First check the controllers' state using ``ros2 control list_controllers``, befo The robot should move, after a few seconds. + .. note:: + By default, the robot's pose is checked for being close to a predefined configuration in order + to make sure that the robot doesn't perform any large, unexpected motions. This configuration + is specified in the ``config/test_goal_publishers.yaml`` config file of the + ``ur_robot_driver`` package. The joint values are + + .. code-block:: yaml + + shoulder_pan_joint: 0 + shoulder_lift_joint: -1.57 + elbow_joint: 0 + wrist_1_joint: -1.57 + wrist_2_joint: 0 + wrist_3_joint: 0 + * To test another controller, simply define it using ``initial_joint_controller`` argument, for example when using mock hardware: .. code-block:: console diff --git a/ur_robot_driver/launch/test_joint_trajectory_controller.launch.py b/ur_robot_driver/launch/test_joint_trajectory_controller.launch.py index 99270dd7f..c048d8c33 100644 --- a/ur_robot_driver/launch/test_joint_trajectory_controller.launch.py +++ b/ur_robot_driver/launch/test_joint_trajectory_controller.launch.py @@ -32,24 +32,41 @@ # Description: After a robot has been loaded, this will execute a series of trajectories. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.actions import DeclareLaunchArgument from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): position_goals = PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "config", "test_goal_publishers_config.yaml"] + [ + FindPackageShare("ur_robot_driver"), + "config", + "test_goal_publishers_config.yaml", + ] ) + check_starting_point = LaunchConfiguration("check_starting_point") + return LaunchDescription( [ + DeclareLaunchArgument( + "check_starting_point", + default_value="true", + description="Verify that the robot is at a preconfigured pose in order to avoid large unexpected motions.", + ), Node( package="ros2_controllers_test_nodes", executable="publisher_joint_trajectory_controller", name="publisher_joint_trajectory_controller", - parameters=[position_goals], + parameters=[ + position_goals, + { + "check_starting_point": check_starting_point, + }, + ], output="screen", - ) + ), ] ) diff --git a/ur_robot_driver/launch/test_scaled_joint_trajectory_controller.launch.py b/ur_robot_driver/launch/test_scaled_joint_trajectory_controller.launch.py index abd1247ed..803340670 100644 --- a/ur_robot_driver/launch/test_scaled_joint_trajectory_controller.launch.py +++ b/ur_robot_driver/launch/test_scaled_joint_trajectory_controller.launch.py @@ -32,24 +32,41 @@ # Description: After a robot has been loaded, this will execute a series of trajectories. from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.actions import DeclareLaunchArgument from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): position_goals = PathJoinSubstitution( - [FindPackageShare("ur_robot_driver"), "config", "test_goal_publishers_config.yaml"] + [ + FindPackageShare("ur_robot_driver"), + "config", + "test_goal_publishers_config.yaml", + ] ) + check_starting_point = LaunchConfiguration("check_starting_point") + return LaunchDescription( [ + DeclareLaunchArgument( + "check_starting_point", + default_value="true", + description="Verify that the robot is at a preconfigured pose in order to avoid large unexpected motions.", + ), Node( package="ros2_controllers_test_nodes", executable="publisher_joint_trajectory_controller", name="publisher_scaled_joint_trajectory_controller", - parameters=[position_goals], + parameters=[ + position_goals, + { + "check_starting_point": check_starting_point, + }, + ], output="screen", - ) + ), ] )