Home | Tutorials | Wiki | Issues
Ask Your Question
1

Model::SetWorldPose() not moving a model permamently

asked 2014-11-21 18:58:21 -0500

NickDP gravatar image

updated 2014-11-25 15:41:51 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

What robot model are you loading? What version of Gazebo are you using?

nkoenig gravatar imagenkoenig ( 2014-11-25 15:11:50 -0500 )edit

I've edited the question to include the robot model I am using. The Gazebo version is 4.1.0.

NickDP gravatar imageNickDP ( 2014-11-25 15:40:34 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2014-11-25 15:46:35 -0500

nkoenig gravatar image

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.

edit flag offensive delete link more

Comments

1

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?

NickDP gravatar imageNickDP ( 2014-11-25 16:37:22 -0500 )edit
1

If the parent is NULL, then it is attached to the world. So, you can just check for a NULL pointer.

nkoenig gravatar imagenkoenig ( 2014-11-26 10:48:22 -0500 )edit

Perfect. Thanks!

NickDP gravatar imageNickDP ( 2014-11-26 11:16:28 -0500 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2014-11-21 18:58:21 -0500

Seen: 473 times

Last updated: Nov 25 '14