diff --git a/docs/source/overview/sim/sim_articulation.md b/docs/source/overview/sim/sim_articulation.md index ecbc518d..f2edfc29 100644 --- a/docs/source/overview/sim/sim_articulation.md +++ b/docs/source/overview/sim/sim_articulation.md @@ -19,9 +19,41 @@ Articulations are configured using the {class}`~cfg.ArticulationCfg` dataclass. | `body_scale` | `List[float]` | `[1.0, 1.0, 1.0]` | Scaling factors for the articulation links. | | `disable_self_collisions` | `bool` | `True` | Whether to disable self-collisions. | | `drive_props` | `JointDrivePropertiesCfg` | `...` | Default drive properties. | -| `attrs` | `RigidBodyAttributesCfg` | `...` | Rigid body attributes configuration. | +| `attrs` | `RigidBodyAttributesCfg` | `...` | Default rigid body attributes applied to all links. | +| `link_attrs` | `dict[str, LinkPhysicsOverrideCfg]` | `None` | Optional per-link overrides keyed by group name; each group matches link names via regex. | +### Per-link physics (`link_attrs`) + +By default, `attrs` applies the same rigid-body physics to every link. Use `link_attrs` to +override specific links (matched by regex, same rules as joint drive dict keys): + +```python +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + LinkPhysicsOverrideCfg, + RigidBodyAttributesCfg, + RigidBodyAttributesOverrideCfg, +) + +art_cfg = ArticulationCfg( + fpath="path/to/robot.urdf", + attrs=RigidBodyAttributesCfg(static_friction=0.5), + link_attrs={ + "eef": LinkPhysicsOverrideCfg( + link_names_expr=[".*(hand|finger|ee).*"], + attrs=RigidBodyAttributesOverrideCfg( + static_friction=0.95, + contact_offset=0.001, + ), + ), + }, +) +``` + +At runtime, use `articulation.set_link_physical_attr(...)` and `get_link_physical_attr(...)` +for the same partial-override behavior. + ### Drive Configuration The `drive_props` parameter controls the joint physics behavior. It is defined using the `JointDrivePropertiesCfg` class. For articulation object without internal drive force, like cabinet and drawer, better set `drive_type` to `"none"`. diff --git a/embodichain/lab/sim/cfg.py b/embodichain/lab/sim/cfg.py index 0b10a725..4e4e0684 100644 --- a/embodichain/lab/sim/cfg.py +++ b/embodichain/lab/sim/cfg.py @@ -305,6 +305,106 @@ def from_dict( return cfg +@configclass +class RigidBodyAttributesOverrideCfg: + """Partial rigid-body attribute overrides for per-link physics configuration. + + Fields set to ``None`` are not applied and retain values from the base + :class:`RigidBodyAttributesCfg`. + """ + + mass: float | None = None + density: float | None = None + angular_damping: float | None = None + linear_damping: float | None = None + max_depenetration_velocity: float | None = None + sleep_threshold: float | None = None + min_position_iters: int | None = None + min_velocity_iters: int | None = None + max_linear_velocity: float | None = None + max_angular_velocity: float | None = None + enable_ccd: bool | None = None + contact_offset: float | None = None + rest_offset: float | None = None + enable_collision: bool | None = None + restitution: float | None = None + dynamic_friction: float | None = None + static_friction: float | None = None + + def merge_with(self, base: RigidBodyAttributesCfg) -> PhysicalAttr: + """Build a :class:`~dexsim.types.PhysicalAttr` from base values and overrides.""" + merged = RigidBodyAttributesCfg() + for field_name in merged.__dataclass_fields__: + override_val = getattr(self, field_name) + if override_val is not None: + setattr(merged, field_name, override_val) + else: + setattr(merged, field_name, getattr(base, field_name)) + return merged.attr() + + @classmethod + def from_dict( + cls, init_dict: Dict[str, Union[str, float, int, bool]] + ) -> RigidBodyAttributesOverrideCfg: + """Initialize the configuration from a dictionary.""" + cfg = cls() + for key, value in init_dict.items(): + if hasattr(cfg, key): + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + return cfg + + +@configclass +class LinkPhysicsOverrideCfg: + """Per-link physics override matched by regex on articulation link names.""" + + link_names_expr: list[str] = MISSING + """Regex patterns matched against link names (full match).""" + + attrs: RigidBodyAttributesOverrideCfg = RigidBodyAttributesOverrideCfg() + """Partial attribute overrides applied on top of :attr:`ArticulationCfg.attrs`.""" + + replace_inertial: bool = False + """Whether to recompute inertia when mass is overridden (DexSim flag).""" + + @classmethod + def from_dict(cls, init_dict: Dict[str, Any]) -> LinkPhysicsOverrideCfg: + """Initialize the configuration from a dictionary.""" + cfg = cls() + for key, value in init_dict.items(): + if key == "attrs" and isinstance(value, dict): + setattr(cfg, key, RigidBodyAttributesOverrideCfg.from_dict(value)) + elif hasattr(cfg, key): + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + return cfg + + +def link_attrs_from_dict( + value: dict[str, Any], +) -> dict[str, LinkPhysicsOverrideCfg]: + """Parse a ``link_attrs`` mapping from YAML/JSON-style dicts.""" + link_attrs: dict[str, LinkPhysicsOverrideCfg] = {} + for group_name, group_cfg in value.items(): + if isinstance(group_cfg, LinkPhysicsOverrideCfg): + link_attrs[group_name] = group_cfg + elif isinstance(group_cfg, dict): + link_attrs[group_name] = LinkPhysicsOverrideCfg.from_dict(group_cfg) + else: + raise TypeError( + f"link_attrs['{group_name}'] must be a dict or " + f"LinkPhysicsOverrideCfg, got {type(group_cfg)}." + ) + return link_attrs + + @configclass class SoftbodyVoxelAttributesCfg: # voxel config @@ -1263,6 +1363,13 @@ class ArticulationCfg(ObjectBaseCfg): The mass and density in attrs will only be used if specified. """ + link_attrs: dict[str, LinkPhysicsOverrideCfg] | None = None + """Named per-link physics override groups keyed by regex on link names. + + Each group applies :attr:`LinkPhysicsOverrideCfg.attrs` on top of :attr:`attrs` for + matched links only. A link must not match more than one group. + """ + fix_base: bool = True """Whether to fix the base of the articulation. @@ -1306,6 +1413,43 @@ class ArticulationCfg(ObjectBaseCfg): Only effective for USD files, ignored for URDF files. """ + @classmethod + def from_dict( + cls, init_dict: Dict[str, Union[str, float, tuple, dict]] + ) -> ArticulationCfg: + """Initialize the configuration from a dictionary.""" + cfg = cls() + for key, value in init_dict.items(): + if key == "link_attrs" and isinstance(value, dict): + cfg.link_attrs = link_attrs_from_dict(value) + elif hasattr(cfg, key): + attr = getattr(cfg, key) + if is_configclass(attr): + setattr(cfg, key, attr.from_dict(value)) + else: + setattr(cfg, key, value) + else: + logger.log_warning( + f"Key '{key}' not found in {cfg.__class__.__name__}." + ) + + if cfg.init_local_pose is None: + from scipy.spatial.transform import Rotation as R + + T = np.eye(4) + T[:3, 3] = np.array(cfg.init_pos) + T[:3, :3] = R.from_euler("xyz", np.deg2rad(cfg.init_rot)).as_matrix() + cfg.init_local_pose = T + else: + from scipy.spatial.transform import Rotation as R + + cfg.init_pos = tuple(cfg.init_local_pose[:3, 3]) + cfg.init_rot = tuple( + R.from_matrix(cfg.init_local_pose[:3, :3]).as_euler("xyz", degrees=True) + ) + + return cfg + @configclass class RobotCfg(ArticulationCfg): @@ -1354,7 +1498,9 @@ def from_dict(cls, init_dict: Dict[str, Union[str, float, tuple]]) -> RobotCfg: cfg = cls() # Create a new instance of the class (cls) for key, value in init_dict.items(): - if hasattr(cfg, key): + if key == "link_attrs" and isinstance(value, dict): + cfg.link_attrs = link_attrs_from_dict(value) + elif hasattr(cfg, key): attr = getattr(cfg, key) if key == "urdf_cfg": from embodichain.lab.sim.cfg import URDFCfg diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index b763bcc4..914f89db 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -31,7 +31,14 @@ from dexsim.engine import CudaArray, PhysicsScene from embodichain.lab.sim import VisualMaterialInst, VisualMaterial -from embodichain.lab.sim.cfg import ArticulationCfg, JointDrivePropertiesCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, + RigidBodyAttributesOverrideCfg, +) +from dexsim.types import PhysicalAttr +from embodichain.utils.string import resolve_matching_names from embodichain.lab.sim.common import BatchEntity from embodichain.utils.math import ( matrix_from_quat, @@ -1289,6 +1296,86 @@ def get_mass( ) return mass_tensor + def get_link_physical_attr( + self, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | None = None, + ) -> list[PhysicalAttr]: + """Get physical attributes for articulation links. + + Args: + link_names: Link names or regex patterns. If None, all links are returned. + env_ids: Environment indices. If None, only env 0 is queried. + + Returns: + List of :class:`~dexsim.types.PhysicalAttr`, one per (env, link) pair in + row-major order (env-major). + """ + if link_names is None: + matched_link_names = self.link_names + elif isinstance(link_names, str): + _, matched_link_names = resolve_matching_names( + keys=link_names, list_of_strings=self.link_names + ) + else: + _, matched_link_names = resolve_matching_names( + keys=link_names, list_of_strings=self.link_names + ) + + local_env_ids = [0] if env_ids is None else list(env_ids) + attrs: list[PhysicalAttr] = [] + for env_idx in local_env_ids: + for name in matched_link_names: + attrs.append(self._entities[env_idx].get_physical_attr(name)) + return attrs + + def set_link_physical_attr( + self, + attrs: RigidBodyAttributesCfg | RigidBodyAttributesOverrideCfg | PhysicalAttr, + link_names: str | Sequence[str] | None = None, + env_ids: Sequence[int] | None = None, + *, + base_attrs: RigidBodyAttributesCfg | None = None, + replace_inertial: bool = False, + ) -> None: + """Set physical attributes for selected articulation links. + + Args: + attrs: Full, partial, or DexSim physical attributes to apply. + link_names: Link names or regex patterns. If None, all links are updated. + env_ids: Environment indices. If None, all environments are updated. + base_attrs: Base config used when ``attrs`` is a partial override. + replace_inertial: Recompute inertia when mass changes. + """ + if link_names is None: + matched_link_names = self.link_names + elif isinstance(link_names, str): + _, matched_link_names = resolve_matching_names( + keys=link_names, list_of_strings=self.link_names + ) + else: + _, matched_link_names = resolve_matching_names( + keys=link_names, list_of_strings=self.link_names + ) + + if isinstance(attrs, RigidBodyAttributesOverrideCfg): + if base_attrs is None: + base_attrs = self.cfg.attrs + physical_attr = attrs.merge_with(base_attrs) + if attrs.mass is not None: + replace_inertial = True + elif isinstance(attrs, RigidBodyAttributesCfg): + physical_attr = attrs.attr() + else: + physical_attr = attrs + + local_env_ids = self._all_indices if env_ids is None else env_ids + for env_idx in local_env_ids: + for name in matched_link_names: + self._entities[env_idx].set_physical_attr( + physical_attr, name, is_replace_inertial=replace_inertial + ) + def set_joint_drive( self, stiffness: torch.Tensor | None = None, diff --git a/embodichain/lab/sim/utility/sim_utils.py b/embodichain/lab/sim/utility/sim_utils.py index 9a3f1eea..7cbc4ea9 100644 --- a/embodichain/lab/sim/utility/sim_utils.py +++ b/embodichain/lab/sim/utility/sim_utils.py @@ -34,10 +34,12 @@ from embodichain.lab.sim.cfg import ( ArticulationCfg, + LinkPhysicsOverrideCfg, RigidObjectCfg, SoftObjectCfg, ClothObjectCfg, ) +from embodichain.utils.string import resolve_matching_names from embodichain.lab.sim.shapes import MeshCfg, CubeCfg, SphereCfg from embodichain.utils import logger from dexsim.kit.meshproc import get_mesh_auto_uv @@ -91,6 +93,48 @@ def get_dexsim_drive_type(drive_type: str) -> DriveType: logger.error(f"Invalid dexsim drive type: {drive_type}") +def _resolve_link_physics_groups( + link_names: list[str], link_attrs: dict[str, LinkPhysicsOverrideCfg] +) -> dict[str, LinkPhysicsOverrideCfg]: + """Map each link name to exactly one override group. + + Raises: + ValueError: If a link matches zero groups (not required) or multiple groups. + """ + link_to_group: dict[str, LinkPhysicsOverrideCfg] = {} + for group_cfg in link_attrs.values(): + _, matched_names = resolve_matching_names( + keys=group_cfg.link_names_expr, list_of_strings=link_names + ) + for name in matched_names: + if name in link_to_group: + raise ValueError( + f"Link '{name}' matched multiple link_attrs groups. Each link must " + "match at most one group." + ) + link_to_group[name] = group_cfg + return link_to_group + + +def _apply_link_physics_overrides( + art: Articulation, cfg: ArticulationCfg, link_names: list[str] +) -> None: + """Apply per-link physics overrides on top of global articulation attrs.""" + if not cfg.link_attrs: + return + + link_to_group = _resolve_link_physics_groups(link_names, cfg.link_attrs) + for name in link_names: + group_cfg = link_to_group.get(name) + if group_cfg is None: + continue + physical_attr = group_cfg.attrs.merge_with(cfg.attrs) + replace_inertial = group_cfg.replace_inertial or ( + group_cfg.attrs.mass is not None + ) + art.set_physical_attr(physical_attr, name, is_replace_inertial=replace_inertial) + + def set_dexsim_articulation_cfg(arts: List[Articulation], cfg: ArticulationCfg) -> None: """Set articulation configuration for a list of dexsim articulations. @@ -119,6 +163,8 @@ def get_drive_type(drive_pros): for i, art in enumerate(arts): art.set_body_scale(cfg.body_scale) art.set_physical_attr(cfg.attrs.attr()) + link_names = art.get_link_names() + _apply_link_physics_overrides(art, cfg, link_names) art.set_articulation_flag(ArticulationFlag.FIX_BASE, cfg.fix_base) art.set_articulation_flag( ArticulationFlag.DISABLE_SELF_COLLISION, cfg.disable_self_collision @@ -127,7 +173,6 @@ def get_drive_type(drive_pros): min_position_iters=cfg.min_position_iters, min_velocity_iters=cfg.min_velocity_iters, ) - link_names = art.get_link_names() for name in link_names: physical_body = art.get_physical_body(name) inertia = physical_body.get_mass_space_inertia_tensor() diff --git a/tests/sim/objects/test_articulation.py b/tests/sim/objects/test_articulation.py index 6f2dc692..8f9b42a1 100644 --- a/tests/sim/objects/test_articulation.py +++ b/tests/sim/objects/test_articulation.py @@ -24,7 +24,14 @@ VisualMaterialCfg, ) from embodichain.lab.sim.objects import Articulation -from embodichain.lab.sim.cfg import ArticulationCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + JointDrivePropertiesCfg, + LinkPhysicsOverrideCfg, + RigidBodyAttributesCfg, + RigidBodyAttributesOverrideCfg, +) +from embodichain.lab.sim.utility.sim_utils import _resolve_link_physics_groups from embodichain.data import get_data_path from dexsim.types import ActorType @@ -32,6 +39,41 @@ NUM_ARENAS = 10 +def _link_static_friction(art: Articulation, link_name: str, env_idx: int = 0) -> float: + return art._entities[env_idx].get_physical_attr(link_name).static_friction + + +class TestRigidBodyAttributesOverride: + """Pure-Python tests for per-link physics config merging.""" + + def test_merge_with_applies_only_set_fields(self): + base = RigidBodyAttributesCfg( + static_friction=0.3, + dynamic_friction=0.25, + linear_damping=0.5, + ) + override = RigidBodyAttributesOverrideCfg(static_friction=0.85) + merged = override.merge_with(base) + assert abs(merged.static_friction - 0.85) < 1e-6 + assert abs(merged.dynamic_friction - 0.25) < 1e-6 + assert abs(merged.linear_damping - 0.5) < 1e-6 + + def test_resolve_link_physics_overlap_raises(self): + link_names = ["outer_box", "handle_xpos", "inner_drawer"] + link_attrs = { + "box": LinkPhysicsOverrideCfg( + link_names_expr=["outer_box", "handle_xpos"], + attrs=RigidBodyAttributesOverrideCfg(static_friction=0.9), + ), + "handle": LinkPhysicsOverrideCfg( + link_names_expr=["handle_xpos"], + attrs=RigidBodyAttributesOverrideCfg(static_friction=0.8), + ), + } + with pytest.raises(ValueError, match="multiple link_attrs groups"): + _resolve_link_physics_groups(link_names, link_attrs) + + class BaseArticulationTest: """Shared test logic for CPU and CUDA.""" @@ -257,6 +299,113 @@ def teardown_method(self): gc.collect() +class BaseArticulationLinkPhysicsTest: + """Tests for per-link physics configuration (isolated sim per test).""" + + def setup_simulation(self, sim_device: str) -> None: + config = SimulationManagerCfg(headless=True, sim_device=sim_device, num_envs=2) + self.sim = SimulationManager(config) + self.art_path = get_data_path(ART_PATH) + assert os.path.isfile(self.art_path) + + def teardown_method(self): + self.sim.destroy() + import embodichain.lab.sim as om + + om.SimulationManager.flush_cleanup_queue() + self.__dict__.clear() + import gc + + gc.collect() + + def test_global_attrs_applied_to_all_links(self): + """Default attrs should set the same static friction on every link.""" + global_friction = 0.31 + cfg = ArticulationCfg( + uid="drawer_global_attrs", + fpath=self.art_path, + drive_pros=JointDrivePropertiesCfg(drive_type="force"), + attrs=RigidBodyAttributesCfg(static_friction=global_friction), + ) + art: Articulation = self.sim.add_articulation(cfg=cfg) + for link_name in art.link_names: + assert abs(_link_static_friction(art, link_name) - global_friction) < 1e-3 + + def test_link_attrs_override_selected_links(self): + """link_attrs should override friction only on matched links.""" + global_friction = 0.31 + handle_friction = 0.87 + cfg = ArticulationCfg( + uid="drawer_link_attrs", + fpath=self.art_path, + drive_pros=JointDrivePropertiesCfg(drive_type="force"), + attrs=RigidBodyAttributesCfg(static_friction=global_friction), + link_attrs={ + "handle": LinkPhysicsOverrideCfg( + link_names_expr=["handle_xpos"], + attrs=RigidBodyAttributesOverrideCfg( + static_friction=handle_friction + ), + ), + }, + ) + art: Articulation = self.sim.add_articulation(cfg=cfg) + assert abs(_link_static_friction(art, "handle_xpos") - handle_friction) < 1e-3 + for link_name in art.link_names: + if link_name == "handle_xpos": + continue + assert abs(_link_static_friction(art, link_name) - global_friction) < 1e-3 + + def test_link_attrs_from_dict(self): + """ArticulationCfg.from_dict should parse nested link_attrs.""" + cfg = ArticulationCfg.from_dict( + { + "uid": "drawer_link_attrs_dict", + "fpath": self.art_path, + "drive_pros": {"drive_type": "force"}, + "attrs": {"static_friction": 0.4}, + "link_attrs": { + "handle": { + "link_names_expr": ["handle_xpos"], + "attrs": {"static_friction": 0.77}, + } + }, + } + ) + art: Articulation = self.sim.add_articulation(cfg=cfg) + assert abs(_link_static_friction(art, "handle_xpos") - 0.77) < 1e-3 + assert abs(_link_static_friction(art, "outer_box") - 0.4) < 1e-3 + + def test_set_link_physical_attr_runtime(self): + """Runtime API should update selected links without affecting others.""" + cfg = ArticulationCfg( + uid="drawer_runtime_attrs", + fpath=self.art_path, + drive_pros=JointDrivePropertiesCfg(drive_type="force"), + ) + art: Articulation = self.sim.add_articulation(cfg=cfg) + handle_friction = 0.66 + art.set_link_physical_attr( + RigidBodyAttributesOverrideCfg(static_friction=handle_friction), + link_names=["handle_xpos"], + ) + assert abs(_link_static_friction(art, "handle_xpos") - handle_friction) < 1e-3 + for link_name in art.link_names: + if link_name == "handle_xpos": + continue + assert abs(_link_static_friction(art, link_name) - 0.5) < 1e-3 + + +class TestArticulationLinkPhysicsCPU(BaseArticulationLinkPhysicsTest): + def setup_method(self): + self.setup_simulation("cpu") + + +class TestArticulationLinkPhysicsCUDA(BaseArticulationLinkPhysicsTest): + def setup_method(self): + self.setup_simulation("cuda") + + class TestArticulationCPU(BaseArticulationTest): def setup_method(self): self.setup_simulation("cpu")