From 7c40b4bf244cec27516bde22089942b3f9454bc2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 14 Feb 2022 09:22:14 +0100 Subject: [PATCH 01/20] Mimic joints independent of command interface combinations --- README.md | 40 +++++++++++- gazebo_ros2_control/src/gazebo_system.cpp | 61 ++++++++++++++----- .../config/gripper_controller_effort.yaml | 15 +++++ ....yaml => gripper_controller_position.yaml} | 0 .../gripper_mimic_joint_example.launch.py | 56 ++++++++++------- .../urdf/test_gripper_mimic_joint.xacro.urdf | 16 ++++- 6 files changed, 148 insertions(+), 40 deletions(-) create mode 100644 gazebo_ros2_control_demos/config/gripper_controller_effort.yaml rename gazebo_ros2_control_demos/config/{gripper_controller.yaml => gripper_controller_position.yaml} (100%) diff --git a/README.md b/README.md index 8f72e015..fec64e07 100644 --- a/README.md +++ b/README.md @@ -93,9 +93,11 @@ include To use `mimic` joints in `gazebo_ros2_control` you should define its parameters to your URDF. We should include: -- `` tag to the mimicked joint ([detailed manual(https://wiki.ros.org/urdf/XML/joint)) -- `mimic` and `multiplier` parameters to joint definition in `` tag +- `` tag to the mimicked joint ([detailed manual](https://wiki.ros.org/urdf/XML/joint)) +- `mimic` and `multiplier` parameters to joint definition in `` tag, +the `offset` parameter is not implemented yet +As an example, `left_finger_joint` mimics the position of `right_finger_joint` ```xml @@ -108,6 +110,12 @@ We should include: ``` ```xml + + + + + + right_finger_joint 1 @@ -117,6 +125,25 @@ We should include: ``` +You can specify a mimicked joint independent of the combination of command interfaces. E.g., if you use an effort command interface for joint 1 but want to let joint 2 mimic the position of joint 1, set +```xml + + + + + + + + right_finger_joint + 1 + + + + + +``` +Be aware that these mimicked joints do not preserve any conservation of energy, i.e., +the necessary effort of joint 1 won't be changed. ## Add the gazebo_ros2_control plugin @@ -248,6 +275,15 @@ Send example commands: ros2 run gazebo_ros2_control_demos example_gripper ``` +To demonstrate the setup of the initial position and a position-mimicked joint in +case of an effort interface on joint 1, add the launch parameter + +```bash +ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py use_effort:=true +``` + + + #### Gazebo + Moveit2 + ROS 2 This example works with [ROS 2 Foxy](https://index.ros.org/doc/ros2/Installation/Foxy/). diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 54fd1f78..19c61c00 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -502,25 +502,58 @@ hardware_interface::return_type GazeboSystem::write() // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION && - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) + + if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION) { - this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * - this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; + if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) + { + // mimic position with identical joint_position_cmd + this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * + this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; + } + else + { + // mimic position, if mimicked joint is velocity- or effort-controlled + this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * + this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index]; + } } - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) + + if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY) { - this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * - this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; + if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) + { + // mimic velocity with identical joint_velocity_cmd_ + this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * + this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; + } + else + { + // mimic velocity, if mimicked joint is position- or effort-controlled + this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * + this->dataPtr->joint_velocity_[mimic_joint.mimicked_joint_index]; + } } - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) + + if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT) { - this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; + if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) + { + // mimic effort with identical joint_effort_cmd_ + this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; + } + else + { + // mimic effort, if mimicked joint is velocity- or position-controlled + this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * + this->dataPtr->joint_effort_[mimic_joint.mimicked_joint_index]; + } } } diff --git a/gazebo_ros2_control_demos/config/gripper_controller_effort.yaml b/gazebo_ros2_control_demos/config/gripper_controller_effort.yaml new file mode 100644 index 00000000..ac5855eb --- /dev/null +++ b/gazebo_ros2_control_demos/config/gripper_controller_effort.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + gripper_controller: + type: forward_command_controller/ForwardCommandController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +gripper_controller: + ros__parameters: + joints: + - right_finger_joint + interface_name: effort diff --git a/gazebo_ros2_control_demos/config/gripper_controller.yaml b/gazebo_ros2_control_demos/config/gripper_controller_position.yaml similarity index 100% rename from gazebo_ros2_control_demos/config/gripper_controller.yaml rename to gazebo_ros2_control_demos/config/gripper_controller_position.yaml diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py index 222d59b9..af98eb44 100644 --- a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -15,36 +15,43 @@ # Author: Denis Stogl from launch import LaunchDescription -from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler, OpaqueFunction, DeclareLaunchArgument from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch.conditions import IfCondition from launch_ros.substitutions import FindPackageShare +import xacro +import os +from ament_index_python.packages import get_package_share_directory +# evaluates LaunchConfigurations in context for use with +# xacro.process_file(). Returns a list of launch actions to be included in +# launch description +def evaluate_xacro(context, *args, **kwargs): -def generate_launch_description(): - - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), - " ", - PathJoinSubstitution( - [FindPackageShare( - 'gazebo_ros2_control_demos'), - 'urdf', - 'test_gripper_mimic_joint.xacro.urdf'] - ), - ] - ) - robot_description = {"robot_description": robot_description_content} + use_effort = LaunchConfiguration('use_effort').perform(context) + print(use_effort) + xacro_path = os.path.join( + get_package_share_directory('gazebo_ros2_control_demos'), + 'urdf', + 'test_gripper_mimic_joint.xacro.urdf') - node_robot_state_publisher = Node( + robot_state_publisher_node = Node( package='robot_state_publisher', executable='robot_state_publisher', + name='robot_state_publisher', output='screen', - parameters=[robot_description] - ) + parameters=[{ + 'robot_description': xacro.process_file(xacro_path, mappings={'effort': use_effort}).toxml() + }]) + return [robot_state_publisher_node] + + +def generate_launch_description(): gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -70,8 +77,13 @@ def generate_launch_description(): ) return LaunchDescription([ + DeclareLaunchArgument( + 'use_effort', + default_value='False', + description='Use effort instead of position command interface'), + RegisterEventHandler( - event_handler=OnProcessExit( + event_handler=OnProcessExit( target_action=spawn_entity, on_exit=[load_joint_state_controller], ) @@ -83,6 +95,8 @@ def generate_launch_description(): ) ), gazebo, - node_robot_state_publisher, + # add OpaqueFunction to evaluate xacro file in context and pass to any + # nodes that need it + OpaqueFunction(function=evaluate_xacro), spawn_entity, ]) diff --git a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf index 8065a6d1..d3576c4b 100644 --- a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf @@ -1,5 +1,5 @@ - + @@ -71,7 +71,12 @@ gazebo_ros2_control/GazeboSystem - + + + + + + 0.15 @@ -90,7 +95,12 @@ - $(find gazebo_ros2_control_demos)/config/gripper_controller.yaml + + $(find gazebo_ros2_control_demos)/config/gripper_controller_effort.yaml + + + $(find gazebo_ros2_control_demos)/config/gripper_controller_position.yaml + From 5a93996d8e550b61a1a6fc1ec961f8905e8d29fc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 14 Feb 2022 09:22:21 +0100 Subject: [PATCH 02/20] Add offset parameter for mimic joint --- README.md | 5 ++-- gazebo_ros2_control/src/gazebo_system.cpp | 25 +++++++++++++------ .../urdf/test_gripper_mimic_joint.xacro.urdf | 1 + 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index fec64e07..cbf73d95 100644 --- a/README.md +++ b/README.md @@ -94,8 +94,7 @@ To use `mimic` joints in `gazebo_ros2_control` you should define its parameters We should include: - `` tag to the mimicked joint ([detailed manual](https://wiki.ros.org/urdf/XML/joint)) -- `mimic` and `multiplier` parameters to joint definition in `` tag, -the `offset` parameter is not implemented yet +- `mimic`, and optional `multiplier`+`offset` parameters to joint definition in `` tag As an example, `left_finger_joint` mimics the position of `right_finger_joint` ```xml @@ -119,6 +118,7 @@ As an example, `left_finger_joint` mimics the position of `right_finger_joint` right_finger_joint 1 + 0 @@ -136,6 +136,7 @@ You can specify a mimicked joint independent of the combination of command inter right_finger_joint 1 + 0 diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 19c61c00..24d09f99 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -32,6 +32,7 @@ struct MimicJoint std::size_t joint_index; std::size_t mimicked_joint_index; double multiplier = 1.0; + double offset = 0.0; }; class gazebo_ros2_control::GazeboSystemPrivate @@ -191,10 +192,17 @@ void GazeboSystem::registerJoints( } else { mimic_joint.multiplier = 1.0; } + param_it = joint_info.parameters.find("offset"); + if (param_it != joint_info.parameters.end()) { + mimic_joint.offset = std::stod(joint_info.parameters.at("offset")); + } else { + mimic_joint.offset = 0.0; + } RCLCPP_INFO_STREAM( this->nh_->get_logger(), - "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << "' with mutiplier: " - << mimic_joint.multiplier); + "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << + "' with mutiplier: " << mimic_joint.multiplier << + "' and offset: " << mimic_joint.offset); this->dataPtr->mimic_joints_.push_back(mimic_joint); suffix = "_mimic"; } @@ -509,14 +517,14 @@ hardware_interface::return_type GazeboSystem::write() { // mimic position with identical joint_position_cmd this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * + mimic_joint.offset + mimic_joint.multiplier * this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; } else { // mimic position, if mimicked joint is velocity- or effort-controlled this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * + mimic_joint.offset + mimic_joint.multiplier * this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index]; } } @@ -527,14 +535,14 @@ hardware_interface::return_type GazeboSystem::write() { // mimic velocity with identical joint_velocity_cmd_ this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * + mimic_joint.offset + mimic_joint.multiplier * this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; } else { // mimic velocity, if mimicked joint is position- or effort-controlled this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * + mimic_joint.offset + mimic_joint.multiplier * this->dataPtr->joint_velocity_[mimic_joint.mimicked_joint_index]; } } @@ -545,13 +553,14 @@ hardware_interface::return_type GazeboSystem::write() { // mimic effort with identical joint_effort_cmd_ this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; + mimic_joint.offset + mimic_joint.multiplier * + this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; } else { // mimic effort, if mimicked joint is velocity- or position-controlled this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * + mimic_joint.offset + mimic_joint.multiplier * this->dataPtr->joint_effort_[mimic_joint.mimicked_joint_index]; } } diff --git a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf index d3576c4b..114578cb 100644 --- a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf @@ -86,6 +86,7 @@ right_finger_joint 1 + 0 From ef0c5c717318d39ba0fb181ace2b1694ac95db72 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 14 Feb 2022 10:32:08 +0100 Subject: [PATCH 03/20] Format code and fix flake8 errors --- gazebo_ros2_control/src/gazebo_system.cpp | 35 ++++++------------- .../gripper_mimic_joint_example.launch.py | 7 ++-- 2 files changed, 15 insertions(+), 27 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 24d09f99..7bd92e5f 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -201,8 +201,8 @@ void GazeboSystem::registerJoints( RCLCPP_INFO_STREAM( this->nh_->get_logger(), "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << - "' with mutiplier: " << mimic_joint.multiplier << - "' and offset: " << mimic_joint.offset); + "' with mutiplier: " << mimic_joint.multiplier << + "' and offset: " << mimic_joint.offset); this->dataPtr->mimic_joints_.push_back(mimic_joint); suffix = "_mimic"; } @@ -510,18 +510,13 @@ hardware_interface::return_type GazeboSystem::write() // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION) - { - if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) - { + if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION) { + if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) { // mimic position with identical joint_position_cmd this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = mimic_joint.offset + mimic_joint.multiplier * this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; - } - else - { + } else { // mimic position, if mimicked joint is velocity- or effort-controlled this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = mimic_joint.offset + mimic_joint.multiplier * @@ -529,17 +524,13 @@ hardware_interface::return_type GazeboSystem::write() } } - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY) - { - if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) - { + if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY) { + if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) { // mimic velocity with identical joint_velocity_cmd_ this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = mimic_joint.offset + mimic_joint.multiplier * this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; - } - else - { + } else { // mimic velocity, if mimicked joint is position- or effort-controlled this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = mimic_joint.offset + mimic_joint.multiplier * @@ -547,17 +538,13 @@ hardware_interface::return_type GazeboSystem::write() } } - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT) - { - if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) - { + if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT) { + if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) { // mimic effort with identical joint_effort_cmd_ this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = mimic_joint.offset + mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; - } - else - { + } else { // mimic effort, if mimicked joint is velocity- or position-controlled this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = mimic_joint.offset + mimic_joint.multiplier * diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py index af98eb44..fed92d91 100644 --- a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -19,15 +19,14 @@ from launch.actions import RegisterEventHandler, OpaqueFunction, DeclareLaunchArgument from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch.conditions import IfCondition from launch_ros.substitutions import FindPackageShare import xacro import os from ament_index_python.packages import get_package_share_directory + # evaluates LaunchConfigurations in context for use with # xacro.process_file(). Returns a list of launch actions to be included in # launch description @@ -46,7 +45,9 @@ def evaluate_xacro(context, *args, **kwargs): name='robot_state_publisher', output='screen', parameters=[{ - 'robot_description': xacro.process_file(xacro_path, mappings={'effort': use_effort}).toxml() + 'robot_description': + xacro.process_file(xacro_path, + mappings={'effort': use_effort}).toxml() }]) return [robot_state_publisher_node] From f8abfb93315e8cea59bc6b72164a9d2ef89f0e19 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 18 Feb 2022 14:05:29 +0100 Subject: [PATCH 04/20] Split gripper example into two separate files for position/effort interface --- README.md | 7 +- ...pper_mimic_joint_example_effort.launch.py} | 59 +++++------ ...per_mimic_joint_example_position.launch.py | 88 +++++++++++++++++ ...test_gripper_mimic_joint_effort.xacro.urdf | 97 +++++++++++++++++++ ...t_gripper_mimic_joint_position.xacro.urdf} | 12 +-- 5 files changed, 212 insertions(+), 51 deletions(-) rename gazebo_ros2_control_demos/launch/{gripper_mimic_joint_example.launch.py => gripper_mimic_joint_example_effort.launch.py} (63%) create mode 100644 gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py create mode 100644 gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf rename gazebo_ros2_control_demos/urdf/{test_gripper_mimic_joint.xacro.urdf => test_gripper_mimic_joint_position.xacro.urdf} (87%) diff --git a/README.md b/README.md index cbf73d95..25ffff9e 100644 --- a/README.md +++ b/README.md @@ -267,7 +267,7 @@ The following example shows parallel gripper with mimic joint: ```bash -ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py +ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_position.launch.py ``` Send example commands: @@ -277,11 +277,12 @@ ros2 run gazebo_ros2_control_demos example_gripper ``` To demonstrate the setup of the initial position and a position-mimicked joint in -case of an effort interface on joint 1, add the launch parameter +case of an effort command interface of the joint to be mimicked, run ```bash -ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py use_effort:=true +ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_effort.launch.py ``` +instead. diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py similarity index 63% rename from gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py rename to gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py index fed92d91..11ab0b74 100644 --- a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py @@ -15,44 +15,36 @@ # Author: Denis Stogl from launch import LaunchDescription -from launch.actions import ExecuteProcess, IncludeLaunchDescription -from launch.actions import RegisterEventHandler, OpaqueFunction, DeclareLaunchArgument +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -import xacro -import os -from ament_index_python.packages import get_package_share_directory -# evaluates LaunchConfigurations in context for use with -# xacro.process_file(). Returns a list of launch actions to be included in -# launch description -def evaluate_xacro(context, *args, **kwargs): +def generate_launch_description(): - use_effort = LaunchConfiguration('use_effort').perform(context) - print(use_effort) - xacro_path = os.path.join( - get_package_share_directory('gazebo_ros2_control_demos'), - 'urdf', - 'test_gripper_mimic_joint.xacro.urdf') + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + " ", + PathJoinSubstitution( + [FindPackageShare( + 'gazebo_ros2_control_demos'), + 'urdf', + 'test_gripper_mimic_joint_effort.xacro.urdf'] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} - robot_state_publisher_node = Node( + node_robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', - name='robot_state_publisher', output='screen', - parameters=[{ - 'robot_description': - xacro.process_file(xacro_path, - mappings={'effort': use_effort}).toxml() - }]) - return [robot_state_publisher_node] - - -def generate_launch_description(): + parameters=[robot_description] + ) gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -78,13 +70,8 @@ def generate_launch_description(): ) return LaunchDescription([ - DeclareLaunchArgument( - 'use_effort', - default_value='False', - description='Use effort instead of position command interface'), - RegisterEventHandler( - event_handler=OnProcessExit( + event_handler=OnProcessExit( target_action=spawn_entity, on_exit=[load_joint_state_controller], ) @@ -96,8 +83,6 @@ def generate_launch_description(): ) ), gazebo, - # add OpaqueFunction to evaluate xacro file in context and pass to any - # nodes that need it - OpaqueFunction(function=evaluate_xacro), + node_robot_state_publisher, spawn_entity, - ]) + ]) \ No newline at end of file diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py new file mode 100644 index 00000000..4e59ad98 --- /dev/null +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py @@ -0,0 +1,88 @@ +# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# +# 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. +# +# Author: Denis Stogl + +from launch import LaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + " ", + PathJoinSubstitution( + [FindPackageShare( + 'gazebo_ros2_control_demos'), + 'urdf', + 'test_gripper_mimic_joint_position.xacro.urdf'] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"] + ), + ) + + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=['-topic', 'robot_description', + '-entity', 'gripper'], + output='screen') + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 'joint_state_broadcaster'], + output='screen' + ) + + load_gripper_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 'gripper_controller'], + output='screen' + ) + + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_gripper_controller], + ) + ), + gazebo, + node_robot_state_publisher, + spawn_entity, + ]) \ No newline at end of file diff --git a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf new file mode 100644 index 00000000..33ba7ddb --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + 0.15 + + + + + + right_finger_joint + 1 + 0 + + + + + + + + + + $(find gazebo_ros2_control_demos)/config/gripper_controller_effort.yaml + + + diff --git a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf similarity index 87% rename from gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf rename to gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf index 114578cb..8abaced3 100644 --- a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf @@ -71,12 +71,7 @@ gazebo_ros2_control/GazeboSystem - - - - - - + 0.15 @@ -96,12 +91,7 @@ - - $(find gazebo_ros2_control_demos)/config/gripper_controller_effort.yaml - - $(find gazebo_ros2_control_demos)/config/gripper_controller_position.yaml - From e6daaf7189ae9b7ffc14bdb7ebf614d3f2797238 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 15 Nov 2022 22:13:50 +0100 Subject: [PATCH 05/20] Satisfy flake8 --- .../launch/gripper_mimic_joint_example_effort.launch.py | 2 +- .../launch/gripper_mimic_joint_example_position.launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py index 725acda2..49262dad 100644 --- a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py @@ -85,4 +85,4 @@ def generate_launch_description(): gazebo, node_robot_state_publisher, spawn_entity, - ]) \ No newline at end of file + ]) diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py index 4e59ad98..d46784ee 100644 --- a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py @@ -85,4 +85,4 @@ def generate_launch_description(): gazebo, node_robot_state_publisher, spawn_entity, - ]) \ No newline at end of file + ]) From 4eeeda2405552118f25a18d43f2a2303ebae0956 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 15 Nov 2022 22:14:05 +0100 Subject: [PATCH 06/20] Fix load_controller in launch file --- .../launch/gripper_mimic_joint_example_position.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py index d46784ee..4247ac4d 100644 --- a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py @@ -58,13 +58,13 @@ def generate_launch_description(): output='screen') load_joint_state_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' ) load_gripper_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'gripper_controller'], output='screen' ) From 0d4468adc61ff3da0d3aaf9b7e16bd0fcf6cc1cd Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 20 Nov 2022 16:44:22 +0000 Subject: [PATCH 07/20] Fix mimic joint with different command interface --- gazebo_ros2_control/src/gazebo_system.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index e213bd30..e77fa93b 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -31,6 +31,7 @@ struct MimicJoint { std::size_t joint_index; std::size_t mimicked_joint_index; + gazebo_ros2_control::GazeboSystemInterface::ControlMethod control_method; double multiplier = 1.0; double offset = 0.0; }; @@ -186,6 +187,17 @@ void GazeboSystem::registerJoints( mimic_joint.joint_index = j; mimic_joint.mimicked_joint_index = std::distance( hardware_info.joints.begin(), mimicked_joint_it); + for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) { + if (joint_info.command_interfaces[i].name == "position") { + mimic_joint.control_method |= POSITION; + } + if (joint_info.command_interfaces[i].name == "velocity") { + mimic_joint.control_method |= VELOCITY; + } + if (joint_info.command_interfaces[i].name == "effort") { + mimic_joint.control_method |= EFFORT; + } + } auto param_it = joint_info.parameters.find("multiplier"); if (param_it != joint_info.parameters.end()) { mimic_joint.multiplier = std::stod(joint_info.parameters.at("multiplier")); @@ -520,7 +532,7 @@ GazeboSystem::perform_command_mode_switch( // mimic joint has the same control mode as mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { this->dataPtr->joint_control_methods_[mimic_joint.joint_index] = - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index]; + mimic_joint.control_method; } return hardware_interface::return_type::OK; } From d9676a3d14d03514b6ca4d4095e9f79922944827 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 20 Nov 2022 16:45:44 +0000 Subject: [PATCH 08/20] Fix typos --- gazebo_ros2_control/src/gazebo_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index e77fa93b..1303518f 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -91,7 +91,7 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief handles to the FT sensors from within Gazebo std::vector sim_ft_sensors_; - /// \brief An array per FT sensor for 3D force and torquee + /// \brief An array per FT sensor for 3D force and torque std::vector> ft_sensor_data_; /// \brief state interfaces that will be exported to the Resource Manager @@ -212,7 +212,7 @@ void GazeboSystem::registerJoints( } RCLCPP_INFO_STREAM( this->nh_->get_logger(), - "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << + "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << "' with multiplier: " << mimic_joint.multiplier << "' and offset: " << mimic_joint.offset); this->dataPtr->mimic_joints_.push_back(mimic_joint); From 55bd3e349a887c7a0987a7cd261261244c65ce7c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 5 Apr 2024 20:00:06 +0000 Subject: [PATCH 09/20] Revert changes to gazebo_system --- gazebo_ros2_control/src/gazebo_system.cpp | 165 +++++++++++----------- 1 file changed, 83 insertions(+), 82 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 1303518f..c688541c 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -25,15 +25,14 @@ #include "gazebo/sensors/SensorManager.hh" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" struct MimicJoint { std::size_t joint_index; std::size_t mimicked_joint_index; - gazebo_ros2_control::GazeboSystemInterface::ControlMethod control_method; double multiplier = 1.0; - double offset = 0.0; }; class gazebo_ros2_control::GazeboSystemPrivate @@ -61,6 +60,9 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief vector with the control method defined in the URDF for each joint. std::vector joint_control_methods_; + /// \brief vector with indication for actuated joints (vs. passive joints) + std::vector is_joint_actuated_; + /// \brief handles to the joints from within Gazebo std::vector sim_joints_; @@ -91,7 +93,7 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief handles to the FT sensors from within Gazebo std::vector sim_ft_sensors_; - /// \brief An array per FT sensor for 3D force and torque + /// \brief An array per FT sensor for 3D force and torquee std::vector> ft_sensor_data_; /// \brief state interfaces that will be exported to the Resource Manager @@ -102,6 +104,9 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief mapping of mimicked joints to index of joint they mimic std::vector mimic_joints_; + + // Should hold the joints if no control_mode is active + bool hold_joints_ = true; }; namespace gazebo_ros2_control @@ -127,6 +132,30 @@ bool GazeboSystem::initSim( return false; } + try { + this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool(); + } catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' not initialized, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' not declared, with error %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } catch (rclcpp::ParameterTypeException & ex) { + RCLCPP_ERROR( + this->nh_->get_logger(), + "Parameter 'hold_joints' has wrong type: %s", ex.what()); + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_); + } + RCLCPP_DEBUG_STREAM( + this->nh_->get_logger(), "hold_joints (system): " << this->dataPtr->hold_joints_ << std::endl); + registerJoints(hardware_info, parent_model); registerSensors(hardware_info, parent_model); @@ -146,6 +175,7 @@ void GazeboSystem::registerJoints( this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_control_methods_.resize(this->dataPtr->n_dof_); + this->dataPtr->is_joint_actuated_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_position_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_velocity_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_effort_.resize(this->dataPtr->n_dof_); @@ -187,48 +217,41 @@ void GazeboSystem::registerJoints( mimic_joint.joint_index = j; mimic_joint.mimicked_joint_index = std::distance( hardware_info.joints.begin(), mimicked_joint_it); - for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) { - if (joint_info.command_interfaces[i].name == "position") { - mimic_joint.control_method |= POSITION; - } - if (joint_info.command_interfaces[i].name == "velocity") { - mimic_joint.control_method |= VELOCITY; - } - if (joint_info.command_interfaces[i].name == "effort") { - mimic_joint.control_method |= EFFORT; - } - } auto param_it = joint_info.parameters.find("multiplier"); if (param_it != joint_info.parameters.end()) { - mimic_joint.multiplier = std::stod(joint_info.parameters.at("multiplier")); + mimic_joint.multiplier = hardware_interface::stod(joint_info.parameters.at("multiplier")); } else { mimic_joint.multiplier = 1.0; } - param_it = joint_info.parameters.find("offset"); - if (param_it != joint_info.parameters.end()) { - mimic_joint.offset = std::stod(joint_info.parameters.at("offset")); - } else { - mimic_joint.offset = 0.0; - } RCLCPP_INFO_STREAM( this->nh_->get_logger(), "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << - "' with multiplier: " << mimic_joint.multiplier << - "' and offset: " << mimic_joint.offset); + "' with mutiplier: " << mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); suffix = "_mimic"; } RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); - auto get_initial_value = [this](const hardware_interface::InterfaceInfo & interface_info) { + auto get_initial_value = + [this, joint_name](const hardware_interface::InterfaceInfo & interface_info) { + double initial_value{0.0}; if (!interface_info.initial_value.empty()) { - double value = std::stod(interface_info.initial_value); - RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", value); - return value; - } else { - return 0.0; + try { + initial_value = hardware_interface::stod(interface_info.initial_value); + RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", initial_value); + } catch (std::invalid_argument &) { + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), + "Failed converting initial_value string to real number for the joint " + << joint_name + << " and state interface " << interface_info.name + << ". Actual value of parameter: " << interface_info.initial_value + << ". Initial value will be set to 0.0"); + throw std::invalid_argument("Failed converting initial_value string"); + } } + return initial_value; }; double initial_position = std::numeric_limits::quiet_NaN(); @@ -313,6 +336,9 @@ void GazeboSystem::registerJoints( this->dataPtr->sim_joints_[j]->SetForce(0, initial_effort); } } + + // check if joint is actuated (has command interfaces) or passive + this->dataPtr->is_joint_actuated_[j] = (joint_info.command_interfaces.size() > 0); } } @@ -495,13 +521,11 @@ GazeboSystem::perform_command_mode_switch( hardware_interface::HW_IF_POSITION)) { this->dataPtr->joint_control_methods_[j] &= static_cast(VELOCITY & EFFORT); - } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT hardware_interface::HW_IF_VELOCITY)) { this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & EFFORT); - } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joint_control_methods_[j] &= @@ -515,13 +539,11 @@ GazeboSystem::perform_command_mode_switch( hardware_interface::HW_IF_POSITION)) { this->dataPtr->joint_control_methods_[j] |= POSITION; - } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT hardware_interface::HW_IF_VELOCITY)) { this->dataPtr->joint_control_methods_[j] |= VELOCITY; - } - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + + } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + // NOLINT hardware_interface::HW_IF_EFFORT)) { this->dataPtr->joint_control_methods_[j] |= EFFORT; @@ -532,7 +554,7 @@ GazeboSystem::perform_command_mode_switch( // mimic joint has the same control mode as mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { this->dataPtr->joint_control_methods_[mimic_joint.joint_index] = - mimic_joint.control_method; + this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index]; } return hardware_interface::return_type::OK; } @@ -589,46 +611,23 @@ hardware_interface::return_type GazeboSystem::write( // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION) { - if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) { - // mimic position with identical joint_position_cmd - this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = - mimic_joint.offset + mimic_joint.multiplier * - this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; - } else { - // mimic position, if mimicked joint is velocity- or effort-controlled - this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = - mimic_joint.offset + mimic_joint.multiplier * - this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index]; - } - } - - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY) { - if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) { - // mimic velocity with identical joint_velocity_cmd_ - this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = - mimic_joint.offset + mimic_joint.multiplier * - this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; - } else { - // mimic velocity, if mimicked joint is position- or effort-controlled - this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = - mimic_joint.offset + mimic_joint.multiplier * - this->dataPtr->joint_velocity_[mimic_joint.mimicked_joint_index]; - } - } - - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT) { - if (this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) { - // mimic effort with identical joint_effort_cmd_ - this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = - mimic_joint.offset + mimic_joint.multiplier * - this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; - } else { - // mimic effort, if mimicked joint is velocity- or position-controlled - this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = - mimic_joint.offset + mimic_joint.multiplier * - this->dataPtr->joint_effort_[mimic_joint.mimicked_joint_index]; - } + if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION && + this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) + { + this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * + this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; + } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && // NOLINT + this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) + { + this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * + this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; + } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && // NOLINT + this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) + { + this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = + mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; } } @@ -636,12 +635,14 @@ hardware_interface::return_type GazeboSystem::write( if (this->dataPtr->sim_joints_[j]) { if (this->dataPtr->joint_control_methods_[j] & POSITION) { this->dataPtr->sim_joints_[j]->SetPosition(0, this->dataPtr->joint_position_cmd_[j], true); - } - if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { + this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); + } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { // NOLINT this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); - } - if (this->dataPtr->joint_control_methods_[j] & EFFORT) { + } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]); + } else if (this->dataPtr->is_joint_actuated_[j] && this->dataPtr->hold_joints_) { + // Fallback case is a velocity command of zero (only for actuated joints) + this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); } } } From 452539b05b309b3d929c448a12fed0bb450ada83 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 5 Apr 2024 20:07:17 +0000 Subject: [PATCH 10/20] Update the documentation --- doc/index.rst | 29 +++++++++++------------------ 1 file changed, 11 insertions(+), 18 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index daff5f2c..e4c12a96 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -101,16 +101,19 @@ include Using mimic joints in simulation ----------------------------------------------------------- -To use ``mimic`` joints in *gazebo_ros2_control* you should define its parameters to your URDF. -We should include: - -* ```` tag to the mimicked joint `detailed manual `__ -* ``mimic`` and ``multiplier`` parameters to joint definition in ```` tag +To use ``mimic`` joints in *gazebo_ros2_control* you should define its parameters in your URDF, i.e, the ```` tag to the mimicked joint `detailed manual `__ .. code-block:: xml + + + + + + + - + @@ -118,18 +121,8 @@ We should include: - -.. code-block:: xml - - - right_finger_joint - 1 - - - - - - +The mimic joint must not have command interfaces configured in the ```` tag, but state interfaces can be configured. +Independent of the interface type of the mimicked joint in the ```` tag, the mimic joint will use the position interface of the gazebo classic physic engine to follow the position of the mimicked joint. Add the gazebo_ros2_control plugin ========================================== From 3a55dc95b9239d89152411060fe6df2f4f3aa8fb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 5 Apr 2024 20:25:56 +0000 Subject: [PATCH 11/20] Use mimic joint info from hardware_info --- gazebo_ros2_control/src/gazebo_system.cpp | 94 +++++++---------------- 1 file changed, 27 insertions(+), 67 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index c688541c..b8621fbf 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -28,13 +28,6 @@ #include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -struct MimicJoint -{ - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; -}; - class gazebo_ros2_control::GazeboSystemPrivate { public: @@ -93,7 +86,7 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief handles to the FT sensors from within Gazebo std::vector sim_ft_sensors_; - /// \brief An array per FT sensor for 3D force and torquee + /// \brief An array per FT sensor for 3D force and torque std::vector> ft_sensor_data_; /// \brief state interfaces that will be exported to the Resource Manager @@ -102,9 +95,6 @@ class gazebo_ros2_control::GazeboSystemPrivate /// \brief command interfaces that will be exported to the Resource Manager std::vector command_interfaces_; - /// \brief mapping of mimicked joints to index of joint they mimic - std::vector mimic_joints_; - // Should hold the joints if no control_mode is active bool hold_joints_ = true; }; @@ -199,36 +189,20 @@ void GazeboSystem::registerJoints( // Accept this joint and continue configuration RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); - std::string suffix = ""; - // check if joint is mimicked - if (joint_info.parameters.find("mimic") != joint_info.parameters.end()) { - const auto mimicked_joint = joint_info.parameters.at("mimic"); - const auto mimicked_joint_it = std::find_if( - hardware_info.joints.begin(), hardware_info.joints.end(), - [&mimicked_joint](const hardware_interface::ComponentInfo & info) { - return info.name == mimicked_joint; - }); - if (mimicked_joint_it == hardware_info.joints.end()) { - throw std::runtime_error( - std::string("Mimicked joint '") + mimicked_joint + "' not found"); - } - MimicJoint mimic_joint; - mimic_joint.joint_index = j; - mimic_joint.mimicked_joint_index = std::distance( - hardware_info.joints.begin(), mimicked_joint_it); - auto param_it = joint_info.parameters.find("multiplier"); - if (param_it != joint_info.parameters.end()) { - mimic_joint.multiplier = hardware_interface::stod(joint_info.parameters.at("multiplier")); - } else { - mimic_joint.multiplier = 1.0; - } + auto it = std::find_if( + hardware_info.mimic_joints.begin(), + hardware_info.mimic_joints.end(), + [j](const hardware_interface::MimicJoint & mj) { + return mj.joint_index == j; + }); + + if (it != hardware_info.mimic_joints.end()) { RCLCPP_INFO_STREAM( this->nh_->get_logger(), - "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << - "' with mutiplier: " << mimic_joint.multiplier); - this->dataPtr->mimic_joints_.push_back(mimic_joint); - suffix = "_mimic"; + "Joint '" << joint_name << "' is mimicking joint '" << + this->dataPtr->joint_names_.at(it->mimicked_joint_index) << + "' with multiplier: " << it->multiplier << " and offset: " << it->offset); } RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); @@ -263,7 +237,7 @@ void GazeboSystem::registerJoints( if (joint_info.state_interfaces[i].name == "position") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); this->dataPtr->state_interfaces_.emplace_back( - joint_name + suffix, + joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_[j]); initial_position = get_initial_value(joint_info.state_interfaces[i]); @@ -272,7 +246,7 @@ void GazeboSystem::registerJoints( if (joint_info.state_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->state_interfaces_.emplace_back( - joint_name + suffix, + joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joint_velocity_[j]); initial_velocity = get_initial_value(joint_info.state_interfaces[i]); @@ -281,7 +255,7 @@ void GazeboSystem::registerJoints( if (joint_info.state_interfaces[i].name == "effort") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->state_interfaces_.emplace_back( - joint_name + suffix, + joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joint_effort_[j]); initial_effort = get_initial_value(joint_info.state_interfaces[i]); @@ -296,7 +270,7 @@ void GazeboSystem::registerJoints( if (joint_info.command_interfaces[i].name == "position") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); this->dataPtr->command_interfaces_.emplace_back( - joint_name + suffix, + joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_cmd_[j]); if (!std::isnan(initial_position)) { @@ -310,7 +284,7 @@ void GazeboSystem::registerJoints( if (joint_info.command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->command_interfaces_.emplace_back( - joint_name + suffix, + joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joint_velocity_cmd_[j]); if (!std::isnan(initial_velocity)) { @@ -324,7 +298,7 @@ void GazeboSystem::registerJoints( if (joint_info.command_interfaces[i].name == "effort") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->command_interfaces_.emplace_back( - joint_name + suffix, + joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joint_effort_cmd_[j]); if (!std::isnan(initial_effort)) { @@ -551,10 +525,11 @@ GazeboSystem::perform_command_mode_switch( } } - // mimic joint has the same control mode as mimicked joint - for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - this->dataPtr->joint_control_methods_[mimic_joint.joint_index] = - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index]; + // mimic joint has always position control mode + for (const auto & mimic_joint : this->info_.mimic_joints) { + this->dataPtr->joint_control_methods_[mimic_joint.joint_index] &= + static_cast(VELOCITY & EFFORT); + this->dataPtr->joint_control_methods_[mimic_joint.joint_index] |= POSITION; } return hardware_interface::return_type::OK; } @@ -610,25 +585,10 @@ hardware_interface::return_type GazeboSystem::write( rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; // set values of all mimic joints with respect to mimicked joint - for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION && - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) - { - this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * - this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; - } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && // NOLINT - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) - { - this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * - this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; - } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && // NOLINT - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) - { - this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = - mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; - } + for (const auto & mimic_joint : this->info_.mimic_joints) { + this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = + mimic_joint.offset + mimic_joint.multiplier * + this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index]; } for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { From 4fffa125cc23f597cf64a6a850c75e407e569774 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 5 Apr 2024 20:56:13 +0000 Subject: [PATCH 12/20] Update docs --- doc/index.rst | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index e4c12a96..36437097 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -243,19 +243,26 @@ When the Gazebo world is launched you can run some of the following commands to ros2 run gazebo_ros2_control_demos example_tricycle_drive -The following example shows parallel gripper with mimic joint: +The following example shows a parallel gripper with a mimic joint: + + +.. code-block:: shell + + ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_position.launch.py .. image:: img/gripper.gif :alt: Cart +To demonstrate the setup of the initial position and a position-mimicked joint in +case of an effort command interface of the joint to be mimicked, run .. code-block:: shell - ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py - + ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_effort.launch.py -Send example commands: +instead. +Send example commands with .. code-block:: shell From 693cd460c73a39246f2e66d3c29d1df4781c3a8d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 5 Apr 2024 20:56:34 +0000 Subject: [PATCH 13/20] Fix mimic definition from URDF --- .../urdf/test_gripper_mimic_joint_effort.xacro.urdf | 6 +----- .../urdf/test_gripper_mimic_joint_position.xacro.urdf | 6 +----- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf index 33ba7ddb..dc5ee4de 100644 --- a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf @@ -59,7 +59,7 @@ - + @@ -79,10 +79,6 @@ - right_finger_joint - 1 - 0 - diff --git a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf index 8abaced3..500cb6f2 100644 --- a/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf @@ -59,7 +59,7 @@ - + @@ -79,10 +79,6 @@ - right_finger_joint - 1 - 0 - From a7793201917d28c459700db489f28a27172db9b2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 7 Apr 2024 17:50:07 +0000 Subject: [PATCH 14/20] Set initial position before simulation starts --- gazebo_ros2_control/src/gazebo_system.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index b8621fbf..4d596db7 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -314,6 +314,15 @@ void GazeboSystem::registerJoints( // check if joint is actuated (has command interfaces) or passive this->dataPtr->is_joint_actuated_[j] = (joint_info.command_interfaces.size() > 0); } + + // set initial position for mimic joints + for (const auto & mimic_joint : hardware_info.mimic_joints) { + this->dataPtr->sim_joints_[mimic_joint.joint_index]->SetPosition( + 0, + mimic_joint.offset + mimic_joint.multiplier * + this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index], + true); + } } void GazeboSystem::registerSensors( From feb2bb8a36613fd22ed949928397cc80cdfd27ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Mon, 8 Apr 2024 18:20:09 +0200 Subject: [PATCH 15/20] Add parenthesis Co-authored-by: Sai Kishor Kothakota --- gazebo_ros2_control/src/gazebo_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 4d596db7..06f1522e 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -319,8 +319,8 @@ void GazeboSystem::registerJoints( for (const auto & mimic_joint : hardware_info.mimic_joints) { this->dataPtr->sim_joints_[mimic_joint.joint_index]->SetPosition( 0, - mimic_joint.offset + mimic_joint.multiplier * - this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index], + mimic_joint.offset + (mimic_joint.multiplier * + this->dataPtr->joint_position_[mimic_joint.mimicked_joint_index]), true); } } From 74556d57dcdb9e519c3f225861a7bddcf687e2a3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 8 Apr 2024 16:23:44 +0000 Subject: [PATCH 16/20] Mark the info as note --- doc/index.rst | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index 36437097..68cbea7d 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -122,7 +122,10 @@ To use ``mimic`` joints in *gazebo_ros2_control* you should define its parameter The mimic joint must not have command interfaces configured in the ```` tag, but state interfaces can be configured. -Independent of the interface type of the mimicked joint in the ```` tag, the mimic joint will use the position interface of the gazebo classic physic engine to follow the position of the mimicked joint. + +.. note:: + + Independent of the interface type of the mimicked joint in the ```` tag, the mimic joint will use the position interface of the gazebo classic physic engine to follow the position of the mimicked joint. Add the gazebo_ros2_control plugin ========================================== From 28e943f4a65800847847b054bd8cc5f13454e075 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 8 Apr 2024 16:43:40 +0000 Subject: [PATCH 17/20] Use explicit assignment --- gazebo_ros2_control/src/gazebo_system.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index bd943f79..1a573cab 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -536,9 +536,8 @@ GazeboSystem::perform_command_mode_switch( // mimic joint has always position control mode for (const auto & mimic_joint : this->info_.mimic_joints) { - this->dataPtr->joint_control_methods_[mimic_joint.joint_index] &= - static_cast(VELOCITY & EFFORT); - this->dataPtr->joint_control_methods_[mimic_joint.joint_index] |= POSITION; + this->dataPtr->joint_control_methods_[mimic_joint.joint_index] = + static_cast(POSITION); } return hardware_interface::return_type::OK; } From 1cb84d12f233c3a55a284c0aa2b91a7d0617f5d7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 8 Apr 2024 16:52:25 +0000 Subject: [PATCH 18/20] Rephrase text --- doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index 68cbea7d..6e2ea166 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -101,7 +101,7 @@ include Using mimic joints in simulation ----------------------------------------------------------- -To use ``mimic`` joints in *gazebo_ros2_control* you should define its parameters in your URDF, i.e, the ```` tag to the mimicked joint `detailed manual `__ +To use ``mimic`` joints in *gazebo_ros2_control* you should define its parameters in your URDF, i.e, set the ```` tag to the mimicked joint (see the `URDF specification `__) .. code-block:: xml From 4bc1ae3b60a79dbd93d48faf66ba3e1d717bae85 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 22 Apr 2024 14:14:06 +0000 Subject: [PATCH 19/20] Fix alt text --- doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index 6e2ea166..63960b3c 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -254,7 +254,7 @@ The following example shows a parallel gripper with a mimic joint: ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_position.launch.py .. image:: img/gripper.gif - :alt: Cart + :alt: Gripper To demonstrate the setup of the initial position and a position-mimicked joint in case of an effort command interface of the joint to be mimicked, run From 929a29f616577561330e163e18737fa7007f9ade Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 23 Apr 2024 19:03:51 +0200 Subject: [PATCH 20/20] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- doc/index.rst | 1 - .../launch/gripper_mimic_joint_example_position.launch.py | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 63960b3c..6591c050 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -248,7 +248,6 @@ When the Gazebo world is launched you can run some of the following commands to The following example shows a parallel gripper with a mimic joint: - .. code-block:: shell ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_position.launch.py diff --git a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py index 4247ac4d..e3bd512f 100644 --- a/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py @@ -1,4 +1,4 @@ -# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) +# Copyright 2024 Stogl Robotics Consulting UG (haftungsbeschränkt) # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License.