mimic plugin for Gazebo

[Gonçalo Cabrita]

Hi guys,

I recently had a problem with using MoveIt with Gazebo due to the fact that Gazebo ignores the <mimic> tag.

The problem is that MoveIt does not send commands to the joints that have the <mimic> tag but Gazebo needs them to be controlled as if they were active joints.

I started writing my own Gazebo plugin controller to control my arm, until I had a different idea, write a simple plugin to implement the mimic joint in Gazebo. I hope this can help anyone having similar problems!

This is the main file:

#include <gazebo_mimic_plugin/mimic_plugin.h>

using namespace gazebo;

MimicPlugin::MimicPlugin()
{
  kill_sim = false;

  joint_.reset();
  mimic_joint_.reset();
}

MimicPlugin::~MimicPlugin()
{
  event::Events::DisconnectWorldUpdateBegin(this->updateConnection);

  kill_sim = true;
}

void MimicPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  this->model_ = _parent;
  this->world_ = this->model_->GetWorld();


  joint_name_ = "joint";
  if (_sdf->HasElement("joint"))
    joint_name_ = _sdf->GetElement("joint")->Get<std::string>();

  mimic_joint_name_ = "mimicJoint";
  if (_sdf->HasElement("mimicJoint"))
    mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get<std::string>();

  multiplier_ = 1.0;
  if (_sdf->HasElement("multiplier"))
    multiplier_ = _sdf->GetElement("multiplier")->Get<double>();


  // Get the name of the parent model
  std::string modelName = _sdf->GetParent()->Get<std::string>("name");

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&MimicPlugin::UpdateChild, this));
  gzdbg << "Plugin model name: " << modelName << "\n";

  joint_ = model_->GetJoint(joint_name_);
  mimic_joint_ = model_->GetJoint(mimic_joint_name_);
}

void MimicPlugin::UpdateChild()
{
    mimic_joint_->SetAngle(0, math::Angle(joint_->GetAngle(0).Radian()*multiplier_));
}

GZ_REGISTER_MODEL_PLUGIN(MimicPlugin);

And then this is how you use it in your URDF file:

  <gazebo>
	<plugin name="mimic_plugin" filename="libgazebo_mimic_plugin.so">
            <joint>upper_arm_joint</joint>
            <mimicJoint>lower_arm_joint</mimicJoint>
            <multiplier>1.0</multiplier>
        </plugin>
  </gazebo>

I used this tutorial to write the plugin http://gazebosim.org/wiki/Tutorials/1.9/Creating_ROS_plugins_for_Gazebo Hope this helps!

Best regards,

Gonçalo Cabrita

[Sachin Chitta]

Hey Goncalo,

That is very cool. The next version of Gazebo is, I believe, on track to have native support for mimic joints - probably through use of a gearbox transmission.

Best Regards,
Sachin

[Sachin Chitta]

This is the corresponding ticket: https://bitbucket.org/osrf/gazebo/issue/1058/update-gearbox-joint-definition

Sachin

[Dave Coleman]

This is great! If that upcoming new feature does not do that, I suggest you add this plugin to gazebo_plugins