diff --git a/genesis/engine/couplers/ipc_coupler/coupler.py b/genesis/engine/couplers/ipc_coupler/coupler.py index 0ce8e50dc5..71244af651 100644 --- a/genesis/engine/couplers/ipc_coupler/coupler.py +++ b/genesis/engine/couplers/ipc_coupler/coupler.py @@ -454,6 +454,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 +512,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 @@ -1123,14 +1150,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 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..384c1dd713 100644 --- a/tests/test_ipc.py +++ b/tests/test_ipc.py @@ -505,6 +505,152 @@ 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) + + 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() + 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) + robot_pure.set_dofs_velocity(qd_c) + @pytest.mark.required @pytest.mark.parametrize("n_envs", [0, 2]) @pytest.mark.parametrize("constraint_strength", [1, 100]) @@ -1489,3 +1635,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()