From 2dd0a0877ec3ee52eb7a25a509efdb28c68fb67a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Thu, 1 Sep 2022 18:54:38 +0200 Subject: [PATCH] Add support for mimic joints. (#33) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Denis Štogl Co-authored-by: Lovro --- README.md | 47 +++++ ign_ros2_control/src/ign_system.cpp | 182 ++++++++++++++++-- ign_ros2_control_demos/CMakeLists.txt | 7 + .../config/gripper_controller.yaml | 15 ++ .../examples/example_gripper.cpp | 61 ++++++ .../gripper_mimic_joint_example.launch.py | 104 ++++++++++ .../urdf/test_gripper_mimic_joint.xacro.urdf | 96 +++++++++ 7 files changed, 491 insertions(+), 21 deletions(-) create mode 100644 ign_ros2_control_demos/config/gripper_controller.yaml create mode 100644 ign_ros2_control_demos/examples/example_gripper.cpp create mode 100644 ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py create mode 100644 ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf diff --git a/README.md b/README.md index 28f5b990..c4346bc6 100644 --- a/README.md +++ b/README.md @@ -118,6 +118,38 @@ include: ``` + +### Using mimic joints in simulation + +To use `mimic` joints in `ign_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 + +```xml + + + + + + + + +``` + +```xml + + right_finger_joint + 1 + + + + + +``` + + ## Add the ign_ros2_control plugin In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that @@ -233,3 +265,18 @@ ros2 run ign_ros2_control_demos example_effort ros2 run ign_ros2_control_demos example_diff_drive ros2 run ign_ros2_control_demos example_tricycle_drive ``` + +The following example shows parallel gripper with mimic joint: + +![](img/gripper.gif) + + +```bash +ros2 launch ign_ros2_control_demos gripper_mimic_joint_example.launch.py +``` + +Send example commands: + +```bash +ros2 run ign_ros2_control_demos example_gripper +``` diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 09410963..760ac77c 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -28,7 +28,6 @@ #include #include #include -#include #include #include #include @@ -72,6 +71,14 @@ struct jointData ign_ros2_control::IgnitionSystemInterface::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: @@ -138,6 +145,9 @@ class ign_ros2_control::IgnitionSystemPrivate /// \brief Ignition communication node. ignition::transport::Node node; + + /// \brief mapping of mimicked joints to index of joint they mimic + std::vector mimic_joints_; }; namespace ign_ros2_control @@ -169,7 +179,8 @@ bool IgnitionSystem::initSim( } for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { - std::string joint_name = this->dataPtr->joints_[j].name = hardware_info.joints[j].name; + auto & joint_info = hardware_info.joints[j]; + std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name; ignition::gazebo::Entity simjoint = enableJoints[joint_name]; this->dataPtr->joints_[j].sim_joint = simjoint; @@ -201,6 +212,60 @@ bool IgnitionSystem::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 = std::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"); + } + 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"; + } + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); auto get_initial_value = [this](const hardware_interface::InterfaceInfo & interface_info) { @@ -218,32 +283,32 @@ bool IgnitionSystem::initSim( double initial_effort = std::numeric_limits::quiet_NaN(); // register the state handles - for (unsigned int i = 0; i < hardware_info.joints[j].state_interfaces.size(); ++i) { - if (hardware_info.joints[j].state_interfaces[i].name == "position") { + for (unsigned int i = 0; i < joint_info.state_interfaces.size(); ++i) { + 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, + joint_name + suffix, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position); - initial_position = get_initial_value(hardware_info.joints[j].state_interfaces[i]); + initial_position = get_initial_value(joint_info.state_interfaces[i]); this->dataPtr->joints_[j].joint_position = initial_position; } - if (hardware_info.joints[j].state_interfaces[i].name == "velocity") { + 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, + joint_name + suffix, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity); - initial_velocity = get_initial_value(hardware_info.joints[j].state_interfaces[i]); + initial_velocity = get_initial_value(joint_info.state_interfaces[i]); this->dataPtr->joints_[j].joint_velocity = initial_velocity; } - if (hardware_info.joints[j].state_interfaces[i].name == "effort") { + 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, + joint_name + suffix, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort); - initial_effort = get_initial_value(hardware_info.joints[j].state_interfaces[i]); + initial_effort = get_initial_value(joint_info.state_interfaces[i]); this->dataPtr->joints_[j].joint_effort = initial_effort; } } @@ -251,29 +316,30 @@ bool IgnitionSystem::initSim( RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:"); // register the command handles - for (unsigned int i = 0; i < hardware_info.joints[j].command_interfaces.size(); ++i) { - if (hardware_info.joints[j].command_interfaces[i].name == "position") { + for (unsigned int i = 0; i < joint_info.command_interfaces.size(); ++i) { + 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, + joint_name + suffix, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position_cmd); if (!std::isnan(initial_position)) { this->dataPtr->joints_[j].joint_position_cmd = initial_position; } - } else if (hardware_info.joints[j].command_interfaces[i].name == "velocity") { + } 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, + joint_name + suffix, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity_cmd); if (!std::isnan(initial_velocity)) { this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity; } - } else if (hardware_info.joints[j].command_interfaces[i].name == "effort") { + } else if (joint_info.command_interfaces[i].name == "effort") { + this->dataPtr->joints_[j].joint_control_method |= EFFORT; RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->command_interfaces_.emplace_back( - joint_name, + joint_name + suffix, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort_cmd); if (!std::isnan(initial_effort)) { @@ -365,10 +431,10 @@ void IgnitionSystem::registerSensors( } CallbackReturn -IgnitionSystem::on_init(const hardware_interface::HardwareInfo & actuator_info) +IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info) { RCLCPP_WARN(this->nh_->get_logger(), "On init..."); - if (hardware_interface::SystemInterface::on_init(actuator_info) != CallbackReturn::SUCCESS) { + if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; @@ -563,6 +629,80 @@ hardware_interface::return_type IgnitionSystem::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]; + + 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 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, + ignition::gazebo::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, + ignition::gazebo::components::JointVelocityCmd({0})); + } else { + const auto jointVelCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointVelCmd = ignition::gazebo::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, + ignition::gazebo::components::JointForceCmd({0})); + } else { + const auto jointEffortCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointEffortCmd = ignition::gazebo::components::JointForceCmd( + {mimic_joint.multiplier * + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort}); + } + } + } + } + return hardware_interface::return_type::OK; } } // namespace ign_ros2_control diff --git a/ign_ros2_control_demos/CMakeLists.txt b/ign_ros2_control_demos/CMakeLists.txt index 838a43d8..e1031c2e 100644 --- a/ign_ros2_control_demos/CMakeLists.txt +++ b/ign_ros2_control_demos/CMakeLists.txt @@ -59,6 +59,12 @@ ament_target_dependencies(example_tricycle_drive geometry_msgs ) +add_executable(example_gripper examples/example_gripper.cpp) +ament_target_dependencies(example_gripper + rclcpp + std_msgs +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) @@ -74,6 +80,7 @@ install( example_effort example_diff_drive example_tricycle_drive + example_gripper DESTINATION lib/${PROJECT_NAME} ) diff --git a/ign_ros2_control_demos/config/gripper_controller.yaml b/ign_ros2_control_demos/config/gripper_controller.yaml new file mode 100644 index 00000000..d72296e1 --- /dev/null +++ b/ign_ros2_control_demos/config/gripper_controller.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: position diff --git a/ign_ros2_control_demos/examples/example_gripper.cpp b/ign_ros2_control_demos/examples/example_gripper.cpp new file mode 100644 index 00000000..449cf6ab --- /dev/null +++ b/ign_ros2_control_demos/examples/example_gripper.cpp @@ -0,0 +1,61 @@ +// Copyright 2022 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 Štogl (Stogl Robotics Consulting) +// + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/float64_multi_array.hpp" + +std::shared_ptr node; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + node = std::make_shared("gripper_test_node"); + + auto publisher = node->create_publisher( + "/gripper_controller/commands", 10); + + RCLCPP_INFO(node->get_logger(), "node created"); + + std_msgs::msg::Float64MultiArray commands; + + using namespace std::chrono_literals; + + commands.data.push_back(0); + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.38; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0.19; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + rclcpp::shutdown(); + + return 0; +} diff --git a/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py new file mode 100644 index 00000000..6018dc9d --- /dev/null +++ b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -0,0 +1,104 @@ +# Copyright 2022 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) +# + +import os + +from ament_index_python.packages import get_package_share_directory + + +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 LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + # Launch arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=True) + + ignition_ros2_control_demos_path = os.path.join( + get_package_share_directory('ign_ros2_control_demos')) + + xacro_file = os.path.join(ignition_ros2_control_demos_path, + 'urdf', + 'test_gripper_mimic_joint.xacro.urdf') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + ignition_spawn_entity = Node( + package='ros_ign_gazebo', + executable='create', + output='screen', + arguments=['-string', doc.toxml(), + '-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( + [os.path.join(get_package_share_directory('ros_ign_gazebo'), + 'launch', 'ign_gazebo.launch.py')]), + launch_arguments=[('ign_args', [' -r -v 4 empty.sdf'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=ignition_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, + ignition_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf new file mode 100644 index 00000000..8e0074f4 --- /dev/null +++ b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf @@ -0,0 +1,96 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + 0.15 + + + + + + right_finger_joint + 1 + + + + + + + + + + $(find ign_ros2_control_demos)/config/gripper_controller.yaml + + +