def collect_contacts(self, physics, geom_ids):
contacts = {}
forcetorque = np.zeros(6)
for i, contact in enumerate(physics.data.contact):
if ((contact.geom1 in geom_ids) or
(contact.geom2 in geom_ids)) and contact.dist < contact.includemargin:
mjlib.mj_contactForce(physics.model.ptr, physics.data.ptr, i,
forcetorque)
contacts[(contact.geom1, contact.geom2)] = (forcetorque[0]
+ contacts.get(
(contact.geom1,
contact.geom2), 0.))
return contacts
def contact_force(self, contact_id):
"""Returns the wrench of a contact as a 2 x 3 array of (forces, torques).
Args:
contact_id: Integer, the index of the contact within the contact buffer
(`self.contact`).
Returns:
2x3 array with stacked (force, torque). Note that the order of dimensions
is (normal, tangent, tangent), in the contact's frame.
Raises:
ValueError: If `contact_id` is negative or bigger than ncon-1.
"""
if not 0 <= contact_id < self.ncon:
raise ValueError(_CONTACT_ID_OUT_OF_RANGE
.format(max_valid=self.ncon-1, actual=contact_id))
wrench = np.empty(6, dtype=np.float64)
mjlib.mj_contactForce(self.model.ptr, self.ptr, contact_id, wrench)
return wrench.reshape(2, 3)
