diff --git a/README.md b/README.md index 39a5ef09..aa358e96 100644 --- a/README.md +++ b/README.md @@ -95,9 +95,10 @@ 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 optional `multiplier`+`offset` parameters to joint definition in `` tag +As an example, `left_finger_joint` mimics the position of `right_finger_joint` ```xml @@ -110,15 +111,42 @@ We should include: ``` ```xml + + + + + + right_finger_joint 1 + 0 ``` +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 + 0 + + + + + +``` +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 @@ -245,7 +273,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: @@ -254,6 +282,16 @@ 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 command interface of the joint to be mimicked, run + +```bash +ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example_effort.launch.py +``` +instead. + + + #### 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 71ba3a9f..1303518f 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -31,7 +31,9 @@ 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 @@ -89,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 @@ -185,16 +187,34 @@ 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")); } 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 mimicking joint '" << mimicked_joint << + "' with multiplier: " << mimic_joint.multiplier << + "' and offset: " << mimic_joint.offset); this->dataPtr->mimic_joints_.push_back(mimic_joint); suffix = "_mimic"; } @@ -512,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; } @@ -569,25 +589,46 @@ 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) - { - 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.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 && - 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]; + + 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 && - 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]; + + 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]; + } } } 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_effort.launch.py similarity index 97% 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 ef3a7f75..49262dad 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 @@ -33,7 +33,7 @@ def generate_launch_description(): [FindPackageShare( 'gazebo_ros2_control_demos'), 'urdf', - 'test_gripper_mimic_joint.xacro.urdf'] + 'test_gripper_mimic_joint_effort.xacro.urdf'] ), ] ) 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..4247ac4d --- /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', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_gripper_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + '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, + ]) 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 92% 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 8065a6d1..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 @@ -1,5 +1,5 @@ - + @@ -81,6 +81,7 @@ right_finger_joint 1 + 0 @@ -90,7 +91,7 @@ - $(find gazebo_ros2_control_demos)/config/gripper_controller.yaml + $(find gazebo_ros2_control_demos)/config/gripper_controller_position.yaml