[Bug Report] reset_mocap_welds doesn't work properly

[pmh5050]

Describe the bug
The function reset_mocap_welds in mujoco_utils.py doesn't work properly.

def reset_mocap_welds(model, data):
    """Resets the mocap welds that we use for actuation."""
    if model.nmocap > 0 and model.eq_data is not None:
        for i in range(model.eq_data.shape[0]):
            if model.eq_type[i] == mujoco.mjtEq.mjEQ_WELD:
                model.eq_data[i, :7] = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
    mujoco.mj_forward(model, data)

The issue appears to be related to Line 79:

model.eq_data[i, :7] = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) # (x, y, z, qx, qy, qz, qw)

It should be corrected to:

model.eq_data[i, 3:10] = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) # (x, y, z, qw, qx, qy, qz)

System Info

• OS: ubuntu 20.04
• Packages

gymnasium 0.29.1 (pip)
gymnasium-robotics 1.2.4 (pip)
mujoco 2.3.7 (pip)

• Python version: 3.8

[Kallinteris-Andreas]

Can you provide some sources about why it is wrong,

it was changed in mujoco==2.2.2:
https://mujoco.readthedocs.io/en/stable/changelog.html#version-2-2-2-september-7-2022

I can not find anything more in the docs:
https://mujoco.readthedocs.io/en/stable/search.html?q=eq_data&check_keywords=yes&area=default

[pmh5050]

Can you provide some sources about why it is wrong,

it was changed in mujoco==2.2.2: https://mujoco.readthedocs.io/en/stable/changelog.html#version-2-2-2-september-7-2022

I can not find anything more in the docs: https://mujoco.readthedocs.io/en/stable/search.html?q=eq_data&check_keywords=yes&area=default

I'm not sure, but I believe it is related to this problem.
See anchor and relpose in mujoco/src/engine/engine_core_constraint.c

case mjEQ_WELD: // fix relative position and orientation
        // find global points
        for (int j=0; j < 2; j++) {
          mjtNum* anchor = data + 3*(1-j);
          mju_rotVecMat(pos[j], anchor, d->xmat + 9*id[j]);
          mju_addTo3(pos[j], d->xpos + 3*id[j]);
        }
        ...
        // compute orientation error: neg(q1) * q0 * relpose (axis components only)
        mjtNum* relpose = data+6;
        mju_mulQuat(quat, d->xquat+4*id[0], relpose);   // quat = q0*relpose
        mju_negQuat(quat1, d->xquat+4*id[1]);           // quat1 = neg(q1)
        mju_mulQuat(quat2, quat1, quat);                // quat2 = neg(q1)*q0*relpose
        mju_scl3(cpos+3, quat2+1, torquescale);         // scale axis components by torquescale

[Kallinteris-Andreas]

Unfortunately, I do not understand how mocap works Internally in MuJoCo Well enough to comment on it.

Can you make an issue at DeepMind/MuJoCo? ( Also, please tag me.)

[Kallinteris-Andreas]

For reference, this is how meta world Does things: https://github.com/Farama-Foundation/Metaworld/blob/c822f28f582ba1ad49eb5dcf61016566f28003ba/metaworld/envs/mujoco/sawyer_xyz/sawyer_xyz_env.py#L80

[DavidPL1]

The standard in MuJoCo is defined here

To represent 3D orientations and rotations, MuJoCo uses unit quaternions - namely 4D unit vectors arranged as q = (w, x, y, z).
Here (x, y, z) is the rotation axis unit vector scaled by sin(a/2), where a is the rotation angle in radians, and w = cos(a/2).
Thus the quaternion corresponding to a null rotation is (1, 0, 0, 0). This is the default setting of all quaternions in MJCF.

I was also struggling with understanding how equality constraints are handled in MuJoCo. I found the xml reader to be a good source for understanding how they are constructed. Here is the relevant section for weld constraints:

   case mjEQ_WELD:
      ReadAttrTxt(elem, "body1", name1, true);
      ReadAttrTxt(elem, "body2", name2);
      ReadAttr(elem, "relpose", 7, pequality->data+3, text);
      ReadAttr(elem, "torquescale", 1, pequality->data+10, text);
      if (!ReadAttr(elem, "anchor", 3, pequality->data, text)) {
        mjuu_zerovec(pequality->data, 3);
      }
      break;

So this basically tells us, that first comes the anchor (3 fields), then the relpose (7 fields, i.e. a position + quaternion, where as stated above the quaternion is of the form w x y z), which is then followed by the torquescale.
So the change suggested by @pmh5050 is actually correct.

For reference, this is how meta world Does things: https://github.com/Farama-Foundation/Metaworld/blob/c822f28f582ba1ad49eb5dcf61016566f28003ba/metaworld/envs/mujoco/sawyer_xyz/sawyer_xyz_env.py#L80

They set eq_data to [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], conforming to anchor of (0,0,0), relpose position (0,0,0), relpose quaternion with null rotation (1, 0, 0, 0), and torquescale of 1.

Does this clear things up, @Kallinteris-Andreas?
If you want to, I can draft up a PR and add a corresponding test.

[Kallinteris-Andreas]

@DavidPL1 Please make a PR, thanks!

Also check the impact of the change on the fetch environments