How to rotate system of interconnected bodies #2524

robertsulej · 2025-03-23T18:26:22Z

robertsulej
Mar 23, 2025

Intro

Hi!

I am quite new to MuJoCo. I've found my way through the basics, though I am stuck at a task that I did not find in any examples nor docs..

My setup

MoJoCo 3.3.0, Python 3.11.9, Win 11.

My question

I need to simulate an object that cannot be described as a single tree with a single root body. I need to connect several simple trees interconnected with welds. The basic example is pasted below (a kind of elastic tetrahedron falling on the floor).

Now, I would like to repeat simulation, starting from randomly rotated initial state. I am working in python. I can to rotate all the positions and quaternions of my object vertices and edges, and the rendering looks correct. But I cannot get correct simulation when starting from the rotated state. The object gets deformed or it is jumping like there were some incorrect forces applied to joints. I attribute this to the problem with rotation of sites that are connected with welds. I tried to apply rotation to the data structure from mujoco.MjData(model), and also tried to compile rotated model created with mujoco.MjSpec, but with similar problems.

How to approach such a simulation task correctly?

Btw.: why there is discrepancy in the quaternions of the model and spec objects, like in the following code (while positions are equal):

import mujoco

spec = mujoco.MjSpec.from_file("model.xml")
model = spec.compile()
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)

print(model.body('edge0').quat)
print(spec.body('edge0').quat)

print(model.body('edge0').pos)
print(spec.body('edge0').pos)

this results with:

[ 0.70710678  0.         -0.70710678  0.        ]
[1. 0. 0. 0.]
[0.1       0.        0.1292893]
[0.1       0.        0.1292893]

Thanks for any hint!

Minimal model and/or code that explain my question

<mujoco>
  <option timestep="0.001"/>

  <asset>
    <texture name="grid" type="2d" builtin="checker" rgb1=".1 .2 .3" rgb2=".2 .3 .4" width="300" height="300"/>
    <material name="grid" texture="grid" texrepeat="8 8" reflectance=".2"/>
  </asset>

  <default>
    <default class="start">
      <geom pos="0. 0. 0.008" size=".002" mass="0.1" rgba=".3 .4 .9 1"/>
    </default>
    <default class="end">
      <geom pos="0. 0. -0.008" size=".002" mass="0.1" rgba=".3 .4 .9 1"/>
    </default>
    <default class="vtx">
      <geom pos="0. 0. 0." size=".005" mass="0.3" rgba=".8 .5 .3 1"/>
      <site pos="0. 0. 0." size=".0001"/>
    </default>
    <default class="edge">
      <geom type="capsule" pos="0. 0. 0." size=".005 .015" mass="0.5" rgba=".1 .3 .9 1"/>
      <joint type="slide" pos="0. 0. 0." springdamper="0.05 0.15"/>
      <site pos="0. 0. 0." zaxis="0. 0. 1." size=".0001"/>
    </default>
  </default>

  <worldbody>
    <geom size="1. 1. .1" type="plane" material="grid"/>
    <light pos="0 0 .6"/>

    <body name="vtx0" pos="0.1 0. 0.1292893">
      <geom class="vtx"/>
      <site name="v00" class="vtx" zaxis="-0.2 0. 0."/>
      <site name="v02" class="vtx" zaxis="0.1 -0.1 -0.1414214"/>
      <site name="v03" class="vtx" zaxis="-0.1 -0.1 0.1414214"/>
      <freejoint />
    </body>
    <body name="vtx1" pos="-0.1 0. 0.1292893">
      <geom class="vtx"/>
      <site name="v10" class="vtx" zaxis="-0.2 0. 0."/>
      <site name="v11" class="vtx" zaxis="0.1 0.1 0.1414214"/>
      <site name="v14" class="vtx" zaxis="0.1 -0.1 0.1414214"/>
      <freejoint />
    </body>
    <body name="vtx2" pos="0. 0.1 0.2707107">
      <geom class="vtx"/>
      <site name="v21" class="vtx" zaxis="0.1 0.1 0.1414214"/>
      <site name="v22" class="vtx" zaxis="0.1 -0.1 -0.1414214"/>
      <site name="v25" class="vtx" zaxis="0. -0.2 0."/>
      <freejoint />
    </body>
    <body name="vtx3" pos="0. -0.1 0.2707107">
      <geom class="vtx"/>
      <site name="v33" class="vtx" zaxis="-0.1 -0.1 0.1414214"/>
      <site name="v34" class="vtx" zaxis="0.1 -0.1 0.1414214"/>
      <site name="v35" class="vtx" zaxis="0. -0.2 0."/>
      <freejoint />
    </body>

    <body name="edge0" pos="0.1 0. 0.1292893" zaxis="-0.2 0. 0.">
      <geom class="start"/>
      <site class="edge" name="s0"/>
      <body pos="0. 0. .1">
        <geom class="edge"/>
        <joint class="edge"/>
        <body pos="0. 0. .1">
          <geom class="end"/>
          <joint class="edge"/>
          <site class="edge" name="e0"/>
        </body>
      </body>
      <freejoint />
    </body>

    <body name="edge1" pos="-0.1 0. 0.1292893" zaxis="0.1 0.1 0.1414214">
      <geom class="start"/>
      <site class="edge" name="s1"/>
      <body pos="0. 0. .1">
        <geom class="edge"/>
        <joint class="edge"/>
        <body pos="0. 0. .1">
          <geom class="end"/>
          <joint class="edge"/>
          <site class="edge" name="e1"/>
        </body>
      </body>
      <freejoint />
    </body>

    <body name="edge2" pos="0. 0.1 0.2707107" zaxis="0.1 -0.1 -0.1414214">
      <geom class="start"/>
      <site class="edge" name="s2"/>
      <body pos="0. 0. .1">
        <geom class="edge"/>
        <joint class="edge"/>
        <body pos="0. 0. .1">
          <geom class="end"/>
          <joint class="edge"/>
          <site class="edge" name="e2"/>
        </body>
      </body>
      <freejoint />
    </body>

    <body name="edge3" pos="0.1 0. 0.1292893" zaxis="-0.1 -0.1 0.1414214">
      <geom class="start"/>
      <site class="edge" name="s3"/>
      <body pos="0. 0. .1">
        <geom class="edge"/>
        <joint class="edge"/>
        <body pos="0. 0. .1">
          <geom class="end"/>
          <joint class="edge"/>
          <site class="edge" name="e3"/>
        </body>
      </body>
      <freejoint />
    </body>

    <body name="edge4" pos="-0.1 0. 0.1292893" zaxis="0.1 -0.1 0.1414214">
      <geom class="start"/>
      <site class="edge" name="s4"/>
      <body pos="0. 0. .1">
        <geom class="edge"/>
        <joint class="edge"/>
        <body pos="0. 0. .1">
          <geom class="end"/>
          <joint class="edge"/>
          <site class="edge" name="e4"/>
        </body>
      </body>
      <freejoint />
    </body>

    <body name="edge5" pos="0. 0.1 0.2707107" zaxis="0. -0.2 0.">
      <geom class="start"/>
      <site class="edge" name="s5"/>
      <body pos="0. 0. .1">
        <geom class="edge"/>
        <joint class="edge"/>
        <body pos="0. 0. .1">
          <geom class="end"/>
          <joint class="edge"/>
          <site class="edge" name="e5"/>
        </body>
      </body>
      <freejoint />
    </body>
  </worldbody>

  <equality>
    <weld name="c00" site1="s0" site2="v00" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c01" site1="e0" site2="v10" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c10" site1="s1" site2="v11" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c11" site1="e1" site2="v21" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c20" site1="s2" site2="v22" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c21" site1="e2" site2="v02" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c30" site1="s3" site2="v03" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c31" site1="e3" site2="v33" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c40" site1="s4" site2="v14" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c41" site1="e4" site2="v34" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c50" site1="s5" site2="v25" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c51" site1="e5" site2="v35" solref=".002 1" solimp="0.9 0.95 0.001"/>
  </equality>
</mujoco>

Confirmations

I searched the latest documentation thoroughly before posting.
I searched previous Issues and Discussions, I am certain this has not been raised before.

yuvaltassa · 2025-03-24T04:23:13Z

yuvaltassa
Mar 24, 2025
Maintainer

Maybe you want to wrap everything with a frame?

https://mujoco.readthedocs.io/en/latest/XMLreference.html#frame-r

5 replies

robertsulej Mar 24, 2025
Author

That works one-time upon compiling from xml, right? What if I need to reset simulation a lot, like training an agent - recompilation from xml is the only way or is there any less brute force method to edit the states of bodies?

robertsulej Mar 24, 2025
Author

OK, I did some tests. If I wrap my objects in a rotated frame at the xml level, and then build and compile from scratch:

spec = mujoco.MjSpec.from_string(my_xml_string)
model = spec.compile()
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)

then everything looks like expected and simulation proceeds correctly.

But, if I assign rotated pos and quat to spec.frames[0] (which is the frame I just have added), and then compile, forward, etc again - then the model is not affected. What should I do to apply/propagate changed pos and quat?

Interesting, that if I modify eg pos of one of bodies in spec, and compile, forward, ... then the change is applied in the model. Just modifications of the frame did not work (which would be a perfect solution to my case).

yuvaltassa Apr 1, 2025
Maintainer

This is interesting. Could you make an MRE of what you are reporting? (That changing the frame and recompiling has no effect)

robertsulej Apr 1, 2025
Author

Thanks for replying! I managed to move forward, but the issue with the frame is still puzzling to me. Let me prepare the sample. I'll be back today or tomorrow.

robertsulej Apr 2, 2025
Author

Here is the sample code. It reads model from xml (tetrahedron as before, just wraped in frame), and:

Code changes the frame quaternion in xml, builds spec, compiles the model - and xpos of tetrahedron vertices are rotated.
Code re-reads xml, builds and compiles the model; then the quaternion frame in spec is changed and model compiled/recompiled again. The frame quaternion is updated but vertex xpos are just not rotated.

Maybe it is a feature, but to me it seems inconsistent behavior. I am still new and have many such "why this and not that" questions, but let's see if the first problem makes sense..

from lxml import etree
import mujoco
import numpy as np
from scipy.spatial.transform import Rotation

root = etree.parse("in_frame.xml")
world = root.find("worldbody")
frame = world.find("frame")
print(f'{frame.get("name")} quat = {frame.get("quat")}')

# Quaternion from xml file:
#
# model_frame quat = 1. 0. 0. 0.

spec = mujoco.MjSpec.from_string(etree.tostring(root))
model = spec.compile()
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)

print(f'{spec.frames[0].name} quat = {spec.frames[0].quat}')
vtx_idx = [i for i in range(model.nbody) if model.body(i).name.startswith('vtx')]
print([model.body(idx).name for idx in vtx_idx])
print(data.geom_xpos[vtx_idx])

# Quaternion and vertex positions in the compiled model:
#
# model_frame quat = [1. 0. 0. 0.]
# ['vtx0', 'vtx1', 'vtx2', 'vtx3']
# [[ 0.1        0.         0.1292893]
#  [-0.1        0.         0.1292893]
#  [ 0.         0.1        0.2707107]
#  [ 0.        -0.1        0.2707107]]

q_rnd = Rotation.random().as_quat(scalar_first=True)
print(q_rnd)

# Random quaternion to set in the frame:
#
# [ 0.35727856  0.04008851 -0.0849224   0.92926483]

# *****************************************************************
# ***** 1. Set quaternion in xml, build from scratch, compile *****
# *****************************************************************

frame.set("quat", f"{q_rnd[0]} {q_rnd[1]} {q_rnd[2]} {q_rnd[3]}")

spec = mujoco.MjSpec.from_string(etree.tostring(root))
model = spec.compile()
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)

print(f'{spec.frames[0].name} quat = {spec.frames[0].quat}')
vtx_idx = [i for i in range(model.nbody) if model.body(i).name.startswith('vtx')]
print([model.body(idx).name for idx in vtx_idx])
print(data.geom_xpos[vtx_idx])

# As expected: quaternion `q_rnd` set in frame, position rotated.
#
# model_frame quat = [ 0.35727856  0.04008851 -0.0849224  0.92926483 ]
# ['vtx0', 'vtx1', 'vtx2', 'vtx3']
# [[ 0.01919083 -0.05977096  0.15091344]
#  [-0.14065393  0.03417579  0.07592239]
#  [-0.08966643  0.06092808  0.26745464]
#  [-0.16465749 -0.11452018  0.20750259]]


# *****************************************************************
# *****           Re-read xml to have a clean start           *****
# *****************************************************************

root = etree.parse("in_frame.xml")
world = root.find("worldbody")
frame = world.find("frame")
print(f'{frame.get("name")} quat = {frame.get("quat")}')

# Quaternion from xml file, as before:
#
# model_frame quat = 1. 0. 0. 0.

spec = mujoco.MjSpec.from_string(etree.tostring(root))
model = spec.compile()
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)

print(f'{spec.frames[0].name} quat = {spec.frames[0].quat}')
vtx_idx = [i for i in range(model.nbody) if model.body(i).name.startswith('vtx')]
print([model.body(idx).name for idx in vtx_idx])
print(data.geom_xpos[vtx_idx])

# Quaternion and vertex positions in the compiled model, as before:
#
# model_frame quat = [1. 0. 0. 0.]
# ['vtx0', 'vtx1', 'vtx2', 'vtx3']
# [[ 0.1        0.         0.1292893]
#  [-0.1        0.         0.1292893]
#  [ 0.         0.1        0.2707107]
#  [ 0.        -0.1        0.2707107]]

# *****************************************************************
# *** 2. Set the frame quaternion in spec, compile or recompile ***
# *****************************************************************

spec.frames[0].quat = q_rnd
print(f'{spec.frames[0].name} quat = {spec.frames[0].quat}')

# model_frame quat = [ 0.35727856  0.04008851 -0.0849224   0.92926483]

model = spec.compile()
data = mujoco.MjData(model)

# optionally recompile instead of two lines above, the same result
#model, data = spec.recompile(model, data)
    
mujoco.mj_forward(model, data)

print(f'{spec.frames[0].name} quat = {spec.frames[0].quat}')
vtx_idx = [i for i in range(model.nbody) if model.body(i).name.startswith('vtx')]
print([model.body(idx).name for idx in vtx_idx])
print(data.geom_xpos[vtx_idx])

# Note, vertex positions not affected:
#
# model_frame quat = [ 0.35727856  0.04008851 -0.0849224   0.92926483]
# ['vtx0', 'vtx1', 'vtx2', 'vtx3']
# [[ 0.1        0.         0.1292893]
#  [-0.1        0.         0.1292893]
#  [ 0.         0.1        0.2707107]
#  [ 0.        -0.1        0.2707107]]

The model file in_frame.xml:

<mujoco>
  <option timestep="0.001"/>

  <asset>
    <texture name="grid" type="2d" builtin="checker" rgb1=".1 .2 .3" rgb2=".2 .3 .4" width="300" height="300"/>
    <material name="grid" texture="grid" texrepeat="8 8" reflectance=".2"/>
  </asset>

  <default>
    <default class="start">
      <geom pos="0. 0. 0.008" size=".002" mass="0.1" rgba=".3 .4 .9 1"/>
    </default>
    <default class="end">
      <geom pos="0. 0. -0.008" size=".002" mass="0.1" rgba=".3 .4 .9 1"/>
    </default>
    <default class="vtx">
      <geom pos="0. 0. 0." size=".005" mass="0.3" rgba=".8 .5 .3 1"/>
      <site pos="0. 0. 0." size=".0001"/>
    </default>
    <default class="edge">
      <geom type="capsule" pos="0. 0. 0." size=".005 .015" mass="0.5" rgba=".1 .3 .9 1"/>
      <joint type="slide" pos="0. 0. 0." springdamper="0.05 0.15"/>
      <site pos="0. 0. 0." zaxis="0. 0. 1." size=".0001"/>
    </default>
  </default>

  <worldbody>
    <geom size="1. 1. .1" type="plane" material="grid"/>
    <light pos="0 0 .6"/>

    <frame name="model_frame" quat="1. 0. 0. 0.">
      <body name="vtx0" pos="0.1 0. 0.1292893">
        <geom class="vtx"/>
        <site name="v00" class="vtx" zaxis="-0.2 0. 0."/>
        <site name="v02" class="vtx" zaxis="0.1 -0.1 -0.1414214"/>
        <site name="v03" class="vtx" zaxis="-0.1 -0.1 0.1414214"/>
        <freejoint />
      </body>
      <body name="vtx1" pos="-0.1 0. 0.1292893">
        <geom class="vtx"/>
        <site name="v10" class="vtx" zaxis="-0.2 0. 0."/>
        <site name="v11" class="vtx" zaxis="0.1 0.1 0.1414214"/>
        <site name="v14" class="vtx" zaxis="0.1 -0.1 0.1414214"/>
        <freejoint />
      </body>
      <body name="vtx2" pos="0. 0.1 0.2707107">
        <geom class="vtx"/>
        <site name="v21" class="vtx" zaxis="0.1 0.1 0.1414214"/>
        <site name="v22" class="vtx" zaxis="0.1 -0.1 -0.1414214"/>
        <site name="v25" class="vtx" zaxis="0. -0.2 0."/>
        <freejoint />
      </body>
      <body name="vtx3" pos="0. -0.1 0.2707107">
        <geom class="vtx"/>
        <site name="v33" class="vtx" zaxis="-0.1 -0.1 0.1414214"/>
        <site name="v34" class="vtx" zaxis="0.1 -0.1 0.1414214"/>
        <site name="v35" class="vtx" zaxis="0. -0.2 0."/>
        <freejoint />
      </body>

      <body name="edge0" pos="0.1 0. 0.1292893" zaxis="-0.2 0. 0.">
        <geom class="start"/>
        <site class="edge" name="s0"/>
        <body pos="0. 0. .1">
          <geom class="edge"/>
          <joint class="edge"/>
          <body pos="0. 0. .1">
            <geom class="end"/>
            <joint class="edge"/>
            <site class="edge" name="e0"/>
          </body>
        </body>
        <freejoint />
      </body>

      <body name="edge1" pos="-0.1 0. 0.1292893" zaxis="0.1 0.1 0.1414214">
        <geom class="start"/>
        <site class="edge" name="s1"/>
        <body pos="0. 0. .1">
          <geom class="edge"/>
          <joint class="edge"/>
          <body pos="0. 0. .1">
            <geom class="end"/>
            <joint class="edge"/>
            <site class="edge" name="e1"/>
          </body>
        </body>
        <freejoint />
      </body>

      <body name="edge2" pos="0. 0.1 0.2707107" zaxis="0.1 -0.1 -0.1414214">
        <geom class="start"/>
        <site class="edge" name="s2"/>
        <body pos="0. 0. .1">
          <geom class="edge"/>
          <joint class="edge"/>
          <body pos="0. 0. .1">
            <geom class="end"/>
            <joint class="edge"/>
            <site class="edge" name="e2"/>
          </body>
        </body>
        <freejoint />
      </body>

      <body name="edge3" pos="0.1 0. 0.1292893" zaxis="-0.1 -0.1 0.1414214">
        <geom class="start"/>
        <site class="edge" name="s3"/>
        <body pos="0. 0. .1">
          <geom class="edge"/>
          <joint class="edge"/>
          <body pos="0. 0. .1">
            <geom class="end"/>
            <joint class="edge"/>
            <site class="edge" name="e3"/>
          </body>
        </body>
        <freejoint />
      </body>

      <body name="edge4" pos="-0.1 0. 0.1292893" zaxis="0.1 -0.1 0.1414214">
        <geom class="start"/>
        <site class="edge" name="s4"/>
        <body pos="0. 0. .1">
          <geom class="edge"/>
          <joint class="edge"/>
          <body pos="0. 0. .1">
            <geom class="end"/>
            <joint class="edge"/>
            <site class="edge" name="e4"/>
          </body>
        </body>
        <freejoint />
      </body>

      <body name="edge5" pos="0. 0.1 0.2707107" zaxis="0. -0.2 0.">
        <geom class="start"/>
        <site class="edge" name="s5"/>
        <body pos="0. 0. .1">
          <geom class="edge"/>
          <joint class="edge"/>
          <body pos="0. 0. .1">
            <geom class="end"/>
            <joint class="edge"/>
            <site class="edge" name="e5"/>
          </body>
        </body>
        <freejoint />
      </body>
    </frame>
  </worldbody>

  <equality>
    <weld name="c00" site1="s0" site2="v00" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c01" site1="e0" site2="v10" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c10" site1="s1" site2="v11" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c11" site1="e1" site2="v21" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c20" site1="s2" site2="v22" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c21" site1="e2" site2="v02" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c30" site1="s3" site2="v03" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c31" site1="e3" site2="v33" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c40" site1="s4" site2="v14" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c41" site1="e4" site2="v34" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c50" site1="s5" site2="v25" solref=".002 1" solimp="0.9 0.95 0.001"/>
    <weld name="c51" site1="e5" site2="v35" solref=".002 1" solimp="0.9 0.95 0.001"/>
  </equality>
</mujoco>

How to rotate system of interconnected bodies #2524

Uh oh!

robertsulej Mar 23, 2025

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 1 comment · 5 replies

Uh oh!

Uh oh!

yuvaltassa Mar 24, 2025 Maintainer

Uh oh!

robertsulej Mar 24, 2025 Author

Uh oh!

Uh oh!

robertsulej Mar 24, 2025 Author

Uh oh!

yuvaltassa Apr 1, 2025 Maintainer

Uh oh!

robertsulej Apr 1, 2025 Author

Uh oh!

Uh oh!

robertsulej Apr 2, 2025 Author

robertsulej
Mar 23, 2025

Replies: 1 comment 5 replies

yuvaltassa
Mar 24, 2025
Maintainer

robertsulej Mar 24, 2025
Author

robertsulej Mar 24, 2025
Author

yuvaltassa Apr 1, 2025
Maintainer

robertsulej Apr 1, 2025
Author

robertsulej Apr 2, 2025
Author