[FEATURE] Add option to force batching of fixed vertices. (#1998)

duburcqa · web-flow · commit ab9ee2d109d1 · 2025-11-17T17:55:08.000+01:00
* Add option to force batching of fixed vertices.
* Add collision-free franka benchmark.
* Report large performance improvement as success.
diff --git a/.github/workflows/alarm.yml b/.github/workflows/alarm.yml
@@ -396,7 +396,6 @@ jobs:
           fi
 
           # Export status
-          echo "CONCLUSION=$([ "$EXIT_CODE" = "0" ] && echo 'success' || echo 'failure')" >> "$GITHUB_ENV"
           echo "HAS_REGRESSIONS=$([ "$EXIT_CODE" = "$EXIT_CODE_REGRESSION" ] && echo 1 || echo 0)" >> "$GITHUB_ENV"
           echo "HAS_ALERTS=$([ "$EXIT_CODE" = "$EXIT_CODE_ALERT" ] && echo 1 || echo 0)" >> "$GITHUB_ENV"
 
@@ -416,7 +415,6 @@ jobs:
         env:
           CHECK_NAME: Benchmark Comparison
           CHECK_OUTPUT: ${{ env.CHECK_OUTPUT }}
-          CONCLUSION: ${{ env.CONCLUSION }}
           HAS_REGRESSIONS: ${{ env.HAS_REGRESSIONS }}
           HAS_ALERTS: ${{ env.HAS_ALERTS }}
           ARTIFACT_URL: ${{ steps.upload.outputs.artifact-url }}
@@ -429,8 +427,10 @@ jobs:
             }
 
             let summary;
+            let conclusion = 'success';
             if ((process.env.HAS_REGRESSIONS || '0') === '1') {
               summary = '🔴 Regressions detected. See tables below.';
+              conclusion = 'failure';
             } else if ((process.env.HAS_ALERTS || '0') === '1') {
               summary = '⚠️ Large deviation detected. See tables below.';
             } else {
@@ -443,7 +443,7 @@ jobs:
               head_sha: context.payload.workflow_run.head_sha,
               name: process.env.CHECK_NAME,
               status: 'completed',
-              conclusion: process.env.CONCLUSION,
+              conclusion: conclusion,
               output: {
                 title: process.env.CHECK_NAME,
                 summary,
diff --git a/examples/manipulation/grasp_env.py b/examples/manipulation/grasp_env.py
@@ -75,6 +75,7 @@ def __init__(
                 size=env_cfg["box_size"],
                 fixed=env_cfg["box_fixed"],
                 collision=env_cfg["box_collision"],
+                batch_fixed_verts=True,
             ),
             # material=gs.materials.Rigid(gravity_compensation=1),
             surface=gs.surfaces.Rough(
diff --git a/examples/sensors/lidar_teleop.py b/examples/sensors/lidar_teleop.py
@@ -87,7 +87,7 @@ def main():
             max_FPS=60,
         ),
         profiling_options=gs.options.ProfilingOptions(
-            show_FPS=True,
+            show_FPS=False,
         ),
         show_viewer=True,
     )
diff --git a/genesis/engine/entities/rigid_entity/rigid_entity.py b/genesis/engine/entities/rigid_entity/rigid_entity.py
@@ -86,9 +86,10 @@ def __init__(
 
         self._free_verts_idx_local = torch.tensor([], dtype=gs.tc_int, device=gs.device)
         self._fixed_verts_idx_local = torch.tensor([], dtype=gs.tc_int, device=gs.device)
-
         self._base_links_idx = torch.tensor([self.base_link_idx], dtype=gs.tc_int, device=gs.device)
 
+        self._batch_fixed_verts = morph.batch_fixed_verts
+
         self._visualize_contact: bool = visualize_contact
 
         self._is_built: bool = False
@@ -565,7 +566,7 @@ def _build(self):
         free_verts_idx_local, fixed_verts_idx_local = [], []
         for link in self.links:
             verts_idx = torch.arange(verts_start, verts_start + link.n_verts, dtype=gs.tc_int, device=gs.device)
-            if link.is_fixed:
+            if link.is_fixed and not self._batch_fixed_verts:
                 fixed_verts_idx_local.append(verts_idx)
             else:
                 free_verts_idx_local.append(verts_idx)
@@ -643,7 +644,7 @@ def _add_by_info(self, l_info, j_infos, g_infos, morph, surface):
         joint_start = self.n_joints + self._joint_start
         free_verts_start, fixed_verts_start = self._free_verts_state_start, self._fixed_verts_state_start
         for link in self.links:
-            if link.is_fixed:
+            if link.is_fixed and not self._batch_fixed_verts:
                 fixed_verts_start += link.n_verts
             else:
                 free_verts_start += link.n_verts
diff --git a/genesis/engine/entities/rigid_entity/rigid_geom.py b/genesis/engine/entities/rigid_entity/rigid_geom.py
@@ -384,7 +384,7 @@ def get_verts(self):
         """
         self._solver.update_verts_for_geoms(self._idx)
 
-        if self.is_fixed:
+        if self.is_fixed and not self._entity._batch_fixed_verts:
             tensor = torch.empty((self.n_verts, 3), dtype=gs.tc_float, device=gs.device)
             _kernel_get_fixed_verts(tensor, self._verts_state_start, self.n_verts, self._solver.fixed_verts_state)
         else:
diff --git a/genesis/engine/entities/rigid_entity/rigid_link.py b/genesis/engine/entities/rigid_entity/rigid_link.py
@@ -79,6 +79,11 @@ def __init__(
         self._root_idx: int = root_idx
         self._is_fixed: bool = is_fixed
 
+        if is_fixed and not entity._batch_fixed_verts:
+            verts_state_start = fixed_verts_state_start
+        else:
+            verts_state_start = free_verts_state_start
+
         self._joint_start: int = joint_start
         self._n_joints: int = n_joints
 
@@ -87,7 +92,7 @@ def __init__(
         self._vert_start: int = vert_start
         self._face_start: int = face_start
         self._edge_start: int = edge_start
-        self._verts_state_start: int = fixed_verts_state_start if is_fixed else free_verts_state_start
+        self._verts_state_start: int = verts_state_start
         self._vgeom_start: int = vgeom_start
         self._vvert_start: int = vvert_start
         self._vface_start: int = vface_start
@@ -302,7 +307,7 @@ def get_verts(self):
         """
         self._solver.update_verts_for_geoms(range(self.geom_start, self.geom_end))
 
-        if self.is_fixed:
+        if self.is_fixed and not self._entity._batch_fixed_verts:
             tensor = torch.empty((self.n_verts, 3), dtype=gs.tc_float, device=gs.device)
             _kernel_get_fixed_verts(tensor, self._verts_state_start, self.n_verts, self._solver.fixed_verts_state)
         else:
diff --git a/genesis/engine/solvers/rigid/rigid_solver_decomp.py b/genesis/engine/solvers/rigid/rigid_solver_decomp.py
@@ -598,7 +598,10 @@ def _init_vert_fields(self):
                     [np.arange(geom.verts_state_start, geom.verts_state_start + geom.n_verts) for geom in geoms],
                     dtype=gs.np_int,
                 ),
-                is_fixed=np.concatenate([np.full(geom.n_verts, geom.is_fixed) for geom in geoms], dtype=gs.np_bool),
+                is_fixed=np.concatenate(
+                    [np.full(geom.n_verts, geom.is_fixed and not geom.entity._batch_fixed_verts) for geom in geoms],
+                    dtype=gs.np_bool,
+                ),
                 # taichi variables
                 verts_info=self.verts_info,
                 faces_info=self.faces_info,
@@ -1484,13 +1487,18 @@ def set_base_links_pos(
         if not unsafe and not torch.isin(links_idx, self._base_links_idx).all():
             gs.raise_exception("`links_idx` contains at least one link that is not a base link.")
 
-        # Raise exception for fixed links with at least one geom, except if setting same location for all envs at once
+        # Raise exception for fixed links with at least one geom and non-batched fixed vertices, except if setting same
+        # location for all envs at once
         set_all_envs = torch.equal(torch.sort(envs_idx).values, self._scene._envs_idx)
-        has_fixed_geoms = any(
-            link.is_fixed and (link.geoms or link.vgeoms) for link in (self.links[i_l] for i_l in links_idx)
+        has_fixed_verts = any(
+            link.is_fixed and (link.geoms or link.vgeoms) and not link.entity._batch_fixed_verts
+            for link in (self.links[i_l] for i_l in links_idx)
         )
-        if has_fixed_geoms and not (set_all_envs and (torch.diff(pos, dim=0).abs() < gs.EPS).all()):
-            gs.raise_exception("Impossible to set env-specific pos for fixed links with at least one geometry.")
+        if has_fixed_verts and not (set_all_envs and (torch.diff(pos, dim=0).abs() < gs.EPS).all()):
+            gs.raise_exception(
+                "Specifying env-specific pos for fixed links with at least one geometry requires setting morph "
+                "option 'batch_fixed_verts=True'."
+            )
 
         kernel_set_links_pos(
             relative,
@@ -1537,10 +1545,11 @@ def set_base_links_quat(
             gs.raise_exception("`links_idx` contains at least one link that is not a base link.")
 
         set_all_envs = torch.equal(torch.sort(envs_idx).values, self._scene._envs_idx)
-        has_fixed_geoms = any(
-            link.is_fixed and (link.geoms or link.vgeoms) for link in (self.links[i_l] for i_l in links_idx)
+        has_fixed_verts = any(
+            link.is_fixed and (link.geoms or link.vgeoms) and not link.entity._batch_fixed_verts
+            for link in (self.links[i_l] for i_l in links_idx)
         )
-        if has_fixed_geoms and not (set_all_envs and (torch.diff(quat, dim=0).abs() < gs.EPS).all()):
+        if has_fixed_verts and not (set_all_envs and (torch.diff(quat, dim=0).abs() < gs.EPS).all()):
             gs.raise_exception("Impossible to set env-specific quat for fixed links with at least one geometry.")
 
         kernel_set_links_quat(
@@ -2435,13 +2444,13 @@ def n_verts(self):
     def n_free_verts(self):
         if self.is_built:
             return self._n_free_verts
-        return sum(link.n_verts if not link.is_fixed else 0 for link in self.links)
+        return sum(link.n_verts if not link.is_fixed or link.entity._batch_fixed_verts else 0 for link in self.links)
 
     @property
     def n_fixed_verts(self):
         if self.is_built:
             return self._n_fixed_verts
-        return sum(link.n_verts if link.is_fixed else 0 for link in self.links)
+        return sum(link.n_verts if link.is_fixed and not link.entity._batch_fixed_verts else 0 for link in self.links)
 
     @property
     def n_vverts(self):
@@ -4953,7 +4962,6 @@ def func_update_geoms(
                         links_state.pos[geoms_info.link_idx[i_g], i_b],
                         links_state.quat[geoms_info.link_idx[i_g], i_b],
                     )
-
                     geoms_state.verts_updated[i_g, i_b] = False
     else:
         for i_g in range(n_geoms):
@@ -4967,7 +4975,6 @@ def func_update_geoms(
                     links_state.pos[geoms_info.link_idx[i_g], i_b],
                     links_state.quat[geoms_info.link_idx[i_g], i_b],
                 )
-
                 geoms_state.verts_updated[i_g, i_b] = False
 
 
@@ -4998,23 +5005,22 @@ def func_update_verts_for_geom(
     fixed_verts_state: array_class.VertsState,
 ):
     if not geoms_state.verts_updated[i_g, i_b]:
-        if geoms_info.is_fixed[i_g]:
-            for i_v in range(geoms_info.vert_start[i_g], geoms_info.vert_end[i_g]):
+        i_g_start = geoms_info.vert_start[i_g]
+        if verts_info.is_fixed[i_g_start]:
+            for i_v in range(i_g_start, geoms_info.vert_end[i_g]):
                 verts_state_idx = verts_info.verts_state_idx[i_v]
                 fixed_verts_state.pos[verts_state_idx] = gu.ti_transform_by_trans_quat(
                     verts_info.init_pos[i_v], geoms_state.pos[i_g, i_b], geoms_state.quat[i_g, i_b]
                 )
-
             _B = geoms_state.verts_updated.shape[1]
             for j_b in range(_B):
                 geoms_state.verts_updated[i_g, j_b] = True
         else:
-            for i_v in range(geoms_info.vert_start[i_g], geoms_info.vert_end[i_g]):
+            for i_v in range(i_g_start, geoms_info.vert_end[i_g]):
                 verts_state_idx = verts_info.verts_state_idx[i_v]
                 free_verts_state.pos[verts_state_idx, i_b] = gu.ti_transform_by_trans_quat(
                     verts_info.init_pos[i_v], geoms_state.pos[i_g, i_b], geoms_state.quat[i_g, i_b]
                 )
-
             geoms_state.verts_updated[i_g, i_b] = True
 
 
diff --git a/genesis/options/morphs.py b/genesis/options/morphs.py
@@ -166,6 +166,9 @@ class Primitive(Morph):
         Defaults to False. **This is only used for RigidEntity.**
     fixed : bool, optional
         Whether the baselink of the entity should be fixed. Defaults to False. **This is only used for RigidEntity.**
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to true. **This is only used for RigidEntity.**
     contype : int, optional
         The 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. Defaults to 0xFFFF.
@@ -176,6 +179,7 @@ class Primitive(Morph):
 
     # Rigid specific
     fixed: bool = False
+    batch_fixed_verts: bool = True
     contype: int = 0xFFFF
     conaffinity: int = 0xFFFF
 
@@ -215,6 +219,9 @@ class Box(Primitive, TetGenMixin):
         **This is only used for RigidEntity.**
     fixed : bool, optional
         Whether the baselink of the entity should be fixed. Defaults to False. **This is only used for RigidEntity.**
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to true. **This is only used for RigidEntity.**
     contype : int, optional
         The 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. Defaults to 0xFFFF.
@@ -297,6 +304,9 @@ class Cylinder(Primitive, TetGenMixin):
         **This is only used for RigidEntity.**
     fixed : bool, optional
         Whether the baselink of the entity should be fixed. Defaults to False. **This is only used for RigidEntity.**
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to true. **This is only used for RigidEntity.**
     contype : int, optional
         The 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. Defaults to 0xFFFF.
@@ -359,6 +369,9 @@ class Sphere(Primitive, TetGenMixin):
         **This is only used for RigidEntity.**
     fixed : bool, optional
         Whether the baselink of the entity should be fixed. Defaults to False. **This is only used for RigidEntity.**
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to true. **This is only used for RigidEntity.**
     contype : int, optional
         The 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. Defaults to 0xFFFF.
@@ -422,6 +435,9 @@ class Plane(Primitive):
         `visualization` and `collision` cannot both be False. **This is only used for RigidEntity.**
     fixed : bool, optional
         Whether the baselink of the entity should be fixed. Defaults to False. **This is only used for RigidEntity.**
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to false. **This is only used for RigidEntity.**
     contype : int, optional
         The 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. Defaults to 0xFFFF.
@@ -435,6 +451,7 @@ class Plane(Primitive):
     """
 
     fixed: bool = True
+    batch_fixed_verts: bool = False
     normal: tuple = (0, 0, 1)
     plane_size: tuple = (1e3, 1e3)
     tile_size: tuple = (1, 1)
@@ -510,6 +527,9 @@ class FileMorph(Morph):
     collision : bool, optional
         Whether the entity needs to be considered for collision checking. Defaults to True.
         `visualization` and `collision` cannot both be False. **This is only used for RigidEntity.**
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to true. **This is only used for RigidEntity.**
     requires_jac_and_IK : bool, optional
         Whether this morph, if created as `RigidEntity`, requires jacobian and inverse kinematics. Defaults to False.
         **This is only used for RigidEntity.**
@@ -526,6 +546,7 @@ class FileMorph(Morph):
     decompose_robot_error_threshold: float = float("inf")
     coacd_options: Optional[CoacdOptions] = None
     recompute_inertia: bool = False
+    batch_fixed_verts: bool = False
 
     def __init__(self, **data):
         super().__init__(**data)
@@ -646,6 +667,9 @@ class Mesh(FileMorph, TetGenMixin):
         Whether to use zup to load glb files. Defaults to False.
     fixed : bool, optional
         Whether the baselink of the entity should be fixed. Defaults to False. **This is only used for RigidEntity.**
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to false. **This is only used for RigidEntity.**
     contype : int, optional
         The 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. Defaults to 0xFFFF.
@@ -779,6 +803,9 @@ class MJCF(FileMorph):
         `visualization` and `collision` cannot both be False.
     requires_jac_and_IK : bool, optional
         Whether this morph, if created as `RigidEntity`, requires jacobian and inverse kinematics. Defaults to True.
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to true. **This is only used for RigidEntity.**
     default_armature : float, optional
         Default rotor inertia of the actuators. In practice it is applied to all joints regardless of whether they are
         actuated. None to disable. Default to 0.1.
@@ -882,6 +909,9 @@ class URDF(FileMorph):
         Whether this morph, if created as `RigidEntity`, requires jacobian and inverse kinematics. Defaults to True.
     fixed : bool, optional
         Whether the baselink of the entity should be fixed. Defaults to False.
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to true. **This is only used for RigidEntity.**
     prioritize_urdf_material : bool, optional
         Sometimes a geom in a urdf file will be assigned a color, and the geom asset file also contains its own visual
         material. This parameter controls whether to prioritize the URDF-defined material over the asset's own material.
@@ -1103,8 +1133,12 @@ class Terrain(Morph):
         This parameter is deprecated.
     subterrain_parameters : dictionary, optional
         Lets users pick their own subterrain parameters.
+    batch_fixed_verts : bool, optional
+        Whether to batch fixed vertices. This will allow setting env-specific poses to fixed geometries, at the cost of
+        significantly increasing memory usage. Default to false. **This is only used for RigidEntity.**
     """
 
+    batch_fixed_verts: bool = False
     randomize: bool = False  # whether to randomize the terrain
     n_subterrains: Tuple[int, int] = (3, 3)  # number of subterrains in x and y directions
     subterrain_size: Tuple[float, float] = (12.0, 12.0)  # meter
diff --git a/tests/test_examples.py b/tests/test_examples.py
@@ -33,7 +33,7 @@
     "genesis_ipc_motion_test.py",  # TODO: wait for IPC's wheel to be released
 }
 
-TIMEOUT = 400.0
+TIMEOUT = 450.0
 
 
 pytestmark = [
diff --git a/tests/test_rigid_benchmarks.py b/tests/test_rigid_benchmarks.py
diff --git a/tests/test_rigid_physics.py b/tests/test_rigid_physics.py
diff --git a/tests/test_sensors.py b/tests/test_sensors.py

Original file line number	Diff line number	Diff line change
`@@ -87,7 +87,7 @@ def main():`
`87`	`87`	`max_FPS=60,`
`88`	`88`	`),`
`89`	`89`	`profiling_options=gs.options.ProfilingOptions(`
`90`		`- show_FPS=True,`
	`90`	`+ show_FPS=False,`
`91`	`91`	`),`
`92`	`92`	`show_viewer=True,`
`93`	`93`	`)`
Original file line number	Diff line number	Diff line change
`@@ -33,7 +33,7 @@`
`33`	`33`	`"genesis_ipc_motion_test.py", # TODO: wait for IPC's wheel to be released`
`34`	`34`	`}`
`35`	`35`
`36`		`-TIMEOUT = 400.0`
	`36`	`+TIMEOUT = 450.0`
`37`	`37`
`38`	`38`
`39`	`39`	`pytestmark = [`