scenario can not determine world position of link

[FirefoxMetzger]

I have the following SDF (called pose_testing.sdf) and test snippet:

<?xml version="1.0" ?>
<sdf version="1.8">
    <world name="pose_world">
        <model name="model">
            <link name="A">
                <pose>1 2 3 0 0 0</pose>
                <must_be_base_link>true</must_be_base_link>
            </link>
            <link name="B">
                <pose>0 0 0 0 0 1.57079632679</pose>
                <must_be_base_link>true</must_be_base_link>
            </link>
        </model>
    </world>
</sdf>

from pathlib import Path

import numpy as np
from scenario import gazebo as scenario_gazebo

env = Path(__file__).parent / "lila" / "sdf" / "pose_testing.sdf"
simulator = scenario_gazebo.GazeboSimulator(
    step_size=0.001, steps_per_run=round((1 / 0.001) / 30)
)
simulator.insert_world_from_sdf(str(env))
simulator.initialize()
simulator.run(paused=True)
simulator.run()

model = simulator.get_world("pose_world").get_model("model")
for name in model.link_names():
    expected = model.get_link(name).position()

The test snippet is a reduced version of a test case that I am writing to ensure that ignition gazebo and scikit-bot's ignition module interpret SDF pose tags in the same way.

I verified the correctness of the above SDF via ign sdf --check pose_testing.sdfand the script runs fine if only a single link is specified. However, when specifying multiple links I receive the following exceptions:

Traceback (most recent call last):
  File "foobar.py", line 17, in <module>
    expected = model.get_link(name).position()
  File "/home/sebastian/.local/lib/python3.8/site-packages/scenario/bindings/core.py", line 2042, in position
    return _core.Link_position(self)
RuntimeError: [B] Failed to get world position

I was thinking that it could be related with me using must_be_base_link twice, but adding a joint that connects A and B (removing must_be_base_link) leads to the same result. Any insights into what might be going on?

Edit: The SDF also loads fine in Gazebo itself (ign gazebo pose_testing.sdf) and I am on ignition edifice.

[diegoferigo]

Hi @FirefoxMetzger, thanks for reporting! The origin of the problem is that we are used to insert worlds and models in different stages, and something was missing in your use case. In fact, a world file with models was initializing only partially the Model objects that are exposed to C++ / Python. The fix is easy and I'll open a PR.

If you add the world first from file, and then the models from a different file (or a string) through the World object, everything seems to work fine. However, if you already tried it, you might have noticed that the poses are not updated in your example. This is because you don't load any physics. The physics is responsible to update the simulation status, pose included. See the script below for more details.

Then, for what concern the must_be_base_link element, I'm not sure if it is currently supported by Ignition, can you please verify somehow? We detect the base frame by finding the link that Ignition Gazebo populated with the CanonicalLink component, if this is not done correctly upstream, then it could be a bug to report there.

Updated script

import tempfile
from scenario import gazebo as scenario_gazebo

# Set the verbosity
scenario_gazebo.set_verbosity(scenario_gazebo.Verbosity_debug)

# Allocate the simulator
simulator = scenario_gazebo.GazeboSimulator(
    step_size=0.001, steps_per_run=round((1 / 0.001) / 30)
)

world_with_model_string = """
<?xml version="1.0" ?>
<sdf version="1.8">
    <world name="pose_world">
        <model name="model">
            <link name="A">
                <pose>1 2 3 0 0 0</pose>
                <!--must_be_base_link>true</must_be_base_link-->
            </link>
            <link name="B">
                <pose>0 1.0 0 0 0 1.57079632679</pose>
                <!--must_be_base_link>true</must_be_base_link-->
            </link>
        </model>
    </world>
</sdf>"""

world_without_model_string = """
<?xml version="1.0" ?>
<sdf version="1.8">
    <world name="pose_world">
    </world>
</sdf>"""

model_string = """
<?xml version="1.0" ?>
<sdf version="1.8">
    <model name="model">
        <link name="A">
            <pose>1 2 3 0 0 0.0</pose>
            <must_be_base_link>true</must_be_base_link>
        </link>
        <link name="B">
            <pose>0 0 0 0 0 1.57079632679</pose>
            <must_be_base_link>true</must_be_base_link>
        </link>
    </model>
</sdf>"""

with tempfile.NamedTemporaryFile(mode="r+") as f:

    # Write the world file
    f.write(world_without_model_string)
    # f.write(world_with_model_string)

    # Insert the world file
    f.seek(0)
    assert simulator.insert_world_from_sdf(f.name)

assert simulator.initialize()
world = simulator.get_world("pose_world")

# Insert the physics
# ==> OTHERWISE THE POSES ARE NOT UPDATED <==
if not world.set_physics_engine(scenario_gazebo.PhysicsEngine_dart):
    raise RuntimeError("Failed to insert the physics plugin")

# Before the fix, worlds have to be added empty and models
# programmatically from the Python
assert world.insert_model_from_string(model_string)

# A paused run should suffice
assert simulator.run(paused=True)

assert len(world.model_names()) == 1
model = world.get_model(world.model_names()[0])
print(f"Base link: {model.base_frame()}")

for name in model.link_names():
    print(f"{name}:", model.get_link(name).position(), model.get_link(name).orientation())

[diegoferigo]

The right way to switch the base link is using //model/@canonical_link. I verified that this works by editing the model of the previous comment as follows:

model_string = """
<?xml version="1.0" ?>
<sdf version="1.8">
    <model name="model" canonical_link="A"> <!-- Switch here A to B to change base frame -->
        <link name="A">
            <pose>1 2 3 0 0 0.0</pose>
        </link>
        <link name="B">
            <pose>0 0 0 0 0 1.57079632679</pose>
        </link>
    </model>
</sdf>"""

[FirefoxMetzger]

The right way to switch the base link is using //model/@canonical_link

If left unspecified, @canonical_link should default to the first link listed inside the sdf. This is documented here: http://sdformat.org/tutorials?tut=pose_frame_semantics_proposal&cat=pose_semantics_docs&#2-1-implicit-frame-defined-by-model-pose-attached-to-canonical-link (however, I didn't investigate actual behavior.) As such, it should be okay to omit, too.

[diegoferigo]

The right way to switch the base link is using //model/@canonical_link

If left unspecified, @canonical_link should default to the first link listed inside the sdf. This is documented here: http://sdformat.org/tutorials?tut=pose_frame_semantics_proposal&cat=pose_semantics_docs&#2-1-implicit-frame-defined-by-model-pose-attached-to-canonical-link (however, I didn't investigate actual behavior.) As such, it should be okay to omit, too.

Yes correct, my comment came from what I interpreted being the reason behind your usage of //link/must_be_base_link. Did you find any use case of this element? I've never seen it before today.

[FirefoxMetzger]

I haven't seen it used apart from me sprinkling it into the unit tests of scikit-bot. There I do it because it allows you to position a link @relative_to another link within the same scope while allowing the same link to be kinematically connected to the world frame (instead of the model's @canonical_link).

It is a bit of a theoretical marble/edge case though, so I have doubts about it being used outside of this.

[diegoferigo]

Just to have it clear, with kinematically connected what do you mean practically? Having a 6DOFs joint (as documented) means that the link would be free to move as it was unconstrained, right? If I understood correctly, this option is not read by Ignition Gazebo nor Gazebo Classic (excluding the simbody backend).

In any case, the linked PR should fix the error you reported. Let me know if you have other related errors 😉

Btw, I like the direction that your project has taken! Mostly all researcher / student use either their own or some lab-wise libraries for transforms and interpolations :) Since long time, I have in mind to bridge SDF to Python, it's something that was missing even though there's some recent interest in upstream to create proper Python bindings.

[diegoferigo]

Closed via #386

[FirefoxMetzger]

Just to have it clear, with kinematically connected what do you mean practically?

SDF applies constraints to how objects can move in the world; usually via a //joint element connecting two links and constraining their relative movement. However, what happens to //links that don't have a joint that connects to them? For example, what constraints is the link B subjected to in the above example and how is it allowed to move? I haven't seen any explicit rule for this in the spec, so - as far as I am aware - this is undefined behavior.

I think Ignition Gazebo simply allows such links to move freely (aka. it adds a 6DoF joint with the parent //world).

In my current implementation of ign.sdformat.to_frame_graph I wasn't sure how this should behave, so - in an effort to remain unopinionated - I do nothing. As a consequence, scikit-bot creates the respective links, but then sheds them as the dynamic graph/tree gets constructed since it doesn't know how the object can move in the world. In the above example, this would mean that the resulting frame graph will only contain link A, but not link B, since its connection to //world can not be resolved. This is where must_be_base_link comes in handy because it allows to explicitly connect a link to the world resolving any ambiguity. (Though I'm happy to change this behavior once I figure out how this ought to behave; especially considering that must_be_base_link might get depreciated.)

In any case, the linked PR should fix the error you reported. Let me know if you have other related errors 😉

Awesome 🚀 I will try that once I get into the office and onto a Linux machine again. Gazebo still doesn't like the new rendering capabilities of WSL on Windows 11, so I can't test it from here.

Btw, I like the direction that your project has taken!

Thank you! I always felt that transformations could be built on top of numpy/scipy and that it might actually work out to be more efficient considering the crazy amount of optimization that goes into numpy.

Since long time, I have in mind to bridge SDF to Python, it's something that was missing even though there's some recent interest in upstream to create proper Python bindings.

You may already be aware, but you can get SDF bindings from scikit-bot (link to docs) and I have made a small example on how to use them here.

I also have ign.sdformat.to_frame_graph which creates a skbot.transform representation of the SDF and ros.create_frame_graph which does the same for URDF. However, I still need to write some nice examples for how to use them.

[FirefoxMetzger]

I just updated to the latest devel version, and it all works as expected now. Thanks for implementing a fix so quickly :)

[diegoferigo]

Just to have it clear, with kinematically connected what do you mean practically?

SDF applies constraints to how objects can move in the world; usually via a //joint element connecting two links and constraining their relative movement. However, what happens to //links that don't have a joint that connects to them? For example, what constraints is the link B subjected to in the above example and how is it allowed to move? I haven't seen any explicit rule for this in the spec, so - as far as I am aware - this is undefined behavior.

I think Ignition Gazebo simply allows such links to move freely (aka. it adds a 6DoF joint with the parent //world).

Oh ok I see. Considering the reply in gazebosim/sdformat#700 (comment), it seems that this SDF element is not used in the default physics engine of both Ignition and Gazebo Classic. Practically speaking, I'm not sure what would be the expected behavior, because the link not being constrained to any other can be thought as part of a different kinematic tree, even if it is described as part of a unique model from the SDF point of view.

Btw, I like the direction that your project has taken!

Thank you! I always felt that transformations could be built on top of numpy/scipy and that it might actually work out to be more efficient considering the crazy amount of optimization that goes into numpy.

Since long time, I have in mind to bridge SDF to Python, it's something that was missing even though there's some recent interest in upstream to create proper Python bindings.

You may already be aware, but you can get SDF bindings from scikit-bot (link to docs) and I have made a small example on how to use them here.

👍 I just had a quick look to the documentation, I haven't yet tried to use it in practice.

I just updated to the latest devel version, and it all works as expected now. Thanks for implementing a fix so quickly :)

Great! I'm glad it worked. It was quite an odd behavior in a use case that we never use, thanks for reporting it, it could have puzzled many users.