Fix: Inertial Origin Resetting During Merge Links

[ConnorTingley]

Hello!

I noticed an issue where the center of mass of the parent object was ignored when a child was merged into it.
Take this example of a cube with an offset center of mass @ [0.5 0 0]

<?xml version="1.0"?>
<robot name="Cube">

  <link name="base_link">
  	<inertial>
      <origin xyz="0.5 0 0" rpy="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="1 1 1"/>
      </geometry>

    </visual>

  </link>

</robot>

and a cube with the same offset center of mass and a (practically) massless child that is rigidly linked to it

<?xml version="1.0"?>
<robot name="CubeWMassless">

  <link name="base_link">
  	<inertial>
      <origin xyz="0.5 0 0" rpy="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="1 1 1"/>
      </geometry>

    </visual>

  </link>

  <link name="child">
  	<inertial>
      <origin xyz="-1 -1 -1" rpy="0 0 0"/>
      <mass value="1e-16"/>
      <inertia ixx="0"  ixy="0"  ixz="0" iyy="0" iyz="0" izz="0" />
    </inertial>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.2 0.2 0.2"/>
      </geometry>

    </visual>

  </link>

    <joint name="child_fix" type="fixed">
    <parent link="base_link"/>
    <child link="child"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

</robot>

If we call getDynamicsInfo on both of them with the following script:

import pybullet as p

p.connect(p.GUI)
cubeID = p.loadURDF("Test/Cube.urdf", [0,0,0], [0,0,0,1],
	flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT | p.URDF_MERGE_FIXED_LINKS | p.URDF_USE_INERTIA_FROM_FILE | p.URDF_MAINTAIN_LINK_ORDER)

print("Cube Dynamics", p.getDynamicsInfo(cubeID, -1))

cubeWMID = p.loadURDF("Test/CubeWMassless.urdf", [3,0,0], [0,0,0,1],
	flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT | p.URDF_MERGE_FIXED_LINKS | p.URDF_USE_INERTIA_FROM_FILE | p.URDF_MAINTAIN_LINK_ORDER)

print("Cube with Massless Child Dynamics", p.getDynamicsInfo(cubeWMID, -1))

we can see that the cube has the correct center of mass (0.5, 0.0, 0.0)
Cube Dynamics (1.0, 0.5, (1.0, 1.0, 1.0), (0.5, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 0.0, 0.0, 0.0, -1.0, -1.0, 2, 0.001)
However, when the massless child is added:
Cube with Massless Child Dynamics (1.0, 0.5, (1.0, 1.0, 1.0), (0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0), 0.0, 0.0, 0.0, -1.0, -1.0, 2, 0.001)
the center of mass is reset to the origin.

The root of the issue is these lines:

bullet3/examples/Importers/ImportURDFDemo/UrdfParser.cpp

Lines 2005 to 2012 in e9c461b

	if (!link->m_inertia.m_hasLinkLocalFrame)
	{
	link->m_inertia.m_linkLocalFrame.setIdentity();
	}
	if (!childLink->m_inertia.m_hasLinkLocalFrame)
	{
	childLink->m_inertia.m_linkLocalFrame.setIdentity();
	}

Nothing sets m_inertia.m_hasLinkLocalFrame to be true before this happens, so it is always reset.

I noticed that this Boolean doesn't do anything as the inertia properties are set to the identity on the creation of the object:

bullet3/examples/Importers/ImportURDFDemo/UrdfParser.h

Line 43 in e9c461b

m_linkLocalFrame.setIdentity();

And again in parse inertia:

bullet3/examples/Importers/ImportURDFDemo/UrdfParser.cpp

Line 236 in e9c461b

inertia.m_linkLocalFrame.setIdentity();

So may as well remove this extra logic.

If you want to keep it around as an extra check, the solution is to set the Boolean to true when the origin was loaded here:

bullet3/examples/Importers/ImportURDFDemo/UrdfParser.cpp

Line 254 in e9c461b

}