From 8de97228f3e708289b3c14b208eb439d4daea722 Mon Sep 17 00:00:00 2001 From: Steven Zhang Date: Fri, 24 Jul 2026 14:01:53 -0400 Subject: [PATCH] Add vial plate testing environment --- .../matterix_assets/labware/__init__.py | 2 + .../matterix_assets/labware/vialplates.py | 49 ++ .../matterix_assets/labware/vials.py | 37 ++ .../test/test_promoted_vialplate_assets.py | 75 +++ .../matterix_tasks/test_dev_tasks/__init__.py | 11 +- .../test_dev_tasks/test_franka_vialplate.py | 461 ++++++++++++++++++ 6 files changed, 634 insertions(+), 1 deletion(-) create mode 100644 source/matterix_assets/matterix_assets/labware/vialplates.py create mode 100644 source/matterix_assets/matterix_assets/labware/vials.py create mode 100644 source/matterix_assets/test/test_promoted_vialplate_assets.py create mode 100644 source/matterix_tasks/matterix_tasks/test_dev_tasks/test_franka_vialplate.py diff --git a/source/matterix_assets/matterix_assets/labware/__init__.py b/source/matterix_assets/matterix_assets/labware/__init__.py index 4a53289..2588677 100644 --- a/source/matterix_assets/matterix_assets/labware/__init__.py +++ b/source/matterix_assets/matterix_assets/labware/__init__.py @@ -8,3 +8,5 @@ ## from .beakers import * +from .vials import * +from .vialplates import * diff --git a/source/matterix_assets/matterix_assets/labware/vialplates.py b/source/matterix_assets/matterix_assets/labware/vialplates.py new file mode 100644 index 0000000..2c563c7 --- /dev/null +++ b/source/matterix_assets/matterix_assets/labware/vialplates.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2026, The Matterix Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration and data paths for the promoted 3_5 vial holder.""" + +import json +from pathlib import Path + +from isaaclab.sensors import OffsetCfg +from isaaclab.utils import configclass + +from matterix_assets import MATTERIX_ASSETS_DATA_DIR + +from ..matterix_rigid_object import MatterixRigidObjectCfg + +VIALPLATE_3_5_DATA_DIR = f"{MATTERIX_ASSETS_DATA_DIR}/labware/vialplate_3_5" +VIALPLATE_3_5_USD_PATH = f"{VIALPLATE_3_5_DATA_DIR}/3_5_vialplate_free_standing_frames.usda" +VIALPLATE_3_5_FRAME_CONTRACT_PATH = f"{VIALPLATE_3_5_DATA_DIR}/holder-hole-frame-contract.json" + + +def _load_holder_hole_frames() -> dict[str, OffsetCfg]: + """Load the public 15-hole frame family from the promoted contract.""" + contract = json.loads(Path(VIALPLATE_3_5_FRAME_CONTRACT_PATH).read_text(encoding="utf-8")) + frames = contract.get("frames") + if not isinstance(frames, list) or len(frames) != 15: + raise ValueError("promoted vial-holder frame contract must contain exactly 15 frames") + orientation = tuple(float(value) for value in contract["frame_orientation_wxyz"]) + return { + frame["name"]: OffsetCfg(pos=tuple(float(value) for value in frame["position_m"]), rot=orientation) + for frame in frames + } + + +VIALPLATE_3_5_HOLE_FRAMES = _load_holder_hole_frames() + + +@configclass +class VIALPLATE_3_5_CFG(MatterixRigidObjectCfg): + """Dynamic 15-well holder with the promoted frame-bearing USD payload.""" + + prim_path = "{ENV_REGEX_NS}/RigidObjects_Labware" + usd_path = VIALPLATE_3_5_USD_PATH + scale = (1.0, 1.0, 1.0) + mass = 0.070350472 + activate_contact_sensors = True + frames = VIALPLATE_3_5_HOLE_FRAMES + semantic_tags = [("class", "vial_holder"), ("asset", "3_5_vialplate")] diff --git a/source/matterix_assets/matterix_assets/labware/vials.py b/source/matterix_assets/matterix_assets/labware/vials.py new file mode 100644 index 0000000..c1ba696 --- /dev/null +++ b/source/matterix_assets/matterix_assets/labware/vials.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2026, The Matterix Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +# Configuration for the self-modelled Fisherbrand 03-339-21F reference vial. + +from isaaclab.sensors import OffsetCfg +from isaaclab.utils import configclass + +from matterix_assets import MATTERIX_ASSETS_DATA_DIR + +from ..matterix_rigid_object import MatterixRigidObjectCfg + +FISHERBRAND_03_339_21F_DATA_DIR = ( + f"{MATTERIX_ASSETS_DATA_DIR}/labware/fisherbrand_03-339-21f" +) +FISHERBRAND_03_339_21F_USD_PATH = ( + f"{FISHERBRAND_03_339_21F_DATA_DIR}/fisherbrand_03-339-21f_z_up_fixed.usda" +) +FISHERBRAND_03_339_21F_FRAME_OFFSETS = { + "grasp": OffsetCfg(pos=(0.0, 0.0, 0.0649)), + "pre_grasp": OffsetCfg(pos=(0.0, 0.0, 0.1649)), + "post_grasp": OffsetCfg(pos=(0.0, 0.0, 0.1649)), +} + + +@configclass +class FISHERBRAND_03_339_21F_CFG(MatterixRigidObjectCfg): + # Dynamic closed Fisherbrand 03-339-21F vial configuration. + + prim_path = "{ENV_REGEX_NS}/RigidObjects_Labware" + usd_path = FISHERBRAND_03_339_21F_USD_PATH + scale = (1.0, 1.0, 1.0) + mass = 0.017734 + activate_contact_sensors = True + frames = FISHERBRAND_03_339_21F_FRAME_OFFSETS + semantic_tags = [("class", "vial"), ("asset", "fisherbrand_03-339-21f")] diff --git a/source/matterix_assets/test/test_promoted_vialplate_assets.py b/source/matterix_assets/test/test_promoted_vialplate_assets.py new file mode 100644 index 0000000..33f2f6b --- /dev/null +++ b/source/matterix_assets/test/test_promoted_vialplate_assets.py @@ -0,0 +1,75 @@ +# Copyright (c) 2022-2026, The Matterix Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Static checks for the promoted vial and holder payloads.""" + +import hashlib +import json +from pathlib import Path + + +MATTERIX_ROOT = Path(__file__).resolve().parents[3] +DATA_ROOT = MATTERIX_ROOT / "source/matterix_assets/data/labware" +VIAL_ROOT = DATA_ROOT / "fisherbrand_03-339-21f" +HOLDER_ROOT = DATA_ROOT / "vialplate_3_5" +TASK_PATH = MATTERIX_ROOT / "source/matterix_tasks/matterix_tasks/test_dev_tasks/test_franka_vialplate.py" + + +def test_promoted_payloads_have_complete_static_contract(): + """Verify files, frame count, hashes, licenses, and official-only task paths.""" + required_files = [ + VIAL_ROOT / "fisherbrand_03-339-21f_z_up_fixed.usda", + VIAL_ROOT / "files/fisherbrand_03-339-21f_z_up_fixed_mesh.usda", + VIAL_ROOT / "files/Fisherbrand_Vial_Z_UP_FIXED.usdc", + VIAL_ROOT / "provenance/LICENSE.asset.txt", + VIAL_ROOT / "provenance/NOTICE.md", + VIAL_ROOT / "provenance/provenance.yaml", + HOLDER_ROOT / "3_5_vialplate_free_standing.usda", + HOLDER_ROOT / "3_5_vialplate_free_standing_frames.usda", + HOLDER_ROOT / "files/3_5_vialplate_free_standing_mesh.usda", + HOLDER_ROOT / "holder-hole-frame-contract.json", + HOLDER_ROOT / "provenance/LICENSE.asset.txt", + HOLDER_ROOT / "provenance/NOTICE.md", + HOLDER_ROOT / "provenance/asset_metadata.yaml", + HOLDER_ROOT / "provenance/license_record_draft.txt", + ] + missing = [str(path) for path in required_files if not path.is_file()] + assert not missing, f"missing promoted payload files: {missing}" + + contract_path = HOLDER_ROOT / "holder-hole-frame-contract.json" + contract = json.loads(contract_path.read_text(encoding="utf-8")) + frames = contract["frames"] + assert len(frames) == 15 + assert len({frame["name"] for frame in frames}) == 15 + selection = contract["initial_dynamic_vial_set"] + assert selection["pick_vial"] == "hole_middle_center" + assert len(selection["witness_vials"]) == 3 + + metadata = (HOLDER_ROOT / "provenance/asset_metadata.yaml").read_text(encoding="utf-8") + assert "original_license: Public Domain" in metadata + assert "package_license: CC0-1.0-Universal" in metadata + assert hashlib.sha256(contract_path.read_bytes()).hexdigest() in metadata + + holder_license = (HOLDER_ROOT / "provenance/license_record_draft.txt").read_text(encoding="utf-8") + assert "https://3d.nih.gov/entries/3DPX-000429" in holder_license + assert "Public Domain" in holder_license + assert "CC BY 4.0" not in holder_license + assert "Pending" not in holder_license + + vial_provenance = (VIAL_ROOT / "provenance/provenance.yaml").read_text(encoding="utf-8") + assert "asset_status: promoted_to_official_matterix_data" in vial_provenance + assert "canonical_visual_source_relative_path: not_packaged_in_official_data" in vial_provenance + assert "creation_method: user_authored_self_modelled_from_official_specification_and_dimensions" in vial_provenance + assert "license_status: CC0-1.0-Universal" in vial_provenance + assert "manufacturer_cad_or_texture_copied: false" in vial_provenance + + vial_notice = (VIAL_ROOT / "provenance/NOTICE.md").read_text(encoding="utf-8") + assert "This promoted Matterix asset" in vial_notice + assert "The candidate is" not in vial_notice + + task = TASK_PATH.read_text(encoding="utf-8") + assert "MATTERIX_PHASE_B_ASSETS_ROOT" not in task + assert "MATTERIX_VIAL_USD" not in task + assert "asset_workbench/phase_b_candidates" not in task diff --git a/source/matterix_tasks/matterix_tasks/test_dev_tasks/__init__.py b/source/matterix_tasks/matterix_tasks/test_dev_tasks/__init__.py index e21147c..07bd20b 100644 --- a/source/matterix_tasks/matterix_tasks/test_dev_tasks/__init__.py +++ b/source/matterix_tasks/matterix_tasks/test_dev_tasks/__init__.py @@ -6,7 +6,7 @@ import gymnasium as gym import os -from . import test_franka_beaker_lift, test_franka_beakers, test_particle_systems, test_semantics_heat_transfer +from . import test_franka_beaker_lift, test_franka_beakers, test_franka_vialplate, test_particle_systems, test_semantics_heat_transfer ## # Register Gym environments. @@ -39,6 +39,15 @@ disable_env_checker=True, ) +gym.register( + id="Matterix-Test-Vialplate-Franka-v1", + entry_point="matterix.envs:MatterixBaseEnv", + kwargs={ + "env_cfg_entry_point": test_franka_vialplate.FrankaVialplateEnvTestCfg, + }, + disable_env_checker=True, +) + gym.register( id="Matterix-Test-Semantics-Heat-Transfer-Franka-v1", entry_point="matterix.envs:MatterixBaseEnv", diff --git a/source/matterix_tasks/matterix_tasks/test_dev_tasks/test_franka_vialplate.py b/source/matterix_tasks/matterix_tasks/test_dev_tasks/test_franka_vialplate.py new file mode 100644 index 0000000..b6691b2 --- /dev/null +++ b/source/matterix_tasks/matterix_tasks/test_dev_tasks/test_franka_vialplate.py @@ -0,0 +1,461 @@ +# Copyright (c) 2022-2026, The Matterix Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Dynamic Reference-Vial task using promoted Matterix labware. + +The scene uses the promoted Fisherbrand 03-339-21F asset rather +than colour-coded cylinders. Every Vial is a gravity-enabled rigid body. Its +initial root pose is derived from the canonical holder-hole contract, with a +small clearance so PhysX—not a kinematic constraint—performs the final settle. + +Set MATTERIX_VIAL_HOLDER_FRAME_DEBUG=1 to render the holder's physical opening +frames during a GUI or WebRTC review. +""" + +from __future__ import annotations + +import copy +import json +import os +from pathlib import Path + +import isaaclab.sim as sim_utils +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.sensors import OffsetCfg +from isaaclab.utils import configclass + +from matterix.envs import mdp +from matterix_assets.labware.vials import ( + FISHERBRAND_03_339_21F_CFG, + FISHERBRAND_03_339_21F_FRAME_OFFSETS, + FISHERBRAND_03_339_21F_USD_PATH, +) +from matterix_assets.labware.vialplates import ( + VIALPLATE_3_5_CFG, + VIALPLATE_3_5_FRAME_CONTRACT_PATH, + VIALPLATE_3_5_USD_PATH, +) +from matterix.managers.semantics.primitive_semantics import IsInContactPhysicsCfg +from matterix_sm import MoveRelativeCfg, MoveToFrameCfg, OpenGripperCfg, PickObjectCfg +from matterix_sm.robot_action_spaces import FRANKA_IK_ACTION_SPACE + +from .test_franka_beaker_lift import FrankaBeakerLiftEnvTestCfg + + +OFFICIAL_HOLDER_USD_PATH = Path(VIALPLATE_3_5_USD_PATH) +OFFICIAL_VIAL_USD_PATH = Path(FISHERBRAND_03_339_21F_USD_PATH) +OFFICIAL_FRAME_CONTRACT_PATH = Path(VIALPLATE_3_5_FRAME_CONTRACT_PATH) +FRAME_DEBUG_ENVIRONMENT_VARIABLE = "MATTERIX_VIAL_HOLDER_FRAME_DEBUG" +HOLDER_INITIAL_POS = (0.65, -0.18, -0.001) # 2 mm above the measured -3 mm Seattle-table support surface. +VIAL_CONTACT_OFFSET_M = 1.0e-6 +VIAL_CONTACT_FILTERS = [ + "robot/panda_leftfinger", + "robot/panda_rightfinger", + "vial_holder", + "witness_vial_middle_mid_right", + "witness_vial_middle_mid_left", + "witness_vial_middle_left", +] +VIAL_INITIAL_SETTLING_CLEARANCE_M = 0.002 +# The free-standing holder settles a fraction of a millimetre on the table +# before the vial reaches its floor. Keep the measured correction explicit. +VIAL_INITIAL_XY_MAPPING_M = (-0.00008, 0.00039) +# The holder/table contact stack needs the same 240 Hz physics cadence as the +# standalone qualification harness; 60 Hz produces measurable penetration and tilt. +VIALPLATE_PHYSICS_DT = 1.0 / 240.0 +HOLDER_PLACE_FRAME_Z_M = 0.00835 + FISHERBRAND_03_339_21F_FRAME_OFFSETS["grasp"].pos[2] +HOLDER_PRE_PLACE_FRAME_Z_M = HOLDER_PLACE_FRAME_Z_M + 0.1 +HOLDER_FRAME_IDENTITY_QUAT = (1.0, 0.0, 0.0, 0.0) +# The elevated view keeps the Franka/table context while making the holder +# and hole alignment legible. +REVIEW_VIEWER_EYE = (0.40, -0.05, 0.60) +REVIEW_VIEWER_LOOKAT = (0.65, -0.18, 0.04) + + +def _official_asset_paths() -> tuple[Path, Path, Path]: + # Locate the promoted holder, vial, and canonical frame contract. + holder_usd = OFFICIAL_HOLDER_USD_PATH + vial_usd = OFFICIAL_VIAL_USD_PATH + frame_contract = OFFICIAL_FRAME_CONTRACT_PATH + missing = [str(path) for path in (holder_usd, vial_usd, frame_contract) if not path.is_file()] + if missing: + raise FileNotFoundError("Missing promoted Matterix asset file(s): " + ", ".join(missing)) + return holder_usd, vial_usd, frame_contract + + +def _load_holder_hole_contract(path: Path) -> tuple[dict[str, OffsetCfg], str, tuple[str, ...], dict[str, object]]: + """Return all physical hole frames and the single source of vial role selection.""" + contract = json.loads(path.read_text(encoding="utf-8")) + frames = contract.get("frames") + orientation = contract.get("frame_orientation_wxyz") + selection = contract.get("initial_dynamic_vial_set") + if not isinstance(frames, list) or len(frames) != 15: + raise ValueError("holder-hole contract must declare exactly 15 frames") + if orientation != [1.0, 0.0, 0.0, 0.0]: + raise ValueError("holder-hole frames must be identity-oriented in holder coordinates") + if not isinstance(selection, dict): + raise ValueError("holder-hole contract is missing its Dynamic Vial Set") + + offsets: dict[str, OffsetCfg] = {} + for frame in frames: + if not isinstance(frame, dict): + raise ValueError("holder-hole frame must be an object") + name = frame.get("name") + position = frame.get("position_m") + if not isinstance(name, str) or not isinstance(position, list) or len(position) != 3: + raise ValueError(f"malformed holder-hole frame: {frame!r}") + if name in offsets: + raise ValueError(f"duplicate holder-hole frame: {name}") + offsets[name] = OffsetCfg(pos=tuple(float(value) for value in position), rot=tuple(orientation)) + + pick = selection.get("pick_vial") + witnesses = selection.get("witness_vials") + if not isinstance(pick, str) or not isinstance(witnesses, list) or not all(isinstance(name, str) for name in witnesses): + raise ValueError("holder-hole Dynamic Vial Set is malformed") + if len(witnesses) != 3 or len(set((pick, *witnesses))) != 4: + raise ValueError("holder-hole Dynamic Vial Set must contain one distinct pick and three witnesses") + if pick not in offsets or not set(witnesses).issubset(offsets): + raise ValueError("holder-hole Dynamic Vial Set names are absent from the frame grid") + return offsets, pick, tuple(witnesses), contract + + +def _vial_object_name(hole_name: str, *, pick: bool) -> str: + """Make role-bearing names without encoding any world-space coordinates.""" + if pick: + return "pick_vial" + return "witness_vial_" + hole_name.removeprefix("hole_") + + +def _dynamic_vial_layout(frame_contract_path: Path) -> tuple[dict[str, str], dict[str, tuple[float, float, float]]]: + """Derive role names and initial Vial roots from holder opening frames. + + The root is intentionally two millimetres above its nominal 30 mm insertion + depth. That clearance proves that the observed seating comes from gravity + and holder collision, instead of an attached or kinematic presentation rig. + """ + offsets, pick, witnesses, contract = _load_holder_hole_contract(frame_contract_path) + heights = contract.get("reference_heights_m") + if not isinstance(heights, dict): + raise ValueError("holder-hole contract is missing reference heights") + try: + opening_plane = float(heights["opening_plane"]) + nominal_vial_bottom = float(heights["nominal_vial_bottom_at_30mm_insertion"]) + except (KeyError, TypeError, ValueError) as error: + raise ValueError("holder-hole contract has malformed vial seating heights") from error + if nominal_vial_bottom >= opening_plane: + raise ValueError("nominal Vial bottom must be below the holder opening plane") + + role_to_hole = {_vial_object_name(pick, pick=True): pick} + role_to_hole.update({_vial_object_name(hole, pick=False): hole for hole in witnesses}) + positions: dict[str, tuple[float, float, float]] = {} + for role_name, hole_name in role_to_hole.items(): + opening = offsets[hole_name].pos + positions[role_name] = ( + HOLDER_INITIAL_POS[0] + opening[0] + VIAL_INITIAL_XY_MAPPING_M[0], + HOLDER_INITIAL_POS[1] + opening[1] + VIAL_INITIAL_XY_MAPPING_M[1], + HOLDER_INITIAL_POS[2] + opening[2] - opening_plane + nominal_vial_bottom + VIAL_INITIAL_SETTLING_CLEARANCE_M, + ) + return role_to_hole, positions + + +def _holder_placement_frame_offsets(hole_offsets: dict[str, OffsetCfg]) -> dict[str, OffsetCfg]: + """Create robot grasping-frame targets above each physical holder opening.""" + placement_frames: dict[str, OffsetCfg] = {} + for hole_name, hole_offset in hole_offsets.items(): + row_column = hole_name.removeprefix("hole_") + x, y, _ = hole_offset.pos + placement_frames[f"place_{row_column}"] = OffsetCfg( + pos=(x, y, HOLDER_PLACE_FRAME_Z_M), + rot=HOLDER_FRAME_IDENTITY_QUAT, + ) + placement_frames[f"pre_place_{row_column}"] = OffsetCfg( + pos=(x, y, HOLDER_PRE_PLACE_FRAME_Z_M), + rot=HOLDER_FRAME_IDENTITY_QUAT, + ) + return placement_frames + + +def _debug_frames_enabled() -> bool: + return os.environ.get(FRAME_DEBUG_ENVIRONMENT_VARIABLE, "").strip().lower() in {"1", "true", "yes", "on"} + + +@configclass +class FreeStandingVialHolderCfg(VIALPLATE_3_5_CFG): + """One dynamic holder with the canonical 15-hole FrameTransformer sensors.""" + + frame_contract_path: str = "" + debug_frame_vis: bool = False + pick_vial_hole_name: str = "" + witness_vial_hole_names: tuple[str, ...] = () + + def __post_init__(self): + offsets, pick, witnesses, _ = _load_holder_hole_contract(Path(self.frame_contract_path)) + # MatterixRigidObjectCfg turns every offset into a FrameTransformer + # named {frame_name}_{asset_name}; keep the physical hole API and add + # grasping-frame placement targets as separate, explicit names. + self.frames = {**offsets, **_holder_placement_frame_offsets(offsets)} + self.sensors = {} + self.pick_vial_hole_name = pick + self.witness_vial_hole_names = witnesses + super().__post_init__() + self.spawn.rigid_props = sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + solver_position_iteration_count=8, + solver_velocity_iteration_count=2, + sleep_threshold=0.01, + stabilization_threshold=0.01, + max_depenetration_velocity=1.0, + linear_damping=0.05, + angular_damping=0.1, + ) + for frame_name, sensor_cfg in self.sensors.items(): + sensor_cfg.debug_vis = self.debug_frame_vis + # Isaac Lab's default frame marker is intentionally large (30 mm in + # MatterixRigidObjectCfg). That is useful for a robot workspace, + # but it is larger than the 21 mm vial bore, so the arrow tips look + # displaced from a correctly centred hole. Give this diagnostic + # task a private, compact marker config and an explicit path per + # hole. The frame origin remains the FrameTransformer target; + # only the inspection glyph is being changed. + visualizer_cfg = copy.deepcopy(sensor_cfg.visualizer_cfg) + visualizer_cfg.prim_path = f"/Visuals/VialHolderFrames/{frame_name}" + visualizer_cfg.markers["frame"].scale = (0.012, 0.012, 0.012) + visualizer_cfg.markers["connecting_line"].radius = 0.0004 + sensor_cfg.visualizer_cfg = visualizer_cfg + + +@configclass +class ReferenceVialCfg(FISHERBRAND_03_339_21F_CFG): + """Dynamic closed Reference Vial with Franka cap-grasp frames.""" + + def __post_init__(self): + self.frames = {**self.frames, **FISHERBRAND_03_339_21F_FRAME_OFFSETS} + self.sensors = {} + self.semantics = [IsInContactPhysicsCfg(filter_prim_paths_expr=VIAL_CONTACT_FILTERS)] + super().__post_init__() + # Match the staged-candidate drop test: dynamic gravity, conservative + # contact offsets, high enough solver iterations, and damping for the + # narrow 0.25 mm radial holder clearance. Nothing is kinematic. + self.spawn = sim_utils.UsdFileCfg( + usd_path=self.usd_path, + mass_props=sim_utils.MassPropertiesCfg(mass=self.mass), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + solver_position_iteration_count=16, + solver_velocity_iteration_count=4, + sleep_threshold=0.1, + stabilization_threshold=0.1, + max_depenetration_velocity=1.0, + linear_damping=0.5, + angular_damping=0.5, + ), + collision_props=sim_utils.CollisionPropertiesCfg( + contact_offset=VIAL_CONTACT_OFFSET_M, + rest_offset=0.0, + ), + scale=self.scale, + activate_contact_sensors=True, + ) + + +@configclass +class VialplateObservationManagerCfg: + """Robot state plus holder/Vial state required by the workflow.""" + + @configclass + class ArticulationsGroup(ObsGroup): + robot__root_world_pos = ObsTerm(func=mdp.root_world_pos, params={"asset_name": "robot"}) + robot__root_world_quat = ObsTerm(func=mdp.root_world_quat, params={"asset_name": "robot"}) + robot__joint_pos = ObsTerm(func=mdp.joint_pos, params={"asset_name": "robot"}) + robot__joint_vel = ObsTerm(func=mdp.joint_vel, params={"asset_name": "robot"}) + robot__ee_world_pos = ObsTerm(func=mdp.ee_world_pos, params={"asset_name": "robot"}) + robot__ee_world_quat = ObsTerm(func=mdp.ee_world_quat, params={"asset_name": "robot"}) + robot__gripper_pos = ObsTerm(func=mdp.gripper_pos, params={"asset_name": "robot"}) + robot__grasping_frame_world_pos = ObsTerm( + func=mdp.frame_world_pos, params={"asset_name": "robot", "frame_name": "grasping_frame"} + ) + robot__grasping_frame_world_quat = ObsTerm( + func=mdp.frame_world_quat, params={"asset_name": "robot", "frame_name": "grasping_frame"} + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class RigidObjectsGroup(ObsGroup): + vial_holder__object_world_pos = ObsTerm(func=mdp.object_world_pos, params={"asset_name": "vial_holder"}) + vial_holder__object_world_quat = ObsTerm(func=mdp.object_world_quat, params={"asset_name": "vial_holder"}) + vial_holder__hole_middle_center_frame = ObsTerm( + func=mdp.frame_world_pose, params={"asset_name": "vial_holder", "frame_name": "hole_middle_center"} + ) + vial_holder__pre_place_middle_center_frame = ObsTerm( + func=mdp.frame_world_pose, params={"asset_name": "vial_holder", "frame_name": "pre_place_middle_center"} + ) + vial_holder__place_middle_center_frame = ObsTerm( + func=mdp.frame_world_pose, params={"asset_name": "vial_holder", "frame_name": "place_middle_center"} + ) + vial_holder__object_lin_vel = ObsTerm(func=mdp.object_lin_vel, params={"asset_name": "vial_holder"}) + vial_holder__object_ang_vel = ObsTerm(func=mdp.object_ang_vel, params={"asset_name": "vial_holder"}) + pick_vial__object_world_pos = ObsTerm(func=mdp.object_world_pos, params={"asset_name": "pick_vial"}) + pick_vial__object_world_quat = ObsTerm(func=mdp.object_world_quat, params={"asset_name": "pick_vial"}) + pick_vial__pre_grasp_frame = ObsTerm( + func=mdp.frame_world_pose, params={"asset_name": "pick_vial", "frame_name": "pre_grasp"} + ) + pick_vial__grasp_frame = ObsTerm( + func=mdp.frame_world_pose, params={"asset_name": "pick_vial", "frame_name": "grasp"} + ) + pick_vial__post_grasp_frame = ObsTerm( + func=mdp.frame_world_pose, params={"asset_name": "pick_vial", "frame_name": "post_grasp"} + ) + pick_vial__object_lin_vel = ObsTerm(func=mdp.object_lin_vel, params={"asset_name": "pick_vial"}) + witness_vial_middle_mid_right__object_world_pos = ObsTerm( + func=mdp.object_world_pos, params={"asset_name": "witness_vial_middle_mid_right"} + ) + witness_vial_middle_mid_left__object_world_pos = ObsTerm( + func=mdp.object_world_pos, params={"asset_name": "witness_vial_middle_mid_left"} + ) + witness_vial_middle_left__object_world_pos = ObsTerm( + func=mdp.object_world_pos, params={"asset_name": "witness_vial_middle_left"} + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + articulations: ArticulationsGroup = ArticulationsGroup() + rigid_objects: RigidObjectsGroup = RigidObjectsGroup() + + +@configclass +class FrankaVialplateEnvTestCfg(FrankaBeakerLiftEnvTestCfg): + """Franka/table scene with one dynamic holder and four real dynamic Vials.""" + + pick_vial_hole_name: str = "" + witness_vial_hole_names: tuple[str, ...] = () + vial_object_to_hole: dict[str, str] = {} + vial_initial_positions: dict[str, tuple[float, float, float]] = {} + + def __post_init__(self): + super().__post_init__() + # The inherited Franka IK action controls a virtual frame 107 mm from + # panda_hand, while the shared diagnostic sensors use 103.4 mm. Keep + # this task observed frame aligned with the actual IK target so the + # 1 mm manipulation gates measure the commanded frame rather than a + # 3.6 mm bookkeeping offset. + self.articulated_assets = dict(self.articulated_assets) + robot = copy.deepcopy(self.articulated_assets["robot"]) + robot.sensors["ee_frame"].target_frames[0].offset = OffsetCfg(pos=(0.0, 0.0, 0.107)) + robot.sensors["grasping_frame"].target_frames[0].offset = OffsetCfg( + pos=(0.0, 0.0, 0.107), rot=(0.0, 1.0, 0.0, 0.0) + ) + self.articulated_assets["robot"] = robot + self.sim.dt = VIALPLATE_PHYSICS_DT + _, _, frame_contract = _official_asset_paths() + holder = FreeStandingVialHolderCfg( + frame_contract_path=str(frame_contract), + pos=HOLDER_INITIAL_POS, + activate_contact_sensors=True, + debug_frame_vis=_debug_frames_enabled(), + semantic_tags=[("class", "vial_holder")], + ) + vial_object_to_hole, vial_initial_positions = _dynamic_vial_layout(frame_contract) + + self.objects = dict(self.objects) + self.objects.pop("beaker", None) + self.objects["vial_holder"] = holder + for vial_name, initial_pos in vial_initial_positions.items(): + self.objects[vial_name] = ReferenceVialCfg( + pos=initial_pos, + activate_contact_sensors=True, + semantic_tags=[("class", "vial")], + ) + + # The inherited beaker-only reset and workflow are replaced by the + # dynamic four-Vial pick-and-return scene. + self.events.randomize_beaker_position = None + # This qualification task is driven by headless workflow diagnostics; + # do not contend with the parent beaker test recorder path. + self.record_path = None + self.observations = VialplateObservationManagerCfg() + self.pick_vial_hole_name = holder.pick_vial_hole_name + self.witness_vial_hole_names = holder.witness_vial_hole_names + self.vial_object_to_hole = vial_object_to_hole + self.vial_initial_positions = vial_initial_positions + pick_hole_suffix = self.pick_vial_hole_name.removeprefix("hole_") + # Provisional evidence route: incremental insertion is used because the + # nominal one-jump PlaceObject route times out against the measured + # 0.25 mm dynamic vial/well clearance. Keep this route until the + # clearance and collision policy are confirmed; it is not Ticket 10 + # acceptance by itself. + pick_action = PickObjectCfg( + description="Pick the Reference Vial by its cap", + agent_assets="robot", + object="pick_vial", + post_grasp_offset=(0.0, 0.0, 0.02), + action_space_info=FRANKA_IK_ACTION_SPACE, + ) + staged_lift_actions = [ + MoveRelativeCfg( + agent_assets="robot", + position_offset=(0.0, 0.0, 0.02), + orientation_offset=None, + position_threshold=0.001, + orientation_threshold=0.02, + settling_time=0.05, + action_space_info=FRANKA_IK_ACTION_SPACE, + ) + for _ in range(4) + ] + pre_place_action = MoveToFrameCfg( + object="vial_holder", + frame=f"pre_place_{pick_hole_suffix}", + agent_assets="robot", + position_threshold=0.001, + orientation_threshold=0.02, + settling_time=0.05, + use_frame_orientation=False, + action_space_info=FRANKA_IK_ACTION_SPACE, + ) + insertion_actions = [ + MoveRelativeCfg( + agent_assets="robot", + position_offset=(0.0, 0.0, -0.02), + orientation_offset=None, + position_threshold=0.001, + orientation_threshold=0.02, + settling_time=0.05, + action_space_info=FRANKA_IK_ACTION_SPACE, + ) + for _ in range(5) + ] + release_action = OpenGripperCfg( + agent_assets="robot", + action_space_info=FRANKA_IK_ACTION_SPACE, + ) + retreat_action = MoveRelativeCfg( + agent_assets="robot", + position_offset=(0.0, 0.0, 0.1), + orientation_offset=None, + position_threshold=0.01, + orientation_threshold=0.02, + settling_time=0.05, + action_space_info=FRANKA_IK_ACTION_SPACE, + ) + self.workflows = { + "pick_return_vial": [ + pick_action, + *staged_lift_actions, + pre_place_action, + *insertion_actions, + release_action, + retreat_action, + ] + } + self.viewer.eye = REVIEW_VIEWER_EYE + self.viewer.lookat = REVIEW_VIEWER_LOOKAT