Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Basic Joint Control Question

Hi everyone,

I'm looking to write a basic joint control plugin. I'm trying to use SetJointPosition to, well, set the joint position. The joint controller can't seem to find the joint.

I believe I'm doing what the API docs, and Gwen's answer tell me to do.

Is there something else I have to do to make the model aware of the joint?

Here's my minimum (not) working example: The plugin:

#include <boost/bind.hpp>
#include <gazebo.hh>
#include <physics/physics.hh>
#include <common/common.hh>
#include <stdio.h>

namespace gazebo
{   
  class SetJoints : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
    {
      // Store the pointer to the model
      this->model = _parent;

      this->j2_controller = new physics::JointController(model);

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
          boost::bind(&SetJoints::OnUpdate, this, _1));
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      // Apply a small linear velocity to the model.
      //this->model->SetLinearVel(math::Vector3(.03, 0, 0));

      double angle(3.0);
      std::string j2name("my_joint");  
      j2_controller->SetJointPosition(j2name,angle);

    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    private: physics::JointController * j2_controller;

  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(SetJoints)
}

The SDF:

<?xml version="1.0" ?>
<sdf version="1.3">

  <world name="default">
    <!-- Ground Plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>

   <model name="box">
   <static>false</static>
      <link name="base_link">
        <pose>0 0 0.5 0 0 0</pose>
        <inertial>
          <pose>0 0 0.5 0 0 0</pose>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1</mass>

        </inertial>

        <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>

      <link name="top_link">
        <pose> 0 0 1.3 0 0 0 </pose>
        <inertial>
          <pose>0 0 0.5 0 0 0</pose>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1</mass>

        </inertial>


        <collision name="collision">
          <geometry>
            <box>
              <size>.5 .5 .5</size>
            </box>>
          </geometry>
        </collision>

        <visual name="visual">
          <geometry>
            <box>
              <size>.5 .5 .5</size>
            </box>>
          </geometry>
        </visual>
      </link>

      <joint name="my_joint" type="revolute">
        <parent>base_link</parent>
        <child>top_link</child>
        <axis>
        <xyz>0 0 1</xyz>
        </axis>
      </joint>


     <plugin name="set_joints" filename="build/libset_joints.so"/>
    </model>
  </world>
</sdf>

Basic Joint Control Question

Hi everyone,

I'm looking to write a basic joint control plugin. I'm trying to use SetJointPosition to, well, set the joint position. The joint controller can't seem to find the joint. After compiling the plugin, running the code gives me an error:

$ gazebo set_joints.sdf
Gazebo multi-robot simulator, version 1.5.0
Copyright (C) 2013 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Msg Waiting for master.Gazebo multi-robot simulator, version 1.5.0
Copyright (C) 2013 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Msg Waiting for master
Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 142.103.111.179

Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 142.103.111.179
Warning [JointController.cc:190] SetJointPosition [my_joint] not found

the last line appears repeatedly.

I believe I'm doing what the API docs, and Gwen's answer tell me to do.

Is there something else I have to do to make the model aware of the joint?

Here's my minimum (not) working example: The plugin:

#include <boost/bind.hpp>
#include <gazebo.hh>
#include <physics/physics.hh>
#include <common/common.hh>
#include <stdio.h>

namespace gazebo
{   
  class SetJoints : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
    {
      // Store the pointer to the model
      this->model = _parent;

      this->j2_controller = new physics::JointController(model);

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
          boost::bind(&SetJoints::OnUpdate, this, _1));
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
      // Apply a small linear velocity to the model.
      //this->model->SetLinearVel(math::Vector3(.03, 0, 0));

      double angle(3.0);
      std::string j2name("my_joint");  
      j2_controller->SetJointPosition(j2name,angle);

    }

    // Pointer to the model
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    private: physics::JointController * j2_controller;

  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(SetJoints)
}

The SDF:

<?xml version="1.0" ?>
<sdf version="1.3">

  <world name="default">
    <!-- Ground Plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>

   <model name="box">
   <static>false</static>
      <link name="base_link">
        <pose>0 0 0.5 0 0 0</pose>
        <inertial>
          <pose>0 0 0.5 0 0 0</pose>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1</mass>

        </inertial>

        <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>

      <link name="top_link">
        <pose> 0 0 1.3 0 0 0 </pose>
        <inertial>
          <pose>0 0 0.5 0 0 0</pose>
          <inertia>
            <ixx>1</ixx>
            <ixy>0</ixy>
            <ixz>0</ixz>
            <iyy>1</iyy>
            <iyz>0</iyz>
            <izz>1</izz>
          </inertia>
          <mass>1</mass>

        </inertial>


        <collision name="collision">
          <geometry>
            <box>
              <size>.5 .5 .5</size>
            </box>>
          </geometry>
        </collision>

        <visual name="visual">
          <geometry>
            <box>
              <size>.5 .5 .5</size>
            </box>>
          </geometry>
        </visual>
      </link>

      <joint name="my_joint" type="revolute">
        <parent>base_link</parent>
        <child>top_link</child>
        <axis>
        <xyz>0 0 1</xyz>
        </axis>
      </joint>


     <plugin name="set_joints" filename="build/libset_joints.so"/>
    </model>
  </world>
</sdf>