example reading out mujoco contacts

contacts.py

#!/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')

Hi, very thanks to your code.
Besides the contact information, I want to also know the contact force.
This is the code how I implemented for the contact force, I put it in line 25:

print('contact force on geom2', sim.data.cfrc_ext[model.geom_bodyid[contact.geom2]])

However, I am not sure if it is right... cfrc_ext is the external force on body, can I directly use it as the contact force? If not, how shall I calculate the contact force?

Thanks in advance.

Hello,

Thank you for the code that was really helpful!

My understanding is that cfrc_ext is the combination of all contact forces and any forces applied by the user in xfrc_applied. See this mujoco forum thread.

Hi

Thank you for your code
I just have a problem to find valid entries of contact array. Could you give a solution?

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

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