diff --git a/isaaclab_arena/embodiments/droid/droid.py b/isaaclab_arena/embodiments/droid/droid.py index ee3686d03c..2b2fc8c7ad 100644 --- a/isaaclab_arena/embodiments/droid/droid.py +++ b/isaaclab_arena/embodiments/droid/droid.py @@ -4,17 +4,13 @@ # SPDX-License-Identifier: Apache-2.0 -import functools import torch -import warnings from abc import ABC -from typing import Any import isaaclab.envs.mdp as mdp_isaac_lab import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets.articulation.articulation_cfg import ArticulationCfg -from isaaclab.assets.asset_base_cfg import AssetBaseCfg from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg from isaaclab.envs.mdp.actions.actions_cfg import ( BinaryJointPositionActionCfg, @@ -30,7 +26,6 @@ from isaaclab.markers.config import FRAME_MARKER_CFG from isaaclab.sensors.camera.camera_cfg import CameraCfg from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg, OffsetCfg -from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg from isaaclab.utils.configclass import configclass from isaaclab_arena.assets.nucleus import ARENA_NUCLEUS_DIR @@ -40,14 +35,24 @@ from isaaclab_arena.embodiments.droid.observations import arm_joint_pos, ee_pos, ee_quat, gripper_pos from isaaclab_arena.embodiments.embodiment_base import EmbodimentBase from isaaclab_arena.embodiments.franka.franka import franka_stack_events +from isaaclab_arena.embodiments.robot_on_stand_utils import RobotPrimSpec, StandPrimSpec, compose_on_stand_usd from isaaclab_arena.utils.cameras import ArenaCameraCfg -from isaaclab_arena.utils.pose import Pose, PosePerEnv, translate_by_xyz_offset +from isaaclab_arena.utils.pose import Pose -# The base stand's x/y footprint. -_STAND_FOOTPRINT_SCALE_XY: tuple[float, float] = (1.2, 1.2) -# The default stand height. -_DEFAULT_STAND_HEIGHT_M: float = 1.35 -_FALLBACK_STAND_UNIT_HEIGHT_M: float = 0.795 +_DROID_ROBOT_PRIM = RobotPrimSpec( + robot_usd_path=f"{ARENA_NUCLEUS_DIR}/Arena/assets/robot_library/droid/franka_robotiq_2f_85_flattened.usd", + root_prim_path="/panda", + robot_base_prim_name="panda_link0", + stand_prim_name="stand_instanceable", +) +_DROID_STAND_PRIM = StandPrimSpec( + stand_usd_path=f"{ARENA_NUCLEUS_DIR}/Arena/assets/object_library/srl_robolab_assets/robots/franka_stand_grey.usda", + ref_prim_path="/World/franka_table", + payload_child_name="franka_table", + footprint_translate_xyz=(-0.05, 0.0, 0.0), + footprint_scale_xy=(1.2, 1.2), + stand_default_height=1.35, +) class DroidEmbodimentBase(EmbodimentBase, ABC): @@ -55,6 +60,12 @@ class DroidEmbodimentBase(EmbodimentBase, ABC): Includes Franka with robotiq gripper and specific set of cameras. Subclasses must set ``self.action_config`` to a concrete action configuration. + + ``initial_pose`` / ``set_initial_pose`` set the base of the robot in world frame. + ``stand_height_m`` sets the height of the stand mesh under the robot base link, + which changes how far the stand extends below the root link. + When manually placing the robot on floor, ``set_initial_pose`` z value and + ``stand_height_m`` should be adjusted together to keep the bottom of stand fixed. """ name = "droid" @@ -67,27 +78,17 @@ def __init__( initial_joint_pose: list[float] | None = None, concatenate_observation_terms: bool = False, arm_mode: ArmMode | None = None, - stand_height_m: float = _DEFAULT_STAND_HEIGHT_M, + stand_height_m: float = _DROID_STAND_PRIM.stand_default_height, ): super().__init__(enable_cameras, initial_pose, concatenate_observation_terms, arm_mode) + self.stand_height_m = stand_height_m self.scene_config = DroidSceneCfg() - # ``stand_height_m`` is an absolute height in meters; convert it to the z-scale the USD needs. - stand_unit_height = _stand_unit_height_m(self.scene_config.stand.spawn.usd_path) - self.scene_config.stand.spawn.scale = (*_STAND_FOOTPRINT_SCALE_XY, stand_height_m / stand_unit_height) - # Lift the robot base (and stand) so a taller/shorter stand keeps its bottom on the floor. - self._robot_base_offset = (0.0, 0.0, stand_height_m - _DEFAULT_STAND_HEIGHT_M) - if self.initial_pose is None: - # No explicit base pose: lift the default robot and stand init states so the robot sits atop - # the lifted stand. - self.scene_config.robot.init_state.pos = translate_by_xyz_offset( - self.scene_config.robot.init_state.pos, self._robot_base_offset - ) - self.scene_config.stand.init_state.pos = translate_by_xyz_offset( - self.scene_config.stand.init_state.pos, self._robot_base_offset - ) - else: - # Explicit base pose: lift it via set_initial_pose; get_scene_cfg writes the scene config later. - self.set_initial_pose(self.initial_pose) + self.scene_config.robot.spawn.usd_path = compose_on_stand_usd( + _DROID_ROBOT_PRIM, + _DROID_STAND_PRIM, + stand_height_m=stand_height_m, + output_basename="droid_franka_robotiq_on_stand", + ) self.action_config = None self.camera_config = DroidCameraCfg() self.observation_config = DroidObservationsCfg() @@ -98,32 +99,6 @@ def __init__( self.mimic_env = None self.add_camera_variations(self.camera_config) - def _translate_pose(self, pose: Pose | PosePerEnv) -> Pose | PosePerEnv: - """Lift a base pose (or per-env poses) by the stand-height offset to match the spawned base.""" - if isinstance(pose, PosePerEnv): - return PosePerEnv(poses=[p.translate(self._robot_base_offset) for p in pose.poses]) - return pose.translate(self._robot_base_offset) - - def set_initial_pose(self, pose: Pose | PosePerEnv, create_reset_event: bool = True) -> None: - """Store the requested base pose(s), lifted by the stand-height offset to match the spawned base.""" - super().set_initial_pose(self._translate_pose(pose), create_reset_event=create_reset_event) - - def has_unplaced_auxiliary_prims(self) -> bool: - # Droid spawns a static stand prim that per-env reset does not yet reposition, so flag it and - # let the relation-placement guard reject a movable Droid rather than orphan the stand at env 0. - # TODO(zihaox): make the stand move with the base on reset, then drop this override. We can - # either override layout_pose_to_scene_writes to also write the stand's pose, or bake the stand - # into the robot USD so it moves with the base. - return True - - def _update_scene_cfg_with_robot_initial_pose(self, scene_config: Any, pose: Pose) -> Any: - # ``pose`` is already lifted by the stand-height offset (see __init__ / set_initial_pose), so the - # base implementation sets the robot base as-is; we only add the stand placement here. - scene_config = super()._update_scene_cfg_with_robot_initial_pose(scene_config, pose) - scene_config.stand.init_state.pos = pose.position_xyz - scene_config.stand.init_state.rot = pose.rotation_xyzw - return scene_config - def set_initial_joint_pose(self, initial_joint_pose: list[float]) -> None: self.event_config.init_franka_arm_pose.params["default_pose"] = initial_joint_pose @@ -148,7 +123,7 @@ def __init__( initial_joint_pose: list[float] | None = None, concatenate_observation_terms: bool = False, arm_mode: ArmMode | None = None, - stand_height_m: float = _DEFAULT_STAND_HEIGHT_M, + stand_height_m: float = _DROID_STAND_PRIM.stand_default_height, ): super().__init__( enable_cameras, @@ -175,7 +150,7 @@ def __init__( initial_joint_pose: list[float] | None = None, concatenate_observation_terms: bool = False, arm_mode: ArmMode | None = None, - stand_height_m: float = _DEFAULT_STAND_HEIGHT_M, + stand_height_m: float = _DROID_STAND_PRIM.stand_default_height, ): super().__init__( enable_cameras, @@ -203,7 +178,7 @@ def __init__( initial_joint_pose: list[float] | None = None, concatenate_observation_terms: bool = False, arm_mode: ArmMode | None = None, - stand_height_m: float = _DEFAULT_STAND_HEIGHT_M, + stand_height_m: float = _DROID_STAND_PRIM.stand_default_height, ): super().__init__( enable_cameras, @@ -218,13 +193,17 @@ def __init__( @configclass class DroidSceneCfg: - """Additions to the scene configuration coming from the Franka embodiment.""" + """Additions to the scene configuration coming from the Droid embodiment. + + The robot USD path is overwritten at embodiment construction via + ``compose_on_stand_usd`` (cached local robot+stand assembly). + """ - # The robot + # The robot (stand is baked into the local on-stand USD, not a separate prim). robot: ArticulationCfg = ArticulationCfg( prim_path="{ENV_REGEX_NS}/Robot", spawn=sim_utils.UsdFileCfg( - usd_path=f"{ARENA_NUCLEUS_DIR}/Arena/assets/robot_library/droid/franka_robotiq_2f_85_flattened.usd", + usd_path=_DROID_ROBOT_PRIM.robot_usd_path, activate_contact_sensors=True, rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=True, @@ -277,19 +256,6 @@ class DroidSceneCfg: ), }, ) - # The stand for the franka - # TODO(alexmillane, 2025-07-28): We probably want to make the stand an optional addition. - stand: AssetBaseCfg = AssetBaseCfg( - prim_path="{ENV_REGEX_NS}/Robot_Stand", - init_state=AssetBaseCfg.InitialStateCfg(pos=[-0.05, 0.0, 0.0], rot=[0.0, 0.0, 0.0, 1.0]), - spawn=UsdFileCfg( - usd_path=( - f"{ARENA_NUCLEUS_DIR}/Arena/assets/object_library/srl_robolab_assets/robots/franka_stand_grey.usda" - ), - scale=(*_STAND_FOOTPRINT_SCALE_XY, _DEFAULT_STAND_HEIGHT_M / _FALLBACK_STAND_UNIT_HEIGHT_M), - activate_contact_sensors=False, - ), - ) # The end-effector frame marker ee_frame: FrameTransformerCfg = FrameTransformerCfg( @@ -460,7 +426,7 @@ class DroidCameraCfg(ArenaCameraCfg): """Configuration for cameras. DROID cameras are mounted with pre-set poses.""" external_camera: CameraCfg = CameraCfg( - prim_path="{ENV_REGEX_NS}/Robot/external_camera", + prim_path="{ENV_REGEX_NS}/Robot/panda_link0/external_camera", height=720, width=1280, data_types=["rgb"], @@ -473,7 +439,7 @@ class DroidCameraCfg(ArenaCameraCfg): offset=CameraCfg.OffsetCfg(pos=(0.05, 0.57, 0.66), rot=(-0.195, 0.399, 0.805, -0.393), convention="opengl"), ) external_camera_2: CameraCfg = CameraCfg( - prim_path="{ENV_REGEX_NS}/Robot/external_camera_2", + prim_path="{ENV_REGEX_NS}/Robot/panda_link0/external_camera_2", height=720, width=1280, data_types=["rgb"], @@ -500,27 +466,3 @@ class DroidCameraCfg(ArenaCameraCfg): pos=(0.011, -0.031, -0.074), rot=(0.570, 0.576, -0.409, -0.420), convention="opengl" ), ) - - -@functools.cache -def _stand_unit_height_m(usd_path: str) -> float: - """Native (scale=1.0) z-height of the stand USD in meters, cached per asset path. - - Falls back to ``_FALLBACK_STAND_UNIT_HEIGHT_M`` if the asset cannot be opened or measured. - """ - try: - from pxr import Usd, UsdGeom - - stage = Usd.Stage.Open(usd_path) - assert stage is not None, f"could not open stand USD: {usd_path}" - root_prim = stage.GetDefaultPrim() or stage.GetPseudoRoot() - bound = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]).ComputeWorldBound(root_prim) - height = bound.ComputeAlignedRange().GetSize()[2] - assert height > 0.0, f"non-positive stand height {height} from {usd_path}" - return height - except Exception as exc: # noqa: BLE001 - any failure falls back to the measured constant - warnings.warn( - f"Falling back to {_FALLBACK_STAND_UNIT_HEIGHT_M} m for the stand height; " - f"could not measure {usd_path}: {exc!r}" - ) - return _FALLBACK_STAND_UNIT_HEIGHT_M diff --git a/isaaclab_arena/embodiments/franka/franka.py b/isaaclab_arena/embodiments/franka/franka.py index 63fc295603..8ec1b1d43f 100644 --- a/isaaclab_arena/embodiments/franka/franka.py +++ b/isaaclab_arena/embodiments/franka/franka.py @@ -26,33 +26,38 @@ from isaaclab.markers.config import FRAME_MARKER_CFG from isaaclab.sensors import CameraCfg from isaaclab.sensors.frame_transformer.frame_transformer_cfg import FrameTransformerCfg, OffsetCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR from isaaclab.utils.configclass import configclass from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG from isaaclab_tasks.manager_based.manipulation.stack.mdp import franka_stack_events from isaaclab_tasks.manager_based.manipulation.stack.mdp.observations import ee_frame_pos, ee_frame_quat -from isaaclab_arena.assets.nucleus import ARENA_NUCLEUS_DIR from isaaclab_arena.assets.register import register_asset from isaaclab_arena.embodiments.common.arm_mode import ArmMode from isaaclab_arena.embodiments.common.mimic_utils import get_rigid_and_articulated_object_poses from isaaclab_arena.embodiments.embodiment_base import EmbodimentBase from isaaclab_arena.embodiments.franka.observations import gripper_pos +from isaaclab_arena.embodiments.robot_on_stand_utils import RobotPrimSpec, StandPrimSpec, compose_on_stand_usd from isaaclab_arena.utils.cameras import ArenaCameraCfg from isaaclab_arena.utils.pose import Pose _DEFAULT_CAMERA_OFFSET = Pose(position_xyz=(0.11, -0.031, -0.074), rotation_xyzw=(0.0, 0.0, 0.70711, 0.70711)) - -# The reason to use our internal panda USD is to combine the panda and the stand within one USD. -# This is not ideal but currently required by the ObjectPlacementSolver to handle the robot placement correctly. -# TODO(cvolk): Move to the IsaacLab supported FRANKA_CFG and handle the handling of the stand internally. -_FRANKA_IK_REL_CFG = FRANKA_PANDA_HIGH_PD_CFG.copy() -_FRANKA_IK_REL_CFG.spawn.usd_path = f"{ARENA_NUCLEUS_DIR}/Arena/assets/robot_library/franka_panda_hand_on_stand.usd" - -# Standard-PD Franka for joint-position control. -# Uses FRANKA_PANDA_CFG (gravity on, stiffness=80, damping=4) instead of HIGH_PD. -_FRANKA_JOINT_POS_CFG = FRANKA_PANDA_CFG.copy() -_FRANKA_JOINT_POS_CFG.spawn.usd_path = _FRANKA_IK_REL_CFG.spawn.usd_path +_FRANKA_ROBOT_PRIM = RobotPrimSpec( + # TODO(qianl): use FRANKA_PANDA_CFG spawn path once IsaacSim version updates to use Legacy path by default. + robot_usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/Legacy/panda_instanceable.usd", + root_prim_path="/panda", + robot_base_prim_name="panda_link0", + stand_prim_name="stand_instanceable", +) +_FRANKA_STAND_PRIM = StandPrimSpec( + stand_usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", + ref_prim_path="/Stand", + payload_child_name="Stand", + footprint_translate_xyz=(-0.05, 0.0, 0.0), + footprint_scale_xy=(1.2, 1.2), + stand_default_height=0.8755, +) class FrankaEmbodimentBase(EmbodimentBase): @@ -113,7 +118,7 @@ def __init__( concatenate_observation_terms=concatenate_observation_terms, arm_mode=arm_mode, ) - self.scene_config.robot = _FRANKA_IK_REL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene_config.robot = _franka_robot_cfg_on_stand(FRANKA_PANDA_HIGH_PD_CFG.copy()) self.action_config = FrankaIKActionCfg() def get_command_body_name(self) -> str: @@ -167,7 +172,7 @@ def __init__( arm_mode=arm_mode, ) self.action_config = FrankaJointPosActionsCfg() - self.scene_config.robot = _FRANKA_JOINT_POS_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene_config.robot = _franka_robot_cfg_on_stand(FRANKA_PANDA_CFG.copy()) def get_command_body_name(self) -> str: return "panda_hand" @@ -449,3 +454,15 @@ def get_object_poses(self, env_ids: Sequence[int] | None = None): object_pose_matrix = get_rigid_and_articulated_object_poses(state, env_ids) return object_pose_matrix + + +def _franka_robot_cfg_on_stand(robot_cfg: ArticulationCfg) -> ArticulationCfg: + """Copy ``robot_cfg`` onto ``{ENV_REGEX_NS}/Robot`` with the composed on-stand USD.""" + cfg = robot_cfg.replace(prim_path="{ENV_REGEX_NS}/Robot") + cfg.spawn.usd_path = compose_on_stand_usd( + _FRANKA_ROBOT_PRIM, + _FRANKA_STAND_PRIM, + stand_height_m=_FRANKA_STAND_PRIM.stand_default_height, + output_basename="franka_panda_on_stand", + ) + return cfg diff --git a/isaaclab_arena/embodiments/robot_on_stand_utils.py b/isaaclab_arena/embodiments/robot_on_stand_utils.py new file mode 100644 index 0000000000..f9dd1b5b39 --- /dev/null +++ b/isaaclab_arena/embodiments/robot_on_stand_utils.py @@ -0,0 +1,171 @@ +# Copyright (c) 2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Shared robot-on-stand USD compose helpers for Franka-based embodiments.""" + +from __future__ import annotations + +import functools +import os +import tempfile +from dataclasses import dataclass +from pathlib import Path + +from isaaclab.utils.assets import retrieve_file_path +from pxr import Gf, Usd, UsdGeom + +from isaaclab_arena.assets.asset_cache import get_arena_asset_cache_dir + +_ROBOT_ON_STAND_USD_CACHE_DIR = "robot_on_stand" + +_HEIGHT_ATOL = 1e-3 +_ALIGN_ATOL = 5e-2 + + +@dataclass(frozen=True) +class RobotPrimSpec: + """Robot USD and prim layout for on-stand compose.""" + + robot_usd_path: str + root_prim_path: str + robot_base_prim_name: str + stand_prim_name: str + + @property + def robot_base_prim_path(self) -> str: + """PhysX articulation root link path under the composed default prim.""" + return f"{self.root_prim_path}/{self.robot_base_prim_name}" + + @property + def stand_prim_path(self) -> str: + """Stand mount prim path parented under the robot base link.""" + return f"{self.robot_base_prim_path}/{self.stand_prim_name}" + + +@dataclass(frozen=True) +class StandPrimSpec: + """Stand USD reference and footprint for normalized on-stand compose.""" + + stand_usd_path: str + ref_prim_path: str + payload_child_name: str + footprint_translate_xyz: tuple[float, float, float] + footprint_scale_xy: tuple[float, float] + stand_default_height: float + + +@functools.cache +def compose_on_stand_usd( + robot: RobotPrimSpec, + stand: StandPrimSpec, + *, + stand_height_m: float, + output_basename: str, +) -> str: + """Build a robot+stand USD with stand under the robot base link. + + Composes once per unique arguments, writes the result to ``~/.cache/isaaclab_arena/usd/robot_on_stand/``, + and returns that path. + + Args: + robot: Robot USD path and prim layout under the composed default prim. + stand: Stand reference and footprint parameters. + stand_height_m: Target absolute stand height after align. + output_basename: Stable basename for the composed USD (embodiment-specific). + + Returns: + Local path to the composed on-stand USD. + """ + assert stand_height_m > 0.0, f"stand_height_m must be positive, got {stand_height_m}" + + cache_root = get_arena_asset_cache_dir().parent / "usd" / _ROBOT_ON_STAND_USD_CACHE_DIR + cache_root.mkdir(parents=True, exist_ok=True) + out_path = cache_root / f"{output_basename}_{stand_height_m:.3f}.usd" + + with tempfile.NamedTemporaryFile(suffix=".usd", dir=cache_root, delete=False) as tmp_file: + tmp_path = Path(tmp_file.name) + + try: + stage = Usd.Stage.CreateNew(str(tmp_path)) + root = stage.DefinePrim(robot.root_prim_path, "Xform") + robot_resolved = retrieve_file_path(robot.robot_usd_path) + root.GetReferences().AddReference(robot_resolved, robot.root_prim_path) + stage.SetDefaultPrim(root) + + _mount_stand_normalized(stage, robot, stand, stand_height_m) + assert stage.GetRootLayer().Save(), f"failed to save composed on-stand USD to {tmp_path}" + os.replace(tmp_path, out_path) + except Exception: + tmp_path.unlink(missing_ok=True) + raise + + return str(out_path) + + +def _mount_stand_normalized( + stage: Usd.Stage, + robot: RobotPrimSpec, + stand: StandPrimSpec, + stand_height_m: float, +) -> None: + """Parent a stand payload under the robot base link, scale, and align to the robot base. + + Composed USD hierarchy:: + + /panda/panda_link0/stand_instanceable # outer mount + translate: (footprint_xy, align_z) + scale: (footprint_xy, stand_height_m / native_height) + // # inner payload + reference: stand USD @ ref_prim_path + + Args: + stage: Composed robot-on-stand stage (robot reference already mounted). + robot: Robot prim layout under the composed default prim. + stand: Stand reference and footprint parameters. + stand_height_m: Target stand height after align. + """ + # Robot-only stage: bottom of the base link is the align target (before stand exists). + robot_base = stage.GetPrimAtPath(robot.robot_base_prim_path) + assert robot_base.IsValid(), f"On-stand USD missing robot base prim at {robot.robot_base_prim_path!r}" + pre_cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + robot_range = pre_cache.ComputeWorldBound(robot_base).ComputeAlignedRange() + assert not robot_range.IsEmpty(), f"empty robot base bounds at {robot_base.GetPath()}" + robot_min_z = float(robot_range.GetMin()[2]) + + stand_resolved = retrieve_file_path(stand.stand_usd_path) + tx, ty, tz = stand.footprint_translate_xyz + sx, sy = stand.footprint_scale_xy + stand_prim_path = robot.stand_prim_path + + # Outer mount under link0; Z scale stays at 1 until native height is measured. + stand_xf = UsdGeom.Xform.Define(stage, stand_prim_path) + translate_op = stand_xf.AddTranslateOp() + translate_op.Set(Gf.Vec3d(tx, ty, tz)) + scale_op = stand_xf.AddScaleOp() + scale_op.Set(Gf.Vec3d(sx, sy, 1.0)) + + payload_prim = stage.DefinePrim(f"{stand_prim_path}/{stand.payload_child_name}") + payload_prim.GetReferences().AddReference(stand_resolved, stand.ref_prim_path) + + stand_prim = stand_xf.GetPrim() + bbox_cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + + # Native height at footprint XY scale; then scale Z to the requested stand height. + stand_range = bbox_cache.ComputeWorldBound(stand_prim).ComputeAlignedRange() + assert not stand_range.IsEmpty(), f"empty stand bounds at {stand_prim.GetPath()}" + native_height = float(stand_range.GetSize()[2]) + assert native_height > 0.0, f"non-positive stand height at {stand_prim.GetPath()}" + scale_op.Set(Gf.Vec3d(sx, sy, stand_height_m / native_height)) + + # Raise/lower outer translate so stand top meets robot_min_z. + translate_op.Set(Gf.Vec3d(tx, ty, robot_min_z)) + + # Verify stand/robot alignment and stand height on a fresh BBoxCache. + verify_cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + stand_range = verify_cache.ComputeWorldBound(stand_prim).ComputeAlignedRange() + stand_height = float(stand_range.GetSize()[2]) + stand_max_z = float(stand_range.GetMax()[2]) + assert abs(stand_height - stand_height_m) < _HEIGHT_ATOL, stand_height + assert abs(stand_max_z - robot_min_z) < _ALIGN_ATOL, (stand_max_z, robot_min_z) diff --git a/isaaclab_arena/environments/relation_solver_interface.py b/isaaclab_arena/environments/relation_solver_interface.py index 46bd389316..c8730b60ce 100644 --- a/isaaclab_arena/environments/relation_solver_interface.py +++ b/isaaclab_arena/environments/relation_solver_interface.py @@ -137,8 +137,6 @@ def _apply_relation_placement_result( anchor_assets = set(get_anchor_objects(assets)) # Prevent external pose-reset events from conflicting with relation-solved assets. _validate_no_conflicting_pose_reset_events(assets, anchor_assets) - # Reject movable compound assets whose auxiliary prims a per-env reset cannot yet reposition. - _validate_no_unplaced_auxiliary_prims(assets, anchor_assets) # Anchor assets do not move, so no need to apply reset event. if anchor_assets == set(assets): @@ -231,27 +229,3 @@ def _validate_no_conflicting_pose_reset_events( "Relational solving should not be combined with explicit setting of " "poses on non-anchor assets." ) - - -def _validate_no_unplaced_auxiliary_prims( - assets: list[PlaceableAsset], - anchor_assets: set[PlaceableAsset], -) -> None: - """Reject movable compound assets whose auxiliary prims a per-env reset cannot reposition. - - A per-env reset repositions only the prims a compound asset emits from - ``layout_pose_to_scene_writes``; any auxiliary prim it omits (e.g. Droid's static stand) would - stay at its env-0 spot. Fail loudly here rather than silently orphaning those prims. - - NOTE: This is a temporary guard. Delete it once auxiliary prims travel with their parent on - reset (see the in-progress auxiliary-prim work), after which compound assets can be placed - like any other and this check is no longer needed. - """ - for asset in assets: - if asset in anchor_assets: - continue - assert not asset.has_unplaced_auxiliary_prims(), ( - f"Non-anchor asset '{asset.name}' owns auxiliary scene prims that a per-environment reset " - "does not reposition, so relation placement would orphan them (e.g. Droid's static stand). " - "Make it an anchor, or implement layout_pose_to_scene_writes for its auxiliary prims." - ) diff --git a/isaaclab_arena/relations/placement_asset.py b/isaaclab_arena/relations/placement_asset.py index 2478a4ba7a..c659f54979 100644 --- a/isaaclab_arena/relations/placement_asset.py +++ b/isaaclab_arena/relations/placement_asset.py @@ -116,8 +116,8 @@ def _get_initial_pose_as_pose(self) -> Pose | None: def layout_pose_to_scene_writes(self, layout_pose: Pose) -> list[tuple[str, Pose]]: """Return the ``(scene entity name, env-local pose)`` writes that realize a solved layout pose. - A simple asset places only its own root; a compound asset (e.g. a robot on a separate - stand) overrides this to also place auxiliary prims that move with the root. + A simple asset places only its own root. Compound assets that still spawn auxiliary prims + outside that root override this to emit additional writes. """ return [(self.get_scene_key(), layout_pose)] @@ -125,15 +125,6 @@ def has_pose_reset_event(self) -> bool: """Return whether the asset owns a root-pose reset event.""" return self._pose_event_cfg is not None - def has_unplaced_auxiliary_prims(self) -> bool: - """Whether this asset owns auxiliary scene prims that per-environment reset does not reposition. - - Defaults to False. A compound asset whose ``layout_pose_to_scene_writes`` does not yet emit - writes for all of its prims (e.g. Droid's static stand) overrides this to True so relation - placement can reject it loudly instead of silently orphaning those prims. - """ - return False - @abstractmethod def get_bounding_box(self) -> AxisAlignedBoundingBox: """Return root-relative axis-aligned bounds.""" diff --git a/isaaclab_arena/tests/test_droid_stand_height.py b/isaaclab_arena/tests/test_droid_stand_height.py deleted file mode 100644 index 300d40c2d9..0000000000 --- a/isaaclab_arena/tests/test_droid_stand_height.py +++ /dev/null @@ -1,83 +0,0 @@ -# Copyright (c) 2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: Apache-2.0 - -import traceback - -from isaaclab_arena.tests.utils.subprocess import run_simulation_app_function - -_CUSTOM_STAND_HEIGHT_M = 2.0 - - -def _test_droid_stand_height(simulation_app) -> bool: - """Check ``stand_height_m`` (absolute meters) sets the stand z-scale and lifts the robot base.""" - - from isaaclab_arena.assets.registries import AssetRegistry - from isaaclab_arena.embodiments.droid.droid import ( - _DEFAULT_STAND_HEIGHT_M, - _STAND_FOOTPRINT_SCALE_XY, - DroidAbsoluteJointPositionEmbodiment, - _stand_unit_height_m, - ) - from isaaclab_arena.utils.pose import Pose - - try: - # ``stand_height_m`` is an absolute height in meters, converted to a z-scale via the stand's - # native (scale=1.0) height. - default_emb = DroidAbsoluteJointPositionEmbodiment() - unit_height = _stand_unit_height_m(default_emb.scene_config.stand.spawn.usd_path) - expected_default_scale = (*_STAND_FOOTPRINT_SCALE_XY, _DEFAULT_STAND_HEIGHT_M / unit_height) - expected_custom_scale = (*_STAND_FOOTPRINT_SCALE_XY, _CUSTOM_STAND_HEIGHT_M / unit_height) - - # The default leaves the robot base at z=0 (no lift relative to the default height). - for got, want in zip(default_emb.scene_config.stand.spawn.scale, expected_default_scale): - assert abs(got - want) < 1e-6 - assert default_emb.scene_config.robot.init_state.pos[2] == 0.0 - - # The lift is the height delta from the default, in meters. - expected_offset = _CUSTOM_STAND_HEIGHT_M - _DEFAULT_STAND_HEIGHT_M - - # An override changes only the z-scale (x/y footprint and robot mesh untouched)... - custom_emb = DroidAbsoluteJointPositionEmbodiment(stand_height_m=_CUSTOM_STAND_HEIGHT_M) - for got, want in zip(custom_emb.scene_config.stand.spawn.scale, expected_custom_scale): - assert abs(got - want) < 1e-6 - assert custom_emb.scene_config.robot.spawn.scale in (None, (1.0, 1.0, 1.0)) - - # ...and lifts the robot base and stand together so the stand's floor contact is preserved. - assert abs(custom_emb.scene_config.robot.init_state.pos[2] - expected_offset) < 1e-6 - assert abs(custom_emb.scene_config.stand.init_state.pos[2] - expected_offset) < 1e-6 - - # An explicit initial_pose is lifted at ingestion (set_initial_pose), so the stored override - # already equals the spawned base: the requested z plus the stand-height offset. - posed_emb = DroidAbsoluteJointPositionEmbodiment(stand_height_m=_CUSTOM_STAND_HEIGHT_M) - posed_emb.set_initial_pose(Pose(position_xyz=(0.3, 0.0, 0.5), rotation_xyzw=(0.0, 0.0, 0.0, 1.0))) - assert abs(posed_emb.initial_pose.position_xyz[2] - (0.5 + expected_offset)) < 1e-6 - scene_cfg = posed_emb.get_scene_cfg() - assert abs(scene_cfg.robot.init_state.pos[2] - (0.5 + expected_offset)) < 1e-6 - assert abs(scene_cfg.stand.init_state.pos[2] - (0.5 + expected_offset)) < 1e-6 - # initial_pose is the single source of truth: it matches the spawned robot base exactly. - assert tuple(posed_emb.initial_pose.position_xyz) == tuple(scene_cfg.robot.init_state.pos) - - # The YAML-spec path instantiates the embodiment via asset_class(**params); a scalar - # stand_height_m from YAML applies just the same. - registry_emb = AssetRegistry().get_asset_by_name("droid_abs_joint_pos")(stand_height_m=_CUSTOM_STAND_HEIGHT_M) - for got, want in zip(registry_emb.scene_config.stand.spawn.scale, expected_custom_scale): - assert abs(got - want) < 1e-6 - - except Exception as e: - print(f"Error: {e}") - traceback.print_exc() - return False - - return True - - -def test_droid_stand_height(): - """Pytest entry point for the Droid stand-height configuration test.""" - result = run_simulation_app_function(_test_droid_stand_height, headless=True) - assert result, f"Test {test_droid_stand_height.__name__} failed" - - -if __name__ == "__main__": - test_droid_stand_height() diff --git a/isaaclab_arena/tests/test_embodiment_collision_mesh.py b/isaaclab_arena/tests/test_embodiment_collision_mesh.py index 9b9487d2e6..8f5c746328 100644 --- a/isaaclab_arena/tests/test_embodiment_collision_mesh.py +++ b/isaaclab_arena/tests/test_embodiment_collision_mesh.py @@ -20,16 +20,22 @@ def _test_embodiment_provides_robot_collision_mesh(simulation_app) -> bool: assert mesh is not None, "embodiment must expose a collision mesh; None forces the loose bbox fallback" assert len(mesh.vertices) > 0 - # The default prim scopes extraction to the arm. The Droid USD also bakes in a 50 m ground - # plane and stray props; leaking those would blow the mesh up to scene scale. + # Mesh extraction scopes to UsdGeom.Mesh under the default prim (arm/gripper). The Droid USD also + # bakes in a stand and may reference non-mesh gprims; leaking a 50 m ground plane would blow this up. extents = mesh.extents assert all(e < 2.0 for e in extents), f"mesh leaked non-robot geometry: extents {extents}" - # The mesh and the bounding box describe the same body, so their extents track each other. + # Placement bbox comes from the full composed on-stand spawn USD (robot + stand). It should be at + # least as large as the arm mesh footprint. bbox = emb.get_bounding_box() bbox_size = (bbox.max_point - bbox.min_point)[0].tolist() for mesh_extent, box_extent in zip(extents, bbox_size): - assert abs(mesh_extent - box_extent) < 0.2, f"mesh extents {extents} disagree with bbox {bbox_size}" + assert ( + box_extent + 1e-3 >= mesh_extent + ), f"placement bbox {bbox_size} should cover robot mesh extents {extents.tolist()}" + # TODO(qianl): Re-enable check for exact match when the stand with non-mesh collision geometry + # is correctly included in get_bounding_box()/extract_trimesh_from_prim() + # assert abs(mesh_extent - box_extent) < 0.2, f"mesh extents {extents} disagree withbox {bbox_size}" # Extraction opens the USD, so the result is cached rather than recomputed per solve. assert emb.get_collision_mesh() is mesh diff --git a/isaaclab_arena/tests/test_relation_solver_interface.py b/isaaclab_arena/tests/test_relation_solver_interface.py index 43d32a534b..561a446770 100644 --- a/isaaclab_arena/tests/test_relation_solver_interface.py +++ b/isaaclab_arena/tests/test_relation_solver_interface.py @@ -7,9 +7,6 @@ import pytest -from isaaclab_arena.tests.dummy_object import DummyObject -from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox - def _make_desk(): from isaaclab_arena.relations.relations import IsAnchor @@ -212,49 +209,6 @@ def test_static_initial_poses_reject_layout_missing_non_anchor(): ) -class _CompoundObject(DummyObject): - """A DummyObject that reports an auxiliary prim its per-env reset cannot reposition (like Droid's stand).""" - - def has_unplaced_auxiliary_prims(self) -> bool: - return True - - -def test_relation_placement_rejects_movable_asset_with_unplaced_auxiliary_prims(): - """A non-anchor compound asset (e.g. Droid's stand) must fail loudly, not orphan its aux prims.""" - from isaaclab_arena.environments.relation_solver_interface import _apply_relation_placement_result - from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams - from isaaclab_arena.relations.relations import On - - desk = _make_desk() - box = _CompoundObject( - name="box", - bounding_box=AxisAlignedBoundingBox(min_point=(0.0, 0.0, 0.0), max_point=(0.2, 0.2, 0.2)), - ) - box.add_relation(On(desk, clearance_m=0.01)) - - with pytest.raises(AssertionError, match="auxiliary scene prims"): - _apply_relation_placement_result( - assets=[desk, box], - placer_params=ObjectPlacerParams(resolve_on_reset=False), - placement_pool=_FakePlacementPool([_fallback_layout(positions={box: (0.1, 0.0, 0.2)})]), - num_envs=1, - ) - - -def test_anchor_asset_with_unplaced_auxiliary_prims_is_allowed(): - """An anchor compound asset does not move, so the auxiliary-prim guard must not fire.""" - from isaaclab_arena.environments.relation_solver_interface import _validate_no_unplaced_auxiliary_prims - from isaaclab_arena.relations.relations import IsAnchor - - anchor = _CompoundObject( - name="anchored_stand", - bounding_box=AxisAlignedBoundingBox(min_point=(0.0, 0.0, 0.0), max_point=(1.0, 1.0, 0.1)), - ) - anchor.add_relation(IsAnchor()) - # Anchors are excluded from the guard; this must not raise. - _validate_no_unplaced_auxiliary_prims([anchor], {anchor}) - - def test_set_initial_pose_create_reset_event_flag_controls_reset_event(): """create_reset_event=False sets the construction pose only; the default also registers the reset event.""" from isaaclab_arena.utils.pose import Pose diff --git a/isaaclab_arena/tests/test_robot_on_stand_utils.py b/isaaclab_arena/tests/test_robot_on_stand_utils.py new file mode 100644 index 0000000000..7754f9a5d9 --- /dev/null +++ b/isaaclab_arena/tests/test_robot_on_stand_utils.py @@ -0,0 +1,235 @@ +# Copyright (c) 2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for ``robot_on_stand_utils``: cached USD compose and runtime stand hierarchy.""" + +from __future__ import annotations + +import torch +import traceback +from pathlib import Path + +import pytest +import warp as wp + +from isaaclab_arena.tests.utils.subprocess import run_simulation_app_function + +_HEIGHT_ATOL = 1e-3 +_ROOT_WRITE_ATOL = 5e-3 +# Root-pose X delta used to verify ``write_root_pose_to_sim`` +_ROOT_WRITE_DELTA_X = 1.0 +_CUSTOM_DROID_STAND_HEIGHT_M = 2.0 + + +def _assert_compose_on_stand_usd( + robot_spec, + stand_spec, + *, + stand_height_m: float, + output_basename: str, + check_orient_180z: bool = False, +) -> str: + from pxr import Usd, UsdGeom + + from isaaclab_arena.embodiments.robot_on_stand_utils import compose_on_stand_usd + + usd_path = compose_on_stand_usd( + robot_spec, + stand_spec, + stand_height_m=stand_height_m, + output_basename=output_basename, + ) + assert Path(usd_path).is_file() + + stage = Usd.Stage.Open(usd_path) + assert stage is not None + stand_prim = stage.GetPrimAtPath(robot_spec.stand_prim_path) + assert stand_prim.IsValid(), f"missing stand prim at {robot_spec.stand_prim_path!r}" + robot_base = stage.GetPrimAtPath(robot_spec.robot_base_prim_path) + assert robot_base.IsValid(), f"missing robot base prim at {robot_spec.robot_base_prim_path!r}" + assert stand_prim.GetParent() == robot_base + + payload = stage.GetPrimAtPath(f"{robot_spec.stand_prim_path}/{stand_spec.payload_child_name}") + assert payload.IsValid(), f"missing payload child {stand_spec.payload_child_name!r}" + assert abs(_stand_world_height_m(stage, robot_spec.stand_prim_path) - stand_height_m) < _HEIGHT_ATOL + if check_orient_180z: + mesh_xf = UsdGeom.Xformable(payload).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + assert mesh_xf[0][0] < 0.0 and mesh_xf[1][1] < 0.0, mesh_xf + return usd_path + + +def _stand_world_height_m(stage, stand_prim_path: str) -> float: + from pxr import Usd, UsdGeom + + stand = stage.GetPrimAtPath(stand_prim_path) + assert stand.IsValid(), f"missing stand prim at {stand_prim_path!r}" + cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + return float(cache.ComputeWorldBound(stand).ComputeAlignedRange().GetSize()[2]) + + +def _stand_top_z(stage, stand_prim_path: str) -> float: + from pxr import Usd, UsdGeom + + stand = stage.GetPrimAtPath(stand_prim_path) + assert stand.IsValid(), f"missing stand prim at {stand_prim_path!r}" + cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + return float(cache.ComputeWorldBound(stand).ComputeAlignedRange().GetMax()[2]) + + +def _assert_prim_parent(prim_path: str, expected_parent_name: str) -> None: + import omni.usd + + stage = omni.usd.get_context().get_stage() + prim = stage.GetPrimAtPath(prim_path) + assert prim.IsValid(), f"missing prim at {prim_path}" + parent = prim.GetParent() + assert ( + parent and parent.GetName() == expected_parent_name + ), f"{prim_path} parent should be {expected_parent_name}, got {parent.GetPath() if parent else None}" + + +def _link0_world_xy(env) -> torch.Tensor: + robot = env.unwrapped.scene["robot"] + body_ids, _ = robot.find_bodies("panda_link0") + return wp.to_torch(robot.data.body_pos_w)[0, body_ids[0], :2].detach().cpu().float() + + +def _write_root_x_delta(env, delta_x: float) -> None: + robot = env.unwrapped.scene["robot"] + root_pose = wp.to_torch(robot.data.root_pose_w).clone() + root_pose[:, 0] += delta_x + env_ids = torch.arange(env.unwrapped.num_envs, device=env.unwrapped.device) + robot.write_root_pose_to_sim(root_pose, env_ids=env_ids) + robot.write_root_velocity_to_sim( + torch.zeros(env.unwrapped.num_envs, 6, device=env.unwrapped.device), env_ids=env_ids + ) + + +def _assert_root_write_moves_link0(env) -> None: + before = _link0_world_xy(env) + _write_root_x_delta(env, _ROOT_WRITE_DELTA_X) + env.unwrapped.sim.step(render=False) + after = _link0_world_xy(env) + assert abs(float(after[0] - before[0]) - _ROOT_WRITE_DELTA_X) < _ROOT_WRITE_ATOL, (before, after) + + +def _test_droid_on_stand_usd_compose(simulation_app) -> bool: + """Droid compose: height, align, orient, distinct paths per height, embodiment wiring.""" + from pxr import Usd + + from isaaclab_arena.embodiments.droid.droid import ( + _DROID_ROBOT_PRIM, + _DROID_STAND_PRIM, + DroidAbsoluteJointPositionEmbodiment, + ) + + try: + default_usd = _assert_compose_on_stand_usd( + _DROID_ROBOT_PRIM, + _DROID_STAND_PRIM, + stand_height_m=_DROID_STAND_PRIM.stand_default_height, + output_basename="droid_franka_robotiq_on_stand", + check_orient_180z=True, + ) + + custom_usd = _assert_compose_on_stand_usd( + _DROID_ROBOT_PRIM, + _DROID_STAND_PRIM, + stand_height_m=_CUSTOM_DROID_STAND_HEIGHT_M, + output_basename="droid_franka_robotiq_on_stand", + ) + assert custom_usd != default_usd + assert ( + abs( + _stand_top_z(Usd.Stage.Open(custom_usd), _DROID_ROBOT_PRIM.stand_prim_path) + - _stand_top_z(Usd.Stage.Open(default_usd), _DROID_ROBOT_PRIM.stand_prim_path) + ) + < _HEIGHT_ATOL + ) + + default_emb = DroidAbsoluteJointPositionEmbodiment() + custom_emb = DroidAbsoluteJointPositionEmbodiment(stand_height_m=_CUSTOM_DROID_STAND_HEIGHT_M) + assert not hasattr(default_emb.scene_config, "stand") + assert custom_emb.scene_config.robot.spawn.usd_path != default_emb.scene_config.robot.spawn.usd_path + assert abs(custom_emb.scene_config.robot.init_state.pos[2]) < 1e-6 + except Exception as e: + print(f"Error: {e}") + traceback.print_exc() + return False + return True + + +def _test_franka_on_stand_usd_compose(simulation_app) -> bool: + """Franka compose reaches the legacy default stand height.""" + from isaaclab_arena.embodiments.franka.franka import _FRANKA_ROBOT_PRIM, _FRANKA_STAND_PRIM + + try: + _assert_compose_on_stand_usd( + _FRANKA_ROBOT_PRIM, + _FRANKA_STAND_PRIM, + stand_height_m=_FRANKA_STAND_PRIM.stand_default_height, + output_basename="franka_panda_on_stand", + ) + except Exception as e: + print(f"Error: {e}") + traceback.print_exc() + return False + return True + + +def _test_droid_stand_and_externals_under_link0(simulation_app) -> bool: + """Droid runtime: stand and external cameras under ``panda_link0``; root write moves link0.""" + from isaaclab_arena.cli.isaaclab_arena_cli import arena_env_builder_cfg_from_argparse, get_isaaclab_arena_cli_parser + from isaaclab_arena.embodiments.droid.droid import DroidAbsoluteJointPositionEmbodiment + from isaaclab_arena.environments.arena_env_builder import ArenaEnvBuilder + from isaaclab_arena.environments.isaaclab_arena_environment import IsaacLabArenaEnvironment + from isaaclab_arena.scene.scene import Scene + + env = None + try: + arena_env = IsaacLabArenaEnvironment( + name="droid_stand_follow", + embodiment=DroidAbsoluteJointPositionEmbodiment(enable_cameras=True, stand_height_m=0.8), + scene=Scene(), + ) + args_cli = get_isaaclab_arena_cli_parser().parse_args(["--num_envs", "1", "--enable_cameras"]) + env = ArenaEnvBuilder(arena_env, arena_env_builder_cfg_from_argparse(args_cli)).make_registered() + env.reset() + env.unwrapped.sim.step(render=True) + + stand_path = "/World/envs/env_0/Robot/panda_link0/stand_instanceable" + cam_paths = ( + "/World/envs/env_0/Robot/panda_link0/external_camera", + "/World/envs/env_0/Robot/panda_link0/external_camera_2", + ) + _assert_prim_parent(stand_path, "panda_link0") + for path in cam_paths: + _assert_prim_parent(path, "panda_link0") + _assert_root_write_moves_link0(env) + _assert_prim_parent(stand_path, "panda_link0") + except Exception as e: + print(f"Error: {e}") + traceback.print_exc() + return False + finally: + if env is not None: + env.close() + return True + + +def test_droid_on_stand_usd_compose(): + result = run_simulation_app_function(_test_droid_on_stand_usd_compose) + assert result, f"Test {test_droid_on_stand_usd_compose.__name__} failed" + + +def test_franka_on_stand_usd_compose(): + result = run_simulation_app_function(_test_franka_on_stand_usd_compose) + assert result, f"Test {test_franka_on_stand_usd_compose.__name__} failed" + + +@pytest.mark.with_cameras +def test_droid_stand_and_externals_under_link0(): + result = run_simulation_app_function(_test_droid_stand_and_externals_under_link0, enable_cameras=True) + assert result, f"Test {test_droid_stand_and_externals_under_link0.__name__} failed"