diff --git a/doc/img/gz_gripper.gif b/doc/img/gz_gripper.gif new file mode 100644 index 00000000..da48c9fc Binary files /dev/null and b/doc/img/gz_gripper.gif differ diff --git a/doc/index.rst b/doc/index.rst index 268ee24a..62c7a7de 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -102,16 +102,19 @@ include: Using mimic joints in simulation ----------------------------------------------------------- -To use ``mimic`` joints in *gz_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 *gz_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 + + + + + + + - + @@ -119,16 +122,7 @@ 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. Add the gz_ros2_control plugin @@ -244,8 +238,19 @@ The following example shows parallel gripper with mimic joint: .. code-block:: shell - ros2 launch gz_ros2_control_demos gripper_mimic_joint_example.launch.py + ros2 launch gz_ros2_control_demos gripper_mimic_joint_example_position.launch.py + +.. image:: img/gz_gripper.gif + :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 gz_ros2_control_demos gripper_mimic_joint_example_effort.launch.py +instead. Send example commands: diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp index 50984e1c..363c37a1 100644 --- a/gz_ros2_control/src/gz_system.cpp +++ b/gz_ros2_control/src/gz_system.cpp @@ -78,14 +78,6 @@ struct jointData gz_ros2_control::GazeboSimSystemInterface::ControlMethod joint_control_method; }; -struct MimicJoint -{ - std::size_t joint_index; - std::size_t mimicked_joint_index; - double multiplier = 1.0; - std::vector interfaces_to_mimic; -}; - class ImuData { public: @@ -153,9 +145,6 @@ class gz_ros2_control::GazeboSimSystemPrivate /// \brief Gazebo communication node. GZ_TRANSPORT_NAMESPACE Node node; - /// \brief mapping of mimicked joints to index of joint they mimic - std::vector mimic_joints_; - /// \brief Gain which converts position error to a velocity command double position_proportional_gain_; @@ -265,58 +254,20 @@ bool GazeboSimSystem::initSim( // 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; - } - - // check joint info of mimicked joint - auto & joint_info_mimicked = hardware_info.joints[mimic_joint.mimicked_joint_index]; - const auto state_mimicked_interface = std::find_if( - joint_info_mimicked.state_interfaces.begin(), joint_info_mimicked.state_interfaces.end(), - [&mimic_joint](const hardware_interface::InterfaceInfo & interface_info) { - bool pos = interface_info.name == "position"; - if (pos) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_POSITION);} - bool vel = interface_info.name == "velocity"; - if (vel) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_VELOCITY);} - bool eff = interface_info.name == "effort"; - if (vel) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_EFFORT);} - return pos || vel || eff; - }); - if (state_mimicked_interface == joint_info_mimicked.state_interfaces.end()) { - throw std::runtime_error( - std::string( - "For mimic joint '") + joint_info.name + - "' no state interface was found in mimicked joint '" + mimicked_joint + - " ' to mimic"); - } + 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->joints_[it->mimicked_joint_index].name << + "' with multiplier: " << it->multiplier << " and offset: " << it->offset); } RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); @@ -351,7 +302,7 @@ bool GazeboSimSystem::initSim( 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->joints_[j].joint_position); initial_position = get_initial_value(joint_info.state_interfaces[i]); @@ -360,7 +311,7 @@ bool GazeboSimSystem::initSim( 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->joints_[j].joint_velocity); initial_velocity = get_initial_value(joint_info.state_interfaces[i]); @@ -369,7 +320,7 @@ bool GazeboSimSystem::initSim( 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->joints_[j].joint_effort); initial_effort = get_initial_value(joint_info.state_interfaces[i]); @@ -384,7 +335,7 @@ bool GazeboSimSystem::initSim( 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->joints_[j].joint_position_cmd); if (!std::isnan(initial_position)) { @@ -393,7 +344,7 @@ bool GazeboSimSystem::initSim( } else 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->joints_[j].joint_velocity_cmd); if (!std::isnan(initial_velocity)) { @@ -402,7 +353,7 @@ bool GazeboSimSystem::initSim( } else 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->joints_[j].joint_effort_cmd); if (!std::isnan(initial_effort)) { @@ -710,76 +661,31 @@ hardware_interface::return_type GazeboSimSystem::write( } // set values of all mimic joints with respect to mimicked joint - for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) { - if (mimic_interface == "position") { - // Get the joint position - double position_mimicked_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; + for (const auto & mimic_joint : this->info_.mimic_joints) { + // Get the joint position + double position_mimicked_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; - double position_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; + double position_mimic_joint = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - double position_error = - position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier; + double position_error = + position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier; - double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); + double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate); - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - - if (vel == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - sim::components::JointVelocityCmd({velocity_sp})); - } else if (!vel->Data().empty()) { - vel->Data()[0] = velocity_sp; - } - } - if (mimic_interface == "velocity") { - // get the velocity of mimicked joint - double velocity_mimicked_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; - - if (!this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - sim::components::JointVelocityCmd({0})); - } else { - const auto jointVelCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - *jointVelCmd = sim::components::JointVelocityCmd( - {mimic_joint.multiplier * velocity_mimicked_joint}); - } - } - if (mimic_interface == "effort") { - // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 - // Get the joint force - // const auto * jointForce = - // _ecm.Component( - // this->dataPtr->sim_joints_[j]); - if (!this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)) - { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - sim::components::JointForceCmd({0})); - } else { - const auto jointEffortCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - *jointEffortCmd = sim::components::JointForceCmd( - {mimic_joint.multiplier * - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort}); - } - } + auto vel = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + + if (vel == nullptr) { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, + sim::components::JointVelocityCmd({velocity_sp})); + } else if (!vel->Data().empty()) { + vel->Data()[0] = velocity_sp; } } diff --git a/gz_ros2_control_demos/config/gripper_controller_effort.yaml b/gz_ros2_control_demos/config/gripper_controller_effort.yaml new file mode 100644 index 00000000..ac5855eb --- /dev/null +++ b/gz_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/gz_ros2_control_demos/config/gripper_controller.yaml b/gz_ros2_control_demos/config/gripper_controller_position.yaml similarity index 100% rename from gz_ros2_control_demos/config/gripper_controller.yaml rename to gz_ros2_control_demos/config/gripper_controller_position.yaml diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py similarity index 98% rename from gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py rename to gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py index 90fbb0db..eab09938 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): " ", PathJoinSubstitution( [FindPackageShare("gz_ros2_control_demos"), - "urdf", "test_gripper_mimic_joint.xacro.urdf"] + "urdf", "test_gripper_mimic_joint_effort.xacro.urdf"] ), ] ) diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.xml b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml similarity index 98% rename from gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.xml rename to gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml index e6b29a01..a1127a48 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example.launch.xml +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml @@ -25,7 +25,7 @@ Author: Dr. Denis description="Start RViz2 automatically with this launch file."/> - + diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py new file mode 100644 index 00000000..2b196a35 --- /dev/null +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py @@ -0,0 +1,100 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# 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 (Stogl Robotics Consulting) +# + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Launch Arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=True) + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("gz_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] + ) + + gz_spawn_entity = Node( + package='ros_gz_sim', + executable='create', + output='screen', + arguments=["-topic", "robot_description", "-name", + "gripper", "-allow_renaming", "true"], + ) + + load_joint_state_broadcaster = 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([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_sim.launch.py'])]), + launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gz_spawn_entity, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_gripper_controller], + ) + ), + node_robot_state_publisher, + gz_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml new file mode 100644 index 00000000..76dd181d --- /dev/null +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gz_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf b/gz_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf new file mode 100644 index 00000000..292e3f2b --- /dev/null +++ b/gz_ros2_control_demos/urdf/test_gripper_mimic_joint_effort.xacro.urdf @@ -0,0 +1,102 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + 0.15 + + + + + + + + + + + + + + $(find gz_ros2_control_demos)/config/gripper_controller_effort.yaml + + + diff --git a/gz_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/gz_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf similarity index 94% rename from gz_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf rename to gz_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf index 555bd493..92dd872d 100644 --- a/gz_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_gripper_mimic_joint_position.xacro.urdf @@ -88,9 +88,6 @@ - right_finger_joint - 1 - @@ -99,7 +96,7 @@ - $(find gz_ros2_control_demos)/config/gripper_controller.yaml + $(find gz_ros2_control_demos)/config/gripper_controller_position.yaml