Respect joint axes' initial_position

[stefanbuettner]

SDF version 1.6 introduced an initial_position for joint axes. This is useful for loading models in a collision free pose. For example, Universal Robot's UR5e is penetrating the ground with the default initial configuration of all axes set to 0.0.

Fixes #2694

[stefanbuettner]

It's still a work in progress because it doesn't seem to work quite right.

I tried it on the double_pendulum_with_base model with an initial_position of 1.58 for the upper_joint and -0.78 for the lower_joint but I get the following result:

Similar results on a UR10

and a Kuka Youbot

Here are the model files I used:
initial_position_test.zip

The first joint having the initial_position attribute in the chain seems to work alright but subsequent joints get messed up.

Is there something to be aware of during the model initialization process?

[scpeters]

we've actually been planning to deprecate that tag in SDFormat 1.8 (see osrf/sdformat bitbucket PR 683, since we figured the <state> tag would be more suitable for specifying initial joint positions.

• http://sdformat.org/spec?ver=1.7&elem=state#model_joint

cc @azeey

[stefanbuettner]

Ok, but since it will be only deprecated and should still be supported in my opinion.

Is the state approach supported? I tried it with no effect.

[chapulina]

Is the state approach supported? I tried it with no effect.

Yes, I just checked with these quick steps:

1. Open gazebo
2. Insert a double pendulum
3. Let it move to a different position from its initial one
4. Pause and notice the pendulum configuration
5. Save the world
6. Close Gazebo
7. Load the saved world, paused so you can see the initial pose
8. The pendulum should start from the saved configuration

You can inspect the saved file to see how the <state> tag is used.

[stefanbuettner]

Ok, I understood it wrong from reading the documentation. The steps you provided work. But I still think the initial_position tag should be fixed as well.

[stefanbuettner]

Another thing I noticed: When loading a robot model, moving it in the desired configuration and saving the world, the state only contains <link> tags with <pose>, <velocity>, <acceleration>, and <wrench>. No <joint> tag. Is there a way to specify initial joint configurations? Because that's what I tried initially because it would be more intuitive in some situations.

[chapulina]

I haven't tried, but there seems to be a joint tag inside state:

http://sdformat.org/spec?ver=1.7&elem=state#model_joint

[stefanbuettner]

That's what I've tried as well but didn't work. This is my world file. I would expect the pendulum to start in a different configuration. But changing the angle has no effect.
Started with gazebo --verbose --pause <world-file>.

<?xml version="1.0" ?>
<sdf version="1.7">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>

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

    <model name="double_pendulum">
      <include>
        <uri>model://double_pendulum_with_base</uri>
      </include>
    </model>

    <state world_name="default">
      <model name="double_pendulum">
        <joint name="double_pendulum_with_base::upper_joint">
          <angle axis="0">-1.58</angle>
        </joint>
      </model>
    </state>

  </world>
</sdf>

[stefanbuettner]

I reverted the commit trying to fix the initial_position because it did not, so far.

The latest commits add support for the joint angle state. The example I posted before is working for me now. I also added regression tests.

[stefanbuettner]

Now the initial_position tag also has the desired effect. Tests include only revolute joints, though.

[stefanbuettner]

@chapulina do you mind having another look or shall we close it?

[rfn123]

Hi,
sorry for bumping this, but will this be merged anytime soon?
I got the same problem and would need this fix :)
@scpeters @chapulina

[monokaie]

@chapulina @azeey just listened to the ignition community meeting and thought about this PR when talking about initial states of the models.