Skip to content

Commit

Permalink
Rewrite mimic joints (#297)
Browse files Browse the repository at this point in the history 
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
  • Loading branch information
@christophfroehlich @ahcorde
christophfroehlich and ahcorde authored Apr 23, 2024
1 parent 105c0ba commit 7b17bcc
Show file tree
Hide file tree
Showing 8 changed files with 257 additions and 94 deletions.
44 changes: 23 additions & 21 deletions doc/index.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,35 +101,31 @@ 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:

* ``<mimic>`` tag to the mimicked joint `detailed manual <https://wiki.ros.org/urdf/XML/joint>`__
* ``mimic`` and ``multiplier`` parameters to joint definition in ``<ros2_control>`` tag
To use ``mimic`` joints in *gazebo_ros2_control* you should define its parameters in your URDF, i.e, set the ``<mimic>`` tag to the mimicked joint (see the `URDF specification <https://wiki.ros.org/urdf/XML/joint>`__)

.. code-block:: xml
<joint name="right_finger_joint" type="prismatic">
<axis xyz="0 1 0"/>
<origin xyz="0.0 -0.48 1" rpy="0.0 0.0 0.0"/>
<parent link="base"/>
<child link="finger_right"/>
<limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
</joint>
<joint name="left_finger_joint" type="prismatic">
<mimic joint="right_finger_joint"/>
<mimic joint="right_finger_joint" multiplier="1" offset="0"/>
<axis xyz="0 1 0"/>
<origin xyz="0.0 0.48 1" rpy="0.0 0.0 3.1415926535"/>
<parent link="base"/>
<child link="finger_left"/>
<limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
</joint>
The mimic joint must not have command interfaces configured in the ``<ros2_control>`` tag, but state interfaces can be configured.

.. code-block:: xml
<joint name="left_finger_joint">
<param name="mimic">right_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
.. note::

Independent of the interface type of the mimicked joint in the ``<ros2_control>`` 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
==========================================
Expand Down Expand Up @@ -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
Expand Down
100 changes: 34 additions & 66 deletions gazebo_ros2_control/src/gazebo_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -93,7 +86,7 @@ class gazebo_ros2_control::GazeboSystemPrivate
/// \brief handles to the FT sensors from within Gazebo
std::vector<gazebo::sensors::ForceTorqueSensorPtr> 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<std::array<double, 6>> ft_sensor_data_;

/// \brief state interfaces that will be exported to the Resource Manager
Expand All @@ -102,9 +95,6 @@ class gazebo_ros2_control::GazeboSystemPrivate
/// \brief command interfaces that will be exported to the Resource Manager
std::vector<hardware_interface::CommandInterface> command_interfaces_;

/// \brief mapping of mimicked joints to index of joint they mimic
std::vector<MimicJoint> mimic_joints_;

// Should hold the joints if no control_mode is active
bool hold_joints_ = true;
};
Expand Down Expand Up @@ -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:");
Expand Down Expand Up @@ -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]);
Expand All @@ -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]);
Expand All @@ -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]);
Expand All @@ -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)) {
Expand All @@ -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)) {
Expand All @@ -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)) {
Expand All @@ -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(
Expand Down Expand Up @@ -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<ControlMethod>(POSITION);
}
return hardware_interface::return_type::OK;
}
Expand Down Expand Up @@ -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++) {
Expand Down
15 changes: 15 additions & 0 deletions gazebo_ros2_control_demos/config/gripper_controller_effort.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
0 ...trol_demos/config/gripper_controller.yaml → ...s/config/gripper_controller_position.yaml
View file
File renamed without changes.
2 changes: 1 addition & 1 deletion ...nch/gripper_mimic_joint_example.launch.py → ...pper_mimic_joint_example_effort.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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']
),
]
)
Expand Down
88 changes: 88 additions & 0 deletions gazebo_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py
View file
Original file line number Diff line number Diff line change
@@ -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,
])
Loading

0 comments on commit 7b17bcc

Please sign in to comment.