Why mean of diagonal elements of matrix A is used in body_invweight0 and efc_diagApprox #2669

sunilgora · 2025-06-06T10:20:27Z

sunilgora
Jun 6, 2025

Intro

Hi!

I am a Ph.D candidate at IIT Kanpur, I use MuJoCo for my research on bipedal locomotion on deformable terrain.

My setup

Python API of MuJoCo 3.1.2 on Windows 10

My question

I am looking to set solimp and solref parameters for a particular value of ground penetration.
However, I have observed that the static equilibrium penetration depends on the initial configuration of the robot.
A simple example is as below:

<mujoco>
  <worldbody>
    <geom name="floor" pos="0 0 -0.1" size="0 0 0.00001" type="plane" />
    <body pos="0 0 0.0">
      <freejoint/>
              <geom mass="1" type="capsule" size="0.1" fromto="-0.1 -0.1 0.0 0.1 -0.1 0.0"/>
              <geom mass="1" type="capsule" size="0.1" fromto="-0.1 0.1 0.0 0.1 0.1 0.0"/>
      <body pos="0 0 0.0">
            <geom mass="1.0" type="sphere" pos = "0 0 0" size="0.1" rgba="1 0 0 1" contype="0" conaffinity="0" />
      </body>
    </body>
  </worldbody>
</mujoco>

The static equilibrium penetration for this model is 0.1077 mm. Now if I increase the height of second body to 1 m as

<mujoco>
  <worldbody>
    <geom name="floor" pos="0 0 -0.1" size="0 0 0.00001" type="plane" />
    <body pos="0 0 0.0">
      <freejoint/>
              <geom mass="1" type="capsule" size="0.1" fromto="-0.1 -0.1 0.0 0.1 -0.1 0.0"/>
              <geom mass="1" type="capsule" size="0.1" fromto="-0.1 0.1 0.0 0.1 0.1 0.0"/>
      <body pos="0 0 1.0">
            <geom mass="1.0" type="sphere" pos = "0 0 0" size="0.1" rgba="1 0 0 1" contype="0" conaffinity="0" />
      </body>
    </body>
  </worldbody>
</mujoco>

The static equilibrium penetration for the above model is 0.1407 mm.
This difference is because efc_diagApprox is the mean of the diagonal elements of $A$.
Theoretically, an increase in the vertical height of the second body should not affect the element of $A=JM^{-1}J^T$ in the vertical direction.

Is there any particular reason for using the mean of the diagonal elements of $A$ in MuJoCo?

Minimal model and/or code that explain my question

Model:

<mujoco>
  <worldbody>
    <geom name="floor" pos="0 0 -0.1" size="0 0 0.00001" type="plane" />
    <body pos="0 0 0.0">
      <freejoint/>
              <geom mass="1" type="capsule" size="0.1" fromto="-0.1 -0.1 0.0 0.1 -0.1 0.0"/>
              <geom mass="1" type="capsule" size="0.1" fromto="-0.1 0.1 0.0 0.1 0.1 0.0"/>
      <body pos="0 0 0.0">
            <geom mass="1.0" type="sphere" pos = "0 0 0" size="0.1" rgba="1 0 0 1" contype="0" conaffinity="0" />
      </body>
    </body>
  </worldbody>
</mujoco>

Python Code:

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

xml = """
<mujoco>
  <worldbody>
    <geom name="floor" pos="0 0 -0.1" size="0 0 0.00001" type="plane" />
    <body pos="0 0 0.0">
      <freejoint/>
              <geom mass="1" type="capsule" size="0.1" fromto="-0.1 -0.1 0.0 0.1 -0.1 0.0"/>
              <geom mass="1" type="capsule" size="0.1" fromto="-0.1 0.1 0.0 0.1 0.1 0.0"/>
      <body pos="0 0 1.0">
            <geom mass="1.0" type="sphere" pos = "0 0 0" size="0.1" rgba="1 0 0 1" contype="0" conaffinity="0" />
      </body>
    </body>
  </worldbody>
</mujoco>
"""

model = mujoco.MjModel.from_xml_string(xml)

#Function to find static equilibrium penetration #source: https://colab.research.google.com/github/deepmind/mujoco/blob/main/python/LQR.ipynb#scrollTo=yftlgN0yznRe
def DepthvsForce(model,data):
  # Find the height at which the vertical force becomes less than the weight, i.e. contact is initiated
  weight = model.body_subtreemass[1] * np.linalg.norm(model.opt.gravity)
  print('weight=',weight)
  mujoco.mj_inverse(model, data)
  if data.ncon: #Contact already exists
    dz=0.000001
  else: #No contact exists
    dz=-0.000001
  while True:
    data.qpos[2] += dz
    mujoco.mj_inverse(model, data)
    # print(data.qpos[2],data.qfrc_inverse[2],weight)
    if (dz>0)*(data.ncon==0) or (dz<0)*(data.ncon>0):
      z_0=data.qpos[2]
      break
  #Plot height vs Vertical Force
  height_arr = np.linspace(z_0-0.002, z_0, 2001)
  vertical_forces = []
  for z in height_arr:
    data.qpos[2] = z
    mujoco.mj_inverse(model, data)
    #if len(vertical_forces)<5: print(z,'weight:',weight,'inv of weight:',1/weight, 'mass of body:',model.body_mass,'inv of body mass:',1/model.body_mass[1],model.body_invweight0, 4*9.81/weight, 4*model.body_invweight0, data.efc_diagApprox[0])
    vertical_forces.append(data.qfrc_inverse[2])

  height_offsets=height_arr-z_0
  vertical_forces=np.array(vertical_forces)

  # Find the height-offset at which the vertical force is smallest.
  idx = np.argmin(np.abs(vertical_forces))
  best_offset = height_offsets[idx]
  # setting font sizeto 30
  plt.rcParams.update({'font.size': 10})
  plt.rcParams["font.family"] = "serif"
  plt.rcParams["mathtext.fontset"] = "dejavuserif"
  plt.rcParams["axes.spines.right"] = "False"
  plt.rcParams["axes.spines.top"] = "False"
  # Plot the relationship.
  plt.figure(figsize=(8, 6))
  plt.plot(abs(height_offsets) * 1, vertical_forces, 'r-',linewidth=3)
  # Vertical line at offset corresponding to smallest vertical force.
  plt.axvline(x=abs(best_offset)*1, color='black', linestyle='-')
  # Horizontal line at the humanoid's weight.
  weight = model.body_subtreemass[1]*np.linalg.norm(model.opt.gravity)
  plt.axhline(y=weight, color='black', linestyle='-')
  plt.xlabel('Deformation (m)')
  plt.ylabel('Vertical force on base (N)')
  plt.grid(which='major', color='#DDDDDD', linewidth=0.8)
  # plt.grid(which='minor', color='#EEEEEE', linestyle=':', linewidth=0.5)
  plt.minorticks_on()
  # ax.spines['right'].set_visible(False)
  # ax.spines['top'].set_visible(False)
  plt.title(f'Min. vertical force at deformation {str(best_offset*1000)[1:5]} mm.')

  return best_offset

#mujoco/humanoid model
data = mujoco.MjData(model)

#model.body_invweight0
mujoco.mj_kinematics(model, data)
mujoco.mj_comPos(model, data)
mujoco.mj_crb(model, data)
mujoco.mj_factorM(model, data)

nv=model.nv
jacp=np.zeros((3,nv))
jacr=np.zeros((3,nv))
tmp=np.zeros((6,nv))
A=np.zeros((6,6))
for i in range(model.nbody):
  # static  bodies: zero   invweight0
  if (model.body_weldid[i] == 0):
    print(i,'zero weight',model.body_invweight0[i])
  elif (model.body_simple[i] == 2):#  // accelerate simple bodies with no rotations
    print(i,'1/m',1/model.body_mass[i],',zero inert',model.body_invweight0[i])
  else:
    mujoco.mj_jacBodyCom(model, data, jacp, jacr, i)
    jac=np.append(jacp,jacr,axis=0)
    #print(jac)
    mujoco.mj_solveM(model, data, tmp, jac) #JM^-1
    mujoco.mju_mulMatMatT(A, jac, tmp) #J(M^-1)J^T
    print('body id:',i,',A:',A)
    print('meandiagA:',sum(np.diag(A[0:3,0:3]))/3,sum(np.diag(A[3:6,3:6]))/3)
    print('body_invweight0:',model.body_invweight0[i])


DepthvsForce(model,data)
plt.show()

Confirmations

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

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Why mean of diagonal elements of matrix A is used in body_invweight0 and efc_diagApprox #2669

Uh oh!

{{title}}

Uh oh!

Replies: 0 comments

Select a reply

Uh oh!

Why mean of diagonal elements of matrix A is used in body_invweight0 and efc_diagApprox #2669

Uh oh!

sunilgora Jun 6, 2025

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 0 comments

sunilgora
Jun 6, 2025