Pose updates for nested models are incorrect

[shameekganguly]

Environment

• OS Version: 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, pose updates seem to be wrong for the included model since updating to Ignition Edifice. I have a simple test sdf file with two double pendulums (video attached). Both pendulums are planar and have the base link fixed to the world, followed by revolute joint - link - revolute joint - link. The left pendulum is modeled with a nested model, i.e. model{revolute joint - link} - revolute joint - link, whereas the right pendulum is modeled as a flat list.

The link poses are correct for the right pendulum, whereas they are wrong for some of the links for the left nested pendulum.

Steps to reproduce

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

Output

test_pendulums.txt
https://user-images.githubusercontent.com/2412842/122818055-92cc0d00-d28d-11eb-926c-ccdc9a169f4c.mov