diff --git a/doc/index.rst b/doc/index.rst index daff5f2c..6591c050 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, set the ```` tag to the mimicked joint (see the `URDF specification `__) .. code-block:: xml + + + + + + + - + @@ -118,18 +121,11 @@ We should include: +The mimic joint must not have command interfaces configured in the ```` tag, but state interfaces can be configured. -.. code-block:: xml - - - right_finger_joint - 1 - - - - - +.. 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 ========================================== @@ -250,19 +246,25 @@ 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 + :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 .. 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 diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 243bd3a8..1a573cab 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 multiplier: " << 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)) { @@ -340,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( @@ -551,10 +534,10 @@ GazeboSystem::perform_command_mode_switch( } } - // mimic joint has the same control mode as mimicked joint - for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + // 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] = - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index]; + static_cast(POSITION); } return hardware_interface::return_type::OK; } @@ -610,25 +593,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++) { 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 85b65398..a016c2f9 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..e3bd512f --- /dev/null +++ b/gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py @@ -0,0 +1,88 @@ +# 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. +# 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..dc5ee4de --- /dev/null +++ b/gazebo_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + 0.15 + + + + + + + + + + + + + + $(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 89% 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..500cb6f2 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 @@ - + @@ -59,7 +59,7 @@ - + @@ -79,9 +79,6 @@ - right_finger_joint - 1 - @@ -90,7 +87,7 @@ - $(find gazebo_ros2_control_demos)/config/gripper_controller.yaml + $(find gazebo_ros2_control_demos)/config/gripper_controller_position.yaml