[BUG FIX] Fix MPR warm-start infinite loop edge case. (Genesis-Embodied-AI#1336)

* Disable timers by default.
* Fix compiler warnings for excessive inlining.
* Fix running benchmarks sequentially.
* Fix benchmark master worker id shadowing.
* Fix contribution guidelines.
* Re-enable warning for exceeding max collision pairs.
* Offset geometry centers for MPR only if necessary.
* Fix MPR warm-start infinite loop edge case.

Author: duburcqa

.github/CONTRIBUTING.md

@@ -61,7 +61,7 @@ Thank you for your interest in contributing to Genesis! We welcome contributions
 - (Optional) You can run CI tests locally to ensure you pass the online CI checks.
 
   ```python
-  python -m unittest discover tests
+  pytest -v --forked -m required ./tests
   ```
 
 - In the title of your Pull Request, please include [BUG FIX], [FEATURE] or [MISC] to indicate the purpose.

genesis/engine/materials/FEM/elastic.py

@@ -432,7 +432,7 @@ def compute_energy_gradient_hessian_linear_corotated(self, mu, lam, J, F, actu,
         for i, k in ti.static(ti.ndrange(3, 3)):
             hessian_field[i_b, i, i, i_e][k, k] = mu
 
-        for i, j, alpha, beta in ti.static(ti.ndrange(3, 3, 3, 3)):
+        for i, j, alpha, beta in ti.ndrange(3, 3, 3, 3):
             hessian_field[i_b, j, beta, i_e][i, alpha] += mu * R[i, beta] * R[alpha, j] + lam * R[alpha, beta] * R[i, j]
 
         return energy, gradient

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/fem_solver.py

@@ -617,7 +617,7 @@ def accumulate_vertex_force_preconditioner(self, f: ti.i32):
                     )
 
             # diagonal 3-by-3 block of hessian
-            for k, i, j in ti.static(ti.ndrange(4, 3, 3)):
+            for k, i, j in ti.ndrange(4, 3, 3):
                 self.pcg_state_v[i_b, i_v[k]].diag3x3 += (
                     V * damping_beta_factor * S[k, i] * S[k, j] * self.elements_el_hessian[i_b, i, j, i_e]
                 )

genesis/engine/solvers/rigid/collider_decomp.py

@@ -153,6 +153,12 @@ def _init_collision_fields(self) -> None:
         self._max_collision_pairs = min(n_possible_pairs, self._solver._max_collision_pairs)
         self._max_contact_pairs = self._max_collision_pairs * self._n_contacts_per_pair
 
+        self._warn_msg_max_collision_pairs = (
+            f"{colors.YELLOW}[Genesis] [00:00:00] [WARNING] Ignoring contact pair to avoid exceeding max "
+            f"({self._max_contact_pairs}). Please increase the value of RigidSolver's option "
+            f"'max_collision_pairs'.{formats.RESET}"
+        )
+
         ############## broad phase SAP ##############
         # This buffer stores the AABBs along the search axis of all geoms
         struct_sort_buffer = ti.types.struct(value=gs.ti_float, i_g=gs.ti_int, is_max=gs.ti_int)
@@ -299,23 +305,23 @@ def _kernel_clear(
                 self.n_contacts[i_b] = 0
 
     def detection(self) -> None:
-        from genesis.utils.tools import create_timer
+        # from genesis.utils.tools import create_timer
 
         self._contacts_info_cache = {}
-        timer = create_timer(name="69477ab0-5e75-47cb-a4a5-d4eebd9336ca", level=3, ti_sync=True, skip_first_call=True)
+        # timer = create_timer(name="69477ab0-5e75-47cb-a4a5-d4eebd9336ca", level=3, ti_sync=True, skip_first_call=True)
         self._func_update_aabbs()
-        timer.stamp("func_update_aabbs")
+        # timer.stamp("func_update_aabbs")
         self._func_broad_phase()
-        timer.stamp("func_broad_phase")
+        # timer.stamp("func_broad_phase")
         self._func_narrow_phase_convex_vs_convex()
         self._func_narrow_phase_convex_specializations()
-        timer.stamp("func_narrow_phase")
+        # timer.stamp("func_narrow_phase")
         if self._has_terrain:
             self._func_narrow_phase_any_vs_terrain()
-            timer.stamp("_func_narrow_phase_any_vs_terrain")
+            # timer.stamp("_func_narrow_phase_any_vs_terrain")
         if self._has_nonconvex_nonterrain:
             self._func_narrow_phase_nonconvex_vs_nonterrain()
-            timer.stamp("_func_narrow_phase_nonconvex_vs_nonterrain")
+            # timer.stamp("_func_narrow_phase_nonconvex_vs_nonterrain")
 
     @ti.func
     def _func_point_in_geom_aabb(self, point, i_g, i_b):
@@ -770,11 +776,7 @@ def _func_broad_phase(self):
                                 continue
 
                             if self.n_broad_pairs[i_b] == self._max_collision_pairs:
-                                # print(
-                                #     f"{colors.YELLOW}[Genesis] [00:00:00] [WARNING] Ignoring collision pair to avoid "
-                                #     f"exceeding max ({self._max_collision_pairs}). Please increase the value of "
-                                #     f"RigidSolver's option 'max_collision_pairs'.{formats.RESET}"
-                                # )
+                                ti.static_print(self._warn_msg_max_collision_pairs)
                                 break
                             self.broad_collision_pairs[self.n_broad_pairs[i_b], i_b][0] = i_ga
                             self.broad_collision_pairs[self.n_broad_pairs[i_b], i_b][1] = i_gb
@@ -1107,12 +1109,7 @@ def _func_add_contact(self, i_ga, i_gb, normal, contact_pos, penetration, i_b):
         i_col = self.n_contacts[i_b]
 
         if i_col == self._max_contact_pairs:
-            # print(
-            #     f"{colors.YELLOW}[Genesis] [00:00:00] [WARNING] Ignoring contact pair to avoid exceeding max "
-            #     f"({self._max_contact_pairs}). Please increase the value of RigidSolver's option "
-            #     f"'max_collision_pairs'.{formats.RESET}"
-            # )
-            pass
+            ti.static_print(self._warn_msg_max_collision_pairs)
         else:
             ga_info = self._solver.geoms_info[i_ga]
             gb_info = self._solver.geoms_info[i_gb]
@@ -1299,6 +1296,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.
@@ -1313,12 +1311,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/constraint_solver_decomp.py

@@ -750,17 +750,17 @@ def handle_constraints(self):
             self.resolve()
 
     def resolve(self):
-        from genesis.utils.tools import create_timer
+        # from genesis.utils.tools import create_timer
 
-        timer = create_timer(name="resolve", level=3, ti_sync=True, skip_first_call=True)
+        # timer = create_timer(name="resolve", level=3, ti_sync=True, skip_first_call=True)
         self._func_init_solver()
-        timer.stamp("_func_init_solver")
+        # timer.stamp("_func_init_solver")
         self._func_solve()
-        timer.stamp("_func_solve")
+        # timer.stamp("_func_solve")
         self._func_update_qacc()
-        timer.stamp("_func_update_qacc")
+        # timer.stamp("_func_update_qacc")
         self._func_update_contact_force()
-        timer.stamp("compute force")
+        # timer.stamp("compute force")
 
     @ti.kernel
     def _func_update_contact_force(self):

genesis/engine/solvers/rigid/mpr_decomp.py

@@ -402,9 +402,11 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b, normal_ws):
                 center_b = gu.ti_transform_by_trans_quat(center_b_local, g_state_b.pos, g_state_b.quat)
                 delta = center_a - center_b
 
-                # Skip offset if normal is almost colinear already
+                # Skip offset if normal is roughly pointing in the same direction already.
+                # Note that a threshold of 0.5 would probably make more sense, but this means that the center of each
+                # geometry would significantly affect collision detection, which is undesirable.
                 normal = delta.normalized()
-                if normal_ws.cross(normal).norm() > self.CCD_TOLERANCE:
+                if normal_ws.cross(normal).norm() > 0.01:
                     # Compute the target offset
                     offset = delta.dot(normal_ws) * normal_ws - delta
                     offset_norm = offset.norm()
@@ -476,6 +478,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 +513,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

genesis/engine/solvers/rigid/rigid_solver_decomp.py

@@ -1705,18 +1705,15 @@ def _kernel_clear_external_force(self):
         self._func_clear_external_force()
 
     def substep(self):
-        from genesis.utils.tools import create_timer
+        # from genesis.utils.tools import create_timer
 
-        timer = create_timer("rigid", level=1, ti_sync=True, skip_first_call=True)
+        # timer = create_timer("rigid", level=1, ti_sync=True, skip_first_call=True)
         self._kernel_step_1()
-        timer.stamp("kernel_step_1")
-
-        # constraint force
+        # timer.stamp("kernel_step_1")
         self._func_constraint_force()
-        timer.stamp("constraint_force")
-
+        # timer.stamp("constraint_force")
         self._kernel_step_2()
-        timer.stamp("kernel_step_2")
+        # timer.stamp("kernel_step_2")
 
     @ti.kernel
     def _kernel_step_1(self):
@@ -1800,20 +1797,20 @@ def _kernel_forward_kinematics_links_geoms(self, envs_idx: ti.types.ndarray()):
             self._func_update_geoms(i_b)
 
     def _func_constraint_force(self):
-        from genesis.utils.tools import create_timer
+        # from genesis.utils.tools import create_timer
 
-        timer = create_timer(name="constraint_force", level=2, ti_sync=True, skip_first_call=True)
+        # timer = create_timer(name="constraint_force", level=2, ti_sync=True, skip_first_call=True)
         if self._enable_collision or self._enable_joint_limit or self.n_equalities > 0:
             self._func_constraint_clear()
-            timer.stamp("constraint_solver.clear")
+            # timer.stamp("constraint_solver.clear")
 
         if self._enable_collision:
             self.collider.detection()
-            timer.stamp("detection")
+            # timer.stamp("detection")
 
         if not self._disable_constraint:
             self.constraint_solver.handle_constraints()
-        timer.stamp("constraint_solver.handle_constraints")
+        # timer.stamp("constraint_solver.handle_constraints")
 
     @ti.kernel
     def _func_constraint_clear(self):

genesis/utils/bvh.py

@@ -693,25 +693,19 @@ def cd_aggregate(self):
                         self.contact_aggregate[ga, gb].penetration = penetration
 
     def collision_detection(self):
-        from genesis.utils.tools import create_timer
+        # from genesis.utils.tools import create_timer
 
-        timer = create_timer(name="solve_quadratic", level=4, ti_sync=True, skip_first_call=True)
+        # timer = create_timer(name="solve_quadratic", level=4, ti_sync=True, skip_first_call=True)
         self.cd_init()
-        timer.stamp("cd_init")
+        # timer.stamp("cd_init")
         self.refit_func()
-        timer.stamp("refit_func")
+        # timer.stamp("refit_func")
         self.cd_tree_phase()  # 18 ms
-        timer.stamp("cd_tree_phase")
+        # timer.stamp("cd_tree_phase")
         self.cd_aggregate()
-        timer.stamp("cd_aggregate")
+        # timer.stamp("cd_aggregate")
         # print("overlapped_face, overlapped_node", self.cd_counter[0], self.cd_counter[1])
 
-        # exit()
-        # self.cd_candidate_phase() # not too much
-        # print("self.cd_counter 2", self.cd_counter)
-        # self.cd_impact_phase() # 1 ms
-        # print("self.cd_counter 3", self.cd_counter)
-
     @ti.kernel
     def compute_constraint_system(self, n_con: int, n_dof: int):
         # TODO TEST

genesis/utils/tools.py

@@ -65,7 +65,7 @@ def reset(self):
         self.just_reset = True
         if self.level == 0 and not self.skip:
             print("─" * os.get_terminal_size()[0])
-        if self.ti_sync:
+        if self.ti_sync and not self.skip:
             ti.sync()
         self.prev_time = self.init_time = time.perf_counter()