Setting //model/static to True on inserted models breaks link placement

[FirefoxMetzger]

I found an interesting bug when inserting models programmatically into a gazebo simulation. If the model sets static=True then it will not be initialized properly.

I created an example of this behavior below showing the same model with and without the static flag set. The MWE is based on the script that @diegoferigo shared in #385. (except that I added code to print poses in the same format they appear in the respective SDF.)

MWE

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

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_static = """
<?xml version="1.0" ?>
<sdf version="1.8">
    <model name="camera_static">
        <static>true</static>
        <pose>1 0 0 -0 0 0</pose>
        <link name="cam_link">
            <pose>0 -2 0 0 0 0</pose>
            <sensor name="camera" type="camera">
                <update_rate>30.0</update_rate>
                <topic>main_camera</topic>
                <camera name="camy">
                    <horizontal_fov>1.13446</horizontal_fov>
                    <image>
                        <width>1920</width>
                        <height>1080</height>
                    </image>
                </camera>
            </sensor>
        </link>
        <link name="B">
            <pose>0 0 3 0 0 0</pose>
            <!-- <pose relative_to="cam_link">0 0 3 0 0 0</pose> -->
        </link>
    </model>
</sdf>"""

model_dynamic = """
<?xml version="1.0" ?>
<sdf version="1.8">
    <model name="camera_dynamic">
        <pose>1 0 0 -0 0 0</pose>
        <link name="cam_link">
            <pose>0 -2 0 0 0 0</pose>
            <sensor name="camera" type="camera">
                <update_rate>30.0</update_rate>
                <topic>main_camera</topic>
                <camera name="camy">
                    <horizontal_fov>1.13446</horizontal_fov>
                    <image>
                        <width>1920</width>
                        <height>1080</height>
                    </image>
                </camera>
            </sensor>
        </link>
        <link name="B">
            <pose>0 0 3 0 0 0</pose>
            <!-- <pose relative_to="cam_link">0 0 3 0 0 0</pose> -->
        </link>
    </model>
</sdf>"""

assert world.insert_model_from_string(model_static)
assert world.insert_model_from_string(model_dynamic)

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

Notice how in the output the pose of B is all zeros in the static model, whereas the dynamic model correctly sets the link's pose.

Model: camera_static
  Base link: cam_link
  cam_link: (1.0, -2.0, 0.0, 0.0, 0.0, 0.0)
  B: (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
Model: camera_dynamic
  Base link: cam_link
  cam_link: (1.0, -2.0, 0.0, 0.0, 0.0, 0.0)
  B: (1.0, 0.0, 3.0, 0.0, 0.0, 0.0)

[diegoferigo]

The modelling that SDF allows is quite large and we didn't explore many of them, including static models. In many cases like it could be this one, it depends a lot on how they are implemented upstream, in most cases we just read the ECM and expose its data through our APIs.

I'm not really sure whether poses of static models are processed by the physics engine (passing through the Physics system). What we do is just returning what's stored in the ignition::gazebo::components::Pose component:

https://github.com/robotology/gym-ignition/blob/83b9b1c61c53118fe30c0578473389482a4e8783/scenario/src/gazebo/src/Model.cpp#L931-L939

If the Physics system does not populate it neither the first time (since after that it should not change being static), this behavior should be addressed upstream. I'll have a more in depth look on this in the next few days.

[FirefoxMetzger]

Interesting. You would indeed expect static models to not be updated by the physics engine. That said, they can definitely collide with dynamic models, so physics should be aware of them...

In the Ignition inspector itself the pose is set correctly; however, it is given relative to camera_static::__model__ instead of world.

... which is partly my reason for trying to use all these combinations and advanced SDFormat features in gym-ignition. It returns all poses relative to world, which is very handy and I haven't figured out how to make ignition do this natively yet.

[diegoferigo]

I just had a look at this. Running a unpaused run instead of a paused run gives the intended result. I'm not yet sure why this is necessary, I need more time to dig into the Physics system, maybe there are some if(is_static) that mess a bit the insertion logic. I'm leaving this issue open so that I can track this check, but in the meantime you can issue regular run commands.

That said, they can definitely collide with dynamic models, so physics should be aware of them...

This for sure 👍