0
$\begingroup$

Gazebo Answers logo

I am running Gazebo (version 4.1.0) as a library in my own main().

I am able load a robot model from a file. Now I want to move the model to some (x,y) position in order to be able to load more than one robot and not have them all sit at the origin. I don't want to have to set the pose in the SDF/URDF file itself because I want to use the same file to spawn several models at different positions.

I am trying to do this using Model::SetWorldPose(). I can see in gzclient that this moves the model. But I have two problems:

  • When I load the robot, all joints are at 0, as is to be expected. After moving the robot, that is no longer the case. They appear to be slightly moved in some arbitrary way. The bigger problem however is:

  • After moving the model, every time I do RunBlocking(1) (the simulation is not free running, I advance it myself after moving the model) on my world pointer, the robot model moves closer to the origin (while the robot's joints change more and more) until it is once again at the origin.

Is this what's supposed to happen? How can I move the robot such that it stays where I put it? I have also tried to move the model using Model::SetLinkWorldPose(), with the same result.

Thanks in advance

Edit:

The robot model in question is the following (it is converted from a URDF file using gz sdf -p):

<sdf version='1.5'>
  <model name='sixaxis_no_plugin'>
    <link name='link0'>
      <pose>0 0 0 0 -0 0</pose>
      <inertial>
        <pose>0 0 0.1 0 -0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.01</izz>
        </inertia>
      </inertial>
      <collision name='link0_collision'>
        <pose>0 0 0.1 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.25 0.25 0.2</size>
          </box>
        </geometry>
      </collision>
      <visual name='link0_visual'>
        <pose>0 0 0.1 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.25 0.25 0.2</size>
          </box>
        </geometry>
      </visual>
      <velocity_decay>
        <linear>0</linear>
        <angular>0</angular>
      </velocity_decay>
    </link>
    <joint name='joint0' type='revolute'>
      <child>link0</child>
      <parent>world</parent>
      <axis>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
        <xyz>0 0 1</xyz>
      </axis>
    </joint>
    <link name='link1'>
      <pose>0 0 0.2 0 -0 0</pose>
      <inertial>
        <pose>0 0 0.1 0 -0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.01</izz>
        </inertia>
      </inertial>
      <collision name='link1_collision'>
        <pose>0 0 0.1 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.2</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='link1_visual'>
        <pose>0 0 0.1 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.2</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Orange</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay>
        <linear>0</linear>
        <angular>0</angular>
      </velocity_decay>
      <self_collide>0</self_collide>
    </link>
    <joint name='joint1' type='revolute'>
      <child>link1</child>
      <parent>link0</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-3.14159</lower>
          <upper>3.14159</upper>
          <effort>1000</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='link2'>
      <pose>0 0 0.4 0 -0 0</pose>
      <inertial>
        <pose>0 0 0.25 0 -0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.01</izz>
        </inertia>
      </inertial>
      <collision name='link2_collision'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.5</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='link2_visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.5</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Green</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay>
        <linear>0</linear>
        <angular>0</angular>
      </velocity_decay>
      <self_collide>0</self_collide>
    </link>
    <joint name='joint2' type='revolute'>
      <child>link2</child>
      <parent>link1</parent>
      <axis>
        <xyz>0 1 0</xyz>
        <limit>
          <lower>-3.14159</lower>
          <upper>3.14159</upper>
          <effort>1000</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='link3'>
      <pose>0 0 0.9 0 -0 0</pose>
      <inertial>
        <pose>0 0 0.1 0 -0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.01</izz>
        </inertia>
      </inertial>
      <collision name='link3_collision'>
        <pose>0 0 0.1 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.2</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='link3_visual'>
        <pose>0 0 0.1 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.2</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Orange</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay>
        <linear>0</linear>
        <angular>0</angular>
      </velocity_decay>
      <self_collide>0</self_collide>
    </link>
    <joint name='joint3' type='revolute'>
      <child>link3</child>
      <parent>link2</parent>
      <axis>
        <xyz>0 1 0</xyz>
        <limit>
          <lower>-3.14159</lower>
          <upper>3.14159</upper>
          <effort>1000</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='link4'>
      <pose>0 0 1.1 0 -0 0</pose>
      <inertial>
        <pose>0 0 0.25 0 -0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.01</izz>
        </inertia>
      </inertial>
      <collision name='link4_collision'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.5</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='link4_visual'>
        <pose>0 0 0.25 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.5</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Green</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay>
        <linear>0</linear>
        <angular>0</angular>
      </velocity_decay>
      <self_collide>0</self_collide>
    </link>
    <joint name='joint4' type='revolute'>
      <child>link4</child>
      <parent>link3</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-3.14159</lower>
          <upper>3.14159</upper>
          <effort>1000</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='link5'>
      <pose>0 0 1.6 0 -0 0</pose>
      <inertial>
        <pose>0 0 0.05 0 -0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.01</izz>
        </inertia>
      </inertial>
      <collision name='link5_collision'>
        <pose>0 0 0.05 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.1</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='link5_visual'>
        <pose>0 0 0.05 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.1</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Orange</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay>
        <linear>0</linear>
        <angular>0</angular>
      </velocity_decay>
      <self_collide>0</self_collide>
    </link>
    <joint name='joint5' type='revolute'>
      <child>link5</child>
      <parent>link4</parent>
      <axis>
        <xyz>0 1 0</xyz>
        <limit>
          <lower>-3.14159</lower>
          <upper>3.14159</upper>
          <effort>1000</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
    <link name='link6'>
      <pose>0 0 1.7 0 -0 0</pose>
      <inertial>
        <pose>0 0 0.05 0 -0 0</pose>
        <mass>0.1</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01</iyy>
          <iyz>0</iyz>
          <izz>0.01</izz>
        </inertia>
      </inertial>
      <collision name='link6_collision'>
        <pose>0 0 0.05 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.1</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='link6_visual'>
        <pose>0 0 0.05 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.1</length>
            <radius>0.05</radius>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Red</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <visual name='link6_visual_link6_1'>
        <pose>0 0 0.09 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.2 0.1 0.02</size>
          </box>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Red</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <visual name='link6_visual_link6_2'>
        <pose>0.09 0 0.14 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.02 0.1 0.1</size>
          </box>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Red</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <visual name='link6_visual_link6_3'>
        <pose>-0.09 0 0.14 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.02 0.1 0.1</size>
          </box>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Red</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay>
        <linear>0</linear>
        <angular>0</angular>
      </velocity_decay>
      <self_collide>0</self_collide>
    </link>
    <joint name='joint6' type='revolute'>
      <child>link6</child>
      <parent>link5</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-3.14159</lower>
          <upper>3.14159</upper>
          <effort>1000</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
  </model>
</sdf>

Originally posted by NickDP on Gazebo Answers with karma: 186 on 2014-11-21

Post score: 1

Original comments

Comment by nkoenig on 2014-11-25:
What robot model are you loading? What version of Gazebo are you using?

Comment by NickDP on 2014-11-25:
I've edited the question to include the robot model I am using. The Gazebo version is 4.1.0.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Gazebo Answers logo

The problem is that you have the model attached to the world:

 <joint name='joint0' type='revolute'>
      <child>link0</child>
      <parent>world</parent>

You can either remove this joint from SDF, or do the following in a plugin:

  1. Detach the world joint.

  2. Move the model

  3. Reattach the world joint.

Originally posted by nkoenig with karma: 7676 on 2014-11-25

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by NickDP on 2014-11-25:
That appears to do the trick. But I am having trouble dealing with the world link. I would like to check if the parent of the first joint is indeed the world link in order to decide if detaching is necessary. However, if I do: gazebo::physics::Joint_V joints = model->GetJoints(); joints[0]->GetParent()->GetName(); I get a boost pointer exception. How can I check whether my parent link is the world or not?

Comment by nkoenig on 2014-11-26:
If the parent is NULL, then it is attached to the world. So, you can just check for a NULL pointer.

Comment by NickDP on 2014-11-26:
Perfect. Thanks!

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.