data.body("...").xpos does not match data.qpos

[Kallinteris-Andreas]

Hi,

I'm a maintainer of Gymnasium-Robotics and I'm trying to use MuJoCo to develop the v5 revision of the Gymansium/mujoco RL environments Farama-Foundation/Gymnasium-Robotics#104.

I'm looking for some help with understanding why the following unit test fails.
data.qpos[0] == data.body("torso").xpos[0] is true
but after steeping the MuJoCo model
data.qpos[0] == data.body("torso").xpos[0] is False

Here is a model which explains my question:
The Ant.xml in Gymnasium/MuJoCo/Ant environment
https://github.com/Farama-Foundation/Gymnasium/blob/main/gymnasium/envs/mujoco/assets/ant.xml

Here is a unit test illustrating my question:

def test_ant_com():
    env = gym.make('Ant-v4')  # `env` contains `data : MjData` and `model : MjModel`
    env.reset()  # randomly initlizies the `data.qpos` and `data.qvel`

    x_position_before = env.unwrapped.data.qpos[0]
    x_position_before_com = env.unwrapped.data.body("torso").xpos[0]
    assert(x_position_before == x_position_before_com)  # This succeeds

    random_control = env.action_space.sample()
    _, _, _, _, info = env.step(random_control)  # This calls mujoco.mj_step(env.model, env.data, nstep=env.frame_skip)

    x_position_after = env.unwrapped.data.qpos[0]
    x_position_after_com = env.unwrapped.data.body("torso").xpos[0]
    assert(x_position_after == x_position_after_com)  # This fails

Note: this is the case for other body.xpos & body.xquat
Is this normal/expected?

$ pip list | grep mujoco
mujoco                        2.3.3

[yuvaltassa]

Hi!

Some clarifications are in order.

1. A clarification for future readers: This model's DoFs start with a body-centered free joint, therefore the first 3 elements of qpos have the same semantics as the first 3 elements of xpos. They are generally not the same thing.
2. A clarification for us: is this an old test that is newly breaking or a new test?
3. Regarding the test: It should fail. If it ever passed that is surprising.

Explanation: The purpose ofmj_step is to advance the state (qpos and qvel, in this case). It does this and nothing more. xpos is a derived quantity that is computed from qpos during mj_step (step 2 in the first link above), but at the end of mj_step, qpos gets updated. The only reason it passes after your Reset call is that (presumably) mj_forward or something similar was called at the end of the Reset. So your options are:

• Compare the current qpos to the xpos measured after the previous step.
• Call mj_kinematics (or the full mj_forward) after the step, and then compare.

Does this make sense?

[Kallinteris-Andreas]

1. this is a new test
2. reset() does call mj_forward
3. calling mj_forward after step() does indeed resolve the issue

def test_ant_com():
    env = gym.make('Ant-v5', frame_skip=5)  # `env` contains `data : MjData` and `model : MjModel`
    env.reset()  # randomly initlizies the `data.qpos` and `data.qvel`, calls mujoco.mj_forward(env.model, env.data) 

    x_position_before = env.unwrapped.data.qpos[0]
    x_position_before_com = env.unwrapped.data.body("torso").xpos[0]
    assert(x_position_before == x_position_before_com), "before failed"  # This succeeds

    random_control = env.action_space.sample()
    _, _, _, _, info = env.step(random_control)  # This calls mujoco.mj_step(env.model, env.data, nstep=env.frame_skip)
    mujoco.mj_forward(env.unwrapped.model, env.unwrapped.data)  # <-- This is new

    x_position_after = env.unwrapped.data.qpos[0]
    x_position_after_com = env.unwrapped.data.body("torso").xpos[0]
    assert(x_position_after == x_position_after_com), "after failed"  # This succeeds now

4. can you explain the difference of xpos and qpos
   my current understanding:
   qpos is part of the state (https://mujoco.readthedocs.io/en/latest/computation.html#physics-state)
   and xpos is from what I can tell the kinematic approximation of the body frames positions

Thanks!

[yuvaltassa]

3. fyi mj_kinematics is enough, but other than performance no harm in mj_forward.
4. Yes qpos is the joint configuration. xpos is the global Cartesian position of the body frames.

[Kallinteris-Andreas]

One last thing (and the reason I created the unit test, in the first place)
If you wanted to calculate the displacement of the torso body after mj_step would you do

Option A:

# note `env` holds `data` and `model`
x_position_before = env.data.body("torso").xpos[0]
mujoco.mj_step(env.model, env.data, nstep=env.frame_skip)
# Note: we do not call `mj_kinematics`
x_position_after = env.data.body("torso").xpos[0]
dx = x_position_after - x_position_before  # displacement

Option B:

# note `env` holds `data` and `model`
x_position_before = env.data.qpos[0]
mujoco.mj_step(env.model, env.data, nstep=env.frame_skip)
x_position_after = env.data.qpos[0]
dx = x_position_after - x_position_before  # displacement 

We currently use option A for Ant-v2, Ant-v3, Ant-v4,
Could you confirm that option B is a more accurate way of getting dx (displacement) (I am considering of updating it to use option B on Ant-v5)

Thanks!

[yuvaltassa]

Both options are equally accurate, but the second one is more up to date (by 1 timestep).

You might legitimately now ask "why would I want a delayed measurement if I can get one that is more up to date?" There are sometimes good reasons for this. For example imagine that you want to compute some value that is a function of your dx and some contact force. Forces are only determined during the step and could not be computed now since they depend on the controls. I.e. contact forces are inherently linked not to a state but a state transition. So while it possible to get some values (i.e. functions only of position and velocity) w.r.t to the current timestep, if you want all your measurements (including force/acc related quantities) to be correctly "synced", you have to pay the price of a delay of 1 timestep. In your case you may not care, but in general this can be important.

Hope this makes sense.