From 7f8931df4c6a7fd078ff7299f13f313b93ac6723 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 13 May 2026 02:13:41 +0800 Subject: [PATCH 1/7] feat: add RobotDef protocol for robot definition system Introduce a runtime-checkable RobotDef protocol that defines the contract for robot definitions in EmbodiChain. The protocol specifies required properties (name, urdf_cfg, control_parts, solver_cfg, drive_pros, attrs) and a build_cfg method that converts a RobotDef into a RobotCfg with support for property copying and merge overrides. Co-Authored-By: Claude Opus 4.7 --- embodichain/lab/sim/robots/protocol.py | 192 +++++++++++++++++++++++++ tests/sim/robots/__init__.py | 15 ++ tests/sim/robots/test_protocol.py | 155 ++++++++++++++++++++ 3 files changed, 362 insertions(+) create mode 100644 embodichain/lab/sim/robots/protocol.py create mode 100644 tests/sim/robots/__init__.py create mode 100644 tests/sim/robots/test_protocol.py 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/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..4c34bf53 --- /dev/null +++ b/tests/sim/robots/test_protocol.py @@ -0,0 +1,155 @@ +# ---------------------------------------------------------------------------- +# 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 and its build_cfg method.""" + +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) From c3d224fc9415d640083825a3ff8db162502f7f74 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 13 May 2026 02:29:41 +0800 Subject: [PATCH 2/7] feat: add robot registry for name-based lookup and instantiation Adds a global robot registry (register_robot, get_robot_def, build_robot_cfg) so callers can reference robots by string name without knowing import paths. Co-Authored-By: Claude Opus 4.7 --- embodichain/lab/sim/robots/registry.py | 130 +++++++++++++++++++++++++ tests/sim/robots/test_protocol.py | 74 +++++++++++++- 2 files changed, 203 insertions(+), 1 deletion(-) create mode 100644 embodichain/lab/sim/robots/registry.py 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/test_protocol.py b/tests/sim/robots/test_protocol.py index 4c34bf53..a97e8f3c 100644 --- a/tests/sim/robots/test_protocol.py +++ b/tests/sim/robots/test_protocol.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Tests for the RobotDef protocol and its build_cfg method.""" +"""Tests for the RobotDef protocol, its build_cfg method, and the robot registry.""" from __future__ import annotations @@ -153,3 +153,75 @@ def test_build_cfg_with_solver_cfg_override(self) -> None: ) 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 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" From 0a21be9cdd02e74fed83b2553880cf13735abceb Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 13 May 2026 13:20:27 +0800 Subject: [PATCH 3/7] refactor: migrate CobotMagic to RobotDef protocol with registry Refactor CobotMagic to implement the RobotDef protocol and register it in the global robot registry under the name "CobotMagic". All existing configuration values (URDF paths, transforms, control parts, solver configs, drive properties, rigid-body attributes) are preserved exactly. CobotMagicCfg.from_dict() continues to work as a backward-compatible wrapper that delegates to CobotMagicDef().build_cfg(). The __init__.py exports both CobotMagicDef and CobotMagicCfg. Tests added: - test_cobotmagic_def_builds_valid_cfg: verifies uid, urdf_cfg, control_parts, solver_cfg - test_cobotmagic_def_registry: verifies get_robot_def("CobotMagic") works and produces correct cfg - test_cobotmagic_backward_compat_from_dict: verifies the legacy CobotMagicCfg.from_dict({}) path still works Co-Authored-By: Claude Opus 4.7 --- embodichain/lab/sim/robots/__init__.py | 2 +- embodichain/lab/sim/robots/cobotmagic.py | 258 ++++++++++++++--------- tests/sim/robots/test_protocol.py | 46 ++++ 3 files changed, 211 insertions(+), 95 deletions(-) diff --git a/embodichain/lab/sim/robots/__init__.py b/embodichain/lab/sim/robots/__init__.py index a6be1a5b..df5cf3ad 100644 --- a/embodichain/lab/sim/robots/__init__.py +++ b/embodichain/lab/sim/robots/__init__.py @@ -15,4 +15,4 @@ # ---------------------------------------------------------------------------- from .dexforce_w1 import * -from .cobotmagic import CobotMagicCfg +from .cobotmagic import CobotMagicCfg, CobotMagicDef 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/tests/sim/robots/test_protocol.py b/tests/sim/robots/test_protocol.py index a97e8f3c..265067dc 100644 --- a/tests/sim/robots/test_protocol.py +++ b/tests/sim/robots/test_protocol.py @@ -201,6 +201,52 @@ def build_cfg(self, **overrides: object) -> RobotCfg: 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 TestRobotRegistry: """Tests for the robot registry (register_robot, get_robot_def, build_robot_cfg).""" From af09f45f59da359dd1916d9641727acacba6fc22 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 13 May 2026 13:44:49 +0800 Subject: [PATCH 4/7] refactor: migrate DexforceW1 to RobotDef protocol with registry Add DexforceW1Def implementing the RobotDef protocol, registered as "DexforceW1" in the global robot registry. The existing DexforceW1Cfg class retains backward compatibility by delegating from_dict to DexforceW1Def. Co-Authored-By: Claude Opus 4.7 --- .../lab/sim/robots/dexforce_w1/__init__.py | 1 + embodichain/lab/sim/robots/dexforce_w1/cfg.py | 44 +- .../lab/sim/robots/dexforce_w1/robot_def.py | 426 ++++++++++++++++++ tests/sim/robots/test_protocol.py | 53 +++ 4 files changed, 497 insertions(+), 27 deletions(-) create mode 100644 embodichain/lab/sim/robots/dexforce_w1/robot_def.py 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/tests/sim/robots/test_protocol.py b/tests/sim/robots/test_protocol.py index 265067dc..74060dea 100644 --- a/tests/sim/robots/test_protocol.py +++ b/tests/sim/robots/test_protocol.py @@ -247,6 +247,59 @@ def test_cobotmagic_backward_compat_from_dict(self) -> None: 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).""" From 7d25f55d3a47ea54e1ee34cb187d051099c39e1c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 13 May 2026 13:47:47 +0800 Subject: [PATCH 5/7] feat: export RobotDef, registry, and new robot defs from __init__ Co-Authored-By: Claude Opus 4.7 --- embodichain/lab/sim/robots/__init__.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/embodichain/lab/sim/robots/__init__.py b/embodichain/lab/sim/robots/__init__.py index df5cf3ad..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 .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", +] From 5dcc7cecf52a16c7f3a5b132f6d501739a846003 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 13 May 2026 14:15:33 +0800 Subject: [PATCH 6/7] docs: update add_robot guide for RobotDef protocol Co-Authored-By: Claude Opus 4.7 --- docs/source/guides/add_robot.rst | 125 +++++++++++++++++++++++++++++-- 1 file changed, 117 insertions(+), 8 deletions(-) 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 From 9ad4934e00d974ff2df5b0ce0e8a7a631716f870 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 13 May 2026 15:03:29 +0800 Subject: [PATCH 7/7] add design docs --- .../2026-05-13-robot-definition-protocol.md | 830 ++++++++++++++++++ ...-05-13-robot-definition-protocol-design.md | 308 +++++++ 2 files changed, 1138 insertions(+) create mode 100644 docs/superpowers/plans/2026-05-13-robot-definition-protocol.md create mode 100644 docs/superpowers/specs/2026-05-13-robot-definition-protocol-design.md 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 |