Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Respect joint axes' initial_position #2827

Open
wants to merge 6 commits into
base: gazebo11
Choose a base branch
from

Conversation

stefanbuettner
Copy link
Contributor

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
Copy link
Contributor Author

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:

gazebo_initial_position_problem_double_pendulum

Similar results on a UR10
gazebo_initial_position_problem_ur10

and a Kuka Youbot
gazebo_initial_position_problem_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
Copy link
Member

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.

cc @azeey

@stefanbuettner
Copy link
Contributor Author

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 chapulina added the 11 Gazebo 11 label Sep 30, 2020
@chapulina
Copy link
Contributor

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.

@chapulina chapulina mentioned this pull request Nov 25, 2020
@stefanbuettner
Copy link
Contributor Author

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
Copy link
Contributor Author

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
Copy link
Contributor

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
Copy link
Contributor Author

stefanbuettner commented Dec 21, 2020

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>

Before the model state did not load the joint states even though the
code was there but only commented out.

This is part of the effort of supporting the SDF's joint state feature.
Because it's currently not loaded.
Use the commented code and update it to the new API.
Also extend it for more than one joint axis.
@stefanbuettner
Copy link
Contributor Author

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.

It was introduced in SDF version 1.6 and is not supported currently.
SDF version 1.6 introduced an initial_position for joint axes.
This is useful for loading models in a collision free pose.
Universal Robot's UR5e is penetrating the ground with the default
initial configuration of all axes set to 0.0.
@stefanbuettner
Copy link
Contributor Author

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

@stefanbuettner
Copy link
Contributor Author

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

@rfn123
Copy link

rfn123 commented Jun 24, 2021

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

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
11 Gazebo 11
Projects
None yet
Development

Successfully merging this pull request may close these issues.

<initial_position> tag seems to have no effect
5 participants