Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/master' into gz_vendor_pkgs
Browse files Browse the repository at this point in the history
  • Loading branch information
@azeey
azeey committed Apr 23, 2024
2 parents 38ac96b + d3e00a5 commit edb3167
Show file tree
Hide file tree
Showing 11 changed files with 343 additions and 155 deletions.
Binary file added doc/img/gz_gripper.gif
View file
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
View file
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
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,14 +78,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 @@ -153,9 +145,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 @@ -265,58 +254,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 @@ -351,7 +302,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 @@ -360,7 +311,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 @@ -369,7 +320,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 @@ -384,7 +335,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 @@ -393,7 +344,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 @@ -402,7 +353,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 @@ -710,76 +661,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
View file
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
0 ...trol_demos/config/gripper_controller.yaml → ...s/config/gripper_controller_position.yaml
View file
File renamed without changes.
2 changes: 1 addition & 1 deletion ...nch/gripper_mimic_joint_example.launch.py → ...pper_mimic_joint_example_effort.launch.py
View file
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
2 changes: 1 addition & 1 deletion ...ch/gripper_mimic_joint_example.launch.xml → ...per_mimic_joint_example_effort.launch.xml
View file
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

0 comments on commit edb3167

Please sign in to comment.