add contact info solve #218 (#220)

* add contact info

* remove all zero value

* put a wrapper for get contact points in object.py and add test

* add doc string for get_contact

* fix get_contact function bug that return all contacts

* fix unit test

Co-authored-by: Stephen James <[email protected]>

Author: lianghongzhuo

pyrep/backend/sim.py

@@ -1381,6 +1381,32 @@ def simCheckOctreePointOccupancy(octreeHandle, options, points):
         return False
 
 
+def simGetContactInfo(contact_obj_handle, get_contact_normal):
+    index = 0
+    contact_list = []
+    result = 1
+
+    while result > 0:
+        if get_contact_normal:
+            contact = ffi.new('float[9]')
+            ext = sim_handleflag_extended
+        else:
+            contact = ffi.new('float[6]')
+            ext = 0
+
+        object_handles = ffi.new('int[2]')
+        result = lib.simGetContactInfo(sim_handle_all, contact_obj_handle, index + ext, object_handles,
+                                       contact)
+        contact_info = {
+            "contact": list(contact),
+            "contact_handles": list(object_handles)
+        }
+        contact_list.append(contact_info)
+        index += 1
+    contact_list.pop(-1)  # remove the all zero value
+    return contact_list
+
+
 def simGetConfigForTipPose(ikGroupHandle, jointHandles, thresholdDist, maxTimeInMs, metric, collisionPairs, jointOptions, lowLimits, ranges):
     jointCnt = len(jointHandles)
     collisionPairCnt = len(collisionPairs) // 2

pyrep/objects/object.py

@@ -330,6 +330,24 @@ def set_collidable(self, value: bool) -> None:
         """
         self._set_property(sim.sim_objectspecialproperty_collidable, value)
 
+    def get_contact(self, contact_obj=None, get_contact_normal: bool = True) -> List:
+        """Get the contact point and force with other object
+
+        :param contact_obj: The object want to check contact info with, set to None to get contact with all objects
+        :param get_contact_normal: Weather get the force and direction
+        :return: a list of all the contact info
+        """
+        contact_info = sim.simGetContactInfo(self.get_handle(), get_contact_normal)
+        if contact_obj is None:
+            return contact_info
+        else:
+            result = []
+            check_handle = contact_obj.get_handle()
+            for contact in contact_info:
+                if check_handle in contact['contact_handles']:
+                    result.append(contact)
+            return result
+
     def is_measurable(self) -> bool:
         """Whether the object is measurable or not.
 

tests/test_objects.py

@@ -143,6 +143,18 @@ def test_get_set_collidable(self):
         self.dynamic_cube.set_collidable(True)
         self.assertTrue(self.dynamic_cube.is_collidable())
 
+    def test_get_contact(self):
+        contact = self.dynamic_cube.get_contact(self.simple_model, get_contact_normal=True)
+        self.assertTrue(len(contact) == 0)
+        for _ in range(20):
+            self.pyrep.step()
+        c1 = Shape('colliding_cube1')
+        c0 = Shape('colliding_cube0')
+        contact = c1.get_contact(None, True)
+        self.assertTrue(len(contact) > 0)
+        contact = c0.get_contact(None, True)
+        self.assertTrue(len(contact) > 0)
+
     def test_get_set_measurable(self):
         self.dynamic_cube.set_measurable(False)
         self.assertFalse(self.dynamic_cube.is_measurable())