From 2771d365d3481da96a0cb9b3087895219303334e Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 24 May 2026 00:44:18 +0200 Subject: [PATCH 1/2] Add robust nonconvex collision detection for large overlap. --- .../solvers/rigid/collider/narrowphase.py | 49 ++++++++++++++++++- tests/test_rigid_physics.py | 6 ++- 2 files changed, 51 insertions(+), 4 deletions(-) diff --git a/genesis/engine/solvers/rigid/collider/narrowphase.py b/genesis/engine/solvers/rigid/collider/narrowphase.py index e70b3cf96..34b7032ce 100644 --- a/genesis/engine/solvers/rigid/collider/narrowphase.py +++ b/genesis/engine/solvers/rigid/collider/narrowphase.py @@ -197,6 +197,45 @@ def func_add_polytope_vertex_contacts_sdf( geoms_info, rigid_global_info, collider_static_config, sdf_info, center_a_world, i_gb, gb_pos, gb_quat ) normal_center = gu.qd_normalize(grad_center, EPS) + # When two comparably-sized bodies meet "across" each other (the crossed-thin-rod regime: A's center sits within + # one A-bounding-radius of B's surface, with both bodies of similar bounding-sphere size), the grid SDF gradient + # at A's center is poorly conditioned - it lands on B's local radial direction, which is perpendicular to the + # closing motion. The geometric line from B's origin to A's center is a stronger reference there: it points + # along the relative-pose offset, which for a head-on closing pair coincides with the closing direction. The + # per-vertex grad is also locally radial and just as biased, so in this regime we use the closing direction as + # the FINAL normal rather than letting per-vertex grad override it. The size-ratio gate keeps the existing + # behavior for one-big-one-small pairs where the SDF grad at A's center is reliable and the closing-direction + # line is wrong (sphere on a large floor mesh). + rbound_b_sq = gs.qd_float(0.0) + b_center_local = geoms_info.center[i_gb] + for k in qd.static(range(8)): + delta_b = geoms_init_AABB[i_gb, k] - b_center_local + d_sq_b = delta_b.dot(delta_b) + if d_sq_b > rbound_b_sq: + rbound_b_sq = d_sq_b + rbound_b = qd.sqrt(rbound_b_sq) + use_closing_dir = qd.abs(sd_center) < rbound_a and rbound_a_sq > gs.qd_float(0.25) * rbound_b_sq + approach_depth_pair = gs.qd_float(0.0) + if use_closing_dir: + closing_dir = center_a_world - gb_pos + if closing_dir.norm() > EPS: + normal_center = gu.qd_normalize(closing_dir, EPS) + # OBB extent of A and B along the closing direction. Using the identity (R . e_i) . d = e_i . (R^T . d), + # we only need one inverse rotation per body to express closing_dir in that body's local frame; the OBB + # extent is then a 3-term weighted sum of local half-extents against the abs components of the + # local-frame direction. The geometric overlap along the closing axis is (h_a + h_b) - |distance between + # centers along that axis|. This is the depth the bodies have advanced into each other along the + # direction the constraint will push them apart, and it is much larger than the per-vertex SDF + # "distance to nearest surface" for crossed thin geoms where most A verts sit on A's outer skin one + # radial gap away from B's lateral surface. + half_ext_a = (geoms_init_AABB[i_ga, 7] - geoms_init_AABB[i_ga, 0]) * gs.qd_float(0.5) + half_ext_b = (geoms_init_AABB[i_gb, 7] - geoms_init_AABB[i_gb, 0]) * gs.qd_float(0.5) + d_local_a = gu.qd_inv_transform_by_quat(normal_center, ga_quat) + d_local_b = gu.qd_inv_transform_by_quat(normal_center, gb_quat) + h_a = half_ext_a.dot(qd.abs(d_local_a)) + h_b = half_ext_b.dot(qd.abs(d_local_b)) + center_proj = qd.abs((center_a_world - gb_pos).dot(normal_center)) + approach_depth_pair = h_a + h_b - center_proj for k in qd.static(range(n_max)): if top_iv[k] >= 0: i_v = top_iv[k] @@ -229,12 +268,18 @@ def func_add_polytope_vertex_contacts_sdf( pen_emit = synthetic_pen_max * (1.0 + pen_v / margin) elif grad_norm > 0.9: if pen_v > 0.0: - pen_emit = qd.min(pen_v, margin) + pen_emit = pen_v elif pen_v > 0.0: pen_emit = synthetic_pen_max normal_v = normal_center - if grad_v.dot(grad_center) > 0.0: + if not use_closing_dir and grad_v.dot(normal_center) > 0.0: normal_v = gu.qd_normalize(grad_v, EPS) + # In the closing-direction regime, the SDF "distance to nearest surface" measured at a vertex on A's + # outer skin is the small radial gap to B's lateral, not the much larger approach depth along the + # closing axis. Use the geometric approach depth as a floor on pen_emit so the constraint solver sees + # the actual overlap rather than just the radial gap. + if use_closing_dir and pen_v > 0.0 and approach_depth_pair > pen_emit: + pen_emit = approach_depth_pair repeated = False for j in range(n_added): idx_prev = collider_state.n_contacts[i_b] - 1 - j diff --git a/tests/test_rigid_physics.py b/tests/test_rigid_physics.py index aada3a766..3dd949642 100644 --- a/tests/test_rigid_physics.py +++ b/tests/test_rigid_physics.py @@ -3029,7 +3029,8 @@ def test_mesh_repair(convexify, show_viewer, gjk_collision): obj = scene.add_entity( gs.morphs.Mesh( file=f"{asset_path}/spoon.glb", - pos=(0.3, 0, 0.015), + pos=(0.3, 0, 0.006), + euler=(0.0, -2.5, 0.0), convexify=convexify, scale=1.0, ), @@ -3037,6 +3038,7 @@ def test_mesh_repair(convexify, show_viewer, gjk_collision): visualize_contact=True, ) scene.build() + init_pos = obj.geoms[0].get_pos() for geom in obj.geoms: assert ("decomposed" in geom.metadata) ^ (not convexify) @@ -3055,7 +3057,7 @@ def test_mesh_repair(convexify, show_viewer, gjk_collision): qvel = obj.get_dofs_velocity() assert_allclose(qvel[:3], 0, atol=tol_pos) assert_allclose(qvel[3:], 0, atol=tol_rot) - assert_allclose(obj.geoms[0].get_pos()[:2], (0.3, 0.0), atol=2e-3) + assert_allclose(obj.geoms[0].get_pos()[:2], init_pos[:2], atol=1e-3) @pytest.mark.slow # ~160s From f328190a3ab7e2e209b813f9014ba720312de754 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 24 May 2026 10:52:10 +0200 Subject: [PATCH 2/2] Fix inverted-winding components caused by watertighten dual contouring. --- .../solvers/rigid/collider/narrowphase.py | 5 +- genesis/utils/mesh.py | 4 +- genesis/utils/watertighten.py | 27 +++++++ tests/test_rigid_physics.py | 76 ++++++++++++++++--- 4 files changed, 99 insertions(+), 13 deletions(-) diff --git a/genesis/engine/solvers/rigid/collider/narrowphase.py b/genesis/engine/solvers/rigid/collider/narrowphase.py index 34b7032ce..b2151a54b 100644 --- a/genesis/engine/solvers/rigid/collider/narrowphase.py +++ b/genesis/engine/solvers/rigid/collider/narrowphase.py @@ -214,10 +214,11 @@ def func_add_polytope_vertex_contacts_sdf( if d_sq_b > rbound_b_sq: rbound_b_sq = d_sq_b rbound_b = qd.sqrt(rbound_b_sq) + center_b_world = gu.qd_transform_by_trans_quat(b_center_local, gb_pos, gb_quat) use_closing_dir = qd.abs(sd_center) < rbound_a and rbound_a_sq > gs.qd_float(0.25) * rbound_b_sq approach_depth_pair = gs.qd_float(0.0) if use_closing_dir: - closing_dir = center_a_world - gb_pos + closing_dir = center_a_world - center_b_world if closing_dir.norm() > EPS: normal_center = gu.qd_normalize(closing_dir, EPS) # OBB extent of A and B along the closing direction. Using the identity (R . e_i) . d = e_i . (R^T . d), @@ -234,7 +235,7 @@ def func_add_polytope_vertex_contacts_sdf( d_local_b = gu.qd_inv_transform_by_quat(normal_center, gb_quat) h_a = half_ext_a.dot(qd.abs(d_local_a)) h_b = half_ext_b.dot(qd.abs(d_local_b)) - center_proj = qd.abs((center_a_world - gb_pos).dot(normal_center)) + center_proj = qd.abs((center_a_world - center_b_world).dot(normal_center)) approach_depth_pair = h_a + h_b - center_proj for k in qd.static(range(n_max)): if top_iv[k] >= 0: diff --git a/genesis/utils/mesh.py b/genesis/utils/mesh.py index b3eebd499..5c392b27f 100644 --- a/genesis/utils/mesh.py +++ b/genesis/utils/mesh.py @@ -39,6 +39,8 @@ ) DEFAULT_PLANE_TEXTURE_PATH = "textures/checker.png" # use checkerboard texture by default +WT_CACHE_VERSION = 2 + def discretize_array_for_hashing(arr: np.ndarray) -> np.ndarray: return np.round(arr / CVX_PATH_QUANTIZE_FACTOR).astype(np.int64) @@ -156,7 +158,7 @@ def get_remesh_path(verts, faces, edge_len_abs, edge_len_ratio, fix): def get_wt_path(verts, faces, aggressiveness): - hashkey = get_hashkey(verts, faces, aggressiveness) + hashkey = get_hashkey(verts, faces, aggressiveness, WT_CACHE_VERSION) return os.path.join(get_wt_cache_dir(), f"{hashkey}.wt") diff --git a/genesis/utils/watertighten.py b/genesis/utils/watertighten.py index 3a71ca4bc..e3a0d7164 100644 --- a/genesis/utils/watertighten.py +++ b/genesis/utils/watertighten.py @@ -980,6 +980,32 @@ def _adaptive_params(verts: np.ndarray, faces: np.ndarray, aggressiveness: int): return alpha, pitch, max_cost +def _keep_outer_shell(verts: np.ndarray, faces: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """Drop inverted-winding components produced by dual contouring around internal cavities of the source mesh. + + When the source has hollow regions, dual contouring on the unsigned distance field extracts one outer shell around + the full source plus one separate inner shell per cavity. The inner shells inherit the outward-from-source gradient + and end up with INWARD normals relative to their own enclosed volume, so their signed volume is negative. The full + mesh is "watertight" only in the edge-manifoldness sense, but its divergence-theorem integrals (volume, center of + mass, inertia) silently cancel outer against inner and produce garbage. Keep only the positive-signed-volume + components so the wrap really is a single closed envelope of the source. + """ + if faces.shape[0] == 0: + return verts, faces + mesh = trimesh.Trimesh(vertices=verts, faces=faces, process=False) + components = mesh.split(only_watertight=False) + if len(components) <= 1: + return verts, faces + keep = [c for c in components if c.volume > 0.0] + if not keep or len(keep) == len(components): + return verts, faces + combined = trimesh.util.concatenate(keep) + return ( + np.ascontiguousarray(combined.vertices, dtype=np.float64), + np.ascontiguousarray(combined.faces, dtype=np.int32), + ) + + def watertighten_mesh( verts: np.ndarray, faces: np.ndarray, @@ -1018,6 +1044,7 @@ def watertighten_mesh( field = gaussian_blur_3d(field, sigma) grad = _sdf_gradient(field, pitch) v, f = _dc_extract(field, grad, alpha, pitch, origin) + v, f = _keep_outer_shell(v, f) # One Newton step on the unsigned distance: snap each wrap vertex to the analytical `alpha`-isosurface of the source # mesh. Erases SDF-grid discretisation noise without changing topology - flat source regions stay flat in the wrap. _, _, closest = igl.point_mesh_squared_distance(v, verts, faces) diff --git a/tests/test_rigid_physics.py b/tests/test_rigid_physics.py index 3dd949642..701439047 100644 --- a/tests/test_rigid_physics.py +++ b/tests/test_rigid_physics.py @@ -3001,10 +3001,57 @@ def test_nonconvex_tunneling(show_viewer): assert_allclose(rod.get_dofs_velocity(dofs_idx_local=slice(None, 3)), 0, atol=0.05) +# Force CPU because nonconvex SDF is slow on GPU +@pytest.mark.parametrize("backend", [gs.cpu]) +def test_nonconvex_overlap(show_viewer): + scene = gs.Scene( + sim_options=gs.options.SimOptions( + dt=0.001, + gravity=(0, 0, 0), + ), + viewer_options=gs.options.ViewerOptions( + camera_pos=(0.0, 0.3, 0.15), + camera_lookat=(0.0, 0.0, 0.0), + ), + show_viewer=show_viewer, + show_FPS=False, + ) + a = scene.add_entity( + gs.morphs.Mesh( + file="meshes/stirrer.obj", + pos=(-0.051, 0.0, 0.0), + convexify=False, + ), + vis_mode="collision", + ) + b = scene.add_entity( + gs.morphs.Mesh( + file="meshes/stirrer.obj", + pos=(+0.05, 0.0, 0.0), + euler=(0, 0, 90), + convexify=False, + ), + vis_mode="collision", + visualize_contact=True, + ) + scene.build() + a.set_dofs_velocity(+1.0, dofs_idx_local=0) + b.set_dofs_velocity(-1.0, dofs_idx_local=0) + + total_energy_history = [] + for step in range(200): + total_energy = tensor_to_array(a.get_total_energy() + b.get_total_energy()) + total_energy_history.append(total_energy) + scene.step() + + # FIXME: The total energy should be not strictly decreasing but is not... relaxing the condition + # assert (np.diff(total_energy_history, axis=0) < 0.0) + assert total_energy_history[0] > 3.0 * total_energy_history[-1] + + @pytest.mark.required @pytest.mark.parametrize("convexify", [True, False]) @pytest.mark.parametrize("gjk_collision", [True, False]) -@pytest.mark.parametrize("backend", [gs.cpu]) def test_mesh_repair(convexify, show_viewer, gjk_collision): scene = gs.Scene( sim_options=gs.options.SimOptions( @@ -3013,6 +3060,10 @@ def test_mesh_repair(convexify, show_viewer, gjk_collision): rigid_options=gs.options.RigidOptions( use_gjk_collision=gjk_collision, ), + viewer_options=gs.options.ViewerOptions( + camera_pos=(0.3, 0.4, 0.01), + camera_lookat=(0.3, 0.0, 0.0), + ), show_viewer=show_viewer, show_FPS=False, ) @@ -3029,7 +3080,7 @@ def test_mesh_repair(convexify, show_viewer, gjk_collision): obj = scene.add_entity( gs.morphs.Mesh( file=f"{asset_path}/spoon.glb", - pos=(0.3, 0, 0.006), + pos=(0.3, 0, 0.007), euler=(0.0, -2.5, 0.0), convexify=convexify, scale=1.0, @@ -3038,7 +3089,11 @@ def test_mesh_repair(convexify, show_viewer, gjk_collision): visualize_contact=True, ) scene.build() - init_pos = obj.geoms[0].get_pos() + + if show_viewer: + obj_com = obj.get_links_pos(ref="link_com")[0] + debug_sphere = scene.draw_debug_sphere(pos=obj_com, radius=0.003, color=(1, 1, 1, 1)) + scene.visualizer.update(force=True) for geom in obj.geoms: assert ("decomposed" in geom.metadata) ^ (not convexify) @@ -3049,14 +3104,15 @@ def test_mesh_repair(convexify, show_viewer, gjk_collision): # MPR collision detection is less reliable than SDF and GJK in terms of penetration depth estimation is_mpr = convexify and not gjk_collision - tol_pos = 0.05 if is_mpr else 0.01 - tol_rot = 1.25 if is_mpr else 0.4 - for i in range(450): + tol_pos = 0.05 if is_mpr else 0.005 + tol_rot = 1.25 if is_mpr else 0.1 + init_pos = obj.geoms[0].get_pos() + for i in range(20): scene.step() - if i > 350: - qvel = obj.get_dofs_velocity() - assert_allclose(qvel[:3], 0, atol=tol_pos) - assert_allclose(qvel[3:], 0, atol=tol_rot) + for i in range(100): + qvel = obj.get_dofs_velocity() + assert_allclose(qvel[:3], 0, atol=tol_pos) + assert_allclose(qvel[3:], 0, atol=tol_rot) assert_allclose(obj.geoms[0].get_pos()[:2], init_pos[:2], atol=1e-3)