diff --git a/docs/source/guides/add_robot.rst b/docs/source/guides/add_robot.rst index f437fd0b..129c759b 100644 --- a/docs/source/guides/add_robot.rst +++ b/docs/source/guides/add_robot.rst @@ -3,15 +3,15 @@ Adding a New Robot — Quick Reference ===================================== -This guide provides a checklist and key reference for adding a new robot to EmbodiChain. For the full step-by-step walkthrough with code examples, see :doc:`/tutorial/add_robot`. +This guide provides a checklist and key reference for adding a new robot to EmbodiChain using the ``RobotDef`` protocol. Checklist --------- 1. **Prepare the URDF** — Place your URDF file (and associated meshes) in the robot assets directory. -2. **Create the config class** — Inherit from ``RobotCfg``, implement ``from_dict`` and ``_build_default_cfgs``. +2. **Create the robot definition** — Implement the ``RobotDef`` protocol with ``@register_robot("MyRobot")``. 3. **Define control parts** — Group joints into logical sets (e.g., ``arm``, ``gripper``). -4. **Configure IK solver** — Choose ``OPWSolverCfg``, ``SRSSolverCfg``, or a generic ``SolverCfg``. +4. **Configure IK solver** — Choose ``OPWSolverCfg``, ``SRSSolverCfg``, or a generic ``SolverCfg`` per control part. 5. **Set drive properties** — Configure stiffness, damping, and max effort per joint group. 6. **Implement** ``build_pk_serial_chain`` — Required for PyTorch-Kinematics IK support. 7. **Register in** ``embodichain/lab/sim/robots/__init__.py``. @@ -21,8 +21,110 @@ Checklist Approaches ---------- -- **Single-file** (simple robots): One ``my_robot.py`` with everything. -- **Package** (complex robots): Directory with ``types.py``, ``params.py``, ``utils.py``, ``cfg.py``, ``__init__.py``. +Simple robot (single-file) +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +For robots without variants, declare all config as class-level fields: + +.. code-block:: python + + from embodichain.lab.sim.robots.protocol import RobotDef + from embodichain.lab.sim.robots.registry import register_robot + from embodichain.lab.sim.cfg import ( + RobotCfg, URDFCfg, JointDrivePropertiesCfg, RigidBodyAttributesCfg, + ) + from embodichain.lab.sim.solvers import SolverCfg, OPWSolverCfg + from embodichain.data import get_data_path + import numpy as np + + @register_robot("MyRobot") + class MyRobotDef: + name: str = "MyRobot" + + urdf_cfg: URDFCfg = URDFCfg(components=[ + {"component_type": "arm", + "urdf_path": get_data_path("MyRobot/arm.urdf")}, + ]) + + control_parts: dict[str, list[str]] = { + "arm": ["JOINT[1-6]"], + "gripper": ["FINGER[1-2]"], + } + + solver_cfg: dict[str, SolverCfg] = { + "arm": OPWSolverCfg( + end_link_name="link6", + root_link_name="base_link", + ), + } + + drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg( + stiffness={"JOINT[1-6]": 1e4, "FINGER[1-2]": 1e2}, + damping={"JOINT[1-6]": 1e3, "FINGER[1-2]": 1e1}, + ) + + attrs: RigidBodyAttributesCfg = RigidBodyAttributesCfg() + + def build_pk_serial_chain(self, device): + return {} + + def build_cfg(self, **overrides): + return RobotDef.build_cfg(self, **overrides) + +Usage:: + + from embodichain.lab.sim.robots import build_robot_cfg + cfg = build_robot_cfg("MyRobot", overrides={"uid": "my_robot"}) + robot = sim.add_robot(cfg=cfg) + +Complex robot (package) +~~~~~~~~~~~~~~~~~~~~~~~ + +For robots with variants (arm types, hand brands, versions), use ``@property`` methods and variant parameters: + +.. code-block:: python + + @register_robot("MyComplexRobot") + class MyComplexRobotDef: + name: str = "MyComplexRobot" + + # Variant parameters + arm_kind: str = "standard" + + def __post_init__(self): + # Fill defaults based on arm_kind + ... + + @property + def urdf_cfg(self) -> URDFCfg: + return _build_urdf(self.arm_kind) + + @property + def control_parts(self) -> dict[str, list[str]]: + return _build_control_parts(self.arm_kind) + + @property + def solver_cfg(self) -> dict[str, SolverCfg]: + return _build_solver_cfg(self.arm_kind) + + @property + def drive_pros(self) -> JointDrivePropertiesCfg: + return _build_drive_pros(self.arm_kind) + + @property + def attrs(self) -> RigidBodyAttributesCfg: + return RigidBodyAttributesCfg() + + def build_pk_serial_chain(self, device): + return {} + + def build_cfg(self, **overrides): + return RobotDef.build_cfg(self, **overrides) + +Variant usage:: + + cfg = build_robot_cfg("MyComplexRobot", arm_kind="extended", + overrides={"uid": "my_robot"}) Key Parameters -------------- @@ -30,7 +132,7 @@ Key Parameters +---------------------+----------------------------+----------------------------------+ | Parameter | Type | Description | +=====================+============================+==================================+ -| ``uid`` | str | Unique robot identifier | +| ``name`` | str | Unique robot identifier | +---------------------+----------------------------+----------------------------------+ | ``urdf_cfg`` | URDFCfg | URDF file and components | +---------------------+----------------------------+----------------------------------+ @@ -40,15 +142,22 @@ Key Parameters +---------------------+----------------------------+----------------------------------+ | ``drive_pros`` | JointDrivePropertiesCfg | Joint stiffness, damping, force | +---------------------+----------------------------+----------------------------------+ +| ``attrs`` | RigidBodyAttributesCfg | Physical attributes | ++---------------------+----------------------------+----------------------------------+ + +Backward Compatibility +---------------------- + +The old ``*Cfg.from_dict()`` API continues to work. ``CobotMagicCfg`` and ``DexforceW1Cfg`` are thin wrappers that delegate to the new ``RobotDef`` implementations. .. tip:: - See the :doc:`full tutorial ` for complete code examples of both approaches. + See :doc:`/tutorial/robot` for using robots in simulation and + :doc:`/resources/robot/index` for existing robot documentation. See Also -------- -- :doc:`/tutorial/add_robot` — Full step-by-step tutorial - :doc:`/tutorial/robot` — Using robots in simulation - :doc:`/overview/sim/solvers/index` — IK solver reference - :doc:`/resources/robot/index` — Existing robot documentation diff --git a/docs/superpowers/plans/2026-05-13-robot-definition-protocol.md b/docs/superpowers/plans/2026-05-13-robot-definition-protocol.md new file mode 100644 index 00000000..298b3d37 --- /dev/null +++ b/docs/superpowers/plans/2026-05-13-robot-definition-protocol.md @@ -0,0 +1,830 @@ +# Robot Definition Protocol Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Replace per-robot boilerplate with a standard `RobotDef` protocol, generic `build_cfg`, and a registry for robot lookup by name. + +**Architecture:** A `RobotDef` protocol defines the contract every robot must satisfy (urdf_cfg, control_parts, solver_cfg, drive_pros, attrs, build_pk_serial_chain). A `build_cfg()` default implementation converts any `RobotDef` into a `RobotCfg` for the spawner. A `@register_robot` decorator + `build_robot_cfg()` function provide name-based lookup. Existing `RobotCfg` subclasses become thin backward-compat wrappers. + +**Tech Stack:** Python dataclasses, `@configclass`, existing `merge_robot_cfg`, `typing.Protocol` + +--- + +## File Structure + +| Action | File | Responsibility | +|--------|------|----------------| +| Create | `embodichain/lab/sim/robots/protocol.py` | `RobotDef` protocol + generic `build_cfg` base mixin | +| Create | `embodichain/lab/sim/robots/registry.py` | `@register_robot`, `get_robot_def`, `build_robot_cfg` | +| Create | `tests/sim/robots/test_protocol.py` | Unit tests for protocol, registry, build_cfg | +| Refactor | `embodichain/lab/sim/robots/cobotmagic.py` | `CobotMagicDef` replaces `CobotMagicCfg` | +| Refactor | `embodichain/lab/sim/robots/dexforce_w1/def.py` | New `DexforceW1Def` replaces `cfg.py` | +| Modify | `embodichain/lab/sim/robots/dexforce_w1/utils.py` | Extract helper functions, keep existing managers | +| Modify | `embodichain/lab/sim/robots/__init__.py` | Export new defs + backward-compat aliases | +| Modify | `embodichain/lab/sim/robots/dexforce_w1/__init__.py` | Export `DexforceW1Def` + backward-compat `DexforceW1Cfg` | +| Modify | `docs/source/guides/add_robot.rst` | Update to reflect new RobotDef protocol | +| Update | `tests/sim/objects/test_robot.py` | Verify existing tests still pass | + +--- + +### Task 1: Create RobotDef Protocol and Base Mixin + +**Files:** +- Create: `embodichain/lab/sim/robots/protocol.py` + +- [ ] **Step 1: Write the test for RobotDef protocol** + +```python +# tests/sim/robots/test_protocol.py + +import numpy as np +import pytest +import torch + +from embodichain.lab.sim.robots.protocol import RobotDef +from embodichain.lab.sim.cfg import ( + RobotCfg, + URDFCfg, + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, +) +from embodichain.lab.sim.solvers import SolverCfg + + +class DummyRobotDef: + """Minimal RobotDef implementation for testing.""" + + name: str = "DummyRobot" + urdf_cfg: URDFCfg = None + control_parts: dict = {} + solver_cfg: dict = {} + drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg() + attrs: RigidBodyAttributesCfg = RigidBodyAttributesCfg() + + def build_pk_serial_chain(self, device): + return {} + + +class TestRobotDefProtocol: + + def test_build_cfg_returns_robot_cfg(self): + robot_def = DummyRobotDef() + cfg = robot_def.build_cfg(uid="test_robot") + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "test_robot" + + def test_build_cfg_applies_overrides(self): + robot_def = DummyRobotDef() + cfg = robot_def.build_cfg(uid="test", init_pos=(0.0, 0.0, 1.0)) + assert cfg.init_pos == (0.0, 0.0, 1.0) + + def test_build_cfg_preserves_control_parts(self): + robot_def = DummyRobotDef() + robot_def.control_parts = {"arm": ["J1", "J2"]} + cfg = robot_def.build_cfg() + assert cfg.control_parts == {"arm": ["J1", "J2"]} + + def test_build_cfg_merges_drive_pros(self): + robot_def = DummyRobotDef() + robot_def.drive_pros = JointDrivePropertiesCfg( + stiffness={"J1": 1e4, "J2": 1e3}, + damping={"J1": 1e3, "J2": 1e2}, + ) + cfg = robot_def.build_cfg( + drive_pros={"stiffness": {"J1": 5e4}} + ) + assert cfg.drive_pros.stiffness["J1"] == 5e4 + assert cfg.drive_pros.stiffness["J2"] == 1e3 + + def test_build_cfg_with_solver_cfg_override(self): + robot_def = DummyRobotDef() + robot_def.solver_cfg = {"arm": SolverCfg(end_link_name="link6")} + cfg = robot_def.build_cfg( + solver_cfg={"arm": {"tcp": np.eye(4)}} + ) + assert np.allclose(cfg.solver_cfg["arm"].tcp, np.eye(4)) +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py -v` +Expected: FAIL — `module 'embodichain.lab.sim.robots.protocol' not found` + +- [ ] **Step 3: Create protocol.py with RobotDef and build_cfg** + +```python +# embodichain/lab/sim/robots/protocol.py + +from __future__ import annotations + +import torch +import numpy as np + +from typing import Dict, List, Union, runtime_checkable, Protocol + +from embodichain.lab.sim.cfg import ( + RobotCfg, + URDFCfg, + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, +) +from embodichain.lab.sim.solvers import SolverCfg +from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg +from embodichain.utils import logger + +__all__ = ["RobotDef"] + + +@runtime_checkable +class RobotDef(Protocol): + """Standard protocol that every robot definition must satisfy. + + Simple robots declare data as class-level fields. + Complex robots with variants use @property methods that compute + values based on constructor parameters. + """ + + name: str + + @property + def urdf_cfg(self) -> URDFCfg: ... + + @property + def control_parts(self) -> Dict[str, List[str]]: ... + + @property + def solver_cfg(self) -> Dict[str, SolverCfg]: ... + + @property + def drive_pros(self) -> JointDrivePropertiesCfg: ... + + @property + def attrs(self) -> RigidBodyAttributesCfg: ... + + def build_pk_serial_chain( + self, device: torch.device + ) -> Dict[str, object]: + ... + + def build_cfg(self, **overrides) -> RobotCfg: + """Convert this definition into a RobotCfg for the spawner. + + Creates a RobotCfg populated with protocol properties, + then applies user overrides via merge_robot_cfg. + """ + cfg = RobotCfg() + cfg.uid = overrides.pop("uid", self.name) + + if self.urdf_cfg is not None: + cfg.urdf_cfg = self.urdf_cfg + + if self.control_parts is not None: + cfg.control_parts = self.control_parts + + if self.solver_cfg is not None: + cfg.solver_cfg = self.solver_cfg + + cfg.drive_pros = self.drive_pros + cfg.attrs = self.attrs + + # Copy extra fields from the def that map to RobotCfg fields + for attr_name in ("min_position_iters", "min_velocity_iters", + "fix_base", "disable_self_collision", + "build_pk_chain", "init_qpos", "body_scale"): + val = getattr(self, attr_name, None) + if val is not None and hasattr(cfg, attr_name): + setattr(cfg, attr_name, val) + + # Override build_pk_serial_chain on the cfg instance + if hasattr(self, "build_pk_serial_chain"): + cfg.build_pk_serial_chain = self.build_pk_serial_chain + + # Apply user overrides (init_pos, drive_pros, solver_cfg, etc.) + if overrides: + cfg = merge_robot_cfg(cfg, overrides) + + return cfg +``` + +- [ ] **Step 4: Run test to verify it passes** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py::TestRobotDefProtocol -v` +Expected: PASS + +- [ ] **Step 5: Commit** + +```bash +git add embodichain/lab/sim/robots/protocol.py tests/sim/robots/test_protocol.py +git commit -m "feat: add RobotDef protocol with generic build_cfg" +``` + +--- + +### Task 2: Create Robot Registry + +**Files:** +- Create: `embodichain/lab/sim/robots/registry.py` +- Modify: `tests/sim/robots/test_protocol.py` — add registry tests + +- [ ] **Step 1: Write the failing tests** + +Append to `tests/sim/robots/test_protocol.py`: + +```python +from embodichain.lab.sim.robots.registry import ( + register_robot, + get_robot_def, + build_robot_cfg, + _ROBOT_REGISTRY, +) + + +class TestRobotRegistry: + + def test_register_and_lookup(self): + @register_robot("TestDummy") + class TestDef: + name: str = "TestDummy" + urdf_cfg = None + control_parts = {} + solver_cfg = {} + drive_pros = JointDrivePropertiesCfg() + attrs = RigidBodyAttributesCfg() + + def build_pk_serial_chain(self, device): + return {} + + def build_cfg(self, **overrides): + from embodichain.lab.sim.robots.protocol import RobotDef + return RobotDef.build_cfg(self, **overrides) + + assert "TestDummy" in _ROBOT_REGISTRY + robot_def = get_robot_def("TestDummy") + assert robot_def.name == "TestDummy" + + def test_get_unknown_robot_raises(self): + with pytest.raises(ValueError, match="Unknown robot"): + get_robot_def("NonExistentRobot999") + + def test_build_robot_cfg_convenience(self): + robot_def = get_robot_def("TestDummy") + cfg = build_robot_cfg("TestDummy", overrides={"uid": "my_test"}) + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "my_test" +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py::TestRobotRegistry -v` +Expected: FAIL — `module 'embodichain.lab.sim.robots.registry' not found` + +- [ ] **Step 3: Create registry.py** + +```python +# embodichain/lab/sim/robots/registry.py + +from __future__ import annotations + +from typing import Dict, Type + +from embodichain.lab.sim.cfg import RobotCfg +from embodichain.utils import logger + +__all__ = ["register_robot", "get_robot_def", "build_robot_cfg"] + +_ROBOT_REGISTRY: Dict[str, Type] = {} + + +def register_robot(name: str): + """Decorator to register a robot definition class. + + Args: + name: Unique name for the robot (e.g., "CobotMagic", "DexforceW1"). + + Returns: + The class unchanged, after registering it. + """ + def wrapper(cls): + if name in _ROBOT_REGISTRY: + logger.log_warning( + f"Robot '{name}' already registered. Overwriting." + ) + _ROBOT_REGISTRY[name] = cls + return cls + return wrapper + + +def get_robot_def(name: str, **variant_kwargs) -> object: + """Look up a robot definition by name and instantiate it. + + Args: + name: Registered robot name. + **variant_kwargs: Constructor arguments for the robot def class + (e.g., arm_kind, version, hand_types for DexforceW1). + + Returns: + An instance of the robot definition class. + + Raises: + ValueError: If the robot name is not registered. + """ + if name not in _ROBOT_REGISTRY: + raise ValueError( + f"Unknown robot: '{name}'. " + f"Available: {list(_ROBOT_REGISTRY.keys())}" + ) + return _ROBOT_REGISTRY[name](**variant_kwargs) + + +def build_robot_cfg(name: str, **kwargs) -> RobotCfg: + """Look up a robot by name and build a RobotCfg. + + Args: + name: Registered robot name. + **kwargs: Variant kwargs are passed to the constructor. + Use ``overrides={...}`` dict for cfg-level overrides + (uid, init_pos, drive_pros, solver_cfg, etc.). + + Returns: + RobotCfg ready for SimulationManager.add_robot(). + """ + overrides = kwargs.pop("overrides", {}) + robot_def = get_robot_def(name, **kwargs) + return robot_def.build_cfg(**overrides) +``` + +- [ ] **Step 4: Run test to verify it passes** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py::TestRobotRegistry -v` +Expected: PASS + +- [ ] **Step 5: Commit** + +```bash +git add embodichain/lab/sim/robots/registry.py tests/sim/robots/test_protocol.py +git commit -m "feat: add robot registry with register_robot and build_robot_cfg" +``` + +--- + +### Task 3: Refactor CobotMagic to use RobotDef + +**Files:** +- Modify: `embodichain/lab/sim/robots/cobotmagic.py` +- Modify: `tests/sim/robots/test_protocol.py` — add CobotMagic tests + +- [ ] **Step 1: Write the failing test** + +Append to `tests/sim/robots/test_protocol.py`: + +```python +from embodichain.lab.sim.robots.cobotmagic import CobotMagicDef + + +class TestCobotMagicDef: + + def test_cobotmagic_def_builds_valid_cfg(self): + robot_def = CobotMagicDef() + cfg = robot_def.build_cfg(uid="test_cobot") + assert cfg.uid == "test_cobot" + assert cfg.urdf_cfg is not None + assert "left_arm" in cfg.control_parts + assert "right_arm" in cfg.control_parts + assert "left_arm" in cfg.solver_cfg + assert "right_arm" in cfg.solver_cfg + + def test_cobotmagic_def_registry(self): + from embodichain.lab.sim.robots.registry import get_robot_def + robot_def = get_robot_def("CobotMagic") + cfg = robot_def.build_cfg(uid="from_registry") + assert cfg.uid == "from_registry" + assert "left_arm" in cfg.control_parts + + def test_cobotmagic_backward_compat_from_dict(self): + from embodichain.lab.sim.robots.cobotmagic import CobotMagicCfg + cfg = CobotMagicCfg.from_dict({"uid": "compat_test"}) + assert cfg.uid == "compat_test" + assert "left_arm" in cfg.control_parts +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py::TestCobotMagicDef -v` +Expected: FAIL — `ImportError: cannot import name 'CobotMagicDef'` + +- [ ] **Step 3: Refactor cobotmagic.py** + +Replace the file content with the new `CobotMagicDef` class and a backward-compatible `CobotMagicCfg` wrapper. The file should: + +1. Keep the Apache 2.0 header. +2. Import `from embodichain.lab.sim.robots.protocol import RobotDef`. +3. Import `from embodichain.lab.sim.robots.registry import register_robot`. +4. Define `@register_robot("CobotMagic") class CobotMagicDef` with flat class-level declarations for: + - `name = "CobotMagic"` + - `urdf_cfg` — the same `URDFCfg(components=[...])` as the current `_build_default_cfgs` but as a class-level field + - `control_parts` — same dict as current + - `solver_cfg` — same dict with `OPWSolverCfg` as current + - `drive_pros` — same `JointDrivePropertiesCfg` as current + - `attrs` — same `RigidBodyAttributesCfg` as current + - `min_position_iters = 8`, `min_velocity_iters = 2` + - `build_pk_serial_chain` — same logic as current + - `build_cfg` — inherited from `RobotDef` protocol (call `RobotDef.build_cfg(self, **overrides)`) +5. Define `CobotMagicCfg` as backward-compat wrapper: + +```python +class CobotMagicCfg(RobotCfg): + """Backward-compatible CobotMagic config. Delegates to CobotMagicDef.""" + + @classmethod + def from_dict(cls, init_dict): + return CobotMagicDef().build_cfg(**init_dict) +``` + +6. Keep the `if __name__ == "__main__"` block but update to use `CobotMagicDef`. + +- [ ] **Step 4: Run test to verify it passes** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py::TestCobotMagicDef -v` +Expected: PASS + +- [ ] **Step 5: Commit** + +```bash +git add embodichain/lab/sim/robots/cobotmagic.py tests/sim/robots/test_protocol.py +git commit -m "refactor: replace CobotMagicCfg with CobotMagicDef + backward-compat wrapper" +``` + +--- + +### Task 4: Refactor DexforceW1 to use RobotDef + +**Files:** +- Create: `embodichain/lab/sim/robots/dexforce_w1/def.py` +- Modify: `embodichain/lab/sim/robots/dexforce_w1/__init__.py` +- Modify: `tests/sim/robots/test_protocol.py` — add DexforceW1 tests + +This is the largest task. The existing `cfg.py` (390 LOC) gets replaced by `def.py` (~80 LOC) that delegates to helper functions extracted from the current static methods. + +- [ ] **Step 1: Write the failing test** + +Append to `tests/sim/robots/test_protocol.py`: + +```python +from embodichain.lab.sim.robots.dexforce_w1 import DexforceW1Def, DexforceW1Cfg + + +class TestDexforceW1Def: + + def test_anthropomorphic_default(self): + robot_def = DexforceW1Def(arm_kind="anthropomorphic") + cfg = robot_def.build_cfg(uid="w1_test") + assert cfg.uid == "w1_test" + assert "left_arm" in cfg.control_parts + assert "right_arm" in cfg.control_parts + assert "left_arm" in cfg.solver_cfg + + def test_industrial_default(self): + robot_def = DexforceW1Def(arm_kind="industrial") + cfg = robot_def.build_cfg(uid="w1_ind") + assert cfg.uid == "w1_ind" + assert "left_arm" in cfg.control_parts + + def test_registry_lookup(self): + from embodichain.lab.sim.robots.registry import build_robot_cfg + cfg = build_robot_cfg( + "DexforceW1", + arm_kind="anthropomorphic", + overrides={"uid": "from_reg"}, + ) + assert cfg.uid == "from_reg" + + def test_backward_compat_from_dict(self): + cfg = DexforceW1Cfg.from_dict({ + "uid": "compat_w1", + "version": "v021", + "arm_kind": "anthropomorphic", + }) + assert cfg.uid == "compat_w1" + assert "left_arm" in cfg.control_parts +``` + +- [ ] **Step 2: Run test to verify it fails** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py::TestDexforceW1Def -v` +Expected: FAIL — `ImportError: cannot import name 'DexforceW1Def'` + +- [ ] **Step 3: Create dexforce_w1/def.py** + +Create the new `def.py` file with: + +1. Apache 2.0 header. +2. Imports from `types.py`, `params.py`, `utils.py`, `protocol.py`, `registry.py`, and `cfg.py` (base classes). +3. `@register_robot("DexforceW1") class DexforceW1Def` with: + - `name = "DexforceW1"` + - Variant fields: `version`, `arm_kind`, `hand_types`, `include_chassis/torso/head/hand` + - `__post_init__` that fills default `hand_types` based on `arm_kind` + - `@property urdf_cfg` — delegates to `_build_urdf_cfg()` helper + - `@property control_parts` — delegates to `_build_control_parts()` helper + - `@property solver_cfg` — delegates to `_build_solver_cfg()` helper + - `@property drive_pros` — delegates to `_build_drive_pros()` helper + - `@property attrs` — returns the standard `RigidBodyAttributesCfg` + - `min_position_iters = 32`, `min_velocity_iters = 8` + - `build_pk_serial_chain` — same logic as current `DexforceW1Cfg.build_pk_serial_chain` + - `build_cfg` — inherited from protocol + +The helper functions (`_build_urdf_cfg`, `_build_control_parts`, `_build_solver_cfg`, `_build_drive_pros`) are extracted from the current `DexforceW1Cfg._build_default_cfg`, `_build_default_physics_cfgs`, `_build_default_solver_cfg` and the existing `utils.py` functions (`build_dexforce_w1_assembly_urdf_cfg`, `build_dexforce_w1_cfg`, `build_dexforce_w1_solver_cfg`). These helpers live in `def.py` as module-level functions. + +- [ ] **Step 4: Update dexforce_w1/__init__.py** + +```python +from .def import DexforceW1Def +from .cfg import DexforceW1Cfg +``` + +The old `cfg.py` file is kept as-is for backward compatibility, with `DexforceW1Cfg` now delegating to `DexforceW1Def`: + +```python +class DexforceW1Cfg(RobotCfg): + """Backward-compatible DexforceW1 config. Delegates to DexforceW1Def.""" + # ... keep existing fields for type-checking ... + + @classmethod + def from_dict(cls, init_dict): + from embodichain.lab.sim.robots.dexforce_w1.def import DexforceW1Def + version = init_dict.get("version", "v021") + arm_kind = init_dict.get("arm_kind", "anthropomorphic") + with_default_eef = init_dict.get("with_default_eef", True) + remaining = {k: v for k, v in init_dict.items() + if k not in ("version", "arm_kind", "with_default_eef")} + return DexforceW1Def( + version=version, arm_kind=arm_kind, + include_hand=with_default_eef, + ).build_cfg(**remaining) +``` + +- [ ] **Step 5: Run test to verify it passes** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py::TestDexforceW1Def -v` +Expected: PASS + +- [ ] **Step 6: Commit** + +```bash +git add embodichain/lab/sim/robots/dexforce_w1/def.py embodichain/lab/sim/robots/dexforce_w1/__init__.py embodichain/lab/sim/robots/dexforce_w1/cfg.py tests/sim/robots/test_protocol.py +git commit -m "refactor: add DexforceW1Def with RobotDef protocol + backward-compat DexforceW1Cfg" +``` + +--- + +### Task 5: Update __init__.py exports + +**Files:** +- Modify: `embodichain/lab/sim/robots/__init__.py` + +- [ ] **Step 1: Update the robots __init__.py** + +```python +from .protocol import RobotDef +from .registry import register_robot, get_robot_def, build_robot_cfg +from .cobotmagic import CobotMagicDef, CobotMagicCfg +from .dexforce_w1 import DexforceW1Def, DexforceW1Cfg + +__all__ = [ + "RobotDef", + "register_robot", + "get_robot_def", + "build_robot_cfg", + "CobotMagicDef", + "CobotMagicCfg", + "DexforceW1Def", + "DexforceW1Cfg", +] +``` + +- [ ] **Step 2: Run all robot tests** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/ tests/sim/objects/test_robot.py -v` +Expected: PASS + +- [ ] **Step 3: Commit** + +```bash +git add embodichain/lab/sim/robots/__init__.py +git commit -m "feat: export RobotDef, registry, and new robot defs from __init__" +``` + +--- + +### Task 6: Update add_robot.rst documentation + +**Files:** +- Modify: `docs/source/guides/add_robot.rst` + +- [ ] **Step 1: Rewrite add_robot.rst to reflect the new RobotDef protocol** + +Replace the content with updated guide that: + +1. Updates the checklist to reference `RobotDef` instead of `from_dict` and `_build_default_cfgs` +2. Shows two approaches using the new system: + - **Simple robot** (single-file): `@register_robot` + flat `RobotDef` declarations + - **Complex robot** (package): variant parameters + `@property` methods + extracted helpers +3. Provides complete code examples for both +4. Shows how to use `build_robot_cfg()` and `SimulationManager.add_robot()` +5. Documents backward compatibility with the old `*Cfg.from_dict()` API + +Key content: + +```rst +.. _guide_add_robot: + +Adding a New Robot — Quick Reference +===================================== + +This guide provides a checklist and key reference for adding a new robot +to EmbodiChain using the ``RobotDef`` protocol. + +Checklist +--------- + +1. **Prepare the URDF** — Place your URDF file (and associated meshes) + in the robot assets directory. +2. **Create the robot definition** — Implement the ``RobotDef`` protocol + with ``@register_robot("MyRobot")``. +3. **Define control parts** — Group joints into logical sets + (e.g., ``arm``, ``gripper``). +4. **Configure IK solver** — Choose ``OPWSolverCfg``, ``SRSSolverCfg``, + or a generic ``SolverCfg`` per control part. +5. **Set drive properties** — Configure stiffness, damping, and max + effort per joint group. +6. **Implement** ``build_pk_serial_chain`` — Required for + PyTorch-Kinematics IK support. +7. **Register in** ``embodichain/lab/sim/robots/__init__.py``. +8. **Add documentation** — Create ``docs/source/resources/robot/my_robot.md`` + and update ``resources/robot/index.rst``. +9. **Test** — Add a ``__main__`` block or use the ``preview-asset`` CLI + to verify. + +Approaches +---------- + +Simple robot (single-file) +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +For robots without variants, declare all config as class-level fields: + +.. code-block:: python + + from embodichain.lab.sim.robots.protocol import RobotDef + from embodichain.lab.sim.robots.registry import register_robot + + @register_robot("MyRobot") + class MyRobotDef: + name: str = "MyRobot" + + urdf_cfg: URDFCfg = URDFCfg(components=[ + {"component_type": "arm", + "urdf_path": get_data_path("MyRobot/arm.urdf")}, + ]) + + control_parts: Dict[str, List[str]] = { + "arm": ["JOINT[1-6]"], + "gripper": ["FINGER[1-2]"], + } + + solver_cfg: Dict[str, SolverCfg] = { + "arm": OPWSolverCfg( + end_link_name="link6", + root_link_name="base_link", + ), + } + + drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg( + stiffness={"JOINT[1-6]": 1e4, "FINGER[1-2]": 1e2}, + damping={"JOINT[1-6]": 1e3, "FINGER[1-2]": 1e1}, + ) + + attrs: RigidBodyAttributesCfg = RigidBodyAttributesCfg() + + def build_pk_serial_chain(self, device): + return {} + + def build_cfg(self, **overrides): + return RobotDef.build_cfg(self, **overrides) + +Usage:: + + from embodichain.lab.sim.robots import build_robot_cfg + cfg = build_robot_cfg("MyRobot", overrides={"uid": "my_robot"}) + robot = sim.add_robot(cfg=cfg) + +Complex robot (package) +~~~~~~~~~~~~~~~~~~~~~~~ + +For robots with variants (arm types, hand brands, versions), use +``@property`` methods and variant parameters: + +.. code-block:: python + + @register_robot("MyComplexRobot") + class MyComplexRobotDef: + name: str = "MyComplexRobot" + + # Variant parameters + arm_kind: str = "standard" + + def __post_init__(self): + # Fill defaults based on arm_kind + ... + + @property + def urdf_cfg(self) -> URDFCfg: + return _build_urdf(self.arm_kind) + + @property + def control_parts(self) -> Dict[str, List[str]]: + return _build_control_parts(self.arm_kind) + + @property + def solver_cfg(self) -> Dict[str, SolverCfg]: + return _build_solver_cfg(self.arm_kind) + + @property + def drive_pros(self) -> JointDrivePropertiesCfg: + return _build_drive_pros(self.arm_kind) + + @property + def attrs(self) -> RigidBodyAttributesCfg: + return RigidBodyAttributesCfg() + + def build_pk_serial_chain(self, device): + return {} + + def build_cfg(self, **overrides): + return RobotDef.build_cfg(self, **overrides) + +Key Parameters +-------------- + ++---------------------+----------------------------+----------------------------------+ +| Parameter | Type | Description | ++=====================+============================+==================================+ +| ``name`` | str | Unique robot identifier | ++---------------------+----------------------------+----------------------------------+ +| ``urdf_cfg`` | URDFCfg | URDF file and components | ++---------------------+----------------------------+----------------------------------+ +| ``control_parts`` | Dict[str, List[str]] | Joint groups for control | ++---------------------+----------------------------+----------------------------------+ +| ``solver_cfg`` | Dict[str, SolverCfg] | IK solver configurations | ++---------------------+----------------------------+----------------------------------+ +| ``drive_pros`` | JointDrivePropertiesCfg | Joint stiffness, damping, force | ++---------------------+----------------------------+----------------------------------+ +| ``attrs`` | RigidBodyAttributesCfg | Physical attributes | ++---------------------+----------------------------+----------------------------------+ + +Backward Compatibility +---------------------- + +The old ``*Cfg.from_dict()`` API continues to work. ``CobotMagicCfg`` +and ``DexforceW1Cfg`` are thin wrappers that delegate to the new +``RobotDef`` implementations. + +.. tip:: + + See :doc:`/tutorial/robot` for using robots in simulation and + :doc:`/resources/robot/index` for existing robot documentation. +``` + +- [ ] **Step 2: Commit** + +```bash +git add docs/source/guides/add_robot.rst +git commit -m "docs: update add_robot guide for RobotDef protocol" +``` + +--- + +### Task 7: Run full test suite and verify + +**Files:** None (verification only) + +- [ ] **Step 1: Run protocol tests** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/robots/test_protocol.py -v` +Expected: All PASS + +- [ ] **Step 2: Run existing robot tests** + +Run: `cd /root/sources/EmbodiChain && python -m pytest tests/sim/objects/test_robot.py -v` +Expected: All PASS (backward compatibility verified) + +- [ ] **Step 3: Run black formatter** + +Run: `cd /root/sources/EmbodiChain && black embodichain/lab/sim/robots/ tests/sim/robots/test_protocol.py` +Expected: No errors + +- [ ] **Step 4: Commit any formatting changes** + +```bash +git add -u +git commit -m "style: apply black formatting to robot definition files" +``` diff --git a/docs/superpowers/specs/2026-05-13-robot-definition-protocol-design.md b/docs/superpowers/specs/2026-05-13-robot-definition-protocol-design.md new file mode 100644 index 00000000..7372586a --- /dev/null +++ b/docs/superpowers/specs/2026-05-13-robot-definition-protocol-design.md @@ -0,0 +1,308 @@ +# Robot Definition Protocol Design + +**Date**: 2026-05-13 +**Status**: Approved +**Scope**: `embodichain/lab/sim/robots/` + +## Problem + +Adding a new robot to EmbodiChain requires significant boilerplate and an inconsistent process: + +- Each robot reimplements `from_dict`, `_build_default_cfgs`, `_build_default_physics_cfgs`, `build_pk_serial_chain`, and serialization methods. +- CobotMagic (~200 LOC, single file) and DexforceW1 (~750 LOC, 4-file package) follow different structural patterns with no shared contract. +- External users must understand the full class hierarchy and import paths to define a robot. +- No central registry to discover or look up available robots. + +## Goal + +1. Define a **standard protocol** that every specific robot must implement. +2. **Reduce complexity** — minimize boilerplate, make the common path declarative. +3. Maintain **full backward compatibility** with existing `SimulationManager.add_robot(cfg: RobotCfg)`. + +## Design + +### RobotDef Protocol + +A `RobotDef` protocol defines the standard contract for robot definitions. It specifies the data every robot must provide: + +```python +# embodichain/lab/sim/robots/protocol.py + +@runtime_checkable +class RobotDef(Protocol): + """Standard protocol that every robot definition must satisfy. + + Simple robots declare data as class-level fields. + Complex robots with variants use @property methods that compute + values based on constructor parameters. + """ + + @property + def name(self) -> str: ... + + @property + def urdf_cfg(self) -> URDFCfg: ... + + @property + def control_parts(self) -> Dict[str, List[str]]: ... + + @property + def solver_cfg(self) -> Dict[str, SolverCfg]: ... + + @property + def drive_pros(self) -> JointDrivePropertiesCfg: ... + + @property + def attrs(self) -> RigidBodyAttributesCfg: ... + + def build_pk_serial_chain( + self, device: torch.device + ) -> Dict[str, "pk.SerialChain"]: ... + + def build_cfg(self, **overrides) -> RobotCfg: + """Convert this definition into a RobotCfg for the spawner. + + Default implementation: + 1. Create RobotCfg(), populate with protocol properties. + 2. Delegate to merge_robot_cfg for user overrides. + """ + ... +``` + +### Bridge to RobotCfg + +The `build_cfg(**overrides) -> RobotCfg` method is the bridge between `RobotDef` and the existing simulation infrastructure. Its generic default implementation: + +1. Creates a `RobotCfg()` instance. +2. Populates it with the protocol's properties (`urdf_cfg`, `control_parts`, `solver_cfg`, `drive_pros`, `attrs`). +3. Copies any extra protocol fields (e.g., `min_position_iters`, `min_velocity_iters`). +4. Calls the existing `merge_robot_cfg(cfg, overrides)` for user-provided overrides (`uid`, `init_pos`, custom drive properties, etc.). + +`SimulationManager.add_robot(cfg: RobotCfg)` is completely unchanged. + +### Simple Robot Example — CobotMagic + +Simple robots with no variants use flat class-level declarations: + +```python +@register_robot("CobotMagic") +class CobotMagicDef: + """CobotMagic dual-arm robot definition.""" + + name: str = "CobotMagic" + + urdf_cfg: URDFCfg = URDFCfg(components=[ + {"component_type": "left_arm", + "urdf_path": get_data_path("CobotMagicArm/CobotMagicWithGripperV100.urdf"), + "transform": np.array([ + [1,0,0,0.233], [0,1,0,0.300], + [0,0,1,0], [0,0,0,1]])}, + {"component_type": "right_arm", + "urdf_path": get_data_path("CobotMagicArm/CobotMagicWithGripperV100.urdf"), + "transform": np.array([ + [1,0,0,0.233], [0,1,0,-0.300], + [0,0,1,0], [0,0,0,1]])}, + ]) + + control_parts: Dict[str, List[str]] = { + "left_arm": ["LEFT_JOINT[1-6]"], + "left_eef": ["LEFT_JOINT[7-8]"], + "right_arm": ["RIGHT_JOINT[1-6]"], + "right_eef": ["RIGHT_JOINT[7-8]"], + } + + solver_cfg: Dict[str, SolverCfg] = { + "left_arm": OPWSolverCfg( + end_link_name="left_link6", + root_link_name="left_arm_base", + tcp=np.array([[-1,0,0,0],[0,-1,0,0],[0,0,1,0.143],[0,0,0,1]])), + "right_arm": OPWSolverCfg( + end_link_name="right_link6", + root_link_name="right_arm_base", + tcp=np.array([[-1,0,0,0],[0,-1,0,0],[0,0,1,0.143],[0,0,0,1]])), + } + + drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg( + stiffness={"LEFT_JOINT[1-6]": 7e4, "RIGHT_JOINT[1-6]": 7e4, + "LEFT_JOINT[7-8]": 3e2, "RIGHT_JOINT[7-8]": 3e2}, + damping={"LEFT_JOINT[1-6]": 1e3, "RIGHT_JOINT[1-6]": 1e3, + "LEFT_JOINT[7-8]": 3e1, "RIGHT_JOINT[7-8]": 3e1}, + max_effort={"LEFT_JOINT[1-6]": 3e6, "RIGHT_JOINT[1-6]": 3e6, + "LEFT_JOINT[7-8]": 3e3, "RIGHT_JOINT[7-8]": 3e3}, + ) + + attrs: RigidBodyAttributesCfg = RigidBodyAttributesCfg( + mass=0.1, static_friction=0.95, dynamic_friction=0.9, + linear_damping=0.7, angular_damping=0.7, + contact_offset=0.001, rest_offset=0.001, + restitution=0.01, max_depenetration_velocity=1e1, + ) + + min_position_iters: int = 8 + min_velocity_iters: int = 2 + + def build_pk_serial_chain(self, device) -> Dict[str, "pk.SerialChain"]: + # Robot-specific PK chain construction + ... + + def build_cfg(self, **overrides) -> RobotCfg: + # Generic implementation from base + ... +``` + +**Reduction**: ~200 LOC (with custom from_dict, _build_default_cfgs, manual attribute setting) -> ~50 LOC of pure declarations. + +### Complex Robot Example — DexforceW1 + +Complex robots with variants (arm_kind x hand_brand x version) use `@property` methods that compute values based on constructor parameters: + +```python +@register_robot("DexforceW1") +class DexforceW1Def: + """DexforceW1 humanoid robot definition with variant support.""" + + name: str = "DexforceW1" + + # Variant parameters (set at construction) + version: DexforceW1Version = DexforceW1Version.V021 + arm_kind: DexforceW1ArmKind = DexforceW1ArmKind.ANTHROPOMORPHIC + hand_types: Dict[DexforceW1ArmSide, DexforceW1HandBrand] = None + include_chassis: bool = True + include_torso: bool = True + include_head: bool = True + include_hand: bool = True + + def __post_init__(self): + if self.hand_types is None: + default = (DexforceW1HandBrand.DH_PGC_GRIPPER_M + if self.arm_kind == DexforceW1ArmKind.INDUSTRIAL + else DexforceW1HandBrand.BRAINCO_HAND) + self.hand_types = { + DexforceW1ArmSide.LEFT: default, + DexforceW1ArmSide.RIGHT: default, + } + + @property + def urdf_cfg(self) -> URDFCfg: + return _build_urdf_cfg( + self.arm_kind, self.hand_types, self.version, + self.include_chassis, self.include_torso, + self.include_head, self.include_hand) + + @property + def control_parts(self) -> Dict[str, List[str]]: + return _build_control_parts( + self.arm_kind, self.include_torso, + self.include_head, self.include_hand) + + @property + def solver_cfg(self) -> Dict[str, SolverCfg]: + return _build_solver_cfg(self.arm_kind, self.version) + + @property + def drive_pros(self) -> JointDrivePropertiesCfg: + return _build_drive_pros(self.arm_kind, self.include_hand) + + @property + def attrs(self) -> RigidBodyAttributesCfg: + return RigidBodyAttributesCfg( + mass=1.0, static_friction=0.95, dynamic_friction=0.9, + linear_damping=0.7, angular_damping=0.7, + contact_offset=0.005, rest_offset=0.001, + restitution=0.05, max_depenetration_velocity=10.0) + + min_position_iters: int = 32 + min_velocity_iters: int = 8 + + def build_pk_serial_chain(self, device) -> Dict[str, "pk.SerialChain"]: + ... + + def build_cfg(self, **overrides) -> RobotCfg: + ... +``` + +Helper functions (`_build_urdf_cfg`, `_build_control_parts`, `_build_solver_cfg`, `_build_drive_pros`) are extracted from the current `_build_default_*` static methods and live alongside the def class (in the same module or a shared utils module within the robot package). + +**Unchanged files**: `types.py` (enums) and `params.py` (kinematics data) remain as-is — they are pure data with no refactoring needed. + +**Reduction**: ~390 LOC cfg.py -> ~80 LOC def.py + extracted helper functions. The `from_dict`, `to_dict`, `save_to_file`, and `_build_default_solver_cfg` methods are all handled by the generic `build_cfg` and base serialization. + +### Robot Registry + +A central registry allows robot lookup by name: + +```python +# embodichain/lab/sim/robots/registry.py + +_ROBOT_REGISTRY: Dict[str, Type[RobotDef]] = {} + +def register_robot(name: str, def_cls: Type[RobotDef]): + """Decorator to register a robot definition class.""" + _ROBOT_REGISTRY[name] = def_cls + return def_cls + +def get_robot_def(name: str, **variant_kwargs) -> RobotDef: + """Look up a robot definition by name and instantiate with variant kwargs.""" + if name not in _ROBOT_REGISTRY: + raise ValueError( + f"Unknown robot: {name}. " + f"Available: {list(_ROBOT_REGISTRY.keys())}") + return _ROBOT_REGISTRY[name](**variant_kwargs) + +def build_robot_cfg(name: str, **kwargs) -> RobotCfg: + """Convenience: look up robot, build cfg, return RobotCfg for the spawner.""" + overrides = kwargs.pop("overrides", {}) + robot_def = get_robot_def(name, **kwargs) + return robot_def.build_cfg(**overrides) +``` + +**Usage**: + +```python +# Simple robot — no variants +cfg = build_robot_cfg("CobotMagic", + overrides={"uid": "my_robot", "init_pos": [0, 0, 1.0]}) +robot = sim.add_robot(cfg=cfg) + +# Complex robot — with variants +cfg = build_robot_cfg("DexforceW1", + arm_kind="anthropomorphic", + overrides={"uid": "w1", "init_pos": [0, 0, 0.5]}) +robot = sim.add_robot(cfg=cfg) + +# Direct instantiation (still works) +cfg = CobotMagicDef().build_cfg(uid="my_robot", init_pos=[0, 0, 1.0]) +``` + +### File Structure + +``` +embodichain/lab/sim/robots/ +├── __init__.py # Updated: import registry, all robot defs +├── protocol.py # NEW: RobotDef protocol + generic build_cfg +├── registry.py # NEW: register_robot decorator + lookup functions +├── cobotmagic.py # REFACTORED: CobotMagicDef replaces CobotMagicCfg +├── dexforce_w1/ +│ ├── __init__.py # Updated: export DexforceW1Def +│ ├── types.py # UNCHANGED +│ ├── params.py # UNCHANGED +│ ├── utils.py # REFACTORED: extract helpers, remove managers now in def +│ └── def.py # NEW: DexforceW1Def class +``` + +### Backward Compatibility + +- Old `CobotMagicCfg` and `DexforceW1Cfg` are kept as thin wrappers that delegate to the new `Def` classes. They will be deprecated after 1-2 release cycles. +- `SimulationManager.add_robot(cfg: RobotCfg)` is completely unchanged. +- Existing task environments importing `CobotMagicCfg` or `DexforceW1Cfg` continue to work without modification. +- `merge_robot_cfg` in `cfg_utils.py` is unchanged. + +### Complexity Reduction Summary + +| Component | Before | After | +|---|---|---| +| CobotMagic | ~200 LOC, custom from_dict + _build_default_cfgs | ~50 LOC flat declarations | +| DexforceW1 cfg.py | ~390 LOC, custom from_dict + 3 builders + serialization | ~80 LOC def + extracted helpers | +| Per-robot from_dict | Custom per robot | Generic build_cfg() from protocol | +| Per-robot to_dict/save_to_file | Custom per robot | Generic serialization from base | +| Robot discovery | Must know import path | Registry lookup by name | diff --git a/embodichain/lab/sim/robots/__init__.py b/embodichain/lab/sim/robots/__init__.py index a6be1a5b..93a9d034 100644 --- a/embodichain/lab/sim/robots/__init__.py +++ b/embodichain/lab/sim/robots/__init__.py @@ -14,5 +14,18 @@ # limitations under the License. # ---------------------------------------------------------------------------- -from .dexforce_w1 import * -from .cobotmagic import CobotMagicCfg +from .protocol import RobotDef +from .registry import register_robot, get_robot_def, build_robot_cfg +from .dexforce_w1 import DexforceW1Cfg, DexforceW1Def +from .cobotmagic import CobotMagicCfg, CobotMagicDef + +__all__ = [ + "RobotDef", + "register_robot", + "get_robot_def", + "build_robot_cfg", + "CobotMagicDef", + "CobotMagicCfg", + "DexforceW1Def", + "DexforceW1Cfg", +] diff --git a/embodichain/lab/sim/robots/cobotmagic.py b/embodichain/lab/sim/robots/cobotmagic.py index ca8e7f6c..d36af646 100644 --- a/embodichain/lab/sim/robots/cobotmagic.py +++ b/embodichain/lab/sim/robots/cobotmagic.py @@ -14,12 +14,14 @@ # limitations under the License. # ---------------------------------------------------------------------------- +"""CobotMagic robot definition using the RobotDef protocol.""" + from __future__ import annotations import torch import numpy as np -from typing import Dict, List, Any, Union +from typing import Dict, Union from embodichain.lab.sim.cfg import ( RobotCfg, @@ -28,32 +30,39 @@ RigidBodyAttributesCfg, ) from embodichain.lab.sim.solvers import SolverCfg, OPWSolverCfg -from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg +from embodichain.lab.sim.robots.protocol import RobotDef +from embodichain.lab.sim.robots.registry import register_robot from embodichain.data import get_data_path -from embodichain.utils import configclass -from embodichain.utils import logger +__all__ = ["CobotMagicDef", "CobotMagicCfg"] -@configclass -class CobotMagicCfg(RobotCfg): - urdf_cfg: URDFCfg = None - control_parts: Dict[str, List[str]] | None = None - solver_cfg: Dict[str, "SolverCfg"] | None = None - @classmethod - def from_dict(cls, init_dict: Dict[str, Union[str, float, int]]) -> CobotMagicCfg: +@register_robot("CobotMagic") +class CobotMagicDef: + """Robot definition for the CobotMagic dual-arm robot. - cfg = cls() - default_cfgs = cls()._build_default_cfgs() - for key, value in default_cfgs.items(): - setattr(cfg, key, value) + This class satisfies the :class:`~embodichain.lab.sim.robots.protocol.RobotDef` + protocol and is registered in the global robot registry under the name + ``"CobotMagic"``. - cfg = merge_robot_cfg(cfg, init_dict) + Attributes: + name: Unique identifier string for this robot. + urdf_cfg: URDF assembly configuration for both arms. + control_parts: Mapping from part name to joint name lists. + solver_cfg: OPW IK solver configuration for each arm. + drive_pros: Joint drive properties (stiffness, damping, max_effort). + attrs: Rigid-body physics attributes. + min_position_iters: Minimum position iterations for the solver. + min_velocity_iters: Minimum velocity iterations for the solver. + """ - return cfg + name: str = "CobotMagic" - @staticmethod - def _build_default_cfgs() -> Dict[str, Any]: + # -- URDF configuration ---------------------------------------------------- + + @property + def urdf_cfg(self) -> URDFCfg: + """URDF assembly configuration for both arms.""" arm_urdf = get_data_path("CobotMagicArm/CobotMagicWithGripperV100.urdf") left_arm_xpos = np.array( [ @@ -71,7 +80,7 @@ def _build_default_cfgs() -> Dict[str, Any]: [0.0, 0.0, 0.0, 1.000], ] ) - urdf_cfg = URDFCfg( + return URDFCfg( components=[ { "component_type": "left_arm", @@ -85,83 +94,106 @@ def _build_default_cfgs() -> Dict[str, Any]: }, ] ) - return { - "uid": "CobotMagic", - "urdf_cfg": urdf_cfg, - "control_parts": { - "left_arm": [ - "LEFT_JOINT1", - "LEFT_JOINT2", - "LEFT_JOINT3", - "LEFT_JOINT4", - "LEFT_JOINT5", - "LEFT_JOINT6", - ], - "left_eef": ["LEFT_JOINT7", "LEFT_JOINT8"], - "right_arm": [ - "RIGHT_JOINT1", - "RIGHT_JOINT2", - "RIGHT_JOINT3", - "RIGHT_JOINT4", - "RIGHT_JOINT5", - "RIGHT_JOINT6", - ], - "right_eef": ["RIGHT_JOINT7", "RIGHT_JOINT8"], - }, - "solver_cfg": { - "left_arm": OPWSolverCfg( - end_link_name="left_link6", - root_link_name="left_arm_base", - tcp=np.array( - [[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0.143], [0, 0, 0, 1]] - ), - ), - "right_arm": OPWSolverCfg( - end_link_name="right_link6", - root_link_name="right_arm_base", - tcp=np.array( - [[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0.143], [0, 0, 0, 1]] - ), - ), - }, - "min_position_iters": 8, - "min_velocity_iters": 2, - "drive_pros": JointDrivePropertiesCfg( - stiffness={ - "LEFT_JOINT[1-6]": 7e4, - "RIGHT_JOINT[1-6]": 7e4, - "LEFT_JOINT[7-8]": 3e2, - "RIGHT_JOINT[7-8]": 3e2, - }, - damping={ - "LEFT_JOINT[1-6]": 1e3, - "RIGHT_JOINT[1-6]": 1e3, - "LEFT_JOINT[7-8]": 3e1, - "RIGHT_JOINT[7-8]": 3e1, - }, - max_effort={ - "LEFT_JOINT[1-6]": 3e6, - "RIGHT_JOINT[1-6]": 3e6, - "LEFT_JOINT[7-8]": 3e3, - "RIGHT_JOINT[7-8]": 3e3, - }, + + # -- Control parts --------------------------------------------------------- + + control_parts: dict[str, list[str]] = { + "left_arm": [ + "LEFT_JOINT1", + "LEFT_JOINT2", + "LEFT_JOINT3", + "LEFT_JOINT4", + "LEFT_JOINT5", + "LEFT_JOINT6", + ], + "left_eef": ["LEFT_JOINT7", "LEFT_JOINT8"], + "right_arm": [ + "RIGHT_JOINT1", + "RIGHT_JOINT2", + "RIGHT_JOINT3", + "RIGHT_JOINT4", + "RIGHT_JOINT5", + "RIGHT_JOINT6", + ], + "right_eef": ["RIGHT_JOINT7", "RIGHT_JOINT8"], + } + + # -- Solver configuration -------------------------------------------------- + + solver_cfg: dict[str, SolverCfg] = { + "left_arm": OPWSolverCfg( + end_link_name="left_link6", + root_link_name="left_arm_base", + tcp=np.array( + [[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0.143], [0, 0, 0, 1]] ), - "attrs": RigidBodyAttributesCfg( - mass=0.1, - static_friction=0.95, - dynamic_friction=0.9, - linear_damping=0.7, - angular_damping=0.7, - contact_offset=0.001, - rest_offset=0.001, - restitution=0.01, - max_depenetration_velocity=1e1, + ), + "right_arm": OPWSolverCfg( + end_link_name="right_link6", + root_link_name="right_arm_base", + tcp=np.array( + [[-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0.143], [0, 0, 0, 1]] ), - } + ), + } + + # -- Drive properties ------------------------------------------------------ + + drive_pros: JointDrivePropertiesCfg = JointDrivePropertiesCfg( + stiffness={ + "LEFT_JOINT[1-6]": 7e4, + "RIGHT_JOINT[1-6]": 7e4, + "LEFT_JOINT[7-8]": 3e2, + "RIGHT_JOINT[7-8]": 3e2, + }, + damping={ + "LEFT_JOINT[1-6]": 1e3, + "RIGHT_JOINT[1-6]": 1e3, + "LEFT_JOINT[7-8]": 3e1, + "RIGHT_JOINT[7-8]": 3e1, + }, + max_effort={ + "LEFT_JOINT[1-6]": 3e6, + "RIGHT_JOINT[1-6]": 3e6, + "LEFT_JOINT[7-8]": 3e3, + "RIGHT_JOINT[7-8]": 3e3, + }, + ) + + # -- Rigid-body attributes ------------------------------------------------- + + attrs: RigidBodyAttributesCfg = RigidBodyAttributesCfg( + mass=0.1, + static_friction=0.95, + dynamic_friction=0.9, + linear_damping=0.7, + angular_damping=0.7, + contact_offset=0.001, + rest_offset=0.001, + restitution=0.01, + max_depenetration_velocity=1e1, + ) + + # -- Extra fields ---------------------------------------------------------- + + min_position_iters: int = 8 + min_velocity_iters: int = 2 + + # -- Methods --------------------------------------------------------------- def build_pk_serial_chain( self, device: torch.device = torch.device("cpu"), **kwargs ) -> Dict[str, "pk.SerialChain"]: + """Build pytorch-kinematics serial chains for each arm. + + Args: + device: Torch device to place chains on. + **kwargs: Additional keyword arguments (unused). + + Returns: + Dictionary mapping ``"left_arm"`` and ``"right_arm"`` to their + respective serial chain objects. + """ from embodichain.lab.sim.utility.solver_utils import ( create_pk_chain, create_pk_serial_chain, @@ -178,11 +210,49 @@ def build_pk_serial_chain( ).to(device=device) return {"left_arm": left_arm_chain, "right_arm": right_arm_chain} + def build_cfg(self, **overrides: object) -> RobotCfg: + """Build a :class:`RobotCfg` from this robot definition. + + Delegates to :meth:`RobotDef.build_cfg` for the default + implementation. + + Args: + **overrides: Optional overrides applied on top of the defaults. + + Returns: + A fully-populated :class:`RobotCfg`. + """ + return RobotDef.build_cfg(self, **overrides) + + +class CobotMagicCfg(RobotCfg): + """Backward-compatible wrapper around :class:`CobotMagicDef`. + + Existing code that calls ``CobotMagicCfg.from_dict(...)`` continues to + work via this thin shim that delegates to the registered + :class:`CobotMagicDef`. + """ + + urdf_cfg: URDFCfg = None + control_parts: Dict[str, list[str]] | None = None + solver_cfg: Dict[str, "SolverCfg"] | None = None + + @classmethod + def from_dict(cls, init_dict: Dict[str, Union[str, float, int]]) -> CobotMagicCfg: + """Create a :class:`RobotCfg` via :class:`CobotMagicDef`. + + Args: + init_dict: Dictionary of configuration overrides. + + Returns: + A fully-populated :class:`RobotCfg` with overrides applied. + """ + return CobotMagicDef().build_cfg(**init_dict) + if __name__ == "__main__": from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.cfg import RenderCfg - from embodichain.lab.sim.robots import CobotMagicCfg torch.set_printoptions(precision=5, sci_mode=False) @@ -194,11 +264,11 @@ def build_pk_serial_chain( ) sim = SimulationManager(config) - config = { + overrides = { "init_pos": [0.0, 0.0, 1.0], } - cfg = CobotMagicCfg.from_dict(config) + cfg = CobotMagicDef().build_cfg(**overrides) robot = sim.add_robot(cfg=cfg) print("CobotMagic added to the simulation.") diff --git a/embodichain/lab/sim/robots/dexforce_w1/__init__.py b/embodichain/lab/sim/robots/dexforce_w1/__init__.py index ee5df793..295605fb 100644 --- a/embodichain/lab/sim/robots/dexforce_w1/__init__.py +++ b/embodichain/lab/sim/robots/dexforce_w1/__init__.py @@ -15,3 +15,4 @@ # ---------------------------------------------------------------------------- from .cfg import DexforceW1Cfg +from .robot_def import DexforceW1Def diff --git a/embodichain/lab/sim/robots/dexforce_w1/cfg.py b/embodichain/lab/sim/robots/dexforce_w1/cfg.py index 40f95b09..17379d4f 100644 --- a/embodichain/lab/sim/robots/dexforce_w1/cfg.py +++ b/embodichain/lab/sim/robots/dexforce_w1/cfg.py @@ -58,39 +58,29 @@ def from_dict( ) -> DexforceW1Cfg: """Initialize DexforceW1Cfg from a dictionary. + Delegates to :class:`DexforceW1Def` for backward compatibility. + Args: init_dict (Dict[str, str | float | tuple | dict): Dictionary of configuration parameters. Returns: DexforceW1Cfg: An instance of DexforceW1Cfg with parameters set. """ - - init_dict_m = init_dict.copy() - version = init_dict_m.get("version", "v021") - arm_kind = init_dict_m.get("arm_kind", "anthropomorphic") - with_default_eef = init_dict_m.get("with_default_eef", True) - init_dict_m.pop("version", None) - init_dict_m.pop("arm_kind", None) - init_dict_m.pop("with_default_eef", None) - - cfg: DexforceW1Cfg = cls()._build_default_cfg( - version=version, arm_kind=arm_kind, with_default_eef=with_default_eef - ) - - default_physics_cfgs = cls()._build_default_physics_cfgs( - arm_kind=arm_kind, with_default_eef=with_default_eef - ) - for key, value in default_physics_cfgs.items(): - setattr(cfg, key, value) - - default_solver_cfg = cls()._build_default_solver_cfg( - is_industrial=(arm_kind == "industrial") - ) - cfg.solver_cfg = default_solver_cfg - - cfg = merge_robot_cfg(cfg, init_dict_m) - - return cfg + from embodichain.lab.sim.robots.dexforce_w1.robot_def import DexforceW1Def + + version = init_dict.get("version", "v021") + arm_kind = init_dict.get("arm_kind", "anthropomorphic") + with_default_eef = init_dict.get("with_default_eef", True) + remaining = { + k: v + for k, v in init_dict.items() + if k not in ("version", "arm_kind", "with_default_eef") + } + return DexforceW1Def( + version=version, + arm_kind=arm_kind, + include_hand=with_default_eef, + ).build_cfg(**remaining) @staticmethod def _build_default_solver_cfg(is_industrial: bool) -> SolverCfg: diff --git a/embodichain/lab/sim/robots/dexforce_w1/robot_def.py b/embodichain/lab/sim/robots/dexforce_w1/robot_def.py new file mode 100644 index 00000000..32abece9 --- /dev/null +++ b/embodichain/lab/sim/robots/dexforce_w1/robot_def.py @@ -0,0 +1,426 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""DexforceW1 robot definition using the RobotDef protocol.""" + +from __future__ import annotations + +import numpy as np +import torch +import typing + +from typing import Dict + +from embodichain.lab.sim.cfg import ( + RobotCfg, + URDFCfg, + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, +) +from embodichain.lab.sim.solvers import SolverCfg, SRSSolverCfg +from embodichain.lab.sim.robots.protocol import RobotDef +from embodichain.lab.sim.robots.registry import register_robot +from embodichain.lab.sim.robots.dexforce_w1.types import ( + DexforceW1ArmKind, + DexforceW1ArmSide, + DexforceW1Version, + DexforceW1HandBrand, +) +from embodichain.lab.sim.robots.dexforce_w1.utils import ( + build_dexforce_w1_assembly_urdf_cfg, + build_dexforce_w1_cfg, +) +from embodichain.data import get_data_path + +__all__ = ["DexforceW1Def"] + + +# --------------------------------------------------------------------------- +# Helper functions extracted from cfg.py +# --------------------------------------------------------------------------- + + +def _build_solver_cfg( + arm_kind: str, +) -> Dict[str, SRSSolverCfg]: + """Build SRS solver configurations for both arms. + + Args: + arm_kind: Arm type string, either ``"industrial"`` or + ``"anthropomorphic"``. + + Returns: + Dictionary mapping ``"left_arm"`` and ``"right_arm"`` to their + respective :class:`SRSSolverCfg` instances. + """ + from embodichain.lab.sim.robots.dexforce_w1.params import W1ArmKineParams + + is_industrial = arm_kind == "industrial" + enum_arm_kind = ( + DexforceW1ArmKind.INDUSTRIAL + if is_industrial + else DexforceW1ArmKind.ANTHROPOMORPHIC + ) + + w1_left_arm_params = W1ArmKineParams( + arm_side=DexforceW1ArmSide.LEFT, + arm_kind=enum_arm_kind, + version=DexforceW1Version.V021, + ) + w1_right_arm_params = W1ArmKineParams( + arm_side=DexforceW1ArmSide.RIGHT, + arm_kind=enum_arm_kind, + version=DexforceW1Version.V021, + ) + + if is_industrial: + left_arm_tcp = np.array( + [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.15], + [0.0, 0.0, 0.0, 1.0], + ] + ) + right_arm_tcp = np.array( + [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.15], + [0.0, 0.0, 0.0, 1.0], + ] + ) + else: + left_arm_tcp = np.array( + [ + [-1.0, 0.0, 0.0, 0.012], + [0.0, 0.0, 1.0, 0.0675], + [0.0, 1.0, 0.0, 0.127], + [0.0, 0.0, 0.0, 1.0], + ] + ) + right_arm_tcp = np.array( + [ + [1.0, 0.0, 0.0, 0.012], + [0.0, 0.0, -1.0, -0.0675], + [0.0, 1.0, 0.0, 0.127], + [0.0, 0.0, 0.0, 1.0], + ] + ) + + return { + "right_arm": SRSSolverCfg( + end_link_name="right_ee", + root_link_name="right_arm_base", + dh_params=w1_right_arm_params.dh_params, + user_qpos_limits=w1_right_arm_params.qpos_limits, + T_e_oe=w1_right_arm_params.T_e_oe, + T_b_ob=w1_right_arm_params.T_b_ob, + link_lengths=w1_right_arm_params.link_lengths, + rotation_directions=w1_right_arm_params.rotation_directions, + tcp=right_arm_tcp, + ), + "left_arm": SRSSolverCfg( + end_link_name="left_ee", + root_link_name="left_arm_base", + dh_params=w1_left_arm_params.dh_params, + user_qpos_limits=w1_left_arm_params.qpos_limits, + T_e_oe=w1_left_arm_params.T_e_oe, + T_b_ob=w1_left_arm_params.T_b_ob, + link_lengths=w1_left_arm_params.link_lengths, + rotation_directions=w1_left_arm_params.rotation_directions, + tcp=left_arm_tcp, + ), + } + + +def _build_drive_pros( + arm_kind: str, + include_hand: bool = True, +) -> JointDrivePropertiesCfg: + """Build joint drive properties for DexforceW1. + + Args: + arm_kind: Arm type string, either ``"industrial"`` or + ``"anthropomorphic"``. + include_hand: Whether to include end-effector drive parameters. + + Returns: + A :class:`JointDrivePropertiesCfg` with appropriate stiffness, + damping, and max_effort settings. + """ + DEFAULT_EEF_JOINT_DRIVE_PARAMS = { + "stiffness": 1e2, + "damping": 1e1, + "max_effort": 1e3, + } + + DEFAULT_EEF_HAND_JOINT_NAMES = ( + "(LEFT|RIGHT)_HAND_(THUMB[12]|INDEX|MIDDLE|RING|PINKY)" + ) + + DEFAULT_EEF_GRIPPER_JOINT_NAMES = "(LEFT|RIGHT)_FINGER[1-2]" + + ARM_JOINTS = "(RIGHT|LEFT)_J[0-9]" + BODY_JOINTS = "(ANKLE|KNEE|BUTTOCK|WAIST)" + + joint_params = { + "stiffness": { + ARM_JOINTS: 1e4, + BODY_JOINTS: 1e7, + }, + "damping": { + ARM_JOINTS: 1e3, + BODY_JOINTS: 1e4, + }, + "max_effort": { + ARM_JOINTS: 1e5, + BODY_JOINTS: 1e10, + }, + } + + drive_pros = JointDrivePropertiesCfg(**joint_params) + + if include_hand: + eef_joint_names = ( + DEFAULT_EEF_HAND_JOINT_NAMES + if arm_kind == "anthropomorphic" + else DEFAULT_EEF_GRIPPER_JOINT_NAMES + ) + drive_pros.stiffness.update( + {eef_joint_names: DEFAULT_EEF_JOINT_DRIVE_PARAMS["stiffness"]} + ) + drive_pros.damping.update( + {eef_joint_names: DEFAULT_EEF_JOINT_DRIVE_PARAMS["damping"]} + ) + drive_pros.max_effort.update( + {eef_joint_names: DEFAULT_EEF_JOINT_DRIVE_PARAMS["max_effort"]} + ) + + return drive_pros + + +def _build_attrs() -> RigidBodyAttributesCfg: + """Build default rigid-body physics attributes for DexforceW1. + + Returns: + A :class:`RigidBodyAttributesCfg` with default physics parameters. + """ + return RigidBodyAttributesCfg( + mass=1.0, + static_friction=0.95, + dynamic_friction=0.9, + linear_damping=0.7, + angular_damping=0.7, + contact_offset=0.005, + rest_offset=0.001, + restitution=0.05, + max_depenetration_velocity=10.0, + ) + + +# --------------------------------------------------------------------------- +# DexforceW1Def +# --------------------------------------------------------------------------- + + +@register_robot("DexforceW1") +class DexforceW1Def: + """Robot definition for the DexforceW1 humanoid robot. + + This class satisfies the :class:`~embodichain.lab.sim.robots.protocol.RobotDef` + protocol and is registered in the global robot registry under the name + ``"DexforceW1"``. + + The DexforceW1 robot supports multiple variants determined by *version*, + *arm_kind*, and optional *hand_types*. All configuration-building logic + delegates to the existing utility functions in + :mod:`embodichain.lab.sim.robots.dexforce_w1.utils`. + + Attributes: + name: Unique identifier string for this robot. + version: Component version string (e.g. ``"v021"``). + arm_kind: Arm type, either ``"industrial"`` or ``"anthropomorphic"``. + hand_types: Optional mapping from arm side to hand brand enum. + include_chassis: Whether the chassis component is included. + include_torso: Whether the torso component is included. + include_head: Whether the head component is included. + include_hand: Whether the hand end-effectors are included. + min_position_iters: Minimum position iterations for the solver. + min_velocity_iters: Minimum velocity iterations for the solver. + """ + + name: str = "DexforceW1" + + def __init__( + self, + version: str = "v021", + arm_kind: str = "anthropomorphic", + hand_types: dict | None = None, + include_chassis: bool = True, + include_torso: bool = True, + include_head: bool = True, + include_hand: bool = True, + ) -> None: + self.version = version + self.arm_kind = arm_kind + self.include_chassis = include_chassis + self.include_torso = include_torso + self.include_head = include_head + self.include_hand = include_hand + self._hand_types = hand_types + self.__post_init__() + + def __post_init__(self) -> None: + """Fill default hand_types based on arm_kind if not explicitly set.""" + if self._hand_types is None: + if self.arm_kind == "industrial": + self._hand_types = { + DexforceW1ArmSide.LEFT: DexforceW1HandBrand.DH_PGC_GRIPPER_M, + DexforceW1ArmSide.RIGHT: DexforceW1HandBrand.DH_PGC_GRIPPER_M, + } + else: + self._hand_types = { + DexforceW1ArmSide.LEFT: DexforceW1HandBrand.BRAINCO_HAND, + DexforceW1ArmSide.RIGHT: DexforceW1HandBrand.BRAINCO_HAND, + } + + # -- URDF configuration ---------------------------------------------------- + + @property + def urdf_cfg(self) -> URDFCfg: + """URDF assembly configuration for the full robot.""" + arm_kind_enum = DexforceW1ArmKind(self.arm_kind) + version_enum = DexforceW1Version(self.version) + hand_versions = { + DexforceW1ArmSide.LEFT: version_enum, + DexforceW1ArmSide.RIGHT: version_enum, + } + return build_dexforce_w1_assembly_urdf_cfg( + arm_kind=arm_kind_enum, + hand_types=self._hand_types, + hand_versions=hand_versions, + include_chassis=self.include_chassis, + include_torso=self.include_torso, + include_head=self.include_head, + include_hand=self.include_hand, + ) + + # -- Control parts --------------------------------------------------------- + + @property + def control_parts(self) -> dict[str, list[str]]: + """Mapping from part name to joint name lists.""" + arm_kind_enum = DexforceW1ArmKind(self.arm_kind) + version_enum = DexforceW1Version(self.version) + hand_versions = { + DexforceW1ArmSide.LEFT: version_enum, + DexforceW1ArmSide.RIGHT: version_enum, + } + cfg = build_dexforce_w1_cfg( + arm_kind=arm_kind_enum, + hand_types=self._hand_types, + hand_versions=hand_versions, + include_chassis=self.include_chassis, + include_torso=self.include_torso, + include_head=self.include_head, + include_hand=self.include_hand, + ) + return cfg.control_parts + + # -- Solver configuration -------------------------------------------------- + + @property + def solver_cfg(self) -> Dict[str, SRSSolverCfg]: + """SRS IK solver configuration for each arm.""" + return _build_solver_cfg(arm_kind=self.arm_kind) + + # -- Drive properties ------------------------------------------------------ + + @property + def drive_pros(self) -> JointDrivePropertiesCfg: + """Joint drive properties (stiffness, damping, max_effort).""" + return _build_drive_pros( + arm_kind=self.arm_kind, + include_hand=self.include_hand, + ) + + # -- Rigid-body attributes ------------------------------------------------- + + @property + def attrs(self) -> RigidBodyAttributesCfg: + """Rigid-body physics attributes.""" + return _build_attrs() + + # -- Extra fields ---------------------------------------------------------- + + min_position_iters: int = 32 + min_velocity_iters: int = 8 + + # -- Methods --------------------------------------------------------------- + + def build_pk_serial_chain( + self, device: torch.device = torch.device("cpu"), **kwargs + ) -> Dict[str, "pk.SerialChain"]: + """Build pytorch-kinematics serial chains for each arm. + + Args: + device: Torch device to place chains on. + **kwargs: Additional keyword arguments (unused). + + Returns: + Dictionary mapping ``"left_arm"`` and ``"right_arm"`` to their + respective serial chain objects. + """ + from embodichain.lab.sim.utility.solver_utils import ( + create_pk_chain, + create_pk_serial_chain, + ) + + arm_kind_enum = DexforceW1ArmKind(self.arm_kind) + + if arm_kind_enum == DexforceW1ArmKind.INDUSTRIAL: + urdf_path = get_data_path("DexforceW1V021/DexforceW1_v02_2.urdf") + else: + urdf_path = get_data_path("DexforceW1V021/DexforceW1_v02_1.urdf") + + chain = create_pk_chain(urdf_path, device) + + left_arm_chain = create_pk_serial_chain( + chain=chain, end_link_name="left_ee", root_link_name="left_arm_base" + ).to(device=device) + right_arm_chain = create_pk_serial_chain( + chain=chain, end_link_name="right_ee", root_link_name="right_arm_base" + ).to(device=device) + + return { + "left_arm": left_arm_chain, + "right_arm": right_arm_chain, + } + + def build_cfg(self, **overrides: object) -> RobotCfg: + """Build a :class:`RobotCfg` from this robot definition. + + Delegates to :meth:`RobotDef.build_cfg` for the default + implementation. + + Args: + **overrides: Optional overrides applied on top of the defaults. + + Returns: + A fully-populated :class:`RobotCfg`. + """ + return RobotDef.build_cfg(self, **overrides) diff --git a/embodichain/lab/sim/robots/protocol.py b/embodichain/lab/sim/robots/protocol.py new file mode 100644 index 00000000..1a4c9b67 --- /dev/null +++ b/embodichain/lab/sim/robots/protocol.py @@ -0,0 +1,192 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""RobotDef protocol for EmbodiChain robot definitions. + +Every robot in the framework should implement the :class:`RobotDef` protocol. +The :meth:`build_cfg` method converts a ``RobotDef``-compatible object into a +:class:`~embodichain.lab.sim.cfg.RobotCfg` which +:meth:`~embodichain.lab.sim.SimulationManager.add_robot` expects. +""" + +from __future__ import annotations + +import torch +from typing import Dict, TYPE_CHECKING, runtime_checkable, Protocol + +from embodichain.lab.sim.cfg import ( + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, + RobotCfg, + URDFCfg, +) +from embodichain.lab.sim.solvers import SolverCfg +from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg + +if TYPE_CHECKING: + pass + +__all__ = ["RobotDef"] + +# Fields that may exist on both the protocol implementer and RobotCfg and +# should be copied across during ``build_cfg``. +_EXTRA_CFG_FIELDS: tuple[str, ...] = ( + "min_position_iters", + "min_velocity_iters", + "fix_base", + "disable_self_collision", + "build_pk_chain", + "init_qpos", + "body_scale", +) + + +@runtime_checkable +class RobotDef(Protocol): + """Structural protocol that every robot definition must satisfy. + + Properties: + name: Unique identifier string for this robot. + urdf_cfg: URDF assembly configuration. + control_parts: Mapping from part name to joint name lists. + solver_cfg: Solver configuration (single or per-part). + drive_pros: Joint drive properties. + attrs: Rigid-body physics attributes. + + Methods: + build_pk_serial_chain: Build pytorch-kinematics serial chains. + build_cfg: Convert this definition into a :class:`RobotCfg`. + """ + + # -- required properties -------------------------------------------------- + + @property + def name(self) -> str: + """Unique identifier for this robot definition.""" + ... + + @property + def urdf_cfg(self) -> URDFCfg | None: + """URDF assembly configuration.""" + ... + + @property + def control_parts(self) -> dict[str, list[str]] | None: + """Mapping from part name to joint name lists.""" + ... + + @property + def solver_cfg(self) -> SolverCfg | dict[str, SolverCfg] | None: + """Solver configuration for IK/FK computation.""" + ... + + @property + def drive_pros(self) -> JointDrivePropertiesCfg: + """Joint drive properties (stiffness, damping, etc.).""" + ... + + @property + def attrs(self) -> RigidBodyAttributesCfg: + """Rigid-body physics attributes.""" + ... + + # -- required methods ----------------------------------------------------- + + def build_pk_serial_chain( + self, device: torch.device = torch.device("cpu") + ) -> Dict[str, object]: + """Build pytorch-kinematics serial chains for each control part. + + Args: + device: Torch device to place chains on. + + Returns: + Dictionary mapping part name to serial chain objects. + """ + ... + + def build_cfg(self, **overrides: object) -> RobotCfg: + """Convert this robot definition into a :class:`RobotCfg`. + + The method creates a new :class:`RobotCfg`, copies protocol + properties and any extra fields that exist on both *self* and + the config, optionally overrides ``build_pk_serial_chain``, and + finally applies any remaining keyword overrides via + :func:`~embodichain.lab.sim.utility.cfg_utils.merge_robot_cfg`. + + Args: + **overrides: Optional overrides applied on top of the defaults. + ``uid`` is consumed first to set the config uid. + + Returns: + A fully-populated :class:`RobotCfg` ready for + :meth:`~embodichain.lab.sim.SimulationManager.add_robot`. + """ + ... + + +# --------------------------------------------------------------------------- +# Mixin-style default implementation so concrete classes can simply inherit +# and call ``super().build_cfg(**overrides)``. +# --------------------------------------------------------------------------- + + +class _RobotDefMixin: + """Mixin that provides the default ``build_cfg`` implementation. + + This is *not* part of the public API. Concrete robot classes inherit + from this mixin (and satisfy the ``RobotDef`` protocol via duck-typing) + so they only need to declare the required properties. + """ + + def build_cfg(self, **overrides: object) -> RobotCfg: + """Build a :class:`RobotCfg` from this robot definition. + + See :meth:`RobotDef.build_cfg` for full documentation. + """ + cfg = RobotCfg() + + # (a) Set uid from overrides or fall back to the protocol name. + cfg.uid = overrides.pop("uid", self.name) + + # (b) Copy core protocol properties. + cfg.urdf_cfg = self.urdf_cfg + cfg.control_parts = self.control_parts + cfg.solver_cfg = self.solver_cfg + cfg.drive_pros = self.drive_pros + cfg.attrs = self.attrs + + # (c) Copy extra scalar/flag fields that exist on both self and + # RobotCfg. + for field_name in _EXTRA_CFG_FIELDS: + if hasattr(self, field_name) and hasattr(cfg, field_name): + setattr(cfg, field_name, getattr(self, field_name)) + + # (d) Override build_pk_serial_chain on the cfg instance if self + # provides one. + if hasattr(self, "build_pk_serial_chain"): + cfg.build_pk_serial_chain = self.build_pk_serial_chain # type: ignore[assignment] + + # (e) Merge remaining overrides. + if overrides: + cfg = merge_robot_cfg(cfg, overrides) + + return cfg + + +# Patch the mixin onto the protocol so callers can do RobotDef.build_cfg(self, ...) +# or simply inherit from the mixin for the default behaviour. +RobotDef.build_cfg = _RobotDefMixin.build_cfg # type: ignore[assignment] diff --git a/embodichain/lab/sim/robots/registry.py b/embodichain/lab/sim/robots/registry.py new file mode 100644 index 00000000..8865e110 --- /dev/null +++ b/embodichain/lab/sim/robots/registry.py @@ -0,0 +1,130 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Global robot registry for name-based lookup and instantiation. + +The registry allows robot definitions to be registered by a unique string +name so that callers do not need to know the full import path. Typical +usage:: + + from embodichain.lab.sim.robots.registry import register_robot, get_robot_def + + @register_robot("my_robot") + class MyRobotDef: + ... + + robot = get_robot_def("my_robot") + cfg = robot.build_cfg() +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Type + +from embodichain.utils import logger + +if TYPE_CHECKING: + from embodichain.lab.sim.cfg import RobotCfg + +__all__ = ["register_robot", "get_robot_def", "build_robot_cfg"] + +# --------------------------------------------------------------------------- +# Internal storage +# --------------------------------------------------------------------------- + +_ROBOT_REGISTRY: dict[str, Type] = {} + +# --------------------------------------------------------------------------- +# Public API +# --------------------------------------------------------------------------- + + +def register_robot(name: str): + """Class decorator that registers a robot definition under *name*. + + If *name* is already present in the registry a warning is logged and the + existing entry is overwritten. + + Args: + name: Unique string identifier for the robot definition. + + Returns: + The original class, unmodified. + """ + + def _decorator(cls: Type) -> Type: + if name in _ROBOT_REGISTRY: + logger.warning( + "Robot '%s' is already registered; overwriting with %s.", + name, + cls.__qualname__, + ) + _ROBOT_REGISTRY[name] = cls + return cls + + return _decorator + + +def get_robot_def(name: str, **variant_kwargs: object) -> object: + """Look up a robot definition by name and instantiate it. + + Args: + name: Registered robot name. + **variant_kwargs: Keyword arguments forwarded to the robot class + constructor. + + Returns: + An instance of the registered robot class. + + Raises: + ValueError: If *name* is not found in the registry. + """ + if name not in _ROBOT_REGISTRY: + raise ValueError( + f"Unknown robot '{name}'. Available: {sorted(_ROBOT_REGISTRY.keys())}" + ) + cls = _ROBOT_REGISTRY[name] + return cls(**variant_kwargs) + + +def build_robot_cfg(name: str, **kwargs: object) -> RobotCfg: + """Convenience helper that builds a :class:`RobotCfg` from a registered robot. + + The ``overrides`` key is extracted from *kwargs* (if present) and passed + to the robot's :meth:`build_cfg` method. All remaining kwargs are + forwarded to :func:`get_robot_def` for instantiation. + + Args: + name: Registered robot name. + **kwargs: Forwarded to the constructor and/or ``build_cfg``. The + special key ``overrides`` is a dict passed to ``build_cfg``. + + Returns: + A :class:`~embodichain.lab.sim.cfg.RobotCfg` produced by the + robot's ``build_cfg`` method. + + Raises: + ValueError: If *name* is not found in the registry. + """ + from embodichain.lab.sim.cfg import ( + RobotCfg, + ) # noqa: F811 – avoid circular import at module level + + overrides = kwargs.pop("overrides", {}) + robot = get_robot_def(name, **kwargs) + cfg = robot.build_cfg(**overrides) + assert isinstance(cfg, RobotCfg) + return cfg diff --git a/tests/sim/robots/__init__.py b/tests/sim/robots/__init__.py new file mode 100644 index 00000000..dd650e90 --- /dev/null +++ b/tests/sim/robots/__init__.py @@ -0,0 +1,15 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- diff --git a/tests/sim/robots/test_protocol.py b/tests/sim/robots/test_protocol.py new file mode 100644 index 00000000..74060dea --- /dev/null +++ b/tests/sim/robots/test_protocol.py @@ -0,0 +1,326 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tests for the RobotDef protocol, its build_cfg method, and the robot registry.""" + +from __future__ import annotations + +import numpy as np +import pytest + +from embodichain.lab.sim.cfg import ( + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, + RobotCfg, + URDFCfg, +) +from embodichain.lab.sim.robots.protocol import RobotDef +from embodichain.lab.sim.solvers import SolverCfg + +# --------------------------------------------------------------------------- +# Minimal concrete implementation for testing +# --------------------------------------------------------------------------- + + +class _StubRobotDef: + """A minimal concrete class that satisfies the RobotDef protocol.""" + + def __init__( + self, + name: str = "test_robot", + urdf_cfg: URDFCfg | None = None, + control_parts: dict[str, list[str]] | None = None, + solver_cfg: dict[str, SolverCfg] | SolverCfg | None = None, + drive_pros: JointDrivePropertiesCfg | None = None, + attrs: RigidBodyAttributesCfg | None = None, + min_position_iters: int = 4, + min_velocity_iters: int = 1, + fix_base: bool = True, + disable_self_collision: bool = True, + build_pk_chain: bool = True, + init_qpos: object | None = None, + body_scale: tuple[float, ...] = (1.0, 1.0, 1.0), + ) -> None: + self.name = name + self.urdf_cfg = urdf_cfg + self.control_parts = control_parts + self.solver_cfg = solver_cfg + self.drive_pros = drive_pros or JointDrivePropertiesCfg() + self.attrs = attrs or RigidBodyAttributesCfg() + self.min_position_iters = min_position_iters + self.min_velocity_iters = min_velocity_iters + self.fix_base = fix_base + self.disable_self_collision = disable_self_collision + self.build_pk_chain = build_pk_chain + self.init_qpos = init_qpos + self.body_scale = body_scale + + +def _make_stub(**kwargs) -> _StubRobotDef: + """Create a stub with sensible defaults for testing.""" + defaults = dict( + control_parts={"arm": ["joint1", "joint2"]}, + drive_pros=JointDrivePropertiesCfg(stiffness=5e3, damping=5e2), + attrs=RigidBodyAttributesCfg(mass=2.0), + min_position_iters=8, + min_velocity_iters=2, + fix_base=False, + disable_self_collision=False, + body_scale=(2.0, 2.0, 2.0), + ) + defaults.update(kwargs) + return _StubRobotDef(**defaults) + + +# --------------------------------------------------------------------------- +# Tests +# --------------------------------------------------------------------------- + + +class TestRobotDefProtocol: + """Verify the RobotDef protocol is satisfied and build_cfg works.""" + + def test_build_cfg_returns_robot_cfg(self) -> None: + """Verify build_cfg returns a RobotCfg instance with correct uid.""" + stub = _make_stub(name="my_bot") + cfg = RobotDef.build_cfg(stub) + + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "my_bot" + + def test_build_cfg_applies_overrides(self) -> None: + """Verify init_pos override works.""" + stub = _make_stub(name="my_bot") + cfg = RobotDef.build_cfg(stub, init_pos=(1.0, 2.0, 3.0)) + + assert cfg.init_pos == (1.0, 2.0, 3.0) + + def test_build_cfg_preserves_control_parts(self) -> None: + """Verify control_parts are copied into the resulting RobotCfg.""" + parts = {"arm": ["j1", "j2"], "hand": ["j3"]} + stub = _make_stub(control_parts=parts) + cfg = RobotDef.build_cfg(stub) + + assert cfg.control_parts == parts + + def test_build_cfg_merges_drive_pros(self) -> None: + """Verify partial drive_pros merge via overrides dict.""" + stub = _make_stub() + cfg = RobotDef.build_cfg(stub, drive_pros={"stiffness": 9e3}) + + # The merge should update stiffness while preserving other fields + assert cfg.drive_pros.stiffness == 9e3 + # damping should still be from the original stub drive_pros + assert cfg.drive_pros.damping == 5e2 + + def test_build_cfg_with_solver_cfg_override(self) -> None: + """Verify solver_cfg TCP override via merge.""" + from embodichain.lab.sim.solvers import PytorchSolverCfg + + original_solver = { + "arm": PytorchSolverCfg( + end_link_name="ee", + root_link_name="base", + tcp=np.eye(4), + ) + } + stub = _make_stub(solver_cfg=original_solver) + + override_tcp = np.array( + [ + [1.0, 0.0, 0.0, 0.1], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + cfg = RobotDef.build_cfg( + stub, + solver_cfg={"arm": {"tcp": override_tcp}}, + ) + + np.testing.assert_array_almost_equal(cfg.solver_cfg["arm"].tcp, override_tcp) + + +# --------------------------------------------------------------------------- +# Registry tests +# --------------------------------------------------------------------------- + +from embodichain.lab.sim.robots.registry import ( + _ROBOT_REGISTRY, + register_robot, + get_robot_def, + build_robot_cfg, +) + + +@register_robot("TestDummy") +class _TestDummyRobotDef: + """Minimal robot def registered as 'TestDummy' for registry tests.""" + + def __init__(self, name: str = "TestDummy", **kwargs: object) -> None: + self.name = name + self._kwargs = kwargs + + @property + def urdf_cfg(self) -> URDFCfg | None: + return None + + @property + def control_parts(self) -> dict[str, list[str]] | None: + return {"arm": ["j1"]} + + @property + def solver_cfg(self) -> SolverCfg | None: + return None + + @property + def drive_pros(self) -> JointDrivePropertiesCfg: + return JointDrivePropertiesCfg() + + @property + def attrs(self) -> RigidBodyAttributesCfg: + return RigidBodyAttributesCfg() + + def build_cfg(self, **overrides: object) -> RobotCfg: + cfg = RobotCfg() + cfg.uid = overrides.pop("uid", self.name) + return cfg + + +class TestCobotMagicDef: + """Tests for the CobotMagic robot definition and registry integration.""" + + def test_cobotmagic_def_builds_valid_cfg(self) -> None: + """Verify CobotMagicDef produces a valid RobotCfg with correct fields.""" + from embodichain.lab.sim.robots.cobotmagic import CobotMagicDef + + robot_def = CobotMagicDef() + cfg = robot_def.build_cfg() + + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "CobotMagic" + assert cfg.urdf_cfg is not None + assert cfg.control_parts is not None + assert "left_arm" in cfg.control_parts + assert "right_arm" in cfg.control_parts + assert cfg.solver_cfg is not None + assert "left_arm" in cfg.solver_cfg + assert "right_arm" in cfg.solver_cfg + + def test_cobotmagic_def_registry(self) -> None: + """Verify CobotMagicDef is registered and can be looked up by name.""" + from embodichain.lab.sim.robots.registry import get_robot_def + + robot_def = get_robot_def("CobotMagic") + cfg = robot_def.build_cfg() + + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "CobotMagic" + assert cfg.control_parts is not None + assert "left_arm" in cfg.control_parts + assert "right_arm" in cfg.control_parts + + def test_cobotmagic_backward_compat_from_dict(self) -> None: + """Verify CobotMagicCfg.from_dict still works as backward-compatible wrapper.""" + from embodichain.lab.sim.robots.cobotmagic import CobotMagicCfg + + cfg = CobotMagicCfg.from_dict({}) + + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "CobotMagic" + assert cfg.control_parts is not None + assert "left_arm" in cfg.control_parts + assert "right_arm" in cfg.control_parts + + +class TestDexforceW1Def: + """Tests for the DexforceW1 robot definition and registry integration.""" + + def test_anthropomorphic_default(self) -> None: + """Verify DexforceW1Def with anthropomorphic arms produces valid cfg.""" + from embodichain.lab.sim.robots.dexforce_w1 import DexforceW1Def + + robot_def = DexforceW1Def(arm_kind="anthropomorphic") + cfg = robot_def.build_cfg() + + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "DexforceW1" + assert cfg.control_parts is not None + assert "left_arm" in cfg.control_parts + assert "right_arm" in cfg.control_parts + assert cfg.solver_cfg is not None + assert "left_arm" in cfg.solver_cfg + assert "right_arm" in cfg.solver_cfg + + def test_industrial_default(self) -> None: + """Verify DexforceW1Def with industrial arms produces valid cfg.""" + from embodichain.lab.sim.robots.dexforce_w1 import DexforceW1Def + + robot_def = DexforceW1Def(arm_kind="industrial") + cfg = robot_def.build_cfg() + + assert isinstance(cfg, RobotCfg) + assert cfg.control_parts is not None + assert "left_arm" in cfg.control_parts + assert "right_arm" in cfg.control_parts + + def test_registry_lookup(self) -> None: + """Verify DexforceW1 can be looked up via the registry.""" + cfg = build_robot_cfg("DexforceW1", arm_kind="anthropomorphic") + + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "DexforceW1" + assert cfg.control_parts is not None + + def test_backward_compat_from_dict(self) -> None: + """Verify DexforceW1Cfg.from_dict still works as backward-compat wrapper.""" + from embodichain.lab.sim.robots.dexforce_w1 import DexforceW1Cfg + + cfg = DexforceW1Cfg.from_dict( + {"version": "v021", "arm_kind": "anthropomorphic"} + ) + + assert isinstance(cfg, RobotCfg) + assert cfg.control_parts is not None + assert "left_arm" in cfg.control_parts + assert "right_arm" in cfg.control_parts + + +class TestRobotRegistry: + """Tests for the robot registry (register_robot, get_robot_def, build_robot_cfg).""" + + def test_register_and_lookup(self) -> None: + """Verify that a registered robot can be looked up and instantiated.""" + instance = get_robot_def("TestDummy") + assert instance.name == "TestDummy" + + def test_get_unknown_robot_raises(self) -> None: + """Verify that looking up an unknown name raises ValueError.""" + with pytest.raises(ValueError, match="Unknown robot"): + get_robot_def("NonExistentRobotXYZ") + + def test_build_robot_cfg_convenience(self) -> None: + """Verify build_robot_cfg returns a RobotCfg with the correct uid.""" + cfg = build_robot_cfg("TestDummy") + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "TestDummy" + + def test_build_robot_cfg_with_overrides(self) -> None: + """Verify build_robot_cfg passes overrides through to build_cfg.""" + cfg = build_robot_cfg("TestDummy", overrides={"uid": "custom_uid"}) + assert isinstance(cfg, RobotCfg) + assert cfg.uid == "custom_uid"