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

Rewrite mimic joints #276

Merged
merged 6 commits into from
Apr 23, 2024
Merged
Show file tree
Hide file tree
Changes from all 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
Binary file added doc/img/gz_gripper.gif
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
39 changes: 22 additions & 17 deletions doc/index.rst
Original file line number Diff line number Diff line change
Expand Up @@ -102,33 +102,27 @@ include:
Using mimic joints in simulation
-----------------------------------------------------------

To use ``mimic`` joints in *gz_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 *gz_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>

.. 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>
The mimic joint must not have command interfaces configured in the ``<ros2_control>`` tag, but state interfaces can be configured.


Add the gz_ros2_control plugin
Expand Down Expand Up @@ -244,8 +238,19 @@ The following example shows parallel gripper with mimic joint:

.. code-block:: shell

ros2 launch gz_ros2_control_demos gripper_mimic_joint_example.launch.py
ros2 launch gz_ros2_control_demos gripper_mimic_joint_example_position.launch.py

.. image:: img/gz_gripper.gif
: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 gz_ros2_control_demos gripper_mimic_joint_example_effort.launch.py

instead.

Send example commands:

Expand Down
170 changes: 38 additions & 132 deletions gz_ros2_control/src/gz_system.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,14 +100,6 @@ struct jointData
gz_ros2_control::GazeboSimSystemInterface::ControlMethod joint_control_method;
};

struct MimicJoint
{
std::size_t joint_index;
std::size_t mimicked_joint_index;
double multiplier = 1.0;
std::vector<std::string> interfaces_to_mimic;
};

class ImuData
{
public:
Expand Down Expand Up @@ -175,9 +167,6 @@ class gz_ros2_control::GazeboSimSystemPrivate
/// \brief Gazebo communication node.
GZ_TRANSPORT_NAMESPACE Node node;

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

/// \brief Gain which converts position error to a velocity command
double position_proportional_gain_;

Expand Down Expand Up @@ -287,58 +276,20 @@ bool GazeboSimSystem::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 = hardware_interface::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");
}
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 mutiplier: "
<< mimic_joint.multiplier);
this->dataPtr->mimic_joints_.push_back(mimic_joint);
suffix = "_mimic";
"Joint '" << joint_name << "'is mimicking joint '" <<
this->dataPtr->joints_[it->mimicked_joint_index].name <<
"' with multiplier: " << it->multiplier << " and offset: " << it->offset);
}

RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
Expand Down Expand Up @@ -373,7 +324,7 @@ bool GazeboSimSystem::initSim(
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->joints_[j].joint_position);
initial_position = get_initial_value(joint_info.state_interfaces[i]);
Expand All @@ -382,7 +333,7 @@ bool GazeboSimSystem::initSim(
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->joints_[j].joint_velocity);
initial_velocity = get_initial_value(joint_info.state_interfaces[i]);
Expand All @@ -391,7 +342,7 @@ bool GazeboSimSystem::initSim(
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->joints_[j].joint_effort);
initial_effort = get_initial_value(joint_info.state_interfaces[i]);
Expand All @@ -406,7 +357,7 @@ bool GazeboSimSystem::initSim(
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->joints_[j].joint_position_cmd);
if (!std::isnan(initial_position)) {
Expand All @@ -415,7 +366,7 @@ bool GazeboSimSystem::initSim(
} 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 + suffix,
joint_name,
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joints_[j].joint_velocity_cmd);
if (!std::isnan(initial_velocity)) {
Expand All @@ -424,7 +375,7 @@ bool GazeboSimSystem::initSim(
} else 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->joints_[j].joint_effort_cmd);
if (!std::isnan(initial_effort)) {
Expand Down Expand Up @@ -732,76 +683,31 @@ hardware_interface::return_type GazeboSimSystem::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<sim::components::JointPosition>(
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
for (const auto & mimic_joint : this->info_.mimic_joints) {
// Get the joint position
double position_mimicked_joint =
this->dataPtr->ecm->Component<sim::components::JointPosition>(
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];

double position_mimic_joint =
this->dataPtr->ecm->Component<sim::components::JointPosition>(
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
double position_mimic_joint =
this->dataPtr->ecm->Component<sim::components::JointPosition>(
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];

double position_error =
position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier;
double position_error =
position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier;

double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);
double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);

auto vel =
this->dataPtr->ecm->Component<sim::components::JointVelocityCmd>(
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,
sim::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<sim::components::JointVelocity>(
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];

if (!this->dataPtr->ecm->Component<sim::components::JointVelocityCmd>(
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
{
this->dataPtr->ecm->CreateComponent(
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
sim::components::JointVelocityCmd({0}));
} else {
const auto jointVelCmd =
this->dataPtr->ecm->Component<sim::components::JointVelocityCmd>(
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
*jointVelCmd = sim::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<sim::components::JointForce>(
// this->dataPtr->sim_joints_[j]);
if (!this->dataPtr->ecm->Component<sim::components::JointForceCmd>(
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
{
this->dataPtr->ecm->CreateComponent(
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
sim::components::JointForceCmd({0}));
} else {
const auto jointEffortCmd =
this->dataPtr->ecm->Component<sim::components::JointForceCmd>(
this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
*jointEffortCmd = sim::components::JointForceCmd(
{mimic_joint.multiplier *
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort});
}
}
auto vel =
this->dataPtr->ecm->Component<sim::components::JointVelocityCmd>(
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,
sim::components::JointVelocityCmd({velocity_sp}));
} else if (!vel->Data().empty()) {
vel->Data()[0] = velocity_sp;
}
}

Expand Down
15 changes: 15 additions & 0 deletions gz_ros2_control_demos/config/gripper_controller_effort.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: effort
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ def generate_launch_description():
" ",
PathJoinSubstitution(
[FindPackageShare("gz_ros2_control_demos"),
"urdf", "test_gripper_mimic_joint.xacro.urdf"]
"urdf", "test_gripper_mimic_joint_effort.xacro.urdf"]
),
]
)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ Author: Dr. Denis
description="Start RViz2 automatically with this launch file."/>

<!-- Load description and start controllers -->
<let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share gz_ros2_control_demos)/urdf/test_gripper_mimic_joint.xacro.urdf')" />
<let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share gz_ros2_control_demos)/urdf/test_gripper_mimic_joint_effort.xacro.urdf')" />

<node pkg="robot_state_publisher" exec="robot_state_publisher" output="both">
<param name="robot_description" value="$(var robot_description_content)" />
Expand Down
Loading
Loading