Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Support for mimic joints and example with gripper.
Browse files Browse the repository at this point in the history
Signed-off-by: Denis Štogl <[email protected]>
destogl committed Jan 31, 2022
1 parent 8a05f4c commit 36d6e63
Showing 8 changed files with 400 additions and 23 deletions.
46 changes: 46 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
@@ -88,6 +88,37 @@ include
</ros2_control>
```

### 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

```xml
<joint name="left_finger_joint" type="prismatic">
<mimic joint="right_finger_joint"/>
<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>
```

```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>
```


## Add the gazebo_ros2_control plugin

In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that
@@ -202,6 +233,21 @@ ros2 run gazebo_ros2_control_demos example_velocity
ros2 run gazebo_ros2_control_demos example_effort
```

The following example shows parallel gripper with mimic joint:

![](img/gripper.gif)


```bash
ros2 launch gazebo_ros2_control_demos gripper_mimic_joint_example.launch.py
```

Send example commands:

```bash
ros2 run gazebo_ros2_control_demos example_gripper
```

#### Gazebo + Moveit2 + ROS 2

This example works with [ROS 2 Foxy](https://index.ros.org/doc/ros2/Installation/Foxy/).
111 changes: 89 additions & 22 deletions gazebo_ros2_control/src/gazebo_system.cpp
Original file line number Diff line number Diff line change
@@ -27,6 +27,13 @@
#include "hardware_interface/hardware_info.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:
@@ -90,6 +97,9 @@ 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_;
};

namespace gazebo_ros2_control
@@ -142,7 +152,8 @@ void GazeboSystem::registerJoints(
this->dataPtr->joint_effort_cmd_.resize(this->dataPtr->n_dof_);

for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
std::string joint_name = this->dataPtr->joint_names_[j] = hardware_info.joints[j].name;
auto & joint_info = hardware_info.joints[j];
std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;

gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name);
this->dataPtr->sim_joints_.push_back(simjoint);
@@ -156,6 +167,42 @@ 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 = 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) {
@@ -173,67 +220,67 @@ void GazeboSystem::registerJoints(
double initial_effort = std::numeric_limits<double>::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->joint_position_[j]);
initial_position = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
initial_position = get_initial_value(joint_info.state_interfaces[i]);
this->dataPtr->joint_position_[j] = 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->joint_velocity_[j]);
initial_velocity = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
initial_velocity = get_initial_value(joint_info.state_interfaces[i]);
this->dataPtr->joint_velocity_[j] = 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->joint_effort_[j]);
initial_effort = get_initial_value(hardware_info.joints[j].state_interfaces[i]);
initial_effort = get_initial_value(joint_info.state_interfaces[i]);
this->dataPtr->joint_effort_[j] = initial_effort;
}
}

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->joint_control_methods_[j] |= POSITION;
this->dataPtr->command_interfaces_.emplace_back(
joint_name,
joint_name + suffix,
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_cmd_[j]);
if (!std::isnan(initial_position)) {
this->dataPtr->joint_position_cmd_[j] = initial_position;
}
}
if (hardware_info.joints[j].command_interfaces[i].name == "velocity") {
if (joint_info.command_interfaces[i].name == "velocity") {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
this->dataPtr->joint_control_methods_[j] |= VELOCITY;
this->dataPtr->command_interfaces_.emplace_back(
joint_name,
joint_name + suffix,
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_cmd_[j]);
if (!std::isnan(initial_velocity)) {
this->dataPtr->joint_velocity_cmd_[j] = initial_velocity;
}
}
if (hardware_info.joints[j].command_interfaces[i].name == "effort") {
if (joint_info.command_interfaces[i].name == "effort") {
this->dataPtr->joint_control_methods_[j] |= 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->joint_effort_cmd_[j]);
if (!std::isnan(initial_effort)) {
@@ -381,9 +428,9 @@ void GazeboSystem::registerSensors(
}

CallbackReturn
GazeboSystem::on_init(const hardware_interface::HardwareInfo & actuator_info)
GazeboSystem::on_init(const hardware_interface::HardwareInfo & system_info)
{
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;
@@ -457,6 +504,27 @@ hardware_interface::return_type GazeboSystem::write()
rclcpp::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
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];
}
if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY &&
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];
}
if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT &&
this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) {
this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
}
}

for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) {
if (this->dataPtr->sim_joints_[j]) {
if (this->dataPtr->joint_control_methods_[j] & POSITION) {
@@ -470,8 +538,7 @@ hardware_interface::return_type GazeboSystem::write()
this->dataPtr->joint_velocity_cmd_[j]);
}
if (this->dataPtr->joint_control_methods_[j] & EFFORT) {
const double effort =
this->dataPtr->joint_effort_cmd_[j];
const double effort = this->dataPtr->joint_effort_cmd_[j];
this->dataPtr->sim_joints_[j]->SetForce(0, effort);
}
}
8 changes: 7 additions & 1 deletion gazebo_ros2_control_demos/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -46,6 +46,12 @@ ament_target_dependencies(example_effort
std_msgs
)

add_executable(example_gripper examples/example_gripper)
ament_target_dependencies(example_gripper
rclcpp
std_msgs
)

if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
@@ -54,7 +60,7 @@ if(BUILD_TESTING)
endif()

## Install
install(TARGETS example_position example_velocity example_effort
install(TARGETS example_position example_velocity example_effort example_gripper
DESTINATION lib/${PROJECT_NAME}
)

15 changes: 15 additions & 0 deletions gazebo_ros2_control_demos/config/gripper_controller.yaml
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: position
58 changes: 58 additions & 0 deletions gazebo_ros2_control_demos/examples/example_gripper.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
// Copyright 2020 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.

#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"

#include "std_msgs/msg/float64_multi_array.hpp"

std::shared_ptr<rclcpp::Node> node;

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);

node = std::make_shared<rclcpp::Node>("gripper_test_node");

auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
"/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;
}
Loading

0 comments on commit 36d6e63

Please sign in to comment.