Add URRobotCfg: UR family (ur3/3e/5/5e/10/10e)

docs/source/resources/robot/index.rst

@@ -8,4 +8,5 @@ Supported Robots
 
    Dexforce W1 <dexforce_w1.md>
    CobotMagic <cobotmagic.md>
+   UR Family <ur_robot.md>
 

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

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"]

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()

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"

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"})