Define Rigid solver error codes as enum.

Author: duburcqa

genesis/engine/solvers/rigid/abd/diff.py

@@ -51,7 +51,7 @@ def func_copy_next_to_curr(
             for i_q in range(n_qs):
                 rigid_global_info.qpos[i_q, i_b] = rigid_global_info.qpos_next[i_q, i_b]
         else:
-            errno[i_b] = errno[i_b] | 0b00000000000000000000000000001000
+            errno[i_b] = errno[i_b] | array_class.ErrorCode.INVALID_ACC_NAN
 
 
 @ti.func

genesis/engine/solvers/rigid/abd/forward_kinematics.py

@@ -1241,7 +1241,7 @@ def func_hibernate__for_all_awake_islands_either_hiberanate_or_update_aabb_sort_
 
                 # Invariant check: ensure entity_id access won't exceed buffer
                 if entity_ref_start + entity_ref_n > contact_island_state.entity_id.shape[0]:
-                    errno[i_b] = errno[i_b] | 0b00000000000000000000000000010000
+                    errno[i_b] = errno[i_b] | array_class.ErrorCode.OVERFLOW_HIBERNATION_ISLANDS
                     continue
 
                 for i_entity_ref_offset_ in range(entity_ref_n):

genesis/engine/solvers/rigid/collider/broadphase.py

@@ -294,7 +294,7 @@ def func_broad_phase(
                             continue
 
                         if n_broad == collider_info.max_collision_pairs_broad[None]:
-                            errno[i_b] = errno[i_b] | 0b00000000000000000000000000000001
+                            errno[i_b] = errno[i_b] | array_class.ErrorCode.OVERFLOW_CANDIDATE_CONTACTS
                             break
                         collider_state.broad_collision_pairs[n_broad, i_b][0] = i_ga
                         collider_state.broad_collision_pairs[n_broad, i_b][1] = i_gb

genesis/engine/solvers/rigid/collider/contact.py

@@ -196,7 +196,7 @@ def func_add_contact(
 
         collider_state.n_contacts[i_b] = i_c + 1
     else:
-        errno[i_b] = errno[i_b] | 0b00000000000000000000000000000010
+        errno[i_b] = errno[i_b] | array_class.ErrorCode.OVERFLOW_COLLISION_PAIRS
 
 
 @ti.func

genesis/engine/solvers/rigid/collider/contact_island.py

@@ -100,7 +100,7 @@ def add_edge(self, link_a, link_b, i_b):
             self.entity_edge.n[eb, i_b] = self.entity_edge.n[eb, i_b] + 1
         else:
             # Signal buffer overflow via errno bit 4 (0b00010000)
-            self.errno[i_b] = self.errno[i_b] | 0b00000000000000000000000000010000
+            self.errno[i_b] = self.errno[i_b] | array_class.ErrorCode.OVERFLOW_HIBERNATION_ISLANDS
 
     @ti.kernel
     def add_contact_edges_to_islands(self):
@@ -205,7 +205,7 @@ def preprocess_island_and_map_entities_to_edges(self):
 
             # Invariant check: ensure total half-edges don't exceed edge_id buffer
             if entity_list_start > self.edge_id.shape[0]:
-                self.errno[i_b] = self.errno[i_b] | 0b00000000000000000000000000010000
+                self.errno[i_b] = self.errno[i_b] | array_class.ErrorCode.OVERFLOW_HIBERNATION_ISLANDS
 
             # process added collider-info edges
             for i in range(self.n_edges[i_b]):

genesis/engine/solvers/rigid/constraint/solver.py

@@ -7,11 +7,12 @@
 import genesis as gs
 import genesis.utils.geom as gu
 import genesis.utils.array_class as array_class
+from genesis.engine.solvers.rigid.abd import func_solve_mass_batch
+from genesis.utils.misc import ti_to_torch
+
 from . import backward as backward_constraint_solver
-import genesis.engine.solvers.rigid.rigid_solver as rigid_solver
 from . import noslip as constraint_noslip
 from ..collider.contact_island import ContactIsland
-from genesis.utils.misc import ti_to_torch
 
 if TYPE_CHECKING:
     from genesis.engine.solvers.rigid.rigid_solver import RigidSolver
@@ -2352,7 +2353,7 @@ def func_update_gradient_batch(
         )
 
     if ti.static(static_rigid_sim_config.solver_type == gs.constraint_solver.CG):
-        rigid_solver.func_solve_mass_batch(
+        func_solve_mass_batch(
             i_b,
             constraint_state.grad,
             constraint_state.Mgrad,
@@ -2388,7 +2389,7 @@ def func_update_gradient_tiled(
     if ti.static(static_rigid_sim_config.solver_type == gs.constraint_solver.CG):
         ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL, block_dim=32)
         for i_b in range(_B):
-            rigid_solver.func_solve_mass_batch(
+            func_solve_mass_batch(
                 i_b,
                 constraint_state.grad,
                 constraint_state.Mgrad,
@@ -2838,7 +2839,7 @@ def func_update_qacc(
         dofs_state.force[i_d, i_b] = dofs_state.qf_smooth[i_d, i_b] + constraint_state.qfrc_constraint[i_d, i_b]
         constraint_state.qacc_ws[i_d, i_b] = constraint_state.qacc[i_d, i_b]
         if ti.math.isnan(constraint_state.qacc[i_d, i_b]):
-            errno[i_b] = errno[i_b] | 0b00000000000000000000000000000100
+            errno[i_b] = errno[i_b] | array_class.ErrorCode.INVALID_FORCE_NAN
 
     ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)
     for i_b in range(_B):

genesis/engine/solvers/rigid/rigid_solver.py

@@ -7,7 +7,6 @@
 
 import genesis as gs
 import genesis.utils.array_class as array_class
-import genesis.utils.geom as gu
 from genesis.engine.entities import DroneEntity, RigidEntity
 from genesis.engine.entities.base_entity import Entity
 from genesis.engine.states import QueriedStates, RigidSolverState
@@ -27,7 +26,18 @@
 from .collider import Collider
 from .constraint import ConstraintSolver, ConstraintSolverIsland
 from .abd.misc import (
+    func_add_safe_backward,
+    func_apply_coupling_force,
+    func_apply_link_external_force,
+    func_apply_external_torque,
+    func_apply_link_external_torque,
+    func_atomic_add_if,
+    func_check_index_range,
+    func_clear_external_force,
+    func_read_field_if,
     func_wakeup_entity_and_its_temp_island,
+    func_write_field_if,
+    func_write_and_read_field_if,
     kernel_init_invweight,
     kernel_init_meaninertia,
     kernel_init_dof_fields,
@@ -43,77 +53,66 @@
     kernel_init_equality_fields,
     kernel_apply_links_external_force,
     kernel_apply_links_external_torque,
-    func_apply_coupling_force,
-    func_apply_link_external_force,
-    func_apply_external_torque,
-    func_apply_link_external_torque,
-    func_clear_external_force,
     kernel_update_geoms_render_T,
     kernel_update_vgeoms_render_T,
     kernel_bit_reduction,
     kernel_set_zero,
-    func_atomic_add_if,
-    func_add_safe_backward,
-    func_read_field_if,
-    func_write_field_if,
-    func_write_and_read_field_if,
-    func_check_index_range,
     kernel_clear_external_force,
 )
 from .abd.forward_kinematics import (
-    kernel_forward_kinematics_links_geoms,
-    kernel_masked_forward_kinematics_links_geoms,
-    kernel_forward_velocity,
-    kernel_masked_forward_velocity,
+    func_aggregate_awake_entities,
     func_COM_links,
     func_COM_links_entity,
     func_forward_kinematics_entity,
     func_forward_kinematics_batch,
-    kernel_forward_kinematics_entity,
+    func_forward_velocity_entity,
+    func_forward_velocity_batch,
+    func_forward_velocity,
+    func_hibernate_entity_and_zero_dof_velocities,
+    func_hibernate__for_all_awake_islands_either_hiberanate_or_update_aabb_sort_buffer,
     func_update_geoms_entity,
     func_update_geoms_batch,
+    func_update_all_verts,
+    func_update_cartesian_space,
+    func_update_cartesian_space_entity,
+    func_update_cartesian_space_batch,
     func_update_geoms,
+    func_update_verts_for_geom,
+    kernel_forward_kinematics_links_geoms,
+    kernel_masked_forward_kinematics_links_geoms,
+    kernel_forward_velocity,
+    kernel_masked_forward_velocity,
+    kernel_forward_kinematics_entity,
     kernel_update_geoms,
-    func_forward_velocity_entity,
-    func_forward_velocity_batch,
-    func_forward_velocity,
     kernel_update_verts_for_geoms,
-    func_update_verts_for_geom,
-    func_update_all_verts,
     kernel_update_all_verts,
     kernel_update_geom_aabbs,
     kernel_update_vgeoms,
-    func_hibernate__for_all_awake_islands_either_hiberanate_or_update_aabb_sort_buffer,
-    func_aggregate_awake_entities,
-    func_hibernate_entity_and_zero_dof_velocities,
-    func_update_cartesian_space_entity,
-    func_update_cartesian_space_batch,
-    func_update_cartesian_space,
     kernel_update_cartesian_space,
 )
 from .abd.forward_dynamics import (
-    update_qacc_from_qvel_delta,
-    update_qvel,
-    kernel_compute_mass_matrix,
-    func_forward_dynamics,
-    kernel_forward_dynamics,
-    kernel_update_acc,
-    func_vel_at_point,
+    func_actuation,
+    func_bias_force,
     func_compute_mass_matrix,
+    func_compute_qacc,
     func_factor_mass,
+    func_forward_dynamics,
     func_solve_mass_entity,
     func_solve_mass_batch,
     func_solve_mass,
     func_torque_and_passive_force,
     func_update_acc,
     func_update_force,
-    func_actuation,
-    func_bias_force,
-    kernel_compute_qacc,
-    func_compute_qacc,
     func_integrate,
-    kernel_forward_dynamics_without_qacc,
     func_implicit_damping,
+    func_vel_at_point,
+    kernel_compute_mass_matrix,
+    kernel_forward_dynamics,
+    kernel_update_acc,
+    kernel_compute_qacc,
+    kernel_forward_dynamics_without_qacc,
+    update_qacc_from_qvel_delta,
+    update_qvel,
 )
 from .abd.accessor import (
     kernel_get_state,
@@ -156,21 +155,20 @@
     kernel_set_geoms_friction,
 )
 from .abd.diff import (
+    func_copy_cartesian_space,
     func_copy_next_to_curr,
     func_copy_next_to_curr_grad,
-    kernel_save_adjoint_cache,
-    func_save_adjoint_cache,
+    func_integrate_dq_entity,
+    func_is_grad_valid,
     func_load_adjoint_cache,
+    func_save_adjoint_cache,
+    kernel_save_adjoint_cache,
     kernel_prepare_backward_substep,
     kernel_begin_backward_substep,
-    func_is_grad_valid,
-    func_copy_cartesian_space,
     kernel_copy_acc,
-    func_integrate_dq_entity,
 )
 
 if TYPE_CHECKING:
-    import genesis.engine.solvers.rigid.array_class
     from genesis.engine.scene import Scene
     from genesis.engine.simulator import Simulator
     from genesis.engine.entities.rigid_entity import RigidJoint, RigidLink, RigidGeom, RigidVisGeom
@@ -1198,23 +1196,23 @@ def check_errno(self):
         else:
             errno = kernel_bit_reduction(self._errno)
 
-        if errno & 0b00000000000000000000000000000001:
+        if errno & array_class.ErrorCode.OVERFLOW_CANDIDATE_CONTACTS:
             max_collision_pairs_broad = self.collider._collider_info.max_collision_pairs_broad[None]
             gs.raise_exception(
                 f"Exceeding max number of broad phase candidate contact pairs ({max_collision_pairs_broad}). "
                 f"Please increase the value of RigidSolver's option 'multiplier_collision_broad_phase'."
             )
-        if errno & 0b00000000000000000000000000000010:
+        if errno & array_class.ErrorCode.OVERFLOW_COLLISION_PAIRS:
             max_contact_pairs = self.collider._collider_info.max_contact_pairs[None]
             gs.raise_exception(
                 f"Exceeding max number of contact pairs ({max_contact_pairs}). Please increase the value of "
                 "RigidSolver's option 'max_collision_pairs'."
             )
-        if errno & 0b00000000000000000000000000000100:
+        if errno & array_class.ErrorCode.INVALID_FORCE_NAN:
             gs.raise_exception("Invalid constraint forces causing 'nan'. Please decrease Rigid simulation timestep.")
-        if errno & 0b00000000000000000000000000001000:
+        if errno & array_class.ErrorCode.INVALID_ACC_NAN:
             gs.raise_exception("Invalid accelerations causing 'nan'. Please decrease Rigid simulation timestep.")
-        if errno & 0b00000000000000000000000000010000:
+        if errno & array_class.ErrorCode.OVERFLOW_HIBERNATION_ISLANDS:
             gs.raise_exception("Contact island buffer overflow. Please increase RigidOptions 'max_collision_pairs'.")
 
     def _kernel_detect_collision(self):

genesis/utils/array_class.py

@@ -1,5 +1,6 @@
 import math
 import dataclasses
+from enum import IntEnum
 from functools import partial
 from typing_extensions import dataclass_transform  # Made it into standard lib from Python 3.12
 
@@ -73,6 +74,18 @@ def V_SCALAR_FROM(dtype, value):
     return data
 
 
+# =========================================== ErrorCode ===========================================
+
+
+class ErrorCode(IntEnum):
+    SUCCESS = 0b000000000000000000000000000000000
+    OVERFLOW_CANDIDATE_CONTACTS = 0b00000000000000000000000000000001
+    OVERFLOW_COLLISION_PAIRS = 0b00000000000000000000000000000010
+    OVERFLOW_HIBERNATION_ISLANDS = 0b00000000000000000000000000000100
+    INVALID_FORCE_NAN = 0b00000000000000000000000000001000
+    INVALID_ACC_NAN = 0b00000000000000000000000000010000
+
+
 # =========================================== RigidGlobalInfo ===========================================