Fix MPR infinite loop edge case.

Author: duburcqa

genesis/engine/simulator.py

@@ -183,7 +183,6 @@ def _add_force_field(self, force_field):
             solver._add_force_field(force_field)
 
     def build(self):
-
         self.n_envs = self.scene.n_envs
         self._B = self.scene._B
         self._para_level = self.scene._para_level

genesis/engine/solvers/rigid/collider_decomp.py

@@ -1298,6 +1298,7 @@ def _func_convex_convex_contact(self, i_ga, i_gb, i_b):
                                 # Because of this, it is necessary to run it twice and take the contact information
                                 # associated with the point of deepest penetration.
                                 try_sdf = True
+
                         ### GJK
                         elif ti.static(self.ccd_algorithm == CCD_ALGORITHM_CODE.GJK):
                             # If it was not the first detection, only detect single contact point.
@@ -1312,12 +1313,11 @@ def _func_convex_convex_contact(self, i_ga, i_gb, i_b):
                                     # Used MuJoCo's multi-contact algorithm to find multiple contact points. Therefore,
                                     # add the discovered contact points and stop multi-contact search.
                                     for i_c in range(n_contacts):
-                                        if i_c >= self._n_contacts_per_pair:
-                                            # Ignore contact points if the number of contacts exceeds the limit.
-                                            break
-                                        contact_pos = self._gjk.contact_pos[i_b, i_c]
-                                        normal = self._gjk.normal[i_b, i_c]
-                                        self._func_add_contact(i_ga, i_gb, normal, contact_pos, penetration, i_b)
+                                        # Ignore contact points if the number of contacts exceeds the limit.
+                                        if i_c < self._n_contacts_per_pair:
+                                            contact_pos = self._gjk.contact_pos[i_b, i_c]
+                                            normal = self._gjk.normal[i_b, i_c]
+                                            self._func_add_contact(i_ga, i_gb, normal, contact_pos, penetration, i_b)
 
                                     break
                                 else:

genesis/engine/solvers/rigid/mpr_decomp.py

@@ -476,6 +476,10 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b, normal_ws):
                         self.mpr_swap(1, 2, i_ga, i_gb, i_b)
                         direction = -direction
 
+                    # FIXME: This algorithm may get stuck in an infinite loop if the actually penetration is smaller
+                    # then `CCD_EPS` and at least one of the center of each geometry is outside their convex hull.
+                    # Since this deadlock happens very rarely, a simple fix is to abord computation after a few trials.
+                    num_trials = gs.ti_int(0)
                     while self.simplex_size[i_b] < 4:
                         v, v1, v2 = self.compute_support(direction, i_ga, i_gb, i_b)
                         dot = v.dot(direction)
@@ -507,6 +511,10 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b, normal_ws):
                             vb = self.simplex_support[2, i_b].v - self.simplex_support[0, i_b].v
                             direction = va.cross(vb)
                             direction = direction.normalized()
+                            num_trials = num_trials + 1
+                            if num_trials == 15:
+                                ret = -1
+                                break
                         else:
                             self.simplex_support[3, i_b].v1 = v1
                             self.simplex_support[3, i_b].v2 = v2