[gazebo_plugins] Plugin for mimic joint support?

[fmessmer]

Hi, I've spend a good time searching the mailinglists and both answers.ros.org and answers.gazebosim.org now in order to find a solution for supporting mimic joints in gazebo.
I've found several old threads (from 2011) discussing that mimic joints are not supported, but none of those actually proposed a solution.

• There is this one http://answers.ros.org/question/60846/urdf-joint-mimic-tag-not-functioning/ that leads to a fix in the joint_state_publisher in order to be able to have the according frames for the links connected by mimic joints (mimic tag support ros/robot_state_publisher#1)
• There is also this one http://answers.ros.org/question/10249/gazebo-mimic-joints-property/ that leads to a ticket that never has been processed https://code.ros.org/trac/ros-pkg/ticket/5005
• I also found this enhancement issue for gazebo https://bitbucket.org/osrf/gazebo/issue/1058/update-gearbox-joint-definition

My usecase is that I have various kinematical models (e.g. parallel gripper, Schunk SDH,...) where several joints are "linked" together or "mimic" a second joint.
For example the Schunk SDH has two of the fingers being actuated by the same motor. In the URDF (https:/ipa-fxm/schunk_modular_robotics/tree/indigo_dev/schunk_description/urdf/sdh) ${name}_finger_21_joint is supposed to mimic the "active joint" ${name}_knuckle_joint. However, in gazebo simulation only ${name}_knuckle_joint is actuated, and ${name}_finger_21_joint is actually just "dangling in the air" as if it is not actuated at all.
Commands are send to the SDH as a FollowJointTrajectoryAction where values ar only given for ${name}_knuckle_joint (and the others) but NOT for ${name}_finger_21_joint.

My question is now, should it be possible to actuate a joint that is using the mimic tag in current gazebo (Trusty, Indigo, gazebo 2.2.3-1~trusty) or is this really currently not supported?
If it should be possible, does someone have an idea what's possibly wrong with the SDH? ros_controllers parameter configuration?
It it is indeed NOT supported, are there ideas how this could be realized? Would it be possilbe to implement a gazebo_plugin, that simply "copies" the commands from the "active joint" to the respective "mimic joint"?

Any suggestions or thoughts?

[rethink-imcmahon]

Does the Gazebo MimicJointPlugin created by @costashatz solve this mimic in Gazebo issue? Was would be required to get this plugin to be included with gazebo_ros_pkgs by default?

[carlosjoserg]

I tried it once, I had around 20 mimic joints, and the simulation was really, really slow.

I then created my own robot plugin to handle the mimicking within the HWiface. I was recently talking about this with @arocchi, who has experience with low-level stuff, and he tells me that perhaps a better way to do this is to pass the mimic info as a constraint to ODE directly.

What would you think?

[rethink-imcmahon]

Admittedly, I have not tried the plugin out myself, so I have yet to see the mimic slowness. Passing the mimic info as a constraint to ODE does sound more elegant. What would it take to put together a base implementation of the constraint passing?

[costashatz]

This plugin was developed as a personal need for a quick fix as gazebo wasn't supporting mimic joints. Thus, it is obviously not very well tuned and/or optimized. I have only used it in robots with 3-4 mimic joints. But, @carlosjoserg , I can't really see how it could slow down the simulation. The whole plugin is barely one line of code:

double effort = gazebo::math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_);

Can you explain how it could slow down the simulation? If you had one simple joint (not mimic via ros_control interface), it'd use the same pid controller.

On the other hand, if I am not mistaken, Gazebo has a generic interface over several physics engines (ode, bullet, dart, etc.). So, if there's an actual need to implement mimic joints into the physics engine, then someone should probably implement it not only in ODE, but in the other libraries too. I am sure it's not that difficult to do, but I am just sharing my thoughts.

@carlosjoserg , I am curious about how you implemented the plugin within the HWiface. It'd be nice to share it (via e-mail for e.g.).

[carlosjoserg]

I know, very simple thing to do, but I was also combining it with the disable link plugin, so it is hard to tell what was causing all to be slow.

In any case, it is open-cource what we did, here is the corresponding part within the write simulation function for the mimic joints. So, we are passing directly the value as a set position instead of using PID control, which is not the best, but so far, working decently. The whole piece of code is a mess, WIP.

In fact, there is an old reply to the same question, by Hsu, himself that suggest to implement this as a constraint.

I suppose the best thing to do is to translate the specification into something already existing in ODE (of course, passing through the gazebo interface), and the same would go for other engines.

[costashatz]

I agree with you and Hsu. The best thing to do, of course, is to implement it as a constraint to the physics engine(s).

As I said, my plugin was a quick workaround.

[arocchi]

In fact, all the underlying structures are already there. As stated here they have some "compatibility" issues between the mimic tag and the underlying implementation of the transmission elements in Gazebo.

More in detail, the stock ODE and the Gazebo ODE have two different ways to implement transmission, and consequently "mimic" joints.
The stock ODE uses Transmission (including gearbox-like transmission in the case where the joint axes are parallel ), while the Gazebo ODE uses Gearbox joints ... You can see that the ODE Transmission implements also backlash: in general, the implementation between the two is similar, even though the Gazebo is simpler, potentially less generic
Here an example of how to use Gearboxes in Gazebo (directly into the SDF)

Gazebo implements interfaces to use the ODE feature (doesn't look like Bullet, Dart or Simbody implement this ATM) at the Gazebo abstraction level

As far as I know, there is still no parsing of the mimic/transmission tags as far as Gazebo is concerned, while the information can be parsed for use with ROS for control purposes. So basically, the mimic tag does not get converted into a Gearbox joint in Gazebo/ODE, but by using clever scripts we can manually create the needed sdf elements for simulation.

While the guys at OSRF figure this think out properly, I might come out with a simple python script that transforms mimics into the proper SDF element for use with underactuated hands. I will keep you updated.

[rethink-imcmahon]

There also appears to be an open pull request against this repo for a mimic_joint plugin: #326 @vsamy & @adolfo-rt were discussing it in May, but it appears there are a few merge conflicts now.

[arocchi]

@rethink-imcmahon the way that plugin is implemented is still not complete, as it is unidirectional. The way I understand it, this means in practice that would not be a transmission, since as you're actuating the mimic joint, you would have different power at the output than what you have at the input. The way it is implemented low-level (as a constraint) makes sure the error between the two joints is kept (ideally) to zero without injecting energy into the system, and it should be the correct way to do it.

[hamzamerzic]

Are there any updates on this?

[ghost]

My closed_loop_plugin is to solve the issue that URDF not support Closed loop chains.

refer: https:/wojiaojiao/pegasus_gazebo_plugins

[BenArtes]

It seems like this discussion was put on hold with two possible paths forward:

• Integrate @costashatz 's quick fix (https:/roboticsgroup/roboticsgroup_gazebo_plugins)
• Implement @arocchi 's suggestion of trying to find a way to transform the URDF to into the corresponding SDF for more accurate simulation

It also seems that the previously mentioned pull request (#326) from 2015 is abandoned with similar capabilities to @costashatz 's plugin, though it looks like @costashatz 's plugin is more fully-fleshed out than this pull-request.

Can we temporarily move this forward by merging the plugin or do we have to get a proposed URDF->SDF translation implementation to decide?