From 7c40b4bf244cec27516bde22089942b3f9454bc2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 14 Feb 2022 09:22:14 +0100 Subject: [PATCH 1/8] 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 2/8] 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 3/8] 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 4/8] 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 5/8] 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 6/8] 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 7/8] 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 8/8] 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);