[CHANGING][MISC] Enable specialized convex-convex SDF fallback when using MPR. (#1983)

* Fix URDF loading for mesh file symlinks.
* Fix 'func_collision_clear' not being called if collision physics disabled.
* Fix fastcache support for MPR.
* Do not allocate GJK if not used.
* Enable specialized convex-convex SDF.

Author: duburcqa

.github/workflows/production.yml

@@ -78,7 +78,7 @@ jobs:
               pip install --no-input --extra-index-url https://pypi.nvidia.com/ omniverse-kit
               pip install --no-input ".[dev,render,usd]"
 
-              pytest -v -rs --backend gpu --dev --forked ./tests
+              pytest -v -ra --backend gpu --dev --forked ./tests
           EOF
       - name: Kill srun job systematically
         if: always()

doc

@@ -38,7 +38,7 @@
 # with the following control: 43M FPS
 # without the following control (arm in collision with the floor): 32M FPS
 franka.control_dofs_position(
-    torch.tile(torch.tensor([0, 0, 0, -1.0, 0, 0, 0, 0.02, 0.02], device=gs.device), (B, 1)),
+    torch.tile(torch.tensor([0, 0, 0, -1.0, 0, 1.0, 0, 0.02, 0.02], device=gs.device), (B, 1)),
 )
 
 for i in range(1000):

examples/speed_benchmark/franka.py

@@ -35,7 +35,7 @@
 
 # control all the robots
 franka.control_dofs_position(
-    torch.tile(torch.tensor([0, 0, 0, -1.0, 0, 0, 0, 0.02, 0.02], device=gs.device), (B, 1)),
+    torch.tile(torch.tensor([0, 0, 0, -1.0, 0, 1.0, 0, 0.02, 0.02], device=gs.device), (B, 1)),
 )
 
 # control only 3 environments: 1, 5, and 7. (You also need to comment out the function call above)

examples/tutorials/parallel_simulation.py

@@ -229,9 +229,9 @@ def init(
     with redirect_stdout(_ti_outputs):
         ti.init(
             arch=TI_ARCH[platform][backend],
-            # Add a (hidden) mechanism to forceable disable taichi debug mode as it is still a bit experimental
-            debug=ti_debug and backend == gs.cpu,
-            check_out_of_bound=debug,
+            # Add a (hidden) mechanism to forcible disable taichi debug mode as it is still a bit experimental
+            debug=ti_debug and backend == gs_backend.cpu,
+            check_out_of_bound=debug and backend != gs_backend.metal,
             # force_scalarize_matrix=True for speeding up kernel compilation
             # Turning off 'force_scalarize_matrix' is causing numerical instabilities ('nan') on MacOS
             force_scalarize_matrix=True,

genesis/__init__.py

@@ -62,6 +62,7 @@ def kernel_noslip(
     rigid_global_info: array_class.RigidGlobalInfo,
     static_rigid_sim_config: ti.template(),
 ):
+    EPS = rigid_global_info.EPS[None]
     _B = constraint_state.jac.shape[2]
     n_dofs = constraint_state.jac.shape[1]
 
@@ -133,7 +134,7 @@ def kernel_noslip(
                     y = 0.5 * (constraint_state.efc_force[j_efc, i_b] - constraint_state.efc_force[j_efc + 1, i_b])
                     K1 = Ac[0] + Ac[3] - Ac[1] - Ac[2]
                     K0 = mid * (Ac[0] - Ac[3]) + bc[0] - bc[1]
-                    if K1 < gs.EPS:
+                    if K1 < EPS:
                         constraint_state.efc_force[j_efc, i_b] = constraint_state.efc_force[j_efc + 1, i_b] = mid
                     else:
                         y = -K0 / K1
@@ -154,6 +155,7 @@ def kernel_noslip(
                         old_force=old_force,
                         res=res,
                         dim=2,
+                        eps=EPS,
                     )
 
                     improvement -= cost_change
@@ -241,6 +243,7 @@ def func_cost_change(
     old_force,
     res,
     dim: int,
+    eps,
 ):
     change = gs.ti_float(0.0)
     if dim == 1:
@@ -254,7 +257,7 @@ def func_cost_change(
             for j in range(dim):
                 change += 0.5 * Ac[i * dim + j] * delta[i] * delta[j]
             change += delta[i] * res[i]
-    if change > gs.EPS:
+    if change > eps:
         for i in range(dim):
             force[force_start + i, i_b] = old_force[i]
         change = 0.0

genesis/engine/solvers/rigid/collider_decomp.py

@@ -46,7 +46,7 @@ class EPA_POLY_INIT_RETURN_CODE(IntEnum):
 
 
 class GJK:
-    def __init__(self, rigid_solver):
+    def __init__(self, rigid_solver, is_active: bool = True):
         # Initialize static configuration.
         # MuJoCo's multi-contact detection algorithm is disabled by default, because it is often less stable than the
         # other multi-contact detection algorithm. However, we keep the code here for compatibility with MuJoCo and for
@@ -112,8 +112,11 @@ def __init__(self, rigid_solver):
             diff_contact_min_penetration=gs.EPS * 100.0,
         )
 
-        # Initialize GJK state.
-        self._gjk_state = array_class.get_gjk_state(rigid_solver, rigid_solver._static_rigid_sim_config, self._gjk_info)
+        # Initialize GJK state
+        self._gjk_state = array_class.get_gjk_state(
+            rigid_solver, rigid_solver._static_rigid_sim_config, self._gjk_info, is_active
+        )
+        self._is_active = is_active
 
     def reset(self):
         pass

genesis/engine/solvers/rigid/constraint_noslip.py

@@ -621,6 +621,7 @@ def guess_geoms_center(
     geoms_state: array_class.GeomsState,
     geoms_info: array_class.GeomsInfo,
     geoms_init_AABB: array_class.GeomsInitAABB,
+    rigid_global_info: array_class.RigidGlobalInfo,
     static_rigid_sim_config: ti.template(),
     mpr_info: array_class.MPRInfo,
     i_ga,
@@ -654,6 +655,7 @@ def guess_geoms_center(
     # is a real issue, one way to address it is to evaluate the exact signed distance of each center wrt their
     # respective geometry. If one of the center is off, its offset from the original center is divided by 2 and the
     # signed distance is computed once again until to find a valid point. This procedure should be cheap.
+    EPS = rigid_global_info.EPS[None]
 
     g_pos_a = geoms_state.pos[i_ga, i_b]
     g_pos_b = geoms_state.pos[i_gb, i_b]
@@ -681,7 +683,7 @@ def guess_geoms_center(
                 offset = delta.dot(normal_ws) * normal_ws - delta
                 offset_norm = offset.norm()
 
-                if offset_norm > gs.EPS:
+                if offset_norm > EPS:
                     # Compute the size of the bounding boxes along the target offset direction.
                     # First, move the direction in local box frame
                     dir_offset = offset / offset_norm
@@ -779,6 +781,7 @@ def func_mpr_contact(
     geoms_state: array_class.GeomsState,
     geoms_info: array_class.GeomsInfo,
     geoms_init_AABB: array_class.GeomsInitAABB,
+    rigid_global_info: array_class.RigidGlobalInfo,
     static_rigid_sim_config: ti.template(),
     collider_state: array_class.ColliderState,
     collider_info: array_class.ColliderInfo,
@@ -795,6 +798,7 @@ def func_mpr_contact(
         geoms_state,
         geoms_info,
         geoms_init_AABB,
+        rigid_global_info,
         static_rigid_sim_config,
         mpr_info,
         i_ga,