diff --git a/configs/agents/rl/push_cube/gym_config.json b/configs/agents/rl/push_cube/gym_config.json index a97cc65d..1399faf0 100644 --- a/configs/agents/rl/push_cube/gym_config.json +++ b/configs/agents/rl/push_cube/gym_config.json @@ -122,13 +122,10 @@ } }, "robot": { + "robot_type": "URRobot", "uid": "Manipulator", "urdf_cfg": { "components": [ - { - "component_type": "arm", - "urdf_path": "UniversalRobots/UR10/UR10.urdf" - }, { "component_type": "hand", "urdf_path": "DH_PGI_140_80/DH_PGI_140_80.urdf" @@ -147,9 +144,6 @@ }, "solver_cfg": { "arm": { - "class_type": "PytorchSolver", - "end_link_name": "ee_link", - "root_link_name": "base_link", "tcp": [ [1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], @@ -157,9 +151,6 @@ [0.0, 0.0, 0.0, 1.0] ] } - }, - "control_parts": { - "arm": ["JOINT[1-6]"] } }, "sensor": [], diff --git a/docs/source/_static/robots/dual/facing_forward.png b/docs/source/_static/robots/dual/facing_forward.png new file mode 100644 index 00000000..68510b05 Binary files /dev/null and b/docs/source/_static/robots/dual/facing_forward.png differ diff --git a/docs/source/_static/robots/dual/mirror.png b/docs/source/_static/robots/dual/mirror.png new file mode 100644 index 00000000..2130239d Binary files /dev/null and b/docs/source/_static/robots/dual/mirror.png differ diff --git a/docs/source/_static/robots/dual/side_by_side.png b/docs/source/_static/robots/dual/side_by_side.png new file mode 100644 index 00000000..a160adf1 Binary files /dev/null and b/docs/source/_static/robots/dual/side_by_side.png differ diff --git a/docs/source/_static/robots/ur/ur10.png b/docs/source/_static/robots/ur/ur10.png new file mode 100644 index 00000000..68cf4614 Binary files /dev/null and b/docs/source/_static/robots/ur/ur10.png differ diff --git a/docs/source/_static/robots/ur/ur10e.png b/docs/source/_static/robots/ur/ur10e.png new file mode 100644 index 00000000..1b2fde6e Binary files /dev/null and b/docs/source/_static/robots/ur/ur10e.png differ diff --git a/docs/source/_static/robots/ur/ur3.png b/docs/source/_static/robots/ur/ur3.png new file mode 100644 index 00000000..8b1ca739 Binary files /dev/null and b/docs/source/_static/robots/ur/ur3.png differ diff --git a/docs/source/_static/robots/ur/ur3e.png b/docs/source/_static/robots/ur/ur3e.png new file mode 100644 index 00000000..abc9e9a9 Binary files /dev/null and b/docs/source/_static/robots/ur/ur3e.png differ diff --git a/docs/source/_static/robots/ur/ur5.png b/docs/source/_static/robots/ur/ur5.png new file mode 100644 index 00000000..b8b09f97 Binary files /dev/null and b/docs/source/_static/robots/ur/ur5.png differ diff --git a/docs/source/_static/robots/ur/ur5e.png b/docs/source/_static/robots/ur/ur5e.png new file mode 100644 index 00000000..42c92464 Binary files /dev/null and b/docs/source/_static/robots/ur/ur5e.png differ diff --git a/docs/source/resources/robot/dual_arm.md b/docs/source/resources/robot/dual_arm.md new file mode 100644 index 00000000..1cb28d9a --- /dev/null +++ b/docs/source/resources/robot/dual_arm.md @@ -0,0 +1,130 @@ +# Dual-Arm Composition + +`DualArmRobotCfg` builds a bimanual robot from a single-arm base config that +already follows the standard `"arm"` convention. In the current EmbodiChain +setup, the dual-arm examples are based on the UR family and are assembled onto a +shared synthetic `base_link` through `URDFCfg`. + +The images below show the three documented dual-arm layouts currently used for +this robot family: a separated side-by-side arrangement, a wide mirrored +forward-facing arrangement, and a tighter mirrored arrangement for close +workspace overlap. + +
+
+ Side-by-side dual arm layout +
Side-by-Side
+
+
+ Wide mirrored dual arm layout +
Wide Mirrored Layout
+
+
+ Close mirrored dual arm layout +
Close Mirrored Layout
+
+
+ +## Key Features + +- **Generic dual-arm builder** from any single-arm `RobotCfg` that exposes one `arm` URDF component, one `control_parts["arm"]`, and one `solver_cfg["arm"]`. +- **Image-backed mount presets** covering separated, inward-facing, and mirrored-`rz` layouts. +- **Automatic left/right derivation** for URDF components, control parts, solver configs, and drive properties. +- **Config-driven construction** through `DualArmRobotCfg.from_dict(...)` with registry-based base robot lookup. +- **Optional composite `dual_arm` part** for commanding both manipulators together. +- **Round-trip support** through `to_dict()` / `from_dict()` on the generated config. + +## Visual Layouts + +- **Side-by-side** keeps both bases offset by `±separation/2` along Y with the same orientation. +- **Wide mirrored layout** spreads the arms apart while rotating them symmetrically so they open away from each other. +- **Close mirrored layout** rotates the arms symmetrically toward a shared center workspace for overlapping reach. + +## Usage + +```python +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.robots import DualArmRobotCfg + +sim = SimulationManager(SimulationManagerCfg(headless=True, num_envs=4)) + +cfg = DualArmRobotCfg.from_dict( + { + "base_robot": "ur5", + "mount": { + "preset": "mirrored_rz", + "separation": 0.6, + "rz": 0.7853981633974483, + }, + } +) +robot = sim.add_robot(cfg=cfg) +``` + +## Mount Presets + +| Preset | Layout | +|--------|--------| +| `side_by_side` | Left arm at `+separation/2` in Y, right arm at `-separation/2` in Y, same orientation on both sides. | +| `facing_inward` | Same `±separation/2` offsets, with yaw `+pi/2` on the left arm and `-pi/2` on the right arm. | +| `mirrored_rz` | Same `±separation/2` offsets, with yaw `+rz` on the left arm and `-rz` on the right arm. | + +The `mount` configuration also supports paired per-arm overrides: + +```python +cfg = DualArmRobotCfg.from_dict( + { + "base_robot": "ur5", + "mount": { + "preset": "side_by_side", + "separation": 0.6, + "left": {"xyz": [0.0, 0.35, 0.0], "rpy": [0.0, 0.0, 0.0]}, + "right": {"xyz": [0.1, -0.35, 0.0], "rpy": [0.0, 0.0, 0.0]}, + }, + } +) +``` + +## Configuration Parameters + +| Parameter | Description | +|-----------|-------------| +| `base_robot` | Registry key such as `"ur3"` through `"ur10e"`, or `{"type": ..., "init": {...}}` for extra base config overrides. | +| `mount` | Mount dict consumed by `resolve_mounts`, including `preset`, `separation`, optional `rz`, and optional paired `left` / `right` overrides. | +| `arm_part` | Base robot manipulator control part name. Default: `"arm"`. | +| `dual_part` | Whether to emit the concatenated `dual_arm` control part. Default: `True`. | + +## Derived Control Parts + +For a UR5 base robot with the default preserved joint casing, `DualArmRobotCfg` +produces: + +| Part | Joints | +|------|--------| +| `left_arm` | `left_joint1` to `left_joint6` | +| `right_arm` | `right_joint1` to `right_joint6` | +| `dual_arm` | concatenation of the 12 joints above | + +For other base robots, the builder preserves the source URDF naming policy and +applies the `left_` / `right_` prefixes consistently with the assembled URDF. + +## Programmatic Build Path + +```python +from embodichain.lab.sim.robots import URRobotCfg, build_dual_arm_cfg, resolve_mounts + +base = URRobotCfg.from_dict({"robot_type": "ur5"}) +mounts = resolve_mounts({"preset": "facing_inward", "separation": 0.6}) +cfg = build_dual_arm_cfg(base, mounts, dual_part=False) +``` + +## Adding a New Base Robot + +A single-arm robot becomes dual-arm-ready by: + +1. following the existing `"arm"` convention in its single-arm cfg, +2. exposing `control_parts["arm"]` and `solver_cfg["arm"]`, +3. adding one registry entry in `embodichain/lab/sim/robots/dual_arm.py`. + +That keeps the dual-arm path generic: no extra mixin or robot-specific dual-arm +class is required. diff --git a/docs/source/resources/robot/index.rst b/docs/source/resources/robot/index.rst index 2ec774f6..e579472d 100644 --- a/docs/source/resources/robot/index.rst +++ b/docs/source/resources/robot/index.rst @@ -6,6 +6,8 @@ Supported Robots .. toctree:: :maxdepth: 1 + UR Family Dexforce W1 CobotMagic + Dual-Arm Composition \ 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..6b802f7b --- /dev/null +++ b/docs/source/resources/robot/ur_robot.md @@ -0,0 +1,93 @@ +# UR Family (UR3 / UR5 / UR10 + e variants) + +`URRobotCfg` covers six Universal Robots manipulators in one configuration class: +`ur3`, `ur3e`, `ur5`, `ur5e`, `ur10`, and `ur10e`. The images below show the exact +variants currently documented in EmbodiChain: the compact UR3 pair, the mid-reach +UR5 pair, and the long-reach UR10 pair, with darker classic models and brighter +silver-arm e-series renders. + +
+
+ UR3 +
UR3
+
+
+ UR3e +
UR3e
+
+
+ UR5 +
UR5
+
+
+ UR5e +
UR5e
+
+
+ UR10 +
UR10
+
+
+ UR10e +
UR10e
+
+
+ +## Key Features + +- **One config, six UR variants** selected through `robot_type`. +- **Image-matched family lineup** from the shortest-reach UR3 pair to the longest-reach UR10 pair. +- **Classic and e-series styling** with darker legacy arms and brighter silver-arm e-series renders. +- **Analytic UR inverse kinematics** through `URSolverCfg` and the Warp-based UR solver path. +- **Simulation-ready defaults** for URDF selection, control parts, drive properties, and rigid-body attributes. + +## Visual Differences Across Variants + +- **UR3 / UR3e** are the most compact renders in the set and keep the wrist close to the base. +- **UR5 / UR5e** extend the shoulder-to-wrist span while keeping the same 6-axis arm layout. +- **UR10 / UR10e** show the longest upper-arm and forearm links, matching the largest reach tier in the family. +- **Classic models (`ur3`, `ur5`, `ur10`)** appear darker overall, especially on the arm links. +- **e-series models (`ur3e`, `ur5e`, `ur10e`)** use brighter silver links and lighter joint accents in these renders. + +## 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`, or `ur10e` | +| Number of joints | 6 revolute joints plus the fixed `ee_link` | +| Control parts | `arm` (6 joints) | +| Root / end link | `base_link` / `ee_link` | +| Solver | `URSolverCfg` (analytic UR IK) | +| Drive `max_effort` | UR3/UR3e about 56 N.m, UR5/UR5e about 150 N.m, UR10/UR10e about 330 N.m | + +> **Note:** The `ur5` URDF uses lowercase joint names (`joint1` to `joint6`), while +> the other variants use `Joint1` to `Joint6`. `URRobotCfg._build_defaults` +> selects the correct naming scheme automatically. + +## Variants at a Glance + +| `robot_type` | Preview | URDF | Reach (m) | Payload (kg) | +|--------------|---------|------|-----------|--------------| +| `ur3` | Compact classic render | `UniversalRobots/UR3/UR3.urdf` | ~0.5 | 3 | +| `ur3e` | Compact e-series render | `UniversalRobots/UR3e/UR3e.urdf` | ~0.5 | 3 | +| `ur5` | Mid-size classic render | `UniversalRobots/UR5/UR5.urdf` | ~0.85 | 5 | +| `ur5e` | Mid-size e-series render | `UniversalRobots/UR5e/UR5e.urdf` | ~0.85 | 5 | +| `ur10` | Long-reach classic render | `UniversalRobots/UR10/UR10.urdf` | ~1.3 | 10 | +| `ur10e` | Long-reach e-series render | `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/cfg.py b/embodichain/lab/sim/cfg.py index 185c5f9c..97f6ae72 100644 --- a/embodichain/lab/sim/cfg.py +++ b/embodichain/lab/sim/cfg.py @@ -1049,10 +1049,16 @@ class URDFCfg: name_case: dict[str, str] = field( default_factory=lambda: { - "joint": "upper", - "link": "lower", + "joint": "original", + "link": "original", } ) + """Case normalization policy applied to joint/link names during URDF assembly. + + Supported values per key are ``"upper"``, ``"lower"`` or ``"original"`` + (legacy alias ``"none"``). The default upper-cases joints and lower-cases + links. Set ``{"joint": "original"}`` to preserve the source URDF casing. + """ def __init__( self, @@ -1117,8 +1123,8 @@ def __init__( if name_case is None: self.name_case = { - "joint": "upper", - "link": "lower", + "joint": "original", + "link": "original", } else: self.name_case = name_case diff --git a/embodichain/lab/sim/robots/__init__.py b/embodichain/lab/sim/robots/__init__.py index d07a640b..be86d163 100644 --- a/embodichain/lab/sim/robots/__init__.py +++ b/embodichain/lab/sim/robots/__init__.py @@ -16,5 +16,13 @@ from .dexforce_w1 import * from .cobotmagic import CobotMagicCfg +from .ur_robot import URRobotCfg +from .dual_arm import DualArmRobotCfg, build_dual_arm_cfg -__all__ = ["DexforceW1Cfg", "CobotMagicCfg"] +__all__ = [ + "DexforceW1Cfg", + "CobotMagicCfg", + "URRobotCfg", + "DualArmRobotCfg", + "build_dual_arm_cfg", +] diff --git a/embodichain/lab/sim/robots/cobotmagic.py b/embodichain/lab/sim/robots/cobotmagic.py index 3faeed15..66041772 100644 --- a/embodichain/lab/sim/robots/cobotmagic.py +++ b/embodichain/lab/sim/robots/cobotmagic.py @@ -88,23 +88,23 @@ def _build_defaults(self, init_dict: dict | None = None) -> None: ) self.control_parts = { "left_arm": [ - "LEFT_JOINT1", - "LEFT_JOINT2", - "LEFT_JOINT3", - "LEFT_JOINT4", - "LEFT_JOINT5", - "LEFT_JOINT6", + "left_joint1", + "left_joint2", + "left_joint3", + "left_joint4", + "left_joint5", + "left_joint6", ], - "left_eef": ["LEFT_JOINT7", "LEFT_JOINT8"], + "left_eef": ["left_joint7", "left_joint8"], "right_arm": [ - "RIGHT_JOINT1", - "RIGHT_JOINT2", - "RIGHT_JOINT3", - "RIGHT_JOINT4", - "RIGHT_JOINT5", - "RIGHT_JOINT6", + "right_joint1", + "right_joint2", + "right_joint3", + "right_joint4", + "right_joint5", + "right_joint6", ], - "right_eef": ["RIGHT_JOINT7", "RIGHT_JOINT8"], + "right_eef": ["right_joint7", "right_joint8"], } self.solver_cfg = { "left_arm": OPWSolverCfg( @@ -126,22 +126,22 @@ def _build_defaults(self, init_dict: dict | None = None) -> None: self.min_velocity_iters = 2 self.drive_pros = JointDrivePropertiesCfg( stiffness={ - "LEFT_JOINT[1-6]": 7e4, - "RIGHT_JOINT[1-6]": 7e4, - "LEFT_JOINT[7-8]": 3e2, - "RIGHT_JOINT[7-8]": 3e2, + "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, + "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, + "left_joint[1-6]": 3e6, + "right_joint[1-6]": 3e6, + "left_joint[7-8]": 3e3, + "right_joint[7-8]": 3e3, }, ) self.attrs = RigidBodyAttributesCfg( diff --git a/embodichain/lab/sim/robots/dual_arm.py b/embodichain/lab/sim/robots/dual_arm.py new file mode 100644 index 00000000..2036ab9d --- /dev/null +++ b/embodichain/lab/sim/robots/dual_arm.py @@ -0,0 +1,622 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Compose a dual-manipulator (two-arm) robot from any single-arm robot cfg. + +The :func:`build_dual_arm_cfg` engine takes a constructed single-arm +:class:`~embodichain.lab.sim.cfg.RobotCfg` (e.g. a ``URRobotCfg``) that follows +the existing ``"arm"`` convention -- one ``"arm"`` URDF component, one +``control_parts["arm"]`` entry and one ``solver_cfg["arm"]`` entry -- and emits a +fully-populated two-arm cfg whose left/right arms are mounted on a shared +synthetic ``base_link`` via :class:`~embodichain.lab.sim.cfg.URDFCfg` +multi-component assembly. + +The :class:`DualArmRobotCfg` wrapper adds dict/YAML construction (``from_dict``) +on top of the engine: a registry resolves ``base_robot`` (e.g. ``"ur5"``) to the +single-arm cfg class, the engine duplicates the parts/solver/drive properties, +and the result plugs straight into ``sim.add_robot(cfg=...)`` and round-trips via +``to_dict()`` / ``from_dict()``. + +Adding a future two-arm robot (e.g. Franka) only requires writing Franka's +single-arm cfg and one registry line -- no dual-arm class, no mixin. + +Example: + + cfg = DualArmRobotCfg.from_dict( + {"base_robot": "ur5", "mount": {"preset": "side_by_side", "separation": 0.6}} + ) + robot = sim.add_robot(cfg=cfg) +""" + +from __future__ import annotations + +from dataclasses import field +from typing import TYPE_CHECKING, Dict, List, Union + +import numpy as np +import torch + +from embodichain.lab.sim.cfg import ( + JointDrivePropertiesCfg, + RobotCfg, + URDFCfg, +) +from embodichain.lab.sim.solvers import SolverCfg +from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg +from embodichain.lab.sim.robots.ur_robot import URRobotCfg +from embodichain.utils import configclass + +if TYPE_CHECKING: + import pytorch_kinematics as pk + +__all__ = ["DualArmRobotCfg", "build_dual_arm_cfg", "resolve_mounts"] + +# Each side -> (component_type, name prefix) used by ``URDFAssemblyManager``. +_SIDES = (("left", "left_arm", "left_"), ("right", "right_arm", "right_")) + +#: Supported mount presets for :func:`resolve_mounts`. +_PRESETS = ("side_by_side", "facing_inward", "mirrored_rz") + +#: Default mount used when none is specified. +_DEFAULT_MOUNT: dict = {"preset": "side_by_side", "separation": 0.6} + +#: Registry mapping a ``base_robot`` key to ``(single-arm cfg class, init dict)``. +#: Adding a new single-arm robot here is all that is needed to make it +#: dual-arm-able: e.g. ``"franka": (FrankaRobotCfg, {})``. +_BASE_ROBOT_REGISTRY: Dict[str, tuple] = { + "ur3": (URRobotCfg, {"robot_type": "ur3"}), + "ur3e": (URRobotCfg, {"robot_type": "ur3e"}), + "ur5": (URRobotCfg, {"robot_type": "ur5"}), + "ur5e": (URRobotCfg, {"robot_type": "ur5e"}), + "ur10": (URRobotCfg, {"robot_type": "ur10"}), + "ur10e": (URRobotCfg, {"robot_type": "ur10e"}), +} + +#: Drive-property fields that may be scalar or a regex->value dict. +_DRIVE_PROPS = ( + "stiffness", + "damping", + "max_effort", + "max_velocity", + "friction", + "armature", +) + + +# --------------------------------------------------------------------------- # +# Naming helper +# --------------------------------------------------------------------------- # + + +def _prefixed_name( + name: str | None, + prefix: str, + kind: str, + name_case: dict[str, str] | None = None, +) -> str | None: + """Apply the same prefix + case convention as ``URDFAssemblyManager``. + + Mirrors ``URDFComponentManager._generate_unique_name`` followed by + ``NameNormalizer``: prepend ``prefix`` unless the name already starts with + it, then case-normalize according to ``name_case``. This keeps the names + predicted by the engine identical to the ones written into the assembled + URDF; an integration test asserts they match. + + Args: + name: The original joint/link name from the single-arm URDF. + prefix: The side prefix (``"left_"`` / ``"right_"``). + kind: ``"joint"`` or ``"link"`` (selects the case policy). + name_case: Optional joint/link case policy, matching + :class:`~embodichain.lab.sim.cfg.URDFCfg`. + + Returns: + The prefixed, case-normalized name. + """ + if name is None: + return None + base = name + if prefix and not name.lower().startswith(prefix.lower()): + base = f"{prefix}{name}" + mode = (name_case or {}).get(kind, "original") + if mode == "upper": + return base.upper() + if mode == "lower": + return base.lower() + return base + + +# --------------------------------------------------------------------------- # +# Mount resolution +# --------------------------------------------------------------------------- # + + +def _transform_from_xyz_rpy(xyz, rpy) -> np.ndarray: + """Build a 4x4 homogeneous transform from xyz translation and xyz euler. + + Args: + xyz: Translation ``[x, y, z]``. + rpy: Intrinsic xyz euler angles in radians ``[r, p, y]``. + + Returns: + The 4x4 transform. + """ + from scipy.spatial.transform import Rotation as R + + T = np.eye(4, dtype=float) + T[:3, 3] = np.asarray(xyz, dtype=float) + T[:3, :3] = R.from_euler("xyz", np.asarray(rpy, dtype=float)).as_matrix() + return T + + +def resolve_mounts(mount_cfg: dict | None) -> Dict[str, np.ndarray]: + """Resolve a mount config into left/right 4x4 transforms. + + Supported presets (the common bimanual layouts): + + * ``side_by_side`` -- left at ``+separation/2`` in Y, right at + ``-separation/2``, same orientation. + * ``facing_inward`` -- same ±Y separation, yawed ±90° so the arms face each + other (mirror-symmetric). + * ``mirrored_rz`` -- same ±Y separation, with yaw ``+rz`` on the left arm + and ``-rz`` on the right arm. + + A per-arm ``left`` / ``right`` override (``{"xyz": [...], "rpy": [...]}``) + may replace either side; both must be given together or neither. + + Args: + mount_cfg: Mount configuration dict. ``None`` uses the default preset. + + Returns: + ``{"left": T_left, "right": T_right}`` with 4x4 transforms. + + Raises: + ValueError: If the preset is unknown, or only one per-arm override is + given. + """ + cfg = dict(mount_cfg or _DEFAULT_MOUNT) + preset = cfg.get("preset", "side_by_side") + if preset not in _PRESETS: + raise ValueError( + f"Unknown mount preset {preset!r}. Expected one of {sorted(_PRESETS)}." + ) + separation = float(cfg.get("separation", _DEFAULT_MOUNT["separation"])) + half = separation / 2.0 + rz = float(cfg.get("rz", 0.0)) + + left_override = cfg.get("left") + right_override = cfg.get("right") + if (left_override is None) != (right_override is None): + raise ValueError( + "mount: provide both 'left' and 'right' overrides, or neither." + ) + + if preset == "side_by_side": + left = {"xyz": [0.0, half, 0.0], "rpy": [0.0, 0.0, 0.0]} + right = {"xyz": [0.0, -half, 0.0], "rpy": [0.0, 0.0, 0.0]} + elif preset == "facing_inward": # mirror yaw (+90 / -90 about Z) + left = {"xyz": [0.0, half, 0.0], "rpy": [0.0, 0.0, np.pi / 2]} + right = {"xyz": [0.0, -half, 0.0], "rpy": [0.0, 0.0, -np.pi / 2]} + else: # mirrored_rz: mirror the user-specified base yaw (+rz / -rz) + left = {"xyz": [0.0, half, 0.0], "rpy": [0.0, 0.0, rz]} + right = {"xyz": [0.0, -half, 0.0], "rpy": [0.0, 0.0, -rz]} + + if left_override: + left.update(left_override) + if right_override: + right.update(right_override) + + return { + "left": _transform_from_xyz_rpy(left["xyz"], left["rpy"]), + "right": _transform_from_xyz_rpy(right["xyz"], right["rpy"]), + } + + +# --------------------------------------------------------------------------- # +# Base-robot resolution +# --------------------------------------------------------------------------- # + + +def _resolve_base_cfg(base_robot: str | dict) -> RobotCfg: + """Resolve a ``base_robot`` spec to a constructed single-arm cfg. + + Args: + base_robot: Either a registry key (e.g. ``"ur5"``) or an explicit + ``{"type": "ur5", "init": {...}}`` mapping for passing extra + base-robot init params. + + Returns: + A constructed single-arm :class:`RobotCfg`. + + Raises: + ValueError: If the key is not in :data:`_BASE_ROBOT_REGISTRY`. + """ + if isinstance(base_robot, dict): + key = base_robot.get("type") + init = base_robot.get("init", {}) or {} + else: + key = base_robot + init = {} + if key not in _BASE_ROBOT_REGISTRY: + raise ValueError( + f"Unknown base_robot {key!r}. Registered base robots: " + f"{sorted(_BASE_ROBOT_REGISTRY)}." + ) + cls_, default_init = _BASE_ROBOT_REGISTRY[key] + merged_init = {**default_init, **init} + return cls_.from_dict(merged_init) + + +# --------------------------------------------------------------------------- # +# Engine +# --------------------------------------------------------------------------- # + + +def _mirror_drive_pros( + base_drive: JointDrivePropertiesCfg, name_case: dict[str, str] | None = None +) -> JointDrivePropertiesCfg: + """Mirror a single-arm drive config across left/right arms. + + Scalar fields apply to all joints uniformly and are copied verbatim. A + regex->value dict (per-joint-index drive) is mirrored by uppercasing each + pattern and emitting ``LEFT_`` / ``RIGHT_`` variants, matching the + assembled (prefixed) joint names. + + .. note:: + Regex-pattern mirroring is best-effort. It prepends the side prefix and + then applies the configured joint name case, so it stays aligned with + the assembled URDF naming policy. + + Args: + base_drive: The single-arm :class:`JointDrivePropertiesCfg`. + name_case: Optional joint/link case policy, matching + :class:`~embodichain.lab.sim.cfg.URDFCfg`. + + Returns: + A fresh :class:`JointDrivePropertiesCfg` for the dual arm. + """ + new = JointDrivePropertiesCfg(drive_type=base_drive.drive_type) + for prop in _DRIVE_PROPS: + val = getattr(base_drive, prop, None) + if val is None: + continue + if isinstance(val, dict): + mirrored: Dict[str, float] = {} + for pattern, v in val.items(): + mirrored[_prefixed_name(str(pattern), "left_", "joint", name_case)] = v + mirrored[_prefixed_name(str(pattern), "right_", "joint", name_case)] = v + setattr(new, prop, mirrored) + else: + setattr(new, prop, val) + return new + + +def _populate_dual_cfg( + cfg: RobotCfg, + base_cfg: RobotCfg, + mounts: Dict[str, np.ndarray], + *, + dual_part: bool, + arm_part: str, +) -> None: + """Populate ``cfg`` (in place) with the dual-arm derivation of ``base_cfg``. + + Args: + cfg: The target :class:`RobotCfg` (typically a ``DualArmRobotCfg``). + base_cfg: A constructed single-arm robot cfg. + mounts: ``{"left": T, "right": T}`` 4x4 mount transforms. + dual_part: Whether to emit a ``"dual_arm"`` composite control part. + arm_part: The base cfg's manipulator part name (default ``"arm"``). + + Raises: + ValueError: If ``base_cfg`` lacks the ``arm_part`` control part or URDF + component. + """ + base_control = base_cfg.control_parts or {} + if arm_part not in base_control: + raise ValueError( + f"Base robot cfg has no control part {arm_part!r} (available: " + f"{list(base_control.keys())}). Set `arm_part` to the base robot's " + f"manipulator part name." + ) + + base_solver_map = base_cfg.solver_cfg + if isinstance(base_solver_map, dict): + if arm_part not in base_solver_map: + raise ValueError( + f"Base robot solver_cfg has no part {arm_part!r} (available: " + f"{list(base_solver_map.keys())})." + ) + base_solver: SolverCfg = base_solver_map[arm_part] + else: + base_solver = base_solver_map + + base_components = base_cfg.urdf_cfg.components if base_cfg.urdf_cfg else {} + if arm_part not in base_components: + raise ValueError( + f"Base robot urdf_cfg has no component {arm_part!r} (available: " + f"{list(base_components.keys())})." + ) + arm_urdf = base_components[arm_part]["urdf_path"] + + # A solver whose ``urdf_path`` is None adopts the *assembled* dual URDF at + # ``Robot.init_solver`` time (e.g. OPW/SRS pk-based solvers), so its link + # names must be prefixed to match the assembled URDF. A solver whose + # ``urdf_path`` is pinned to the single-arm URDF (e.g. ``URSolverCfg``, set + # in ``__post_init__``) operates arm-local, so link names stay unprefixed + # and must match the single-arm URDF. This keeps FK/IK consistent per arm. + use_assembled = getattr(base_solver, "urdf_path", None) is None + + cfg.uid = f"DualArm_{base_cfg.uid}" if base_cfg.uid else "DualArmRobot" + + cfg.urdf_cfg = URDFCfg( + components=[ + { + "component_type": comp_type, + "urdf_path": arm_urdf, + "transform": mounts[side], + } + for side, comp_type, _ in _SIDES + ] + ) + name_case = cfg.urdf_cfg.name_case + + # control_parts: duplicate every base part into left_/right_ with prefixed + # joint names (generic over all base parts, e.g. arm *and* eef). + new_control: Dict[str, List[str]] = {} + for part_name, joints in base_control.items(): + for side, _comp, prefix in _SIDES: + new_control[f"{side}_{part_name}"] = [ + _prefixed_name(j, prefix, "joint", name_case) for j in joints + ] + if dual_part: + new_control["dual_arm"] = ( + new_control[f"left_{arm_part}"] + new_control[f"right_{arm_part}"] + ) + cfg.control_parts = new_control + + # solver_cfg: one solver per arm. ``replace`` makes a fresh instance per side + # (init_solver mutates urdf_path/joint_names in place, so sharing one + # instance across sides would corrupt them). + new_solver: Dict[str, SolverCfg] = {} + for side, _comp, prefix in _SIDES: + if use_assembled: + root = _prefixed_name(base_solver.root_link_name, prefix, "link", name_case) + end = _prefixed_name(base_solver.end_link_name, prefix, "link", name_case) + else: + root = base_solver.root_link_name + end = base_solver.end_link_name + new_solver[f"{side}_{arm_part}"] = base_solver.replace( + root_link_name=root, end_link_name=end + ) + cfg.solver_cfg = new_solver + + cfg.drive_pros = _mirror_drive_pros(base_cfg.drive_pros, name_case) + cfg.attrs = base_cfg.attrs.copy() + cfg.min_position_iters = base_cfg.min_position_iters + cfg.min_velocity_iters = base_cfg.min_velocity_iters + cfg.fix_base = base_cfg.fix_base + cfg.disable_self_collision = base_cfg.disable_self_collision + cfg.sleep_threshold = base_cfg.sleep_threshold + + +def build_dual_arm_cfg( + base_cfg: RobotCfg, + mounts: Dict[str, np.ndarray], + *, + dual_part: bool = True, + arm_part: str = "arm", +) -> "DualArmRobotCfg": + """Build a dual-arm cfg from a single-arm robot cfg. + + Args: + base_cfg: A constructed single-arm :class:`RobotCfg` following the + ``"arm"`` convention. + mounts: ``{"left": T, "right": T}`` 4x4 mount transforms from + :func:`resolve_mounts`. + dual_part: Whether to include a ``"dual_arm"`` composite control part. + arm_part: The base cfg's manipulator part name. + + Returns: + A populated :class:`DualArmRobotCfg`. + + Example: + + base = URRobotCfg.from_dict({"robot_type": "ur5"}) + mounts = resolve_mounts({"preset": "side_by_side", "separation": 0.6}) + cfg = build_dual_arm_cfg(base, mounts) + """ + cfg = DualArmRobotCfg() + _populate_dual_cfg(cfg, base_cfg, mounts, dual_part=dual_part, arm_part=arm_part) + return cfg + + +# --------------------------------------------------------------------------- # +# Wrapper cfg -- dict/YAML entry point +# --------------------------------------------------------------------------- # + + +@configclass +class DualArmRobotCfg(RobotCfg): + """Configuration for a dual-manipulator composed from a single-arm robot. + + Two identical arms (the ``base_robot``) are mounted on a shared synthetic + ``base_link``. The left/right ``control_parts``, per-arm ``solver_cfg`` and + mirrored ``drive_pros`` are derived automatically by + :func:`build_dual_arm_cfg`. + + Example: + + cfg = DualArmRobotCfg.from_dict( + {"base_robot": "ur5", + "mount": {"preset": "side_by_side", "separation": 0.6}} + ) + robot = sim.add_robot(cfg=cfg) + """ + + base_robot: Union[str, dict] = "ur10" + """Registry key (e.g. ``"ur5"``) or ``{"type": ..., "init": {...}}``.""" + + arm_part: str = "arm" + """Name of the base robot's manipulator control part.""" + + dual_part: bool = True + """Whether to emit a ``"dual_arm"`` composite control part.""" + + mount: dict = field(default_factory=lambda: dict(_DEFAULT_MOUNT)) + """Mount configuration consumed by :func:`resolve_mounts`.""" + + @classmethod + def from_dict(cls, init_dict: dict) -> "DualArmRobotCfg": + """Initialize from a dictionary. + + Args: + init_dict: Configuration dict. ``base_robot``, ``mount``, + ``arm_part`` and ``dual_part`` drive the dual-arm derivation; + all other recognized :class:`RobotCfg` keys are merged on top + via :func:`merge_robot_cfg`. + + Returns: + A ``DualArmRobotCfg`` 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 dual arm. + + Args: + init_dict: The raw override dict passed to ``from_dict``. + """ + init_dict = init_dict or {} + base_robot = init_dict.get("base_robot", self.base_robot) + arm_part = init_dict.get("arm_part", self.arm_part) + dual_part = init_dict.get("dual_part", self.dual_part) + mount_cfg = init_dict.get("mount", self.mount) + + base_cfg = _resolve_base_cfg(base_robot) + mounts = resolve_mounts(mount_cfg) + + _populate_dual_cfg( + self, base_cfg, mounts, dual_part=dual_part, arm_part=arm_part + ) + + # Store the user-facing inputs so to_dict()/from_dict() round-trips. + self.base_robot = base_robot + self.arm_part = arm_part + self.dual_part = dual_part + self.mount = mount_cfg + + @property + def _pk_urdf_path(self) -> str | None: + """Single-arm URDF used for the per-arm FK/IK serial chain. + + Both arms are kinematically identical, so the left arm's URDF suffices. + Returns ``None`` before :meth:`_build_defaults` has populated + ``urdf_cfg`` (the property is queried by ``@configclass`` post-init + before defaults are applied). + """ + if self.urdf_cfg is None or "left_arm" not in self.urdf_cfg.components: + return None + return self.urdf_cfg.components["left_arm"]["urdf_path"] + + def build_pk_serial_chain( + self, device: torch.device = torch.device("cpu"), **kwargs + ) -> Dict[str, "pk.SerialChain"]: + """Build the per-arm pytorch-kinematics serial chains. + + Each chain is built from the single-arm URDF with the (arm-local) root + and end link names taken from the left-arm solver, mirroring the + :class:`CobotMagicCfg` pattern. Both arms share one URDF; the chains are + keyed ``"left_arm"`` / ``"right_arm"`` for API symmetry. + + Args: + device: The device to move the chains to. Defaults to CPU. + **kwargs: Additional arguments for building the serial chains. + + Returns: + A ``{"left_arm": chain, "right_arm": chain}`` mapping. + """ + from embodichain.lab.sim.utility.solver_utils import ( + create_pk_serial_chain, + ) + + urdf_path = self._pk_urdf_path + solver = self.solver_cfg["left_arm"] + return { + "left_arm": create_pk_serial_chain( + urdf_path=urdf_path, + device=device, + end_link_name=solver.end_link_name, + root_link_name=solver.root_link_name, + ), + "right_arm": create_pk_serial_chain( + urdf_path=urdf_path, + device=device, + end_link_name=solver.end_link_name, + root_link_name=solver.root_link_name, + ), + } + + +if __name__ == "__main__": + 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="cpu", + num_envs=1, + render_cfg=RenderCfg(renderer="fast-rt"), + ) + sim = SimulationManager(config) + + cfg = DualArmRobotCfg.from_dict( + { + "base_robot": "ur5", + "mount": {"preset": "side_by_side", "separation": 0.6}, + "init_qpos": [ + 0.0, + 0.0, + -1.57, + -1.57, + 1.57, + 1.57, + -1.57, + -1.57, + -1.57, + -1.57, + 0.0, + 0.0, + ], + } + ) + 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 = DualArmRobotCfg.from_dict(cfg.to_dict()) + assert cfg2.base_robot == cfg.base_robot + assert cfg2.control_parts == cfg.control_parts + print(f"DualArm robot added ({cfg.base_robot}); round-trip OK.") + + from IPython import embed + + embed() # noqa: F401 diff --git a/embodichain/lab/sim/robots/ur_robot.py b/embodichain/lab/sim/robots/ur_robot.py new file mode 100644 index 00000000..b6a4f913 --- /dev/null +++ b/embodichain/lab/sim/robots/ur_robot.py @@ -0,0 +1,210 @@ +# ---------------------------------------------------------------------------- +# 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.drive_pros = JointDrivePropertiesCfg( + stiffness={"arm": 1e4}, + damping={"arm": 1e3}, + max_effort={"arm": _UR_MAX_EFFORT[robot_type]}, + ) + + @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=False, + sim_device="cpu", + num_envs=1, + 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": "ur10e", "init_qpos": [0.0, -1.57, 1.57, -1.57, -1.57, 0.0]} + ) + robot = sim.add_robot(cfg=cfg) + sim.open_window() + + if sim.is_use_gpu_physics: + sim.init_gpu_physics() + + from IPython import embed + + embed() # noqa: F401 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/embodichain/lab/sim/utility/cfg_utils.py b/embodichain/lab/sim/utility/cfg_utils.py index 9251b1b8..ffeb1831 100644 --- a/embodichain/lab/sim/utility/cfg_utils.py +++ b/embodichain/lab/sim/utility/cfg_utils.py @@ -163,6 +163,14 @@ def merge_robot_cfg(base_cfg: RobotCfg, override_cfg_dict: dict[str, any]) -> Ro # merge urdf components user_urdf_cfg = override_cfg_dict.get("urdf_cfg") if isinstance(user_urdf_cfg, dict): + # Merge name_case policy if the override specifies one. + user_name_case = user_urdf_cfg.get("name_case") + if isinstance(user_name_case, dict): + if base_cfg.urdf_cfg.name_case is None: + base_cfg.urdf_cfg.name_case = dict(user_name_case) + else: + base_cfg.urdf_cfg.name_case.update(user_name_case) + components = user_urdf_cfg.get("components", []) # to_dict serializes components as a dict keyed by type; # normalize to a list of dicts for merge_robot_cfg. diff --git a/embodichain/toolkits/urdf_assembly/component.py b/embodichain/toolkits/urdf_assembly/component.py index ae027224..20380505 100644 --- a/embodichain/toolkits/urdf_assembly/component.py +++ b/embodichain/toolkits/urdf_assembly/component.py @@ -102,10 +102,10 @@ def __init__( rewriting mesh references. name_case (dict[str, str] | None): Optional mapping controlling how joint and link names are normalized. Supported keys are - ``"joint"`` and ``"link"`` with values ``"upper``, - ``"lower"`` or ``"none"``. When omitted, joints are - uppercased and links are lowercased (the previous default - behavior). + ``"joint"`` and ``"link"`` with values ``"upper"``, + ``"lower"`` or ``"original"`` (legacy alias ``"none"``). + When omitted, joints are uppercased and links are lowercased + (the previous default behavior). """ self.mesh_manager = mesh_manager diff --git a/embodichain/toolkits/urdf_assembly/connection.py b/embodichain/toolkits/urdf_assembly/connection.py index 7309118c..fbbda1f4 100644 --- a/embodichain/toolkits/urdf_assembly/connection.py +++ b/embodichain/toolkits/urdf_assembly/connection.py @@ -40,7 +40,8 @@ def __init__(self, base_link_name: str, name_case: dict[str, str] | None = None) components may be attached. name_case: Optional mapping controlling how joint and link names are normalized. Supported keys are ``"joint"`` and ``"link"`` with - values ``"upper"``, ``"lower"`` or ``"none"``. + values ``"upper"``, ``"lower"`` or ``"original"`` (legacy alias + ``"none"``). When omitted, joints are uppercased and links are lowercased (the previous default behavior). diff --git a/embodichain/toolkits/urdf_assembly/name_normalizer.py b/embodichain/toolkits/urdf_assembly/name_normalizer.py index ffd9ee16..5cdeba1b 100644 --- a/embodichain/toolkits/urdf_assembly/name_normalizer.py +++ b/embodichain/toolkits/urdf_assembly/name_normalizer.py @@ -19,13 +19,15 @@ class NameNormalizer: """Handles name normalization for different entity types.""" VALID_KEYS = {"joint", "link"} - VALID_MODES = {"upper", "lower", "none"} + VALID_MODES = {"upper", "lower", "none", "original"} def __init__(self, default_case: dict[str, str] | None = None): """Initialize the NameNormalizer with default cases. Args: default_case (dict[str, str] | None): Default normalization modes for "joint" and "link". + Each value can be ``"upper"``, ``"lower"``, ``"original"`` (keep source casing), + or the legacy alias ``"none"``. """ self._name_case = { "joint": "upper", @@ -46,7 +48,7 @@ def set_case(self, key: str, mode: str): Args: key (str): The entity type ("joint" or "link"). - mode (str): The normalization mode ("upper", "lower", "none"). + mode (str): The normalization mode ("upper", "lower", "original" or "none"). """ if key in self.VALID_KEYS and mode in self.VALID_MODES: self._name_case[key] = mode @@ -64,7 +66,7 @@ def normalize(self, kind: str, name: str | None) -> str | None: name (str | None): The original name. Returns: - str | None: The normalized name, or the original value if kind is unknown or mode is "none". + str | None: The normalized name, or the original value if kind is unknown or the mode is "original"/"none". """ if name is None: return None diff --git a/scripts/tutorials/atomic_action/move_end_effector.py b/scripts/tutorials/atomic_action/move_end_effector.py index 2194be8c..ae601053 100644 --- a/scripts/tutorials/atomic_action/move_end_effector.py +++ b/scripts/tutorials/atomic_action/move_end_effector.py @@ -29,7 +29,6 @@ import torch -from embodichain.data import get_data_path from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.atomic_actions import ( @@ -38,28 +37,18 @@ MoveEndEffector, MoveEndEffectorCfg, ) -from embodichain.lab.sim.cfg import ( - JointDrivePropertiesCfg, - LightCfg, - RenderCfg, - RobotCfg, - URDFCfg, -) +from embodichain.lab.sim.cfg import LightCfg, RenderCfg from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg -from embodichain.lab.sim.solvers import PytorchSolverCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + create_ur5_gripper_robot_cfg, draw_axis_marker, get_tutorial_window_size, start_auto_play_recording, stop_auto_play_recording, ) -GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" -GRIPPER_HAND_JOINT_PATTERN = "GRIPPER_FINGER1_JOINT_1" -GRIPPER_TCP_Z = 0.15 - MOVE_SAMPLE_INTERVAL = 80 POST_TRAJECTORY_STEPS = 120 @@ -106,41 +95,7 @@ def initialize_simulation(args: argparse.Namespace) -> SimulationManager: def create_robot(sim: SimulationManager, position=(0.0, 0.0, 0.0)) -> Robot: - ur5_urdf_path = get_data_path("UniversalRobots/UR5/UR5.urdf") - gripper_urdf_path = get_data_path(GRIPPER_URDF_PATH) - cfg = RobotCfg( - uid="UR5", - urdf_cfg=URDFCfg( - components=[ - {"component_type": "arm", "urdf_path": ur5_urdf_path}, - {"component_type": "hand", "urdf_path": gripper_urdf_path}, - ] - ), - drive_pros=JointDrivePropertiesCfg( - stiffness={"JOINT[0-9]": 1e4, GRIPPER_HAND_JOINT_PATTERN: 1e3}, - damping={"JOINT[0-9]": 1e3, GRIPPER_HAND_JOINT_PATTERN: 1e2}, - max_effort={"JOINT[0-9]": 1e5, GRIPPER_HAND_JOINT_PATTERN: 1e4}, - drive_type="force", - ), - control_parts={ - "arm": ["JOINT[0-9]"], - "hand": [GRIPPER_HAND_JOINT_PATTERN], - }, - solver_cfg={ - "arm": PytorchSolverCfg( - end_link_name="ee_link", - root_link_name="base_link", - tcp=[ - [1.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, GRIPPER_TCP_Z], - [0.0, 0.0, 0.0, 1.0], - ], - ) - }, - init_qpos=[0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], - init_pos=position, - ) + cfg = create_ur5_gripper_robot_cfg(init_pos=position) return sim.add_robot(cfg=cfg) diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index 6873be12..3a1620d2 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -35,6 +35,7 @@ from embodichain.lab.sim.atomic_actions import ( AntipodalAffordance, AtomicActionEngine, + EndEffectorPoseTarget, GraspTarget, HeldObjectPoseTarget, MoveEndEffector, @@ -44,21 +45,16 @@ ObjectSemantics, PickUp, PickUpCfg, - EndEffectorPoseTarget, ) from embodichain.lab.sim.cfg import ( - JointDrivePropertiesCfg, LightCfg, RenderCfg, RigidBodyAttributesCfg, RigidObjectCfg, - RobotCfg, - URDFCfg, ) from embodichain.lab.sim.objects import RigidObject, Robot from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg from embodichain.lab.sim.shapes import MeshCfg -from embodichain.lab.sim.solvers import PytorchSolverCfg from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import ( AntipodalSamplerCfg, GraspGeneratorCfg, @@ -68,19 +64,17 @@ ) from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + create_ur5_gripper_robot_cfg, draw_axis_marker, get_tutorial_window_size, start_auto_play_recording, stop_auto_play_recording, ) -GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" -GRIPPER_HAND_JOINT_PATTERN = "GRIPPER_FINGER1_JOINT_1" GRIPPER_MAX_OPEN_WIDTH = 0.080 GRIPPER_FINGER_LENGTH = 0.088 GRIPPER_ROOT_Z_WIDTH = 0.096 GRIPPER_Y_THICKNESS = 0.040 -GRIPPER_TCP_Z = 0.15 OBJECT_LABEL = "sugar_box" OBJECT_MESH_PATH = "SugarBox/sugar_box_usd/sugar_box.usda" @@ -152,42 +146,7 @@ def initialize_simulation(args: argparse.Namespace) -> SimulationManager: def create_robot(sim: SimulationManager, position=(0.0, 0.0, 0.0)) -> Robot: - ur5_urdf_path = get_data_path("UniversalRobots/UR5/UR5.urdf") - gripper_urdf_path = get_data_path(GRIPPER_URDF_PATH) - - cfg = RobotCfg( - uid="UR5", - urdf_cfg=URDFCfg( - components=[ - {"component_type": "arm", "urdf_path": ur5_urdf_path}, - {"component_type": "hand", "urdf_path": gripper_urdf_path}, - ] - ), - drive_pros=JointDrivePropertiesCfg( - stiffness={"JOINT[0-9]": 1e4, GRIPPER_HAND_JOINT_PATTERN: 1e3}, - damping={"JOINT[0-9]": 1e3, GRIPPER_HAND_JOINT_PATTERN: 1e2}, - max_effort={"JOINT[0-9]": 1e5, GRIPPER_HAND_JOINT_PATTERN: 1e4}, - drive_type="force", - ), - control_parts={ - "arm": ["JOINT[0-9]"], - "hand": [GRIPPER_HAND_JOINT_PATTERN], - }, - solver_cfg={ - "arm": PytorchSolverCfg( - end_link_name="ee_link", - root_link_name="base_link", - tcp=[ - [1.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, GRIPPER_TCP_Z], - [0.0, 0.0, 0.0, 1.0], - ], - ) - }, - init_qpos=[0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], - init_pos=position, - ) + cfg = create_ur5_gripper_robot_cfg(init_pos=position) return sim.add_robot(cfg=cfg) diff --git a/scripts/tutorials/atomic_action/move_joints.py b/scripts/tutorials/atomic_action/move_joints.py index 32cfc44d..1d45816b 100644 --- a/scripts/tutorials/atomic_action/move_joints.py +++ b/scripts/tutorials/atomic_action/move_joints.py @@ -29,7 +29,6 @@ import torch -from embodichain.data import get_data_path from embodichain.lab.gym.utils.gym_utils import add_env_launcher_args_to_parser from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.atomic_actions import ( @@ -39,28 +38,18 @@ MoveJointsCfg, NamedJointPositionTarget, ) -from embodichain.lab.sim.cfg import ( - JointDrivePropertiesCfg, - LightCfg, - RenderCfg, - RobotCfg, - URDFCfg, -) +from embodichain.lab.sim.cfg import LightCfg, RenderCfg from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg -from embodichain.lab.sim.solvers import PytorchSolverCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + create_ur5_gripper_robot_cfg, draw_axis_marker, get_tutorial_window_size, start_auto_play_recording, stop_auto_play_recording, ) -GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" -GRIPPER_HAND_JOINT_PATTERN = "GRIPPER_FINGER1_JOINT_1" -GRIPPER_TCP_Z = 0.15 - MOVE_JOINTS_SAMPLE_INTERVAL = 80 POST_TRAJECTORY_STEPS = 120 @@ -107,41 +96,7 @@ def initialize_simulation(args: argparse.Namespace) -> SimulationManager: def create_robot(sim: SimulationManager, position=(0.0, 0.0, 0.0)) -> Robot: - ur5_urdf_path = get_data_path("UniversalRobots/UR5/UR5.urdf") - gripper_urdf_path = get_data_path(GRIPPER_URDF_PATH) - cfg = RobotCfg( - uid="UR5", - urdf_cfg=URDFCfg( - components=[ - {"component_type": "arm", "urdf_path": ur5_urdf_path}, - {"component_type": "hand", "urdf_path": gripper_urdf_path}, - ] - ), - drive_pros=JointDrivePropertiesCfg( - stiffness={"JOINT[0-9]": 1e4, GRIPPER_HAND_JOINT_PATTERN: 1e3}, - damping={"JOINT[0-9]": 1e3, GRIPPER_HAND_JOINT_PATTERN: 1e2}, - max_effort={"JOINT[0-9]": 1e5, GRIPPER_HAND_JOINT_PATTERN: 1e4}, - drive_type="force", - ), - control_parts={ - "arm": ["JOINT[0-9]"], - "hand": [GRIPPER_HAND_JOINT_PATTERN], - }, - solver_cfg={ - "arm": PytorchSolverCfg( - end_link_name="ee_link", - root_link_name="base_link", - tcp=[ - [1.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, GRIPPER_TCP_Z], - [0.0, 0.0, 0.0, 1.0], - ], - ) - }, - init_qpos=[0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], - init_pos=position, - ) + cfg = create_ur5_gripper_robot_cfg(init_pos=position) return sim.add_robot(cfg=cfg) diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index 4edd089a..8c995d31 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -41,18 +41,14 @@ PickUpCfg, ) from embodichain.lab.sim.cfg import ( - JointDrivePropertiesCfg, LightCfg, RenderCfg, RigidBodyAttributesCfg, RigidObjectCfg, - RobotCfg, - URDFCfg, ) from embodichain.lab.sim.objects import RigidObject, Robot from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg from embodichain.lab.sim.shapes import MeshCfg -from embodichain.lab.sim.solvers import PytorchSolverCfg from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import ( AntipodalSamplerCfg, GraspGeneratorCfg, @@ -62,19 +58,17 @@ ) from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + create_ur5_gripper_robot_cfg, draw_axis_marker, get_tutorial_window_size, start_auto_play_recording, stop_auto_play_recording, ) -GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" -GRIPPER_HAND_JOINT_PATTERN = "GRIPPER_FINGER1_JOINT_1" GRIPPER_MAX_OPEN_WIDTH = 0.080 GRIPPER_FINGER_LENGTH = 0.088 GRIPPER_ROOT_Z_WIDTH = 0.096 GRIPPER_Y_THICKNESS = 0.040 -GRIPPER_TCP_Z = 0.15 OBJECT_MIN_HAND_CLOSE_QPOS = 0.024 OBJECT_XY = (-0.42, -0.08) @@ -174,42 +168,7 @@ def initialize_simulation(args: argparse.Namespace) -> SimulationManager: def create_robot(sim: SimulationManager, position=(0.0, 0.0, 0.0)) -> Robot: - ur5_urdf_path = get_data_path("UniversalRobots/UR5/UR5.urdf") - gripper_urdf_path = get_data_path(GRIPPER_URDF_PATH) - - cfg = RobotCfg( - uid="UR5", - urdf_cfg=URDFCfg( - components=[ - {"component_type": "arm", "urdf_path": ur5_urdf_path}, - {"component_type": "hand", "urdf_path": gripper_urdf_path}, - ] - ), - drive_pros=JointDrivePropertiesCfg( - stiffness={"JOINT[0-9]": 1e4, GRIPPER_HAND_JOINT_PATTERN: 1e3}, - damping={"JOINT[0-9]": 1e3, GRIPPER_HAND_JOINT_PATTERN: 1e2}, - max_effort={"JOINT[0-9]": 1e5, GRIPPER_HAND_JOINT_PATTERN: 1e4}, - drive_type="force", - ), - control_parts={ - "arm": ["JOINT[0-9]"], - "hand": [GRIPPER_HAND_JOINT_PATTERN], - }, - solver_cfg={ - "arm": PytorchSolverCfg( - end_link_name="ee_link", - root_link_name="base_link", - tcp=[ - [1.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, GRIPPER_TCP_Z], - [0.0, 0.0, 0.0, 1.0], - ], - ) - }, - init_qpos=[0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], - init_pos=position, - ) + cfg = create_ur5_gripper_robot_cfg(init_pos=position) return sim.add_robot(cfg=cfg) diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index f6ec66bc..940dc44b 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -44,18 +44,14 @@ PlaceCfg, ) from embodichain.lab.sim.cfg import ( - JointDrivePropertiesCfg, LightCfg, RenderCfg, RigidBodyAttributesCfg, RigidObjectCfg, - RobotCfg, - URDFCfg, ) from embodichain.lab.sim.objects import RigidObject, Robot from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg from embodichain.lab.sim.shapes import MeshCfg -from embodichain.lab.sim.solvers import PytorchSolverCfg from embodichain.toolkits.graspkit.pg_grasp.antipodal_generator import ( AntipodalSamplerCfg, GraspGeneratorCfg, @@ -65,19 +61,17 @@ ) from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + create_ur5_gripper_robot_cfg, draw_axis_marker, get_tutorial_window_size, start_auto_play_recording, stop_auto_play_recording, ) -GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" -GRIPPER_HAND_JOINT_PATTERN = "GRIPPER_FINGER1_JOINT_1" GRIPPER_MAX_OPEN_WIDTH = 0.080 GRIPPER_FINGER_LENGTH = 0.088 GRIPPER_ROOT_Z_WIDTH = 0.096 GRIPPER_Y_THICKNESS = 0.040 -GRIPPER_TCP_Z = 0.15 OBJECT_LABEL = "sugar_box" OBJECT_MESH_PATH = "SugarBox/sugar_box_usd/sugar_box.usda" @@ -149,41 +143,7 @@ def initialize_simulation(args: argparse.Namespace) -> SimulationManager: def create_robot(sim: SimulationManager, position=(0.0, 0.0, 0.0)) -> Robot: - ur5_urdf_path = get_data_path("UniversalRobots/UR5/UR5.urdf") - gripper_urdf_path = get_data_path(GRIPPER_URDF_PATH) - cfg = RobotCfg( - uid="UR5", - urdf_cfg=URDFCfg( - components=[ - {"component_type": "arm", "urdf_path": ur5_urdf_path}, - {"component_type": "hand", "urdf_path": gripper_urdf_path}, - ] - ), - drive_pros=JointDrivePropertiesCfg( - stiffness={"JOINT[0-9]": 1e4, GRIPPER_HAND_JOINT_PATTERN: 1e3}, - damping={"JOINT[0-9]": 1e3, GRIPPER_HAND_JOINT_PATTERN: 1e2}, - max_effort={"JOINT[0-9]": 1e5, GRIPPER_HAND_JOINT_PATTERN: 1e4}, - drive_type="force", - ), - control_parts={ - "arm": ["JOINT[0-9]"], - "hand": [GRIPPER_HAND_JOINT_PATTERN], - }, - solver_cfg={ - "arm": PytorchSolverCfg( - end_link_name="ee_link", - root_link_name="base_link", - tcp=[ - [1.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, GRIPPER_TCP_Z], - [0.0, 0.0, 0.0, 1.0], - ], - ) - }, - init_qpos=[0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], - init_pos=position, - ) + cfg = create_ur5_gripper_robot_cfg(init_pos=position) return sim.add_robot(cfg=cfg) diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index c846455c..8b8ea6ba 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -23,8 +23,10 @@ import torch +from embodichain.data import get_data_path from embodichain.lab.sim import SimulationManager -from embodichain.lab.sim.cfg import MarkerCfg +from embodichain.lab.sim.cfg import MarkerCfg, RobotCfg +from embodichain.lab.sim.robots import URRobotCfg RECORD_WIDTH = 640 RECORD_HEIGHT = 480 @@ -40,6 +42,10 @@ DEFAULT_AXIS_LEN = 0.06 DEFAULT_AXIS_SIZE = 0.003 +GRIPPER_URDF_PATH = "DH_PGI_140_80/DH_PGI_140_80.urdf" +GRIPPER_HAND_JOINT_PATTERN = "gripper_finger1_joint_1" +GRIPPER_TCP_Z = 0.15 + def get_tutorial_window_size(args: argparse.Namespace) -> tuple[int, int]: """Return the viewer window size used by atomic-action tutorials.""" @@ -111,10 +117,82 @@ def draw_axis_marker( ) +def create_ur5_gripper_robot_cfg( + init_pos: Sequence[float] = (0.0, 0.0, 0.0), +) -> RobotCfg: + """Build a UR5 arm + DH_PGI_140_80 gripper robot configuration. + + The arm is taken from :class:`~embodichain.lab.sim.robots.ur_robot.URRobotCfg` + so the URDF, joint names and drive defaults match the canonical UR family + config. The gripper and tool-center-point offset are added on top to match + the existing atomic-action tutorial setups. + + .. attention:: + :class:`~embodichain.lab.sim.cfg.URDFCfg` defaults to upper-casing joint + names during multi-component assembly. The override dict passed to + :meth:`URRobotCfg.from_dict` sets + ``urdf_cfg.name_case = {"joint": "lower", "link": "lower"}`` so the + assembled robot keeps the source URDF's lowercase joint names + (``joint1``..``joint6`` and ``gripper_finger1_joint_1``), matching the + control parts produced by + :class:`~embodichain.lab.sim.robots.ur_robot.URRobotCfg`. + + Args: + init_pos: Initial root position of the robot in the arena. + + Returns: + A fully populated :class:`~embodichain.lab.sim.cfg.RobotCfg`. + """ + return URRobotCfg.from_dict( + { + "robot_type": "ur5", + "uid": "UR5", + "urdf_cfg": { + "components": [ + { + "component_type": "hand", + "urdf_path": GRIPPER_URDF_PATH, + } + ], + }, + "control_parts": { + "hand": [GRIPPER_HAND_JOINT_PATTERN], + }, + "drive_pros": { + "stiffness": { + GRIPPER_HAND_JOINT_PATTERN: 1e3, + }, + "damping": { + GRIPPER_HAND_JOINT_PATTERN: 1e2, + }, + "max_effort": { + GRIPPER_HAND_JOINT_PATTERN: 1e4, + }, + }, + "solver_cfg": { + "arm": { + "tcp": [ + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 1.0, GRIPPER_TCP_Z], + [0.0, 0.0, 0.0, 1.0], + ] + } + }, + "init_qpos": [0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], + "init_pos": init_pos, + } + ) + + __all__ = [ "DEFAULT_AUTO_PLAY_LOOK_AT", "DEFAULT_AXIS_LEN", "DEFAULT_AXIS_SIZE", + "GRIPPER_HAND_JOINT_PATTERN", + "GRIPPER_TCP_Z", + "GRIPPER_URDF_PATH", + "create_ur5_gripper_robot_cfg", "get_tutorial_window_size", "start_auto_play_recording", "stop_auto_play_recording", diff --git a/tests/sim/objects/test_dual_arm.py b/tests/sim/objects/test_dual_arm.py new file mode 100644 index 00000000..93a5bc06 --- /dev/null +++ b/tests/sim/objects/test_dual_arm.py @@ -0,0 +1,244 @@ +# ---------------------------------------------------------------------------- +# 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 xml.etree.ElementTree as ET + +import numpy as np +import pytest + +from embodichain.lab.sim.robots.dual_arm import ( + DualArmRobotCfg, + _transform_from_xyz_rpy, + build_dual_arm_cfg, + resolve_mounts, +) +from embodichain.lab.sim.robots.ur_robot import URRobotCfg +from embodichain.lab.sim.solvers import URSolverCfg + +# --------------------------------------------------------------------------- # +# resolve_mounts +# --------------------------------------------------------------------------- # + + +def test_resolve_mounts_side_by_side_is_symmetric(): + m = resolve_mounts({"preset": "side_by_side", "separation": 0.6}) + # Left at +separation/2 in Y, right at -separation/2, identity orientation. + assert np.allclose(m["left"][:3, 3], [0.0, 0.3, 0.0]) + assert np.allclose(m["right"][:3, 3], [0.0, -0.3, 0.0]) + assert np.allclose(m["left"][:3, :3], np.eye(3)) + assert np.allclose(m["right"][:3, :3], np.eye(3)) + + +def test_resolve_mounts_side_by_side_default_preset(): + # No preset -> side_by_side. + m = resolve_mounts({"separation": 0.6}) + assert np.allclose(m["left"][:3, 3], [0.0, 0.3, 0.0]) + + +def test_resolve_mounts_facing_inward_is_mirrored(): + m = resolve_mounts({"preset": "facing_inward", "separation": 0.6}) + # Both translated in Y, orientations are mirrored (transpose of each other) + # and non-identity. + assert np.allclose(m["left"][:3, 3], [0.0, 0.3, 0.0]) + assert np.allclose(m["right"][:3, 3], [0.0, -0.3, 0.0]) + assert not np.allclose(m["left"][:3, :3], np.eye(3)) + assert np.allclose(m["left"][:3, :3], m["right"][:3, :3].T) + + +def test_resolve_mounts_mirrored_rz_uses_signed_yaw(): + rz = np.pi / 4 + m = resolve_mounts({"preset": "mirrored_rz", "separation": 0.6, "rz": rz}) + assert np.allclose(m["left"][:3, 3], [0.0, 0.3, 0.0]) + assert np.allclose(m["right"][:3, 3], [0.0, -0.3, 0.0]) + expected_left = _transform_from_xyz_rpy([0.0, 0.3, 0.0], [0.0, 0.0, rz]) + expected_right = _transform_from_xyz_rpy([0.0, -0.3, 0.0], [0.0, 0.0, -rz]) + assert np.allclose(m["left"], expected_left) + assert np.allclose(m["right"], expected_right) + + +def test_resolve_mounts_mirrored_rz_defaults_to_zero_yaw(): + m = resolve_mounts({"preset": "mirrored_rz", "separation": 0.6}) + assert np.allclose(m["left"][:3, 3], [0.0, 0.3, 0.0]) + assert np.allclose(m["right"][:3, 3], [0.0, -0.3, 0.0]) + assert np.allclose(m["left"][:3, :3], np.eye(3)) + assert np.allclose(m["right"][:3, :3], np.eye(3)) + + +def test_resolve_mounts_per_arm_override(): + m = resolve_mounts( + { + "preset": "side_by_side", + "separation": 0.6, + "left": {"xyz": [0.1, 0.35, 0.0], "rpy": [0, 0, 0]}, + "right": {"xyz": [0.1, -0.35, 0.0], "rpy": [0, 0, 0]}, + } + ) + assert np.allclose(m["left"][:3, 3], [0.1, 0.35, 0.0]) + assert np.allclose(m["right"][:3, 3], [0.1, -0.35, 0.0]) + + +def test_resolve_mounts_one_sided_override_raises(): + with pytest.raises(ValueError): + resolve_mounts( + { + "preset": "side_by_side", + "separation": 0.6, + "left": {"xyz": [0, 0, 0], "rpy": [0, 0, 0]}, + } + ) + + +def test_resolve_mounts_unknown_preset_raises(): + with pytest.raises(ValueError): + resolve_mounts({"preset": "telepathic", "separation": 0.6}) + + +# --------------------------------------------------------------------------- # +# build_dual_arm_cfg on a URRobotCfg (generic engine) +# --------------------------------------------------------------------------- # + + +def _ur5_dual(): + base = URRobotCfg.from_dict({"robot_type": "ur5"}) + mounts = resolve_mounts({"preset": "side_by_side", "separation": 0.6}) + return build_dual_arm_cfg(base, mounts) + + +def test_build_dual_arm_ur_control_parts(): + cfg = _ur5_dual() + assert cfg.control_parts["left_arm"] == [f"left_joint{i}" for i in range(1, 7)] + assert cfg.control_parts["right_arm"] == [f"right_joint{i}" for i in range(1, 7)] + # dual_arm composite part concatenates both arms (12 joints). + assert cfg.control_parts["dual_arm"] == ( + [f"left_joint{i}" for i in range(1, 7)] + + [f"right_joint{i}" for i in range(1, 7)] + ) + + +def test_build_dual_arm_ur_solver_is_per_arm_and_arm_local(): + cfg = _ur5_dual() + for side in ("left_arm", "right_arm"): + solver = cfg.solver_cfg[side] + assert isinstance(solver, URSolverCfg) + assert solver.ur_type == "ur5" + # URSolverCfg pins urdf_path to the single-arm URDF in __post_init__, so + # the engine keeps link names UNPREFIXED to match that URDF (arm-local). + assert solver.root_link_name == "base_link" + assert solver.end_link_name == "ee_link" + + +def test_build_dual_arm_ur_urdf_components(): + cfg = _ur5_dual() + assert set(cfg.urdf_cfg.components.keys()) == {"left_arm", "right_arm"} + # Both arms use the same base arm URDF. + base = URRobotCfg.from_dict({"robot_type": "ur5"}) + arm_urdf = base.urdf_cfg.components["arm"]["urdf_path"] + assert cfg.urdf_cfg.components["left_arm"]["urdf_path"] == arm_urdf + assert cfg.urdf_cfg.components["right_arm"]["urdf_path"] == arm_urdf + + +def test_build_dual_arm_dual_part_toggle(): + base = URRobotCfg.from_dict({"robot_type": "ur5"}) + mounts = resolve_mounts({"preset": "side_by_side", "separation": 0.6}) + cfg = build_dual_arm_cfg(base, mounts, dual_part=False) + assert "dual_arm" not in cfg.control_parts + + +# --------------------------------------------------------------------------- # +# DualArmRobotCfg from_dict + round-trip +# --------------------------------------------------------------------------- # + + +def test_dual_arm_from_dict_one_liner(): + cfg = DualArmRobotCfg.from_dict( + {"base_robot": "ur5", "mount": {"preset": "side_by_side", "separation": 0.6}} + ) + assert set(["left_arm", "right_arm", "dual_arm"]).issubset(cfg.control_parts.keys()) + assert isinstance(cfg.solver_cfg["left_arm"], URSolverCfg) + assert cfg.solver_cfg["left_arm"].ur_type == "ur5" + + +def test_dual_arm_from_dict_explicit_base_robot(): + cfg = DualArmRobotCfg.from_dict( + { + "base_robot": {"type": "ur5", "init": {"robot_type": "ur5"}}, + "mount": {"preset": "side_by_side", "separation": 0.6}, + } + ) + assert cfg.solver_cfg["left_arm"].ur_type == "ur5" + + +def test_dual_arm_unknown_base_robot_raises(): + with pytest.raises(ValueError): + DualArmRobotCfg.from_dict({"base_robot": "franka"}) + + +def test_dual_arm_roundtrip(): + cfg = DualArmRobotCfg.from_dict( + {"base_robot": "ur5", "mount": {"preset": "side_by_side", "separation": 0.6}} + ) + d = cfg.to_dict() + assert d["base_robot"] == "ur5" + cfg2 = DualArmRobotCfg.from_dict(d) + assert cfg2.control_parts == cfg.control_parts + assert isinstance(cfg2.solver_cfg["left_arm"], URSolverCfg) + assert cfg2.solver_cfg["left_arm"].ur_type == "ur5" + + +# --------------------------------------------------------------------------- # +# PK drift-guard: build_pk_serial_chain DOF matches control_parts +# --------------------------------------------------------------------------- # + + +def _dof_of_pk_chain(chain) -> int: + """Number of actuated joints in a pk.SerialChain.""" + return len(chain.get_joint_parameter_names()) + + +def test_dual_arm_pk_dof_matches_control_parts(): + pytest.importorskip("pytorch_kinematics") + cfg = DualArmRobotCfg.from_dict( + {"base_robot": "ur5", "mount": {"preset": "side_by_side", "separation": 0.6}} + ) + try: + chains = cfg.build_pk_serial_chain() + except Exception as exc: + pytest.skip(f"PK URDF asset unavailable: {exc}") + for arm in ("left_arm", "right_arm"): + assert _dof_of_pk_chain(chains[arm]) == len( + cfg.control_parts[arm] + ), f"{arm}: PK chain DOF drifted from control_parts" + + +# --------------------------------------------------------------------------- # +# Integration: assemble a real dual UR5 URDF and verify predicted names match +# --------------------------------------------------------------------------- # + + +def test_dual_arm_ur5_assembled_joint_names_match_prediction(): + cfg = DualArmRobotCfg.from_dict( + {"base_robot": "ur5", "mount": {"preset": "side_by_side", "separation": 0.6}} + ) + fpath = cfg.urdf_cfg.assemble_urdf() + tree = ET.parse(fpath) + joint_names = {j.get("name") for j in tree.findall("joint")} + for i in range(1, 7): + assert f"left_joint{i}" in joint_names, ( + f"predicted left_joint{i} not in assembled URDF; " + f"prefix/case convention drifted from URDFAssemblyManager" + ) + assert f"right_joint{i}" in joint_names diff --git a/tests/sim/objects/test_robot_cfg.py b/tests/sim/objects/test_robot_cfg.py index 92947c65..a345e5f6 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["arm"] # 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"}) diff --git a/tests/sim/sensors/test_contact.py b/tests/sim/sensors/test_contact.py index aa38fc22..a972979d 100644 --- a/tests/sim/sensors/test_contact.py +++ b/tests/sim/sensors/test_contact.py @@ -32,8 +32,8 @@ SensorCfg, ) from embodichain.lab.sim.shapes import CubeCfg -from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg, Robot, RobotCfg -from embodichain.data import get_data_path +from embodichain.lab.sim.objects import RigidObject, RigidObjectCfg, Robot +from embodichain.lab.sim.robots import URRobotCfg NUM_ENVS = 4 @@ -108,46 +108,26 @@ def create_robot(self, uid: str, position: list = (0.0, 0.0, 0)) -> Robot: Returns: Robot: _description_ """ - ur10_urdf_path = get_data_path("UniversalRobots/UR10/UR10.urdf") - pgi_urdf_path = get_data_path("DH_PGC_140_50/DH_PGC_140_50.urdf") robot_cfg_dict = { - "uid": "UR10_PGI", + "robot_type": "ur10", + "uid": uid, "urdf_cfg": { "components": [ - { - "component_type": "arm", - "urdf_path": ur10_urdf_path, - "transform": [ - [1.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0], - [0.0, 0.0, 0.0, 1.0], - ], - }, { "component_type": "hand", - "urdf_path": pgi_urdf_path, - "transform": [ - [1.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0], - [0.0, 0.0, 1.0, 0.0], - [0.0, 0.0, 0.0, 1.0], - ], + "urdf_path": "DH_PGC_140_50/DH_PGC_140_50.urdf", }, ], }, "init_pos": position, "init_qpos": [0.0, -1.57, 1.57, -1.57, -1.57, 0.0, 0.0, 0.0], "drive_pros": { - "stiffness": {"JOINT[1-6]": 1e4, "FINGER[1-2]_JOINT": 1e2}, - "damping": {"JOINT[1-6]": 1e3, "FINGER[1-2]_JOINT": 1e1}, - "max_effort": {"JOINT[1-6]": 1e5, "FINGER[1-2]_JOINT": 1e3}, + "stiffness": {"finger[1-2]_joint": 1e2}, + "damping": {"finger[1-2]_joint": 1e1}, + "max_effort": {"finger[1-2]_joint": 1e3}, }, "solver_cfg": { "arm": { - "class_type": "PytorchSolver", - "end_link_name": "ee_link", - "root_link_name": "base_link", "tcp": [ [1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], @@ -156,9 +136,9 @@ def create_robot(self, uid: str, position: list = (0.0, 0.0, 0)) -> Robot: ], } }, - "control_parts": {"arm": ["JOINT[1-6]"], "hand": ["FINGER[1-2]_JOINT"]}, + "control_parts": {"hand": ["finger[1-2]_joint"]}, } - robot: Robot = self.sim.add_robot(cfg=RobotCfg.from_dict(robot_cfg_dict)) + robot: Robot = self.sim.add_robot(cfg=URRobotCfg.from_dict(robot_cfg_dict)) return robot def to_grasp_pose(self, cube: RigidObject): diff --git a/tests/sim/solvers/test_ur_solver.py b/tests/sim/solvers/test_ur_solver.py index 626ff9d6..0c84d8e1 100644 --- a/tests/sim/solvers/test_ur_solver.py +++ b/tests/sim/solvers/test_ur_solver.py @@ -94,13 +94,13 @@ def setup_simulation(self, sim_device): ] ), drive_pros=JointDrivePropertiesCfg( - stiffness={"JOINT[0-9]": 1e4, "FINGER[1-2]": 1e3}, - damping={"JOINT[0-9]": 1e3, "FINGER[1-2]": 1e2}, - max_effort={"JOINT[0-9]": 1e5, "FINGER[1-2]": 1e4}, + stiffness={"Joint[0-9]": 1e4, "FINGER[1-2]": 1e3}, + damping={"Joint[0-9]": 1e3, "FINGER[1-2]": 1e2}, + max_effort={"Joint[0-9]": 1e5, "FINGER[1-2]": 1e4}, drive_type="force", ), control_parts={ - "arm": ["JOINT[0-9]"], + "arm": ["Joint[0-9]"], "hand": ["FINGER[1-2]"], }, solver_cfg={ diff --git a/tests/toolkits/test_grasp_pose_generator.py b/tests/toolkits/test_grasp_pose_generator.py index ecb9cfac..3aba2c25 100644 --- a/tests/toolkits/test_grasp_pose_generator.py +++ b/tests/toolkits/test_grasp_pose_generator.py @@ -102,13 +102,13 @@ def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]) -> Robot: ] ), drive_pros=JointDrivePropertiesCfg( - stiffness={"JOINT[0-9]": 1e4, "FINGER[1-2]": 1e3}, - damping={"JOINT[0-9]": 1e3, "FINGER[1-2]": 1e2}, - max_effort={"JOINT[0-9]": 1e5, "FINGER[1-2]": 1e4}, + stiffness={"Joint[0-9]": 1e4, "FINGER[1-2]": 1e3}, + damping={"Joint[0-9]": 1e3, "FINGER[1-2]": 1e2}, + max_effort={"Joint[0-9]": 1e5, "FINGER[1-2]": 1e4}, drive_type="force", ), control_parts={ - "arm": ["JOINT[0-9]"], + "arm": ["Joint[0-9]"], "hand": ["FINGER[1-2]"], }, solver_cfg={