Collisions are ignored for nested models connected to parent models with fixed joint

[shameekganguly]

Environment

• OS: Ubuntu
• Source: Built from latest bazel branches

Description

A common model composition paradigm used in our org is to model robot arms and end-effectors separately and connect them together with fixed joints, as such:

<model name = "parent_model">
    <link name="parent_link">...</link>
    <include><uri>model://child</uri> <name> child_model </name></include>
    <joint name="parent_joint">
        <parent> parent_model </parent>
        <child> parent_model </child>
    </joint>
</model>

However, it seems like collisions in the child model are being ignored in dartsim since the update to Ignition Edifice. I have attached a simple sdf (test_cylinders.txt) with two cylinder models that shows this issue. cylinder1 on the left is defined with a nested model structure whereas cylinder2 on the right is defined with a flat model structure. When simulated, cylinder2 behaves correctly and falls on to the ground plane whereas cylinder1 simply falls through.

Steps to reproduce

blaze run -c opt ign_tools:ign -- gazebo -r -v 4 test_cylinders.txt

Output

https://user-images.githubusercontent.com/2412842/122488705-e17a5e00-cf92-11eb-9257-2a1e3e251d69.mov
test_cylinders.txt

[azeey]

I believe this is an issue in ign-gazebo. The problem stems from the recent performance improvements we've been making to ign-gazbebo. In gazebosim/gz-sim#678, we iterate through only the links whose absolute poses have changed. Consider a scenario where the canonical link has a change in pose but the other links don't. What will happen is that we update the pose of the model that is associated with the canonical link, as well as the pose of the canonical link relative to the model. However we skip the links whose absolute poses haven't changed, but since the components::Pose component in ign-gazebo is always relative to the parent model, it looks like these have moved even though their absolute poses have not changed.

Here's a simplified version of test_cylinders from the issue description that demonstrates the problem.
test_cylinders_simplified.sdf.txt
Here base_link does not have a collision shape so it will fall indefinitely. link0_outer on the other hand does have a collision shape and it should fall until it hits the ground plane and then stop.

Here's what it looks like when I run it in ign-gazebo 5.0.0 and 4.9.1.

test_cylinders_ign-gazebo4.mp4

And this is in ign-gazebo 3.8.0

test_cylinders_ign-gazebo3.mp4

/cc @adlarkin

[azeey]

Looks like temporary fixing the issue I described above doesn't resolve the original issue with nested models raised by OP.

[shameekganguly]

Thanks for looking into this @azeey.
I filed a related issue regarding pose updates for nested models here:
gazebosim/gz-sim#871
which I am guessing is caused by the optimizations you mentioned above.

[azeey]

This should be partially fixed by #268.