Skip to content

Instantly share code, notes, and snippets.

@machinaut
Last active October 25, 2024 18:47
Show Gist options
  • Save machinaut/209c44e8c55245c0d0f0094693053158 to your computer and use it in GitHub Desktop.
Save machinaut/209c44e8c55245c0d0f0094693053158 to your computer and use it in GitHub Desktop.
example reading out mujoco contacts
#!/usr/bin/env python
import os
import mujoco_py
import numpy as np
PATH_TO_HUMANOID_XML = os.path.expanduser('~/.mujoco/mjpro150/model/humanoid.xml')
# Load the model and make a simulator
model = mujoco_py.load_model_from_path(PATH_TO_HUMANOID_XML)
sim = mujoco_py.MjSim(model)
# Simulate 1000 steps so humanoid has fallen on the ground
for _ in range(10000):
sim.step()
print('number of contacts', sim.data.ncon)
for i in range(sim.data.ncon):
# Note that the contact array has more than `ncon` entries,
# so be careful to only read the valid entries.
contact = sim.data.contact[i]
print('contact', i)
print('dist', contact.dist)
print('geom1', contact.geom1, sim.model.geom_id2name(contact.geom1))
print('geom2', contact.geom2, sim.model.geom_id2name(contact.geom2))
# There's more stuff in the data structure
# See the mujoco documentation for more info!
geom2_body = sim.model.geom_bodyid[sim.data.contact[i].geom2]
print(' Contact force on geom2 body', sim.data.cfrc_ext[geom2_body])
print('norm', np.sqrt(np.sum(np.square(sim.data.cfrc_ext[geom2_body]))))
# Use internal functions to read out mj_contactForce
c_array = np.zeros(6, dtype=np.float64)
print('c_array', c_array)
mujoco_py.functions.mj_contactForce(sim.model, sim.data, i, c_array)
print('c_array', c_array)
print('done')
@Krishnendu1984
Copy link

how to get ground reaction contact force and also its location/position coordinates in mujoco can anyone tell?

@Krishnendu1984
Copy link

Could you please tell this c_array has 6 values in array .What does the first three values if it is force what is the order like first z.y.x or x,y,z.
Since I am getting more values in first one I guess it is z axis force upwards

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment