Replace one free joint with six scalar joints to use a joint equality constraint

[lbergmann1]

Intro

Hi!

I am a PhD student at Bielefeld University, and I use MuJoCo for my research on Magnetic Robotics / magnetic levitation systems. In these systems, the so-called movers (moving permanent magnets) have six DoFs, and I represent one mover as a body with a free joint.

For my current project, I need to couple only the rotation around the z-axis of a mover with another hinge joint. I think the easiest way to do this is to use a joint equality constraint. However, this constraint can only be used with scalar joint types (slide and hinge) joints, and it is not possible to use it for a specific axis of a free joint. Therefore, I considered replacing the free joint with six scalar joints (3 slide joints, 3 hinge joints).

To ensure that I can replace the free joint with six scalar joints, I compared the qpos, qvel, and qacc of the bodies. All values for the translational axes are similar, but the values for the rotations diverge. I assume that the problem is related to how the rotation is internally represented and the order in which the rotation is performed for the scalar joints.

My setup

MuJoCo: 3.4.0 (Python API)
Python: 3.13.11
OS: Ubuntu 22.04.5 LTS

My question

1. Is it generally possible to obtain similar values for qpos, qvel, and qacc for one free joint and six scalar joints? If so, where is my mistake?

2. Is there a better way than using an equality constraint to couple only the rotation around the z-axis of a free joint with another hinge joint?

Many thanks in advance!

Minimal model and/or code that explain my question

import numpy as np
import mujoco

# mujoco utils
def set_actuator_ctrl(model, data, actuator_name, value):
    """Set the control inputs for the desired actuator."""
    actuator_id = model.actuator(actuator_name).id
    assert actuator_id != -1, 'Actuator name not found in MuJoCo model.'
    data.ctrl[actuator_id] = value
    
def get_joint_qpos(model, data, name):
    """Return the joint's position and orientation (qpos) depending on the type of the joint"""
    joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
    assert joint_id != -1, 'Joint name not found in MuJoCo model.'
    joint_type = model.jnt_type[joint_id]
    joint_addr = model.jnt_qposadr[joint_id]

    if joint_type == mujoco.mjtJoint.mjJNT_FREE:
        ndim = 7
    elif joint_type == mujoco.mjtJoint.mjJNT_BALL:
        ndim = 4
    else:
        assert joint_type in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE)
        ndim = 1

    start_idx = joint_addr
    end_idx = joint_addr + ndim

    return data.qpos[start_idx:end_idx].copy()
    
def get_joint_qvel(model, data, name):
    """Return the joint's linear and angular velocities (qvel) depending on the type of the joint."""
    joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
    assert joint_id != -1, 'Joint name not found in MuJoCo model.'
    joint_type = model.jnt_type[joint_id]
    joint_addr = model.jnt_dofadr[joint_id]

    if joint_type == mujoco.mjtJoint.mjJNT_FREE:
        ndim = 6
    elif joint_type == mujoco.mjtJoint.mjJNT_BALL:
        ndim = 4
    else:
        assert joint_type in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE)
        ndim = 1

    start_idx = joint_addr
    end_idx = joint_addr + ndim

    return data.qvel[start_idx:end_idx].copy()
    
def get_joint_qacc(model, data, name):
    """Return the joint's linear and angular acceleration (qacc) depending on the type of joint."""
    joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name)
    assert joint_id != -1, 'Joint name not found in MuJoCo model.'
    joint_type = model.jnt_type[joint_id]
    joint_addr = model.jnt_dofadr[joint_id]

    if joint_type == mujoco.mjtJoint.mjJNT_FREE:
        ndim = 6
    elif joint_type == mujoco.mjtJoint.mjJNT_BALL:
        ndim = 4
    else:
        assert joint_type in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE)
        ndim = 1

    start_idx = joint_addr
    end_idx = joint_addr + ndim

    return data.qacc[start_idx:end_idx].copy()

def get_qpos_free(model, data, body_names, joint_names, body_name):
    """Returns the position and orientation of the desired body."""
    idx = body_names.index(body_name)
    joint_name = joint_names[idx]
    qpos = get_joint_qpos(model, data, joint_name)

    return qpos

def get_qpos_single(model, data, body_name):
    body_pos = data.body(body_name).xpos
    body_quat = data.body(body_name).xquat

    qpos = np.array([*body_pos, *body_quat])
    return qpos

def get_qvel_free(model, data, body_names, joint_names, body_name):
    """Return the linear and angular velocities (qvel) of the desired body."""
    idx = body_names.index(body_name)
    joint_name = joint_names[idx]
    qvel = get_joint_qvel(model, data, joint_name)
    return qvel

def get_qvel_single(model, data, body_name):
    """Return the 6D body velocity of the desired body."""
    cvel = data.body(body_name).cvel
    qvel = np.array([*cvel[3:],*cvel[:3]])
    return qvel.copy()

def get_qacc_free(model, data, body_names, joint_names, body_name):
    """Returns the linear and angular acceleration (qacc) of the desired body."""
    idx = body_names.index(body_name)
    joint_name = joint_names[idx]
    qacc = get_joint_qacc(model, data, joint_name)
    return qacc

def get_qacc_single(model, data, body_names, joint_names_x, joint_names_y, joint_names_z, joint_names_a, joint_names_b, joint_names_c, body_name):
    """Return the linear and angular acceleration (qacc) of the desired body."""
    idx = body_names.index(body_name)
    # x
    joint_name_x = joint_names_x[idx]
    qacc_x = get_joint_qacc(model, data, joint_name_x)
    # y
    joint_name_y = joint_names_y[idx]
    qacc_y = get_joint_qacc(model, data, joint_name_y)
    # z
    joint_name_z = joint_names_z[idx]
    qacc_z = get_joint_qacc(model, data, joint_name_z)
    # y
    joint_name_a = joint_names_a[idx]
    qacc_a = get_joint_qacc(model, data, joint_name_a)
    # y
    joint_name_b = joint_names_b[idx]
    qacc_b = get_joint_qacc(model, data, joint_name_b)
    # y
    joint_name_c = joint_names_c[idx]
    qacc_c = get_joint_qacc(model, data, joint_name_c)
    
    qacc = np.array([*qacc_x, *qacc_y, *qacc_z, *qacc_a, *qacc_b, *qacc_c])
    return qacc

# models
XML_freejoint=r"""
<mujoco>
  <worldbody>
    <body name="body_0" pos="0.12 0.12 0.00700" gravcomp="1">
      <joint name="joint_0" type="free" damping="0" />
      <geom name="geom_0" type="box" size="0.0775 0.0775 0.006" mass="1.24" pos="0 0 0"/>
    </body>
  </worldbody>
  
  <actuator>
		<!-- mover actuators -->
		<general name="actuator_x_0" joint="joint_0" gear="1 0 0 0 0 0" dyntype="none" gaintype="fixed" biastype="none"/>
		<general name="actuator_y_0" joint="joint_0" gear="0 1 0 0 0 0" dyntype="none" gaintype="fixed" biastype="none"/>
		<general name="actuator_z_0" joint="joint_0" gear="0 0 1 0 0 0" dyntype="none" gaintype="fixed" biastype="none"/>
		<general name="actuator_a_0" joint="joint_0" gear="0 0 0 1 0 0" dyntype="none" gaintype="fixed" biastype="none"/>
		<general name="actuator_b_0" joint="joint_0" gear="0 0 0 0 1 0" dyntype="none" gaintype="fixed" biastype="none"/>
		<general name="actuator_c_0" joint="joint_0" gear="0 0 0 0 0 1" dyntype="none" gaintype="fixed" biastype="none"/>
	</actuator>
  
</mujoco>
"""

XML_singlejoints=r"""
<mujoco>
  <worldbody>
    <body name="body_0" pos="0.12 0.12 0.00700" gravcomp="1">
      <joint name="joint_x_0" type="slide" axis="1 0 0" damping="0" />
	  <joint name="joint_y_0" type="slide" axis="0 1 0" damping="0" />
      <joint name="joint_z_0" type="slide" axis="0 0 1" damping="0" />
      
      <joint name="joint_a_0" type="hinge" axis="1 0 0" damping="0" />
      <joint name="joint_b_0" type="hinge" axis="0 1 0" damping="0" />
	  <joint name="joint_c_0" type="hinge" axis="0 0 1" damping="0" />
      
      <geom name="geom_0" type="box" size="0.0775 0.0775 0.006" mass="1.24" pos="0 0 0"/>
    </body>
  </worldbody>
  
  <actuator>
		<!-- mover actuators -->
		<general name="actuator_x_0" joint="joint_x_0" gear="1" dyntype="none" gaintype="fixed" biastype="none" />
		<general name="actuator_y_0" joint="joint_y_0" gear="1" dyntype="none" gaintype="fixed" biastype="none" />
		<general name="actuator_z_0" joint="joint_z_0" gear="1" dyntype="none" gaintype="fixed" biastype="none" />
		<general name="actuator_a_0" joint="joint_a_0" gear="1" dyntype="none" gaintype="fixed" biastype="none" />
		<general name="actuator_b_0" joint="joint_b_0" gear="1" dyntype="none" gaintype="fixed" biastype="none" />
		<general name="actuator_c_0" joint="joint_c_0" gear="1" dyntype="none" gaintype="fixed" biastype="none" />
	</actuator>
  
</mujoco>
"""

# free joint
model_fj = mujoco.MjModel.from_xml_string(XML_freejoint)
data_fj = mujoco.MjData(model_fj)

body_names_fj = ["body_0"]
joint_names_fj = ["joint_0"]

actuator_names_fj = ["actuator_x_0","actuator_y_0","actuator_z_0","actuator_a_0","actuator_b_0","actuator_c_0"]

# single joints
model_sj = mujoco.MjModel.from_xml_string(XML_singlejoints)
data_sj = mujoco.MjData(model_sj)

body_names_sj = ["body_0"]
joint_names_x_sj = ["joint_x_0"]
joint_names_y_sj = ["joint_y_0"]
joint_names_z_sj = ["joint_z_0"]
joint_names_a_sj = ["joint_a_0"]
joint_names_b_sj = ["joint_b_0"]
joint_names_c_sj = ["joint_c_0"]

actuator_names_sj = ["actuator_x_0","actuator_y_0","actuator_z_0","actuator_a_0","actuator_b_0","actuator_c_0"]

mujoco.mj_step(model_fj, data_fj)
mujoco.mj_step(model_sj, data_sj)

print()

for i in range(0,10):
    # set actuator controls for freejoint and single joint variants
    for i in range(0,len(actuator_names_fj)):
        set_actuator_ctrl(model=model_fj, data=data_fj, actuator_name=actuator_names_fj[i], value=0.5)
        set_actuator_ctrl(model=model_sj, data=data_sj, actuator_name=actuator_names_sj[i], value=0.5)
    
    # integration step 
    mujoco.mj_step(model_fj, data_fj)
    mujoco.mj_step(model_sj, data_sj)
    mujoco.mj_forward(model_sj, data_sj)
    
    # get qpos and dynamics
    qpos_fj = get_qpos_free(model=model_fj, data=data_fj, body_names=body_names_fj, joint_names=joint_names_fj, body_name=body_names_fj[0])
    qvel_fj = get_qvel_free(model=model_fj, data=data_fj, body_names=body_names_fj, joint_names=joint_names_fj, body_name=body_names_fj[0])
    qacc_fj = get_qacc_free(model=model_fj, data=data_fj, body_names=body_names_fj, joint_names=joint_names_fj, body_name=body_names_fj[0])

    qpos_sj = get_qpos_single(model=model_sj, data=data_sj, body_name=body_names_sj[0])
    qvel_sj = get_qvel_single(model=model_sj, data=data_sj, body_name=body_names_sj[0])
    qacc_sj = get_qacc_single(
        model=model_sj,
        data=data_sj,
        body_names=body_names_sj,
        joint_names_x=joint_names_x_sj,
        joint_names_y=joint_names_y_sj,
        joint_names_z=joint_names_z_sj,
        joint_names_a=joint_names_a_sj,
        joint_names_b=joint_names_b_sj,
        joint_names_c=joint_names_c_sj,
        body_name=body_names_sj[0]
    )
    
    assert np.allclose(np.linalg.norm(qpos_fj[3:], ord=2), 1)
    assert np.allclose(np.linalg.norm(qpos_sj[3:], ord=2), 1)

    print("*"*100)
    print(f"QPos (freejoint):\t{qpos_fj}")
    print(f"QPos (singlejoint):\t{qpos_sj}")
    print()
    print(f"QVel (freejoint):\t{qvel_fj}")
    print(f"QVel (singlejoint):\t{qvel_sj}")
    print()
    print(f"QAcc (freejoint):\t{qacc_fj}")
    print(f"QAcc (singlejoint):\t{qacc_sj}")

Confirmations

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

[caelum-artifex]

1 —Why the States Don’t Match

It is not possible for a free joint and a chain of six scalar joints (3 slide + 3 hinge) to produce matching qpos, qvel, or qacc values for the rotational DoFs.

A MuJoCo free joint represents orientation using a unit quaternion, while three hinge joints represent rotation using sequential Euler rotations. These two rotation representations are mathematically different, use different integration methods, and lead to different velocities and accelerations even if the initial states are identical.

Translation can match, but rotation will always diverge.
So the difference you are observing is expected and not an error in your code.

2 — Why Replacing the Free Joint Doesn’t Work

Replacing a free joint with six scalar joints creates a kinematically similar setup, but the dynamics and the state representation are not equivalent. Quaternion updates are smooth and globally minimal, while Euler hinge updates are sequential and axis-dependent. This causes divergence in rotation state, angular velocity, and angular acceleration immediately after stepping.

Therefore, matching the free-joint state using scalar joints is not feasible.

3 — How to Correctly Couple Only the Z-Rotation

There is no need to decompose a free joint into multiple scalar joints. MuJoCo already allows coupling or constraining only one rotational axis of a free joint using better tools than joint equality:

Recommended methods:

Weld equality with soft constraints
A weld constraint can be tuned so that only the z-axis rotation is constrained while the other 5 DoFs remain free. This is done by adjusting solref/solimp.

Tendon-based equality
Use a tendon to represent the rotational error around the z-axis and enforce equality between the free joint and a hinge joint.

Both approaches preserve the free joint (and its quaternion representation) without breaking the dynamics.

4 — Simpler Alternative (If Hard Constraint Not Required)

If you only need the hinge joint to track the free joint’s z-rotation, you can extract the yaw angle from the free joint using:

yaw = mju_mat2Euler(data.body_xmat[body_id])[2]

[v-r-a]

Hello!

Here is something:

Is it generally possible to obtain similar values for qpos, qvel, and qacc for one free joint and six scalar joints? If so, where is my mistake?

Angular velocity and acceleration of a body attached with free joint is expressed in body-fixed frame. See issue 691
The same can be computed using functions mj_objectVelocity and mj_objectAcceleration. I am not an expert, but using ``c'' quantities directly is discouraged in the last paragraph here.

Editted your function (now your angular velocities match). You could do the same for acceleration.

def get_qvel_single(model, data, body_name):
    """Return the 6D body velocity of the desired body."""
    # cvel = data.body(body_name).cvel
    # qvel = np.array([*cvel[3:],*cvel[:3]])
    # return qvel.copy()

    bID = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name)
    vel6D = np.zeros(6)
    mujoco.mj_objectVelocity(model, data, mujoco.mjtObj.mjOBJ_BODY, bID, vel6D, 1) # in local (body-fixed) orientation
    return np.array([*vel6D[3:], *vel6D[:3]])

[caelum-artifex]

Notes on replacing a free joint with six scalar joints in MuJoCo

---------------------------------------------------------------

1) A free joint uses a quaternion for rotation, while hinge joints use

sequential Euler rotations. These two representations are not

dynamically equivalent. Translation may match, but rotation (qpos,

qvel, qacc) will not match. Divergence is expected.

2) Angular velocity from a free joint is expressed in the body-fixed

coordinate frame. Using data.body(...).cvel mixes coordinate frames

and will not match the free-joint angular velocity.

3) To compare rotational velocities correctly, use mj_objectVelocity

with flg_local=1 so MuJoCo returns body-fixed velocities:

import mujoco
import numpy as np

def get_qvel_single(model, data, body_name):
body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name)
vel6d = np.zeros(6)
# mj_objectVelocity computes velocity in body-fixed frame when flg_local=1
mujoco.mj_objectVelocity(model, data, mujoco.mjtObj.mjOBJ_BODY, body_id, vel6d, 1)
# reorder to match free-joint convention: [angular, linear]
return np.array([*vel6d[3:], *vel6d[:3]])

4) You can apply the same pattern with mj_objectAcceleration for matching qacc.

5) Recommendation:

Instead of decomposing a free joint into six scalar joints (which will

never match quaternion dynamics), use one of MuJoCo's built-in

mechanisms to couple only the z-axis rotation of the free joint:

- Weld equality (soft constraint via solref/solimp)

- Tendon equality (build a tendon that measures yaw error)

- Or extract yaw via mju_mat2Euler(...) and control a hinge joint

to track that angle if a soft coupling is acceptable.

Example: extracting yaw from the free joint orientation

def get_yaw(model, data, body_name):
body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name)
mat = data.body_xmat[body_id]
# MuJoCo helper: convert rotation matrix to Euler angles (xyz)
euler = mujoco.mju_mat2Euler(mat)
yaw = euler[2]
return yaw

Summary:

--------

• Free joint (quaternion) and 6-joint Euler chain will not match.

• Use mj_objectVelocity / mj_objectAcceleration for proper comparisons.

• For coupling only yaw, prefer weld/tendon equality or yaw-tracking control.