Skip to content

Commit f01bb28

Browse files
committed
Refactor: unify IPC coupling API and update examples
Replace set_ipc_link_filter() and set_link_only_in_ipc() with a single set_link_ipc_coupling_type() API that supports three coupling modes: - 'both': Two-way coupling (IPC <-> Genesis, default behavior) - 'ipc_only': One-way coupling (IPC -> Genesis, now supports non-base links) - 'genesis_only': Exclude links from IPC entirely Key improvements: - Unified interface for all coupling configurations - Per-link coupling control via link_names/link_indices - Support non-base links in ipc_only mode - Better flexibility for mixed coupling scenarios - Update examples to use new API
1 parent e9c80c7 commit f01bb28

File tree

3 files changed

+126
-88
lines changed

3 files changed

+126
-88
lines changed

examples/IPC_Solver/ipc_arm_cloth.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -110,8 +110,9 @@ def build_scene(use_ipc=False, show_viewer=False, enable_ipc_gui=False):
110110
euler=(0, 0, 0),
111111
),
112112
)
113-
scene.sim.coupler.set_ipc_link_filter(
113+
scene.sim.coupler.set_link_ipc_coupling_type(
114114
entity=entities["robot"],
115+
coupling_type="both",
115116
link_names=["left_finger", "right_finger"],
116117
)
117118

examples/IPC_Solver/ipc_grasp.py

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -47,8 +47,9 @@ def main():
4747
)
4848

4949
if args.ipc:
50-
scene.sim.coupler.set_ipc_link_filter(
50+
scene.sim.coupler.set_link_ipc_coupling_type(
5151
entity=franka,
52+
coupling_type="both",
5253
link_names=["left_finger", "right_finger"],
5354
)
5455

genesis/engine/couplers/ipc_coupler.py

Lines changed: 122 additions & 86 deletions
Original file line numberDiff line numberDiff line change
@@ -641,84 +641,101 @@ def is_active(self) -> bool:
641641
"""Check if IPC coupling is active"""
642642
return self._ipc_world is not None
643643

644-
def set_ipc_link_filter(self, entity, link_names=None, link_indices=None):
644+
def set_link_ipc_coupling_type(self, entity, coupling_type: str, link_names=None, link_indices=None):
645645
"""
646-
Set which links of an entity should participate in IPC simulation.
646+
Set IPC coupling type for links of an entity.
647647
648648
Parameters
649649
----------
650650
entity : RigidEntity
651-
The rigid entity to set the filter for
651+
The rigid entity to configure
652+
coupling_type : str
653+
Type of coupling: 'both', 'ipc_only', or 'genesis_only'
654+
- 'both': Two-way coupling between IPC and Genesis (default behavior)
655+
- 'ipc_only': Links only simulated in IPC, transforms copied to Genesis (one-way)
656+
- 'genesis_only': Links only simulated in Genesis, excluded from IPC
652657
link_names : list of str, optional
653-
Names of links to include in IPC. If None and link_indices is None, all links participate.
658+
Names of links to configure. If None and link_indices is None, applies to all links.
654659
link_indices : list of int, optional
655-
Local indices of links to include in IPC. If None and link_names is None, all links participate.
656-
"""
657-
if link_names is None and link_indices is None:
658-
# Remove filter for this entity (all links participate)
659-
if entity._idx in self._ipc_link_filters:
660-
del self._ipc_link_filters[entity._idx]
661-
return
662-
663-
link_filter = set()
664-
665-
if link_names is not None:
666-
# Convert link names to solver-level indices
667-
for name in link_names:
668-
link = entity.get_link(name=name)
669-
if link is not None:
670-
# Use solver-level index
671-
link_filter.add(link.idx)
672-
else:
673-
gs.logger.warning(f"Link name '{name}' not found in entity")
674-
675-
if link_indices is not None:
676-
# Convert local link indices to solver-level indices
677-
for local_idx in link_indices:
678-
solver_link_idx = local_idx + entity._link_start
679-
link_filter.add(solver_link_idx)
680-
681-
# Store filter for this entity
682-
self._ipc_link_filters[entity._idx] = link_filter
683-
684-
def set_link_only_in_ipc(self, entity):
685-
"""
686-
Mark an entity to only exist in IPC (not simulated in Genesis).
660+
Local indices of links to configure. If None and link_names is None, applies to all links.
687661
688-
The entity must have only a base link (single rigid body, no joints).
689-
690-
These links will:
691-
- Not have SoftTransformConstraint in IPC (no coupling forces)
692-
- Use full density in IPC (not divided by 2)
693-
- Directly set Genesis transform to IPC result (one-way: IPC -> Genesis)
694-
- Skip Genesis rigid body solver (if possible)
695-
696-
Parameters
697-
----------
698-
entity : RigidEntity
699-
The rigid entity that should only exist in IPC (must have only base link)
662+
Notes
663+
-----
664+
- 'both': Links use half density in IPC, have SoftTransformConstraint, bidirectional forces
665+
- 'ipc_only': Links use full density in IPC, no SoftTransformConstraint, transforms copied to Genesis
666+
- 'genesis_only': Links excluded from IPC simulation entirely
700667
"""
701668
entity_idx = entity._idx
702669

703-
# Assert that the entity only has a base link (no joints/child links)
704-
n_links = entity.n_links
705-
assert n_links == 1, (
706-
f"IPC-only entities must have only a base link (single rigid body). "
707-
f"Entity {entity_idx} has {n_links} links. "
708-
f"IPC-only mode is designed for simple rigid bodies without articulated joints."
709-
)
710-
711-
# Get the base link index
712-
base_link_idx = entity.base_link_idx
713-
link_indices = {base_link_idx}
714-
715-
# Store IPC-only links for this entity
716-
self._ipc_only_links[entity_idx] = link_indices
717-
718-
# Also add to IPC link filter to ensure these links are included in IPC
719-
self._ipc_link_filters[entity_idx] = link_indices
720-
721-
gs.logger.info(f"Entity {entity_idx} marked as IPC-only (base link only: {base_link_idx})")
670+
# Determine which links to configure
671+
if link_names is None and link_indices is None:
672+
# Apply to all links
673+
target_links = set()
674+
for local_idx in range(entity.n_links):
675+
solver_link_idx = local_idx + entity._link_start
676+
target_links.add(solver_link_idx)
677+
else:
678+
# Apply to specified links
679+
target_links = set()
680+
681+
if link_names is not None:
682+
for name in link_names:
683+
link = entity.get_link(name=name)
684+
if link is not None:
685+
target_links.add(link.idx)
686+
else:
687+
gs.logger.warning(f"Link name '{name}' not found in entity")
688+
689+
if link_indices is not None:
690+
for local_idx in link_indices:
691+
solver_link_idx = local_idx + entity._link_start
692+
target_links.add(solver_link_idx)
693+
694+
# Apply coupling type
695+
if coupling_type == 'both':
696+
# Two-way coupling: include in IPC, not in IPC-only
697+
self._ipc_link_filters[entity_idx] = target_links
698+
699+
# Remove from IPC-only if present
700+
if entity_idx in self._ipc_only_links:
701+
self._ipc_only_links[entity_idx] -= target_links
702+
if not self._ipc_only_links[entity_idx]:
703+
del self._ipc_only_links[entity_idx]
704+
705+
gs.logger.info(f"Entity {entity_idx}: {len(target_links)} link(s) set to 'both' coupling")
706+
707+
elif coupling_type == 'ipc_only':
708+
# One-way coupling: IPC -> Genesis
709+
if entity_idx not in self._ipc_only_links:
710+
self._ipc_only_links[entity_idx] = set()
711+
self._ipc_only_links[entity_idx].update(target_links)
712+
713+
# Also add to IPC link filter
714+
if entity_idx not in self._ipc_link_filters:
715+
self._ipc_link_filters[entity_idx] = set()
716+
self._ipc_link_filters[entity_idx].update(target_links)
717+
718+
gs.logger.info(f"Entity {entity_idx}: {len(target_links)} link(s) set to 'ipc_only' coupling")
719+
720+
elif coupling_type == 'genesis_only':
721+
# Genesis-only: remove from both filters
722+
if entity_idx in self._ipc_link_filters:
723+
self._ipc_link_filters[entity_idx] -= target_links
724+
if not self._ipc_link_filters[entity_idx]:
725+
del self._ipc_link_filters[entity_idx]
726+
727+
if entity_idx in self._ipc_only_links:
728+
self._ipc_only_links[entity_idx] -= target_links
729+
if not self._ipc_only_links[entity_idx]:
730+
del self._ipc_only_links[entity_idx]
731+
732+
gs.logger.info(f"Entity {entity_idx}: {len(target_links)} link(s) set to 'genesis_only' (excluded from IPC)")
733+
734+
else:
735+
raise ValueError(
736+
f"Invalid coupling_type '{coupling_type}'. "
737+
f"Must be 'both', 'ipc_only', or 'genesis_only'."
738+
)
722739

723740
def preprocess(self, f):
724741
"""Preprocessing step before coupling"""
@@ -958,8 +975,6 @@ def _set_genesis_transforms_from_ipc(self, abd_data_by_link):
958975
"""
959976
For IPC-only links, directly set Genesis transform to IPC result (one-way coupling).
960977
961-
Note: IPC-only entities must have only a base link, so we only handle base links here.
962-
963978
Parameters
964979
----------
965980
abd_data_by_link : dict
@@ -1006,27 +1021,48 @@ def _set_genesis_transforms_from_ipc(self, abd_data_by_link):
10061021
pos_tensor = torch.as_tensor(pos, dtype=gs.tc_float, device=gs.device).unsqueeze(0) # (1, 3)
10071022
quat_tensor = torch.as_tensor(quat_wxyz, dtype=gs.tc_float, device=gs.device).unsqueeze(0) # (1, 4) [w, x, y, z]
10081023

1009-
# Create base links index tensor
1010-
base_links_idx = torch.tensor([link_idx], dtype=gs.tc_int, device=gs.device)
1024+
# Determine if this is a base link
1025+
is_base_link = (link_idx == entity.base_link_idx)
1026+
1027+
if is_base_link:
1028+
# Use base link methods for base links
1029+
base_links_idx = torch.tensor([link_idx], dtype=gs.tc_int, device=gs.device)
10111030

1012-
# Set base link transform using solver methods
1031+
if is_parallelized:
1032+
rigid_solver.set_base_links_pos(
1033+
pos_tensor, base_links_idx, envs_idx=env_idx, relative=False, unsafe=True, skip_forward=False
1034+
)
1035+
rigid_solver.set_base_links_quat(
1036+
quat_tensor, base_links_idx, envs_idx=env_idx, relative=False, unsafe=True, skip_forward=False
1037+
)
1038+
else:
1039+
rigid_solver.set_base_links_pos(
1040+
pos_tensor, base_links_idx, envs_idx=None, relative=False, unsafe=True, skip_forward=False
1041+
)
1042+
rigid_solver.set_base_links_quat(
1043+
quat_tensor, base_links_idx, envs_idx=None, relative=False, unsafe=True, skip_forward=False
1044+
)
1045+
else:
1046+
# Use regular link methods for non-base links
1047+
if is_parallelized:
1048+
rigid_solver.set_links_pos(
1049+
pos_tensor, links_idx=link_idx, envs_idx=env_idx, unsafe=True
1050+
)
1051+
rigid_solver.set_links_quat(
1052+
quat_tensor, links_idx=link_idx, envs_idx=env_idx, unsafe=True
1053+
)
1054+
else:
1055+
rigid_solver.set_links_pos(
1056+
pos_tensor, links_idx=link_idx, unsafe=True
1057+
)
1058+
rigid_solver.set_links_quat(
1059+
quat_tensor, links_idx=link_idx, unsafe=True
1060+
)
1061+
1062+
# Zero velocities after setting transform to avoid spurious forces
10131063
if is_parallelized:
1014-
rigid_solver.set_base_links_pos(
1015-
pos_tensor, base_links_idx, envs_idx=env_idx, relative=False, unsafe=True, skip_forward=False
1016-
)
1017-
rigid_solver.set_base_links_quat(
1018-
quat_tensor, base_links_idx, envs_idx=env_idx, relative=False, unsafe=True, skip_forward=False
1019-
)
1020-
# Zero velocities after setting transform to avoid spurious forces
10211064
entity.zero_all_dofs_velocity(envs_idx=env_idx, unsafe=True)
10221065
else:
1023-
rigid_solver.set_base_links_pos(
1024-
pos_tensor, base_links_idx, envs_idx=None, relative=False, unsafe=True, skip_forward=False
1025-
)
1026-
rigid_solver.set_base_links_quat(
1027-
quat_tensor, base_links_idx, envs_idx=None, relative=False, unsafe=True, skip_forward=False
1028-
)
1029-
# Zero velocities after setting transform to avoid spurious forces
10301066
entity.zero_all_dofs_velocity(envs_idx=None, unsafe=True)
10311067

10321068
except Exception as e:

0 commit comments

Comments
 (0)