From 2843042744d2d4d5f6e6efe505d800e31282f143 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 29 Jun 2025 21:19:50 +0200 Subject: [PATCH 1/8] Disable timers by default. --- .../engine/solvers/rigid/collider_decomp.py | 14 +++++------ .../solvers/rigid/constraint_solver_decomp.py | 12 +++++----- .../solvers/rigid/rigid_solver_decomp.py | 23 ++++++++----------- genesis/utils/bvh.py | 18 +++++---------- genesis/utils/tools.py | 2 +- 5 files changed, 30 insertions(+), 39 deletions(-) diff --git a/genesis/engine/solvers/rigid/collider_decomp.py b/genesis/engine/solvers/rigid/collider_decomp.py index 66584960ee..9348474d5f 100644 --- a/genesis/engine/solvers/rigid/collider_decomp.py +++ b/genesis/engine/solvers/rigid/collider_decomp.py @@ -299,23 +299,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): diff --git a/genesis/engine/solvers/rigid/constraint_solver_decomp.py b/genesis/engine/solvers/rigid/constraint_solver_decomp.py index cc6102c535..905709c7b7 100644 --- a/genesis/engine/solvers/rigid/constraint_solver_decomp.py +++ b/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): diff --git a/genesis/engine/solvers/rigid/rigid_solver_decomp.py b/genesis/engine/solvers/rigid/rigid_solver_decomp.py index 209c817724..d3797e4ba2 100644 --- a/genesis/engine/solvers/rigid/rigid_solver_decomp.py +++ b/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): diff --git a/genesis/utils/bvh.py b/genesis/utils/bvh.py index 398745beb4..999e178f36 100644 --- a/genesis/utils/bvh.py +++ b/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 diff --git a/genesis/utils/tools.py b/genesis/utils/tools.py index 1e0945b4f3..fe2d5b56ce 100644 --- a/genesis/utils/tools.py +++ b/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() From 3a048bd7eb61d1005ff400ddc28d2f1167a6d255 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Fri, 27 Jun 2025 16:21:28 +0200 Subject: [PATCH 2/8] Fix compiler warnings for excessive inlining. --- genesis/engine/materials/FEM/elastic.py | 2 +- genesis/engine/solvers/fem_solver.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/genesis/engine/materials/FEM/elastic.py b/genesis/engine/materials/FEM/elastic.py index b6adbf485b..dc8428d598 100644 --- a/genesis/engine/materials/FEM/elastic.py +++ b/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 diff --git a/genesis/engine/solvers/fem_solver.py b/genesis/engine/solvers/fem_solver.py index c8cc5976e2..6a861ab9eb 100644 --- a/genesis/engine/solvers/fem_solver.py +++ b/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] ) From d9a78cc80ceceab167908de73901d4dff8903651 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Fri, 27 Jun 2025 16:21:56 +0200 Subject: [PATCH 3/8] Fix running benchmarks sequentially. --- tests/test_rigid_benchmarks.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_rigid_benchmarks.py b/tests/test_rigid_benchmarks.py index 60aa85fc90..4bf98136a0 100644 --- a/tests/test_rigid_benchmarks.py +++ b/tests/test_rigid_benchmarks.py @@ -231,7 +231,7 @@ def stream_writers(backend, printer_session): path.unlink() # Create new empty worker-specific report - report_name = "-".join((report_path.stem, worker_id)) + report_name = "-".join(filter(None, (report_path.stem, worker_id))) report_path = report_path.with_name(f"{report_name}.txt") if report_path.exists(): report_path.unlink() From 5876a307265bebee99361901dde8e4f3876d60e9 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Mon, 30 Jun 2025 08:47:41 +0200 Subject: [PATCH 4/8] Fix benchmark master worker id shadowing. --- tests/test_rigid_benchmarks.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_rigid_benchmarks.py b/tests/test_rigid_benchmarks.py index 4bf98136a0..76906b84b7 100644 --- a/tests/test_rigid_benchmarks.py +++ b/tests/test_rigid_benchmarks.py @@ -225,8 +225,8 @@ def stream_writers(backend, printer_session): worker_count = int(os.environ["PYTEST_XDIST_WORKER_COUNT"]) for path in report_path.parent.glob("-".join((report_path.stem, "*.txt"))): - _, worker_id = path.stem.rsplit("-", 1) - worker_num = int(worker_id[2:]) + _, worker_id_ = path.stem.rsplit("-", 1) + worker_num = int(worker_id_[2:]) if worker_num >= worker_count: path.unlink() From b305066918f98192673494ccb7903543121077e1 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Mon, 30 Jun 2025 09:27:58 +0200 Subject: [PATCH 5/8] Fix contribution guidelines. --- .github/CONTRIBUTING.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/CONTRIBUTING.md b/.github/CONTRIBUTING.md index 6ede5d6c1a..a3bb430adf 100644 --- a/.github/CONTRIBUTING.md +++ b/.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. From 62fd868dc344cb8dcf3b265e063b022ba910e736 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 29 Jun 2025 20:43:27 +0200 Subject: [PATCH 6/8] Re-enable warning for exceeding max collision pairs. --- .../engine/solvers/rigid/collider_decomp.py | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/genesis/engine/solvers/rigid/collider_decomp.py b/genesis/engine/solvers/rigid/collider_decomp.py index 9348474d5f..4dd9f30638 100644 --- a/genesis/engine/solvers/rigid/collider_decomp.py +++ b/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) @@ -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] From 7a38d8e56d912dba1bef60ac7d0e13a7450e31ee Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 29 Jun 2025 20:41:49 +0200 Subject: [PATCH 7/8] Offset geometry centers for MPR only if necessary. --- genesis/engine/solvers/rigid/mpr_decomp.py | 6 ++++-- tests/test_rigid_physics.py | 5 +++++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/genesis/engine/solvers/rigid/mpr_decomp.py b/genesis/engine/solvers/rigid/mpr_decomp.py index a289f49ef8..2716482146 100644 --- a/genesis/engine/solvers/rigid/mpr_decomp.py +++ b/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() diff --git a/tests/test_rigid_physics.py b/tests/test_rigid_physics.py index dbe02801ee..855ca61586 100644 --- a/tests/test_rigid_physics.py +++ b/tests/test_rigid_physics.py @@ -2082,6 +2082,11 @@ def test_urdf_parsing(show_viewer, tol): DOOR_JOINT_DAMPING = 1.5 scene = gs.Scene( + rigid_options=gs.options.RigidOptions( + # Must use GJK to make collision detection independent from the center of each geometry. + # Note that it is also the case for MPR+SDF most of the time due to warm-start. + use_gjk_collision=True, + ), show_viewer=show_viewer, show_FPS=False, ) From 841c821e1944baa36ecced57d52b0c378e352def Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 29 Jun 2025 20:28:25 +0200 Subject: [PATCH 8/8] Fix MPR infinite loop edge case. --- genesis/engine/simulator.py | 1 - genesis/engine/solvers/rigid/collider_decomp.py | 12 ++++++------ genesis/engine/solvers/rigid/mpr_decomp.py | 8 ++++++++ 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/genesis/engine/simulator.py b/genesis/engine/simulator.py index a8003b8aad..1982678512 100644 --- a/genesis/engine/simulator.py +++ b/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 diff --git a/genesis/engine/solvers/rigid/collider_decomp.py b/genesis/engine/solvers/rigid/collider_decomp.py index 4dd9f30638..66d8c995bc 100644 --- a/genesis/engine/solvers/rigid/collider_decomp.py +++ b/genesis/engine/solvers/rigid/collider_decomp.py @@ -1296,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. @@ -1310,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: diff --git a/genesis/engine/solvers/rigid/mpr_decomp.py b/genesis/engine/solvers/rigid/mpr_decomp.py index 2716482146..66b2f4361f 100644 --- a/genesis/engine/solvers/rigid/mpr_decomp.py +++ b/genesis/engine/solvers/rigid/mpr_decomp.py @@ -478,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) @@ -509,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