-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Description
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.
mujoco/src/user/user_objects.cc
Lines 2244 to 2267 in e07adc3
| // 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
- Load the model below, which is a cross and has
fusestaticset to false. - Load in
simulate. Pressspaceto pause,backspaceto reset the model to the origin, then pressito visualize inertias. They look correct. - Modify the model to set
fusestatictotrue - Load in
simulateand observe the wrong inertia
Here is the model with fusestatic set to false:
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.
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:
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
- I searched the latest documentation thoroughly before posting.
- I searched previous Issues and Discussions, I am certain this has not been raised before.