def get_gripper_table_contact()

in robogym/robot/ur16e/mujoco/simulation/base.py [0:0]

• 14 lines of code
• 8 McCabe index (conditional complexity)

    def get_gripper_table_contact(self) -> bool:
        """
        Determine if any part of the gripper is touching the table by checking if there
        is a collision between the table_collision_plane id and any gripper geom id.
        """

        contacts = []
        gripper_table_contact = False

        # Sweep through all mj_sim contacts
        for i in range(self.mj_sim.data.ncon):
            c = self.mj_sim.data.contact[i]

            # Check if any of the contacts involve at gripper geom id, append them to contacts:
            if c.geom1 in self.geom_ids:
                contacts.append(c.geom2)
            elif c.geom2 in self.geom_ids:
                contacts.append(c.geom1)

            # Check if any of the contacts correspond to the `table` id:
            for contact in contacts:
                contact_name = self.mj_sim.model.geom_id2name(contact)
                if contact_name == "table_collision_plane":
                    gripper_table_contact = True

        return gripper_table_contact