diff --git a/docs/source/resources/robot/index.rst b/docs/source/resources/robot/index.rst index 2ec774f6..8a5a0b43 100644 --- a/docs/source/resources/robot/index.rst +++ b/docs/source/resources/robot/index.rst @@ -8,4 +8,5 @@ Supported Robots Dexforce W1 CobotMagic + UR Family \ No newline at end of file diff --git a/docs/source/resources/robot/ur_robot.md b/docs/source/resources/robot/ur_robot.md new file mode 100644 index 00000000..c34525c5 --- /dev/null +++ b/docs/source/resources/robot/ur_robot.md @@ -0,0 +1,62 @@ +# UR Family (UR3 / UR5 / UR10 + e variants) + +`URRobotCfg` is a single config class covering the Universal Robots UR family — +UR3, UR3e, UR5, UR5e, UR10, UR10e — selected via the ``robot_type`` field. The +kinematic (DH) parameters are owned by ``URSolverCfg``; the robot config owns the +URDF, control parts, drive properties, and rigid-body attributes. + +## Key Features + +- **One class, six variants** — switch with ``robot_type`` (``"ur3"`` / ``"ur3e"`` + / ``"ur5"`` / ``"ur5e"`` / ``"ur10"`` / ``"ur10e"``). +- **Analytic UR IK** via ``URSolverCfg`` (Warp GPU kernel, 6-DOF closed-form). +- **Scale-aware defaults** — ``max_effort`` is sized per variant (UR3 < UR5 < UR10). +- **Forward kinematics** through ``build_pk_serial_chain`` (pytorch-kinematics), + routed via ``_pk_urdf_path`` so it cannot drift from the simulation URDF. +- **Round-trippable** — ``URRobotCfg.from_dict(cfg.to_dict())`` reproduces the cfg. + +## Usage + +```python +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.robots import URRobotCfg + +sim = SimulationManager(SimulationManagerCfg(headless=True, num_envs=4)) +cfg = URRobotCfg.from_dict({"robot_type": "ur5"}) +robot = sim.add_robot(cfg=cfg) +``` + +## Robot Parameters + +| Parameter | Description | +|--------------------|-------------------------------------------------------------------| +| ``robot_type`` | UR variant: ``ur3`` / ``ur3e`` / ``ur5`` / ``ur5e`` / ``ur10`` / ``ur10e`` | +| Number of joints | 6 revolute + 1 fixed (``ee_link``) | +| Control parts | ``arm`` (6 joints) | +| Root / end link | ``base_link`` / ``ee_link`` | +| Solver | ``URSolverCfg`` (analytic UR IK, Warp kernel) | +| Drive ``max_effort`` | UR3/UR3e ≈ 56 N·m · UR5/UR5e ≈ 150 N·m · UR10/UR10e ≈ 330 N·m (sim defaults, not factory specs) | + +.. note:: + + The UR5 URDF uses lowercase joint names (``joint1``..``joint6``); every other + variant uses ``Joint1``..``Joint6``. ``URRobotCfg._build_defaults`` selects the + correct joint-name casing per ``robot_type`` automatically. + +## Variants at a glance + +| ``robot_type`` | URDF | Reach (m) | Payload (kg) | +|----------------|-------------------------------|-----------|--------------| +| ``ur3`` | ``UniversalRobots/UR3/UR3.urdf`` | ~0.5 | 3 | +| ``ur3e`` | ``UniversalRobots/UR3e/UR3e.urdf`` | ~0.5 | 3 | +| ``ur5`` | ``UniversalRobots/UR5/UR5.urdf`` | ~0.85 | 5 | +| ``ur5e`` | ``UniversalRobots/UR5e/UR5e.urdf`` | ~0.85 | 5 | +| ``ur10`` | ``UniversalRobots/UR10/UR10.urdf`` | ~1.3 | 10 | +| ``ur10e`` | ``UniversalRobots/UR10e/UR10e.urdf`` | ~1.3 | 10 | + +See Also +-------- + +- :doc:`/guides/add_robot` — Adding a new robot (quick reference) +- :doc:`/tutorial/add_robot` — Adding a new robot (full tutorial) +- :doc:`/overview/sim/solvers/index` — IK solver reference diff --git a/embodichain/lab/sim/robots/__init__.py b/embodichain/lab/sim/robots/__init__.py index d07a640b..a7794daa 100644 --- a/embodichain/lab/sim/robots/__init__.py +++ b/embodichain/lab/sim/robots/__init__.py @@ -16,5 +16,6 @@ from .dexforce_w1 import * from .cobotmagic import CobotMagicCfg +from .ur_robot import URRobotCfg -__all__ = ["DexforceW1Cfg", "CobotMagicCfg"] +__all__ = ["DexforceW1Cfg", "CobotMagicCfg", "URRobotCfg"] diff --git a/embodichain/lab/sim/robots/ur_robot.py b/embodichain/lab/sim/robots/ur_robot.py new file mode 100644 index 00000000..3ce40e8c --- /dev/null +++ b/embodichain/lab/sim/robots/ur_robot.py @@ -0,0 +1,228 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +from __future__ import annotations + +import numpy as np +import torch +from typing import TYPE_CHECKING, Dict + +from embodichain.lab.sim.cfg import ( + RobotCfg, + URDFCfg, + JointDrivePropertiesCfg, + RigidBodyAttributesCfg, +) +from embodichain.lab.sim.solvers import URSolverCfg +from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg +from embodichain.data import get_data_path +from embodichain.utils import configclass + +if TYPE_CHECKING: + import pytorch_kinematics as pk + +__all__ = ["URRobotCfg"] + +# ``robot_type`` -> URDF directory / file name. The base is capitalized +# (UR3/UR5/UR10) and the "-e" suffix keeps a lowercase ``e`` (UR3e, UR5e, UR10e). +_URDF_DIR: Dict[str, str] = { + "ur3": "UR3", + "ur3e": "UR3e", + "ur5": "UR5", + "ur5e": "UR5e", + "ur10": "UR10", + "ur10e": "UR10e", +} + +# Approximate per-variant joint torque limit (N·m), scaled by robot size. +# These are sim defaults (safety clamp on the PD drive), not factory motor specs. +_UR_MAX_EFFORT: Dict[str, float] = { + "ur3": 56.0, + "ur3e": 56.0, + "ur5": 150.0, + "ur5e": 150.0, + "ur10": 330.0, + "ur10e": 330.0, +} + + +@configclass +class URRobotCfg(RobotCfg): + """Configuration for the UR family of robots. + + One config class covers UR3 / UR3e / UR5 / UR5e / UR10 / UR10e, selected via + ``robot_type``. The kinematic (DH) parameters are owned by + :class:`~embodichain.lab.sim.solvers.URSolverCfg`; this config owns the URDF, + control parts, drive properties and rigid-body attributes. + + Example: + + cfg = URRobotCfg.from_dict({"robot_type": "ur5"}) + robot = sim.add_robot(cfg=cfg) + """ + + robot_type: str = "ur10" + + @classmethod + def from_dict(cls, init_dict): + """Initialize ``URRobotCfg`` from a dictionary. + + Args: + init_dict: Dictionary of configuration parameters. ``robot_type`` + selects the UR variant (``ur3``/``ur3e``/``ur5``/``ur5e``/ + ``ur10``/``ur10e``); all other keys are merged on top of the + defaults via :func:`merge_robot_cfg`. + + Returns: + A ``URRobotCfg`` instance. + """ + cfg = cls() + cfg._build_defaults(init_dict) + return merge_robot_cfg(cfg, init_dict) + + def _build_defaults(self, init_dict: dict | None = None) -> None: + """Populate default urdf/control/solver/physics for the chosen UR variant. + + Args: + init_dict: The raw override dict passed to ``from_dict``. ``robot_type`` + is read from here (falling back to the class default). + """ + init_dict = init_dict or {} + robot_type = init_dict.get("robot_type", self.robot_type) + if robot_type not in _URDF_DIR: + raise ValueError( + f"Unknown UR robot_type: {robot_type!r}. " + f"Expected one of {sorted(_URDF_DIR)}." + ) + + self.robot_type = robot_type + self.uid = "URRobot" + + urdf_dir = _URDF_DIR[robot_type] + urdf_path = get_data_path(f"UniversalRobots/{urdf_dir}/{urdf_dir}.urdf") + + self.urdf_cfg = URDFCfg( + components=[ + { + "component_type": "arm", + "urdf_path": urdf_path, + "transform": np.eye(4), + } + ] + ) + + # The UR5 URDF uses lowercase joint names; every other variant uses + # ``Joint1``..``Joint6``. Build the explicit list per variant so control + # parts match the loaded URDF exactly. + joint_prefix = "joint" if robot_type == "ur5" else "Joint" + self.control_parts = { + "arm": [f"{joint_prefix}{i}" for i in range(1, 7)], + } + + self.solver_cfg = { + "arm": URSolverCfg( + ur_type=robot_type, + end_link_name="ee_link", + root_link_name="base_link", + ), + } + + self.min_position_iters = 8 + self.min_velocity_iters = 2 + + self.drive_pros = JointDrivePropertiesCfg( + stiffness=7e4, + damping=1e3, + max_effort=_UR_MAX_EFFORT[robot_type], + ) + + self.attrs = RigidBodyAttributesCfg( + mass=1.0, + static_friction=0.95, + dynamic_friction=0.9, + linear_damping=0.7, + angular_damping=0.7, + contact_offset=0.002, + rest_offset=0.0, + restitution=0.0, + max_depenetration_velocity=10.0, + ) + + @property + def _pk_urdf_path(self) -> str: + """URDF used for the FK/IK serial chain (the same arm URDF as the sim). + + .. attention:: + The ``base_link``→``ee_link`` kinematics here must match the arm in + the simulation URDF. A DOF drift guard in the tests checks this. + """ + urdf_dir = _URDF_DIR[self.robot_type] + return get_data_path(f"UniversalRobots/{urdf_dir}/{urdf_dir}.urdf") + + def build_pk_serial_chain( + self, device: torch.device = torch.device("cpu"), **kwargs + ) -> Dict[str, "pk.SerialChain"]: + """Build the pytorch-kinematics serial chain for the arm. + + Args: + device: The device to which the chain will be moved. Defaults to CPU. + **kwargs: Additional arguments for building the serial chain. + + Returns: + A ``{"arm": pk.SerialChain}`` mapping. + """ + from embodichain.lab.sim.utility.solver_utils import create_pk_serial_chain + + chain = create_pk_serial_chain( + urdf_path=self._pk_urdf_path, + device=device, + end_link_name="ee_link", + root_link_name="base_link", + ) + return {"arm": chain} + + +if __name__ == "__main__": + import numpy as np + + np.set_printoptions(precision=5, suppress=True) + + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import RenderCfg + + config = SimulationManagerCfg( + headless=True, + sim_device="cuda", + num_envs=2, + render_cfg=RenderCfg(renderer="fast-rt"), + ) + sim = SimulationManager(config) + + # Switch the UR variant via robot_type (ur3 / ur3e / ur5 / ur5e / ur10 / ur10e). + cfg = URRobotCfg.from_dict({"robot_type": "ur5"}) + robot = sim.add_robot(cfg=cfg) + sim.open_window() + + if sim.is_use_gpu_physics: + sim.init_gpu_physics() + + # Round-trip check: from_dict(to_dict()) reproduces the cfg. + cfg2 = URRobotCfg.from_dict(cfg.to_dict()) + assert cfg2.robot_type == cfg.robot_type + print(f"URRobot added ({cfg.robot_type}); round-trip OK.") + + from IPython import embed + + embed() diff --git a/embodichain/lab/sim/solvers/ur_solver.py b/embodichain/lab/sim/solvers/ur_solver.py index 1ff83afb..f3a7bd00 100644 --- a/embodichain/lab/sim/solvers/ur_solver.py +++ b/embodichain/lab/sim/solvers/ur_solver.py @@ -30,6 +30,7 @@ @configclass class URSolverCfg(SolverCfg): + class_type: str = "URSolver" ur_type: str = "ur10" end_link_name: str = "ee_link" root_link_name: str = "base_link" diff --git a/tests/sim/objects/test_robot_cfg.py b/tests/sim/objects/test_robot_cfg.py index 92947c65..6aa5c8d0 100644 --- a/tests/sim/objects/test_robot_cfg.py +++ b/tests/sim/objects/test_robot_cfg.py @@ -161,3 +161,68 @@ def test_cobotmagic_pk_dof_matches_control_parts(): assert _dof_of_pk_chain(chains[arm]) == len( cfg.control_parts[arm] ), f"{arm}: PK chain DOF drifted from control_parts" + + +# --------------------------------------------------------------------------- # +# URRobotCfg -- UR family (ur3 / ur3e / ur5 / ur5e / ur10 / ur10e) +# --------------------------------------------------------------------------- # + +from embodichain.lab.sim.robots.ur_robot import URRobotCfg +from embodichain.lab.sim.solvers import URSolverCfg + +UR_TYPES = ["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e"] + + +@pytest.mark.parametrize("robot_type", UR_TYPES) +def test_ur_robot_from_dict(robot_type): + cfg = URRobotCfg.from_dict({"robot_type": robot_type}) + assert cfg.robot_type == robot_type + assert isinstance(cfg.solver_cfg["arm"], URSolverCfg) + assert cfg.solver_cfg["arm"].ur_type == robot_type + assert cfg.solver_cfg["arm"].end_link_name == "ee_link" + assert cfg.solver_cfg["arm"].root_link_name == "base_link" + # one arm control part with 6 joints + assert list(cfg.control_parts.keys()) == ["arm"] + assert len(cfg.control_parts["arm"]) == 6 + + +def test_ur_robot_default_type_is_ur10(): + cfg = URRobotCfg.from_dict({}) + assert cfg.robot_type == "ur10" + + +@pytest.mark.parametrize("robot_type", UR_TYPES) +def test_ur_robot_roundtrip(robot_type): + cfg = URRobotCfg.from_dict({"robot_type": robot_type}) + d = cfg.to_dict() + assert d["robot_type"] == robot_type + cfg2 = URRobotCfg.from_dict(d) + assert cfg2.robot_type == robot_type + assert isinstance(cfg2.solver_cfg["arm"], URSolverCfg) + + +def test_ur_robot_max_effort_scales_with_size(): + """Larger UR variants have larger max_effort defaults.""" + ur3 = URRobotCfg.from_dict({"robot_type": "ur3"}) + ur5 = URRobotCfg.from_dict({"robot_type": "ur5"}) + ur10 = URRobotCfg.from_dict({"robot_type": "ur10"}) + eff = lambda c: c.drive_pros.max_effort # noqa: E731 + assert eff(ur3) < eff(ur5) < eff(ur10) + + +@pytest.mark.parametrize("robot_type", UR_TYPES) +def test_ur_robot_pk_dof_matches_control_parts(robot_type): + pytest.importorskip("pytorch_kinematics") + cfg = URRobotCfg.from_dict({"robot_type": robot_type}) + try: + chains = cfg.build_pk_serial_chain() + except Exception as exc: + pytest.skip(f"PK URDF asset unavailable: {exc}") + assert _dof_of_pk_chain(chains["arm"]) == len( + cfg.control_parts["arm"] + ), "arm: PK chain DOF drifted from control_parts" + + +def test_ur_robot_unknown_type_raises(): + with pytest.raises(ValueError): + URRobotCfg.from_dict({"robot_type": "ur99"})