Does gym-ignition support nested models?

[FirefoxMetzger]

SDFormat v1.5 introduced the ability to nest models within itself (the //model/model element). As part of my effort to test my python SDFormat bindings, I created a simple world with a nested model, which loads fine in gazebo itself, but fails to load in gym-ignition due to its inability to determine the canonical link (which I specify explicitly).

Here is the full SDF

SDF

<?xml version="1.0"?>
<sdf version="1.8">
    <world name="pose_world">
        <model name="parent" canonical_link="box::box_link">
            <frame name="table2">
                <pose>0.794 0 0 0 -0 1.5708</pose>
            </frame>
            <frame name="tabletop_2">
                <pose relative_to="table2">0 0 1.015 0 0 0</pose>
            </frame>
            <model name="box">
                <pose relative_to="tabletop_2">-0.2379753249844183 -0.036526411138213755 0.025 0 0 0</pose>
                <link name="box_link"></link>
                <link name="B">
                    <pose relative_to="box_link">1 0 0 0 0 0</pose>
                </link>
            </model>
        </model>
    </world>
</sdf>

And here is a MWE python script to reproduce the behavior

SDF

import tempfile
from scenario import gazebo as scenario_gazebo
from scipy.spatial.transform import Rotation as R
import numpy as np

# Allocate the simulator
simulator = scenario_gazebo.GazeboSimulator()

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

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

    # Write the world file
    f.write(world_without_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")

model_string_A = """
<?xml version="1.0" ?>
<sdf version="1.8">
    <model name="parent" canonical_link="box::box_link">
        <frame name="table2">
            <pose>0.794 0 0 0 -0 1.5708</pose>
        </frame>
        <frame name="tabletop_2">
            <pose relative_to="table2">0 0 1.015 0 0 0</pose>
        </frame>
        <model name="box">
            <pose relative_to="tabletop_2">-0.2379753249844183 -0.036526411138213755 0.025 0 0 0</pose>
            <link name="box_link"></link>
            <link name="B">
                <pose relative_to="box_link">1 0 0 0 0 0</pose>
            </link>
        </model>
    </model>
</sdf>"""
assert world.insert_model_from_string(model_string_A)

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

for model_name in world.model_names():
    model = world.get_model(model_name)
    print(f"Model: {model_name}")
    print(f"  Base link: {model.base_frame()}")

    for name in model.link_names():
        position = model.get_link(name).position()
        orientation_wxyz = np.asarray(model.get_link(name).orientation())
        orientation = R.from_quat(orientation_wxyz[[1, 2, 3, 0]]).as_euler("xyz")
        print(f"  {name}:", (*position, *tuple(orientation)))

On my machine, this produces:

Model: parent
Traceback (most recent call last):
  File "/usr/lib/python3.8/runpy.py", line 194, in _run_module_as_main
    return _run_code(code, main_globals, None,
  File "/usr/lib/python3.8/runpy.py", line 87, in _run_code
    exec(code, run_globals)
  File "/home/sebastian/.vscode/extensions/ms-python.python-2021.2.633441544/pythonFiles/lib/python/debugpy/__main__.py", line 45, in <module>
    cli.main()
  File "/home/sebastian/.vscode/extensions/ms-python.python-2021.2.633441544/pythonFiles/lib/python/debugpy/../debugpy/server/cli.py", line 444, in main
    run()
  File "/home/sebastian/.vscode/extensions/ms-python.python-2021.2.633441544/pythonFiles/lib/python/debugpy/../debugpy/server/cli.py", line 285, in run_file
    runpy.run_path(target_as_str, run_name=compat.force_str("__main__"))
  File "/usr/lib/python3.8/runpy.py", line 265, in run_path
    return _run_module_code(code, init_globals, run_name,
  File "/usr/lib/python3.8/runpy.py", line 97, in _run_module_code
    _run_code(code, mod_globals, init_globals,
  File "/usr/lib/python3.8/runpy.py", line 87, in _run_code
    exec(code, run_globals)
  File "/home/sebastian/coding_projects/lila-legibility/lila/test_poses2.py", line 60, in <module>
    print(f"  Base link: {model.base_frame()}")
  File "/home/sebastian/.local/lib/python3.8/site-packages/scenario/bindings/core.py", line 2724, in base_frame
    return _core.Model_base_frame(self)
RuntimeError: [parent] Failed to find the canonical link

Is this a known limitation of gym-ignition, and if so, what are the plans to supporting nested models? For me, they are useful in this particular context, because I want to position a model @relative_to something, which only works if I can insert a full world with models (currently not possible because models should be inserted dynamically after physics are loaded) or if they are inside a nested model.

This is because due to SDF scoping rules, which prevent using @relative_to on a model that is a direct child of //sdf (since there are no other frames within that scope).

[diegoferigo]

The support of nested models has been included in Ignition Gazebo quite recently (sdformat had it few months before). At the moment we haven't yet explored what we can do from the gym-ignition side, few comments that come in my mind:

• How should we treat nested models? In the sense, should all of them (nested included) be part of World::modelNames? Should a Model object list the nested model (maybe only the Ignition implementation and not the core APIs)?
• What would happen when Model::resetBasePose is called on a nested model?
• Which dofs should we list on a nested model? Only those of the direct joint childs or recursively those of all nested models?
• [...]

There are a lot of open points at the moment, I'm following the overall situation upstream but so far they have not yet been discussed. I'm not even fully aware of what should be the complete semantics nor the allowed combinations of these new SDF version (e.g. is your example ok? Should a model have at least one link? Likely it's ok, but I personally haven't read in detail the feature proposal for nested models).

The alternative of building a world entirely from the SDF with these new features is using programmatic APIs, i.e. inserting a model, asking from the code its pose, and insert other models relative to that. Up to today, this is the recommended way to build such worlds.

[FirefoxMetzger]

How should we treat nested models? In the sense, should all of them (nested included) be part of World::modelNames? Should a Model object list the nested model (maybe only the Ignition implementation and not the core APIs)?

You could keep all models inside a single World::modelNames and namespace them appropriately (ParentModel::ChildModel if you follow the SDFormat convention). If this is essentially a flat view/hash-map into the actual pose graph which is maintained by Ignition this will probably work fine.

Things get a bit painful with a flat structure if you wish to do modification of the graph; in particular, if you want to remove individual models. In this case nested models are probably better placed into a new Model::modelNames element. It's much easier to remove a single item than to (recursively) trace all the ParentModel::ChildModel::... elements in a flat structure.

So I think it is quite use-case-dependent. In scikit-bot I use a tree in the version-specific SDF bindings as well as the version agnostic ones that I am working on at the moment. (agnostic in the sense that the bindings are fully backwards compatible; you can read any version, consume it as any (other) version, and get depreciation warnings (instead of crashes) if you use outdated parameters)

What would happen when Model::resetBasePose is called on a nested model?

I'm not too familiar with the C bindings of SDFormat ... what does Model::restBasePose return? The initial position of the implicit __model__ frame in world coordinates?

Which dofs should we list on a nested model? Only those of the direct joint childs or recursively those of all nested models?

I think this, too, is a question of "what is the use-case".

The ability to nest models was introduced together with the ability to include them in models, so I think the main intention was to compartmentalize model building. I've read that the first implementations simply copied all links/joints/etc. elements into the parent (and namespaced them); so, coming from this angle I think it makes sense to recursively list all dofs from nested models for each model.

So instead of having a SDF where you have an arm + gripper, you can have a model that has a nested arm model and includes a gripper, which is placed @relative_to the attachment position of the arm and linked to the gripper via a single fixed joint in the mutual parent. This way you can easily swap grippers by changing the model being included and you can choose which aspect you wish to control. If you only want to control the arm, you get the nested arm model's DoF, if you want the gripper, you get the nested gripper's DoF, and if you want to control both, you get the parent's DoF.

I'm not even fully aware of what should be the complete semantics nor the allowed combinations of these new SDF version

I spent what now feels like too much time working through that. One of the results was gazebosim/sdformat#666 , which hopefully clarifies at least the new pose semantics. If you want, I can try to answer questions on the parts I do understand and maybe from there it will become clear how a good way forward looks like :D

is your example ok? Should a model have at least one link? Likely it's ok, but I personally haven't read in detail the feature proposal for nested models).

It should be. Models used to require links until v1.5 at which point they are allowed to have 0 links. As far as I understand, this was implicitly resolved by nested models or includes (which were copied into the parent) which would supply the missing canonical link. In v1.7 this was made explicit by introducing @canonical_link and it is preferred to set it explicitly to avoid this ambiguity. In the case of models with no direct links, @canonical_link needs to be set and will have to use the scoping semantics introduced in v1.7 to refer to the desired link.

The alternative of building a world entirely from the SDF with these new features is using programmatic APIs, i.e. inserting a model, asking from the code its pose, and insert other models relative to that. Up to today, this is the recommended way to build such worlds.

Haha, this makes me feel really stubborn @diegoferigo . I feel like the answer to most issues I open is "you just build it programmatically". xD

That aside, in my current use-case the point is exactly to not work out any relative displacement manually; in particular, because my scenario involves a rotation relative to an already rotated frame. I wish to have ignition work out the @relative_to pose, so that I can get "ground truth" on what the simulator does and then match my code to what is done internally.

One example of why this is necessary is actually in the code I shared above. To convert the quaternion that comes from ignition into the euler angles used in SDFormat I have to use intrinsic xyz euler angles, whereas the documentation says that I should use extrinsic euler angles. Finding bugs like this in single transformations is hard enough; finding bugs like this in transformation chains (especially if they involve projecting into 2D pixel space) is almost impossible. Thus, I wish to proactively check for these things in small examples before I build a big simulation on top :)

That said, I do understand if this will not be supported by gym-ignition in the near future. The main point of this issue is to ask about the current plans and to then take it from there.