[FEATURE] Add support of 'contype'/'conaffinity' for primitive and mesh entities. (#1438)

Co-authored-by: Alexis DUBURCQ <alexis.duburcq@gmail.com>
Co-authored-by: YilingQiao <49262224+YilingQiao@users.noreply.github.com>

Author: 3 people

examples/collision/contype.py

@@ -0,0 +1,89 @@
+"""
+NOTE: contype and conaffinity are 32-bit integer bitmasks used for contact filtering of contact pairs.
+When the contype of one geom and the conaffinity of the other geom share a common bit set to 1, two geoms can collide.
+Plane:      contype=0xFFFF, conaffinity=0xFFFF (1111 1111 1111 1111)
+Red Cube:   contype=1, conaffinity=1 (0001) -> collide with Plane and Blue Cube
+Green Cube: contype=2, conaffinity=2 (0010) -> collide with Plane and Blue Cube
+Blue Cube:  contype=3, conaffinity=3 (0011) -> collide with Plane, Red Cube, and Green Cube
+Dragon:     contype=4, conaffinity=4 (0100) -> collide with Plane only
+"""
+
+import argparse
+
+import genesis as gs
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-v", "--vis", action="store_true", default=False)
+    args = parser.parse_args()
+
+    gs.init()
+
+    scene = gs.Scene(
+        viewer_options=gs.options.ViewerOptions(
+            camera_pos=(0.0, -2, 1.5),
+            camera_lookat=(0.0, 0.0, 0.5),
+            camera_fov=40,
+            max_FPS=200,
+        ),
+        show_viewer=args.vis,
+    )
+
+    scene.add_entity(gs.morphs.Plane())
+
+    scene.add_entity(
+        gs.morphs.Box(
+            pos=(0.025, 0, 0.5),
+            quat=(0, 0, 0, 1),
+            size=(0.1, 0.1, 0.1),
+            contype=1,
+            conaffinity=1,
+        ),
+        surface=gs.surfaces.Default(
+            color=(1.0, 0.0, 0.0, 1.0),
+        ),
+    )
+    scene.add_entity(
+        gs.morphs.Box(
+            pos=(-0.025, 0, 1.0),
+            quat=(0, 0, 0, 1),
+            size=(0.1, 0.1, 0.1),
+            contype=2,
+            conaffinity=2,
+        ),
+        surface=gs.surfaces.Default(
+            color=(0.0, 1.0, 0.0, 1.0),
+        ),
+    )
+    scene.add_entity(
+        gs.morphs.Box(
+            pos=(0.0, 0, 1.5),
+            quat=(0, 0, 0, 1),
+            size=(0.1, 0.1, 0.1),
+            contype=3,
+            conaffinity=3,
+        ),
+        surface=gs.surfaces.Default(
+            color=(0.0, 0.0, 1.0, 1.0),
+        ),
+    )
+    scene.add_entity(
+        morph=gs.morphs.Mesh(
+            file="meshes/dragon/dragon.obj",
+            scale=0.004,
+            euler=(0, 0, 90),
+            pos=(-0.1, 0.0, 1.0),
+            contype=4,
+            conaffinity=4,
+        ),
+    )
+
+    scene.build()
+
+    for i in range(1000):
+        scene.step()
+
+
+if __name__ == "__main__":
+    main()

genesis/engine/entities/rigid_entity/rigid_entity.py

@@ -17,7 +17,7 @@
 from genesis.utils import mjcf as mju
 from genesis.utils import terrain as tu
 from genesis.utils import urdf as uu
-from genesis.utils.misc import tensor_to_array, ti_field_to_torch, ALLOCATE_TENSOR_WARNING
+from genesis.utils.misc import ALLOCATE_TENSOR_WARNING, tensor_to_array, ti_field_to_torch
 from genesis.utils.path_planing import RRT, RRTConnect
 
 from ..base_entity import Entity
@@ -169,8 +169,8 @@ def _load_primitive(self, morph, surface):
         if morph.collision:
             g_infos.append(
                 dict(
-                    contype=1,
-                    conaffinity=1,
+                    contype=morph.contype,
+                    conaffinity=morph.conaffinity,
                     mesh=gs.Mesh.from_trimesh(cmesh, surface=gs.surfaces.Collision()),
                     type=geom_type,
                     data=geom_data,
@@ -240,8 +240,8 @@ def _load_mesh(self, morph, surface):
             for mesh in meshes:
                 g_infos.append(
                     dict(
-                        contype=1,
-                        conaffinity=1,
+                        contype=morph.contype,
+                        conaffinity=morph.conaffinity,
                         mesh=mesh,
                         type=gs.GEOM_TYPE.MESH,
                         sol_params=gu.default_solver_params(),

genesis/engine/solvers/rigid/collider_decomp.py

@@ -197,7 +197,7 @@ def _compute_collision_pair_validity(self):
                         continue
 
                 # contype and conaffinity
-                if links_entity_idx[i_la] == links_entity_idx[i_lb] and not (
+                if not (
                     (geoms_contype[i_ga] & geoms_conaffinity[i_gb]) or (geoms_contype[i_gb] & geoms_conaffinity[i_ga])
                 ):
                     continue