From dfba5d70a1d2d7fdffdde95022c48f6287263063 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sun, 6 Feb 2022 00:30:44 +0100 Subject: [PATCH 1/5] Add code for supporting mimic joints. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Denis Štogl --- README.md | 47 ++++++ ign_ros2_control/src/ign_system.cpp | 141 +++++++++++++++--- ign_ros2_control_demos/CMakeLists.txt | 7 + .../config/gripper_controller.yaml | 15 ++ .../examples/example_gripper.cpp | 61 ++++++++ .../gripper_mimic_joint_example.launch.py | 99 ++++++++++++ .../urdf/test_gripper_mimic_joint.xacro.urdf | 96 ++++++++++++ 7 files changed, 445 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..efe0103e 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -72,6 +72,13 @@ 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; +}; + 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,38 @@ 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; + } + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "Joint '" << joint_name << "'is mimicing 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 +261,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 +294,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 +409,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; @@ -558,7 +602,62 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); *jointEffortCmd = ignition::gazebo::components::JointForceCmd( - {this->dataPtr->joints_[i].joint_effort_cmd}); + {this->dataPtr->joints_[i].joint_effort_cmd});Ž + } + } + } + + // set values of all mimic joints with respect to mimicked joint + for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { + if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & POSITION && + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & POSITION) + { + 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::JointPositionReset( + {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position})); + const auto jointPosCmd = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); + *jointPosCmd = ignition::gazebo::components::JointPositionReset( + {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position_cmd}); + } + } + if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & VELOCITY && + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) + { + 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 * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd}); + } + } + if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & EFFORT && + this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) + { + 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_cmd}); } } } 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..51b3bd98 --- /dev/null +++ b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -0,0 +1,99 @@ +# 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) +# + +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( + 'ign_ros2_control_demos'), + 'urdf', + 'test_gripper_mimic_joint.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] + ) + + ignition = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("ros_ign_gazebo"), "/launch", "/ign_gazebo.launch.py"] + ), + launch_arguments=[('ign_args', [' -r -v 4 empty.sdf'])], + ) + + spawn_entity = Node( + package="ros_ign_gazebo", + executable="create", + output="screen", + arguments=[ + "-string", + robot_description_content, + "-name", + "gripper", + "-allow_renaming", + "true", + ], + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 'joint_state_broadcaster'], + output='screen' + ) + + load_gripper_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + '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], + ) + ), + ignition, + node_robot_state_publisher, + spawn_entity, + ]) 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 + + + From 70e02d1053bdc95d594840e4882f06e481a124fb Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 14 Apr 2022 12:50:05 +0200 Subject: [PATCH 2/5] Improve joint mimic behavior. --- ign_ros2_control/src/ign_system.cpp | 161 ++++++++++++++++++---------- 1 file changed, 103 insertions(+), 58 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index efe0103e..51db43be 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 @@ -77,6 +76,7 @@ struct MimicJoint std::size_t joint_index; std::size_t mimicked_joint_index; double multiplier = 1.0; + std::vector interfaces_to_mimic; }; class ImuData @@ -223,10 +223,11 @@ bool IgnitionSystem::initSim( 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"); + throw std::runtime_error( + std::string("Mimicked joint '") + mimicked_joint + "' not found"); } - MimicJoint mimic_joint; + + MimicJoint mimic_joint; mimic_joint.joint_index = j; mimic_joint.mimicked_joint_index = std::distance( hardware_info.joints.begin(), mimicked_joint_it); @@ -236,12 +237,32 @@ bool IgnitionSystem::initSim( } else { mimic_joint.multiplier = 1.0; } - RCLCPP_INFO_STREAM( + + // 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 mimicing joint '" << mimicked_joint << "' with mutiplier: " << mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); - suffix = "_mimic"; + suffix = "_mimic"; + } RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); @@ -602,64 +623,88 @@ hardware_interface::return_type IgnitionSystem::write( this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); *jointEffortCmd = ignition::gazebo::components::JointForceCmd( - {this->dataPtr->joints_[i].joint_effort_cmd});Ž + {this->dataPtr->joints_[i].joint_effort_cmd}); } } } // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { - if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & POSITION && - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & POSITION) - { - 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::JointPositionReset( - {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position})); - const auto jointPosCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - *jointPosCmd = ignition::gazebo::components::JointPositionReset( - {mimic_joint.multiplier * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position_cmd}); - } - } - if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & VELOCITY && - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) - { - 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 * this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_cmd}); - } - } - if (this->dataPtr->joints_[mimic_joint.joint_index].joint_control_method & EFFORT && - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_control_method & VELOCITY) - { - 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_cmd}); - } - } + + 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; From 03ae76654d72e9b8bae9eda1e3f694809c05a83c Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 19 Apr 2022 16:36:59 +0200 Subject: [PATCH 3/5] Get robot description as param. --- .../gripper_mimic_joint_example.launch.py | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) 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 index 51b3bd98..97871586 100644 --- a/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py +++ b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -51,7 +51,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( [FindPackageShare("ros_ign_gazebo"), "/launch", "/ign_gazebo.launch.py"] ), - launch_arguments=[('ign_args', [' -r -v 4 empty.sdf'])], + launch_arguments=[('ign_args', [' -r -v 3 empty.sdf'])], ) spawn_entity = Node( @@ -59,13 +59,26 @@ def generate_launch_description(): executable="create", output="screen", arguments=[ - "-string", - robot_description_content, + "-param", + "robot_description", "-name", "gripper", "-allow_renaming", "true", + "-x", + "0.0", + "-y", + "0.0", + "-z", + "0.0", + "-R", + "0.0", + "-P", + "0.0", + "-Y", + "0.0", ], + parameters=[robot_description] ) load_joint_state_controller = ExecuteProcess( From ce69cb62acc678b36eba6186cb3e39c495e22513 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 25 Aug 2022 17:02:15 +0200 Subject: [PATCH 4/5] Follow launch file template from other examples. --- .../gripper_mimic_joint_example.launch.py | 108 ++++++++---------- 1 file changed, 50 insertions(+), 58 deletions(-) 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 index 97871586..6018dc9d 100644 --- a/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py +++ b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py @@ -15,98 +15,90 @@ # 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 ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +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, PathJoinSubstitution +from launch.substitutions import LaunchConfiguration + from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare + +import xacro def generate_launch_description(): + # Launch arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=True) - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), - " ", - PathJoinSubstitution( - [FindPackageShare( - 'ign_ros2_control_demos'), - 'urdf', - 'test_gripper_mimic_joint.xacro.urdf'] - ), - ] - ) - robot_description = {"robot_description": robot_description_content} + 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=[robot_description] - ) - - ignition = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [FindPackageShare("ros_ign_gazebo"), "/launch", "/ign_gazebo.launch.py"] - ), - launch_arguments=[('ign_args', [' -r -v 3 empty.sdf'])], + parameters=[params] ) - spawn_entity = Node( - package="ros_ign_gazebo", - executable="create", - output="screen", - arguments=[ - "-param", - "robot_description", - "-name", - "gripper", - "-allow_renaming", - "true", - "-x", - "0.0", - "-y", - "0.0", - "-z", - "0.0", - "-R", - "0.0", - "-P", - "0.0", - "-Y", - "0.0", - ], - parameters=[robot_description] + ignition_spawn_entity = Node( + package='ros_ign_gazebo', + executable='create', + output='screen', + arguments=['-string', doc.toxml(), + '-name', 'gripper', + '-allow_renaming', 'true'], ) - load_joint_state_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 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', 'start', + 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=spawn_entity, - on_exit=[load_joint_state_controller], + event_handler=OnProcessExit( + target_action=ignition_spawn_entity, + on_exit=[load_joint_state_broadcaster], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_controller, + target_action=load_joint_state_broadcaster, on_exit=[load_gripper_controller], ) ), - ignition, node_robot_state_publisher, - spawn_entity, + ignition_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), ]) From 67ae9a90cf82cc20775ad9a7ddad9cc52cb11cbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Wed, 31 Aug 2022 17:31:28 +0200 Subject: [PATCH 5/5] Fixup linters. --- ign_ros2_control/src/ign_system.cpp | 186 ++++++++++++++-------------- 1 file changed, 91 insertions(+), 95 deletions(-) diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp index 51db43be..760ac77c 100644 --- a/ign_ros2_control/src/ign_system.cpp +++ b/ign_ros2_control/src/ign_system.cpp @@ -223,11 +223,11 @@ bool IgnitionSystem::initSim( 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"); + throw std::runtime_error( + std::string("Mimicked joint '") + mimicked_joint + "' not found"); } - MimicJoint mimic_joint; + MimicJoint mimic_joint; mimic_joint.joint_index = j; mimic_joint.mimicked_joint_index = std::distance( hardware_info.joints.begin(), mimicked_joint_it); @@ -240,29 +240,30 @@ bool IgnitionSystem::initSim( // 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"); + 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( + RCLCPP_INFO_STREAM( this->nh_->get_logger(), - "Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << "' with mutiplier: " + "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << "' with mutiplier: " << mimic_joint.multiplier); this->dataPtr->mimic_joints_.push_back(mimic_joint); - suffix = "_mimic"; - + suffix = "_mimic"; } RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); @@ -630,81 +631,76 @@ 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]; - 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}); - } - } + 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;