Skip to content

Frame issue in mjCBody::AccumulateInertia causing issues with fusestatic #2982

@dicarlo236

Description

@dicarlo236

Intro

Hi!

I'm using mujoco to simulate robots. I ran into an issue with loading URDFs containing fixed joints that I think is a bug in adding together inertia.

My setup

I'm using mujoco built from master on commit e07adc3 on Ubuntu Linux 22.04 x86_64. I've also observed the issue on release 3.3.7. I think the issue will happen on all platforms.

What's happening? What did you expect?

I noticed the inertias are not added together correctly when fusestatic is enabled and the child body is rotated. Translation is handled as expected.

I believe the issue is introduced in this commit: 7932b4b

In particular, line 2266 should use other_iquat (which includes the pose of the child body) instead of other->iquat.

// body_ipose = body_pose * body_ipose
double other_ipos[3];
double other_iquat[4];
mjuu_copyvec(other_ipos, other->ipos, 3);
mjuu_copyvec(other_iquat, other->iquat, 4);
mjuu_frameaccum(other_ipos, other_iquat, other->pos, other->quat);
// organize data
double mass[2] = {
result->mass,
other->mass
};
double inertia[2][3] = {
{result->inertia[0], result->inertia[1], result->inertia[2]},
{other->inertia[0], other->inertia[1], other->inertia[2]}
};
double ipos[2][3] = {
{result->ipos[0], result->ipos[1], result->ipos[2]},
{other_ipos[0], other_ipos[1], other_ipos[2]}
};
double iquat[2][4] = {
{result->iquat[0], result->iquat[1], result->iquat[2], result->iquat[3]},
{other->iquat[0], other->iquat[1], other->iquat[2], other->iquat[3]}
};

Steps for reproduction

  1. Load the model below, which is a cross and has fusestatic set to false.
  2. Load in simulate. Press space to pause, backspace to reset the model to the origin, then press i to visualize inertias. They look correct.
  3. Modify the model to set fusestatic to true
  4. Load in simulate and observe the wrong inertia

Here is the model with fusestatic set to false:

Image

Here is the model with fusestatic set to true. The inertia of the child body (blue) is added to the inertia of the parent body (red) incorrectly. The expected behavior is pizza-box-shaped inertia that goes around the cross.

Image

Next, change line 2266 in user_objects.cc to

    {other_iquat[0], other_iquat[1], other_iquat[2], other_iquat[3]}

rebuild mujoco, load the model, view inertia and see that the inertia looks correct:

Image

Minimal model for reproduction

<mujoco model="inertia_bug">
  <compiler fusestatic="true" angle="degree"/>

  <visual>
    <rgba inertia="1 1 1 0.2"/>
  </visual>
  <worldbody>
    <body>
      <freejoint/>
      <geom type="capsule" fromto="-1 0 0  1 0 0" size="0.05" rgba="0.8 0.2 0.2 1"/>
      <inertial pos="0 0 0" mass="1" diaginertia="0.01 1 1"/>
      <body pos="0 0 0" euler="0 90 0">
        <geom type="capsule" fromto="-1 0 0  1 0 0" size="0.05" rgba="0.2 0.2 0.8 1"/>
        <inertial pos="0 0 0" mass="1" diaginertia="0.01 1 1"/>
        </body>
    </body>
  </worldbody>
</mujoco>

Code required for reproduction

no code is needed, you can load the model in the simulate binary to see the issue.

Confirmations

Metadata

Metadata

Assignees

No one assigned

    Labels

    bugSomething isn't working

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions