Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support for mimic joints and example with gripper. #107

Merged
Merged
Show file tree
Hide file tree
Changes from 3 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 46 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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/).
Expand Down
110 changes: 88 additions & 22 deletions gazebo_ros2_control/src/gazebo_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -156,6 +167,38 @@ 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) {
Expand All @@ -173,67 +216,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)) {
Expand Down Expand Up @@ -381,9 +424,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;
Expand Down Expand Up @@ -457,6 +500,30 @@ 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) {
Expand All @@ -470,8 +537,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);
}
}
Expand Down
8 changes: 7 additions & 1 deletion gazebo_ros2_control_demos/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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}
)

Expand Down
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 2022 Stogl Robotics Consulting UG (haftungsbeschränkt)
destogl marked this conversation as resolved.
Show resolved Hide resolved
//
// 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