react-manip 0.1.0__py3-none-any.whl

react_manip/robots/legacy_agilex_robosuite.py

@@ -0,0 +1,169 @@
+"""Lazy robosuite integration of pinned official AgileX follower models."""
+from pathlib import Path
+import os
+import numpy as np
+
+
+def asset_dir():
+    return Path(os.environ.get("LEGACY_AGILEX_ASSET_DIR", Path(__file__).resolve().parents[2] / "assets/robots/agilex/legacy"))
+
+
+def validate_bundle():
+    import hashlib, json
+    directory = asset_dir()
+    manifest = directory / 'provenance.json'
+    if not manifest.exists():
+        raise FileNotFoundError('Run python scripts/build_agilex_assets.py --family legacy')
+    data = json.loads(manifest.read_text())
+    lock = Path(__file__).resolve().parents[2] / 'scripts/legacy_agilex_assets.lock.json'
+    if data.get('source_lock_sha256') != hashlib.sha256(lock.read_bytes()).hexdigest():
+        raise ValueError('Legacy asset lock mismatch; rebuild assets')
+    for name, expected in data['generated_sha256'].items():
+        if hashlib.sha256((directory / name).read_bytes()).hexdigest() != expected:
+            raise ValueError(f'Modified legacy asset: {name}')
+
+
+def controller_config(arms=("right",)):
+    return {"type": "BASIC", "body_parts": {arm: {
+        "type": "OSC_POSE", "input_max": 1, "input_min": -1,
+        "output_max": [0.015]*3 + [0.08]*3, "output_min": [-0.015]*3 + [-0.08]*3,
+        "kp": 60, "damping_ratio": 1.5, "impedance_mode": "fixed",
+        "input_type": "delta", "input_ref_frame": "world", "uncouple_pos_ori": True,
+        "goal_update_mode": "achieved", "interpolation": None,
+        "gripper": {"type": "GRIP"},
+    } for arm in arms}}
+
+
+def register_agilex():
+    """Register classes without importing robosuite during registry discovery."""
+    validate_bundle()
+    from robosuite.models.robots.robot_model import REGISTERED_ROBOTS
+    if "AgileXPiper" in REGISTERED_ROBOTS:
+        return
+    from robosuite.models.robots.manipulators.manipulator_model import ManipulatorModel
+    from robosuite.models.grippers.gripper_model import GripperModel
+    from robosuite.models.grippers import GRIPPER_MAPPING
+    from robosuite.robots import ROBOT_CLASS_MAPPING, FixedBaseRobot
+
+    class AgileXPiperGripper(GripperModel):
+        def __init__(self, idn=0):
+            super().__init__(str(asset_dir() / "gripper.xml"), idn=idn)
+
+        @property
+        def dof(self):
+            return 1
+
+        @property
+        def init_qpos(self):
+            return np.array([0.035, -0.035])
+
+        def format_action(self, action):
+            # +1 closes both opposing slides; -1 opens.  The keyboard demo
+            # sends a persistent toggle state, so map that state directly to
+            # the normalized gripper target instead of accumulating only a
+            # small increment on every simulation step.  The downstream
+            # position actuators still provide the physical smoothing.
+            self.current_action = np.clip(np.asarray(action, dtype=float), -1, 1)
+            return np.array([-self.current_action[0], self.current_action[0]])
+
+        @property
+        def _important_geoms(self):
+            import json
+            names = json.loads((asset_dir() / 'provenance.json').read_text())['gripper_collision_names']
+            return {'left_finger': names['finger7'], 'right_finger': names['finger8'],
+                    'left_fingerpad': names['finger7'], 'right_fingerpad': names['finger8']}
+
+    class AgileXPiper(ManipulatorModel):
+        arms = ["right"]
+        asset_name = "legacy_piperx.xml"
+        fixed_stand = dict(height=.80, half_depth=.09, half_width=.10)
+
+        def __init__(self, idn=0):
+            super().__init__(str(asset_dir() / self.asset_name), idn=idn)
+            if self.fixed_stand is not None:
+                from .mounts import install_fixed_stand
+                install_fixed_stand(self, **self.fixed_stand)
+
+        def add_gripper(self, gripper, arm_name=None):
+            super().add_gripper(gripper, arm_name)
+            from ..cameras import ensure_robot_cameras
+            ensure_robot_cameras(self)
+            self.cameras = self.get_element_names(self.worldbody, 'camera')
+
+        @property
+        def default_base(self):
+            return "NullMount"
+
+        @property
+        def default_gripper(self):
+            return {arm: "AgileXPiperGripper" for arm in self.arms}
+
+        @property
+        def default_controller_config(self):
+            return {arm: "osc_pose" for arm in self.arms}
+
+        @property
+        def init_qpos(self):
+            # Calibrated tabletop starting posture used by the physical
+            # manipulation tasks.  A zero posture places the wrist high and
+            # causes the controller's first goal update to sweep through
+            # nearby objects.
+            return np.tile([0, 0.8, -1.1, 0, 0.3, 0], len(self.arms))
+
+        @property
+        def base_xpos_offset(self):
+            return {"empty": (-0.45, 0, 0), "bins": (-0.45, 0, 0),
+                    "table": lambda length: (-0.1-length/2, 0, 0)}
+
+        @property
+        def top_offset(self):
+            return np.array([0, 0, 0.7])
+
+        @property
+        def _horizontal_radius(self):
+            return 0.7
+
+        @property
+        def arm_type(self):
+            return "single" if len(self.arms) == 1 else "bimanual"
+
+        @property
+        def _eef_name(self):
+            return {arm: arm + "_hand" for arm in self.arms}
+
+    class AgileXAloha(AgileXPiper):
+        arms = ["right", "left"]
+        asset_name = "aloha.xml"
+        # Imported dual-arm bases are at x=.23875, y=+/- .30.
+        # Support their mounting plane, not the empty asset origin.
+        fixed_stand = dict(height=.65, half_depth=.09, half_width=.375, center_xy=(.23875, 0.))
+
+    class AgileXMobileAloha(AgileXAloha):
+        asset_name = "aloha_mobile.xml"
+        fixed_stand = None
+
+        # FixedBaseRobot is used only for the two arm controllers. The XML
+        # still contains the physical free root and ten wheel/caster joints;
+        # they must not enter robosuite's scalar arm-qpos reset/indexing.
+        @property
+        def joints(self):
+            return [name for name in super().joints if "mobile_" not in name]
+
+        @property
+        def actuators(self):
+            return [name for name in super().actuators if "mobile_" not in name]
+
+    GRIPPER_MAPPING["AgileXPiperGripper"] = AgileXPiperGripper
+    for cls in (AgileXPiper, AgileXAloha, AgileXMobileAloha):
+        ROBOT_CLASS_MAPPING[cls.__name__] = FixedBaseRobot
+
+
+def resolve_robot(robot_id):
+    if robot_id not in ("legacy_piperx", "aloha", "aloha_mobile"):
+        raise ValueError(f"not an AgileX robot: {robot_id}")
+    register_agilex()
+    if robot_id == "aloha_mobile":
+        import xml.etree.ElementTree as ET
+        if ET.parse(asset_dir() / "aloha_mobile.xml").find(".//freejoint[@name='mobile_free']") is None:
+            raise ValueError("Stale planar ALOHA assets: run python scripts/build_agilex_assets.py")
+    return "AgileXPiper" if robot_id == "legacy_piperx" else ("AgileXMobileAloha" if robot_id == "aloha_mobile" else "AgileXAloha")

react_manip/robots/mobile_base.py

@@ -0,0 +1,33 @@
+"""Shared wheel-drive helpers for the custom Mobile Dual PiPER-X asset."""
+from __future__ import annotations
+
+import numpy as np
+
+
+def set_wheel_command(env, vx: float, wz: float) -> None:
+    """Write normalized differential-drive wheel targets into ``data.ctrl``.
+
+    This only sets actuator targets; MuJoCo integrates wheel/body dynamics on
+    the subsequent physical step.  Tasks that expose ``set_mobile_base_command``
+    may keep their own wrapper, while generic frontends can use this helper.
+    """
+    if not np.all(np.isfinite([vx, wz])):
+        raise ValueError("mobile command must be finite")
+    robot = getattr(env, "robots", ())[0]
+    if getattr(robot, "name", "") not in ("MobileDualPiperX", "AgileXMobileAloha"):
+        return
+    model, data = env.sim.model, env.sim.data
+    prefix = robot.robot_model.naming_prefix
+    values = (float(np.clip(vx, -1, 1)), float(np.clip(wz, -1, 1)))
+    try:
+        radius_id = model.geom_name2id(prefix + "mobile_left_wheel_link_tire")
+        radius = float(model.geom_size[radius_id, 0])
+    except (ValueError, KeyError, IndexError):
+        return
+    for side, sign in (("left", -1.0), ("right", 1.0)):
+        try:
+            actuator = model.actuator_name2id(prefix + "mobile_" + side + "_wheel_motor")
+        except (ValueError, KeyError):
+            continue
+        target = (0.25 * values[0] + sign * 0.5 * values[1] * 0.17) / radius
+        data.ctrl[actuator] = float(np.clip(target, -6, 6))

react_manip/robots/mounts.py

@@ -0,0 +1,80 @@
+"""Robot-owned fixed stands. Never add these as task arena geometry."""
+import xml.etree.ElementTree as ET
+
+
+def use_existing_mount(robot):
+    """Remove an automatic floor stand for tasks with a table/car mounting point.
+
+    Call before task-specific set_base_xpos. Preserve all original arm bodies,
+    sites and cameras; only undo the wrapper installed by install_fixed_stand.
+    """
+    root = robot.worldbody.find('body')
+    mount = root.find(f"body[@name='{robot.naming_prefix}fixed_stand_mount']")
+    if mount is None:
+        return
+    root.remove(mount)
+    for child in list(mount):
+        root.append(child)
+    for suffix in ('fixed_stand_collision', 'fixed_stand_visual'):
+        geom = root.find(f"geom[@name='{robot.naming_prefix}{suffix}']")
+        if geom is not None:
+            root.remove(geom)
+
+
+def install_fixed_stand(robot, *, height, half_width, half_depth, center_xy=(0., 0.)):
+    """Raise the complete fixed model once, keeping the root on the floor.
+
+    All imported bodies/sites/cameras move together. A collision geom and a
+    non-colliding visual geom share the same dimensions. Mobile models must
+    not use this helper: their chassis already defines the mounting height.
+    """
+    if min(height, half_width, half_depth) <= 0:
+        raise ValueError('stand dimensions must be positive')
+    root = robot.worldbody.find('body')
+    if root is None:
+        raise ValueError('robot has no root body')
+    if root.find('.//freejoint') is not None or root.find(".//joint[@type='free']") is not None:
+        raise ValueError('cannot add a fixed stand to a free-root robot')
+    name = robot.naming_prefix + 'fixed_stand_mount'
+    if root.find(f"body[@name='{name}']") is not None:
+        return
+    mount = ET.Element('body', name=name, pos=f'0 0 {height}')
+    for child in list(root):
+        root.remove(child)
+        mount.append(child)
+    root.append(mount)
+    attrs = dict(type='box', pos=f'{center_xy[0]} {center_xy[1]} {height / 2}',
+                 size=f'{half_depth} {half_width} {height / 2}', rgba='.25 .28 .32 1',
+                 friction='.8 .01 .001', solref='.005 1', condim='6')
+    ET.SubElement(root, 'geom', name=robot.naming_prefix+'fixed_stand_collision', group='0', **attrs)
+    ET.SubElement(root, 'geom', name=robot.naming_prefix+'fixed_stand_visual', group='1', contype='0', conaffinity='0', density='0', **attrs)
+
+
+def set_fixed_mount_pose(robot, *, position, yaw, height):
+    """Place a complete floor stand using its arm-mount frame, not its floor root.
+
+    position is the world position of the imported asset origin at mounting
+    height. Only yaw is allowed: the stand stays vertical and reaches z=0.
+    Both arms, sites and cameras keep their asset-relative transforms.
+    """
+    import math
+    import numpy as np
+    position = np.asarray(position, dtype=float)
+    if position.shape != (3,) or not np.isfinite(position).all() or not math.isfinite(yaw):
+        raise ValueError('mount pose must be finite')
+    if not math.isfinite(height) or height <= 0 or abs(position[2] - height) > 1e-12:
+        raise ValueError('mount height must equal its world z above the floor')
+    root = robot.worldbody.find('body')
+    mount = root.find(f"body[@name='{robot.naming_prefix}fixed_stand_mount']")
+    if mount is None:
+        raise ValueError('fixed mount pose requires an installed floor stand')
+    mount.set('pos', f'0 0 {height}')
+    for suffix in ('fixed_stand_collision', 'fixed_stand_visual'):
+        geom = root.find(f"geom[@name='{robot.naming_prefix}{suffix}']")
+        pos = np.fromstring(geom.get('pos'), sep=' ')
+        size = np.fromstring(geom.get('size'), sep=' ')
+        pos[2] = size[2] = height / 2
+        geom.set('pos', ' '.join(map(str, pos)))
+        geom.set('size', ' '.join(map(str, size)))
+    robot.set_base_xpos([position[0], position[1], 0.])
+    robot.set_base_ori(np.array([0., 0., yaw]))

react_manip/robots/resolution.py

@@ -0,0 +1,103 @@
+"""Canonical robot-id to robosuite construction resolution.
+
+Keeping this mapping in one place prevents task factories from silently
+falling back to Panda and ensures every physical task uses the same arm order,
+controller and gripper configuration.
+"""
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any
+
+
+
+@dataclass(frozen=True)
+class RobosuiteResolution:
+    robot_id: str
+    robot_name: str
+    controller_configs: Any = None
+    gripper_types: str = "default"
+    initialization_noise: Any = None
+    arms: tuple[str, ...] = ("right",)
+    mobile_base: bool = False
+
+
+def _builtin_resolution(key: str, *, include_controller: bool = True) -> RobosuiteResolution:
+    if key in ('panda_libero','aloha_agilex_robotwin','dual_arx_x5_robodojo','panda_omron_robocasa'):
+        from integrations.rpent_embodiments.models import resolve
+        return resolve(key, include_controller=include_controller)
+    if key == "panda":
+        return RobosuiteResolution(key, "Panda", gripper_types="PandaGripper", initialization_noise="default", arms=("right",))
+    if key == "sawyer":
+        from .sawyer_robosuite import register_sawyer_with_base
+        register_sawyer_with_base()
+        return RobosuiteResolution(key, "CLSawyer", arms=("right",))
+    from .agilex_robosuite import controller_config, resolve_robot
+    arms = ("right",) if key in ("piperx", "legacy_piperx") else ("right", "left")
+    return RobosuiteResolution(
+        key, resolve_robot(key),
+        controller_configs=controller_config(arms) if include_controller else None,
+        initialization_noise=None, arms=arms, mobile_base=key in ("mobile_dual_piperx", "aloha_mobile"),
+    )
+
+
+_PROVIDERS = {key: (lambda *, include_controller=True, key=key:
+                   _builtin_resolution(key, include_controller=include_controller))
+              for key in ('panda', 'sawyer', 'piperx', 'dual_piperx', 'mobile_dual_piperx', 'legacy_piperx', 'aloha', 'aloha_mobile', 'panda_libero', 'aloha_agilex_robotwin', 'dual_arx_x5_robodojo', 'panda_omron_robocasa')}
+_MODEL_IDS = {'liberopanda':'panda_libero', 'robotwinaloha':'aloha_agilex_robotwin', 'robodojox5':'dual_arx_x5_robodojo', 'robocasapandaomron':'panda_omron_robocasa', 'panda': 'panda', 'sawyer': 'sawyer', 'clsawyer': 'sawyer',
+              'agilexpiper': 'legacy_piperx', 'agilexaloha': 'aloha', 'agilexmobilealoha': 'aloha_mobile',
+              'piperx': 'piperx', 'dualpiperx': 'dual_piperx', 'mobiledualpiperx': 'mobile_dual_piperx'}
+
+
+def robot_id_from_model(name):
+    """Resolve a canonical ID or a registered simulator-model alias."""
+    key = str(name).strip().lower()
+    if key in _PROVIDERS:
+        return key
+    if key in _MODEL_IDS:
+        return _MODEL_IDS[key]
+    raise ValueError(f'unknown robot/model {name!r}; register its provider first')
+
+
+def default_controller_config(robot):
+    """Resolve embodiment controller defaults for direct environment creation."""
+    resolved = resolve_robosuite_robot(robot_id_from_model(robot))
+    if resolved.controller_configs is not None:
+        return resolved.controller_configs
+    from robosuite.controllers import load_composite_controller_config
+    return load_composite_controller_config(robot='Sawyer' if resolved.robot_id == 'sawyer' else resolved.robot_name)
+
+
+def registered_robot_ids():
+    """Live catalog; registration does not import simulation dependencies."""
+    return tuple(_PROVIDERS)
+
+
+def register_robosuite_robot(robot_id, provider):
+    """Register a lazy provider accepting ``include_controller=bool``.
+
+    The provider registers its model with robosuite and returns a
+    RobosuiteResolution. This does not assert physical task compatibility.
+    """
+    if not isinstance(robot_id, str) or not robot_id or robot_id != robot_id.strip().lower() or any(c.isspace() for c in robot_id):
+        raise ValueError('robot_id must be a nonempty lowercase identifier')
+    if robot_id in _PROVIDERS:
+        raise ValueError(f'robot already registered: {robot_id}')
+    if robot_id in _MODEL_IDS and _MODEL_IDS[robot_id] != robot_id:
+        raise ValueError(f'robot id conflicts with model alias: {robot_id}')
+    if not callable(provider):
+        raise TypeError('provider must be callable')
+    _PROVIDERS[robot_id] = provider
+
+
+def resolve_robosuite_robot(robot_id: str, *, include_controller: bool = True) -> RobosuiteResolution:
+    from .catalog import normalize_robot_id
+    key = normalize_robot_id(robot_id)
+    result = _PROVIDERS[key](include_controller=include_controller)
+    if not isinstance(result, RobosuiteResolution) or result.robot_id != key:
+        raise ValueError(f'invalid robot provider result for {key}')
+    alias = result.robot_name.lower()
+    if (alias in _MODEL_IDS and _MODEL_IDS[alias] != key) or (alias in _PROVIDERS and alias != key):
+        raise ValueError(f'robot model alias already owned: {result.robot_name}')
+    _MODEL_IDS[alias] = key
+    return result

react_manip/robots/sawyer_robosuite.py

@@ -0,0 +1,147 @@
+"""Sawyer model with its physical pedestal integrated into the robot asset.
+
+The upstream robosuite Sawyer is normally composed with ``RethinkMount``.
+That mount is an environment-time support model whose root is offset below
+the arm by roughly 0.912 m.  This is convenient for table arenas, but makes a
+stand-alone robot appear to float when the environment does not provide that
+mount.  ``SawyerWithBase`` keeps the arm at the calibrated height and adds a
+simple collision/visual pedestal to the Sawyer MJCF itself.  It is therefore
+part of the robot embodiment and follows the robot whenever its root is
+placed or moved.
+"""
+
+from __future__ import annotations
+
+import xml.etree.ElementTree as ET
+
+import numpy as np
+
+
+# Height of the upstream Rethink pedestal from its floor contact to the
+# Sawyer arm root.  The original RethinkMount XML has a child root at -0.922 m
+# and a top offset of -0.01 m, yielding -0.912 m as the model bottom offset.
+SAWYER_ARM_ROOT_Z = 0.912
+SAWYER_BASE_RADIUS = 0.31
+SAWYER_BASE_HEIGHT = SAWYER_ARM_ROOT_Z
+
+
+def register_sawyer_with_base() -> str:
+    """Register and return the robosuite name for the integrated-base Sawyer.
+
+    Imports robosuite lazily so task discovery and schema checks remain usable
+    without simulator dependencies.
+    """
+
+    from robosuite.models.robots.manipulators.sawyer_robot import Sawyer
+    from robosuite.models.robots.robot_model import REGISTERED_ROBOTS
+    from robosuite.robots import ROBOT_CLASS_MAPPING, FixedBaseRobot
+    from robosuite.utils.mjcf_utils import find_elements
+
+    name = "CLSawyer"
+    if name not in REGISTERED_ROBOTS:
+
+        class CLSawyer(Sawyer):
+            """Sawyer with a floor-contacting pedestal in its own MJCF."""
+
+            def __init__(self, idn=0):
+                super().__init__(idn=idn)
+
+                # The arm link in robosuite's Sawyer XML is rooted at z=0 and
+                # relies on RethinkMount to lift it.  Move only the arm branch;
+                # the robot root remains at the floor for normal placement.
+                arm_root = find_elements(
+                    root=self.worldbody,
+                    tags="body",
+                    attribs={"name": self.correct_naming("right_arm_base_link")},
+                    return_first=True,
+                )
+                if arm_root is None:
+                    raise ValueError("Sawyer asset is missing right_arm_base_link")
+                arm_root.set("pos", f"0 0 {SAWYER_ARM_ROOT_Z:.6f}")
+
+                # The upstream robotview is attached directly to the XML
+                # root at z=0.4 because RethinkMount lifts that root by
+                # 0.912 m.  With the integrated pedestal the root is on the
+                # floor, so preserve the original world camera height.
+                robotview = find_elements(
+                    root=self.worldbody,
+                    tags="camera",
+                    attribs={"name": self.correct_naming("robotview")},
+                    return_first=True,
+                )
+                if robotview is not None:
+                    robotview.set("pos", f"1.0 0 {0.4 + SAWYER_ARM_ROOT_Z:.6f}")
+
+                root = self._elements["root_body"]
+                base = ET.Element(
+                    "body",
+                    {
+                        "name": self.correct_naming("sawyer_integrated_base"),
+                        "pos": "0 0 0",
+                    },
+                )
+                # Static root-body geometry is physically attached to the
+                # robot and therefore moves with it, while the visual copy is
+                # excluded from contacts to avoid duplicate collision forces.
+                ET.SubElement(
+                    base,
+                    "geom",
+                    {
+                        "name": self.correct_naming("sawyer_base_collision"),
+                        "type": "cylinder",
+                        "size": f"{SAWYER_BASE_RADIUS:.4f} {SAWYER_BASE_HEIGHT / 2:.4f}",
+                        "pos": f"0 0 {SAWYER_BASE_HEIGHT / 2:.4f}",
+                        "rgba": "0.12 0.14 0.16 1",
+                        "density": "450",
+                        "friction": "0.8 0.01 0.001",
+                        "condim": "6",
+                    },
+                )
+                ET.SubElement(
+                    base,
+                    "geom",
+                    {
+                        "name": self.correct_naming("sawyer_base_visual"),
+                        "type": "cylinder",
+                        # Keep the underside exactly on the floor; only the
+                        # radius is enlarged for a small visual lip.
+                        "size": f"{SAWYER_BASE_RADIUS + 0.006:.4f} {SAWYER_BASE_HEIGHT / 2:.4f}",
+                        "pos": f"0 0 {SAWYER_BASE_HEIGHT / 2:.4f}",
+                        "rgba": "0.18 0.20 0.23 1",
+                        "contype": "0",
+                        "conaffinity": "0",
+                        "group": "1",
+                    },
+                )
+                root.append(base)
+
+            @property
+            def default_base(self):
+                # NullMount contributes no environment-specific pedestal or
+                # vertical offset.  The pedestal above is the robot asset.
+                return "NullMount"
+
+            @property
+            def top_offset(self):
+                # Preserve the upstream top envelope (1.0 m above the arm
+                # root) while the arm root itself is lifted by the integrated
+                # pedestal.
+                return np.array((0.0, 0.0, SAWYER_ARM_ROOT_Z + 1.0))
+
+            @property
+            def _horizontal_radius(self):
+                return max(float(super()._horizontal_radius), SAWYER_BASE_RADIUS)
+
+        # RobotModelMeta registers the class in REGISTERED_ROBOTS.  The
+        # wrapper map is what RobotEnv uses when creating the runtime robot.
+        ROBOT_CLASS_MAPPING[name] = FixedBaseRobot
+        # Keep the public robosuite robot id stable.  Existing task code often
+        # passes ``robots="Sawyer"`` directly; that path must receive the same
+        # integrated-base embodiment as the benchmark factory's ``sawyer`` ID.
+        REGISTERED_ROBOTS["Sawyer"] = CLSawyer
+        ROBOT_CLASS_MAPPING["Sawyer"] = FixedBaseRobot
+
+    return name
+
+
+__all__ = ["register_sawyer_with_base", "SAWYER_ARM_ROOT_Z"]

react_manip/robots/sensors.py

@@ -0,0 +1,137 @@
+"""Embodiment-owned sensor profiles; no task or scene dependencies.
+
+Third-party robot models may expose ``sensor_profile`` or register their
+exact model class name. Unknown models retain imported camera extrinsics.
+"""
+from dataclasses import dataclass
+import xml.etree.ElementTree as ET
+
+
+@dataclass(frozen=True)
+class MountedCamera:
+    """Body and role names are unprefixed, in the robot model namespace."""
+    role: str
+    body: str
+    position: tuple[float, float, float]
+    xyaxes: tuple[float, float, float, float, float, float]
+    fovy: float = 65.
+
+    def __post_init__(self):
+        if self.role not in ('head', 'wrist_right', 'wrist_left', 'overhead_debug'):
+            raise ValueError(f'unknown mounted camera role: {self.role}')
+        if not 0 < self.fovy < 180:
+            raise ValueError('fovy must be between 0 and 180 degrees')
+
+
+@dataclass(frozen=True)
+class SensorProfile:
+    overhead_height: float = 1.8
+    head_min_x: float | None = None
+    head_xyaxes: str | None = None
+    # Wrist camera mount in the gripper/eef local frame.  ``None`` preserves
+    # an imported asset offset for models that do not have a canonical mount.
+    wrist_position: tuple[float, float, float] | None = None
+    wrist_target: tuple[float, float, float] | None = None
+    wrist_up: tuple[float, float, float] = (0., 1., 0.)
+    cameras: tuple[MountedCamera, ...] = ()
+
+
+_PROFILES = {
+    'Panda': SensorProfile(overhead_height=.65, cameras=(MountedCamera('head', 'base', (-.12, -.20, .80), (0., -1., 0., .866025403784, 0., .5), 80.),)),
+    'CLSawyer': SensorProfile(cameras=(MountedCamera('head', 'base', (-.12, -.20, 1.70), (0., -1., 0., .866025403784, 0., .5), 80.),)),
+    # First physical-fit trial: lift the wrist sensor 10 cm in the eef local
+    # ``-X`` direction (the neutral image-up direction) to reduce gripper-root
+    # occlusion while keeping the same TCP aiming point.
+    'PiperX': SensorProfile(wrist_position=(-.1, 0., -.08), wrist_target=(0., 0., .06), wrist_up=(-1., 0., 0.), cameras=(
+        MountedCamera('head', 'right_base', (-.12, -.20, .80),
+                      (0., -1., 0., .866025403784, 0., .5), 80.),
+    )),
+    'DualPiperX': SensorProfile(head_min_x=.24, head_xyaxes='0 -1 0 .5 0 .866025403784', wrist_position=(-.1, 0., -.08), wrist_target=(0., 0., .06), wrist_up=(-1., 0., 0.)),
+    'MobileDualPiperX': SensorProfile(head_min_x=.24, head_xyaxes='0 -1 0 .5 0 .866025403784', wrist_position=(-.1, 0., -.08), wrist_target=(0., 0., .06), wrist_up=(-1., 0., 0.)),
+}
+
+
+_PROFILES['AgileXPiper'] = _PROFILES['PiperX']
+_PROFILES['AgileXAloha'] = _PROFILES['DualPiperX']
+_PROFILES['AgileXMobileAloha'] = _PROFILES['MobileDualPiperX']
+
+
+def register_sensor_profile(model_class_name, profile):
+    if not isinstance(profile, SensorProfile):
+        raise TypeError('profile must be a SensorProfile')
+    if model_class_name in _PROFILES:
+        raise ValueError(f'sensor profile already registered: {model_class_name}')
+    _PROFILES[model_class_name] = profile
+
+
+def sensor_profile(robot):
+    profile = getattr(robot, 'sensor_profile', None)
+    if profile is None:
+        profile = _PROFILES.get(type(robot).__name__, SensorProfile())
+    if not isinstance(profile, SensorProfile):
+        raise TypeError('robot.sensor_profile must be a SensorProfile')
+    return profile
+
+
+def install_declared_cameras(robot):
+    """New models specify mounts explicitly, without legacy name inference."""
+    profile = sensor_profile(robot)
+    roles = [c.role for c in profile.cameras]
+    if len(set(roles)) != len(roles):
+        raise ValueError('duplicate mounted camera roles')
+    for spec in profile.cameras:
+        name = robot.naming_prefix + spec.role
+        body_name = robot.correct_naming(spec.body)
+        body = next((b for b in robot.worldbody.iter('body') if b.get('name') == body_name), None)
+        if body is None:
+            raise ValueError(f'{name}: camera mount body not found: {body_name}')
+        matches = [(b, c) for b in robot.worldbody.iter('body') for c in b.findall('camera') if c.get('name') == name]
+        if len(matches) > 1 or (matches and matches[0][0] is not body):
+            raise ValueError(f'{name}: conflicting camera mount')
+        camera = matches[0][1] if matches else ET.SubElement(body, 'camera', name=name)
+        for attr in ('quat', 'euler', 'axisangle', 'zaxis', 'target', 'mode'):
+            camera.attrib.pop(attr, None)
+        camera.set('pos', ' '.join(map(str, spec.position)))
+        camera.set('xyaxes', ' '.join(map(str, spec.xyaxes)))
+        camera.set('fovy', str(spec.fovy))
+        if spec.role == 'head':
+            # Keep the existing public main-camera ID for all three models.
+            from copy import deepcopy
+            alias = robot.naming_prefix + 'head_camera'
+            if not any(c.get('name') == alias for c in robot.worldbody.iter('camera')):
+                clone = deepcopy(camera); clone.set('name', alias); body.append(clone)
+
+
+def ensure_robot_overhead_camera(robot_model, *, suffix='overhead_camera'):
+    """Attach virtual debug camera to moving root, independent of scene."""
+    world = getattr(robot_model, 'worldbody', None)
+    if world is None:
+        return None
+    naming = getattr(robot_model, 'correct_naming', None)
+    name = naming(suffix) if callable(naming) else suffix
+    if any(c.get('name') == name for c in world.iter('camera')):
+        return name
+    root = world.find('body')
+    if root is None:
+        return None
+    for body in root.iter('body'):
+        if body.find('freejoint') is not None or body.find("joint[@type='free']") is not None:
+            root = body
+            break
+    height = sensor_profile(robot_model).overhead_height
+    ET.SubElement(root, 'camera', name=name, pos=f'0.20 0 {height}',
+                  xyaxes='0 1 0 -1 0 0', fovy='65')
+    return name
+
+
+PIPERX_ROBOTS = ('piperx', 'dual_piperx', 'mobile_dual_piperx')
+CAMERA_CONTRACT_VERSION = '3.0'
+
+
+def policy_camera_ids(robot_id):
+    """Ordered public views; left/right names follow robot arms, not screen sides."""
+    main = ('robot0_head_camera',)
+    if robot_id in PIPERX_ROBOTS:
+        return main + ('robot0_wrist_right',) + (() if robot_id == 'piperx' else ('robot0_wrist_left',))
+    from integrations.rpent_embodiments.catalog import EMBODIMENTS, camera_ids
+    return camera_ids(robot_id) if robot_id in EMBODIMENTS else main

react_manip/utils/__init__.py

@@ -0,0 +1 @@
+"""Small, reusable utilities shared by benchmark environments and rollouts."""