A script for detecting the collision and calculating the contact forces in mujoco environments.

collision.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')
#PATH_TO_HUMANOID_XML = os.path.expanduser('/home/rfa-xw/anaconda3/envs/mujoco-py/lib/python3.6/site-packages/gym/envs/robotics/assets/fetch/reach.xml')

# Load the model and make a simulator
model = mujoco_py.load_model_from_path(PATH_TO_HUMANOID_XML) # model: class PyMjModel
sim = mujoco_py.MjSim(model)
viewer = mujoco_py.MjViewer(sim)

for _ in range(1):
    sim.reset()
    # Simulate 1000 steps so humanoid has fallen on the ground
    for _ in range(1000):
        viewer.render()
        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('distance:', contact.dist)
        print('geom1:', contact.geom1, sim.model.geom_id2name(contact.geom1))
        print('geom2:', contact.geom2, sim.model.geom_id2name(contact.geom2))
        print('contact position:', contact.pos)
        
        # Use internal functions to read out mj_contactForce
        c_array = np.zeros(6, dtype=np.float64)
        mujoco_py.functions.mj_contactForce(sim.model, sim.data, i, c_array)
        
        # Convert the contact force from contact frame to world frame
        ref = np.reshape(contact.frame, (3,3))
        c_force = np.dot(np.linalg.inv(ref), c_array[0:3])
        c_torque = np.dot(np.linalg.inv(ref), c_array[3:6])
        print('contact force in world frame:', c_force)  
        print('contact torque in world frame:', c_torque)
        print()
        
        # The contact force can be used to define if one contact is a normal touch or a dangerous collison, according to customized demands in future.

How do you know on which objects are the contact force happening? Example if you have a robot hitting a box, and at the same time hitting another box, how can you identify the two contact forces between the robot and two boxes?