scenario can not determine world position of link about gym-ignition HOT 10 CLOSED

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.

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())

from gym-ignition.

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>"""

from gym-ignition.

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.

from gym-ignition.

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.

from gym-ignition.

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.

from gym-ignition.

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.

from gym-ignition.

Closed via #386

from gym-ignition.

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.

from gym-ignition.

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

from gym-ignition.

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.

from gym-ignition.