Validation of mj_contactForce in simulation different from theoretical calculation #2622

Chuanfang-Neptune · 2025-05-11T17:49:56Z

Chuanfang-Neptune
May 11, 2025

Intro

Hi!

I am a Phd student using MuJoCo to simulate some quadrupod robots. I want to reduce the reality gap between MuJoCo simulation and my real robot experiments, and I'm validating some metrics like the contact force to compare with real sensor readings.

I have one question about the contact force calculation in MuJoCo. I take a look at the solver parameters here. I also read this relevant issue #732, where the OP tried to validate the penetration at steady state and compared with real results (which was matching).

I'm more interested in getting the contact force during simulation at each step rather than looking at the end state because this allows me to make some comparison of my sensor data. From the documentation MuJoCo didn't explicitly compute contact force but computed a target acceleration with $a_1+d⋅(bv+kr)=(1−d)⋅a_0 $. Meanwhile MuJoCo provides some function such as 'mj_contactForce' that put the data in 'physics.sensordata' that can be accessed later. But I didn't find how the mj_contactForce was exactly calculated. Code seemed to copy it from result of mju_decodePyramid.

In the end, I tried to calculate it myself with the equation above. If $a_0$ is acceleration in absent of the contact, it should be -9.8 from gravity. And the $a_1$ should be the acceleration with contact. Therefore, I suppose, the contact force should be $m*(a_1-a_0)$, which caused the change of acceleration (here only considering normal force (z)). If I take d is almost 1, the equation should downgrade to $F_{contact} = m*(bv+kr)$ and I wrote the script below to validate the result.

In the code I set a cube to start at a position below the ground with different mass, position and velocity. I do one mj_step and record the contact force to compare with the theoretical value and print the error.

import mujoco
import numpy as np
import matplotlib.pyplot as plt

# create a cube with 0.05m side
#SET solref to 0.02 1
#SET solimp to 1 to disable it
mjcf = """
<mujoco>
    <option gravity="0 0 -9.81"/>
    <worldbody>
        <geom name="floor" type="plane" pos="0 0 0" mass="1" size="1 1 0.1" rgba="0.8 0.9 0.8 1"/>
        <body name="cube" pos="0 0 0.2">
            <joint name="free" type="free"/>
            <geom name="box" type="box" mass="1" size="0.05 0.05 0.05" rgba="1 0 0 1"
                  solref="0.02 1" solimp="1 1 0.001" contype="1" conaffinity="1"/>
        </body>
    </worldbody>
</mujoco>
"""



model = mujoco.MjModel.from_xml_string(mjcf)
data = mujoco.MjData(model)

body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "cube")


# Grid of penetration depths and speeds
depths = np.linspace(0.002, 0.01, 5)   # meters
speeds = np.linspace(0.0, -1.0, 3)     # m/s (negative: downward)
mass = np.linspace(1.0, 3.0, 3)
forces = np.zeros((len(depths), len(speeds)))
for m in mass:
    model.body_mass[body_id] = m
    for i, depth in enumerate(depths):
        for j, speed in enumerate(speeds):
            # Reset sim
            data = mujoco.MjData(model)

            # teleport cube below surface (z = 0.05 box half-height - depth)
            cube_z = 0.05 - depth
            data.qpos[2] = cube_z
            data.qvel[2] = speed
            
            #one step to update force
            mujoco.mj_forward(model, data)
            # Get contact force (notmal z)
            force_val = 0
            if data.ncon > 0:
                contact = data.contact[0]
                force = np.zeros(6)
                mujoco.mj_contactForce(model, data, 0, force)
                force_val = force[0]  # normal force

            forces[i, j] = force_val
            
            
            # Theoretical force
            # convert kd to indirect format
            # dmax=1 so not scaling here
            K = 1/0.02**2 
            D = 2*1/0.02
            expected_force = (K * depth + D * abs(speed))*m #from mujuco equation (solimp is always 1 here so unscaled)

            print(f"Mass: {m:.1f} kg, Exp: Depth: {depth:.4f}, Speed: {speed:.2f} → expforce: {force_val:.2f} theoryforce: {expected_force:.2f} error:{abs(expected_force-force_val)/expected_force*100:.2f}%")

However, the result is different from the calculation. Do you have any ideas why it is off so much? What could be wrong in my calculation/code?

Mass: 1.0 kg, Exp: Depth: 0.0020, Speed: 0.00 → expforce: 3.71 theoryforce: 5.00 error:25.78%
Mass: 1.0 kg, Exp: Depth: 0.0020, Speed: -0.50 → expforce: 16.45 theoryforce: 55.00 error:70.09%
Mass: 1.0 kg, Exp: Depth: 0.0020, Speed: -1.00 → expforce: 29.19 theoryforce: 105.00 error:72.20%
Mass: 1.0 kg, Exp: Depth: 0.0040, Speed: 0.00 → expforce: 4.98 theoryforce: 10.00 error:50.15%
Mass: 1.0 kg, Exp: Depth: 0.0040, Speed: -0.50 → expforce: 17.72 theoryforce: 60.00 error:70.46%
Mass: 1.0 kg, Exp: Depth: 0.0040, Speed: -1.00 → expforce: 30.46 theoryforce: 110.00 error:72.31%
Mass: 1.0 kg, Exp: Depth: 0.0060, Speed: 0.00 → expforce: 6.26 theoryforce: 15.00 error:58.28%
Mass: 1.0 kg, Exp: Depth: 0.0060, Speed: -0.50 → expforce: 19.00 theoryforce: 65.00 error:70.77%
Mass: 1.0 kg, Exp: Depth: 0.0060, Speed: -1.00 → expforce: 31.74 theoryforce: 115.00 error:72.40%
Mass: 1.0 kg, Exp: Depth: 0.0080, Speed: 0.00 → expforce: 7.53 theoryforce: 20.00 error:62.34%
Mass: 1.0 kg, Exp: Depth: 0.0080, Speed: -0.50 → expforce: 20.27 theoryforce: 70.00 error:71.04%
Mass: 1.0 kg, Exp: Depth: 0.0080, Speed: -1.00 → expforce: 33.01 theoryforce: 120.00 error:72.49%
Mass: 1.0 kg, Exp: Depth: 0.0100, Speed: 0.00 → expforce: 8.81 theoryforce: 25.00 error:64.77%
Mass: 1.0 kg, Exp: Depth: 0.0100, Speed: -0.50 → expforce: 21.55 theoryforce: 75.00 error:71.27%
Mass: 1.0 kg, Exp: Depth: 0.0100, Speed: -1.00 → expforce: 34.28 theoryforce: 125.00 error:72.57%
Mass: 2.0 kg, Exp: Depth: 0.0020, Speed: 0.00 → expforce: 6.15 theoryforce: 10.00 error:38.52%
Mass: 2.0 kg, Exp: Depth: 0.0020, Speed: -0.50 → expforce: 18.89 theoryforce: 110.00 error:82.83%
Mass: 2.0 kg, Exp: Depth: 0.0020, Speed: -1.00 → expforce: 31.63 theoryforce: 210.00 error:84.94%
Mass: 2.0 kg, Exp: Depth: 0.0040, Speed: 0.00 → expforce: 7.42 theoryforce: 20.00 error:62.89%
Mass: 2.0 kg, Exp: Depth: 0.0040, Speed: -0.50 → expforce: 20.16 theoryforce: 120.00 error:83.20%
Mass: 2.0 kg, Exp: Depth: 0.0040, Speed: -1.00 → expforce: 32.90 theoryforce: 220.00 error:85.05%
Mass: 2.0 kg, Exp: Depth: 0.0060, Speed: 0.00 → expforce: 8.70 theoryforce: 30.00 error:71.02%
Mass: 2.0 kg, Exp: Depth: 0.0060, Speed: -0.50 → expforce: 21.43 theoryforce: 130.00 error:83.51%
Mass: 2.0 kg, Exp: Depth: 0.0060, Speed: -1.00 → expforce: 34.17 theoryforce: 230.00 error:85.14%
Mass: 2.0 kg, Exp: Depth: 0.0080, Speed: 0.00 → expforce: 9.97 theoryforce: 40.00 error:75.08%
Mass: 2.0 kg, Exp: Depth: 0.0080, Speed: -0.50 → expforce: 22.71 theoryforce: 140.00 error:83.78%
Mass: 2.0 kg, Exp: Depth: 0.0080, Speed: -1.00 → expforce: 35.45 theoryforce: 240.00 error:85.23%
Mass: 2.0 kg, Exp: Depth: 0.0100, Speed: 0.00 → expforce: 11.24 theoryforce: 50.00 error:77.51%
Mass: 2.0 kg, Exp: Depth: 0.0100, Speed: -0.50 → expforce: 23.98 theoryforce: 150.00 error:84.01%
Mass: 2.0 kg, Exp: Depth: 0.0100, Speed: -1.00 → expforce: 36.72 theoryforce: 250.00 error:85.31%
Mass: 3.0 kg, Exp: Depth: 0.0020, Speed: 0.00 → expforce: 8.58 theoryforce: 15.00 error:42.77%
Mass: 3.0 kg, Exp: Depth: 0.0020, Speed: -0.50 → expforce: 21.32 theoryforce: 165.00 error:87.08%
Mass: 3.0 kg, Exp: Depth: 0.0020, Speed: -1.00 → expforce: 34.06 theoryforce: 315.00 error:89.19%
Mass: 3.0 kg, Exp: Depth: 0.0040, Speed: 0.00 → expforce: 9.86 theoryforce: 30.00 error:67.14%
Mass: 3.0 kg, Exp: Depth: 0.0040, Speed: -0.50 → expforce: 22.60 theoryforce: 180.00 error:87.45%
Mass: 3.0 kg, Exp: Depth: 0.0040, Speed: -1.00 → expforce: 35.34 theoryforce: 330.00 error:89.29%
Mass: 3.0 kg, Exp: Depth: 0.0060, Speed: 0.00 → expforce: 11.13 theoryforce: 45.00 error:75.26%
Mass: 3.0 kg, Exp: Depth: 0.0060, Speed: -0.50 → expforce: 23.87 theoryforce: 195.00 error:87.76%
Mass: 3.0 kg, Exp: Depth: 0.0060, Speed: -1.00 → expforce: 36.61 theoryforce: 345.00 error:89.39%
Mass: 3.0 kg, Exp: Depth: 0.0080, Speed: 0.00 → expforce: 12.41 theoryforce: 60.00 error:79.32%
Mass: 3.0 kg, Exp: Depth: 0.0080, Speed: -0.50 → expforce: 25.15 theoryforce: 210.00 error:88.03%
Mass: 3.0 kg, Exp: Depth: 0.0080, Speed: -1.00 → expforce: 37.88 theoryforce: 360.00 error:89.48%
Mass: 3.0 kg, Exp: Depth: 0.0100, Speed: 0.00 → expforce: 13.68 theoryforce: 75.00 error:81.76%
Mass: 3.0 kg, Exp: Depth: 0.0100, Speed: -0.50 → expforce: 26.42 theoryforce: 225.00 error:88.26%
Mass: 3.0 kg, Exp: Depth: 0.0100, Speed: -1.00 → expforce: 39.16 theoryforce: 375.00 error:89.56%

My setup

mujoco: 3.1.6
OS: result is same in Win/Ubuntu

My question

No response

Minimal model and/or code that explain my question

No response

Confirmations

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

quagla · 2025-05-12T09:19:52Z

quagla
May 12, 2025
Collaborator

I don't know if that fixes the calculation but note that d is clamped to mjMAXIMP https://mujoco.readthedocs.io/en/latest/APIreference/APIglobals.html#glnumeric

1 reply

Chuanfang-Neptune May 12, 2025
Author

Thank you very much for your response!

Indeed I tried to consider the solimp but found it was not significant.
I consider $a_1+d(bv+kr)=(1-d)a_0$, where $a_0$ is $g$. The contact force $F_{contact} = ma_1-mg$ (we take up as positive and g is negative here)
embedding the contact force in the equation above $F_{contact} =ma_1-mg= -d(g+bv+kr)$. The expected force is only scaled by d (0.95 to 0.99 if I use default value. $g$ is much smaller compared to $bv+kr$) This doesn't explain the fact that the expected force is off the simulation that much. I think there is something wrong in my calculation and the contact_force in MuJoCo is indeed not calculated in the way I suppose.

Here is the updated code:

expected_force = 0.95* (9.8 + K * depth + D * abs(speed))

And I still get big errors:

Mass: 1.0 kg, Exp: Depth: 0.0020, Speed: 0.00 → expforce: 3.72 theoryforce: 14.06 error:73.55%
Mass: 1.0 kg, Exp: Depth: 0.0020, Speed: -0.50 → expforce: 16.71 theoryforce: 61.56 error:72.86%
Mass: 1.0 kg, Exp: Depth: 0.0020, Speed: -1.00 → expforce: 29.69 theoryforce: 109.06 error:72.77%
Mass: 1.0 kg, Exp: Depth: 0.0040, Speed: 0.00 → expforce: 5.02 theoryforce: 18.81 error:73.32%
Mass: 1.0 kg, Exp: Depth: 0.0040, Speed: -0.50 → expforce: 18.01 theoryforce: 66.31 error:72.85%
Mass: 1.0 kg, Exp: Depth: 0.0040, Speed: -1.00 → expforce: 30.99 theoryforce: 113.81 error:72.77%
Mass: 1.0 kg, Exp: Depth: 0.0060, Speed: 0.00 → expforce: 6.32 theoryforce: 23.56 error:73.19%
Mass: 1.0 kg, Exp: Depth: 0.0060, Speed: -0.50 → expforce: 19.30 theoryforce: 71.06 error:72.83%
Mass: 1.0 kg, Exp: Depth: 0.0060, Speed: -1.00 → expforce: 32.29 theoryforce: 118.56 error:72.76%
Mass: 1.0 kg, Exp: Depth: 0.0080, Speed: 0.00 → expforce: 7.62 theoryforce: 28.31 error:73.10%
Mass: 1.0 kg, Exp: Depth: 0.0080, Speed: -0.50 → expforce: 20.60 theoryforce: 75.81 error:72.82%
Mass: 1.0 kg, Exp: Depth: 0.0080, Speed: -1.00 → expforce: 33.59 theoryforce: 123.31 error:72.76%
Mass: 1.0 kg, Exp: Depth: 0.0100, Speed: 0.00 → expforce: 8.91 theoryforce: 33.06 error:73.04%
Mass: 1.0 kg, Exp: Depth: 0.0100, Speed: -0.50 → expforce: 21.90 theoryforce: 80.56 error:72.81%
Mass: 1.0 kg, Exp: Depth: 0.0100, Speed: -1.00 → expforce: 34.89 theoryforce: 128.06 error:72.76%
Mass: 2.0 kg, Exp: Depth: 0.0020, Speed: 0.00 → expforce: 6.14 theoryforce: 14.06 error:56.33%
Mass: 2.0 kg, Exp: Depth: 0.0020, Speed: -0.50 → expforce: 19.13 theoryforce: 61.56 error:68.93%
Mass: 2.0 kg, Exp: Depth: 0.0020, Speed: -1.00 → expforce: 32.11 theoryforce: 109.06 error:70.55%
Mass: 2.0 kg, Exp: Depth: 0.0040, Speed: 0.00 → expforce: 7.44 theoryforce: 18.81 error:60.45%
Mass: 2.0 kg, Exp: Depth: 0.0040, Speed: -0.50 → expforce: 20.43 theoryforce: 66.31 error:69.20%
Mass: 2.0 kg, Exp: Depth: 0.0040, Speed: -1.00 → expforce: 33.41 theoryforce: 113.81 error:70.64%
Mass: 2.0 kg, Exp: Depth: 0.0060, Speed: 0.00 → expforce: 8.74 theoryforce: 23.56 error:62.91%
Mass: 2.0 kg, Exp: Depth: 0.0060, Speed: -0.50 → expforce: 21.72 theoryforce: 71.06 error:69.43%
Mass: 2.0 kg, Exp: Depth: 0.0060, Speed: -1.00 → expforce: 34.71 theoryforce: 118.56 error:70.72%
Mass: 2.0 kg, Exp: Depth: 0.0080, Speed: 0.00 → expforce: 10.04 theoryforce: 28.31 error:64.55%
Mass: 2.0 kg, Exp: Depth: 0.0080, Speed: -0.50 → expforce: 23.02 theoryforce: 75.81 error:69.63%
Mass: 2.0 kg, Exp: Depth: 0.0080, Speed: -1.00 → expforce: 36.01 theoryforce: 123.31 error:70.80%
Mass: 2.0 kg, Exp: Depth: 0.0100, Speed: 0.00 → expforce: 11.33 theoryforce: 33.06 error:65.71%
Mass: 2.0 kg, Exp: Depth: 0.0100, Speed: -0.50 → expforce: 24.32 theoryforce: 80.56 error:69.81%
Mass: 2.0 kg, Exp: Depth: 0.0100, Speed: -1.00 → expforce: 37.31 theoryforce: 128.06 error:70.87%
Mass: 3.0 kg, Exp: Depth: 0.0020, Speed: 0.00 → expforce: 8.56 theoryforce: 14.06 error:39.11%
Mass: 3.0 kg, Exp: Depth: 0.0020, Speed: -0.50 → expforce: 21.55 theoryforce: 61.56 error:65.00%
Mass: 3.0 kg, Exp: Depth: 0.0020, Speed: -1.00 → expforce: 34.53 theoryforce: 109.06 error:68.33%
Mass: 3.0 kg, Exp: Depth: 0.0040, Speed: 0.00 → expforce: 9.86 theoryforce: 18.81 error:47.58%
Mass: 3.0 kg, Exp: Depth: 0.0040, Speed: -0.50 → expforce: 22.85 theoryforce: 66.31 error:65.55%
Mass: 3.0 kg, Exp: Depth: 0.0040, Speed: -1.00 → expforce: 35.83 theoryforce: 113.81 error:68.51%
Mass: 3.0 kg, Exp: Depth: 0.0060, Speed: 0.00 → expforce: 11.16 theoryforce: 23.56 error:52.64%
Mass: 3.0 kg, Exp: Depth: 0.0060, Speed: -0.50 → expforce: 24.15 theoryforce: 71.06 error:66.02%
Mass: 3.0 kg, Exp: Depth: 0.0060, Speed: -1.00 → expforce: 37.13 theoryforce: 118.56 error:68.68%
Mass: 3.0 kg, Exp: Depth: 0.0080, Speed: 0.00 → expforce: 12.46 theoryforce: 28.31 error:56.00%
Mass: 3.0 kg, Exp: Depth: 0.0080, Speed: -0.50 → expforce: 25.44 theoryforce: 75.81 error:66.44%
Mass: 3.0 kg, Exp: Depth: 0.0080, Speed: -1.00 → expforce: 38.43 theoryforce: 123.31 error:68.83%
Mass: 3.0 kg, Exp: Depth: 0.0100, Speed: 0.00 → expforce: 13.76 theoryforce: 33.06 error:58.39%
Mass: 3.0 kg, Exp: Depth: 0.0100, Speed: -0.50 → expforce: 26.74 theoryforce: 80.56 error:66.80%
Mass: 3.0 kg, Exp: Depth: 0.0100, Speed: -1.00 → expforce: 39.73 theoryforce: 128.06 error:68.98%

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Validation of mj_contactForce in simulation different from theoretical calculation #2622

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{editor}}'s edit

{{editor}}'s edit

Uh oh!

Replies: 1 comment 1 reply

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Select a reply

Uh oh!

Validation of mj_contactForce in simulation different from theoretical calculation #2622

Uh oh!

Uh oh!

Chuanfang-Neptune May 11, 2025

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 1 comment · 1 reply

Uh oh!

quagla May 12, 2025 Collaborator

Uh oh!

Chuanfang-Neptune May 12, 2025 Author

Chuanfang-Neptune
May 11, 2025

Replies: 1 comment 1 reply

quagla
May 12, 2025
Collaborator

Chuanfang-Neptune May 12, 2025
Author