[FEATURE] Filter out by default self-collision pairs active in neutral configuration. (#2251)

* Fix RigidEntity init.
* Filter out collision pairs that are active in neutral configuration.

Author: duburcqa

examples/speed_benchmark/franka.py

@@ -7,16 +7,16 @@
 
 ########################## create a scene ##########################
 scene = gs.Scene(
-    show_viewer=False,
+    rigid_options=gs.options.RigidOptions(
+        dt=0.01,
+    ),
     viewer_options=gs.options.ViewerOptions(
         camera_pos=(3.5, -1.0, 2.5),
         camera_lookat=(0.0, 0.0, 0.5),
         camera_fov=40,
         res=(1920, 1080),
     ),
-    rigid_options=gs.options.RigidOptions(
-        dt=0.01,
-    ),
+    show_viewer=False,
 )
 
 ########################## entities ##########################

genesis/engine/entities/rigid_entity/rigid_entity.py

@@ -596,6 +596,8 @@ def _build(self):
         self._n_qs = self.n_qs
         self._n_dofs = self.n_dofs
         self._n_geoms = self.n_geoms
+        self._geoms = self.geoms
+        self._vgeoms = self.vgeoms
         self._is_built = True
 
         verts_start = 0
@@ -614,9 +616,6 @@ def _build(self):
         self._n_free_verts = len(self._free_verts_idx_local)
         self._n_fixed_verts = len(self._fixed_verts_idx_local)
 
-        self._geoms = self.geoms
-        self._vgeoms = self.vgeoms
-
         self._init_jac_and_IK()
 
     def _init_jac_and_IK(self):
@@ -2993,6 +2992,13 @@ def get_mass(self):
     # ----------------------------------- properties -------------------------------------
     # ------------------------------------------------------------------------------------
 
+    @property
+    def is_built(self):
+        """
+        Whether this rigid entity is built.
+        """
+        return self._is_built
+
     @property
     def visualize_contact(self):
         """Whether to visualize contact force."""
@@ -3171,22 +3177,14 @@ def geoms(self) -> list[RigidGeom]:
         """The list of collision geoms (`RigidGeom`) in the entity."""
         if self.is_built:
             return self._geoms
-        else:
-            geoms = gs.List()
-            for link in self._links:
-                geoms += link.geoms
-            return geoms
+        return gs.List(geom for link in self._links for geom in link.geoms)
 
     @property
     def vgeoms(self):
         """The list of visual geoms (`RigidVisGeom`) in the entity."""
         if self.is_built:
             return self._vgeoms
-        else:
-            vgeoms = gs.List()
-            for link in self._links:
-                vgeoms += link.vgeoms
-            return vgeoms
+        return gs.List(vgeom for link in self._links for vgeom in link.vgeoms)
 
     @property
     def links(self) -> list[RigidLink]:

genesis/engine/entities/rigid_entity/rigid_equality.py

@@ -42,7 +42,7 @@ def set_sol_params(self, sol_params):
         """
         Set the solver parameters of this equality constraint.
         """
-        if self.is_built:
+        if self._solver.is_built:
             self._solver.set_sol_params(sol_params, eqs_idx=self._idx, envs_idx=None)
         else:
             self._sol_params = sol_params
@@ -52,7 +52,7 @@ def sol_params(self):
         """
         Returns the solver parameters of this equality constraint.
         """
-        if self.is_built:
+        if self._solver.is_built:
             return self._solver.get_sol_params(eqs_idx=self._idx, envs_idx=None)[..., 0, :]
         return self._sol_params
 

genesis/engine/entities/rigid_entity/rigid_geom.py

@@ -349,7 +349,7 @@ def set_friction(self, friction):
             gs.raise_exception("`friction` must be non-negative.")
         self._friction = friction
 
-        if self.is_built:
+        if self._solver.is_built:
             self._solver.set_geom_friction(friction, self._idx)
 
     # ------------------------------------------------------------------------------------
@@ -400,7 +400,7 @@ def set_sol_params(self, sol_params):
         """
         Set the solver parameters of this geometry.
         """
-        if self.is_built:
+        if self._solver.is_built:
             self._solver.set_sol_params(sol_params, geoms_idx=self._idx, envs_idx=None)
         else:
             self._sol_params = sol_params
@@ -410,7 +410,7 @@ def sol_params(self):
         """
         Get the solver parameters of this geometry.
         """
-        if self.is_built:
+        if self._solver.is_built:
             return self._solver.get_sol_params(geoms_idx=self._idx, envs_idx=None)[0]
         return self._sol_params
 

genesis/engine/entities/rigid_entity/rigid_joint.py

@@ -139,17 +139,17 @@ def set_sol_params(self, sol_params):
         """
         Set the solver parameters of this joint.
         """
-        if self.is_built:
+        if self._solver.is_built:
             self._solver.set_sol_params(sol_params, joints_idx=self._idx, envs_idx=None)
         else:
             self._sol_params = sol_params
 
     @property
     def sol_params(self):
         """
-        Retruns the solver parameters of the joint.
+        Returns the solver parameters of the joint.
         """
-        if self.is_built:
+        if self._solver.is_built:
             return self._solver.get_sol_params(joints_idx=self._idx, envs_idx=None)[..., 0, :]
         return self._sol_params
 

genesis/engine/sensors/camera.py

@@ -17,7 +17,7 @@
     SensorOptions,
 )
 
-from genesis.utils.geom import pos_lookat_up_to_T, trans_quat_to_T
+from genesis.utils.geom import pos_lookat_up_to_T, trans_quat_to_T, transform_by_quat
 from genesis.utils.misc import tensor_to_array
 from genesis.vis.batch_renderer import BatchRenderer
 from genesis.options.renderers import BatchRenderer as BatchRendererOptions
@@ -490,8 +490,6 @@ def _update_camera_pose(self):
             link_quat = self._link.get_quat()
 
             # Apply pos directly as offset from link
-            from genesis.utils.geom import transform_by_quat
-
             pos_world = transform_by_quat(pos, link_quat) + link_pos
             pos = pos_world
         elif self._link is not None:
@@ -596,8 +594,6 @@ def build(self):
             link_quat = self._link.get_quat()
 
             # Apply pos directly as offset from link
-            from genesis.utils.geom import transform_by_quat
-
             pos_world = transform_by_quat(torch.tensor(pos, dtype=gs.tc_float, device=gs.device), link_quat) + link_pos
             pos = pos_world.tolist()
 

genesis/engine/solvers/rigid/collider_decomp.py

@@ -3,6 +3,7 @@
 
 import numpy as np
 import torch
+import trimesh
 
 import gstaichi as ti
 
@@ -186,11 +187,13 @@ def _compute_collision_pair_idx(self):
         Compute flat indices of all valid collision pairs.
 
         For each pair of geoms, determine if they can collide based on their properties and the solver configuration.
+        Pairs that are already colliding at the initial configuration (qpos0) are filtered out with a warning.
         """
         solver = self._solver
         n_geoms = solver.n_geoms_
         n_equalities = solver.n_equalities
         enable_self_collision = solver._enable_self_collision
+        enable_neutral_collision = solver._enable_neutral_collision
         enable_adjacent_collision = solver._enable_adjacent_collision
         batch_links_info = solver._static_rigid_sim_config.batch_links_info
 
@@ -211,6 +214,18 @@ def _compute_collision_pair_idx(self):
             links_is_fixed = links_is_fixed[:, 0]
         entities_is_local_collision_mask = solver.entities_info.is_local_collision_mask.to_numpy()
 
+        # Compute vertices all geometries, shrunk by 0.1% to avoid false positive when detecting self-collision
+        geoms_verts: list[np.ndarray] = []
+        for geom in solver.geoms:
+            verts = tensor_to_array(geom.get_verts())
+            verts = verts.reshape((-1, *verts.shape[-2:]))
+            centroid = verts.mean(axis=1, keepdims=True)
+            verts = centroid + (1.0 - 1e-3) * (verts - centroid)
+            geoms_verts.append(verts)
+
+        # Track pairs that are colliding in neutral configuration for warning
+        self_colliding_pairs: list[tuple[int, int]] = []
+
         n_possible_pairs = 0
         collision_pair_idx = np.full((n_geoms, n_geoms), fill_value=-1, dtype=gs.np_int)
         for i_ga in range(n_geoms):
@@ -224,17 +239,6 @@ def _compute_collision_pair_idx(self):
                 if i_la == i_lb:
                     continue
 
-                # self collision
-                if links_root_idx[i_la] == links_root_idx[i_lb]:
-                    if not enable_self_collision:
-                        continue
-
-                    # adjacent links
-                    if not enable_adjacent_collision and (
-                        links_parent_idx[i_la] == i_lb or links_parent_idx[i_lb] == i_la
-                    ):
-                        continue
-
                 # Filter out right away weld constraint that have been declared statically and cannot be removed
                 is_valid = True
                 for i_eq in range(n_equalities):
@@ -258,9 +262,49 @@ def _compute_collision_pair_idx(self):
                 if links_is_fixed[i_la] and links_is_fixed[i_lb]:
                     continue
 
+                # self collision
+                if links_root_idx[i_la] == links_root_idx[i_lb]:
+                    if not enable_self_collision:
+                        continue
+
+                    # adjacent links
+                    if not enable_adjacent_collision and (
+                        links_parent_idx[i_la] == i_lb or links_parent_idx[i_lb] == i_la
+                    ):
+                        continue
+
+                    # active in neutral configuration (qpos0)
+                    is_self_colliding = False
+                    for i_b in range(1 if not enable_neutral_collision else 0):
+                        mesh_a = trimesh.Trimesh(
+                            vertices=geoms_verts[i_ga][i_b], faces=solver.geoms[i_ga].init_faces, process=False
+                        )
+                        mesh_b = trimesh.Trimesh(
+                            vertices=geoms_verts[i_gb][i_b], faces=solver.geoms[i_gb].init_faces, process=False
+                        )
+                        bounds_a, bounds_b = mesh_a.bounds, mesh_b.bounds
+                        if not ((bounds_a[1] < bounds_b[0]).any() or (bounds_b[1] < bounds_a[0]).any()):
+                            if (mesh_a.is_watertight and mesh_a.contains(mesh_b.vertices).any()) or (
+                                mesh_b.is_watertight and mesh_b.contains(mesh_a.vertices).any()
+                            ):
+                                is_self_colliding = True
+                                self_colliding_pairs.append((i_ga, i_gb))
+                                break
+                    if is_self_colliding:
+                        continue
+
                 collision_pair_idx[i_ga, i_gb] = n_possible_pairs
                 n_possible_pairs = n_possible_pairs + 1
 
+        # Emit warning for self-collision pairs
+        if self_colliding_pairs:
+            pairs = ", ".join((f"({i_ga}, {i_gb})") for i_ga, i_gb in self_colliding_pairs)
+            gs.logger.warning(
+                f"Filtered out geometry pairs causing self-collision for the neutral configuration (qpos0): {pairs}. "
+                "Consider tuning Morph option 'decompose_robot_error_threshold' or specify dedicated collision meshes. "
+                "This behavior can be disabled by setting Morph option 'enable_neutral_collision=True'."
+            )
+
         return n_possible_pairs, collision_pair_idx
 
     def _compute_verts_connectivity(self):

genesis/engine/solvers/rigid/rigid_solver_decomp.py

@@ -93,6 +93,7 @@ def __init__(self, scene: "Scene", sim: "Simulator", options: RigidOptions) -> N
         self._enable_mujoco_compatibility = options.enable_mujoco_compatibility
         self._enable_joint_limit = options.enable_joint_limit
         self._enable_self_collision = options.enable_self_collision
+        self._enable_neutral_collision = options.enable_neutral_collision
         self._enable_adjacent_collision = options.enable_adjacent_collision
         self._disable_constraint = options.disable_constraint
         self._max_collision_pairs = options.max_collision_pairs
@@ -132,6 +133,9 @@ def __init__(self, scene: "Scene", sim: "Simulator", options: RigidOptions) -> N
 
         self._options = options
 
+        self.collider = None
+        self.constraint_solver = None
+
         self.qpos: ti.Template | ti.types.NDArray | None = None
 
         self._is_backward: bool = False
@@ -350,12 +354,13 @@ def build(self):
 
         self._init_envs_offset()
         self._init_sdf()
-        self._init_collider()
-        self._init_constraint_solver()
 
         self._init_invweight_and_meaninertia(force_update=False)
         self._func_update_geoms(self._scene._envs_idx, force_update_fixed_geoms=True)
 
+        self._init_collider()
+        self._init_constraint_solver()
+
         # FIXME: when the migration is finished, we will remove the about two lines
         self._func_vel_at_point = func_vel_at_point
         self._func_apply_coupling_force = func_apply_coupling_force
@@ -639,7 +644,7 @@ def _init_link_fields(self):
             self.qpos.from_numpy(init_qpos)
         if is_init_qpos_out_of_bounds:
             gs.logger.warning(
-                "Reference robot position exceeds joint limits."
+                "Neutral robot position (qpos0) exceeds joint limits."
                 # "Clipping initial position too make sure it is valid."
             )
 
@@ -1833,8 +1838,10 @@ def set_qpos(self, qpos, qs_idx=None, envs_idx=None, *, skip_forward=False):
                 qpos = qpos[None]
             kernel_set_qpos(qpos, qs_idx, envs_idx, self._rigid_global_info, self._static_rigid_sim_config)
 
-        self.collider.reset(envs_idx)
-        self.constraint_solver.reset(envs_idx)
+        if self.collider is not None:
+            self.collider.reset(envs_idx)
+        if self.constraint_solver is not None:
+            self.constraint_solver.reset(envs_idx)
 
         if not skip_forward:
             if not isinstance(envs_idx, torch.Tensor):
@@ -2220,7 +2227,7 @@ def get_sol_params(self, geoms_idx=None, envs_idx=None, *, joints_idx=None, eqs_
             tensor = ti_to_torch(self.joints_info.sol_params, envs_idx, joints_idx, transpose=True, copy=True)
             if self.n_envs == 0 and self._options.batch_joints_info:
                 tensor = tensor[0]
-        else:
+        else:  # geoms_idx is not None
             # Never batched
             assert envs_idx is None
             tensor = ti_to_torch(self.geoms_info.sol_params, geoms_idx, transpose=True, copy=True)

genesis/options/solvers.py

@@ -294,6 +294,9 @@ class RigidOptions(Options):
         Whether to enable joint limit. Defaults to True.
     enable_self_collision : bool, optional
         Whether to enable self collision within each entity. Defaults to True.
+    enable_neutral_collision : bool, optional
+        Whether to enable self collision occurring in neutral configuration (qpos0) within each entity. Defaults to
+        False.
     enable_adjacent_collision : bool, optional
         Whether to enable collision between successive parent-child body pairs within each entity. Defaults to False.
     disable_constraint: bool, optional
@@ -362,6 +365,7 @@ class RigidOptions(Options):
     enable_collision: bool = True
     enable_joint_limit: bool = True
     enable_self_collision: bool = True
+    enable_neutral_collision: bool = False
     enable_adjacent_collision: bool = False
     disable_constraint: bool = False
     max_collision_pairs: int = 150

genesis/utils/geom.py

@@ -1535,7 +1535,7 @@ def _tc_z_up_to_R(z, eps: float, up=None, out: torch.Tensor | None = None):
 
     # Compute x vectors (first column)
     if up is not None:
-        x[:] = torch.cross(up, z, dim=-1)
+        x[:] = torch.cross(torch.broadcast_to(up, z.shape), z, dim=-1)
     else:
         up_mask = z[..., 2:].abs() < 1.0 - eps
         torch.where(up_mask, z[..., 1], z[..., 2], out=x[..., 0])