From ed15b45c01145b31cabe6d43cc987544ecae21db Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Mon, 2 Mar 2026 17:43:00 +0100 Subject: [PATCH 1/6] WIP --- genesis/engine/entities/fem_entity.py | 6 +- genesis/engine/solvers/fem_solver.py | 3 +- tests/test_ipc.py | 94 +++++++++++++++++++++++++++ 3 files changed, 99 insertions(+), 4 deletions(-) diff --git a/genesis/engine/entities/fem_entity.py b/genesis/engine/entities/fem_entity.py index 1c0452833d..7a3e5537b7 100644 --- a/genesis/engine/entities/fem_entity.py +++ b/genesis/engine/entities/fem_entity.py @@ -966,10 +966,10 @@ def remove_vertex_constraints(self, verts_idx_local=None, envs_idx=None): @qd.kernel def _kernel_get_verts_pos(self, f: qd.i32, pos: qd.types.ndarray(), verts_idx: qd.types.ndarray()): # get current position of vertices - for i_v, i_b in qd.ndrange(verts_idx.shape[0], verts_idx.shape[1]): - i_global = verts_idx[i_v, i_b] + self.v_start + for i_b, i_v_ in qd.ndrange(verts_idx.shape[0], verts_idx.shape[1]): + i_v = verts_idx[i_b, i_v_] + self.v_start for j in qd.static(range(3)): - pos[i_b, i_v, j] = self._solver.elements_v[f, i_global, i_b].pos[j] + pos[i_b, i_v_, j] = self._solver.elements_v[f, i_v, i_b].pos[j] def get_el2v(self): """ diff --git a/genesis/engine/solvers/fem_solver.py b/genesis/engine/solvers/fem_solver.py index 827f8c31bc..f4ae824234 100644 --- a/genesis/engine/solvers/fem_solver.py +++ b/genesis/engine/solvers/fem_solver.py @@ -967,7 +967,8 @@ def substep_pre_coupling(self, f): from genesis.engine.couplers import IPCCoupler if isinstance(self.sim._coupler, IPCCoupler): - pass # IPC coupler handles FEM simulation + if self._constraints_initialized: + self.apply_soft_constraints(f) elif self._use_implicit_solver: self.precompute_material_data(f) self.init_pos_and_inertia(f) diff --git a/tests/test_ipc.py b/tests/test_ipc.py index 32382e2aeb..326cd43e3a 100644 --- a/tests/test_ipc.py +++ b/tests/test_ipc.py @@ -1489,3 +1489,97 @@ def test_cloth_uniform_biaxial_stretching(E, nu, strech_scale, n_envs, show_view cloth_aabb_extent = cloth_aabb_max - cloth_aabb_min assert (cloth_aabb_extent[..., :2] < STRETCH_RATIO_1 * (2.0 * CLOTH_HALF)).all() assert ((0.001 < cloth_aabb_extent[..., 2]) & (cloth_aabb_extent[..., 2] < 0.1)).all() + + +@pytest.mark.required +@pytest.mark.parametrize("n_envs", [0, 2]) +@pytest.mark.parametrize("E, rho", [(1e4, 200), (5e4, 400)]) +def test_cloth_gravity_deflection(n_envs, E, rho, show_viewer): + """Cloth held at corners sags under gravity. Verify Hencky membrane deflection scaling.""" + DT = 0.01 + THICKNESS = 0.001 + GRAVITY = (0.0, 0.0, -9.8) + CLOTH_HALF = 0.5 + + scene = gs.Scene( + sim_options=gs.options.SimOptions( + dt=DT, + gravity=GRAVITY, + ), + coupler_options=gs.options.IPCCouplerOptions(), + viewer_options=gs.options.ViewerOptions( + camera_pos=(1.5, -1.5, 0.8), + camera_lookat=(0.0, 0.0, 0.0), + ), + show_viewer=show_viewer, + ) + + asset_path = get_hf_dataset(pattern="IPC/grid20x20.obj") + cloth = scene.add_entity( + morph=gs.morphs.Mesh( + file=f"{asset_path}/IPC/grid20x20.obj", + scale=2 * CLOTH_HALF, + pos=(0.0, 0.0, 0.0), + euler=(90, 0, 0), + ), + material=gs.materials.FEM.Cloth( + E=E, + nu=0.49, + rho=rho, + thickness=THICKNESS, + bending_stiffness=None, + friction_mu=0.8, + ), + ) + + # ============= REMOVE AFTER FIXING VERTEX SOFT CONSTRAINT BUG ============= + BOX_SIZE = 0.02 + GAP = 0.005 + + boxes = [] + for x_sign, y_sign in ((-1, -1), (-1, 1), (1, -1), (1, 1)): + for z_sign in (+1, -1): + box = scene.add_entity( + gs.morphs.Box( + size=(BOX_SIZE, BOX_SIZE, BOX_SIZE), + pos=( + x_sign * (CLOTH_HALF - BOX_SIZE), + y_sign * (CLOTH_HALF - BOX_SIZE), + z_sign * (0.5 * BOX_SIZE + GAP), + ), + ), + material=gs.materials.Rigid( + coupling_type="two_way_soft_constraint", + coup_friction=0.8, + ), + surface=gs.surfaces.Plastic( + color=np.random.rand(3), + ), + ) + boxes.append(box) + # ========================================================================== + + scene.build(n_envs=n_envs) + + # Attach the corner vertices + cloth_positions = tensor_to_array(cloth.get_state().pos) + verts_idx_local = [] + for x_sign, y_sign in ((-1, -1), (-1, 1), (1, -1), (1, 1)): + corner_pos = (x_sign * CLOTH_HALF, y_sign * CLOTH_HALF, 0.0) + corner_idx = np.argmin(np.linalg.norm(cloth_positions - corner_pos, axis=-1), axis=-1) + verts_idx_local.append(corner_idx) + verts_idx_local = np.stack(verts_idx_local, axis=-1) + # FIXME: This is not working with IPC for now. + # cloth.set_vertex_constraints(verts_idx_local, stiffness=1e4, is_soft_constraint=True) + + # ============= REMOVE AFTER FIXING VERTEX SOFT CONSTRAINT BUG ============= + for box in boxes: + box.set_dofs_kp(2000.0) + box.set_dofs_kv(500.0) + init_dof = tensor_to_array(box.get_dofs_position()) + init_dof[..., 2] = 0.0 + box.control_dofs_position(init_dof) + # ========================================================================== + + for _ in range(50): + scene.step() From 7aeabd97cc8f5782e0f47df0de0b6189895c4f40 Mon Sep 17 00:00:00 2001 From: alan-ray Date: Tue, 3 Mar 2026 13:37:24 +0100 Subject: [PATCH 2/6] add more ipc tests --- .../engine/couplers/ipc_coupler/coupler.py | 71 +++++++-- tests/test_ipc.py | 149 ++++++++++++++++++ 2 files changed, 211 insertions(+), 9 deletions(-) diff --git a/genesis/engine/couplers/ipc_coupler/coupler.py b/genesis/engine/couplers/ipc_coupler/coupler.py index 0ce8e50dc5..16646dab61 100644 --- a/genesis/engine/couplers/ipc_coupler/coupler.py +++ b/genesis/engine/couplers/ipc_coupler/coupler.py @@ -176,6 +176,8 @@ def __init__(self, simulator: "Simulator", options: IPCCouplerOptions) -> None: self._articulation_non_fixed_base_entities: list["RigidEntity"] = [] # entities with non-fixed base self.articulation_data: dict["RigidEntity", ArticulatedEntityData] = {} + uipc.Logger.set_level(uipc.Logger.Level.Info) + # ============================================================ # Section 1: Configuration API # ============================================================ @@ -454,6 +456,29 @@ def _add_rigid_geoms_to_ipc(self) -> None: meshes.append(mesh) + # If this link has no collision geoms, use visual meshes so the link still gets an ABD slot + # (e.g. for external_articulation joints whose child link has only visual geometry). + if not source_link.geoms: + for vgeom in source_link.vgeoms: + if vgeom.n_vverts: + vgeom_verts = gu.transform_by_trans_quat( + np.asarray(vgeom.init_vverts, dtype=np.float64), + vgeom.init_pos, + vgeom.init_quat, + ) + if source_link is not target_link: + vgeom_verts = gu.transform_by_trans_quat( + vgeom_verts, *merge_transforms[source_link] + ) + try: + mesh = uipc.geometry.trimesh( + vgeom_verts.astype(np.float64, copy=False), + np.asarray(vgeom.init_vfaces, dtype=np.int32), + ) + except RuntimeError as e: + gs.raise_exception_from(f"Failed to process vgeom {vgeom.idx} for IPC.", e) + meshes.append(mesh) + if not meshes: continue @@ -489,7 +514,11 @@ def _add_rigid_geoms_to_ipc(self) -> None: self._ipc_abd = AffineBodyConstitution() self._ipc_constitution_tabular.insert(self._ipc_abd) - self._ipc_abd.apply_to(merged_mesh, kappa=ABD_KAPPA * uipc.unit.MPa, mass_density=entity.material.rho) + if(uipc.geometry.is_trimesh_closed(merged_mesh)): + self._ipc_abd.apply_to(merged_mesh, kappa=ABD_KAPPA * uipc.unit.MPa, mass_density=entity.material.rho) + else: + abd_shell = uipc.constitution.AffineBodyShell() + abd_shell.apply_to(merged_mesh, kappa=ABD_KAPPA * uipc.unit.MPa, mass_density=entity.material.rho, thickness=0.001) # Determine coupling behavior is_ipc_only = entity_coupling_type == COUPLING_TYPE.IPC_ONLY @@ -1092,6 +1121,22 @@ def _retrieve_rigid_states(self): self._abd_data_by_link[link][env_idx].aim_transform[:] = aim_transform self._abd_data_by_link[link][env_idx].velocity[:] = velocities[abd_body_idx] + # ---------------------------------------------------------------------------------------------------------------------- + # Debug: difference between Genesis solved transform (FK) and IPC result + pos_aim = np.asarray(aim_transform[:3, 3]) + pos_ipc = np.asarray(transform_matrix[:3, 3]) + quat_aim = gu.R_to_quat(np.asarray(aim_transform[:3, :3])) + quat_ipc = gu.R_to_quat(np.asarray(transform_matrix[:3, :3])) + pos_diff = float(np.linalg.norm(pos_ipc - pos_aim)) + qa = np.ravel(quat_aim) / np.linalg.norm(quat_aim) + qi = np.ravel(quat_ipc) / np.linalg.norm(quat_ipc) + quat_diff = 1.0 - abs(np.dot(qa, qi).item()) + gs.logger.info( + f" [coupler] {link.name} env={env_idx}: Genesis(FK) vs IPC pos_diff={pos_diff:.9e} quat_diff={quat_diff:.9e}" + ) + # ----------------------------------------------------------------------------------------------------------------------- + + link_idx_local = self.coupling_data.link_to_idx_local[link] self.coupling_data.ipc_transforms[env_idx, link_idx_local] = transform_matrix self.coupling_data.aim_transforms[env_idx, link_idx_local] = aim_transform @@ -1123,14 +1168,8 @@ def _store_gs_rigid_states(self): def _apply_abd_coupling_forces(self): """ - Apply coupling forces from IPC ABD constraint to Genesis rigid bodies. - - Data has already been populated in data by _retrieve_rigid_states, so this function computes forces and applies - the results. - - This ensures action-reaction force consistency: - - IPC constraint force: G_ipc = M * (q_ipc^{n+1} - q_genesis^n) - - Genesis reaction force: F_genesis = M * (q_ipc^{n+1} - q_genesis^n) = G_ipc + Apply coupling from IPC ABD state back to Genesis rigid bodies (two_way_soft_constraint). + Computes coupling forces/torques and applies them as external force/torque. """ if ( not self.options.two_way_coupling @@ -1151,6 +1190,20 @@ def _apply_abd_coupling_forces(self): self.coupling_data.out_torques, ) + # ---------------------------------------------------------------------------------------------------------------------- + # Debug: print force and torque per link (if diff is zero, these should be ~0) + links_in_order = [ + link for link, _ in sorted(self.coupling_data.link_to_idx_local.items(), key=lambda x: x[1]) + ] + for env_idx in range(self.coupling_data.out_forces.shape[0]): + for i, link in enumerate(links_in_order): + f = self.coupling_data.out_forces[env_idx, i] + t = self.coupling_data.out_torques[env_idx, i] + gs.logger.info( + f" [coupler] {link.name} env={env_idx}: force=[{f[0]:.9e} {f[1]:.9e} {f[2]:.9e}] | torque=[{t[0]:.9e} {t[1]:.9e} {t[2]:.9e}]" + ) + # ---------------------------------------------------------------------------------------------------------------------- + if np.isnan(self.coupling_data.out_forces).any() or np.isnan(self.coupling_data.out_torques).any(): gs.raise_exception( "Invalid coupling forces/torques causing 'nan'. This indicates numerical instability. Please decrease " diff --git a/tests/test_ipc.py b/tests/test_ipc.py index 326cd43e3a..9f7e958412 100644 --- a/tests/test_ipc.py +++ b/tests/test_ipc.py @@ -505,6 +505,155 @@ def test_single_joint(n_envs, coupling_type, joint_type, fixed, show_viewer): assert (dist_min < 1.5 * CONTACT_MARGIN).all() +# 12 leg joints for Anymal C (same order as examples IPC_Solver ipc_floating_anymal*.py) +_ANYMAL_JOINT_NAMES = ( + "RH_HAA", + "LH_HAA", + "RF_HAA", + "LF_HAA", + "RH_HFE", + "LH_HFE", + "RF_HFE", + "LF_HFE", + "RH_KFE", + "LH_KFE", + "RF_KFE", + "LF_KFE", +) + +_MAX_STEPS_FLOATING_ANYMAL = 5 + +@pytest.mark.required +@pytest.mark.parametrize("n_envs", [1]) +@pytest.mark.parametrize( + "fixed,coupling_type", + [ + (True, "two_way_soft_constraint"), + (True, "external_articulation"), + (False, "two_way_soft_constraint"), + pytest.param( + False, + "external_articulation", + marks=pytest.mark.skip(reason="external_articulation does not support floating base"), + ), + ], +) +def test_anymal_coupled_vs_pure(n_envs, fixed, coupling_type, show_viewer): + """ + IPC-coupled Anymal vs pure Genesis: same random joint commands; every frame we sync + coupled state (qpos, qd) to the pure robot and check error is zero (sync round-trip). + Covers fixed base (both coupling types) and floating base (two_way_soft_constraint only). + """ + from genesis.engine.entities import RigidEntity + + np.random.seed(42) + if fixed: + coupler_opts = gs.options.IPCCouplerOptions( + constraint_strength_translation=10000.0, + constraint_strength_rotation=10000.0, + enable_rigid_rigid_contact=False, + enable_rigid_ground_contact=False, + newton_tolerance=1e-8, + newton_translation_tolerance=1.0, + linear_system_tolerance=1e-8, + newton_semi_implicit_enable=False, + enable_rigid_dofs_sync=True, + two_way_coupling=True, + ) + else: + coupler_opts = gs.options.IPCCouplerOptions( + constraint_strength_translation=1.0, + constraint_strength_rotation=1.0, + enable_rigid_rigid_contact=False, + enable_rigid_ground_contact=False, + newton_tolerance=1e-10, + newton_translation_tolerance=1e-4, + linear_system_tolerance=1e-12, + newton_semi_implicit_enable=False, + enable_rigid_dofs_sync=True, + two_way_coupling=True, + ) + scene = gs.Scene( + sim_options=gs.options.SimOptions( + dt=0.01, + gravity=(0.0, 0.0, 0.0), + ), + rigid_options=gs.options.RigidOptions( + enable_collision=False, + ), + coupler_options=coupler_opts, + viewer_options=gs.options.ViewerOptions( + camera_pos=(3.0, 1.5, 1.2), + camera_lookat=(0.25, 0.0, 0.5), + camera_fov=40, + ), + show_viewer=show_viewer, + ) + + # keep them in the same position to compare the results + robot_coupled = scene.add_entity( + gs.morphs.URDF( + file="urdf/anymal_c/urdf/anymal_c.urdf", + pos=(0.0, 0.0, 0.5), + fixed=fixed, + ), + material=gs.materials.Rigid( + coupling_type=coupling_type, + coup_friction=0.0, + ), + ) + robot_pure = scene.add_entity( + gs.morphs.URDF( + file="urdf/anymal_c/urdf/anymal_c.urdf", + pos=(0.0, 0.0, 0.5), + fixed=fixed, + ), + material=gs.materials.Rigid(coupling_type=None), + ) + assert isinstance(robot_coupled, RigidEntity) + assert isinstance(robot_pure, RigidEntity) + + scene.build(n_envs=n_envs) + assert scene.sim is not None + + motors_dof_idx_coupled = [robot_coupled.get_joint(name).dofs_idx_local[0] for name in _ANYMAL_JOINT_NAMES] + motors_dof_idx_pure = [robot_pure.get_joint(name).dofs_idx_local[0] for name in _ANYMAL_JOINT_NAMES] + robot_coupled.set_dofs_kp(1000.0, dofs_idx_local=motors_dof_idx_coupled) + robot_coupled.set_dofs_kv(50.0, dofs_idx_local=motors_dof_idx_coupled) + robot_pure.set_dofs_kp(1000.0, dofs_idx_local=motors_dof_idx_pure) + robot_pure.set_dofs_kv(50.0, dofs_idx_local=motors_dof_idx_pure) + + n_envs_actual = max(n_envs, 1) + low_limit, high_limit = robot_coupled.get_dofs_limit(dofs_idx_local=motors_dof_idx_coupled) + low = tensor_to_array(low_limit) + high = tensor_to_array(high_limit) + default_bounds = (-2.0 * np.pi, 2.0 * np.pi) + low = np.where(np.isfinite(low), low, default_bounds[0]) + high = np.where(np.isfinite(high), high, default_bounds[1]) + high = np.maximum(high, low) + if low.ndim == 1: + low = np.broadcast_to(low, (n_envs_actual, len(motors_dof_idx_coupled))) + high = np.broadcast_to(high, (n_envs_actual, len(motors_dof_idx_coupled))) + target_joints = np.random.uniform(low, high, (n_envs_actual, len(motors_dof_idx_coupled))).astype(gs.np_float) + + for _ in range(_MAX_STEPS_FLOATING_ANYMAL): + target_joints[:] = np.random.uniform(low, high, (n_envs_actual, len(motors_dof_idx_coupled))) + robot_coupled.control_dofs_position(target_joints, dofs_idx_local=motors_dof_idx_coupled) + robot_pure.control_dofs_position(target_joints, dofs_idx_local=motors_dof_idx_pure) + scene.step() + + + qpos_c = tensor_to_array(robot_coupled.get_qpos()) + qd_c = tensor_to_array(robot_coupled.get_dofs_velocity()) + qpos_p = tensor_to_array(robot_pure.get_qpos()) + qd_p = tensor_to_array(robot_pure.get_dofs_velocity()) + assert_allclose(qpos_p, qpos_c, atol=TOL_SINGLE) + assert_allclose(qd_p, qd_c, atol=TOL_SINGLE) + + # Sync coupled result to pure Genesis + robot_pure.set_qpos(qpos_c, zero_velocity=False) + robot_pure.set_dofs_velocity(qd_c) + @pytest.mark.required @pytest.mark.parametrize("n_envs", [0, 2]) @pytest.mark.parametrize("constraint_strength", [1, 100]) From 2fa3e9653745153712d16d046ec3c27d044952a8 Mon Sep 17 00:00:00 2001 From: alan-ray Date: Tue, 3 Mar 2026 15:04:58 +0100 Subject: [PATCH 3/6] IPC: add test_anymal_coupled_vs_pure, visual-only links use no contact Made-with: Cursor --- tests/test_ipc.py | 51 ++++++++++++++++++++++------------------------- 1 file changed, 24 insertions(+), 27 deletions(-) diff --git a/tests/test_ipc.py b/tests/test_ipc.py index 9f7e958412..384c1dd713 100644 --- a/tests/test_ipc.py +++ b/tests/test_ipc.py @@ -547,32 +547,20 @@ def test_anymal_coupled_vs_pure(n_envs, fixed, coupling_type, show_viewer): from genesis.engine.entities import RigidEntity np.random.seed(42) - if fixed: - coupler_opts = gs.options.IPCCouplerOptions( - constraint_strength_translation=10000.0, - constraint_strength_rotation=10000.0, - enable_rigid_rigid_contact=False, - enable_rigid_ground_contact=False, - newton_tolerance=1e-8, - newton_translation_tolerance=1.0, - linear_system_tolerance=1e-8, - newton_semi_implicit_enable=False, - enable_rigid_dofs_sync=True, - two_way_coupling=True, - ) - else: - coupler_opts = gs.options.IPCCouplerOptions( - constraint_strength_translation=1.0, - constraint_strength_rotation=1.0, - enable_rigid_rigid_contact=False, - enable_rigid_ground_contact=False, - newton_tolerance=1e-10, - newton_translation_tolerance=1e-4, - linear_system_tolerance=1e-12, - newton_semi_implicit_enable=False, - enable_rigid_dofs_sync=True, - two_way_coupling=True, - ) + + coupler_opts = gs.options.IPCCouplerOptions( + constraint_strength_translation=1.0, + constraint_strength_rotation=1.0, + enable_rigid_rigid_contact=False, + enable_rigid_ground_contact=False, + newton_tolerance=1e-10, + newton_translation_tolerance=1e-4, + linear_system_tolerance=1e-12, + newton_semi_implicit_enable=False, + enable_rigid_dofs_sync=True, + two_way_coupling=True, + ) + scene = gs.Scene( sim_options=gs.options.SimOptions( dt=0.01, @@ -634,6 +622,7 @@ def test_anymal_coupled_vs_pure(n_envs, fixed, coupling_type, show_viewer): if low.ndim == 1: low = np.broadcast_to(low, (n_envs_actual, len(motors_dof_idx_coupled))) high = np.broadcast_to(high, (n_envs_actual, len(motors_dof_idx_coupled))) + target_joints = np.random.uniform(low, high, (n_envs_actual, len(motors_dof_idx_coupled))).astype(gs.np_float) for _ in range(_MAX_STEPS_FLOATING_ANYMAL): @@ -641,14 +630,22 @@ def test_anymal_coupled_vs_pure(n_envs, fixed, coupling_type, show_viewer): robot_coupled.control_dofs_position(target_joints, dofs_idx_local=motors_dof_idx_coupled) robot_pure.control_dofs_position(target_joints, dofs_idx_local=motors_dof_idx_pure) scene.step() + print(f"frame={_}") - qpos_c = tensor_to_array(robot_coupled.get_qpos()) qd_c = tensor_to_array(robot_coupled.get_dofs_velocity()) + acc_c = tensor_to_array(robot_coupled.get_links_acc()) + acc_ang_c = tensor_to_array(robot_coupled.get_links_acc_ang()) + qpos_p = tensor_to_array(robot_pure.get_qpos()) qd_p = tensor_to_array(robot_pure.get_dofs_velocity()) + acc_p = tensor_to_array(robot_pure.get_links_acc()) + acc_ang_p = tensor_to_array(robot_pure.get_links_acc_ang()) + assert_allclose(qpos_p, qpos_c, atol=TOL_SINGLE) assert_allclose(qd_p, qd_c, atol=TOL_SINGLE) + assert_allclose(acc_c, acc_p, atol=TOL_SINGLE) + assert_allclose(acc_ang_c, acc_ang_p, atol=TOL_SINGLE) # Sync coupled result to pure Genesis robot_pure.set_qpos(qpos_c, zero_velocity=False) From 647eb709e8845c8af1ef2ccea50a2167b2a9761c Mon Sep 17 00:00:00 2001 From: alan-ray Date: Tue, 3 Mar 2026 15:07:48 +0100 Subject: [PATCH 4/6] remove uipc logger --- genesis/engine/couplers/ipc_coupler/coupler.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/genesis/engine/couplers/ipc_coupler/coupler.py b/genesis/engine/couplers/ipc_coupler/coupler.py index 16646dab61..3575cb3767 100644 --- a/genesis/engine/couplers/ipc_coupler/coupler.py +++ b/genesis/engine/couplers/ipc_coupler/coupler.py @@ -176,8 +176,6 @@ def __init__(self, simulator: "Simulator", options: IPCCouplerOptions) -> None: self._articulation_non_fixed_base_entities: list["RigidEntity"] = [] # entities with non-fixed base self.articulation_data: dict["RigidEntity", ArticulatedEntityData] = {} - uipc.Logger.set_level(uipc.Logger.Level.Info) - # ============================================================ # Section 1: Configuration API # ============================================================ From ba1823b826049cd3f1ec6ba8142d71791e2066c0 Mon Sep 17 00:00:00 2001 From: alan-ray Date: Tue, 3 Mar 2026 15:42:25 +0100 Subject: [PATCH 5/6] remove debug info --- .../engine/couplers/ipc_coupler/coupler.py | 34 ++----------------- 1 file changed, 2 insertions(+), 32 deletions(-) diff --git a/genesis/engine/couplers/ipc_coupler/coupler.py b/genesis/engine/couplers/ipc_coupler/coupler.py index 3575cb3767..ae4b9d21cc 100644 --- a/genesis/engine/couplers/ipc_coupler/coupler.py +++ b/genesis/engine/couplers/ipc_coupler/coupler.py @@ -116,8 +116,8 @@ def __init__(self, simulator: "Simulator", options: IPCCouplerOptions) -> None: self.rigid_solver: "RigidSolver" = self.sim.rigid_solver self.fem_solver: "FEMSolver" = self.sim.fem_solver - self._constraint_strength_translation_scaled = self.options.constraint_strength_translation / self.sim.dt**2 - self._constraint_strength_rotation_scaled = self.options.constraint_strength_rotation / self.sim.dt**2 + self._constraint_strength_translation_scaled = 1.0 #self.options.constraint_strength_translation / self.sim.dt**2 + self._constraint_strength_rotation_scaled = 1.0 #self.options.constraint_strength_rotation / self.sim.dt**2 # ==== IPC System Infrastructure ==== self._ipc_engine: Engine | None = None @@ -1119,22 +1119,6 @@ def _retrieve_rigid_states(self): self._abd_data_by_link[link][env_idx].aim_transform[:] = aim_transform self._abd_data_by_link[link][env_idx].velocity[:] = velocities[abd_body_idx] - # ---------------------------------------------------------------------------------------------------------------------- - # Debug: difference between Genesis solved transform (FK) and IPC result - pos_aim = np.asarray(aim_transform[:3, 3]) - pos_ipc = np.asarray(transform_matrix[:3, 3]) - quat_aim = gu.R_to_quat(np.asarray(aim_transform[:3, :3])) - quat_ipc = gu.R_to_quat(np.asarray(transform_matrix[:3, :3])) - pos_diff = float(np.linalg.norm(pos_ipc - pos_aim)) - qa = np.ravel(quat_aim) / np.linalg.norm(quat_aim) - qi = np.ravel(quat_ipc) / np.linalg.norm(quat_ipc) - quat_diff = 1.0 - abs(np.dot(qa, qi).item()) - gs.logger.info( - f" [coupler] {link.name} env={env_idx}: Genesis(FK) vs IPC pos_diff={pos_diff:.9e} quat_diff={quat_diff:.9e}" - ) - # ----------------------------------------------------------------------------------------------------------------------- - - link_idx_local = self.coupling_data.link_to_idx_local[link] self.coupling_data.ipc_transforms[env_idx, link_idx_local] = transform_matrix self.coupling_data.aim_transforms[env_idx, link_idx_local] = aim_transform @@ -1188,20 +1172,6 @@ def _apply_abd_coupling_forces(self): self.coupling_data.out_torques, ) - # ---------------------------------------------------------------------------------------------------------------------- - # Debug: print force and torque per link (if diff is zero, these should be ~0) - links_in_order = [ - link for link, _ in sorted(self.coupling_data.link_to_idx_local.items(), key=lambda x: x[1]) - ] - for env_idx in range(self.coupling_data.out_forces.shape[0]): - for i, link in enumerate(links_in_order): - f = self.coupling_data.out_forces[env_idx, i] - t = self.coupling_data.out_torques[env_idx, i] - gs.logger.info( - f" [coupler] {link.name} env={env_idx}: force=[{f[0]:.9e} {f[1]:.9e} {f[2]:.9e}] | torque=[{t[0]:.9e} {t[1]:.9e} {t[2]:.9e}]" - ) - # ---------------------------------------------------------------------------------------------------------------------- - if np.isnan(self.coupling_data.out_forces).any() or np.isnan(self.coupling_data.out_torques).any(): gs.raise_exception( "Invalid coupling forces/torques causing 'nan'. This indicates numerical instability. Please decrease " From 513ae5e1e76733890c04b105160a5b51435bf959 Mon Sep 17 00:00:00 2001 From: alan-ray Date: Tue, 3 Mar 2026 15:45:57 +0100 Subject: [PATCH 6/6] recover the scaling of force --- genesis/engine/couplers/ipc_coupler/coupler.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/genesis/engine/couplers/ipc_coupler/coupler.py b/genesis/engine/couplers/ipc_coupler/coupler.py index ae4b9d21cc..71244af651 100644 --- a/genesis/engine/couplers/ipc_coupler/coupler.py +++ b/genesis/engine/couplers/ipc_coupler/coupler.py @@ -116,8 +116,8 @@ def __init__(self, simulator: "Simulator", options: IPCCouplerOptions) -> None: self.rigid_solver: "RigidSolver" = self.sim.rigid_solver self.fem_solver: "FEMSolver" = self.sim.fem_solver - self._constraint_strength_translation_scaled = 1.0 #self.options.constraint_strength_translation / self.sim.dt**2 - self._constraint_strength_rotation_scaled = 1.0 #self.options.constraint_strength_rotation / self.sim.dt**2 + self._constraint_strength_translation_scaled = self.options.constraint_strength_translation / self.sim.dt**2 + self._constraint_strength_rotation_scaled = self.options.constraint_strength_rotation / self.sim.dt**2 # ==== IPC System Infrastructure ==== self._ipc_engine: Engine | None = None