try to insert a joint in runtime by plugin

asked 2018-05-27 13:02:20 -0500

Jack gravatar image

updated 2018-05-30 08:40:38 -0500

Jose Luis Rivero gravatar image

I am try to add a joint in runtime by the model and link name in a world plugin. But there is no reaction in gazebo. here is the plugin code:

#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/physics/World.hh>
#include <gazebo/physics/Model.hh>
#include <ignition/math.hh>

namespace gazebo
{
  class dynamicjoint : public WorldPlugin
  {
   public: void Load(physics::WorldPtr _parent, sdf::ElementPtr /* _sdf */)
   {
    //_parent->InsertModelFile("model://box");
    this->world=_parent;
    physics::ModelPtr model1=this->world->ModelByName("box");
    physics::ModelPtr model2=this->world->ModelByName("box1");
    physics::JointPtr joint; 
    this->joint = this->world->PhysicsEngine()->CreateJoint("fixed",model1);
    this->joint->SetName("joint1");
    this->joint->SetModel(model2);
    physics::LinkPtr link1=model1->GetLink();
    physics::LinkPtr link2=model2->GetLink();
    this->joint->Attach(link1,link2);
    this->joint->Load(link1,link2,ignition::math::Pose3d(0,1,0.5,0,0,0,0));
    this->joint->SetAxis(0,ignition::math::Vector3d(0,0,1));
    this->joint->Init();
   }
   private: physics::WorldPtr world;
   private: physics::JointPtr joint;
   };
   GZ_REGISTER_WORLD_PLUGIN(dynamicjoint);
}

the world file:

<?xml version="1.0"?> 
<sdf version="1.4">
  <world name="default">

    <!-- Ground Plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://sun</uri>
    </include>

    <model name="box">
      <link name="link1">
        <pose>0 0 0.5 0 0 0</pose>
        <collision name="collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </collision>

        <visual name="visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </visual>
      </link>
    </model>
    <model name="box1">
      <link name="link2">
           <pose>0 1 0.5 0 0 0</pose>
        <collision name="collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </collision>

        <visual name="visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </visual>
      </link>
    </model> 

    <plugin name="dynamicjoint" filename="libdynamicjoint.so"/>      
  </world>
</sdf>

can someone help me? thanks.

edit retag flag offensive close merge delete