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)