Problem implementing mimic joints

asked 2015-04-19 04:03:28 -0600

hal9000 gravatar image

updated 2015-04-19 04:14:25 -0600

Hi,

A while back I noticed this post from the MoveIt! group regarding mimic joints in Gazebo (I'm using gazebo 2.x with ROS Indigo in Ubuntu 14.04): MooveIt! Users

Gonçalo Cabrita posted gazebo plugin code that seemed to make sense. Just set the mimic joint angle based on the reference joint. This is (essentially) his code:

#include "gazebo_plugins/mimic_plugin.h"
#include <gazebo/common/Plugin.hh>
#include <ros/ros.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);

My problem is that, after I load/use this plugin, I get really strange physics behavior any time I try to move an object with the link connected to the mimic joint. With the slightest contact, the object seems to bounce in random directions. It seems pretty straight forward, but I can't seem to figure out what I'm doing wrong.

John Hsu discussed this a while back in an answer to the question back in 2011 ROS Answers, Gazebo mimic joints , and made some mention of needing to implement a mimic constraint in the physics engine.

Is there any way in Gazebo 2.x to get mimic tags to work?

Thanks!

edit retag flag offensive close merge delete

Comments

Did you get the mimic tags to work? I also used that plugin, however, it slowed down my whole simulation.

sal14 gravatar imagesal14 ( 2017-02-24 03:46:53 -0600 )edit