[FEATURE] More robust MPR+SDF collision detection algorithm. (#1985)

* Fix errno error message.
* Further reduce max number of collision pairs by default.
* Improve runtime speed by optimizing memory locality.
* Cleanup collision examples.
* More robust MPR+SDF collision detection algorithm.

Author: duburcqa

examples/collision/pyramid.py

@@ -5,57 +5,42 @@
 
 def main():
     parser = argparse.ArgumentParser()
-    parser.add_argument("--pile_type", type=str, default="falling", choices=["static", "falling"])
-    parser.add_argument("--num_cubes", type=int, default=5, choices=range(5, 11))
-    parser.add_argument("--cpu", action="store_true", help="Use CPU backend instead of GPU")
-    parser.add_argument("--steps", type=int, default=100)
+    parser.add_argument("--pile_type", type=str, default="falling", choices=("static", "falling"))
+    parser.add_argument("--num_cubes", type=int, default=5, choices=(5, 6, 7, 8, 9, 10))
+    parser.add_argument("--cpu", action="store_true", help="Use CPU backend instead of GPU", default=True)
+    parser.add_argument("--steps", type=int, default=150)
     parser.add_argument("-v", "--vis", action="store_true", default=False)
-
     args = parser.parse_args()
 
-    pile_type = args.pile_type
-    num_cubes = args.num_cubes
-    cpu = args.cpu
-    backend = gs.cpu if cpu else gs.gpu
-
-    gs.init(backend=backend, precision="32")
+    gs.init(backend=gs.cpu if args.cpu else gs.gpu, precision="32")
 
     scene = gs.Scene(
         sim_options=gs.options.SimOptions(
             dt=0.01,
         ),
-        rigid_options=gs.options.RigidOptions(
-            box_box_detection=False,
-            max_collision_pairs=1000,
-            use_gjk_collision=True,
-            enable_mujoco_compatibility=False,
-        ),
         viewer_options=gs.options.ViewerOptions(
             camera_pos=(0, -5.5, 2.5),
             camera_lookat=(0, 0.0, 1.5),
-            camera_fov=30,
             max_FPS=60,
         ),
         show_viewer=args.vis,
     )
 
-    plane = scene.add_entity(gs.morphs.Plane(pos=(0, 0, 0)))
+    plane = scene.add_entity(gs.morphs.Plane())
 
     # create pyramid of boxes
     box_size = 0.25
-    if pile_type == "static":
-        box_spacing = box_size
-    else:
-        box_spacing = 1.1 * box_size
-    vec_one = np.array([1.0, 1.0, 1.0])
-    box_pos_offset = (0 - 0.5, 1, 0.0) + 0.5 * box_size * vec_one
+    box_spacing = (1.0 - 1e-3 + 0.1 * (args.pile_type == "static")) * box_size
+    box_pos_offset = (0 - 0.5, 1, 0.0) + 0.5 * np.array([box_size, box_size, box_size])
     boxes = {}
-    for i in range(num_cubes):
-        for j in range(num_cubes - i):
+    for i in range(args.num_cubes):
+        for j in range(args.num_cubes - i):
             box = scene.add_entity(
                 gs.morphs.Box(
-                    size=box_size * vec_one, pos=box_pos_offset + box_spacing * np.array([i + 0.5 * j, 0, j])
+                    size=[box_size, box_size, box_size],
+                    pos=box_pos_offset + box_spacing * np.array([i + 0.5 * j, 0, j]),
                 ),
+                # visualize_contact=True,
             )
 
     scene.build()

examples/collision/tower.py

@@ -5,12 +5,7 @@
 
 def main():
     parser = argparse.ArgumentParser()
-    parser.add_argument(
-        "--object",
-        type=str,
-        default="cylinder",
-        choices=["sphere", "cylinder", "duck"],
-    )
+    parser.add_argument("--object", type=str, default="cylinder", choices=("sphere", "cylinder", "duck"))
     parser.add_argument("-v", "--vis", action="store_true", default=False)
     args = parser.parse_args()
     object_type = args.object
@@ -22,46 +17,37 @@ def main():
             dt=0.005,
         ),
         rigid_options=gs.options.RigidOptions(
-            box_box_detection=False,
-            max_collision_pairs=2000,
-            use_gjk_collision=True,
-            enable_mujoco_compatibility=False,
+            max_collision_pairs=200,
         ),
         viewer_options=gs.options.ViewerOptions(
             camera_pos=(20, -20, 20),
             camera_lookat=(0.0, 0.0, 5.0),
-            camera_fov=30,
             max_FPS=60,
         ),
         show_viewer=args.vis,
     )
 
-    plane = scene.add_entity(gs.morphs.Plane(pos=(0, 0, 0)))
+    plane = scene.add_entity(gs.morphs.Plane())
 
     # create pyramid of boxes
-    box_width = 0.25
-    box_length = 2.0
-    box_height = 0.1
+    box_width, box_length, box_height = 0.25, 2.0, 0.1
     num_stacks = 50
-    height_offset = 0.0
     for i in range(num_stacks):
-        horizontal = i % 2 == 0
-
-        if horizontal:
+        if i % 2 == 0:  # horizontal stack
             box_size = np.array([box_width, box_length, box_height])
-            box_pos_0 = (-0.4 * box_length, 0, i * (height_offset + box_size[2]) + 0.5 * box_size[2])
-            box_pos_1 = (+0.4 * box_length, 0, i * (height_offset + box_size[2]) + 0.5 * box_size[2])
-        else:
+            box_pos_0 = (-0.4 * box_length, 0, i * (box_height - 1e-3) + 0.5 * box_height)
+            box_pos_1 = (+0.4 * box_length, 0, i * (box_height - 1e-3) + 0.5 * box_height)
+        else:  # vertical stack
             box_size = np.array([box_length, box_width, box_height])
-            box_pos_0 = (0, -0.4 * box_length, i * (height_offset + box_size[2]) + 0.5 * box_size[2])
-            box_pos_1 = (0, +0.4 * box_length, i * (height_offset + box_size[2]) + 0.5 * box_size[2])
-
-        scene.add_entity(
-            gs.morphs.Box(size=box_size, pos=box_pos_0),
-        )
-        scene.add_entity(
-            gs.morphs.Box(size=box_size, pos=box_pos_1),
-        )
+            box_pos_0 = (0, -0.4 * box_length, i * (box_height - 1e-3) + 0.5 * box_height)
+            box_pos_1 = (0, +0.4 * box_length, i * (box_height - 1e-3) + 0.5 * box_height)
+        for box_pos in (box_pos_0, box_pos_1):
+            scene.add_entity(
+                gs.morphs.Box(
+                    size=box_size,
+                    pos=box_pos,
+                ),
+            )
 
     # Drop a huge mesh
     if object_type == "duck":
@@ -70,14 +56,15 @@ def main():
             morph=gs.morphs.Mesh(
                 file="meshes/duck.obj",
                 scale=duck_scale,
-                pos=(0, 0, num_stacks * (height_offset + box_height) + 10 * duck_scale),
+                pos=(0, 0, num_stacks * box_height + 10 * duck_scale),
             ),
         )
     elif object_type == "sphere":
         sphere_radius = 2.0
         scene.add_entity(
             morph=gs.morphs.Sphere(
-                radius=sphere_radius, pos=(0.0, 0.0, num_stacks * (height_offset + box_height) + 5 * sphere_radius)
+                radius=sphere_radius,
+                pos=(0.0, 0.0, num_stacks * box_height + 5 * sphere_radius),
             ),
         )
     elif object_type == "cylinder":
@@ -87,12 +74,12 @@ def main():
             morph=gs.morphs.Cylinder(
                 radius=cylinder_radius,
                 height=cylinder_height,
-                pos=(0.0, 0.0, num_stacks * (height_offset + box_height) + 5 * cylinder_height),
+                pos=(0.0, 0.0, num_stacks * box_height + 5 * cylinder_height),
             ),
         )
 
     scene.build()
-    for i in range(500):
+    for i in range(600):
         scene.step()
 
 

genesis/engine/simulator.py

@@ -44,6 +44,9 @@
     from .solvers.base_solver import Solver
 
 
+RATE_CHECK_ERRNO = 10
+
+
 @ti.data_oriented
 class Simulator(RBC):
     """
@@ -287,7 +290,7 @@ def step(self, in_backward=False):
 
         if self.rigid_solver.is_active:
             self.rigid_solver.clear_external_force()
-            if self._cur_substep_global % 10 == 0:
+            if self._cur_substep_global % RATE_CHECK_ERRNO == 0:
                 self.rigid_solver.check_errno()
 
         self._sensor_manager.step()