diff --git a/tasks/README.md b/tasks/README.md new file mode 100644 index 0000000..847ab16 --- /dev/null +++ b/tasks/README.md @@ -0,0 +1,142 @@ +# rpl_openarm_centrifuge + +Bimanual OpenArm **centrifuge pick-and-place** task for [Isaac Lab](https://github.com/isaac-sim/IsaacLab) — a plastic centrifuge tube starts on the rack holder; the goal is to drop it inside the centrifuge bucket. Designed for human teleoperation and imitation learning. + +The package is an external Isaac Lab task extension: it bundles its USD assets, registers two Gym IDs on import, and ships a `.pth` file so the registration happens automatically in any Python process running inside the Isaac Lab venv — no edits to Isaac Lab itself required. + +## Registered tasks + +| Gym ID | Action mode | Suited for | +|---|---|---| +| `Isaac-Centrifuge-Bimanual-OpenArm-IK-Rel-v0` | Relative differential IK (14-dim Δpose + grippers) | Keyboard / SpaceMouse / fallback; left-arm-padded keyboard wrapper included | +| `Isaac-Centrifuge-Bimanual-OpenArm-IK-Abs-v0` | Absolute differential IK | Hand-tracking via OpenXR (recommended) | + +## Requirements + +* Isaac Lab installed (provides `isaaclab`, `isaaclab.devices`, and `isaaclab_assets.robots.openarm.OPENARM_BI_HIGH_PD_CFG`) +* Python ≥ 3.10 +* (Optional) An OpenXR-compatible headset + hand tracking for the IK-Abs / bimanual teleop path + +## Install + +From inside the Isaac Lab venv (so the Isaac Lab Python sees it): + +```bash +cd /path/to/rpl_openarm_centrifuge +/path/to/isaaclab.sh -p -m pip install -e . +``` + +The install drops a `rpl_openarm_centrifuge_autoregister.pth` file next to your site-packages. From then on, every Python process inside the Isaac Lab venv auto-imports `rpl_openarm_centrifuge` at startup, so `gym.make("Isaac-Centrifuge-...")` works without any script edits. + +If the `.pth` mechanism doesn't fire (some environments strip them, especially when running scripts outside the venv), add `import rpl_openarm_centrifuge # noqa: F401` to your script before the first `gym.make` call as a fallback. + +## Run + +Smoke-test (no teleop): + +```bash +./isaaclab.sh -p scripts/environments/zero_agent.py \ + --task Isaac-Centrifuge-Bimanual-OpenArm-IK-Rel-v0 --num_envs 1 +``` + +Keyboard teleop (right arm only — left arm holds its default pose): + +```bash +./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-Centrifuge-Bimanual-OpenArm-IK-Rel-v0 --teleop_device keyboard +``` + +Key bindings: `WSAD`/`QE` translate, `ZX`/`TG`/`CV` rotate, `K` toggles gripper, `L` resets deltas. + +Bimanual hand-tracking teleop: + +```bash +LIVESTREAM=2 ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-Centrifuge-Bimanual-OpenArm-IK-Abs-v0 --teleop_device handtracking +``` + +Record demonstrations: + +```bash +./isaaclab.sh -p scripts/tools/record_demos.py \ + --task Isaac-Centrifuge-Bimanual-OpenArm-IK-Abs-v0 \ + --teleop_device handtracking \ + --dataset_file ./datasets/centrifuge_demos.hdf5 \ + --num_demos 10 +``` + +## Assets + +The package bundles physics-ready USDs in `src/rpl_openarm_centrifuge/assets/`: + +* `centrifuge_tube_big.usd` — the plastic tube (dynamic rigid body, convex-hull collision, mass) +* `centrifuge_bucket_big.usd` — the bucket (dynamic rigid body, convex-hull collision, mass) +* `centrifuge_tube_rack.usd` — static rack with convex-decomposition collision (the wells stay concave so the tube can be inserted) + +These are what `gym.make()` actually loads. To regenerate them from raw geometry-only USDs (e.g. if you have new mesh authoring), use the included one-shot script: + +```bash +# in-place re-preparation of the bundled assets (default when no src dir given) +./isaaclab.sh -p /path/to/rpl_openarm_centrifuge/scripts/prepare_assets.py + +# or, prepare from a separate source directory +./isaaclab.sh -p /path/to/rpl_openarm_centrifuge/scripts/prepare_assets.py /path/to/raw/usds +CENTRIFUGE_SRC_DIR=/path/to/raw/usds \ + ./isaaclab.sh -p /path/to/rpl_openarm_centrifuge/scripts/prepare_assets.py +``` + +The script knows per-asset whether to prep it as `dynamic` (tube, bucket — adds `RigidBodyAPI` + `MassAPI` + `MeshCollisionAPI(convexHull)`) or `static` (rack — adds `MeshCollisionAPI(triangleMesh)` only). It's idempotent: it never overwrites existing schemas, so re-running is safe. + +## Scene layout + +| Asset | Position (env frame) | Notes | +|---|---|---| +| Ground plane | z = 0 | | +| Robot pedestal | (-0.62, 0, 0.289), 0.413 × 0.413 × 0.579 m cube | Top at z = 0.578 | +| Robot base (OpenArm bimanual) | (-0.62, 0, 0.5786) | Mounted on pedestal top | +| Table | (0.1, 0, 0.40), 1.0 × 0.6 × 0.80 m cube | Top at z = 0.80; non-overlapping with pedestal | +| Tube rack | (-0.30, -0.20, 0.80) | Static fixture on the robot's right (-y) side; holds the tube | +| Tube | (-0.30, -0.20, 0.90) | Starts above the rack, settles into a well; randomized ±5 cm xy + ±0.3 rad yaw on reset | +| Bucket | (-0.20, 0.00, 0.85) | On the centre-line, reachable by either hand; receptacle for the task | +| Chest camera | offset (0.08, 0, 0.55) from `openarm_body_link` | Intel RealSense D435-style RGB + depth, 640×480 @ 30 Hz, ~69° H-FOV. Not in obs by default — see notes below. | + +The OpenArm USD ships without a camera prim (the included `openarm_bimanual_sensor.usd` layer is an empty stub), so this package attaches a `CameraCfg` to `openarm_body_link` in the scene cfg. The camera rotates with the chest if the body link moves. + +**Pixel data is not in the policy observation by default** — adding it changes the observation space and would break any downstream IL configs trained against the current obs shape. To opt in, add to `ObservationsCfg.PolicyCfg`: + +```python +chest_rgb = ObsTerm(func=mdp.image, + params={"sensor_cfg": SceneEntityCfg("chest_camera"), + "data_type": "rgb"}) +``` + +When using the camera in **headless runs without livestream**, the rendering pipeline must be on — pass `--enable_cameras` on the launch command (livestream and GUI modes have rendering on already). + +## Success criterion + +`tube_inside_bucket` — true per-env iff: +* tube xy is within 4 cm of bucket xy +* tube z is between (bucket_z − 5 cm) and (bucket_z + 20 cm) +* tube linear speed < 5 cm/s + +The `record_demos.py` tooling picks this up by name and exports an episode once it holds for `--num_success_steps` consecutive frames. + +## Package layout + +``` +rpl_openarm_centrifuge/ +├── pyproject.toml +├── src/ +│ ├── rpl_openarm_centrifuge_autoregister.pth ← .pth auto-import shim +│ └── rpl_openarm_centrifuge/ +│ ├── __init__.py ← gym.register +│ ├── centrifuge_env_cfg.py ← base scene + MDP +│ ├── centrifuge_ik_rel_env_cfg.py ← Relative-IK + keyboard + handtracking +│ ├── centrifuge_ik_abs_env_cfg.py ← Absolute-IK + handtracking +│ ├── mdp/ ← observations / events / terminations +│ ├── devices/ ← BimanualKeyboardRightArm wrapper +│ ├── agents/ ← (empty, for robomimic JSONs later) +│ └── assets/ ← bundled USDs +└── scripts/ + └── prepare_assets.py ← raw → rigid-prepared USDs +``` diff --git a/tasks/pyproject.toml b/tasks/pyproject.toml new file mode 100644 index 0000000..d97157d --- /dev/null +++ b/tasks/pyproject.toml @@ -0,0 +1,56 @@ +[build-system] +requires = ["setuptools>=64", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "rpl-openarm-centrifuge" +version = "0.1.0" +description = "Isaac Lab bimanual OpenArm centrifuge pick-and-place task (tube into bucket)." +readme = "README.md" +requires-python = ">=3.10" +license = { text = "BSD-3-Clause" } +authors = [{ name = "RPL" }] +keywords = ["isaaclab", "openarm", "manipulation", "bimanual", "centrifuge", "imitation-learning"] +classifiers = [ + "License :: OSI Approved :: BSD License", + "Programming Language :: Python :: 3", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", +] + +# Runtime deps. ``isaaclab`` brings ``isaaclab_assets`` (which provides +# OPENARM_BI_HIGH_PD_CFG) with it, so we don't list it separately. +dependencies = [ + "gymnasium", + "torch", +] + +[project.optional-dependencies] +dev = ["pytest"] + +[project.scripts] +# One-shot helper: run once after ``pip install`` to drop an auto-import .pth +# file into site-packages so the gym IDs register without any script edits. +rpl-openarm-centrifuge-install-autoreg = "rpl_openarm_centrifuge._install_autoreg:main" + +[project.urls] +Homepage = "https://github.com/AD-SDL/openarm_module" + +## +## setuptools configuration +## + +[tool.setuptools] +include-package-data = true +# Note: the auto-import .pth file (src/rpl_openarm_centrifuge_autoregister.pth) is +# installed into site-packages by ``setup.py`` rather than configured here — +# pyproject's ``data-files`` form can't reliably target the purelib path across +# Python versions, so we use a small setuptools install hook. + +[tool.setuptools.packages.find] +where = ["src"] +include = ["rpl_openarm_centrifuge*"] + +[tool.setuptools.package-data] +# Bundle USDs and the auto-register .pth shim alongside the Python modules. +"rpl_openarm_centrifuge" = ["assets/*.usd", "assets/*.usdc", "assets/*.usda", "*.pth"] diff --git a/tasks/scripts/prepare_assets.py b/tasks/scripts/prepare_assets.py new file mode 100644 index 0000000..22be32b --- /dev/null +++ b/tasks/scripts/prepare_assets.py @@ -0,0 +1,219 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Apply physics schemas to the centrifuge tube/bucket/rack USDs. + +The raw geometry-only USDs need rigid-body / collision schemas applied before +Isaac Lab will spawn them with physics (``UsdFileCfg.rigid_props`` only +*modifies* an existing API — it does not *apply* one). + +Two preparation modes: + * ``dynamic``: applies ``PhysicsRigidBodyAPI`` + ``MassAPI`` + + ``PhysxRigidBodyAPI`` on the root, plus ``CollisionAPI`` + + ``MeshCollisionAPI("convexHull")`` on every Mesh under it. Use for + free-moving objects like the tube and bucket. + * ``static``: applies ``CollisionAPI`` + ``MeshCollisionAPI("triangleMesh")`` + only — no rigid body. Use for fixtures like the tube rack that should + stay in place but need concave collision (wells, holes, etc.) preserved. + +If no source dir is given, the script prepares the package's bundled assets +in place (assets/ -> assets/). Useful for re-preparing after dropping a new +raw USD into the package. + +Examples: + ./isaaclab.sh -p scripts/prepare_assets.py # in-place + ./isaaclab.sh -p scripts/prepare_assets.py /my/dataset/objects + CENTRIFUGE_SRC_DIR=/my/dataset/objects ./isaaclab.sh -p scripts/prepare_assets.py + ./isaaclab.sh -p scripts/prepare_assets.py /my/raw --dst-dir /tmp/prepared +""" + +from __future__ import annotations + +import argparse +import os +import shutil +import sys +from dataclasses import dataclass +from importlib.resources import files as _pkg_files + +from isaaclab.app import AppLauncher + + +@dataclass(frozen=True) +class _AssetSpec: + filename: str + mode: str # "dynamic" or "static" + + +# Authoritative list of bundled assets and how each should be prepared. +ASSETS: tuple[_AssetSpec, ...] = ( + _AssetSpec("centrifuge_tube_big.usd", "dynamic"), + # Bucket has wells, so convexHull would fill them and block the tube. SDF + # is the only approximation valid for dynamic bodies that preserves + # concave geometry, so we use the dedicated dynamic_sdf mode. + _AssetSpec("centrifuge_bucket_big.usd", "dynamic_sdf"), + # Rack is a static fixture; triangleMesh preserves the concave wells so + # tubes can actually be inserted (a convex hull would fill them in). + _AssetSpec("centrifuge_tube_rack.usd", "static"), +) + + +def _bundled_assets_dir() -> str: + return str(_pkg_files("rpl_openarm_centrifuge") / "assets") + + +def _parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=__doc__.splitlines()[0], + formatter_class=argparse.RawDescriptionHelpFormatter, + ) + parser.add_argument( + "src_dir", + nargs="?", + default=os.environ.get("CENTRIFUGE_SRC_DIR"), + help=( + "Directory holding the raw geometry-only USDs. If omitted (and " + "CENTRIFUGE_SRC_DIR is not set), the script falls back to the " + "package's bundled assets/ dir for *in-place* re-preparation." + ), + ) + parser.add_argument( + "--dst-dir", + default=os.environ.get("CENTRIFUGE_DST_DIR"), + help=( + "Destination dir for prepared USDs. Defaults to the package's " + "bundled assets/ dir (resolved via importlib.resources). May also " + "be set via CENTRIFUGE_DST_DIR." + ), + ) + AppLauncher.add_app_launcher_args(parser) + return parser.parse_args() + + +def _ensure_dynamic_root_apis(stage_root, usd_physics, physx_schema) -> None: + if not stage_root.HasAPI(usd_physics.RigidBodyAPI): + usd_physics.RigidBodyAPI.Apply(stage_root) + print(f" + RigidBodyAPI on {stage_root.GetPath()}") + if not stage_root.HasAPI(usd_physics.MassAPI): + usd_physics.MassAPI.Apply(stage_root) + print(f" + MassAPI on {stage_root.GetPath()}") + if not stage_root.HasAPI(physx_schema.PhysxRigidBodyAPI): + physx_schema.PhysxRigidBodyAPI.Apply(stage_root) + print(f" + PhysxRigidBodyAPI on {stage_root.GetPath()}") + + +def _set_mesh_collision(stage_root, usd_physics, approximation: str) -> int: + """Apply CollisionAPI + MeshCollisionAPI to every Mesh under ``stage_root`` + and force the approximation to ``approximation``. + + Idempotent — always overwrites the approximation attribute so re-running + the prep script after a change to the spec actually takes effect. + """ + from pxr import Usd # noqa: PLC0415 + + n = 0 + for prim in Usd.PrimRange(stage_root): + if prim.GetTypeName() != "Mesh": + continue + if not prim.HasAPI(usd_physics.CollisionAPI): + usd_physics.CollisionAPI.Apply(prim) + mca = ( + usd_physics.MeshCollisionAPI(prim) + if prim.HasAPI(usd_physics.MeshCollisionAPI) + else usd_physics.MeshCollisionAPI.Apply(prim) + ) + attr = mca.GetApproximationAttr() or mca.CreateApproximationAttr() + attr.Set(approximation) + n += 1 + return n + + +def _apply_dynamic(stage_root, usd_physics, physx_schema) -> None: + _ensure_dynamic_root_apis(stage_root, usd_physics, physx_schema) + n = _set_mesh_collision(stage_root, usd_physics, "convexHull") + print(f" + CollisionAPI(convexHull) on {n} mesh(es)") + + +def _apply_dynamic_sdf(stage_root, usd_physics, physx_schema) -> None: + """Same as dynamic, but uses SDF (signed distance field) collision so the + mesh's concave features (e.g. bucket wells) are preserved. SDF is the only + PhysX approximation valid for dynamic bodies that supports concavity. + """ + from pxr import Usd # noqa: PLC0415 + + _ensure_dynamic_root_apis(stage_root, usd_physics, physx_schema) + n = _set_mesh_collision(stage_root, usd_physics, "sdf") + for prim in Usd.PrimRange(stage_root): + if prim.GetTypeName() == "Mesh": + physx_schema.PhysxSDFMeshCollisionAPI.Apply(prim) + print(f" + CollisionAPI(sdf) on {n} mesh(es)") + + +def _apply_static(stage_root, usd_physics, _physx_schema) -> None: + n = _set_mesh_collision(stage_root, usd_physics, "triangleMesh") + print(f" + CollisionAPI(triangleMesh) on {n} mesh(es)") + + +def _prepare_one(spec: _AssetSpec, src_dir: str, dst_dir: str, usd_physics, physx_schema) -> None: + src = os.path.join(src_dir, spec.filename) + dst = os.path.join(dst_dir, spec.filename) + print(f"[{spec.mode:>7}] {src} -> {dst}") + # If src == dst we're operating in-place; avoid the no-op self-copy that + # would race on some filesystems. + if os.path.abspath(src) != os.path.abspath(dst): + shutil.copyfile(src, dst) + + from pxr import Usd # noqa: PLC0415 + + stage = Usd.Stage.Open(dst) + if stage is None: + raise RuntimeError(f"Could not open stage at {dst}") + root = stage.GetDefaultPrim() + if not root or not root.IsValid(): + raise RuntimeError(f"{dst} has no defaultPrim") + + if spec.mode == "dynamic": + _apply_dynamic(root, usd_physics, physx_schema) + elif spec.mode == "dynamic_sdf": + _apply_dynamic_sdf(root, usd_physics, physx_schema) + elif spec.mode == "static": + _apply_static(root, usd_physics, physx_schema) + else: + raise ValueError(f"unknown mode {spec.mode!r} for {spec.filename}") + + stage.GetRootLayer().Save() + print(" saved.") + + +def main() -> None: + args = _parse_args() + + bundled = _bundled_assets_dir() + src_dir = args.src_dir or bundled + dst_dir = args.dst_dir or bundled + + if not os.path.isdir(src_dir): + sys.exit(f"error: source directory does not exist or is not a directory: {src_dir}") + + missing = [s.filename for s in ASSETS if not os.path.isfile(os.path.join(src_dir, s.filename))] + if missing: + sys.exit(f"error: source dir {src_dir} is missing required USD(s): {missing}") + + # Launch SimulationApp only after args + files are validated so --help is fast. + app_launcher = AppLauncher(args) + simulation_app = app_launcher.app + try: + from pxr import PhysxSchema, UsdPhysics # noqa: PLC0415 + + os.makedirs(dst_dir, exist_ok=True) + for spec in ASSETS: + _prepare_one(spec, src_dir, dst_dir, UsdPhysics, PhysxSchema) + print(f"done. prepared {len(ASSETS)} USD(s) into {dst_dir}") + finally: + simulation_app.close() + + +if __name__ == "__main__": + main() diff --git a/tasks/src/rpl_openarm_centrifuge/__init__.py b/tasks/src/rpl_openarm_centrifuge/__init__.py new file mode 100644 index 0000000..e6b997e --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/__init__.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Bimanual OpenArm centrifuge pick-and-place task for Isaac Lab. + +A plastic centrifuge tube starts on the table; the goal is to drop it inside +the centrifuge bucket. Designed for human teleoperation + imitation learning +via Isaac Lab's ``scripts/tools/record_demos.py``. + +This package self-registers its gym IDs on import: + * ``Isaac-Centrifuge-Bimanual-OpenArm-IK-Rel-v0`` — IK-Rel + bimanual + hand-tracking; keyboard wrapped to drive the right arm only. + * ``Isaac-Centrifuge-Bimanual-OpenArm-IK-Abs-v0`` — IK-Abs + bimanual + hand-tracking. Recommended for hand-tracking teleop. + +A companion ``.pth`` file shipped with the wheel runs ``import +rpl_openarm_centrifuge`` at every Python startup, so the IDs are visible to +``gym.make()`` without any user code changes. As a fallback, explicitly +``import rpl_openarm_centrifuge # noqa: F401`` before ``gym.make``. +""" + +import gymnasium as gym + +from . import agents # noqa: F401 + +gym.register( + id="Isaac-Centrifuge-Bimanual-OpenArm-IK-Rel-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.centrifuge_ik_rel_env_cfg:CentrifugeBimanualIkRelEnvCfg", + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-Centrifuge-Bimanual-OpenArm-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.centrifuge_ik_abs_env_cfg:CentrifugeBimanualIkAbsEnvCfg", + }, + disable_env_checker=True, +) diff --git a/tasks/src/rpl_openarm_centrifuge/_install_autoreg.py b/tasks/src/rpl_openarm_centrifuge/_install_autoreg.py new file mode 100644 index 0000000..7c315ba --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/_install_autoreg.py @@ -0,0 +1,39 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Installs the auto-import ``.pth`` shim into the active venv's site-packages. + +Setuptools' PEP 660 editable installer doesn't honor ``data_files`` for ``.pth`` +files, so we install the shim with a one-shot console script instead. Run via: + + rpl-openarm-centrifuge-install-autoreg + +After this runs, every Python process inside the Isaac Lab venv will execute +``import rpl_openarm_centrifuge`` at startup, which triggers ``gym.register`` +without any user script edits. +""" + +from __future__ import annotations + +import shutil +import sysconfig +from importlib.resources import files as _pkg_files +from pathlib import Path + +_PTH_BASENAME = "rpl_openarm_centrifuge_autoregister.pth" + + +def main() -> None: + src = _pkg_files("rpl_openarm_centrifuge").joinpath(_PTH_BASENAME) + purelib = Path(sysconfig.get_paths()["purelib"]) + if not purelib.exists(): + raise RuntimeError(f"purelib does not exist: {purelib}") + dst = purelib / _PTH_BASENAME + shutil.copyfile(str(src), str(dst)) + print(f"[rpl_openarm_centrifuge] auto-register hook installed: {dst}") + + +if __name__ == "__main__": + main() diff --git a/tasks/src/rpl_openarm_centrifuge/agents/__init__.py b/tasks/src/rpl_openarm_centrifuge/agents/__init__.py new file mode 100644 index 0000000..71c5e43 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/agents/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Agent configurations (robomimic, rsl_rl, ...) for the centrifuge task. + +Empty for now — add JSON/Python cfgs here and reference them from +``centrifuge/__init__.py``'s ``gym.register(..., kwargs={"_cfg_entry_point": ...})``. +""" diff --git a/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_bucket_big.usd b/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_bucket_big.usd new file mode 100644 index 0000000..d0186f5 Binary files /dev/null and b/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_bucket_big.usd differ diff --git a/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_tube_big.usd b/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_tube_big.usd new file mode 100644 index 0000000..e3429ba Binary files /dev/null and b/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_tube_big.usd differ diff --git a/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_tube_rack.usd b/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_tube_rack.usd new file mode 100644 index 0000000..fc99229 Binary files /dev/null and b/tasks/src/rpl_openarm_centrifuge/assets/centrifuge_tube_rack.usd differ diff --git a/tasks/src/rpl_openarm_centrifuge/centrifuge_env_cfg.py b/tasks/src/rpl_openarm_centrifuge/centrifuge_env_cfg.py new file mode 100644 index 0000000..4305567 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/centrifuge_env_cfg.py @@ -0,0 +1,407 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base configuration for the bimanual centrifuge pick-and-place task. + +The task: a bimanual OpenArm robot picks a plastic centrifuge tube from the +table and places it into a centrifuge bucket. Both arms are teleoperated; the +task does not prescribe which hand does which step. + +This base cfg leaves the robot articulation and action terms ``MISSING`` so +that concrete variants (e.g. IK-rel) can plug in robot-specific bindings. +""" + +from dataclasses import MISSING +from importlib.resources import files as _pkg_files + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg as ActionTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg, FrameTransformerCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.sim.spawners.meshes.meshes_cfg import MeshCuboidCfg +from isaaclab.utils import configclass + +from . import mdp + +# Rigid-body-prepared USDs bundled inside the package wheel. ``importlib.resources.files`` +# resolves the path correctly for both editable (``pip install -e .``) and built-wheel +# installs. See ``scripts/prepare_assets.py`` for how to regenerate these from raw +# geometry-only originals. +CENTRIFUGE_DATASET_DIR = str(_pkg_files("rpl_openarm_centrifuge") / "assets") + + +## +# Scene +## + + +@configclass +class CentrifugeSceneCfg(InteractiveSceneCfg): + """Scene: ground, light, table, robot pedestal, bimanual robot, tube rack, tube, bucket. + + Geometry tuned so the OpenArm bimanual can comfortably reach the rack and + bucket from a natural shoulders-above-table pose. World/env frame: + * Ground plane at z=0. + * Robot pedestal: 0.413 x 0.413 x 0.579 m cube centred at (-0.62, 0, 0.289) + so its top is at z=0.578 (preserved from ``aiet_scene.usd``). + * Table: top at z=0.80, painted yellow for contrast against the props. + * Robot base mounted on the pedestal top at (-0.62, 0, 0.5786). + * Tube rack: static fixture on the table at (-0.35, -0.15, 0.80) on the + robot's right (-y) side, rotated 90 deg CCW about +z so its long axis + runs along world y. The row of 4 large wells faces robot-centre (+y); + the row of 6 small wells faces -y. Painted black. + * Rack floor: invisible-by-design collision plate inside the rack + (the rack USD's wells are open through-bores), so the tube tip rests + inside the rack rather than dropping onto the table top. + * Tube: light-gray rigid body, starts inserted into the back-right large + well at (-0.332, -0.1425, 0.825). The tube body Ø is within ~0.1 mm of + the well bore Ø. Two cooperating fixes prevent friction lock: + 1. The rack uses triangle-mesh collision and the bucket uses SDF + collision (instead of convexDecomposition) so the well bores are + faithfully empty in collision space -- convex decomp hulls would + otherwise intrude into the wells and block the tube body + regardless of clearance. + 2. The tube's PhysX collision surface is shrunk by 1 mm + (``rest_offset = -0.001`` in ``collision_props``), giving ~1 mm + radial clearance through both rack and bucket wells. The wider + cap still collides with the rim as a real centrifuge tube does. + * Bucket: green rigid body at (-0.35, 0, 0.80) -- same depth as the rack, + on the robot's centre line. Forms an in-line layout with the rack along + world y so right-hand pick-from-rack + place-into-bucket is a short + translation. + * Chest camera: Intel RealSense D435-like sensor attached to + ``openarm_body_link`` so it travels with the robot torso. Pitched 45 + deg down to frame the rack + bucket. Not wired into the policy obs by + default -- see the ``chest_camera`` field's docstring to opt in. + + The asymmetric layout (rack on -y, bucket on centre) reflects the + "right hand picks first, then transports to centre" intended operation + order. Mirror to +y if you want left-hand-first operation. + + Rack / tube / bucket positions are starting values intended for in-sim + tuning. The success criterion (tube inside the bucket) is invariant to + these positions, so retuning doesn't change the task definition. + """ + + # robot: filled in by the concrete variant + robot: ArticulationCfg = MISSING + # end-effector frames: filled in by the concrete variant (need to know body names) + left_ee_frame: FrameTransformerCfg = MISSING + right_ee_frame: FrameTransformerCfg = MISSING + + # ground + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 0.0)), + spawn=GroundPlaneCfg(), + ) + + # dome light + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + # robot pedestal — static cuboid; top at z = 0.289 + 0.579/2 = 0.5785. + pedestal = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Pedestal", + init_state=AssetBaseCfg.InitialStateCfg(pos=(-0.62, 0.0, 0.289), rot=(1.0, 0.0, 0.0, 0.0)), + spawn=MeshCuboidCfg( + size=(0.413, 0.413, 0.579), + collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.3, 0.3, 0.3)), + ), + ) + + # table — static cuboid; top at z = 0.0 + 0.80 = 0.80. + # Footprint chosen so the -x edge (x=-0.4) doesn't overlap the pedestal + # (whose +x edge is at x=-0.413), leaving a small visual gap. + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Table", + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.1, 0.0, 0.40), rot=(1.0, 0.0, 0.0, 0.0)), + spawn=MeshCuboidCfg( + size=(1.0, 0.6, 0.80), + collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.95, 0.85, 0.2)), + ), + ) + + # tube rack — static fixture sitting on the table. Collision approximation + # is set to ``triangleMesh`` inside the rack USD itself (see + # scripts/prepare_assets.py): a convex-hull or convex-decomp approximation + # cannot represent the well bores faithfully, since convex hulls can't + # have holes and the decomp hulls intrude into the wells, blocking the + # tube body regardless of any rest-offset clearance. Triangle mesh is + # only allowed on static bodies, which is fine here. Placed on the + # robot's -y (right) side so the right hand operates first. Rotated 90 + # deg CCW about +z so the long axis points along world y; the row of 4 + # large wells faces robot-centre (+y) and the row of 6 small wells faces + # away (-y). Painted black so the well openings read clearly. + rack = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/Rack", + init_state=AssetBaseCfg.InitialStateCfg( + pos=(-0.35, -0.15, 0.80), + rot=(0.7071068, 0.0, 0.0, 0.7071068), + ), + spawn=UsdFileCfg( + usd_path=f"{CENTRIFUGE_DATASET_DIR}/centrifuge_tube_rack.usd", + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.05, 0.05, 0.05)), + ), + ) + + # rack floor — invisible collision plate inside the rack. The rack USD's + # wells are open through-bores (no authored floor), so without this the + # tube tip would fall to the table top through the well. The plate is + # axis-aligned in world frame after the rack rotation: 82 mm (x) x 125 mm + # (y) x 5 mm (z), spanning the rack footprint. Painted to match the rack so + # it's hidden visually but provides the floor PhysX needs. + rack_floor = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/RackFloor", + init_state=AssetBaseCfg.InitialStateCfg(pos=(-0.35, -0.15, 0.8025), rot=(1.0, 0.0, 0.0, 0.0)), + spawn=MeshCuboidCfg( + size=(0.082, 0.125, 0.005), + collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.05, 0.05, 0.05)), + ), + ) + + # plastic centrifuge tube (rigid) — inserted into one of the four large + # wells of the rotated rack. Tube body Ø ~28.7 mm fits the well Ø ~29 mm + # (~0.05 mm raw clearance per side), which without intervention would + # friction-lock and make picking/placing impossible. We shrink the tube's + # PhysX collision surface by 1 mm via ``rest_offset = -0.001`` so the + # effective body Ø is ~26.7 mm — that gives ~1 mm radial clearance through + # the well bore for both the rack and bucket wells, but the cap (Ø ~35 mm) + # is still wider than the bore so it catches on the rim as intended. + # Tip rests on the rack_floor plate (top face z=0.805) at init; cap sits + # ~33 mm above the rack rim. xy = (-0.332, -0.1425) is the back-right + # large well in the rotated layout. + tube = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Tube", + init_state=RigidObjectCfg.InitialStateCfg( + pos=(-0.332, -0.1425, 0.825), + rot=(0.7071068, 0.0, 0.0, 0.7071068), + ), + spawn=UsdFileCfg( + usd_path=f"{CENTRIFUGE_DATASET_DIR}/centrifuge_tube_big.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=0.05), + collision_props=sim_utils.CollisionPropertiesCfg( + contact_offset=0.005, + rest_offset=-0.001, + ), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.82, 0.82, 0.82)), + ), + ) + + # OpenArm chest camera — Intel RealSense D435-like sensor mounted on the + # robot's chest, looking forward at the work area. The OpenArm USD ships + # without a camera prim authored (the ``openarm_bimanual_sensor.usd`` + # layer is a stub), so we attach one here as a child of openarm_body_link. + # If the body link ever rotates, the camera moves with it. + # + # Pose is approximate — front face of upper chest, looking forward (+x in + # robot body frame). Tune in-sim after seeing the camera frustum. + # + # Notes: + # * The camera's pixel output is NOT wired into the policy observation + # by default (would change obs shape and break downstream IL configs). + # To use it: add an ObsTerm like + # chest_rgb = ObsTerm(func=mdp.image, + # params={"sensor_cfg": SceneEntityCfg("chest_camera"), + # "data_type": "rgb"}) + # * Cameras render only when the render pipeline is active. For headless + # runs without livestream, pass ``--enable_cameras``. + # * Intrinsics target ~69 deg horizontal FOV (RealSense D435 color stream + # in 4:3 mode). Tune ``focal_length`` to change FOV. + chest_camera = CameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_body_link/chest_camera", + update_period=0.0333, # ~30 Hz, matches RealSense default + height=480, + width=640, + data_types=["rgb", "distance_to_image_plane"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=15.25, + focus_distance=400.0, + horizontal_aperture=20.955, + clipping_range=(0.05, 5.0), + ), + offset=CameraCfg.OffsetCfg( + pos=(0.08, 0.0, 0.55), # 8 cm forward of body axis, mid-chest height + # Pitched 45 deg downward (rotation about +y by +45 deg) so the + # frustum covers the centrifuge bucket (~0.34 m forward, ~0.28 m + # below the camera) and the tube on the rack. Quaternion is + # (cos 22.5, 0, sin 22.5, 0). + rot=(0.9238795, 0.0, 0.3826834, 0.0), + convention="world", # forward = +x (robot body frame), up = +z + ), + ) + + # centrifuge bucket (rigid — heavy enough to act as a stable receptacle). + # Sits on the table at the same x as the rack (-0.35), centred on the + # robot's y axis. The bucket extends the rack's rotated long axis toward + # the robot's centre, so right-hand pick-and-place from rack to bucket is + # a short y-translation. Bucket mesh origin is at its bottom -> z=0.80 + # places it flush on the table top. + bucket = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Bucket", + init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.35, 0.0, 0.80), rot=(1.0, 0.0, 0.0, 0.0)), + spawn=UsdFileCfg( + usd_path=f"{CENTRIFUGE_DATASET_DIR}/centrifuge_bucket_big.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=16, + solver_velocity_iteration_count=1, + max_angular_velocity=1000.0, + max_linear_velocity=1000.0, + max_depenetration_velocity=5.0, + disable_gravity=False, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=0.5), + # Collision approximation is set to ``sdf`` inside the bucket USD + # itself (see scripts/prepare_assets.py): convexHull or convex + # decomposition cannot represent the wells, and SDF is the only + # approximation valid for dynamic bodies that preserves concave + # geometry. Without this the wells would be filled and the tube + # could not be inserted. + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.7, 0.25)), + ), + ) + + +## +# MDP +## + + +@configclass +class ActionsCfg: + """Bimanual action terms — left/right arm IK + left/right gripper binary.""" + + left_arm_action: ActionTerm = MISSING + left_gripper_action: ActionTerm = MISSING + right_arm_action: ActionTerm = MISSING + right_gripper_action: ActionTerm = MISSING + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """State-only observation group used for record_demos + robomimic IL.""" + + # robot proprioception + joint_pos = ObsTerm(func=mdp.joint_pos_rel) + joint_vel = ObsTerm(func=mdp.joint_vel_rel) + actions = ObsTerm(func=mdp.last_action) + + # left end-effector pose + left_eef_pos = ObsTerm( + func=mdp.ee_frame_position_in_env_frame, + params={"ee_frame_cfg": SceneEntityCfg("left_ee_frame")}, + ) + left_eef_quat = ObsTerm( + func=mdp.ee_frame_orientation, + params={"ee_frame_cfg": SceneEntityCfg("left_ee_frame")}, + ) + + # right end-effector pose + right_eef_pos = ObsTerm( + func=mdp.ee_frame_position_in_env_frame, + params={"ee_frame_cfg": SceneEntityCfg("right_ee_frame")}, + ) + right_eef_quat = ObsTerm( + func=mdp.ee_frame_orientation, + params={"ee_frame_cfg": SceneEntityCfg("right_ee_frame")}, + ) + + # object poses + tube_pos = ObsTerm( + func=mdp.object_position_in_env_frame, params={"asset_cfg": SceneEntityCfg("tube")} + ) + tube_quat = ObsTerm(func=mdp.object_orientation, params={"asset_cfg": SceneEntityCfg("tube")}) + bucket_pos = ObsTerm( + func=mdp.object_position_in_env_frame, params={"asset_cfg": SceneEntityCfg("bucket")} + ) + bucket_quat = ObsTerm( + func=mdp.object_orientation, params={"asset_cfg": SceneEntityCfg("bucket")} + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + # tube fell off the table — table top is at z=0.80, so anything well below + # that counts as a drop. + tube_dropped = DoneTerm( + func=mdp.root_height_below_minimum, + params={"minimum_height": 0.3, "asset_cfg": SceneEntityCfg("tube")}, + ) + + # success — record_demos.py picks this term up by name + success = DoneTerm(func=mdp.tube_inside_bucket) + + +## +# Env +## + + +@configclass +class CentrifugeEnvCfg(ManagerBasedRLEnvCfg): + """Base env config for bimanual centrifuge pick-and-place.""" + + # Scene + scene: CentrifugeSceneCfg = CentrifugeSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=False) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP + terminations: TerminationsCfg = TerminationsCfg() + + # unused managers + commands = None + rewards = None + curriculum = None + events = None + + def __post_init__(self): + # general settings + self.decimation = 5 + self.episode_length_s = 30.0 + # simulation settings (100 Hz physics, rendered every other step) + self.sim.dt = 0.01 + self.sim.render_interval = 2 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.friction_correlation_distance = 0.00625 + self.sim.physx.gpu_found_lost_aggregate_pairs_capacity = 1024 * 1024 * 4 + self.sim.physx.gpu_total_aggregate_pairs_capacity = 16 * 1024 + # viewer — look at the work area from in front of and above the robot + self.viewer.eye = (1.2, 1.0, 1.6) + self.viewer.lookat = (-0.25, 0.0, 0.85) diff --git a/tasks/src/rpl_openarm_centrifuge/centrifuge_ik_abs_env_cfg.py b/tasks/src/rpl_openarm_centrifuge/centrifuge_ik_abs_env_cfg.py new file mode 100644 index 0000000..9a64640 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/centrifuge_ik_abs_env_cfg.py @@ -0,0 +1,154 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Bimanual OpenArm + absolute differential-IK teleop variant. + +This is the recommended variant for hand-tracking: the user's wrist pose in the +XR-anchor frame maps directly to the robot end-effector's absolute target pose. +No delta integration, no drift. + +Wires: + * Robot: ``OPENARM_BI_HIGH_PD_CFG`` (stiffer PD for IK tracking). + * Two absolute-IK action terms (one per arm) on ``openarm_{left,right}_hand``. + * Two binary gripper action terms on ``openarm_{left,right}_finger_joint.*``. + * Bimanual hand-tracking teleop via OpenXR (one Se3Abs + Gripper retargeter per hand). + +The XR anchor (``self.xr``) defines where the user is *physically* standing in +the env frame; tune it so the user's natural arm-extension pose lines up with +the robot's reachable workspace above the table. +""" + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg +from isaaclab.devices.openxr.retargeters.manipulator.se3_abs_retargeter import Se3AbsRetargeterCfg +from isaaclab.envs.mdp.actions.actions_cfg import ( + BinaryJointPositionActionCfg, + DifferentialInverseKinematicsActionCfg, +) +from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.utils import configclass + +from isaaclab_assets.robots.openarm import OPENARM_BI_HIGH_PD_CFG + +from .centrifuge_env_cfg import CentrifugeEnvCfg + + +@configclass +class CentrifugeBimanualIkAbsEnvCfg(CentrifugeEnvCfg): + """Bimanual OpenArm with absolute differential-IK actions for hand-tracking teleop.""" + + xr: XrCfg = XrCfg( + # User stands ~1.4 m in front of the robot at chest height, facing it. + # Tune this once you put a headset on — small XR-anchor shifts make a + # large difference in absolute-mode comfort. + anchor_pos=(1.4, 0.0, 1.1), + anchor_rot=(0.0, 0.0, 0.0, 1.0), + ) + + def __post_init__(self): + super().__post_init__() + + # robot — mount the bimanual OpenArm on top of the pedestal (matches aiet_scene.usd) + self.scene.robot = OPENARM_BI_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.init_state.pos = (-0.62, 0.0, 0.5786) + + # end-effector frame transformers (relative to robot body root) + self.scene.left_ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_body_link", + debug_vis=False, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_left_hand", + name="left_end_effector", + offset=OffsetCfg(pos=(0.0, 0.0, 0.0)), + ), + ], + ) + self.scene.right_ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_body_link", + debug_vis=False, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_right_hand", + name="right_end_effector", + offset=OffsetCfg(pos=(0.0, 0.0, 0.0)), + ), + ], + ) + + # IK action terms — one per arm. Absolute mode: action = 6-D target pose + # in the robot base frame (no per-step scaling). + self.actions.left_arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["openarm_left_joint[1-7]"], + body_name="openarm_left_hand", + controller=DifferentialIKControllerCfg( + command_type="pose", use_relative_mode=False, ik_method="dls" + ), + ) + self.actions.right_arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["openarm_right_joint[1-7]"], + body_name="openarm_right_hand", + controller=DifferentialIKControllerCfg( + command_type="pose", use_relative_mode=False, ik_method="dls" + ), + ) + + # Binary gripper action terms — open/close per arm. + self.actions.left_gripper_action = BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["openarm_left_finger_joint.*"], + open_command_expr={"openarm_left_finger_joint.*": 0.044}, + close_command_expr={"openarm_left_finger_joint.*": 0.0}, + ) + self.actions.right_gripper_action = BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["openarm_right_finger_joint.*"], + open_command_expr={"openarm_right_finger_joint.*": 0.044}, + close_command_expr={"openarm_right_finger_joint.*": 0.0}, + ) + + # Bimanual hand-tracking teleop. Each hand owns one Se3Abs retargeter and + # one gripper retargeter. The OpenXR device concatenates their outputs + # into a 14-dim action vector that matches the env action manager's + # term order: + # [left_arm(6), left_gripper(1), right_arm(6), right_gripper(1)] + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3AbsRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, + sim_device=self.sim.device, + ), + Se3AbsRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, + sim_device=self.sim.device, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/tasks/src/rpl_openarm_centrifuge/centrifuge_ik_rel_env_cfg.py b/tasks/src/rpl_openarm_centrifuge/centrifuge_ik_rel_env_cfg.py new file mode 100644 index 0000000..b076789 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/centrifuge_ik_rel_env_cfg.py @@ -0,0 +1,163 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Bimanual OpenArm + relative differential-IK teleop variant. + +Wires: + * Robot: ``OPENARM_BI_HIGH_PD_CFG`` (stiffer PD for IK tracking). + * Two relative-IK action terms (one per arm) on ``openarm_{left,right}_hand``. + * Two binary gripper action terms on ``openarm_{left,right}_finger_joint.*``. + * Bimanual hand-tracking teleop via OpenXR (one Se3Rel + Gripper retargeter per hand). + * ``keyboard`` device wrapped so the keyboard drives the **right arm only**; + left arm holds its default joint pose. Smoke-test path before hand-tracking. +""" + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg +from isaaclab.devices.openxr import XrCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg +from isaaclab.devices.openxr.retargeters.manipulator.se3_rel_retargeter import Se3RelRetargeterCfg +from isaaclab.envs.mdp.actions.actions_cfg import ( + BinaryJointPositionActionCfg, + DifferentialInverseKinematicsActionCfg, +) +from isaaclab.sensors import FrameTransformerCfg +from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg +from isaaclab.utils import configclass + +from isaaclab_assets.robots.openarm import OPENARM_BI_HIGH_PD_CFG + +from .centrifuge_env_cfg import CentrifugeEnvCfg +from .devices import BimanualKeyboardRightArmCfg + + +@configclass +class CentrifugeBimanualIkRelEnvCfg(CentrifugeEnvCfg): + """Bimanual OpenArm with relative differential-IK actions for hand-tracking teleop.""" + + xr: XrCfg = XrCfg( + # Anchor places the user roughly in front of the table at chest height, facing the robot. + anchor_pos=(1.4, 0.0, 1.1), + anchor_rot=(0.0, 0.0, 0.0, 1.0), + ) + + def __post_init__(self): + super().__post_init__() + + # robot — mount the bimanual OpenArm on top of the pedestal (matches aiet_scene.usd) + self.scene.robot = OPENARM_BI_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.init_state.pos = (-0.62, 0.0, 0.5786) + + # end-effector frame transformers (relative to robot body root) + self.scene.left_ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_body_link", + debug_vis=False, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_left_hand", + name="left_end_effector", + offset=OffsetCfg(pos=(0.0, 0.0, 0.0)), + ), + ], + ) + self.scene.right_ee_frame = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_body_link", + debug_vis=False, + target_frames=[ + FrameTransformerCfg.FrameCfg( + prim_path="{ENV_REGEX_NS}/Robot/openarm_right_hand", + name="right_end_effector", + offset=OffsetCfg(pos=(0.0, 0.0, 0.0)), + ), + ], + ) + + # IK action terms — one per arm. Relative mode: action = 6-D delta pose. + self.actions.left_arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["openarm_left_joint[1-7]"], + body_name="openarm_left_hand", + controller=DifferentialIKControllerCfg( + command_type="pose", use_relative_mode=True, ik_method="dls" + ), + scale=0.5, + ) + self.actions.right_arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="robot", + joint_names=["openarm_right_joint[1-7]"], + body_name="openarm_right_hand", + controller=DifferentialIKControllerCfg( + command_type="pose", use_relative_mode=True, ik_method="dls" + ), + scale=0.5, + ) + + # Binary gripper action terms — open/close per arm. + # OpenArm finger joints are prismatic; "open" pushes fingers outward, "close" pulls in. + # The exact open/close values come from the joint limits — leaving the defaults here + # and tuning during the smoke test. + self.actions.left_gripper_action = BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["openarm_left_finger_joint.*"], + open_command_expr={"openarm_left_finger_joint.*": 0.044}, + close_command_expr={"openarm_left_finger_joint.*": 0.0}, + ) + self.actions.right_gripper_action = BinaryJointPositionActionCfg( + asset_name="robot", + joint_names=["openarm_right_finger_joint.*"], + open_command_expr={"openarm_right_finger_joint.*": 0.044}, + close_command_expr={"openarm_right_finger_joint.*": 0.0}, + ) + + # Teleop devices: + # * ``handtracking`` — bimanual hand-tracking via OpenXR. Each hand owns + # one Se3Rel retargeter and one gripper retargeter. The OpenXR device + # concatenates the four retargeters' outputs into a single 14-dim + # action vector matching the env action manager's term order: + # [left_arm(6), left_gripper(1), right_arm(6), right_gripper(1)] + # * ``keyboard`` — Se3Keyboard wrapped to drive the right arm only; + # left arm holds its default joint pose. Smoke-test path. + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": BimanualKeyboardRightArmCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "handtracking": OpenXRDeviceCfg( + retargeters=[ + Se3RelRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + delta_pos_scale_factor=10.0, + delta_rot_scale_factor=10.0, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, + sim_device=self.sim.device, + ), + Se3RelRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, + zero_out_xy_rotation=True, + use_wrist_rotation=False, + use_wrist_position=True, + delta_pos_scale_factor=10.0, + delta_rot_scale_factor=10.0, + sim_device=self.sim.device, + ), + GripperRetargeterCfg( + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, + sim_device=self.sim.device, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/tasks/src/rpl_openarm_centrifuge/devices/__init__.py b/tasks/src/rpl_openarm_centrifuge/devices/__init__.py new file mode 100644 index 0000000..47f2986 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/devices/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Custom teleop devices for the centrifuge task.""" + +from .bimanual_keyboard import BimanualKeyboardRightArm, BimanualKeyboardRightArmCfg + +__all__ = ["BimanualKeyboardRightArm", "BimanualKeyboardRightArmCfg"] diff --git a/tasks/src/rpl_openarm_centrifuge/devices/bimanual_keyboard.py b/tasks/src/rpl_openarm_centrifuge/devices/bimanual_keyboard.py new file mode 100644 index 0000000..f74c787 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/devices/bimanual_keyboard.py @@ -0,0 +1,79 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Bimanual-action keyboard wrapper. + +``Se3Keyboard.advance()`` returns a 7-dim tensor (6-D delta pose + 1-D gripper) +that drives a single arm. The centrifuge env's action manager publishes a +14-dim action vector with term order: + + [left_arm(6), left_gripper(1), right_arm(6), right_gripper(1)] + +This wrapper pads the keyboard output so it slots into the right-arm half of +that vector while the left arm holds its default joint pose and the left +gripper stays closed. Lets you smoke-test the bimanual env from a keyboard +without authoring a separate unimanual env cfg. +""" + +from __future__ import annotations + +from collections.abc import Callable +from dataclasses import dataclass, field +from typing import Any + +import torch + +from isaaclab.devices.device_base import DeviceBase, DeviceCfg +from isaaclab.devices.keyboard import Se3Keyboard, Se3KeyboardCfg + + +class BimanualKeyboardRightArm(DeviceBase): + """``Se3Keyboard`` mapped onto the right arm of the bimanual centrifuge env. + + Output layout (matches the env's action term order): + idx 0-5 : left arm delta pose (zeros — hold) + idx 6 : left gripper (0.0 — closed; below BinaryJoint threshold 0.5) + idx 7-12 : right arm delta pose (keyboard delta) + idx 13 : right gripper (+1.0 open / -1.0 close, toggled with "K") + """ + + def __init__(self, cfg: BimanualKeyboardRightArmCfg): + super().__init__() + self._sim_device = cfg.sim_device + self._keyboard = Se3Keyboard( + Se3KeyboardCfg( + pos_sensitivity=cfg.pos_sensitivity, + rot_sensitivity=cfg.rot_sensitivity, + sim_device=cfg.sim_device, + ) + ) + + def __str__(self) -> str: + return "BimanualKeyboardRightArm(wrapping Se3Keyboard; keyboard drives right arm only)" + + def reset(self) -> None: + self._keyboard.reset() + + def add_callback(self, key: Any, func: Callable) -> None: + self._keyboard.add_callback(key, func) + + def advance(self) -> torch.Tensor: + kb = self._keyboard.advance() # shape: (7,) + out = torch.zeros(14, dtype=kb.dtype, device=self._sim_device) + # left arm: zero delta (hold pose), left gripper: 0.0 < 0.5 → closed + out[7:13] = kb[:6] + out[13] = kb[6] + return out + + +@dataclass +class BimanualKeyboardRightArmCfg(DeviceCfg): + """Configuration for :class:`BimanualKeyboardRightArm`.""" + + pos_sensitivity: float = 0.05 + rot_sensitivity: float = 0.05 + class_type: type[DeviceBase] = BimanualKeyboardRightArm + # No retargeters — advance() returns the final 14-dim action directly. + retargeters: list = field(default_factory=list) diff --git a/tasks/src/rpl_openarm_centrifuge/mdp/__init__.py b/tasks/src/rpl_openarm_centrifuge/mdp/__init__.py new file mode 100644 index 0000000..f0cced9 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/mdp/__init__.py @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""MDP helpers for the bimanual centrifuge pick-and-place task.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .events import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/tasks/src/rpl_openarm_centrifuge/mdp/events.py b/tasks/src/rpl_openarm_centrifuge/mdp/events.py new file mode 100644 index 0000000..4087798 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/mdp/events.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + +def reset_object_uniform( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + pose_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg, +): + """Reset a rigid object to a uniformly-sampled pose, expressed in the env origin frame. + + ``pose_range`` keys: x, y, z, roll, pitch, yaw. Missing keys default to (0.0, 0.0). + The translation is added to the env origin; the orientation is composed from Euler XYZ. + """ + if env_ids is None or len(env_ids) == 0: + return + + asset: RigidObject = env.scene[asset_cfg.name] + device = env.device + n = len(env_ids) + + def _u(key: str) -> torch.Tensor: + lo, hi = pose_range.get(key, (0.0, 0.0)) + return torch.empty(n, device=device).uniform_(lo, hi) + + dx, dy, dz = _u("x"), _u("y"), _u("z") + droll, dpitch, dyaw = _u("roll"), _u("pitch"), _u("yaw") + + default_state = asset.data.default_root_state[env_ids].clone() + positions = default_state[:, 0:3] + torch.stack([dx, dy, dz], dim=1) + env.scene.env_origins[env_ids] + base_quat = default_state[:, 3:7] + delta_quat = math_utils.quat_from_euler_xyz(droll, dpitch, dyaw) + orientations = math_utils.quat_mul(base_quat, delta_quat) + + asset.write_root_pose_to_sim(torch.cat([positions, orientations], dim=-1), env_ids=env_ids) + asset.write_root_velocity_to_sim(torch.zeros(n, 6, device=device), env_ids=env_ids) diff --git a/tasks/src/rpl_openarm_centrifuge/mdp/observations.py b/tasks/src/rpl_openarm_centrifuge/mdp/observations.py new file mode 100644 index 0000000..2875d17 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/mdp/observations.py @@ -0,0 +1,53 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import FrameTransformer + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def object_position_in_env_frame( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, +) -> torch.Tensor: + """Position of a rigid object in the per-env origin frame.""" + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.root_pos_w - env.scene.env_origins + + +def object_orientation( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, +) -> torch.Tensor: + """Quaternion (w, x, y, z) of a rigid object in world frame.""" + asset: RigidObject = env.scene[asset_cfg.name] + return asset.data.root_quat_w + + +def ee_frame_position_in_env_frame( + env: ManagerBasedRLEnv, + ee_frame_cfg: SceneEntityCfg, +) -> torch.Tensor: + """Position of the first target frame of a FrameTransformer in the env frame.""" + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + return ee_frame.data.target_pos_w[:, 0, :] - env.scene.env_origins + + +def ee_frame_orientation( + env: ManagerBasedRLEnv, + ee_frame_cfg: SceneEntityCfg, +) -> torch.Tensor: + """Quaternion of the first target frame of a FrameTransformer in world frame.""" + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + return ee_frame.data.target_quat_w[:, 0, :] diff --git a/tasks/src/rpl_openarm_centrifuge/mdp/terminations.py b/tasks/src/rpl_openarm_centrifuge/mdp/terminations.py new file mode 100644 index 0000000..fcc8870 --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/mdp/terminations.py @@ -0,0 +1,47 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def tube_inside_bucket( + env: ManagerBasedRLEnv, + tube_cfg: SceneEntityCfg = SceneEntityCfg("tube"), + bucket_cfg: SceneEntityCfg = SceneEntityCfg("bucket"), + xy_radius: float = 0.04, + z_below: float = 0.05, + z_above: float = 0.20, + max_lin_speed: float = 0.05, +) -> torch.Tensor: + """Tube is inside the bucket and roughly at rest. + + True per-env iff: + - tube xy is within ``xy_radius`` of bucket xy, and + - tube z is between ``bucket_z - z_below`` and ``bucket_z + z_above``, and + - tube linear speed is below ``max_lin_speed``. + """ + tube: RigidObject = env.scene[tube_cfg.name] + bucket: RigidObject = env.scene[bucket_cfg.name] + + diff = tube.data.root_pos_w - bucket.data.root_pos_w + xy_dist = torch.linalg.vector_norm(diff[:, :2], dim=1) + z_offset = diff[:, 2] + z_in = (z_offset > -z_below) & (z_offset < z_above) + xy_in = xy_dist < xy_radius + + speed = torch.linalg.vector_norm(tube.data.root_lin_vel_w, dim=1) + at_rest = speed < max_lin_speed + + return xy_in & z_in & at_rest diff --git a/tasks/src/rpl_openarm_centrifuge/rpl_openarm_centrifuge_autoregister.pth b/tasks/src/rpl_openarm_centrifuge/rpl_openarm_centrifuge_autoregister.pth new file mode 100644 index 0000000..83e66be --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge/rpl_openarm_centrifuge_autoregister.pth @@ -0,0 +1 @@ +import rpl_openarm_centrifuge diff --git a/tasks/src/rpl_openarm_centrifuge_autoregister.pth b/tasks/src/rpl_openarm_centrifuge_autoregister.pth new file mode 100644 index 0000000..83e66be --- /dev/null +++ b/tasks/src/rpl_openarm_centrifuge_autoregister.pth @@ -0,0 +1 @@ +import rpl_openarm_centrifuge