diff --git a/.gitignore b/.gitignore index 31a3a87..a19c34d 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ __pycache__/ .pytest_cache/ .ruff_cache/ .mypy_cache/ +.cache/ .coverage htmlcov/ build/ @@ -13,7 +14,14 @@ ros2_ws/log/ dist/ *.egg-info/ outputs/ +artifacts/ +log/ .radcounter/ *.log +runtime/llm/bin/*/ +runtime/llm/models/*.gguf +runtime/llm/logs/ +!runtime/llm/bin/.gitkeep +!runtime/llm/models/.gitkeep .idea/ .vscode/ diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..036e480 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,19 @@ +# RadCounterSim repository instructions + +## Surface sources and decontamination + +- Before creating or modifying a surface/planar source, its visualization, or a + decontamination workflow, read and follow + `docs/decontamination-authoring-rules.md`. +- The August 6, 2026 irregular-source and wall-decontamination videos named in + that document are the canonical behavior and appearance references. Do not + substitute a tidy framed panel, a uniformly filled rectangle, or a small set + of hand-arranged polygons. +- A change to surface-source generation or decontamination behavior is incomplete + unless the implementation, regression tests, and + `docs/decontamination-authoring-rules.md` are updated together. If behavior is + intentionally unchanged, the rule document need not be edited, but the change + must retain its invariants and tests. +- Validation must operate on the same irregular activity-bearing geometry shown + to the user. Never decontaminate a hidden regular proxy while displaying an + unrelated irregular overlay. diff --git a/README.md b/README.md index 26c8f39..9e45d5a 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,22 @@ and shared by rendering, PhysX collision, and Embree attenuation. Proprietary formats can use a no-shell external converter or an importer plugin. See `docs/environment-import.md`; CAD tessellation uses the `cad` uv group. +Complete runtime compositions are selected independently from a system catalog: + +```bash +uv run radcounter-system list +uv run radcounter-system activate --profile fukushima-packbot +radcounter-app +uv run radcounter-system activate --profile vertical-slice +``` + +The Operations window exposes the same catalog as ordinary preset, +environment, robot, and detector selectors, so no command or file path is +required for normal switching. The LLM instruction area remains a separate +robot-task control. The Fukushima Daiichi profile fetches the pinned CC BY 4.0 +SolidWorks source and converts it directly to USD on Linux with Isaac Sim's +bundled HOOPS converter; see `docs/system-profiles.md`. + The experiment command writes the required manifest, resolved configuration, JSONL events, Parquet tables, metrics, NPZ maps, snapshots directory, and HTML report under `outputs//_/`. @@ -62,6 +78,61 @@ Isaac Sim requires the user to review and accept NVIDIA's Omniverse EULA. The launch script never accepts it implicitly. After acceptance, launch with `OMNI_KIT_ACCEPT_EULA=YES ./scripts/run_isaac.sh`. +## Articulated GUI workflow + +The GUI workflow loads NVIDIA's Clearpath Ridgeback + Franka Panda and Nova +Carter assets. It drives real articulation and wheel joints, solves the +seven-axis arm with Lula IK, closes the physical gripper before attaching a +payload constraint, and performs contact-driven decontamination. Shield +placement/correction, contaminated-drum relocation/disposal, and obstacle +relocation all use the same base-arm-gripper sequence; operation-time USD pose +teleports are prohibited. + +```bash +export OMNI_KIT_ACCEPT_EULA=YES RADCOUNTER_HOST_ENV_NO_ROS=1 +source scripts/host_env.sh +uv run --project "$RADCOUNTER_ISAAC_ROOT" --locked python scripts/run_gui.py +``` + +Use `--headless --no-keep-open --phase-hold-s 0` for a noninteractive gate. The +complete public audit is written to `artifacts/gui-validation/latest.json`. +The visible GUI is capped at 60 FPS by default to avoid consuming a full GPU +while idle. Override it with `--max-fps 30`, or use `--max-fps 0` to remove the +cap. + +The viewport keeps one active robot explicit in a top status bar and provides +one-click follow/onboard views, a 12 FPS building overview, through-wall robot +beacons, routes, targets, measurement locations, and contact-derived +decontamination progress. Only the main viewport is rendered; selecting an +onboard view switches that viewport instead of rendering every robot camera. +See `docs/robot-monitoring.md`. + +## Local natural-language application + +The interactive application accepts English instructions, maps +them to a strict allowlist, previews physical operations, and executes them +through the existing workflow boundary. Release builds own a bundled +`llama.cpp` sidecar and an official Qwen3-4B GGUF model; users do not install +Ollama, PyTorch, or a Python inference SDK. + +Complex instructions can sequence up to 24 logical steps, tour every feasible +measurement station, run bounded multi-pass irregular-surface decontamination +against public removal/remaining/coverage conditions, and place then reposition +physical shield panels at host-derived source-line fractions. Every physical +attempt is revalidated against the live scene and remains confirmation-gated. + +```bash +./scripts/build_llama_runtime.sh +uv run python scripts/fetch_llm_model.py +OMNI_KIT_ACCEPT_EULA=YES ./scripts/run_app.sh +``` + +Isaac Sim remains a user-installed prerequisite and is never redistributed by +the OSS package. After initial setup, `radcounter-app` or the optional desktop +entry launches the simulator and private local model as one application. See +`docs/natural-language-control.md` for runtime layout, safety policy, hardware +fallbacks, and packaging details. + ## Safety boundary Estimator and planner APIs accept `BeliefState` and public observations only. diff --git a/THIRD_PARTY_NOTICES.md b/THIRD_PARTY_NOTICES.md index 26dd921..536329c 100644 --- a/THIRD_PARTY_NOTICES.md +++ b/THIRD_PARTY_NOTICES.md @@ -27,5 +27,35 @@ this repository unless a file explicitly states otherwise. - Python packages resolved by `uv.lock`: governed by each package's metadata and license files. +## Optional packaged local-inference components + +Release packages may include these independently licensed components under +`runtime/llm`. They are not stored in this Git repository. + +- llama.cpp, including `llama-server`: MIT License, + https://github.com/ggml-org/llama.cpp +- Qwen3-4B and its official GGUF quantization: Apache License 2.0, + https://huggingface.co/Qwen/Qwen3-4B-GGUF + +The release packaging process pins and verifies both inputs. Their copyright, +license, and notice files must accompany redistributed binaries and models. + +## Optional Fukushima Daiichi CAD environment + +The system catalog can fetch and import the independently distributed +`Qualot/fukushima_daiichi_solidworks` generic Fukushima Daiichi CAD model. +Neither its SolidWorks source nor derived USD assets are stored in this Git +repository. + +- Project: https://github.com/Qualot/fukushima_daiichi_solidworks +- Pinned integration revision: `f6541deb6159c5d908a4f028d021e3d2c9f7f8e8` +- License: Creative Commons Attribution 4.0 International (CC BY 4.0) +- Attribution: Qualot/fukushima_daiichi_solidworks contributors + +The fetch and Linux conversion scripts preserve source revision, license, +attribution, converter metadata, and SHA-256 provenance. Anyone redistributing +a derived USD, render, or other adaptation is responsible for retaining the CC BY 4.0 +attribution and indicating modifications. + Users are responsible for reviewing and accepting the terms of external software before installing or running it. diff --git a/configs/environment-converters/solidworks_to_usd.hoops.json b/configs/environment-converters/solidworks_to_usd.hoops.json new file mode 100644 index 0000000..887dedd --- /dev/null +++ b/configs/environment-converters/solidworks_to_usd.hoops.json @@ -0,0 +1,28 @@ +{ + "accurateSurfaceCurvatures": true, + "accurateTessellation": false, + "compositionStyle": 0, + "convertCurves": false, + "convertHidden": false, + "convertMetadata": true, + "convertPhysicsData": false, + "dedup": true, + "dMetersPerUnit": 0.0, + "filterStyle": 1, + "globalXforms": false, + "iUpAxis": 0, + "instancing": false, + "instancingStyle": 0, + "materialType": 1, + "omitHiddenOnLoad": true, + "physicsAccuracyLevel": 0.99, + "physicsUseGeometryOnRiBRep": false, + "reportProgress": true, + "reportProgressFreq": 4, + "sOptimizeConfig": "", + "tessLOD": 2, + "upAxis": 0, + "useMaterials": true, + "useNormals": true, + "viewLayerName": "" +} diff --git a/configs/environments/fukushima_daiichi_solidworks.yaml b/configs/environments/fukushima_daiichi_solidworks.yaml new file mode 100644 index 0000000..49e1513 --- /dev/null +++ b/configs/environments/fukushima_daiichi_solidworks.yaml @@ -0,0 +1,39 @@ +schema_version: "1.0" +environment: + environment_id: fukushima-daiichi-generic-cad + uri: ../../.cache/external/fukushima_daiichi_solidworks/export/Building.usdc + format: usd + coordinate_system: + units: auto + # The SolidWorks-exported USD declares Y-up; let the importer read and + # normalize that metadata instead of relabelling the geometry as Z-up. + up_axis: auto + forward_axis: +X + handedness: right + scale: 1.0 + # Put the authored -18 m lower operating floor at simulator Z=0 and keep + # the open CAD cut face next to the catalog's robot/work-surface anchors. + translation_world_m: [2.2, 0.0, 18.0] + rotation_world_rpy_deg: [0.0, 0.0, 0.0] + default_material_id: steel + material_rules: + - pattern: "*Building*" + material_id: concrete + - pattern: "*Structure*" + material_id: steel + - pattern: "*PCV*" + material_id: steel + - pattern: "*RPV*" + material_id: steel + - pattern: "*Pedestal*" + material_id: concrete + collision: + enabled: true + geometry_source: auto + approximation: triangle_mesh + show_collision_geometry: false + max_triangles: 8000000 + cad_mesh_size_m: 0.10 + cache_directory: ~/.cache/radcountersim/environments + streaming: + enabled: false diff --git a/configs/robots/countermeasure_robot.yaml b/configs/robots/countermeasure_robot.yaml index 69fed74..5f3ca82 100644 --- a/configs/robots/countermeasure_robot.yaml +++ b/configs/robots/countermeasure_robot.yaml @@ -1,16 +1,16 @@ robot_id: countermeasure-robot-1 -reference_model_id: mhi-meister -manufacturer: Mitsubishi Heavy Industries -model: MEISTeR -geometry_fidelity: reference_procedural -source: https://www.mhi.com/business/products-services/energy-environment/nuclear-power-generation/robot-mechatronics/meister -controller: mhi_meister_dual_arm_crawler -asset: assets/robots/mhi_meister.xacro +reference_model_id: clearpath-ridgeback-franka +manufacturer: Clearpath Robotics / Franka Robotics +model: Ridgeback + Franka Emika Panda +geometry_fidelity: manufacturer_asset +source: https://docs.isaacsim.omniverse.nvidia.com/latest/assets/usd_assets_robots.html +controller: ridgeback_franka_lula_ik +asset: /Isaac/Robots/Clearpath/RidgebackFranka/ridgeback_franka.usd articulation_root: /World/CountermeasureRobot -base_dofs: 7 -arm_dofs: 14 -gripper_dofs: 4 -end_effector_frames: [left_tool0, right_tool0] -decon_tool_path: /World/CountermeasureRobot/right_tool0 -maximum_payload_kg_per_arm: 15.0 -capabilities: [place_shield, move_object, remove_object, cut_obstacle, core_sample, suction_decontaminate, abrasive_blast_decontaminate] +base_dofs: 3 +arm_dofs: 7 +gripper_dofs: 2 +end_effector_frames: [right_gripper, panda_hand] +decon_tool_path: /World/CountermeasureRobot/panda_hand/RadCounterDeconTool/ContactPad +maximum_payload_kg_per_arm: 3.0 +capabilities: [place_shield, move_shield, move_object, remove_object, contact_decontaminate] diff --git a/configs/robots/fukushima_packbot_fleet.yaml b/configs/robots/fukushima_packbot_fleet.yaml new file mode 100644 index 0000000..8c6f543 --- /dev/null +++ b/configs/robots/fukushima_packbot_fleet.yaml @@ -0,0 +1,34 @@ +schema_version: 1 +robots: + - id: packbot + uri: ../../assets/robots/packbot_fukushima.urdf + format: urdf + prim_path: /World/Robots/PackBot + robot_type: wheeled + fixed_base: false + collision_from_visuals: false + collision_type: Convex Hull + translation_m: [2.0, -1.0, 0.0] + reference: + model_id: irobot-packbot-fukushima + manufacturer: iRobot + model: PackBot, Fukushima Daiichi deployment configuration + source_urls: + - https://www.tepco.co.jp/en/nu/fukushima-np/f1-roadmap/images/11042801a-e.pdf + geometry_fidelity: reference_procedural + dimensions_m: [0.70, 0.53, 0.18] + mass_kg: 35.0 + joint_groups: + - name: flippers + joint_names: [left_flipper_joint, right_flipper_joint] + mode: position + - name: sensor_head + joint_names: [mast_pan_joint, camera_tilt_joint] + mode: position + base_controller: + type: differential + joint_names: [left_track_joint, right_track_joint] + wheel_radius_m: 0.08 + track_width_m: 0.41 + maximum_linear_speed_m_s: 2.58 + maximum_angular_speed_rad_s: 2.0 diff --git a/configs/robots/measurement_robot.yaml b/configs/robots/measurement_robot.yaml index bd4d1a4..3448150 100644 --- a/configs/robots/measurement_robot.yaml +++ b/configs/robots/measurement_robot.yaml @@ -1,16 +1,16 @@ robot_id: measurement-robot-1 -reference_model_id: irobot-packbot-fukushima -manufacturer: iRobot -model: PackBot, Fukushima Daiichi deployment configuration -geometry_fidelity: reference_procedural -source: https://www.tepco.co.jp/en/nu/fukushima-np/f1-roadmap/images/11042801a-e.pdf -controller: packbot_differential_crawler -asset: assets/robots/packbot_fukushima.urdf -articulation_root: /World/MeasurementRobot/base_link -wheel_dof_names: [left_track_joint, right_track_joint] -flipper_dof_names: [left_flipper_joint, right_flipper_joint] -wheel_radius_m: 0.08 -wheel_base_m: 0.41 +reference_model_id: nvidia-nova-carter +manufacturer: NVIDIA +model: Nova Carter +geometry_fidelity: manufacturer_asset +source: https://docs.isaacsim.omniverse.nvidia.com/latest/assets/usd_assets_robots.html +controller: nova_carter_differential_wheels +asset: /Isaac/Robots/NVIDIA/NovaCarter/nova_carter.usd +articulation_root: /World/MeasurementRobot/chassis_link +wheel_dof_names: [joint_wheel_left, joint_wheel_right] +flipper_dof_names: [] +wheel_radius_m: 0.14 +wheel_base_m: 0.413 detector_id: omni-1 -detector_path: /World/MeasurementRobot/radiation_link/Detector -detector_extrinsic_xyz_m: [0.03, 0.16, 0.41] +detector_path: /World/MeasurementRobot/chassis_link/Detector +detector_extrinsic_xyz_m: [0.0, 0.0, 0.72] diff --git a/configs/system/catalog.yaml b/configs/system/catalog.yaml new file mode 100644 index 0000000..b5f8972 --- /dev/null +++ b/configs/system/catalog.yaml @@ -0,0 +1,180 @@ +schema_version: 1 +default_profile: vertical-slice + +environments: + vertical-slice: + display_name: RadCounterSim vertical slice + descriptor_uri: ../environments/vertical_slice_import.yaml + description: Canonical four-room decommissioning and validation environment. + spawn_anchors: + ground-primary: + kind: ground_robot + translation_m: [0.0, -1.0, 0.0] + description: Supported starting pose in the original validation cell. + ground-secondary: + kind: ground_robot + translation_m: [-4.1, -2.5, 0.0] + aerial-primary: + kind: aerial_robot + translation_m: [2.0, 1.0, 1.5] + decon-surface: + kind: work_surface + translation_m: [14.39, 0.80, 1.15] + inspection-camera-eye: + kind: camera + translation_m: [10.25, -2.10, 2.75] + inspection-camera-target: + kind: camera + translation_m: [14.30, 0.80, 1.12] + fukushima-daiichi: + display_name: Fukushima Daiichi generic CAD model + descriptor_uri: ../environments/fukushima_daiichi_solidworks.yaml + description: >- + Qualot's CC BY 4.0 SolidWorks assembly converted directly to USD on Linux + and normalized for rendering, PhysX collision, and radiation transport. + setup_hint: >- + Run scripts/fetch_fukushima_daiichi_cad.py and then + scripts/convert_solidworks_to_usd.py. Isaac Sim's bundled Linux HOOPS + converter writes Building.usdc into the configured export directory. + preparation_scripts: + - ../../scripts/fetch_fukushima_daiichi_cad.py + - ../../scripts/convert_solidworks_to_usd.py + spawn_anchors: + ground-primary: + kind: ground_robot + translation_m: [0.0, -1.0, 0.0] + description: Supported lower-floor pose inside the Building CAD cutaway. + ground-secondary: + kind: ground_robot + translation_m: [0.0, 1.0, 0.0] + aerial-primary: + kind: aerial_robot + translation_m: [0.0, 1.0, 1.5] + decon-surface: + kind: work_surface + translation_m: [2.18, 0.0, 1.15] + description: Irregular contamination overlay on the visible CAD cut face. + inspection-camera-eye: + kind: camera + translation_m: [5.2, -4.0, 3.2] + inspection-camera-target: + kind: camera + translation_m: [1.3, 0.0, 1.1] + jsi-triga: + display_name: JSI TRIGA 2026 point-cloud environment + descriptor_uri: ../environments/jsi_triga_2026.yaml + description: Dataset-backed JSI TRIGA point-cloud environment. + spawn_anchors: + ground-primary: + kind: ground_robot + translation_m: [0.0, -1.0, 0.0] + ground-secondary: + kind: ground_robot + translation_m: [0.0, 1.0, 0.0] + aerial-primary: + kind: aerial_robot + translation_m: [0.0, 1.0, 1.5] + decon-surface: + kind: work_surface + translation_m: [2.0, 0.0, 1.15] + inspection-camera-eye: + kind: camera + translation_m: [5.0, -4.0, 3.0] + inspection-camera-target: + kind: camera + translation_m: [1.0, 0.0, 1.0] + +robot_sets: + articulated-decommissioning: + display_name: Ridgeback+Franka and Nova Carter + kind: decommissioning + description: Manufacturer assets and the full physical decommissioning workflow. + fukushima-response-reference: + display_name: PackBot and Elios 3 radiation survey team + kind: reference + description: Lightweight traceable reference geometry for rapid environment exploration. + reference_robots: + - robot_id: packbot + reference_model_id: irobot-packbot-fukushima + prim_path: /World/Robots/PackBot + spawn_anchor: ground-primary + translation_m: [2.0, -1.0, 0.0] + yaw_deg: 0.0 + - robot_id: elios3 + reference_model_id: flyability-elios3-rad + prim_path: /World/Robots/Elios3 + spawn_anchor: aerial-primary + translation_m: [2.0, 1.0, 1.5] + yaw_deg: 0.0 + packbot-articulated: + display_name: Articulated Fukushima PackBot URDF + kind: fleet + fleet_uri: ../robots/fukushima_packbot_fleet.yaml + description: Isaac-imported URDF with named track, flipper, and mast joints. + no-robots: + display_name: Environment only + kind: none + +detector_sets: + vertical-slice-gamma: + display_name: Vertical-slice gamma counter + detectors: + - detector_id: gamma-counter + model_id: nai_tl + parent_prim_path: /World/MeasurementRobot/chassis_link/Detector + fukushima-survey: + display_name: PackBot GM survey meter and Elios coded-aperture imager + description: Two independently simulated gamma instruments mounted on reference robots. + detectors: + - detector_id: packbot-gm + model_id: gm_tube + parent_robot_id: packbot + parent_sensor_link: radiation + translation_m: [0.03, 0.16, 0.41] + - detector_id: elios-imager + model_id: coded_aperture_czt + parent_robot_id: elios3 + parent_sensor_link: radiation + translation_m: [-0.105, 0.0, -0.115] + fixed-survey-station: + display_name: Fixed NaI survey station + detectors: + - detector_id: fixed-nai + model_id: nai_tl + parent_prim_path: /World + translation_m: [0.0, 0.0, 1.0] + no-detectors: + display_name: No radiation detector + detectors: [] + +profiles: + vertical-slice: + display_name: Vertical-slice decommissioning + environment: vertical-slice + robot_set: articulated-decommissioning + detector_set: vertical-slice-gamma + runtime_config_uri: ../scenarios/vertical_slice.runtime.json + application_mode: decommissioning + description: Existing full articulated application and validation workflow. + fukushima-packbot: + display_name: Fukushima Daiichi PackBot survey + environment: fukushima-daiichi + robot_set: fukushima-response-reference + detector_set: fukushima-survey + runtime_config_uri: ../scenarios/vertical_slice.runtime.json + application_mode: configurable + description: Imported Fukushima CAD with quickly movable reference survey robots. + fukushima-environment-only: + display_name: Fukushima Daiichi environment inspection + environment: fukushima-daiichi + robot_set: no-robots + detector_set: fixed-survey-station + runtime_config_uri: ../scenarios/vertical_slice.runtime.json + application_mode: configurable + vertical-slice-packbot: + display_name: Vertical slice with PackBot/Elios survey team + environment: vertical-slice + robot_set: fukushima-response-reference + detector_set: fukushima-survey + runtime_config_uri: ../scenarios/vertical_slice.runtime.json + application_mode: configurable diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index d4843c6..ee3a3aa 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -1,5 +1,17 @@ # Changelog +## Natural-language application control + +- Added English command interpretation through a bundled, + loopback-only llama.cpp sidecar and Qwen3-4B GGUF model. +- Added schema-constrained plans, live candidate resolution, deterministic + feasibility validation, physical-action confirmation, and JSONL audits. +- Added bounded public-result workflow conditions, multi-pass irregular-surface + decontamination, multi-target station/surface expansion, and stateful shield + placement-to-reposition transitions without duplicate inventory consumption. +- Added CPU/Vulkan/CUDA runtime packaging, verified model installation, a single app + launcher, and a Linux desktop entry while keeping Isaac Sim user-installed. + ## Unreleased - Created the uv-managed independent RadCounterSim repository. @@ -30,4 +42,11 @@ boundary, uv-managed batch execution, reproducibility manifests, JSONL, Parquet/JSON/NPZ outputs, self-contained HTML reports, analytic validation, and one-command demo scripts. -- External Isaac Sim, Embree, and ROS 2 acceptance gates remain open. +- Added manufacturer-asset Ridgeback + Franka and Nova Carter execution with + Lula IK, articulated base/arm/finger control, PhysX fixed-joint grasping, + contact-driven decontamination, shield placement, object relocation, and + disposal validation. +- Passed the local Isaac Sim, Embree, PhysX, and ROS 2 runtime gates. The + canonical GPU/renderer release manifest remains open. +- Cleared all 163 repository-wide Ruff violations and documented the remaining + specification gaps in `SIMULATOR_REQUIREMENT_AUDIT.md`. diff --git a/docs/SIMULATOR_REQUIREMENT_AUDIT.md b/docs/SIMULATOR_REQUIREMENT_AUDIT.md index 8e1bc3a..e7be200 100644 --- a/docs/SIMULATOR_REQUIREMENT_AUDIT.md +++ b/docs/SIMULATOR_REQUIREMENT_AUDIT.md @@ -1,12 +1,29 @@ # RadCounterSim simulator requirement audit -Audit date: 2026-07-15 +Audit date: 2026-08-06 ## Scope and verdict -All simulator-side requirements in the implementation specification are implemented. Source-estimation algorithm development was explicitly excluded from this completion pass. Existing grid/TV estimation, Fisher uncertainty, residual diagnosis, and planner implementations remain available and are connected through injected public interfaces. +The simulator's core architecture, radiation transport, countermeasure actions, +closed-loop workflow, ROS 2 adapters, and articulated Isaac execution are +implemented. The implementation specification is nevertheless **not literally +100% complete**. -The original specification is therefore not literally 100% complete if source-estimation research is included: continuous point-source position/activity MLE refinement and a concrete PF+MLE estimator are not claimed as completed here. +The remaining required gaps are continuous point-source position/activity MLE +refinement and the complete eight-section operational UI/visualization surface. +The current dashboard loads the articulated scene, synchronizes the radiation +runtime, authors explicitly authorized sources, measures detectors, renders a +dose proxy, and displays externally supplied estimate/residual/plan artifacts. +It does not yet run estimation, action preview/execution, closed-loop control, or +experiment configuration directly from the GUI, and it lacks the specified +source-uncertainty, residual-map, selected-ray, and action overlays. The exact +performance-target matrix in section 29.3 also has not been recorded; the live +performance gate validates cache behavior on a smaller workload with a looser +limit. + +`PFPlusMLEEstimator` remains an optional-phase item in the specification and is +not implemented. Dependency locking uses `uv.lock` rather than the spec's +suggested `requirements-lock.txt`. ## Requirement matrix @@ -27,26 +44,39 @@ The original specification is therefore not literally 100% complete if source-es | Closed-loop execution | Complete | Measure -> injected estimate -> plan -> physical action -> synchronize -> remeasure -> residual -> belief update in `IsaacWorkflowServices` | | Physics-step integration | Complete | Lifecycle-safe PhysX event subscription and periodic radiation synchronization | | ROS 2 integration | Complete | Messages/actions/services, Nav2, MoveIt 2, FollowJointTrajectory, gripper, cancellation/error propagation, Isaac DDS command host | -| UI and artifacts | Complete | Source authoring with explicit hidden-Truth consent, measurement/dose map, estimate/residual/plan panels, atomic workflow artifact loading/writing | +| UI and artifacts | Partial | Scene/runtime/source/measurement/dose-proxy controls and estimate/residual/plan artifact views exist; the specified Estimation, Countermeasure, Closed Loop, Visualization, and Experiment controls are not complete | +| Visualization layers | Partial | Batched point-based dose proxy exists; source uncertainty, predicted/observed post-action, residual-map, selected-ray material path, and action overlays are not implemented as the specified layers | | Experiments and reproducibility | Complete | Seeded batch runner, JSON/JSONL/Parquet/report artifacts, regression bounds, vertical-slice multi-seed runner | | Vertical-slice scenario | Complete | Room, surface source, hidden contaminated object, measurement/countermeasure robots, shield, decon patch, movable obstacle, verification stations | -| Grid Poisson sparse and surface-TV estimators | Existing | Implemented and unit-tested before this simulator completion pass; consumed through an injected estimator callback | +| Grid Poisson sparse and surface-TV estimators | Complete | Nonnegative Poisson MLE/L1, smooth graph-TV, connected components, Fisher covariance, and bootstrap are unit-tested and consumed through an injected estimator callback | | Fisher uncertainty and residual hypotheses | Existing | Active-set Fisher covariance, bootstrap, decon/shield/hidden-source/gain/source-location residual handling and belief updates | | Planner baselines | Existing | Open-loop, greedy dose reduction, nearest source, random, oracle, and closed-loop residual planners | -| Continuous point-source position/activity MLE | Out of scope | Coarse sparse estimation and connected-component support exist; continuous refinement is not claimed | -| Concrete PF+MLE estimator | Out of scope | A generic `SourceEstimator` protocol exists; no concrete particle-filter estimator is claimed | +| Continuous point-source position/activity MLE | Missing | Coarse sparse estimation and connected-component summaries exist, but no continuous-coordinate refinement follows them | +| Concrete PF+MLE estimator | Optional / missing | A generic `SourceEstimator` protocol exists; no concrete particle-filter estimator is implemented | +| Section 29.3 performance matrix | Not demonstrated | Cache and selective invalidation are live-tested, but the four exact target workloads/timings are not captured by the current benchmark suite | ## Validation evidence -- `uv run ruff check .`: passed. -- Portable pytest suite: 85 passed, 1 skipped. +- `uv lock --check`: passed. +- `uv run ruff check .`: passed with zero violations (163 repository-wide + violations fixed in this audit pass). +- Portable pytest suite: 142 passed, 3 skipped. The skipped tests require the + optional native Embree module in the active `uv` environment; the native host + build/runtime gate has separate passing evidence. - Native Embree build and integration tests: passed, including a 17-ray packet/tail case. - ROS 2 `colcon` build: passed. - ROS DDS motion gateway gate: passed for Nav2, MoveIt 2, FollowJointTrajectory, gripper, cancellation/result handling. - Isaac extension-load, vertical-slice, performance/cache, PhysX action/contact-decon, articulation/IK-contract, dashboard, and closed-loop workflow gates: passed. - Cross-process ROS 2 to Isaac command-host gate: passed. -- Visible Isaac Sim GUI end-to-end run: passed all 8 operations and all 10 final invariants; 170 native traces, 206 traced rays, 83 cache hits, and 149 selectively patched rays. +- Visible Isaac Sim GUI end-to-end articulated run: passed all 8 operations and + all 13 final invariants with a 12-DOF Ridgeback + Franka and 7-DOF Nova + Carter; 61 native traces, 157 traced rays, 102 cache hits, and 89 selectively + patched rays. -The articulation gate validates a live Isaac articulation and the controller's IK contract with an injected deterministic IK solver. `IsaacLulaIkSolver` is implemented as the production adapter, but this audit does not claim physical-hardware validation or a robot-specific Lula calibration file. +The articulation-contract gate still validates an injected deterministic IK +solver. In addition, `real_robot_gate.py`, `articulated_object_gate.py`, and the +full GUI workflow execute the manufacturer Franka asset with Isaac's supported +Lula kinematics configuration. This remains simulation validation and does not +claim physical-hardware validation. `AnalyticHostBridge` remains intentionally available as a portable baseline. The actual simulator path uses `IsaacRos2CommandHost`; retaining the analytic bridge is not a fallback in the validated Isaac/ROS gate. diff --git a/docs/decontamination-authoring-rules.md b/docs/decontamination-authoring-rules.md new file mode 100644 index 0000000..1a7559d --- /dev/null +++ b/docs/decontamination-authoring-rules.md @@ -0,0 +1,179 @@ +# Surface-source and decontamination authoring rules + +This file is the repository's canonical specification for surface contamination +and its removal. It records the implementation used for the August 6, 2026 +reference videos so later work does not regress to a tidy framed rectangle or an +unverified animation. + +## Canonical references + +The authoritative visual and behavioral references are: + +- `scripts/isaac_shield_placement_video.py`, especially + `author_irregular_drum_surface_source`, and + `/home/moeu/Pictures/Research/RadCounterSim/manipulator_surface_source_shield_video_20260806/manipulator_surface_source_shield_placement_20s.mp4`. + The recorded source has 252 active faces selected from 968 candidates. +- `scripts/isaac_surface_decon_validation.py`, especially `activity_field`, + `create_high_wall_surface_source`, `_high_reach_scan_target`, and + `render_high_reach_decontamination_video`, and + `/home/moeu/Pictures/Research/RadCounterSim/high_reach_wall_decontamination_video_20260806/high_reach_wall_decontamination_20s.mp4`. + The recorded 20-second run reduced activity from 61,772,808.55 Bq to + 6,670,376.89 Bq (89.20% removed). + +The absolute video paths are evidence locations on the development machine, not +runtime dependencies. The scripts and the rules below are the portable source of +truth. + +## Surface-source construction + +1. Sample a dense two-dimensional lattice on the existing host surface. The + reference implementations use 44 x 22 candidates on the drum and 48 x 28 on + a flat wall or floor. +2. Form a deterministic scalar deposition field from several offset, + differently scaled Gaussian lobes plus correlated sinusoidal roughness. A + single ellipse, rectangle, or manually arranged handful of polygons is not an + acceptable substitute. +3. Threshold the field, subtract at least one clean hole, and add detached + droplets or satellite islands. The result must have an asymmetric ragged + boundary, concavities, internal gaps, and disconnected deposits. +4. Author geometry only for active cells/faces. Inactive candidates must expose + the underlying wall, floor, vessel, or component; they must not become a + visible rectangular backing plate. +5. Store activity per active face/cell, proportional to the local field value. + Color is a muted scientific overlay blended with the host material and varies + with local activity. The contamination is not a uniformly colored red sign. +6. Keep fixtures separate. A frame is allowed only when it represents real + equipment required by the scenario, never to outline or tidy the source. +7. The geometry used by contact/raycast decontamination must be the same + activity-bearing irregular geometry that is rendered. A hidden regular + collision or activity proxy is forbidden. +8. A generator must be deterministic for a fixed configuration and expose enough + metadata to audit candidate count, active count, total activity, and the + irregular-mask method. + +## Decontamination operation + +1. Use a continuous boustrophedon/serpentine raster over the contaminated extent, + as in the six-lane high-wall reference scan. Alternate lane direction and + choose spacing from the physical tool footprint so the sweep covers the + irregular field, its holes, and its satellite deposits without converting the + source to a rectangle. Inset each centerline endpoint by the pad's projected + half-extent; the pad boundary, rather than an unnecessarily extreme center + pose, covers the edge of the source. +2. Move the real articulated robot and tool through approach, scan, and retreat + phases. A visual-only tool animation is insufficient for operational + validation. Scripted reference renders may use analytical motion, but product + workflows must retain articulation/IK and physics evidence. +3. Change activity only from verified treatment contact. Enforce surface identity, + tool-to-surface distance, treatment-axis/normal alignment, maximum surface + speed, and dwell/exposure. Misses and invalid contacts must not remove + activity. Sample the full physical pad footprint densely enough for the + activity-cell pitch; a few center/corner rays are not an area-contact model. +4. Use the reference cumulative-exposure model: each contacted cell/triangle + decays exponentially according to footprint exposure, local efficiency, and + the configured removal-rate constant. Give each contacted face one + speed-adjusted frame of exposure regardless of how many footprint rays hit + it; ray count is spatial sampling density, not elapsed time. Use the shared + `effective_contact_exposure_s` model for analytical grids and PhysX contact. + Removed activity must be transferred to the configured waste sink when that + mode is enabled. +5. Update the overlay from remaining per-face activity during treatment. Fade it + toward the host material and hide a face only below the documented residual + threshold; the reference renderer uses 10% of initial local activity. +6. Success requires physical tool motion, accepted contacts, reduced total + activity, and meaningful coverage of the active irregular source. Merely + touching one triangle is not a successful request to decontaminate the whole + source. +7. Record at minimum: initial/final/removed activity, removed fraction, accepted + and rejected contacts, treated face indices or coverage, waypoint errors, tool + path length, robot/navigation audit, and collateral-object displacement. +8. Preserve the full approach -> raster treatment -> retreat sequence and verify + that navigation and manipulation do not move unrelated drums, shields, or + obstacles. +9. Natural-language multi-pass requests must remain bounded. A single logical + decontamination step may request at most five complete passes and may stop + early only from host-computed public metrics: cumulative remaining or removed + activity relative to the first pass baseline, or treated + active-face coverage. Every pass must independently re-resolve feasibility + and execute the complete approach -> irregular-surface raster -> retreat + sequence. The LLM must never declare that a threshold was reached, and an + unmet threshold at the attempt limit must be reported to the operator rather + than silently presented as success. + +## Operator monitoring visualization + +1. The viewport may draw a planned base route, the physical tool's planned + raster centerline, a target marker, robot beacons, and detector measurement + positions as operator guidance. These overlays are annotations only and must + never be used as collision, contact, activity, coverage, or success geometry. +2. Planned and treated decontamination areas must remain visibly distinct. The + path is the planned sweep; the treated result is the live per-face fade on + the same irregular activity-bearing mesh accepted by contact validation. + Never show a regular rectangular coverage proxy, hidden or visible, in place + of those real faces. +3. Coverage and progress shown in the operator bar must come from controller + events and the contact report. `coverage_fraction` remains the fraction of + actual active irregular faces treated, not the fraction of a raster bounding + rectangle traversed. +4. The automatic work camera may frame both tool and surface, but it must not + modify surface geometry, collision, activity, waypoint reachability, contact + thresholds, or treatment results. It switches once when the controller + enters the treatment phase; operator camera input cancels automatic tracking. + +## Complex validation environment + +The facility in this section belongs to the standalone `vertical-slice` +environment. It must not be silently authored over an imported CAD, point +cloud, or other catalog environment. External environments keep their own +visible/collision geometry and use named catalog spawn anchors for robots, +work surfaces, and inspection cameras. A decontamination overlay placed in an +external environment must still lie on the visible host CAD surface and retain +the exact rendered/contact/activity geometry required above. + +1. Product GUI validation uses the deterministic layout returned by + `decommissioning_facility_layout`: the original cell, remote decontamination + room, reactor service room, and shield-staging room are connected by three + declared corridor segments. Do not collapse this validation scene back to a + single room or a straight two-room demonstration. +2. Author at least ten fixed, individually identified decommissioning obstacles + or equipment assemblies, including process, maintenance, waste, and service + equipment. Their room, type, and stable equipment ID must remain available as + public facility metadata so GUI artifacts can audit the environment without + depending on display names. +3. Keep the primary decontamination and staging service centerlines clear. The + pure layout regression uses a 0.65 m minimum planar clearance from declared + walls and equipment, exceeding the current 0.55 m mobile-planner clearance. + Added visual complexity must not make a required robot route physically + impossible or rely on collision-disabled obstacles. +4. The remote room's east wall and the irregular activity-bearing mesh retain + the same visible collision/contact geometry described above. Additional rooms + and fixtures must not introduce a regular activity proxy or replace the + canonical irregular source. +5. A staged secondary physical shield may be present in the shield-staging room, + but it must have a distinct inventory identity and must not alter or obstruct + the existing primary shield's pickup, placement, or correction routes. +6. The primary shield's single service handle is authored on its west face. The + current controller preserves payload world orientation, so shield placement + must use the matching yaw-zero, handle-side approach. The remote-room service + alcove keeps the 25% placement base and the return route to the 65% placement + clear. Do not restore a yaw-pi fallback unless payload rotation or a verified + second grasp frame and chassis-to-payload collision check are implemented. + +## Required maintenance workflow + +Any pull request or local change that intentionally alters source generation, +contact constraints, scan planning, activity decay, visual fading, or success +criteria must: + +1. update this document with the new invariant and rationale; +2. update or add regression tests for the invariant; +3. run the relevant unit tests and at least one Isaac validation for behavior + that depends on articulation, PhysX contact, or raycasts; and +4. retain an auditable artifact containing the metrics listed above. + +For a bounded multi-pass request, the artifact must additionally record the +logical workflow step, attempt number, configured maximum, completion criterion, +threshold, observed public value, and whether the stop condition was met. + +Do not weaken these rules solely to make a failing run pass. Fix geometry, +reachability, scan planning, or physics setup and document any intentional change. diff --git a/docs/definition-of-done.md b/docs/definition-of-done.md index e5ead55..4d302ca 100644 --- a/docs/definition-of-done.md +++ b/docs/definition-of-done.md @@ -14,15 +14,15 @@ ## Host integrations -- [ ] Isaac Sim runtime gate: load the extension in a supported Isaac Sim installation. -- [ ] Embree runtime gate: build and execute native occlusion queries against Embree 4. -- [ ] Physics runtime gate: execute shield placement and object motion in PhysX. -- [ ] ROS 2 runtime gate: build the Jazzy workspace and pass message/action/service round trips. +- [x] Isaac Sim runtime gate: load the extension in a supported Isaac Sim installation. +- [x] Embree runtime gate: build and execute native occlusion queries against Embree 4. +- [x] Physics runtime gate: execute shield placement and object motion in PhysX. +- [x] ROS 2 runtime gate: build the Jazzy workspace and pass message/action/service round trips. - [ ] GPU runtime gate: record the target GPU and Isaac renderer versions in a release manifest. -Isaac Sim 6.0.1, Embree 4.3, ROS 2 Jazzy, and the NVIDIA GPU are installed on this -machine. The unchecked items remain unchecked until their real build or runtime test -passes; installed files and static inspection alone are insufficient evidence. +The first four host gates passed on the local Isaac Sim 6.0.1, Embree 4.3, and +ROS 2 Jazzy host. The canonical release-manifest GPU/renderer record remains +open; runtime reports alone do not satisfy that release-artifact requirement. Run the portable release checks with: diff --git a/docs/environment-import.md b/docs/environment-import.md index e311d62..30ab210 100644 --- a/docs/environment-import.md +++ b/docs/environment-import.md @@ -51,12 +51,26 @@ cache directory: pickle data. - `environment.usda`: generated lazily when Isaac Sim loads the result. -In the RadCounterSim Isaac window, the load field accepts a mesh/CAD/map file, -an environment descriptor, a generated `manifest.json`, or an existing USD -stage. A direct USD path preserves the existing stage and prim paths. Put an -arbitrary untagged USD asset in a descriptor when it should be wrapped with -default collision and radiation-material metadata. Existing USD-only workflows -are unchanged. +Named system profiles can combine an environment descriptor with independently +selected robot and detector sets. See `docs/system-profiles.md` and use +`uv run radcounter-system list`. The Fukushima Daiichi SolidWorks integration +also documents its native SolidWorks-to-USD conversion there. On Linux this is +performed by the HOOPS Exchange converter bundled with Isaac Sim; `.SLDASM` +and `.SLDPRT` are not passed to the simulator as if they were portable meshes. + +In the RadCounterSim Operations window, operators select an environment, robot +set, and detector set by display name and press **Apply selected configuration**. +No catalog ID, file path, or CLI knowledge is needed for normal operation. +Invalid detector/robot combinations are rejected before the stage changes. +The separate LLM field controls robot tasks; it does not interpret or change +the system-selection controls. Catalog environments may declare reviewed local +preparation scripts; when their asset is missing the Apply button becomes a +one-click **Prepare** action instead of asking the operator to run commands. + +A direct USD path preserves the existing stage and prim paths. Put an arbitrary +untagged USD asset in a descriptor when it should be wrapped with default +collision and radiation-material metadata. Existing USD-only workflows are +unchanged. ## Descriptor diff --git a/docs/isaac-workflow.md b/docs/isaac-workflow.md index 9607777..cf69760 100644 --- a/docs/isaac-workflow.md +++ b/docs/isaac-workflow.md @@ -8,7 +8,9 @@ 3. Invoke an injected estimator with `tuple[PublicMeasurement, ...]`. 4. Generate candidates from the live USD scene and the returned `BeliefState`. 5. Preview the selected action from belief strengths and public geometry only. -6. Execute the action through `IsaacPhysicsRobotController` or contact decontamination. +6. Execute manufacturer-asset workflows through `RidgebackFrankaController` + and `NovaCarterController`; the generic rigid-body controller remains + available only for portable/custom-robot integrations. 7. Synchronize the actual USD pose/activity state into Embree. 8. Re-measure, calculate a public residual, and invoke an injected belief updater. @@ -25,11 +27,14 @@ This keeps simulator Truth inaccessible to estimator and planner APIs. - obstacle-aware shield placements; - movable-object relocation and disposal actions. -Each candidate includes checks for mobile path clearance, manipulator workspace or -configured IK, placement collision, grasp-frame availability, support stability, -disposal-class compatibility, robot availability, and mission resources. Dose terms -are computed from `BeliefState.source_strength_bq`, public source sample positions, -and Embree transmission. Runtime source activity is never read for planner metrics. +Each candidate includes checks for mobile path clearance, the articulated base +pose, horizontal and vertical manipulator reach, pickup and placement grasp-frame +poses, placement collision, support stability, disposal-class compatibility, +robot availability, and mission resources. Object-root poses and grasp-frame +offsets are tracked separately, including their rotation when the mobile base +changes yaw. Dose terms are computed from `BeliefState.source_strength_bq`, public +source sample positions, and Embree transmission. Runtime source activity is never +read for planner metrics. ## Physics-step lifecycle @@ -59,3 +64,13 @@ uv run --project "$RADCOUNTER_ISAAC_ROOT" --locked python tests/isaac/workflow_g The gate requires a physical shield displacement, an actual PhysX callback, initial and verification detector integrations, a planner decision, and no Truth field in the injected estimator input. + +Run the complete articulated GUI path headlessly with: + +```bash +uv run --project "$RADCOUNTER_ISAAC_ROOT" --locked python scripts/run_gui_validation.py \ + --headless --no-keep-open --phase-hold-s 0 +``` + +That gate requires all eight operations and all final invariants to pass without +operation-time pose teleports. diff --git a/docs/natural-language-control.md b/docs/natural-language-control.md new file mode 100644 index 0000000..bcbd94e --- /dev/null +++ b/docs/natural-language-control.md @@ -0,0 +1,128 @@ +# Natural-language application control + +RadCounterSim accepts English instructions through one local, +schema-constrained command surface. The language model proposes a plan; it +never receives direct Python, shell, USD, or robot-controller access. + +## Product architecture + +```text +RadCounterSim UI + -> public scene/action snapshot + -> loopback OpenAI-compatible request + -> bundled llama.cpp + Qwen3-4B GGUF + -> strict CommandPlan validation + -> preview / physical-action confirmation + -> existing Isaac workflow services +``` + +The final package uses the user's separately installed Isaac Sim. RadCounterSim +owns and starts `llama-server` as a hidden child process, selects the packaged +CPU, Vulkan, or CUDA binary, assigns a random loopback port, waits for its +health check, and stops it with the application. No Ollama daemon, Python +inference library, PyTorch installation, API key, or internet connection is +required after the release assets are installed. + +RadCounterSim is BSD-licensed, but its OSS status does not change NVIDIA's +terms. The package deliberately does not contain Isaac Sim or Omniverse Kit; +each operator installs and accepts the terms for their own copy. NVIDIA's +[Isaac Sim license FAQ](https://docs.isaacsim.omniverse.nvidia.com/6.0.1/common/license-faq.html) +states that internal R&D use is free, while redistributing Isaac Sim with +Omniverse Kit to third parties requires NVIDIA AI Enterprise licensing. A +future all-in-one RadCounterSim installer must therefore keep this same +user-installed-Isaac boundary unless separate redistribution rights are +obtained. + +## Source-tree setup + +Install the pinned release assets once: + +```bash +./scripts/build_llama_runtime.sh +uv run python scripts/fetch_llm_model.py +``` + +The model downloader installs the official Qwen `Qwen3-4B-Q4_K_M.gguf` and +verifies its SHA-256. The llama.cpp setup installs verified official CPU and +Vulkan binaries and, when `nvcc` is available, also produces an NVIDIA CUDA +build. These files live under +`runtime/llm` and are excluded from Git; a binary release includes them with +their upstream license and notice files. + +Launch the application with: + +```bash +OMNI_KIT_ACCEPT_EULA=YES ./scripts/run_app.sh +``` + +After NVIDIA's EULA marker exists, subsequent launches do not need that +environment variable. A desktop entry can be installed with: + +```bash +uv run python scripts/install_desktop_entry.py +``` + +## Operator behavior + +Examples: + +- `Move to the protected area and measure for 2 seconds.` +- `Decontaminate the contaminated surface, then measure.` +- `Decontaminate the irregular wall source up to three times until at least 70% is removed.` +- `Place the shield at 25% of the source-to-protected-area line, then move it to 65%.` +- `Visit every measurement station in order and measure for 2 seconds at each station.` +- `Pause the simulation.` +- `Move the contaminated drum to the disposal area.` + +Pause, status, measurement, and visualization commands run immediately. +Stage replacement, reset, and every scene-derived physical action require an +explicit confirmation. A single instruction may contain up to 24 ordered +steps, including multiple physical actions. The complete workflow is confirmed +once; the live scene and candidate feasibility are regenerated after every step +before the next operation can execute. + +Complex plans may contain up to 24 logical steps and at most 48 bounded +executions after repeats are expanded. One decontamination step may run up to +five complete passes. It can stop early on an allowlisted public result: +removed fraction, cumulative remaining activity fraction, or active-face +coverage. Measurement repeats may stop on a public measured-rate threshold. +The host evaluates these conditions; model text can neither claim success nor +create an unbounded loop. If the attempt limit is reached first, the GUI reports +that the requested condition remains unmet. + +Shield candidates expose the physical panel and their source-to-protected-area +placement fraction. The first successful deployment is a `place_shield` +operation; later corrections of that same panel are regenerated as +`move_shield` and do not consume another inventory unit. Pickup poses, routes, +grasp frames, placement poses, and collision checks remain host-derived. + +The model sees only: + +- session state and stage name; +- allowlisted application capabilities; +- action IDs, public labels, target paths, and feasibility flags generated from + the current planning boundary. + +It does not see `TruthState`. Original instructions, validated plans, +confirmation decisions, public results, and failures are appended to +`artifacts/ui/natural_language_commands.jsonl`. + +## Hardware behavior + +The default model is a 4-bit 4B GGUF so it remains practical beside Isaac Sim +on an RTX 3080-class machine. `llama-server` uses automatic GPU layer fitting +through CUDA or Vulkan when NVIDIA hardware is available and uses the packaged +CPU build otherwise. +The context is capped at 8192 tokens so multi-room scenes with several surface, +shield, object, and detector candidates still fit without granting the model a +chat-sized or open-ended context. + +Overrides for development and diagnosis: + +- `RADCOUNTER_LLM_GPU_MODE=cpu|hybrid|auto|gpu` +- `RADCOUNTER_LLM_MODEL=/absolute/model.gguf` +- `RADCOUNTER_LLAMA_SERVER=/absolute/llama-server` +- `RADCOUNTER_LLM_RUNTIME_DIR=/absolute/runtime/llm` +- `RADCOUNTER_LLM_ENDPOINT=http://127.0.0.1:PORT` + +Only loopback inference endpoints are accepted by default. diff --git a/docs/real_robot_validation.md b/docs/real_robot_validation.md index c8598f9..6b723e7 100644 --- a/docs/real_robot_validation.md +++ b/docs/real_robot_validation.md @@ -6,16 +6,61 @@ official Isaac Sim 6 assets below. - Countermeasure: Clearpath Ridgeback + Franka Panda - Measurement: NVIDIA Nova Carter -The validation performs these physical operations in one scene: +The full GUI validation performs these physical operations in one scene: -1. Nova Carter drives using its left and right wheel joints. -2. Franka moves a decontamination pad through Lula IK joint targets. -3. Activity changes only after live PhysX contact queries accept the pad pose. -4. Franka opens its fingers, grasps a 2.2 kg lead cassette, lifts it, carries it, - places it between source and detector, and opens its fingers. -5. Embree transport is synchronized and the source-specific response is checked. +1. Nova Carter drives through every authored measurement station using its + differential wheel joints. Each leg is replanned from the robot's live pose; + a route generated at an earlier station is never reused with a stale start, + and collision bounds are rebuilt from the live scene before each leg. Route + planning excludes only the robot being commanded; the other articulated + robot remains a collision obstacle and must be physically bypassed. +2. Ridgeback drives with three planar articulation joints and folds Franka into + a repeatable travel posture between manipulation tasks. +3. Franka moves a decontamination pad through a Lula-IK raster. Activity changes + only after live PhysX contact queries accept pad distance, normal, speed, and + dwell. +4. Franka opens its fingers, approaches an authored grasp frame, closes its + fingers, attaches the verified payload to `panda_hand`, lifts, carries, + places, releases, and checks physical settling. The closed aperture matches + the authored 30 mm service-handle thickness, preventing an artificial + interpenetration impulse before the grasp constraint is created. The handle + projects 240 mm from the plate to preserve wrist clearance during the + vertical approach while remaining part of the same rigid shield assembly. +5. The same sequence places and corrects a 2.2 kg lead cassette, relocates and + disposes the contaminated drum, and relocates the large obstacle. Disposal + is accepted only when the released object's bounds are contained by the + disposal zone and supported by its floor. Generic-object pickup keeps the + Ridgeback base 900 mm from the grasp frame: outside the combined chassis and + drum envelope but inside the verified 950 mm Franka workspace. The target + object remains a route obstacle during pickup approach and is excluded only + after it has been grasped and becomes the robot's carried payload. The drum + crossbar is 500 mm from its center and is joined to the drum by an authored + support stem, giving the descending wrist clearance from the 320 mm radius. + Loaded pickup uses an 80 mm vertical clearance lift followed by a diagonal + 140 mm retraction toward the actual pickup-base pose and a 220 mm lift, + keeping the payload inside Franka's spherical workspace for either approach + yaw instead of lifting vertically at maximum extension. After relocation, + disposal re-approaches the drum from the opposite side at the same 900 mm + stand-off. Base yaw does not rotate the payload in world space, so the + planner preserves the authored grasp-to-root offset throughout transport; + this keeps the carried drum on the straight disposal-zone entry line. +6. Embree is synchronized after every operation, then Nova Carter re-measures + the scene and the public belief is updated from the residual. -Run the visible validation with Isaac Sim's uv environment: +Run the complete visible GUI workflow with Isaac Sim's uv environment: + +```bash +export RADCOUNTER_HOST_ENV_NO_ROS=1 OMNI_KIT_ACCEPT_EULA=YES +source scripts/host_env.sh +uv run --project "$RADCOUNTER_ISAAC_ROOT" --locked python scripts/run_gui.py +``` + +The GUI audit is written to `artifacts/gui-validation/latest.json`. For a +noninteractive run, append `--headless --no-keep-open --phase-hold-s 0`. +The persistent visible GUI loop is capped at 60 FPS by default; pass +`--max-fps 30` to lower it or `--max-fps 0` to disable the cap. + +The smaller decontamination/shield asset audit remains available with: ```bash export RADCOUNTER_HOST_ENV_NO_ROS=1 OMNI_KIT_ACCEPT_EULA=YES @@ -26,3 +71,11 @@ uv run --project "$RADCOUNTER_ISAAC_ROOT" --locked \ The audit is written to `artifacts/real-robot/latest.json`; phase captures are written to `artifacts/real-robot/frames/`. + +That audit uses the same irregular vertical wall as the full workflow. The +Ridgeback folds the arm, follows the authored corridor into the separate +decontamination room, executes the six-lane contact raster, and returns through +the corridor before starting the shield task. + +`tests/isaac/articulated_object_gate.py` separately checks the drum and obstacle +service-handle approach poses after an arm-stow and articulated base move. diff --git a/docs/robot-monitoring.md b/docs/robot-monitoring.md new file mode 100644 index 0000000..1365a06 --- /dev/null +++ b/docs/robot-monitoring.md @@ -0,0 +1,54 @@ +# Robot monitoring and camera behavior + +The Isaac Operations workspace is designed so an operator can understand every +robot without knowing USD paths or camera commands. The selected robot-set from +the central catalog is the single source of robot names, prim paths, and list +membership. + +## Operator surfaces + +- The viewport top bar always names one active robot and shows its world + position, current operation, phase, contact-derived coverage, and progress. +- Each robot row has **View** for a smooth diagonal rear follow view and **Onboard** + for the selected robot's onboard view. Camera input or a camera-path change + cancels automatic tracking; pressing **View** restores it. +- A permanent top-right building overview projects structural CAD bounds, + contamination markers, all robot positions and headings, the planned route, + and the current target into a common top-down coordinate frame. +- During travel the selected robot remains in follow view. The first controller + transition sequence is `approaching` -> `contact_confirmed` -> + `decontaminating`; the last transition makes one smooth transition to a work + view framing the robot and treatment target. Later progress events do not + retrigger the camera. +- Colored 3D bounding beacons, robot names, and camera distance remain visible + through building geometry. This preserves the opaque CAD model while keeping + hidden robots locatable. +- The 3D route and target show intent. For measurement, the target is the + detector measurement position. For decontamination, the physical tool raster + is the planned path, while treated coverage remains the live per-face fade of + the exact irregular activity-bearing surface. + +## Performance policy + +The main 3D viewport retains the application-wide 60 FPS default cap. Monitoring +state and the overview update at 12 Hz, within the 10–15 FPS budget. The overview +is a lightweight `omni.ui.scene` projection and is not another rendered camera. +Onboard mode changes the camera of the single main viewport; no cameras are +rendered for unselected robots. + +`RobotMonitorOverlay.audit()` records the update rate, active robot, camera mode, +catalog robot count, map geometry counts, contamination markers, route points, +overlay errors, and the single-rendered-viewport invariant in GUI validation +artifacts. + +The articulated CAD decontamination gate must explicitly select the matching +central-catalog robot set; it never injects a hidden task robot into a PackBot or +other operator roster: + +```bash +python scripts/run_gui_validation.py \ + --profile fukushima-packbot \ + --robot-set articulated-decommissioning \ + --detector-set vertical-slice-gamma \ + --decontamination-smoke-test --no-keep-open +``` diff --git a/docs/specs/RadCounterSim_Codex_Implementation_Spec.md b/docs/specs/RadCounterSim_Codex_Implementation_Spec.md index 0a8e3e2..b2ad7fd 100644 --- a/docs/specs/RadCounterSim_Codex_Implementation_Spec.md +++ b/docs/specs/RadCounterSim_Codex_Implementation_Spec.md @@ -1,88 +1,106 @@ -# RadCounterSim 実装仕様書 +# RadCounterSim Implementation Specification -## 0. この仕様書の目的 +## 0. Purpose -本仕様書は、NVIDIA Isaac Sim 上に、ロボット行動と放射線場変化を連成させた閉ループ型放射線源対策シミュレーター **RadCounterSim** を実装するための Codex 向け設計仕様である。 +This document is the Codex-facing design specification for implementing +**RadCounterSim**, a closed-loop radiation-source countermeasure simulator that couples +robot actions with changes in the radiation field on NVIDIA Isaac Sim. -実装対象は次の全機能を含む。 +The implementation scope includes all of the following capabilities: -- 移動ロボットによる環境内移動 -- ロボットアームによる遮蔽材、障害物、汚染物体の把持・移動・設置・撤去 -- 面線源、点線源、汚染物体に付随する線源の表現 -- 除染による線源強度の局所的減少 -- 遮蔽による線源―検出器間の伝達率の変化 -- 汚染物体の移動・撤去による線源位置・有無の変化 -- 非指向性検出器、回転遮蔽体付き検出器、エネルギービン付きカウント計測 -- 線量率マップと検出器カウントの高速再計算 -- 放射線源位置・強度・不確かさの推定 -- 対策前予測、対策実行、対策後再計測、予測―実測残差、再推定、再計画の閉ループ -- 除染残り、遮蔽材ずれ、未発見線源、推定スケール誤差の故障モード生成・識別 -- 計測時間、作業時間、対策回数、遮蔽材量、ロボット稼働時間を考慮する行動評価 -- GUI、ヘッドレス実験実行、ログ、再現性、単体試験、統合試験、性能ベンチマーク -- ROS 2 / MoveIt 2 / Nav2 連携を追加できる拡張インターフェース +- Mobile-robot navigation through the environment +- Robot-arm grasping, movement, placement, and removal of shields, obstacles, and + contaminated objects +- Representation of surface sources, point sources, and sources attached to contaminated objects +- Local reduction of source intensity through decontamination +- Changes in source-to-detector transmission through shielding +- Changes in source position or presence when contaminated objects move or are removed +- Omnidirectional detectors, detectors with rotating shields, and energy-binned counting +- Fast recalculation of dose-rate maps and detector counts +- Estimation of radiation-source position, intensity, and uncertainty +- A closed loop covering pre-action prediction, execution, post-action measurement, + predicted-versus-observed residuals, re-estimation, and replanning +- Generation and identification of residual contamination, shield displacement, hidden + sources, and estimation-scale errors +- Action evaluation that includes measurement time, work time, intervention count, + shielding material, and robot operating time +- A GUI, headless experiments, logging, reproducibility, unit and integration tests, and + performance benchmarks +- Extension interfaces for ROS 2, MoveIt 2, and Nav2 integration -## 1. 最重要設計判断 +## 1. Critical design decisions -### 1.1 OceanSim を直接フォークしない +### 1.1 Do not fork OceanSim directly -OceanSim は設計参考とし、RadCounterSim は独立した Isaac Sim extension として実装する。理由は次の通り。 +Use OceanSim as a design reference, but implement RadCounterSim as an independent Isaac +Sim extension for the following reasons: -- 水中カメラ・ソナー用コードと放射線輸送・対策コードを混在させない。 -- Isaac Sim の将来版への追従を容易にする。 -- 放射線カーネルを Isaac Sim 非依存で単体試験できるようにする。 -- 論文上の貢献を独立したシミュレーション基盤として示せる。 +- Keep underwater camera and sonar code separate from radiation transport and + countermeasure code. +- Make future Isaac Sim upgrades easier. +- Allow the radiation kernel to be unit-tested without Isaac Sim. +- Present the research contribution as an independent simulation platform. -### 1.2 Truth と Belief を完全分離する +### 1.2 Separate truth and belief completely -シミュレーション内部には次の二つの状態を持たせる。 +Maintain two distinct internal states: -- **TruthState**: 実際の線源分布、実際の除染効率、実際の遮蔽位置、未発見線源、検出器誤差など。 -- **BeliefState**: 推定された線源分布、推定不確かさ、計画時に仮定した対策効果。 +- **TruthState**: Actual source distribution, decontamination efficiency, shield pose, + hidden sources, detector errors, and other ground-truth quantities. +- **BeliefState**: Estimated source distribution, estimated uncertainty, and assumed + countermeasure effects used during planning. -Estimator と Planner は TruthState を参照してはならない。利用可能なのは公開環境形状、ロボット状態、計測結果、対策完了通知のみとする。 +Estimators and planners must never read `TruthState`. They may use only public +environment geometry, robot state, measurements, and countermeasure-completion notices. -### 1.3 放射線計算を物理ステップから分離する +### 1.3 Decouple radiation computation from physics steps -放射線場全体を毎 physics tick 再計算しない。計算を行うイベントは以下に限定する。 +Do not recompute the entire radiation field on every physics tick. Compute it only for: -- 計測要求 -- 線量マップ更新要求 -- 対策候補評価 -- 遮蔽・汚染物体・環境形状の pose 変更完了 -- 除染による activity 更新 -- 閉ループの各ステップ +- Measurement requests +- Dose-map update requests +- Candidate countermeasure evaluation +- Completed pose changes for shields, contaminated objects, or environment geometry +- Activity updates caused by decontamination +- Each step of the closed loop -### 1.4 二つの実行モードを持つ +### 1.4 Provide two execution modes -- **Deterministic action mode**: ナビゲーション・把持の成功を高レベルで判定し、最終 pose と作用だけを適用する。閉ループアルゴリズム評価と CI に使用。 -- **Physics action mode**: PhysX、コントローラ、グリッパ、MoveIt 2 等を使って実際にロボットを動かす。実機接続前の評価に使用。 +- **Deterministic action mode**: Determine navigation and grasp success at a high level + and apply only the final pose and effect. Use this mode for closed-loop algorithm + evaluation and CI. +- **Physics action mode**: Move robots with PhysX, controllers, grippers, MoveIt 2, and + related components. Use this mode for evaluation before hardware integration. -両モードで同じ `CountermeasureAction` と `ActionResult` を使用する。 +Both modes must use the same `CountermeasureAction` and `ActionResult` types. -## 2. 対象バージョンと開発環境 +## 2. Target versions and development environment -### 2.1 推奨固定環境 +### 2.1 Recommended pinned environment - Ubuntu 24.04 - NVIDIA Isaac Sim 6.0.1 - ROS 2 Jazzy -- Python は Isaac Sim 同梱環境 -- C++17 以上 -- Intel Embree 4 系 -- NumPy、SciPy、Pydantic、PyYAML、pandas、pyarrow +- Python from the Isaac Sim bundled environment +- C++17 or newer +- Intel Embree 4.x +- NumPy, SciPy, Pydantic, PyYAML, pandas, and pyarrow - pytest -`requirements-lock.txt` と `environment_manifest.json` にバージョンを固定すること。Isaac Sim 5.0 を使う必要がある場合は別ブランチを切り、同一コード内で大量の version conditional を書かない。 +Pin versions in `requirements-lock.txt` and `environment_manifest.json`. If Isaac Sim +5.0 support is necessary, create a separate branch instead of adding large numbers of +version conditionals to the same codebase. -### 2.2 開発方式 +### 2.2 Development approach -Isaac Sim source workspace で extension template を生成し、以下の三層に分割する。 +Generate extension templates in an Isaac Sim source workspace and divide the system into +three layers: -1. `radcounter.core`: Isaac Sim 非依存の Python モデル、推定、計画、ログ。 -2. `radcounter.radiation.native`: Embree を使う C++/pybind11 backend。 -3. `radcounter.isaac`: USD、UI、ロボット、物理、ROS 2 との接続。 +1. `radcounter.core`: Isaac Sim-independent Python models, estimation, planning, and logging. +2. `radcounter.radiation.native`: C++/pybind11 backend using Embree. +3. `radcounter.isaac`: Integration with USD, UI, robots, physics, and ROS 2. -## 3. リポジトリ構成 +## 3. Repository structure ```text RadCounterSim/ @@ -180,51 +198,52 @@ RadCounterSim/ └── data/ ``` -## 4. extension の役割 +## 4. Extension responsibilities ### 4.1 `radcounter.radiation.native` -責務: +Responsibilities: -- Embree device、scene、geometry、instance の生成・破棄 -- USD から抽出済みの三角形 mesh を受け取る -- source point と detector point の segment ray tracing -- 材料別通過長または energy 別 transmission のバッチ計算 -- 動的 object transform の更新 -- GIL を解放した並列計算 -- Embree が使えないときに明示的なエラーを返す +- Create and destroy Embree devices, scenes, geometry, and instances +- Receive triangle meshes extracted from USD +- Trace finite segments between source and detector points +- Batch-compute per-material path lengths or per-energy transmission +- Update dynamic-object transforms +- Perform parallel computation with the GIL released +- Return an explicit error when Embree is unavailable -この extension は UI や USD API を直接呼ばない。 +This extension must not call UI or USD APIs directly. ### 4.2 `radcounter.isaac` -責務: +Responsibilities: -- UI extension と Examples Browser 登録 +- UI extension and Examples Browser registration - scenario load/reset/clear -- USD mesh、transform、custom attribute の読み取り -- robot、sensor、shield、tool、contaminated object の生成 -- Physics callback と event subscription +- Read USD meshes, transforms, and custom attributes +- Create robots, sensors, shields, tools, and contaminated objects +- Physics callbacks and event subscriptions - visualization - optional ROS 2 bridge ### 4.3 `radcounter.core` -責務: +Responsibilities: -- TruthState、BeliefState、Action、Measurement の型 -- 放射線源、材料、検出器モデル -- 放射線演算の高レベル API -- activity map と cache 管理 -- 線源推定 +- Types for `TruthState`, `BeliefState`, actions, and measurements +- Radiation-source, material, and detector models +- High-level radiation-computation API +- Activity-map and cache management +- Source estimation - residual diagnosis - planner - closed-loop state machine -- experiment runner と logger +- Experiment runner and logger -## 5. 基本型と状態モデル +## 5. Core types and state model -すべての public data model は Pydantic v2 または frozen dataclass で定義する。単位を field 名に含める。 +Define every public data model with Pydantic v2 or a frozen dataclass. Include units in +field names. ```python from dataclasses import dataclass, field @@ -298,7 +317,7 @@ class RadiationMeasurement: scene_revision: int ``` -### 5.1 Revision 管理 +### 5.1 Revision management ```python @dataclass @@ -310,21 +329,25 @@ class RevisionState: detector_revision: int = 0 ``` -更新規則: +Update rules: -- 遮蔽材、障害物、汚染物体の pose 変更: `geometry_revision += 1` -- 材質・厚さ変更: `material_revision += 1` -- 線源付き物体移動: `source_pose_revision += 1` と必要に応じて `geometry_revision += 1` -- 除染: `source_activity_revision += 1` のみ -- detector calibration 変更: `detector_revision += 1` +- Shield, obstacle, or contaminated-object pose change: `geometry_revision += 1` +- Material or thickness change: `material_revision += 1` +- Movement of an object carrying a source: `source_pose_revision += 1`, plus + `geometry_revision += 1` when needed +- Decontamination: only `source_activity_revision += 1` +- Detector-calibration change: `detector_revision += 1` -除染時は transfer matrix を再 ray trace せず、既存行列と新 activity vector の積だけで再計算できる設計にする。 +Decontamination must not retrace the transfer matrix. Recompute using only the existing +matrix multiplied by the new activity vector. -## 6. USD metadata 実装 +## 6. USD metadata implementation -最初の版では独自 USD schema plugin を作らず、namespaced custom attribute を使用する。巨大な per-face activity array は `.npz` sidecar に保存し、USD には URI と checksum を格納する。 +The first version must use namespaced custom attributes instead of a custom USD schema +plugin. Store large per-face activity arrays in `.npz` sidecars and keep their URI and +checksum in USD. -### 6.1 共通属性 +### 6.1 Common attributes ```text rad:role token @@ -332,7 +355,7 @@ rad:enabled bool rad:objectId string ``` -`rad:role` の候補: +Allowed `rad:role` values: ```text source @@ -347,7 +370,7 @@ robot obstacle ``` -### 6.2 線源属性 +### 6.2 Source attributes ```text rad:source:type token point|surface|volume @@ -360,7 +383,7 @@ rad:source:hiddenFromEstimator bool rad:source:movableWithPrim bool ``` -### 6.3 遮蔽物属性 +### 6.3 Attenuator attributes ```text rad:material:id string @@ -371,7 +394,7 @@ rad:shield:movable bool rad:shield:resourceUnits double ``` -### 6.4 除染対象属性 +### 6.4 Decontamination-target attributes ```text rad:decon:enabled bool @@ -382,7 +405,7 @@ rad:decon:minToolDwellS double rad:decon:surfaceId string ``` -### 6.5 操作対象属性 +### 6.5 Manipulation-target attributes ```text rad:manipulation:movable bool @@ -394,7 +417,7 @@ rad:manipulation:disposalClass string ### 6.6 `UsdRadiationRegistry` -実装ファイル: +Implementation file: ```text radcounter/isaac/usd/radiation_registry.py @@ -414,17 +437,18 @@ class UsdRadiationRegistry: def get_decon_surfaces(self) -> list[DeconSurfaceDescriptor]: ... ``` -USD change notice を購読し、transform と radiation attribute の変更を分類して revision を更新する。変更ごとに全 stage を再走査しない。 +Subscribe to USD change notices, classify transform and radiation-attribute changes, and +update revisions. Do not rescan the entire stage for every change. -## 7. USD mesh 抽出 +## 7. USD mesh extraction -実装ファイル: +Implementation file: ```text radcounter/isaac/usd/mesh_extractor.py ``` -### 7.1 出力型 +### 7.1 Output type ```python @dataclass(frozen=True) @@ -441,17 +465,19 @@ class MeshGeometry: dynamic: bool ``` -### 7.2 実装要件 +### 7.2 Implementation requirements -- `UsdGeom.Mesh` の points、faceVertexCounts、faceVertexIndices を三角形化する。 -- quad と n-gon は fan triangulation ではなく、USD triangulation utility または安定な ear clipping を使う。 -- metersPerUnit を読み、すべて meter に変換する。 -- world transform は double で保持し、Embree 入力時に float32 へ変換する。 -- negative scale、non-uniform scale、instance、prototype を扱う。 -- material binding と `rad:material:id` を triangle ごとに解決する。 -- collision mesh と visual mesh が異なる場合、放射線計算用 mesh を明示属性で指定できるようにする。 -- `rad:transportMesh=true` を優先し、なければ render mesh を使用する。 -- source surface の triangle index と transport mesh の triangle index が一致するよう、抽出後の index map を保存する。 +- Triangulate `UsdGeom.Mesh` points, `faceVertexCounts`, and `faceVertexIndices`. +- For quads and n-gons, use a USD triangulation utility or stable ear clipping rather + than fan triangulation. +- Read `metersPerUnit` and convert all values to meters. +- Keep world transforms as double precision and convert to `float32` for Embree input. +- Handle negative scale, non-uniform scale, instances, and prototypes. +- Resolve material bindings and `rad:material:id` for each triangle. +- When collision and visual meshes differ, allow an explicit transport mesh for radiation. +- Prefer `rad:transportMesh=true`; otherwise use the render mesh. +- Preserve an extraction index map so source-surface triangle indices match transport-mesh + triangle indices. ## 8. Embree backend @@ -524,17 +550,18 @@ public: }; ``` -### 8.3 scene 構成 +### 8.3 Scene organization -- 静的 environment mesh は一つまたは material ごとの少数 geometry にまとめる。 -- 可動 shield、汚染物体、障害物は instance として登録し、transform update 後に commit する。 -- ray query は C++ 内で並列化する。 -- pybind11 binding は計算中 GIL を解放する。 -- scene commit と trace を同時実行しない。read-write lock を使用する。 +- Combine static environment meshes into one geometry or a small set grouped by material. +- Register movable shields, contaminated objects, and obstacles as instances, then commit + after transform updates. +- Parallelize ray queries in C++. +- Release the GIL during computation in pybind11 bindings. +- Use a read-write lock so scene commits and traces never run concurrently. ### 8.4 segment ray tracing -各 ray は source sample から detector までの有限 segment とする。 +Each ray is a finite segment from a source sample to a detector. ```text origin = source + eps * dir @@ -542,26 +569,30 @@ tnear = eps tfar = distance - eps ``` -最近接 hit を反復取得し、`tnear = hit_t + eps` として全交差を収集する。最大 hit 数を設定し、超過時は error flag を返す。 +Repeatedly retrieve the closest hit and set `tnear = hit_t + eps` to collect every +intersection. Enforce a maximum hit count and return an error flag when exceeded. -### 8.5 solid geometry の通過長 +### 8.5 Path length through solid geometry -- hit を距離順に保持する。 -- geom ID ごとに entry/exit を判定する。 -- normal orientation が信頼できる場合は `dot(ray_dir, geometric_normal)` で entry/exit を判断する。 -- orientation が不安定な mesh は geom ごとの hit を pair にして長さを合計する。 -- odd number の hit は invalid geometry として警告し、設定に応じて conservative または zero attenuation とする。 -- nested material は active geometry の material をすべて加算する。 +- Keep hits ordered by distance. +- Determine entry and exit for each geometry ID. +- When normal orientation is reliable, use `dot(ray_dir, geometric_normal)` to classify + entry and exit. +- For meshes with unreliable orientation, pair hits per geometry and sum their lengths. +- Warn that an odd number of hits is invalid geometry, then apply conservative or zero + attenuation according to configuration. +- For nested materials, add the materials of every active geometry. ### 8.6 thin sheet -一枚板、フィルム、簡略化した遮蔽板は `thin_sheet` とし、交差一回につき +Represent plates, films, and simplified shield panels as `thin_sheet`; add the following +for each intersection: ```text effective_thickness = thickness / max(abs(dot(ray_dir, normal)), cos_limit) ``` -を加算する。grazing angle で無限大にならないよう上限を設定する。 +Apply a cap to prevent an infinite value at grazing angles. ### 8.7 transmission @@ -569,44 +600,44 @@ effective_thickness = thickness / max(abs(dot(ray_dir, normal)), cos_limit) T(E) = exp(-sum_m mu_m(E) * length_m) ``` -数値 underflow を避けるため exponent を下限 clamp する。`trace_path_lengths` は debug・検証用、`trace_transmission` は高速通常経路とする。 +Clamp the exponent at a lower bound to avoid numerical underflow. Use +`trace_path_lengths` for debugging and validation and `trace_transmission` as the normal +fast path. ### 8.8 fallback backend -`AnalyticTransportBackend` を必ず実装する。 +Always implement `AnalyticTransportBackend` with: -- 遮蔽物なし -- 単一平板の解析式 -- CI で Embree 未インストール時にも core test を実行可能 +- No attenuator +- An analytic single-slab solution +- Core tests that can run in CI when Embree is not installed -## 9. 放射線源モデル +## 9. Radiation-source model ### 9.1 point source -各 isotope emission line に対して +For each isotope emission line, compute: ```text photon_rate_s = activity_bq * photons_per_decay fluence_rate = photon_rate_s / (4*pi*r^2) ``` -を計算する。 - ### 9.2 surface source -triangle ごとに activity を持たせる。 +Store activity per triangle. ```text activity_triangle_bq = surface_activity_bq_m2 * triangle_area_m2 ``` -quadrature mode: +Quadrature modes: -- `centroid`: triangle centroid 一点 -- `stratified_n`: triangle 内に n 点 -- `adaptive`: detector との距離と triangle サイズで n を調整 +- `centroid`: one point at the triangle centroid +- `stratified_n`: `n` points within the triangle +- `adaptive`: adjust `n` using detector distance and triangle size -各 sample は local coordinate と weight を持つ。 +Each sample has local coordinates and a weight. ```python @dataclass(frozen=True) @@ -615,14 +646,16 @@ class SourceSampleBatch: activity_bq: np.ndarray # [S] isotope_index: np.ndarray # [S] source_id_index: np.ndarray # [S] - triangle_index: np.ndarray # [S], point source は -1 + triangle_index: np.ndarray # [S], -1 for a point source ``` -可動 object に付随する sample は local position を cache し、object transform 変更時に world position だけ更新する。 +For samples attached to movable objects, cache local positions and update only world +positions when the object transform changes. ### 9.3 volume source -初回論文に必須ではないが interface は用意する。voxel center と voxel activity の sample batch に変換する。 +This is not required for the first paper, but provide the interface. Convert voxel centers +and voxel activity into a sample batch. ### 9.4 activity repository @@ -635,9 +668,9 @@ class SourceRepository: def deactivate_source(self, source_id): ... ``` -Truth と Belief の repository instance は別にする。 +Use separate repository instances for truth and belief. -## 10. 放射線 forward model +## 10. Radiation forward model ```python class RadiationForwardModel: @@ -653,18 +686,18 @@ class RadiationForwardModel: def build_transfer_matrix(...): ... ``` -energy line `e`、source sample `s`、detector pose `d` の寄与: +Contribution from energy line `e`, source sample `s`, and detector pose `d`: ```text lambda_sde = A_s * Y_e * G(r_sd) * T_sd(E_e) * epsilon_d(E_e, theta_sd) ``` - `G(r)=1/(4*pi*max(r,r_min)^2)` -- `T` は Embree attenuation -- `epsilon` は detector response interpolation -- energy bin に集約 -- background を加える -- dead time model を適用 +- `T` is Embree attenuation. +- `epsilon` is detector-response interpolation. +- Aggregate into energy bins. +- Add background. +- Apply the dead-time model. ### 10.1 direct/scatter plugin @@ -673,17 +706,19 @@ class ScatterModel(Protocol): def add_scatter(self, direct_prediction, context) -> np.ndarray: ... ``` -実装: +Implementations: - `NoScatterModel` - `EmpiricalBuildupModel` -- `TruthOnlyBiasModel`: ground truth 側だけに spatial bias、energy redistribution、background drift を与え、推定モデルとの mismatch を生成 +- `TruthOnlyBiasModel`: Apply spatial bias, energy redistribution, and background drift + only on the ground-truth side to create mismatch with the estimation model. -散乱を未実装のまま暗黙に無視せず、設定ファイルとログに必ず model 名を保存する。 +Never silently ignore unimplemented scatter. Always store the model name in configuration +and logs. -## 11. detector 実装 +## 11. Detector implementation -### 11.1 センサ階層 +### 11.1 Sensor hierarchy ```python class RadiationSensor: @@ -697,14 +732,15 @@ class RotatingShieldCounter(RadiationSensor): ... class DoseRateMeter(RadiationSensor): ... ``` -### 11.2 計測状態機械 +### 11.2 Measurement state machine ```text IDLE -> INTEGRATING -> FINALIZING -> READY \-> CANCELLED ``` -積算中に detector が動く場合は `trajectory_subsamples` 回だけ pose を採取して平均 rate を求める。初期設定は stationary measurement とする。 +If the detector moves during integration, sample its pose `trajectory_subsamples` times +and compute the average rate. The default configuration uses stationary measurement. ### 11.3 Poisson sampling @@ -713,27 +749,29 @@ expected_counts_bin = rate_cps_bin * duration_s observed_counts_bin ~ Poisson(expected_counts_bin) ``` -乱数 generator は run seed から detector ごとの child seed を作る。再現性試験で完全一致すること。 +Derive a child seed for each detector from the run seed. Reproducibility tests must match +exactly. -### 11.4 回転遮蔽体 +### 11.4 Rotating shield -二方式を実装する。 +Implement two modes: -1. `physical_geometry`: 実際の遮蔽体 mesh を detector 周囲で回転し、Embree で減衰を計算。 -2. `response_mask`: 事前計算した角度 response を掛ける高速モード。 +1. `physical_geometry`: Rotate an actual shield mesh around the detector and compute + attenuation with Embree. +2. `response_mask`: Fast mode that applies a precomputed angular response. -回転角、回転速度、各角度の積算時間、encoder noise を記録する。 +Record angle, rotation speed, integration time at each angle, and encoder noise. -## 12. transfer matrix と cache +## 12. Transfer matrix and cache -### 12.1 行列定義 +### 12.1 Matrix definition ```text y = H x + b ``` -- `x`: candidate source basis の activity -- `H`: detector pose × energy bin × candidate basis の unit-activity count response +- `x`: Activity of the candidate source basis +- `H`: Unit-activity count response over detector pose, energy bin, and candidate basis - `b`: background ### 12.2 cache key @@ -749,19 +787,21 @@ class TransferMatrixKey: energy_grid_hash: str ``` -`source_activity_revision` は key に含めない。除染は `x` のみを変える。 +Do not include `source_activity_revision` in the key. Decontamination changes only `x`. ### 12.3 partial invalidation -- shield pose 変更: geometry revision により全体無効化する MVP を実装。 -- その後、shield bounding box と交差可能な ray の行のみ再計算する optional optimization。 -- movable source pose 変更: source basis columns のみ更新。 +- Shield-pose change: for the MVP, invalidate everything through the geometry revision. +- Optional later optimization: recompute only ray rows that can intersect the shield's + bounding box. +- Movable-source pose change: update only source-basis columns. -### 12.4 chunk 計算 +### 12.4 Chunked computation -大規模 map は detector evaluation points と source samples を chunk し、最大一時メモリを設定値以下にする。 +For large maps, chunk detector evaluation points and source samples so peak temporary +memory remains below the configured limit. -## 13. 線量マップ +## 13. Dose maps ```python class DoseMapEvaluator: @@ -771,7 +811,7 @@ class DoseMapEvaluator: def evaluate(grid, state, chunk_size) -> DoseMap: ... ``` -出力: +Output: ```python @dataclass(frozen=True) @@ -782,9 +822,9 @@ class DoseMap: revision: RevisionState ``` -## 14. visualization +## 14. Visualization -実装ファイル: +Implementation files: ```text radcounter/isaac/visualization/ @@ -795,15 +835,17 @@ radcounter/isaac/visualization/ └── action_visualizer.py ``` -要件: +Requirements: -- 2D heatmap は `UsdGeom.Points` または instancer を使い、cell ごとの cube を大量生成しない。 -- color range は fixed、percentile、log の三方式。 -- Truth source overlay は debug 権限かつ明示 toggle 時のみ表示。 -- Belief source、uncertainty、predicted post-action、observed post-action、normalized residual を別 layer にする。 -- selected ray の material path と通過長を表示できる。 +- Use `UsdGeom.Points` or an instancer for 2D heatmaps; do not create large numbers of + per-cell cubes. +- Support fixed, percentile, and logarithmic color ranges. +- Show the truth-source overlay only with debug authorization and an explicit toggle. +- Put belief sources, uncertainty, predicted post-action values, observed post-action + values, and normalized residuals on separate layers. +- Display material paths and path lengths for selected rays. -## 15. robot abstraction +## 15. Robot abstraction ```python class RobotController(Protocol): @@ -819,28 +861,30 @@ class IsaacPhysicsRobotController(RobotController): ... class Ros2RobotController(RobotController): ... ``` -### 15.1 measurement robot +### 15.1 Measurement robot -- differential drive または omnidirectional mobile base -- detector mast -- detector pose は robot base transform と sensor extrinsic から取得 -- Nav2 接続は optional +- Differential-drive or omnidirectional mobile base +- Detector mast +- Derive detector pose from the robot-base transform and sensor extrinsics +- Optional Nav2 integration -### 15.2 countermeasure robot +### 15.2 Countermeasure robot -- mobile base + manipulator + gripper を推奨 -- 初期段階では fixed manipulator でもよいが、controller interface は mobile manipulator を前提にする -- shield、obstacle、contaminated object を操作できる -- decon tool を tool changer または固定 attachment として持つ +- A mobile base, manipulator, and gripper are recommended. +- A fixed manipulator is acceptable initially, but design the controller interface for a + mobile manipulator. +- It can manipulate shields, obstacles, and contaminated objects. +- It carries the decontamination tool through a tool changer or fixed attachment. -### 15.3 grasp 実装 +### 15.3 Grasp implementation -二方式: +Two modes: -- deterministic: target の grasp frame 到達可能性と collision-free 条件を確認後、target prim を gripper prim に parent/constraint する。 -- physics: surface gripper または fixed joint constraint を使用し、接触と相対 pose を確認する。 +- Deterministic: after verifying reachability of the target grasp frame and a collision-free + path, parent or constrain the target prim to the gripper prim. +- Physics: use a surface gripper or fixed-joint constraint and verify contact and relative pose. -把持終了後に対象 pose を radiation registry に反映する。 +After grasping, reflect the target pose in the radiation registry. ## 16. action model @@ -878,13 +922,13 @@ class ActionResult: after_revision: RevisionState ``` -`truth_details` は experiment logger のみアクセス可能で、Estimator/Planner には渡さない。 +Only the experiment logger may access `truth_details`; never pass it to estimators or planners. -## 17. 除染実装 +## 17. Decontamination implementation ### 17.1 activity map -surface source は triangle ごとの activity を `.npz` で持つ。 +Store surface-source activity per triangle in `.npz`. ```text triangle_indices @@ -908,16 +952,16 @@ class DeconToolSpec: rate_constant_s_inv: float ``` -physics tick ごとに footprint sample ray を tool axis 方向に cast する。 +On each physics tick, cast footprint sample rays along the tool axis. -有効接触条件: +Valid-contact conditions: -- target surface までの距離が閾値以下 -- tool axis と surface normal の角度が閾値以下 -- end-effector の surface tangential speed が上限以下 -- target prim が `rad:decon:enabled=true` +- Distance to the target surface is at or below the threshold. +- Angle between the tool axis and surface normal is at or below the threshold. +- End-effector tangential speed over the surface is at or below the limit. +- The target prim has `rad:decon:enabled=true`. -triangle ごとに exposure を積算する。 +Accumulate exposure per triangle. ```text E_i += contact_weight * dt @@ -926,23 +970,24 @@ nominal_removal_fraction_i = 1 - exp(-k * E_i) ### 17.3 ground truth failure -Truth 側では +On the truth side, use: ```text actual_fraction_i = clamp(nominal_fraction_i * local_efficiency_i, 0, 1) local_efficiency_i ~ spatially correlated random field ``` -とし、未処理 spot、工具位置ずれ、効率ばらつきを生成できる。 +This allows generation of untreated spots, tool-position offsets, and efficiency variation. ### 17.4 removed activity -設定により二方式: +Support two configured modes: -- `discard`: 除去 activity を scene から消す。 -- `transfer_to_waste`: 除去 activity を waste container source に移す。 +- `discard`: Remove treated activity from the scene. +- `transfer_to_waste`: Move treated activity to a waste-container source. -研究用には後者を推奨する。除染しただけで放射能が消滅したことにしない。 +The second mode is recommended for research; do not imply that radioactivity disappears +merely because a surface was decontaminated. ### 17.5 API @@ -952,11 +997,11 @@ class DecontaminationExecutor: def preview_nominal_effect(self, action, belief_state) -> SourceStateDelta: ... ``` -## 18. 遮蔽実装 +## 18. Shielding implementation ### 18.1 shield asset -各 shield asset は以下を持つ。 +Each shield asset contains: - visual mesh - collider @@ -965,7 +1010,7 @@ class DecontaminationExecutor: - support/contact frame - radiation transport mesh - material ID -- solid または thin sheet mode +- Solid or thin-sheet mode - nominal thickness - resource units @@ -976,38 +1021,42 @@ PLAN -> NAVIGATE_TO_SHIELD -> GRASP -> NAVIGATE_TO_TARGET -> PLACE -> RELEASE -> WAIT_SETTLE -> COMMIT_RADIATION_SCENE -> COMPLETE ``` -`WAIT_SETTLE` では線速度・角速度が閾値以下になるまで待つ。timeout 時は FAILED または PARTIAL。 +In `WAIT_SETTLE`, wait until linear and angular velocity fall below their thresholds. On +timeout, return `FAILED` or `PARTIAL`. -### 18.3 遮蔽ずれ +### 18.3 Shield displacement -Truth mode では target pose に対し平行移動・回転誤差を加える。 +In truth mode, add translational and rotational errors to the target pose. ```text actual_pose = target_pose * pose_error_transform ``` -pose は USD から読み取った actual pose を radiation scene に反映する。Planner の predicted pose を直接使わない。 +Read the actual pose from USD and apply it to the radiation scene. Do not use the planner's +predicted pose directly. -### 18.4 即時効果測定 +### 18.4 Immediate effect measurement -shield placement 完了後: +After shield placement: -1. geometry revision を更新 -2. Embree instance transform 更新・commit -3. selected verification poses の predicted dose を計算 -4. measurement robot を verification pose に移動 -5. measurement 実行 -6. residual を生成 +1. Update the geometry revision. +2. Update and commit the Embree instance transform. +3. Compute predicted dose at selected verification poses. +4. Move the measurement robot to a verification pose. +5. Perform the measurement. +6. Generate the residual. -## 19. 汚染物体・障害物の移動と撤去 +## 19. Moving and removing contaminated objects and obstacles ### 19.1 contaminated object -source sample は object local frame に保持する。object pose 更新時に world sample pose を更新する。 +Store source samples in the object's local frame. Update their world poses when the object +pose changes. ### 19.2 obstacle -非汚染 obstacle は線源を持たないが、robot reachability、path planning、放射線 attenuation に影響し得る。`rad:material:id` が設定されていれば attenuation geometry として登録する。 +A non-contaminated obstacle has no source, but can affect robot reachability, path planning, +and radiation attenuation. If `rad:material:id` is set, register it as attenuation geometry. ### 19.3 move action @@ -1015,29 +1064,30 @@ source sample は object local frame に保持する。object pose 更新時に NAVIGATE -> GRASP/PUSH -> MOVE -> RELEASE -> SETTLE -> UPDATE SOURCE/GEOMETRY ``` -push と pick の action subtype を持つ。 +Support push and pick action subtypes. ### 19.4 remove action -単に source を API で無効化してはならない。対象が disposal zone に入ったことを検証した後に、次のいずれかを行う。 +Do not simply disable a source through the API. After verifying that the target entered a +disposal zone, do one of the following: -- disposal container の shielding を含めた状態で scene 内に残す -- evaluation domain 外へ搬出して source を deactivate +- Leave it in the scene with the disposal container's shielding. +- Move it outside the evaluation domain and deactivate the source. -ログに除去前後の activity 保存先を記録する。 +Log where activity is stored before and after removal. -## 20. measurement・source estimation +## 20. Measurement and source estimation ### 20.1 candidate basis -最初の実装は二種類。 +The initial implementation supports two basis types: -- 3D grid basis: unknown point/small voxel source 用 -- surface triangle basis: 床・壁・物体表面の汚染分布用 +- 3D grid basis for unknown point or small voxel sources +- Surface-triangle basis for contamination on floors, walls, and object surfaces ### 20.2 Poisson sparse estimator -観測 count `y`、response matrix `H`、background `b` に対し +For observed counts `y`, response matrix `H`, and background `b`, solve: ```text minimize_x>=0 sum_i [(Hx+b)_i - y_i log((Hx+b)_i)] @@ -1045,9 +1095,7 @@ minimize_x>=0 sum_i [(Hx+b)_i - y_i log((Hx+b)_i)] + lambda_tv TV(x) ``` -を解く。 - -実装クラス: +Implementation classes: ```python class SourceEstimator(Protocol): @@ -1061,16 +1109,16 @@ class PFPlusMLEEstimator(SourceEstimator): ... # optional phase ### 20.3 solver -初期版は SciPy を使用し、次を実装する。 +The initial version uses SciPy and implements: -- nonnegative L-BFGS-B for unregularized MLE -- proximal gradient/FISTA for L1 -- graph incidence matrix を使う TV proximal の簡略版、または split Bregman -- gradient test を finite difference で検証 +- Nonnegative L-BFGS-B for unregularized MLE +- Proximal gradient or FISTA for L1 +- A simplified TV proximal using a graph incidence matrix, or split Bregman +- Finite-difference verification of gradient tests ### 20.4 local refinement -sparse grid で得た上位候補を連続座標 MLE で refinement する。 +Refine leading sparse-grid candidates with continuous-coordinate MLE. ```text coarse sparse grid -> connected components -> source seeds @@ -1079,16 +1127,16 @@ coarse sparse grid -> connected components -> source seeds ### 20.5 uncertainty -最低限、active set 上の Fisher information 近似を実装する。 +At minimum, implement a Fisher-information approximation on the active set. ```text F = H_A^T diag(1/max(lambda, eps)) H_A + regularization Cov = pseudo_inverse(F) ``` -bootstrap option も用意する。 +Also provide a bootstrap option. -### 20.6 出力 +### 20.6 Output ```python @dataclass(frozen=True) @@ -1107,7 +1155,7 @@ class SourceEstimate: ### 21.1 predicted post-action -Planner は BeliefState の clone に action の nominal effect を適用する。 +The planner applies the action's nominal effect to a clone of `BeliefState`. ```python predicted_belief_after = action_model.preview(action, belief_before) @@ -1119,7 +1167,8 @@ predicted_measurement = forward_model.predict( ### 21.2 observed post-action -ActionExecutor は TruthState に stochastic actual effect を適用し、実際の USD pose と source activity を更新する。verification measurement を実施する。 +The action executor applies a stochastic actual effect to `TruthState`, updates the actual +USD pose and source activity, and performs a verification measurement. ### 21.3 normalized residual @@ -1128,7 +1177,7 @@ r = y_observed - y_predicted z = r / sqrt(max(y_predicted + variance_model, 1)) ``` -energy bin、pose、time を保持する。 +Preserve energy bin, pose, and time. ### 21.4 residual hypotheses @@ -1146,15 +1195,17 @@ class SourceLocalizationErrorHypothesis: ... #### DeconResidualHypothesis -処理領域内 activity の残存係数を回帰する。 +Regress the residual activity coefficient inside the treated region. #### ShieldPoseErrorHypothesis -nominal pose 周辺の有限候補を生成し、各候補で predicted measurement を再計算して likelihood 最大の pose correction を求める。 +Generate a finite set of candidates around the nominal pose, recompute predicted +measurements for each, and select the maximum-likelihood pose correction. #### HiddenSourceHypothesis -既知 source contribution を引いた residual に対し、未使用 candidate basis 上で sparse inversion を行う。 +Subtract known source contributions from the residual and perform sparse inversion over +unused candidate bases. #### GlobalGainBackgroundHypothesis @@ -1162,11 +1213,12 @@ nominal pose 周辺の有限候補を生成し、各候補で predicted measurem y_observed ≈ gain * y_predicted + background_offset ``` -を fit する。 +Fit this model. ### 21.5 hypothesis selection -各仮説について negative log-likelihood と parameter 数から BIC を計算する。最良仮説と confidence を返し、BeliefState と action effect parameter を更新する。 +For each hypothesis, compute BIC from negative log likelihood and parameter count. Return +the best hypothesis and confidence, then update `BeliefState` and action-effect parameters. ## 22. planner @@ -1183,20 +1235,18 @@ class ActionCandidateGenerator: ### 22.2 feasibility -各 candidate に対し: +For each candidate, evaluate: -- mobile path の有無 -- manipulator reachability +- Availability of a mobile path +- Manipulator reachability - collision - grasp frame - shield placement stability -- disposal zone capacity -- resource availability -- robot availability - -を評価する。 +- Disposal-zone capacity +- Resource availability +- Robot availability -MVP では deterministic geometric feasibility、physics mode では controller dry-run を使う。 +Use deterministic geometric feasibility for the MVP and a controller dry run in physics mode. ### 22.3 objective @@ -1211,7 +1261,7 @@ score(a) = - w_info * expected_information_gain(a) ``` -小さいほど良いとする。 +Lower scores are better. ### 22.4 resource state @@ -1228,14 +1278,14 @@ class ResourceState: ### 22.5 baseline planners -論文比較用に必ず実装する。 +Always implement these planners for paper comparisons: - `OpenLoopPlanner` - `GreedyDoseReductionPlanner` - `NearestSourcePlanner` - `RandomPlanner` -- `OraclePlanner` — TruthState を使うが実験評価専用 -- `ClosedLoopResidualPlanner` — 提案手法 +- `OraclePlanner` — uses `TruthState`, but only for experimental evaluation +- `ClosedLoopResidualPlanner` — proposed method ## 23. closed-loop orchestrator @@ -1263,7 +1313,7 @@ class ClosedLoopCoordinator: def stop(self) -> None: ... ``` -遷移: +Transitions: ```text INITIALIZE @@ -1278,20 +1328,20 @@ INITIALIZE -> PLAN or COMPLETE ``` -終了条件: +Termination conditions: -- task path dose が閾値以下 -- peak dose が閾値以下 -- resources 枯渇 -- 最大 step 数 -- 有効 action なし +- Task-path dose is below the threshold +- Peak dose is below the threshold +- Resources are exhausted +- Maximum step count is reached +- No valid action remains - safety violation -各遷移で immutable snapshot を保存する。 +Save an immutable snapshot at every transition. ## 24. UI -`radcounter.isaac` は current UI template を基に実装する。 +Implement `radcounter.isaac` from the current UI template. ### 24.1 Frames @@ -1335,11 +1385,12 @@ INITIALIZE - run ID - save report -UI callback 内で重い計算を同期実行しない。async task を生成し、進捗と cancel token を管理する。 +Do not run heavy computation synchronously inside UI callbacks. Create asynchronous tasks +and manage progress and cancellation tokens. ## 25. ROS 2 interface -core functionality は ROS 2 なしで動作する。ROS 2 は adapter とする。 +Core functionality must work without ROS 2. Treat ROS 2 as an adapter. ### 25.1 messages @@ -1392,7 +1443,7 @@ ResetEpisode.srv ### 25.3 robot topics -標準 `/tf`, `/joint_states`, `/cmd_vel`, FollowJointTrajectory、MoveIt 2 を使用する。 +Use standard `/tf`, `/joint_states`, `/cmd_vel`, `FollowJointTrajectory`, and MoveIt 2. ## 26. scenario configuration @@ -1501,9 +1552,10 @@ outputs: save_video: false ``` -`validate_scenario.py` で schema、asset path、unit、prim path、material table の整合性を Isaac Sim 起動前に検査する。 +Before starting Isaac Sim, use `validate_scenario.py` to check consistency among the +schema, asset paths, units, prim paths, and material table. -## 27. logging と実験再現性 +## 27. Logging and experiment reproducibility run directory: @@ -1528,13 +1580,13 @@ outputs//_/ - dirty status - Isaac Sim version - Embree version -- OS、CPU、GPU +- OS, CPU, and GPU - Python package versions - random seeds - config SHA256 - asset SHA256 -重要な event: +Important events: - scene loaded - radiation scene committed @@ -1551,44 +1603,44 @@ outputs//_/ ### 28.1 unit tests — radiation -1. 自由空間 point source が `1/r^2` に従う。 -2. 単一 slab が `exp(-mu*l)` に従う。 -3. 二材料 slab の exponent が加算される。 -4. thin sheet の斜入射厚さが正しい。 -5. closed cube の entry/exit 通過長が正しい。 -6. nested solids が正しい。 -7. surface source rectangle が高密度数値積分の基準値に収束する。 -8. zero activity、disabled source、zero efficiency を扱える。 -9. Poisson seed で結果が再現する。 -10. cache invalidation が revision 規則どおり。 +1. A point source in free space follows `1/r^2`. +2. A single slab follows `exp(-mu*l)`. +3. Exponents from a two-material slab add correctly. +4. Oblique-incidence thickness is correct for a thin sheet. +5. Entry/exit path length through a closed cube is correct. +6. Nested solids are correct. +7. A rectangular surface source converges to a high-density numerical-integration reference. +8. Zero activity, disabled sources, and zero efficiency are handled. +9. A Poisson seed reproduces results. +10. Cache invalidation follows the revision rules. ### 28.2 unit tests — actions -1. 50% nominal decon で対象 triangle activity が半減する。 -2. 非対象 triangle は変化しない。 -3. repeated decon が累積する。 -4. removed activity が waste source に移る。 -5. shield placement で geometry revision が増える。 -6. shield move で transmission が変化する。 -7. contaminated object move で source sample pose が追従する。 -8. disposal zone 前には source が deactivate されない。 +1. A nominal 50% decontamination action halves target-triangle activity. +2. Non-target triangles do not change. +3. Repeated decontamination accumulates. +4. Removed activity moves to the waste source. +5. Shield placement increments the geometry revision. +6. Shield movement changes transmission. +7. Source-sample poses follow movement of a contaminated object. +8. A source is not deactivated before reaching the disposal zone. ### 28.3 unit tests — estimation -1. noiseless single source を回収する。 -2. Poisson noisy single source の平均誤差が許容範囲。 -3. two-source case。 -4. surface sparse patch。 -5. regularization zero と nonzero。 -6. gradient finite-difference check。 -7. Fisher covariance shape/positive semidefinite。 +1. Recover a noiseless single source. +2. Mean error for a Poisson-noisy single source is within tolerance. +3. Two-source case. +4. Sparse surface patch. +5. Test zero and nonzero regularization. +6. Finite-difference gradient check. +7. Fisher covariance shape and positive semidefiniteness. ### 28.4 unit tests — residual -1. decon residual hypothesis を正しく選ぶ。 -2. shield translation error を近似回収する。 -3. hidden source を新規 candidate として検出する。 -4. global gain error を source error と誤分類しにくい。 +1. Select the decontamination-residual hypothesis correctly. +2. Approximately recover shield translation error. +3. Detect a hidden source as a new candidate. +4. Avoid misclassifying global gain error as source error. ### 28.5 integration tests — headless Isaac @@ -1617,7 +1669,8 @@ outputs//_/ ### 28.6 regression tests -fixed seed の小シナリオについて、主要 metric と map hash を保存する。version update で差が出た場合は理由をレビューする。 +For small fixed-seed scenarios, save key metrics and map hashes. Review the reason for any +difference after a version update. ## 29. validation criteria @@ -1625,29 +1678,30 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - free-space analytic relative error < 1e-6 - slab attenuation relative error < 1e-4 -- surface quadrature convergence を report -- material path debug で期待長と一致 +- Report surface-quadrature convergence. +- Material-path debugging matches expected lengths. ### 29.2 functional -- 全 action が deterministic mode で完走 -- physics mode で最低一つの shield pick-and-place と obstacle move が完走 -- decon footprint が tool path に沿って activity map を更新 -- action 後の detector measurement が scene state を反映 -- estimator が TruthState を参照していないことを test double で保証 +- Every action completes in deterministic mode. +- At least one shield pick-and-place and obstacle movement completes in physics mode. +- The decontamination footprint updates the activity map along the tool path. +- Detector measurements after an action reflect scene state. +- A test double proves that the estimator does not read `TruthState`. ### 29.3 performance target -基準機を manifest に明記し、次を初期目標とする。 +Identify reference hardware in the manifest and use these initial targets: -- 100,000 segment rays の transmission query: 0.2 s 以下 -- 2D 128×128 map、source samples 2,000、cache 未使用: 2 s 以下 -- activity-only update 後の map: 0.1 s 以下 -- shield pose update + verification 32 poses: 0.5 s 以下 +- Transmission query for 100,000 segment rays: at most 0.2 s +- 2D 128 x 128 map with 2,000 source samples and no cache: at most 2 s +- Map after an activity-only update: at most 0.1 s +- Shield-pose update plus verification at 32 poses: at most 0.5 s -未達でも correctness を優先し、benchmark result を保存する。性能値を論文に使う場合は reference hardware と scene complexity を併記する。 +Prioritize correctness even when a target is missed, and save benchmark results. When +publishing performance values, report the reference hardware and scene complexity. -## 30. 実装順序 +## 30. Implementation order ### Milestone 0: scaffold @@ -1657,7 +1711,7 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - config validation - logging skeleton -完了条件: Isaac Sim で UI が表示され、headless startup test が通る。 +Completion criterion: the UI appears in Isaac Sim and the headless startup test passes. ### Milestone 1: analytic radiation core @@ -1668,7 +1722,7 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - analytic backend - unit tests -完了条件: free-space/slab tests pass。 +Completion criterion: free-space and slab tests pass. ### Milestone 2: USD registry + mesh extraction @@ -1677,7 +1731,8 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - mesh triangulation - revision management -完了条件: demo USD から期待した source/attenuator descriptors が得られる。 +Completion criterion: the expected source and attenuator descriptors are obtained from +the demo USD. ### Milestone 3: Embree backend @@ -1687,7 +1742,7 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - multi-hit path length - benchmarks -完了条件: analytic slab/cube tests と batch performance test pass。 +Completion criterion: analytic slab/cube tests and the batch performance test pass. ### Milestone 4: source sampling + radiation sensor @@ -1696,7 +1751,7 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - Poisson measurement - rotating shield sensor -完了条件: robot-mounted sensor が計測を返す。 +Completion criterion: a robot-mounted sensor returns a measurement. ### Milestone 5: map/cache/visualization @@ -1705,7 +1760,8 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - dose map - UI visualization -完了条件: decon activity update が ray rebuild なしに map を更新。 +Completion criterion: a decontamination activity update refreshes the map without +rebuilding rays. ### Milestone 6: deterministic countermeasure actions @@ -1715,7 +1771,7 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - resources - action state machine -完了条件: 全 action の統合試験 pass。 +Completion criterion: integration tests for every action pass. ### Milestone 7: robot physics execution @@ -1726,7 +1782,7 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - obstacle/source object movement - decon tool contact -完了条件: physics mode smoke test pass。 +Completion criterion: the physics-mode smoke test passes. ### Milestone 8: estimation @@ -1735,7 +1791,7 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - uncertainty - estimate visualization -完了条件: synthetic recovery tests pass。 +Completion criterion: synthetic recovery tests pass. ### Milestone 9: residual diagnosis @@ -1744,7 +1800,7 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - four residual hypotheses - belief update -完了条件: fault injection tests pass。 +Completion criterion: fault-injection tests pass. ### Milestone 10: planner + closed loop @@ -1755,7 +1811,9 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - baselines - coordinator -完了条件: closed-loop demo が open-loop baseline より指定 metric で改善するテストを生成。ただし固定の有意差を CI pass 条件にはしない。 +Completion criterion: generate a test in which the closed-loop demo improves the specified +metric over an open-loop baseline. Do not require a fixed statistical significance margin +for CI to pass. ### Milestone 11: ROS 2 and experiment automation @@ -1764,39 +1822,43 @@ fixed seed の小シナリオについて、主要 metric と map hash を保存 - batch runner - reports -完了条件: ROS 2 measurement と action round trip、headless sweep 実行。 +Completion criterion: ROS 2 measurement and action round trips work, and the headless +sweep runs. ## 31. Definition of Done -次の全項目を満たした時点で「シミュレーション実装完了」とする。 - -- GUI と headless の双方で起動できる。 -- point/source surface/contaminated object source を扱える。 -- air/solid/thin-sheet attenuation を扱える。 -- decon、shield、move、remove が scene と放射線場を更新する。 -- mobile robot と manipulator の action interface がある。 -- deterministic mode で全 action が完走する。 -- physics mode で shield と object manipulation の実例がある。 -- detector count と dose map を action 前後で即時更新できる。 -- source estimation と uncertainty が動く。 -- predicted/observed/residual が保存・可視化される。 -- residual に基づく belief update が動く。 -- resource-constrained planner と baselines が動く。 -- TruthState leakage test が通る。 -- 全 unit/integration tests が通る。 -- version/seed/config/hardware を含む manifest が保存される。 -- API と scenario schema の文書がある。 -- 一つの end-to-end closed-loop demo と再現スクリプトがある。 - -## 32. 実装上の禁止事項 - -- Estimator/Planner から TruthState にアクセスしない。 -- 除染と遮蔽を同一の activity scale 操作として実装しない。 -- 遮蔽設置後に predicted pose をそのまま radiation engine に渡さず、USD actual pose を読む。 -- 毎 physics tick に全 dose map を再計算しない。 -- 巨大 per-face activity array を USD custom attribute に直接埋め込まない。 -- source activity、dose、count、length の単位を曖昧にしない。 -- UI callback で重い同期計算をしない。 -- 物理データ値を出典・version なしに hardcode しない。 -- action 実装を単なる見た目の animation にしない。scene state と radiation state の一貫性を必ず更新する。 -- action 後の効果を TruthState の差から Planner に直接伝えない。必ず再計測を経由する。 +The simulator is implementation-complete only when every item below is satisfied: + +- It starts in both GUI and headless modes. +- It supports point sources, surface sources, and sources on contaminated objects. +- It supports air, solid, and thin-sheet attenuation. +- Decontamination, shielding, movement, and removal update both scene and radiation field. +- Mobile-robot and manipulator action interfaces exist. +- Every action completes in deterministic mode. +- Physics mode includes demonstrated shield and object manipulation. +- Detector counts and dose maps update immediately before and after actions. +- Source estimation and uncertainty work. +- Predicted values, observations, and residuals are saved and visualized. +- Belief updates based on residuals work. +- The resource-constrained planner and baselines work. +- The `TruthState` leakage test passes. +- All unit and integration tests pass. +- A manifest containing version, seed, configuration, and hardware is saved. +- The API and scenario schema are documented. +- One end-to-end closed-loop demo and reproduction script exist. + +## 32. Prohibited implementation practices + +- Never access `TruthState` from an estimator or planner. +- Do not implement decontamination and shielding as the same activity-scaling operation. +- After shield placement, read the actual USD pose instead of passing the predicted pose + directly to the radiation engine. +- Do not recompute the entire dose map on every physics tick. +- Do not embed large per-face activity arrays directly in USD custom attributes. +- Do not leave units for source activity, dose, count, or length ambiguous. +- Do not run heavy synchronous computation in UI callbacks. +- Do not hardcode physical data without a source and version. +- Do not implement actions as visual animation only. Always update scene state and radiation + state consistently. +- Do not reveal post-action effects to the planner through direct `TruthState` differences. + Always obtain them through remeasurement. diff --git a/docs/specs/RadCounterSim_Codex_Milestone_Prompts.md b/docs/specs/RadCounterSim_Codex_Milestone_Prompts.md index 8c1f5fd..94e2df8 100644 --- a/docs/specs/RadCounterSim_Codex_Milestone_Prompts.md +++ b/docs/specs/RadCounterSim_Codex_Milestone_Prompts.md @@ -1,180 +1,185 @@ -# RadCounterSim — Codex 用段階実装プロンプト +# RadCounterSim — Staged Codex Implementation Prompts -## 使用方法 +## How to use this document -一度に全機能を実装させず、以下を上から一つずつ Codex に渡す。各 milestone では、まず既存コードと対象 Isaac Sim version の API を調査し、設計変更を `docs/decisions/ADR-XXXX.md` に残し、テスト成功後に次へ進む。 +Do not ask Codex to implement every feature at once. Submit the prompts below one +milestone at a time, in order. At each milestone, first inspect the existing code and +the API of the targeted Isaac Sim version, record design changes in +`docs/decisions/ADR-XXXX.md`, and proceed only after the tests pass. -共通指示: +Instructions common to every milestone: ```text -- RadCounterSim_Codex_Implementation_Spec.md を最上位仕様とする。 -- 関係のない既存コードを変更しない。 -- public API には型注釈と docstring を付ける。 -- 単位を変数名に含める。 -- TruthState と BeliefState の依存方向を破らない。 -- 実装前に変更予定ファイル一覧とテスト計画を提示する。 -- 実装後に実行したコマンド、成功したテスト、未解決事項を報告する。 -- mock だけで完了扱いにしない。ただし外部依存がない unit-testable core を先に作る。 +- Treat RadCounterSim_Codex_Implementation_Spec.md as the highest-level specification. +- Do not modify unrelated existing code. +- Add type annotations and docstrings to public APIs. +- Include units in variable names. +- Preserve the dependency boundary between TruthState and BeliefState. +- Before implementation, list the files to be changed and provide a test plan. +- After implementation, report the commands run, passing tests, and unresolved items. +- Do not declare completion with mocks alone. Build the unit-testable, dependency-free + core first where appropriate. ``` --- -## Prompt 0 — リポジトリと extension scaffold +## Prompt 0 — Repository and extension scaffold ```text -RadCounterSim_Codex_Implementation_Spec.md の Milestone 0 を実装してください。 - -要件: -1. Isaac Sim 6.0.1 source workspace の UI extension template と C++ extension templateを使用する。 -2. radcounter.core、radcounter.radiation.native、radcounter.isaac の三層を作る。 -3. UI extension は Examples Browser に登録し、Load/Reset/Clear が動く。 -4. headless startup test と純 Python unit test の実行経路を用意する。 -5. YAML scenario schema、Pydantic config model、validate_scenario.py を作る。 -6. run manifest と JSONL event logger の skeleton を作る。 -7. CI 用コマンドと local setup を README に書く。 -8. Isaac Sim の extension dependency 名はローカル 6.0.1 の公式 template/example から確認し、推測で書かない。 - -受入条件: -- extension startup test pass -- pytest の空でない test suite pass -- sample scenario validation pass -- GUI で RadCounterSim example が表示される +Implement Milestone 0 of RadCounterSim_Codex_Implementation_Spec.md. + +Requirements: +1. Use the UI extension and C++ extension templates from an Isaac Sim 6.0.1 source workspace. +2. Create the radcounter.core, radcounter.radiation.native, and radcounter.isaac layers. +3. Register the UI extension in the Examples Browser and make Load, Reset, and Clear work. +4. Provide execution paths for a headless startup test and pure-Python unit tests. +5. Create a YAML scenario schema, Pydantic configuration model, and validate_scenario.py. +6. Create skeletons for the run manifest and JSONL event logger. +7. Document CI commands and local setup in the README. +8. Verify Isaac Sim extension dependency names against local 6.0.1 official templates or + examples; do not guess them. + +Acceptance criteria: +- The extension startup test passes. +- A non-empty pytest suite passes. +- The sample scenario validates. +- The RadCounterSim example appears in the GUI. ``` -## Prompt 1 — 基本モデルと解析 radiation backend +## Prompt 1 — Core models and analytic radiation backend ```text -Milestone 1 を実装してください。 +Implement Milestone 1. -実装対象: -- SourceType、EmissionLine、IsotopeSpec、PointSourceState、SurfaceSourceState -- MaterialSpec、DetectorSpec、RadiationMeasurement、RevisionState -- MaterialTable の energy interpolation -- Point source inverse-square forward model +Implementation scope: +- SourceType, EmissionLine, IsotopeSpec, PointSourceState, SurfaceSourceState +- MaterialSpec, DetectorSpec, RadiationMeasurement, RevisionState +- MaterialTable energy interpolation +- Point-source inverse-square forward model - NoScatterModel -- AnalyticTransportBackend: no attenuation と single slab -- detector efficiency、background、dead time、Poisson sampling -- deterministic RNG hierarchy +- AnalyticTransportBackend with no attenuation and a single slab +- Detector efficiency, background, dead time, and Poisson sampling +- Deterministic RNG hierarchy -テスト: +Tests: - 1/r^2 - exp(-mu*l) -- energy interpolation +- Energy interpolation - Poisson reproducibility -- zero/disabled source -- unit validation +- Zero or disabled source +- Unit validation -受入条件: -- 全 math unit test pass -- source/length/activity/count/dose の単位が docs に明記 +Acceptance criteria: +- All mathematical unit tests pass. +- The units for source, length, activity, count, and dose are documented. ``` -## Prompt 2 — USD radiation metadata と mesh extraction +## Prompt 2 — USD radiation metadata and mesh extraction ```text -Milestone 2 を実装してください。 +Implement Milestone 2. -実装対象: -- rad:* custom attributes の helper +Implementation scope: +- Helpers for rad:* custom attributes - UsdRadiationRegistry - SceneDescriptor - UsdGeom.Mesh extraction -- triangulation、metersPerUnit、world transform、instances、negative/non-uniform scale -- material ID per triangle -- transport mesh selection -- USD change notice と revision update -- per-face activity sidecar URI/checksum - -作る demo USD: -- room -- concrete wall -- thin shield panel -- contaminated floor -- movable contaminated box -- detector mount - -テスト: -- demo stage scan -- descriptor counts -- triangle/area/transform -- revision classification -- full rescan が不要な変更通知 +- Triangulation, metersPerUnit, world transforms, instances, and negative/non-uniform scale +- Per-triangle material IDs +- Transport-mesh selection +- USD change notices and revision updates +- URI and checksum for per-face activity sidecars + +Create a demo USD containing: +- A room +- A concrete wall +- A thin shield panel +- A contaminated floor +- A movable contaminated box +- A detector mount + +Tests: +- Demo-stage scan +- Descriptor counts +- Triangles, areas, and transforms +- Revision classification +- Change notification without a full rescan ``` ## Prompt 3 — Embree C++ backend ```text -Milestone 3 を実装してください。 +Implement Milestone 3. -実装対象: +Implementation scope: - EmbreeTransportScene C++ class - pybind11 bindings -- triangle mesh registration -- static geometry と dynamic instances -- transform update、remove、commit -- finite segment rays -- repeated closest-hit による全交差収集 -- solid entry/exit path length -- thin-sheet effective thickness -- material-wise path lengths -- energy-wise transmission -- GIL release、parallel batch query、thread safety -- explicit error handling - -テスト geometry: -- slab -- cube -- nested cubes -- two materials -- thin sheet normal/oblique -- moving instance -- odd-hit invalid mesh - -benchmark: -- 1k、10k、100k rays -- result を JSON に保存 - -受入条件: -- analytic tests pass -- memory leak がない -- commit と trace の race test pass +- Triangle-mesh registration +- Static geometry and dynamic instances +- Transform update, removal, and commit +- Finite segment rays +- Collection of all intersections through repeated closest-hit queries +- Solid entry/exit path length +- Thin-sheet effective thickness +- Per-material path lengths +- Per-energy transmission +- GIL release, parallel batch queries, and thread safety +- Explicit error handling + +Test geometry: +- Slab +- Cube +- Nested cubes +- Two materials +- Thin sheet at normal and oblique incidence +- Moving instance +- Invalid odd-hit mesh + +Benchmark: +- 1k, 10k, and 100k rays +- Save results as JSON. + +Acceptance criteria: +- Analytic tests pass. +- No memory leak is detected. +- Commit/trace race tests pass. ``` -## Prompt 4 — Surface source、sensor、dose map、cache +## Prompt 4 — Surface sources, sensors, dose maps, and cache ```text -Milestone 4 と 5 の radiation 部分を実装してください。 +Implement the radiation portions of Milestones 4 and 5. -実装対象: +Implementation scope: - SourceSampleBatch -- point/surface quadrature: centroid、stratified、adaptive -- attached source transform update +- Point and surface quadrature: centroid, stratified, and adaptive +- Transform updates for attached sources - RadiationForwardModel -- OmnidirectionalCounter、RotatingShieldCounter、DoseRateMeter -- measurement state machine -- moving integration trajectory sampling -- transfer matrix H -- TransferMatrixCache と revision rules +- OmnidirectionalCounter, RotatingShieldCounter, and DoseRateMeter +- Measurement state machine +- Moving-integration trajectory sampling +- Transfer matrix H +- TransferMatrixCache and revision rules - DoseMapEvaluator -- chunk processing - -重要: -- decon activity change では ray trace cache を無効化しない。 -- shield geometry change では geometry-dependent cache を無効化する。 -- truth scatter/bias と planner model を分ける。 - -テスト: -- surface rectangle convergence -- sensor on moving prim -- cache hit/miss -- activity-only fast update -- rotating shield physical geometry mode +- Chunked processing + +Important: +- Decontamination activity changes must not invalidate the ray-trace cache. +- Shield geometry changes must invalidate geometry-dependent cache entries. +- Keep truth scatter/bias separate from the planner model. + +Tests: +- Surface-rectangle convergence +- Sensor on a moving prim +- Cache hits and misses +- Activity-only fast update +- Rotating-shield physical-geometry mode ``` -## Prompt 5 — UI と visualization +## Prompt 5 — UI and visualization ```text -Milestone 5 の UI/visualization を実装してください。 +Implement the UI and visualization portions of Milestone 5. Frames: - Scenario @@ -188,227 +193,227 @@ Frames: Visualization: - 2D dose heatmap -- truth source debug overlay -- belief source overlay placeholder -- selected ray path/material lengths -- revision and timing display - -要件: -- heavy computation は async task -- cancel token -- stale revision の result は破棄 -- UI callback で blocking trace をしない +- Truth-source debug overlay +- Belief-source overlay placeholder +- Selected ray path and material lengths +- Revision and timing display + +Requirements: +- Run heavy computation in asynchronous tasks. +- Provide cancellation tokens. +- Discard results from stale revisions. +- Never perform blocking traces in UI callbacks. ``` ## Prompt 6 — Deterministic countermeasure actions ```text -Milestone 6 を実装してください。 +Implement Milestone 6. -実装対象: -- CountermeasureAction、ActionResult、ActionType -- action lifecycle/state machine -- ResourceState と consumption +Implementation scope: +- CountermeasureAction, ActionResult, and ActionType +- Action lifecycle and state machine +- ResourceState and consumption - DeterministicRobotController - DecontaminationExecutor - ShieldPlacementExecutor - MoveObjectExecutor - RemoveObjectExecutor - DisposalZone -- truth action uncertainty -- public_details と truth_details の分離 - -除染: -- triangle activity map -- footprint path -- exposure model -- spatial efficiency random field -- discard/transfer_to_waste - -遮蔽: -- shield asset spawn/move -- actual pose error +- Truth-side action uncertainty +- Separation of public_details and truth_details + +Decontamination: +- Triangle activity map +- Footprint path +- Exposure model +- Spatial efficiency random field +- discard and transfer_to_waste modes + +Shielding: +- Shield asset spawn and movement +- Actual pose error - Embree update -移動・撤去: -- attached source follow -- disposal validation +Movement and removal: +- Attached-source following +- Disposal validation -統合テストで全 action 前後の measurement と map 変化を確認する。 +Integration tests must verify measurement and map changes before and after every action. ``` -## Prompt 7 — Physics robot execution +## Prompt 7 — Physics-based robot execution ```text -Milestone 7 を実装してください。 +Implement Milestone 7. -実装対象: +Implementation scope: - IsaacPhysicsRobotController -- measurement mobile robot navigation -- countermeasure mobile manipulator or base+arm composition -- end-effector planning -- grasp/release constraint -- shield pick-and-place -- obstacle pick/push -- contaminated object movement -- decon tool footprint ray/contact integration -- settle detection -- timeout、abort、recovery - -要件: -- deterministic action と同じ API -- actual USD pose を radiation registry に反映 -- physics execution failure を action result に記録 -- manipulation animation だけでなく radiation state が更新される - -最低 demo: -1. shield panel を把持し target pose に設置 -2. contaminated box を移動 -3. non-contaminated obstacle をどかす -4. decon tool で指定 patch を処理 +- Mobile measurement-robot navigation +- Countermeasure mobile manipulator, or composed base and arm +- End-effector planning +- Grasp/release constraints +- Shield pick-and-place +- Obstacle picking or pushing +- Contaminated-object movement +- Decontamination-tool footprint integration from rays or contact +- Settle detection +- Timeout, abort, and recovery + +Requirements: +- Use the same API as deterministic actions. +- Reflect the actual USD pose in the radiation registry. +- Record physics-execution failures in the action result. +- Update radiation state, not only the manipulation animation. + +Minimum demo: +1. Grasp a shield panel and place it at the target pose. +2. Move a contaminated box. +3. Move a non-contaminated obstacle out of the way. +4. Treat a specified patch with the decontamination tool. ``` ## Prompt 8 — Source estimation ```text -Milestone 8 を実装してください。 +Implement Milestone 8. -実装対象: -- CandidateBasis: 3D grid、surface triangle graph -- measurement stacking +Implementation scope: +- CandidateBasis: 3D grid and surface-triangle graph +- Measurement stacking - Poisson negative log likelihood -- nonnegative MLE +- Nonnegative MLE - L1 proximal solver -- surface TV regularization -- coarse grid -> connected components -> continuous MLE refinement +- Surface total-variation regularization +- Coarse grid to connected components to continuous MLE refinement - Fisher uncertainty -- optional bootstrap -- SourceEstimate serialization/visualization - -禁止: -- TruthState 参照 -- ground-truth source count を solver に与える - -テスト: -- noiseless/noisy one source -- two sources -- hidden surface patch -- source height variation -- gradient finite difference -- uncertainty shape +- Optional bootstrap +- SourceEstimate serialization and visualization + +Prohibited: +- Reading TruthState +- Supplying the ground-truth source count to the solver + +Tests: +- Noiseless and noisy single source +- Two sources +- Hidden surface patch +- Source-height variation +- Finite-difference gradient +- Uncertainty shape ``` -## Prompt 9 — Predicted/observed residual と再推定 +## Prompt 9 — Predicted/observed residuals and re-estimation ```text -Milestone 9 を実装してください。 +Implement Milestone 9. -実装対象: -- nominal action preview on BeliefState clone -- predicted verification measurement -- observed verification measurement -- raw/normalized residual +Implementation scope: +- Nominal action preview on a BeliefState clone +- Predicted verification measurement +- Observed verification measurement +- Raw and normalized residuals - DeconResidualHypothesis - ShieldPoseErrorHypothesis - HiddenSourceHypothesis - GlobalGainBackgroundHypothesis - SourceLocalizationErrorHypothesis -- likelihood/BIC selection -- belief update -- action-effect parameter update -- residual visualization and logs +- Likelihood/BIC selection +- Belief updates +- Action-effect parameter updates +- Residual visualization and logs -fault injection tests: -- 30% decon residual +Fault-injection tests: +- 30% decontamination residual - 5 cm shield translation error -- hidden source -- detector gain bias -- mixed failure case +- Hidden source +- Detector gain bias +- Mixed failure case ``` -## Prompt 10 — Planner と closed-loop coordinator +## Prompt 10 — Planner and closed-loop coordinator ```text -Milestone 10 を実装してください。 - -実装対象: -- action candidate generators -- measurement pose candidates -- decon region candidates -- shield placement candidates -- object move/remove candidates -- repair candidates -- navigation/manipulation/resource feasibility -- action objective -- expected information gain approximation -- OpenLoop、Greedy、Nearest、Random、Oracle、ClosedLoopResidual planners +Implement Milestone 10. + +Implementation scope: +- Action candidate generators +- Measurement-pose candidates +- Decontamination-region candidates +- Shield-placement candidates +- Object movement/removal candidates +- Repair candidates +- Navigation, manipulation, and resource feasibility +- Action objective +- Approximate expected information gain +- OpenLoop, Greedy, Nearest, Random, Oracle, and ClosedLoopResidual planners - ClosedLoopCoordinator state machine -- pause/resume/stop -- termination conditions -- snapshot persistence +- Pause, resume, and stop +- Termination conditions +- Snapshot persistence -end-to-end demo: +End-to-end demo: MEASURE -> ESTIMATE -> PLAN -> PREDICT -> EXECUTE -> VERIFY -> DIAGNOSE -> UPDATE -> REPLAN -要件: -- Oracle 以外は TruthState を参照しない -- baseline と proposed の同一 seed batch runner +Requirements: +- Only Oracle may read TruthState. +- Run baselines and the proposed planner with the same seed batch runner. ``` -## Prompt 11 — ROS 2、MoveIt 2、Nav2 adapter +## Prompt 11 — ROS 2, MoveIt 2, and Nav2 adapters ```text -Milestone 11 の ROS 2 部分を実装してください。 +Implement the ROS 2 portion of Milestone 11. -実装対象: +Implementation scope: - radcounter_msgs -- RadiationMeasurement、SourceEstimate、CountermeasureStatus +- RadiationMeasurement, SourceEstimate, and CountermeasureStatus - MeasureRadiation.action - ExecuteCountermeasure.action -- GetDoseMap/EvaluateCountermeasure services +- GetDoseMap and EvaluateCountermeasure services - Ros2RobotController -- standard tf/joint_states/cmd_vel/trajectory integration +- Standard tf, joint_states, cmd_vel, and trajectory integration - MoveIt 2 manipulation adapter - Nav2 navigation adapter -- namespace/multi-robot support +- Namespace and multi-robot support -要件: -- ROS 2 なしでも core と GUI が動く optional dependency -- Jazzy を primary target -- simulated time を使用 -- timeout と QoS を設定ファイル化 +Requirements: +- ROS 2 must remain an optional dependency; the core and GUI work without it. +- Target Jazzy primarily. +- Use simulated time. +- Configure timeouts and QoS in configuration files. ``` -## Prompt 12 — 実験自動化、性能、文書、リリース +## Prompt 12 — Experiment automation, performance, documentation, and release ```text -最終 milestone を実装してください。 +Implement the final milestone. -実装対象: -- headless batch runner -- seed sweep -- baseline sweep -- output parquet/json/npz +Implementation scope: +- Headless batch runner +- Seed sweep +- Baseline sweep +- Parquet, JSON, and NPZ output - HTML report -- git/config/asset/hardware manifest -- benchmark suite -- regression baselines -- API docs -- scenario authoring guide -- troubleshooting -- one-command demo scripts - -生成する実験: -1. analytic radiation validation -2. decon primitive -3. shield primitive -4. movable contaminated object -5. hidden source residual -6. closed-loop vs open-loop -7. resource-constrained multi-action - -最終確認: -- Definition of Done 全項目を checklist 化 -- 未実装項目を明示 -- versioned release tag 用 changelog を作る +- Git, configuration, asset, and hardware manifest +- Benchmark suite +- Regression baselines +- API documentation +- Scenario authoring guide +- Troubleshooting guide +- One-command demo scripts + +Generate these experiments: +1. Analytic radiation validation +2. Decontamination primitive +3. Shielding primitive +4. Movable contaminated object +5. Hidden-source residual +6. Closed-loop versus open-loop +7. Resource-constrained multi-action workflow + +Final checks: +- Convert every Definition of Done item into a checklist. +- Identify every unimplemented item. +- Create a changelog suitable for a versioned release tag. ``` diff --git a/docs/system-profiles.md b/docs/system-profiles.md new file mode 100644 index 0000000..080c35d --- /dev/null +++ b/docs/system-profiles.md @@ -0,0 +1,142 @@ +# Switchable environments, robots, and detectors + +RadCounterSim keeps environments, robot sets, and detector sets independent in +`configs/system/catalog.yaml`. A profile is only a named combination of those +three components plus a radiation runtime configuration. The active selection +is stored outside the repository by default at +`~/.config/radcountersim/system-selection.json`. + +The normal operator path is the **SYSTEM CONFIGURATION** section in the +Isaac Operations window. Choose a preset, or change Environment, Robot, and +Detector independently, then press **Apply selected configuration**. Display names +are shown instead of internal IDs. The GUI validates dependencies, pauses the +timeline, rebuilds the stage, initializes radiation transport, and saves the +successful selection for the next launch. **Restore current** discards unapplied +choices. If a catalog environment has not been downloaded or converted yet, +the same button changes to **Prepare environment and apply** and runs its reviewed, +repository-local preparation steps automatically. + +The **NATURAL LANGUAGE COMMAND / ROBOT LLM** section appears above the system +configuration and is intentionally separate. It accepts natural-language robot +workflow instructions only; environment, robot-set, and detector-set switching +always uses the explicit controls below it. + +The robot list immediately above the LLM section is rebuilt from the selected +robot-set catalog entry. Its **View** and **Onboard** controls, the active-robot bar, +and the building overview therefore follow the same explicit configuration; +see `robot-monitoring.md`. + +Each environment catalog entry also owns named `spawn_anchors`. Robot sets refer +to semantic anchors such as `ground-primary` or `aerial-primary` instead of +assuming one world coordinate works in every CAD. The environment also provides +`decon-surface` and inspection-camera anchors for supported task workflows. A +selected combination fails with a named configuration error when a required +anchor is absent; it does not spawn a robot at an unverified origin. Ground +anchors are authored on the environment's normalized Z=0 operating floor, and +live articulated validation rejects a spawn that drops more than 0.15 m during +physics settling. + +## Common commands + +```bash +# Inspect every available component and profile. +uv run radcounter-system list + +# Resolve a profile without changing the active selection. +uv run radcounter-system show --profile fukushima-packbot + +# Save a selection for later radcounter-app launches. +uv run radcounter-system activate --profile vertical-slice-packbot + +# Return to the complete articulated decommissioning workflow. +uv run radcounter-system activate --profile vertical-slice + +# Override components independently for one launch. +radcounter-app \ + --profile vertical-slice-packbot \ + --environment vertical-slice \ + --robot-set fukushima-response-reference \ + --detector-set fukushima-survey +``` + +Environment, robot-set, and detector-set overrides are validated before Isaac +starts. A detector set that requires a robot not present in the selected robot +set fails with a named configuration error rather than mounting to a guessed +prim. The existing `vertical-slice` profile retains the full articulated +decontamination workflow. Other combinations use the generic compositor and do +not silently author the vertical-slice-only room, source, or task geometry. + +## Fukushima Daiichi SolidWorks environment + +The upstream repository contains SolidWorks `.SLDASM` and `.SLDPRT` files only. +RadCounterSim converts the native top-level assembly directly to USD on Linux +with the HOOPS Exchange converter bundled with Isaac Sim 6.0.1. This preserves +the assembly hierarchy and avoids a Windows or STEP prerequisite. + +1. Fetch the pinned upstream source and record its hashes. + + ```bash + uv run python scripts/fetch_fukushima_daiichi_cad.py + ``` + + This writes the checkout below + `.cache/external/fukushima_daiichi_solidworks/source` and a + `source-provenance.json` record. The source is pinned to commit + `f6541deb6159c5d908a4f028d021e3d2c9f7f8e8`. + +2. Convert the top-level assembly with the local headless Isaac converter. + + ```bash + uv run python scripts/convert_solidworks_to_usd.py + ``` + + The converter runs through `omni.app.empty.kit`, so it does not initialize an + RTX viewport. It emits a monolithic `Building.usdc`, refuses to overwrite an + existing result unless `--force` is supplied, and records input/output + hashes, source revision, converter versions, mesh count, and triangle count + in `Building.usdc.provenance.json`. + +3. Validate and activate the selection. These commands are useful for + automation; GUI users only need to select and apply the profile. + + ```bash + uv run radcounter-system check --profile fukushima-packbot + uv run radcounter-system prepare --profile fukushima-packbot + uv run radcounter-system activate --profile fukushima-packbot + radcounter-app + ``` + +The normalized manifest and composed USD stage are content-addressed below the +configured environment cache (by default +`~/.cache/radcountersim/environments`). The same imported triangles are used for visible USD +geometry, static PhysX collision, and radiation material geometry. The +Fukushima profile adds no contamination surface or hidden regular proxy. Any +later contamination authoring must still follow +`docs/decontamination-authoring-rules.md` and operate on the visible irregular +activity-bearing geometry. + +The upstream CAD model is described as generic and incomplete: its own TODO +list includes pipes, biological shielding, supports, scaffolding, catwalks, +shield plugs, and floor-height adjustments. It must not be presented as an +as-built engineering record or used for safety decisions. + +## Adding catalog entries + +- An environment entry references the existing environment descriptor contract + from `docs/environment-import.md` and declares the semantic spawn anchors + needed by compatible robot/task sets. +- A `fleet` robot set references the generic USD/URDF/Xacro/MJCF fleet contract. + A `reference` set uses the traceable lightweight nuclear-response models. +- A detector set references built-in model IDs from + `radcounter.core.sensors.catalog` or a custom YAML descriptor. +- A profile selects one entry from each group and a base runtime JSON file. + +Run `uv run radcounter-system show --profile NAME` after edits. It loads every +referenced descriptor, rebases fleet-relative asset paths, validates detector +models and parent relationships, and reports whether the environment source is +ready without starting Isaac Sim. + +In a configurable composition, the selected detector set replaces detector +roles already authored by the environment. Existing detector geometry remains +visible but is marked inactive in the generated cache stage, so measurements +cannot accidentally combine the selected instruments with hidden defaults. diff --git a/docs/vertical-slice.md b/docs/vertical-slice.md index 9473652..f0cd17c 100644 --- a/docs/vertical-slice.md +++ b/docs/vertical-slice.md @@ -2,6 +2,11 @@ `assets/environments/radcounter_vertical_slice.usda` is the minimum end-to-end research scene. It contains a room, a triangle activity-map surface, one estimator-hidden movable point source, a measurement robot, a countermeasure robot, a lead shield, a movable steel obstacle, a decontamination tool, four validation stations, and a disposal zone. +The portable stage keeps lightweight robot placeholders so it can be inspected +without Isaac assets. `scripts/run_gui.py` replaces those placeholders before +physics starts with the official Ridgeback + Franka Panda and Nova Carter USDs, +then authors task tools and grasp frames under the real articulations. + Generate deterministic assets with: ```bash diff --git a/pyproject.toml b/pyproject.toml index 15309d8..556a7d1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -29,6 +29,8 @@ radcounter-report = "radcounter.core.experiments.cli:report_main" radcounter-import-environment = "radcounter.core.environment.cli:main" radcounter-build-large-environment = "radcounter.core.environment.large_cli:main" radcounter-robot-command = "radcounter.core.robots.command_io:command_main" +radcounter-app = "radcounter.launcher:main" +radcounter-system = "radcounter.core.system_cli:main" [dependency-groups] dev = [ diff --git a/radcounter/core/environment/sdf_visual.py b/radcounter/core/environment/sdf_visual.py index 969edaf..2cab4cb 100644 --- a/radcounter/core/environment/sdf_visual.py +++ b/radcounter/core/environment/sdf_visual.py @@ -8,9 +8,9 @@ from __future__ import annotations +import xml.etree.ElementTree as ET from dataclasses import dataclass from pathlib import Path -import xml.etree.ElementTree as ET @dataclass(frozen=True) @@ -111,9 +111,9 @@ def load_sdf_visual_model(path: str | Path) -> SdfVisualModel: metal = visual.find("material/pbr/metal") - def texture(tag: str) -> Path | None: + def texture(tag: str, material: ET.Element | None = metal) -> Path | None: resolved = _resolve_uri( - metal.findtext(tag) if metal is not None else None, + material.findtext(tag) if material is not None else None, model_root, model_name, ) diff --git a/radcounter/core/natural_language/__init__.py b/radcounter/core/natural_language/__init__.py new file mode 100644 index 0000000..7da6699 --- /dev/null +++ b/radcounter/core/natural_language/__init__.py @@ -0,0 +1,47 @@ +"""Safe natural-language command interpretation for RadCounterSim.""" + +from .client import ( + CommandInterpretationError, + CommandInterpreter, + OpenAICompatibleCommandInterpreter, + normalize_operator_instruction, +) +from .models import ( + AvailableAction, + CommandContext, + CommandName, + CommandPlan, + CommandStep, + CompletionCriterion, + PlanValidationError, + StepCompletionCondition, + ValidatedCommandPlan, + validate_command_plan, +) +from .runtime import ( + LlamaCppRuntime, + LlamaCppRuntimeConfig, + RuntimeAssetMissingError, + RuntimeStatus, +) + +__all__ = [ + "AvailableAction", + "CompletionCriterion", + "CommandContext", + "CommandInterpretationError", + "CommandInterpreter", + "CommandName", + "CommandPlan", + "CommandStep", + "LlamaCppRuntime", + "LlamaCppRuntimeConfig", + "OpenAICompatibleCommandInterpreter", + "PlanValidationError", + "RuntimeAssetMissingError", + "RuntimeStatus", + "StepCompletionCondition", + "ValidatedCommandPlan", + "normalize_operator_instruction", + "validate_command_plan", +] diff --git a/radcounter/core/natural_language/client.py b/radcounter/core/natural_language/client.py new file mode 100644 index 0000000..45bc5b8 --- /dev/null +++ b/radcounter/core/natural_language/client.py @@ -0,0 +1,1041 @@ +"""OpenAI-compatible client used with a private local llama.cpp server.""" + +from __future__ import annotations + +import asyncio +import copy +import json +import re +from typing import Protocol +from urllib.error import HTTPError, URLError +from urllib.parse import urlparse +from urllib.request import Request, urlopen + +from pydantic import ValidationError + +from .models import AvailableAction, CommandContext, CommandPlan + + +class CommandInterpretationError(RuntimeError): + """Raised when local inference fails or returns an invalid command plan.""" + + +class CommandInterpreter(Protocol): + async def interpret(self, instruction: str, context: CommandContext) -> CommandPlan: ... + + +_SYSTEM_PROMPT = """You translate English operator instructions into a +RadCounterSim command plan. Return only JSON matching the supplied schema. Never invent +action IDs, robot IDs, paths, commands, or coordinates. Use execute_candidate only with +an exact available_actions.action_id. Prefer the shortest plan that completes the request. +Write the summary in English. If the request is ambiguous or impossible +using the listed capabilities, return a show_status step and explain the limitation in summary. +Never claim an operation was executed; you only propose it. +An execute_candidate whose action_type is measure only navigates to a measurement station. +If the operator also asks to measure, count, or take a reading, add a separate measure step. +An available place_shield action already contains host-derived pickup, grasp, route, and +placement coordinates. When the operator asks to pick up and place the lead shield between +the source and protected area, select a feasible place_shield action instead of rejecting it +for not exposing those coordinates in the public context. +Available shield labels and placement_fraction values identify host-planned positions along +the source-to-protected-area line. Preserve every explicitly requested placement and +repositioning percentage as an ordered execute_candidate step. A later candidate for the same +physical shield is a host-controlled move, not a request for model-generated coordinates. +Each decontaminate action owns its approach, full irregular-surface serpentine raster, and +retreat. If the operator requests multiple passes or says "until" with a maximum number of +passes, use max_attempts (never more than five) and an applicable until criterion. Fractions in +completion thresholds are JSON fractions: 70 percent is 0.70. The host evaluates conditions +only from public contact, coverage, activity, placement-error, or measured-rate results. +For "all", "each", or multiple explicitly named targets, emit one ordered candidate step for +every distinct feasible target instead of silently choosing only the first. +Use return_measurement_robot when the operator asks the measurement robot to return to its +starting, initial, original, or home position. Multi-step requests are supported: preserve the +operator's order and emit as many steps as needed, up to twenty-four. The host confirms the complete +workflow once and revalidates the live scene after every step before continuing. Do not invent +conditional results. Bounded until conditions stop a repeated step early; they never authorize +an unbounded loop or an action outside the listed capabilities. +""" + + +def _inlined_command_schema() -> dict[str, object]: + """Inline Pydantic references for reliable llama.cpp grammar generation.""" + + schema = copy.deepcopy(CommandPlan.model_json_schema()) + definitions = schema.pop("$defs", {}) + + def resolve(value: object) -> object: + if isinstance(value, list): + return [resolve(item) for item in value] + if not isinstance(value, dict): + return value + reference = value.get("$ref") + if isinstance(reference, str) and reference.startswith("#/$defs/"): + name = reference.rsplit("/", 1)[-1] + target = definitions.get(name) + if target is None: + raise RuntimeError(f"unresolved command schema reference: {reference}") + merged = copy.deepcopy(target) + merged.update({key: item for key, item in value.items() if key != "$ref"}) + return resolve(merged) + return {key: resolve(item) for key, item in value.items()} + + resolved = resolve(schema) + assert isinstance(resolved, dict) + return resolved + + +def _instruction_language(instruction: str) -> str: + """Return the only language accepted by the English operator interface.""" + + del instruction + return "en" + + +def normalize_operator_instruction(instruction: str) -> str: + """Remove accidental triple-or-more repetitions from one-line UI input. + + Some IME/paste interactions can append the complete composition more than once. + Two repetitions are left intact because they can be an intentional request; three + or more exact consecutive copies are treated as input duplication. + """ + + normalized = re.sub(r"\s{2,}>\s*", " ", instruction.strip()) + # Paste/IME glitches can repeat a long instruction with a slightly different + # final copy (for example, after a stripped Markdown quote marker). Detect + # three-or-more recurrences by a stable leading anchor before exact matching. + if len(normalized) >= 96: + anchor = normalized[:48] + starts = [match.start() for match in re.finditer(re.escape(anchor), normalized)] + if len(starts) >= 3 and starts[0] == 0: + return normalized[: starts[1]].strip() + for unit_length in range(4, len(normalized) // 3 + 1): + if len(normalized) % unit_length: + continue + repetition_count = len(normalized) // unit_length + if repetition_count >= 3 and normalized == normalized[:unit_length] * repetition_count: + return normalized[:unit_length].strip() + return normalized + + +def _requested_measurement(instruction: str) -> bool: + lowered = instruction.casefold() + return any( + token in lowered + for token in ( + "measure", + "measurement", + "take a reading", + "count radiation", + ) + ) + + +def _requested_status(instruction: str) -> bool: + lowered = instruction.casefold() + return any( + token in lowered + for token in ( + "show status", + "show the current status", + "display status", + "display the current status", + "report status", + ) + ) + + +def _requested_duration_s(instruction: str) -> float | None: + match = re.search( + r"(? bool: + lowered = instruction.casefold() + english_return = bool( + re.search(r"\b(?:return|go|come|send|move)\b.*\b(?:back|home)\b", lowered) + or re.search( + r"\breturn\b.*\b(?:starting|start|initial|original|home)\b.*\b(?:position|point|location)?\b", + lowered, + ) + ) + return english_return + + +def _requested_shield_placement(instruction: str) -> bool: + lowered = instruction.casefold() + mentions_shield = "shield" in lowered + requests_manipulation = any( + token in lowered + for token in ( + "pick up", + "pickup", + "place", + "position", + "manipulator", + "gripper", + ) + ) + return mentions_shield and requests_manipulation + + +def _requested_decontamination(instruction: str) -> bool: + lowered = instruction.casefold() + return any(token in lowered for token in ("decontaminate", "decontamination")) + + +def _requested_max_attempts(instruction: str) -> int | None: + """Return an explicit bounded pass/attempt count, never a time value.""" + + patterns = ( + r"\b(?:up\s+to|max(?:imum)?(?:\s+of)?)\s*([1-5])\s*(?:times?|passes?|attempts?)\b", + r"\b([1-5])\s*(?:times?|passes?|attempts?)\b", + ) + for pattern in patterns: + match = re.search(pattern, instruction, flags=re.IGNORECASE) + if match is not None: + return int(match.group(1)) + lowered = instruction.casefold() + if any(token in lowered for token in ("again", "one more pass")): + return 2 + return None + + +def _percent_after_labels(instruction: str, labels: tuple[str, ...]) -> float | None: + label_pattern = "|".join(re.escape(label) for label in labels) + patterns = ( + rf"(?:{label_pattern})[^\d%]{{0,28}}(\d{{1,3}}(?:\.\d+)?)\s*%", + rf"(\d{{1,3}}(?:\.\d+)?)\s*%[^,.]{{0,28}}(?:{label_pattern})", + ) + for pattern in patterns: + match = re.search(pattern, instruction, flags=re.IGNORECASE) + if match is not None: + value = float(match.group(1)) / 100.0 + if 0.0 <= value <= 1.0: + return value + return None + + +def _requested_decontamination_condition( + instruction: str, +) -> tuple[str, float] | None: + removed = _percent_after_labels( + instruction, + ("removal fraction", "removed fraction"), + ) + if removed is not None: + return "decontamination_removed_fraction_at_least", removed + remaining = _percent_after_labels( + instruction, + ("remaining fraction", "residual fraction"), + ) + if remaining is not None: + return "decontamination_remaining_fraction_at_most", remaining + coverage = _percent_after_labels( + instruction, + ("coverage", "surface coverage"), + ) + if coverage is not None: + return "decontamination_coverage_fraction_at_least", coverage + return None + + +def _requested_measurement_condition(instruction: str) -> float | None: + match = re.search( + r"(? bool: + lowered = instruction.casefold() + all_tokens = ("all ", "each ", "every ") + if not any(token in lowered for token in all_tokens): + return False + type_tokens = { + "decontaminate": ("decontamin", "surface"), + "measure": ("measurement", "station"), + "shield": ("shield", "panel"), + }[action_type] + return any(token in lowered for token in type_tokens) + + +def _requests_multiple_shields(instruction: str) -> bool: + lowered = instruction.casefold() + return any( + token in lowered + for token in ( + "all shields", + "both shields", + "multiple shields", + "two shields", + "two panels", + ) + ) + + +def _requested_shield_fractions( + instruction: str, + context: CommandContext, +) -> tuple[float, ...]: + available = sorted( + { + round(float(action.placement_fraction), 6) + for action in context.available_actions + if action.action_type in {"place_shield", "move_shield"} + and action.placement_fraction is not None + } + ) + if not available: + return () + requested: list[float] = [] + for match in re.finditer(r"(\d{1,3}(?:\.\d+)?)\s*%", instruction): + fraction = float(match.group(1)) / 100.0 + closest = min(available, key=lambda item: abs(item - fraction)) + if abs(closest - fraction) <= 0.005: + requested.append(closest) + return tuple(requested) + + +def _requested_object_manipulation(instruction: str) -> bool: + """Distinguish moving through a facility from moving a physical object.""" + + lowered = instruction.casefold() + return any( + token in lowered + for token in ( + "drum", + "move the obstacle", + "move an object", + "remove the object", + "dispose", + "relocate the object", + ) + ) + + +def _normalize_plan( + plan: CommandPlan, + instruction: str, + context: CommandContext, +) -> CommandPlan: + """Repair harmless small-model omissions using explicit operator wording.""" + + language = _instruction_language(instruction) + by_id = {action.action_id: action for action in context.available_actions} + from .models import ( + CommandName, + CommandStep, + CompletionCriterion, + StepCompletionCondition, + ) + + steps: list[CommandStep] = [] + for step in plan.steps: + candidate_id = step.candidate_id + if ( + step.command == CommandName.EXECUTE_CANDIDATE + and candidate_id not in by_id + and candidate_id in context.capabilities + ): + try: + built_in = CommandName(candidate_id) + except ValueError: + pass + else: + if built_in != CommandName.EXECUTE_CANDIDATE: + step = CommandStep(command=built_in, duration_s=step.duration_s) + steps.append(step) + repaired_shield_placement = False + if _requested_shield_placement(instruction): + feasible_shields = sorted( + ( + action + for action in context.available_actions + if action.feasible and action.action_type in {"place_shield", "move_shield"} + ), + key=lambda action: action.action_id, + ) + # Scene geometry can invalidate one of several equivalent shield-line + # candidates. Small local models commonly select the first identifier; + # substitute the first host-verified safe candidate before validation. + if feasible_shields: + safe_shield_id = feasible_shields[0].action_id + steps = [ + step.model_copy(update={"candidate_id": safe_shield_id}) + if step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type in {"place_shield", "move_shield"} + and not by_id[step.candidate_id].feasible + else step + for step in steps + ] + selected_shield = any( + step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type in {"place_shield", "move_shield"} + for step in steps + ) + if not selected_shield and feasible_shields: + steps = [step for step in steps if step.command != CommandName.SHOW_STATUS] + steps.insert( + 0, + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=feasible_shields[0].action_id, + ), + ) + repaired_shield_placement = True + if _requested_decontamination(instruction): + feasible_decon = sorted( + ( + action + for action in context.available_actions + if action.feasible and action.action_type == "decontaminate" + ), + key=lambda action: action.action_id, + ) + selected_decon = any( + step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "decontaminate" + and by_id[step.candidate_id].feasible + for step in steps + ) + if not selected_decon and feasible_decon: + steps = [ + step + for step in steps + if not ( + step.command == CommandName.SHOW_STATUS + or ( + step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "decontaminate" + ) + ) + ] + steps.insert( + 0, + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=feasible_decon[0].action_id, + ), + ) + if not _requested_object_manipulation(instruction): + # A small local model can interpret "move through the corridor" as a + # request to move an unrelated prop. Candidate routes already contain + # robot navigation, so unmentioned object manipulation is never needed. + object_action_types = {"move_object", "remove_object"} + steps = [ + step + for step in steps + if not ( + step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type in object_action_types + ) + ] + requested_duration_s = _requested_duration_s(instruction) + if requested_duration_s is not None: + steps = [ + step.model_copy(update={"duration_s": requested_duration_s}) + if step.command == CommandName.MEASURE and step.duration_s is None + else step + for step in steps + ] + if _requested_measurement(instruction): + selected_measurement_navigation = any( + step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "measure" + for step in steps + ) + available_measurements = [ + action + for action in context.available_actions + if action.feasible and action.action_type == "measure" + ] + lowered = instruction.casefold() + wants_protected = any( + token in lowered + for token in ( + "protected area", + "protected zone", + "protected measurement station", + ) + ) + wants_remote = any(token in lowered for token in ("remote room", "decontamination room")) + if ( + not selected_measurement_navigation + and available_measurements + and (wants_protected or wants_remote) + ): + preferred_token = "protected" if wants_protected else "remote" + available_measurements.sort( + key=lambda action: ( + preferred_token + not in " ".join( + ( + action.action_id, + action.target or "", + action.target_label or "", + ) + ).casefold() + ) + ) + measurement_navigation = CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=available_measurements[0].action_id, + ) + measurement_index = next( + (index for index, step in enumerate(steps) if step.command == CommandName.MEASURE), + len(steps), + ) + steps.insert(measurement_index, measurement_navigation) + navigates_for_measurement = any( + step.command.value == "execute_candidate" + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "measure" + for step in steps + ) + has_measurement = any(step.command.value == "measure" for step in steps) + executes_candidate = any(step.command == CommandName.EXECUTE_CANDIDATE for step in steps) + if executes_candidate and _requested_measurement(instruction) and not has_measurement: + steps.append( + CommandStep( + command=CommandName.MEASURE, + duration_s=requested_duration_s, + ) + ) + verification_action_types = { + by_id[step.candidate_id].action_type + for step in steps + if step.command == CommandName.EXECUTE_CANDIDATE and step.candidate_id in by_id + } + verification_builtins_only = all( + step.command + in { + CommandName.EXECUTE_CANDIDATE, + CommandName.MEASURE, + CommandName.RETURN_MEASUREMENT_ROBOT, + CommandName.SHOW_STATUS, + } + for step in steps + ) + repeated_workflow_requested = any( + token in instruction.casefold() + for token in ( + "again", + "repeat", + "re-measure", + "remeasure", + "until", + ) + ) + if ( + _requested_decontamination(instruction) + and _requested_measurement(instruction) + and verification_builtins_only + and verification_action_types <= {"decontaminate", "measure"} + and not repeated_workflow_requested + ): + # For this common verification workflow, collapse small-model + # repetitions into the three physical semantics the operator asked for: + # decontaminate, navigate to the requested detector, then integrate. + decon_steps = [ + step + for step in steps + if step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "decontaminate" + and by_id[step.candidate_id].feasible + ] + measurement_steps = [ + step + for step in steps + if step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "measure" + and by_id[step.candidate_id].feasible + ] + wants_protected = any( + token in instruction.casefold() + for token in ( + "protected area", + "protected zone", + "protected measurement station", + ) + ) + wants_remote_room = any( + token in instruction.casefold() + for token in ( + "remote room", + "decontamination room", + ) + ) + available_measurements = [ + action + for action in context.available_actions + if action.feasible and action.action_type == "measure" + ] + if wants_protected: + measurement_steps.sort( + key=lambda step: "protected" not in str(step.candidate_id).casefold() + ) + available_measurements.sort( + key=lambda action: "protected" not in action.action_id.casefold() + ) + elif wants_remote_room: + measurement_steps.sort( + key=lambda step: "remote" not in str(step.candidate_id).casefold() + ) + available_measurements.sort( + key=lambda action: "remote" not in action.action_id.casefold() + ) + preferred_available = available_measurements[0] if available_measurements else None + if ( + measurement_steps + and preferred_available is not None + and ( + (wants_protected and "protected" not in str(measurement_steps[0].candidate_id)) + or (wants_remote_room and "remote" not in str(measurement_steps[0].candidate_id)) + ) + ): + measurement_steps = [ + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=preferred_available.action_id, + ) + ] + if not measurement_steps and available_measurements: + measurement_steps = [ + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=available_measurements[0].action_id, + ) + ] + measurement_step = next( + (step for step in steps if step.command == CommandName.MEASURE), + CommandStep( + command=CommandName.MEASURE, + duration_s=requested_duration_s, + ), + ) + return_step = next( + (step for step in steps if step.command == CommandName.RETURN_MEASUREMENT_ROBOT), + None, + ) + status_step = next( + (step for step in steps if step.command == CommandName.SHOW_STATUS), + None, + ) + steps = [ + *decon_steps[:1], + *measurement_steps[:1], + measurement_step, + *(() if return_step is None else (return_step,)), + *(() if status_step is None else (status_step,)), + ] + if navigates_for_measurement and _requested_return_to_start(instruction): + # Small models sometimes translate "return to the starting position" as + # another measurement-station candidate. Once measurement has happened, + # such a candidate is not a valid representation of home; the dedicated + # allowlisted command is deterministic and does not expose coordinates. + after_measurement = False + measurement_navigation_seen = False + repaired_steps: list[CommandStep] = [] + trailing_status_steps: list[CommandStep] = [] + for step in steps: + if step.command == CommandName.MEASURE: + after_measurement = True + if step.command == CommandName.RETURN_MEASUREMENT_ROBOT: + continue + if step.command == CommandName.SHOW_STATUS: + trailing_status_steps.append(step) + continue + if step.command == CommandName.EXECUTE_CANDIDATE and step.candidate_id in by_id: + is_measurement_navigation = by_id[step.candidate_id].action_type == "measure" + if is_measurement_navigation: + if measurement_navigation_seen and ( + after_measurement or _requested_measurement(instruction) + ): + continue + measurement_navigation_seen = True + repaired_steps.append(step) + if len(repaired_steps) >= 24: + repaired_steps = repaired_steps[:23] + steps = [ + *repaired_steps, + CommandStep(command=CommandName.RETURN_MEASUREMENT_ROBOT), + *trailing_status_steps[:1], + ] + + # Expand explicit "all surfaces" requests from the allowlisted live + # candidates. The model never receives or invents raster coordinates; each + # host candidate owns its approach, irregular-surface scan, and retreat. + if _requests_all_targets(instruction, "decontaminate"): + feasible_by_target: dict[str, AvailableAction] = {} + for action in sorted(context.available_actions, key=lambda item: item.action_id): + if action.feasible and action.action_type == "decontaminate": + feasible_by_target.setdefault(action.target or action.action_id, action) + present = { + step.candidate_id + for step in steps + if step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "decontaminate" + } + missing = [ + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=action.action_id, + ) + for action in feasible_by_target.values() + if action.action_id not in present + ] + if missing: + decon_indices = [ + index + for index, step in enumerate(steps) + if step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "decontaminate" + ] + insertion = decon_indices[-1] + 1 if decon_indices else 0 + steps[insertion:insertion] = missing + + decon_condition = _requested_decontamination_condition(instruction) + requested_attempts = _requested_max_attempts(instruction) + if decon_condition is not None or requested_attempts is not None: + condition = ( + None + if decon_condition is None + else StepCompletionCondition( + criterion=CompletionCriterion(decon_condition[0]), + threshold=decon_condition[1], + ) + ) + attempts = requested_attempts or 3 + repaired: list[CommandStep] = [] + repeated_decon_ids: set[str] = set() + for step in steps: + is_decon = ( + step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "decontaminate" + ) + if not is_decon: + repaired.append(step) + continue + assert step.candidate_id is not None + if step.candidate_id in repeated_decon_ids: + continue + repeated_decon_ids.add(step.candidate_id) + repaired.append( + step.model_copy( + update={ + "max_attempts": max(step.max_attempts, attempts), + "until": condition if condition is not None else step.until, + } + ) + ) + steps = repaired + + # An explicit series of source-line percentages means place and then move + # the same shield through those host-planned positions. A request for + # multiple panels instead selects one distinct target prim per panel. + shield_fractions = _requested_shield_fractions(instruction, context) + shield_actions = [ + action + for action in context.available_actions + if action.feasible and action.action_type in {"place_shield", "move_shield"} + ] + if shield_actions and (shield_fractions or _requests_multiple_shields(instruction)): + existing_shield_steps = [ + step + for step in steps + if step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type in {"place_shield", "move_shield"} + ] + preferred_target = None + if existing_shield_steps: + preferred_target = by_id[existing_shield_steps[0].candidate_id].target + if preferred_target is None: + preferred_target = sorted(shield_actions, key=lambda action: action.action_id)[0].target + + desired_actions: list[AvailableAction] = [] + if _requests_multiple_shields(instruction): + targets: list[str] = [] + for action in sorted(shield_actions, key=lambda item: item.action_id): + key = action.target or action.action_id + if key not in targets: + targets.append(key) + for index, target in enumerate(targets): + fraction = ( + shield_fractions[min(index, len(shield_fractions) - 1)] + if shield_fractions + else (0.35 if index % 2 == 0 else 0.65) + ) + options = [ + action + for action in shield_actions + if (action.target or action.action_id) == target + ] + desired_actions.append( + min( + options, + key=lambda action: abs(float(action.placement_fraction or 0.0) - fraction), + ) + ) + else: + options = [action for action in shield_actions if action.target == preferred_target] + for fraction in shield_fractions: + desired_actions.append( + min( + options, + key=lambda action: abs(float(action.placement_fraction or 0.0) - fraction), + ) + ) + + desired_steps = [ + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=action.action_id, + ) + for action in desired_actions + ] + shield_indices = [ + index for index, step in enumerate(steps) if step in existing_shield_steps + ] + insertion = shield_indices[0] if shield_indices else 0 + steps = [step for step in steps if step not in existing_shield_steps] + lowered = instruction.casefold() + decon_position = min( + (lowered.find(token) for token in ("decontamin",) if token in lowered), + default=-1, + ) + shield_position = min( + (lowered.find(token) for token in ("shield",) if token in lowered), + default=-1, + ) + if decon_position >= 0 and shield_position > decon_position: + decon_indices = [ + index + for index, step in enumerate(steps) + if step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "decontaminate" + ] + if decon_indices: + insertion = decon_indices[-1] + 1 + steps[insertion:insertion] = desired_steps + + # "All/each station" is a facility tour: navigate to each distinct + # allowlisted station and integrate there before any requested return-home + # or status step. Routes are regenerated from the live robot pose per leg. + if _requests_all_targets(instruction, "measure") and _requested_measurement(instruction): + measurement_actions: list[AvailableAction] = [] + seen_targets: set[str] = set() + for action in sorted(context.available_actions, key=lambda item: item.action_id): + if not action.feasible or action.action_type != "measure": + continue + target = action.target or action.action_id + if target in seen_targets: + continue + seen_targets.add(target) + measurement_actions.append(action) + if measurement_actions: + old_measurement_steps = [ + step + for step in steps + if step.command == CommandName.MEASURE + or ( + step.command == CommandName.EXECUTE_CANDIDATE + and step.candidate_id in by_id + and by_id[step.candidate_id].action_type == "measure" + ) + ] + old_indices = [ + index for index, step in enumerate(steps) if step in old_measurement_steps + ] + steps = [step for step in steps if step not in old_measurement_steps] + trailing_index = next( + ( + index + for index, step in enumerate(steps) + if step.command + in {CommandName.RETURN_MEASUREMENT_ROBOT, CommandName.SHOW_STATUS} + ), + len(steps), + ) + insertion = min(old_indices[0], trailing_index) if old_indices else trailing_index + expanded_measurements: list[CommandStep] = [] + for action in measurement_actions: + expanded_measurements.extend( + ( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=action.action_id, + ), + CommandStep( + command=CommandName.MEASURE, + duration_s=requested_duration_s, + ), + ) + ) + steps[insertion:insertion] = expanded_measurements + + measurement_threshold = _requested_measurement_condition(instruction) + if measurement_threshold is not None: + attempts = requested_attempts or 3 + steps = [ + step.model_copy( + update={ + "max_attempts": max(step.max_attempts, attempts), + "until": StepCompletionCondition( + criterion=CompletionCriterion.MEASURED_RATE_CPS_AT_MOST, + threshold=measurement_threshold, + ), + } + ) + if step.command == CommandName.MEASURE + else step + for step in steps + ] + + if _requested_status(instruction) and not any( + step.command == CommandName.SHOW_STATUS for step in steps + ): + steps.append(CommandStep(command=CommandName.SHOW_STATUS)) + + if len(steps) > 24: + # The deterministic host limit is intentionally smaller than the + # candidate context. Refuse silent truncation by returning a status-only + # plan that explains the bounded-workflow limit. + steps = [CommandStep(command=CommandName.SHOW_STATUS)] + summary = "The requested workflow exceeds the 24-step safety limit; split the targets." + else: + summary = plan.summary + if ( + repaired_shield_placement + and not _requested_decontamination(instruction) + and len(shield_fractions) <= 1 + and not _requests_multiple_shields(instruction) + ): + duration = 2.0 if requested_duration_s is None else requested_duration_s + summary = ( + "Place the lead shield between the radiation source and protected area, " + f"then measure for {duration:g} seconds." + ) + return CommandPlan.model_validate( + { + **plan.model_dump(), + "language": language, + "summary": summary, + "steps": tuple(steps), + } + ) + + +class OpenAICompatibleCommandInterpreter: + """Interpret commands through llama.cpp without adding an SDK dependency.""" + + def __init__( + self, + endpoint: str, + *, + model: str = "radcounter-qwen3-4b", + timeout_s: float = 90.0, + allow_remote: bool = False, + ) -> None: + parsed = urlparse(endpoint) + if parsed.scheme not in {"http", "https"} or not parsed.hostname: + raise ValueError("endpoint must be an HTTP(S) URL") + if not allow_remote and parsed.hostname not in {"127.0.0.1", "localhost", "::1"}: + raise ValueError("the natural-language endpoint must be loopback-only") + self.endpoint = endpoint.rstrip("/") + self.model = model + self.timeout_s = timeout_s + + @property + def completion_url(self) -> str: + if self.endpoint.endswith("/v1"): + return f"{self.endpoint}/chat/completions" + return f"{self.endpoint}/v1/chat/completions" + + async def interpret(self, instruction: str, context: CommandContext) -> CommandPlan: + normalized = normalize_operator_instruction(instruction) + if not normalized: + raise CommandInterpretationError("instruction is empty") + return await asyncio.to_thread(self._interpret_sync, normalized, context) + + def _interpret_sync(self, instruction: str, context: CommandContext) -> CommandPlan: + context_json = context.model_dump_json(exclude_none=True) + schema = _inlined_command_schema() + payload = { + "model": self.model, + "messages": [ + {"role": "system", "content": _SYSTEM_PROMPT}, + { + "role": "user", + "content": ( + f"Current public application context:\n{context_json}\n\n" + f"Operator instruction:\n{instruction}" + ), + }, + ], + "temperature": 0.0, + "max_tokens": 2048, + "stream": False, + "response_format": { + "type": "json_schema", + "json_schema": { + "name": "radcounter_command_plan", + "strict": True, + "schema": schema, + }, + }, + } + request = Request( + self.completion_url, + data=json.dumps(payload, separators=(",", ":")).encode("utf-8"), + headers={ + "Authorization": "Bearer radcounter-local", + "Content-Type": "application/json", + }, + method="POST", + ) + try: + with urlopen(request, timeout=self.timeout_s) as response: # noqa: S310 + result = json.loads(response.read().decode("utf-8")) + except HTTPError as error: + details = error.read().decode("utf-8", errors="replace")[-800:] + raise CommandInterpretationError( + f"local model server returned HTTP {error.code}: {details}" + ) from error + except (OSError, URLError, json.JSONDecodeError) as error: + raise CommandInterpretationError(f"local model request failed: {error}") from error + try: + content = result["choices"][0]["message"]["content"] + if isinstance(content, list): + content = "".join( + str(item.get("text", "")) if isinstance(item, dict) else str(item) + for item in content + ) + plan = CommandPlan.model_validate_json(str(content)) + return _normalize_plan(plan, instruction, context) + except (KeyError, IndexError, TypeError, ValidationError, json.JSONDecodeError) as error: + raise CommandInterpretationError( + "local model returned a response that does not match the command schema" + ) from error diff --git a/radcounter/core/natural_language/models.py b/radcounter/core/natural_language/models.py new file mode 100644 index 0000000..4be4f62 --- /dev/null +++ b/radcounter/core/natural_language/models.py @@ -0,0 +1,252 @@ +"""Strict command contracts between a language model and the simulator.""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import StrEnum +from typing import Literal + +from pydantic import BaseModel, ConfigDict, Field, model_validator + + +class CommandName(StrEnum): + """Only operations that the natural-language layer may request.""" + + LOAD_DEFAULT_SCENE = "load_default_scene" + PLAY = "play" + PAUSE = "pause" + STEP = "step" + RESET = "reset" + INITIALIZE_RADIATION = "initialize_radiation" + SYNC_SCENE = "sync_scene" + MEASURE = "measure" + RENDER_DOSE_MAP = "render_dose_map" + EXPORT_MEASUREMENT = "export_measurement" + EXECUTE_CANDIDATE = "execute_candidate" + RETURN_MEASUREMENT_ROBOT = "return_measurement_robot" + SHOW_STATUS = "show_status" + + +class CompletionCriterion(StrEnum): + """Public-result predicates allowed to stop a bounded repeated step.""" + + DECONTAMINATION_REMOVED_FRACTION_AT_LEAST = "decontamination_removed_fraction_at_least" + DECONTAMINATION_REMAINING_FRACTION_AT_MOST = "decontamination_remaining_fraction_at_most" + DECONTAMINATION_COVERAGE_FRACTION_AT_LEAST = "decontamination_coverage_fraction_at_least" + MEASURED_RATE_CPS_AT_MOST = "measured_rate_cps_at_most" + SHIELD_PLACEMENT_ERROR_M_AT_MOST = "shield_placement_error_m_at_most" + + +class StepCompletionCondition(BaseModel): + """One allowlisted stop-early condition evaluated from public action results.""" + + model_config = ConfigDict(extra="forbid", frozen=True) + + criterion: CompletionCriterion + threshold: float = Field(ge=0.0, le=1.0e12) + + @model_validator(mode="after") + def validate_threshold_units(self) -> StepCompletionCondition: + if ( + self.criterion + in { + CompletionCriterion.DECONTAMINATION_REMOVED_FRACTION_AT_LEAST, + CompletionCriterion.DECONTAMINATION_REMAINING_FRACTION_AT_MOST, + CompletionCriterion.DECONTAMINATION_COVERAGE_FRACTION_AT_LEAST, + } + and self.threshold > 1.0 + ): + raise ValueError("fraction completion thresholds must be between zero and one") + return self + + +class AvailableAction(BaseModel): + """One scene-derived physical action exposed to the language model.""" + + model_config = ConfigDict(extra="forbid", frozen=True) + + action_id: str = Field(min_length=1, max_length=160) + action_type: str = Field(min_length=1, max_length=80) + label: str = Field(min_length=1, max_length=240) + target: str | None = Field(default=None, max_length=320) + target_label: str | None = Field(default=None, max_length=240) + robot_id: str | None = Field(default=None, max_length=320) + execution_mode: str | None = Field(default=None, max_length=160) + placement_fraction: float | None = Field(default=None, ge=0.0, le=1.0) + predicted_duration_s: float | None = Field(default=None, ge=0.0, le=3600.0) + feasible: bool = True + + +class CommandContext(BaseModel): + """Small, public-only snapshot supplied to the interpreter.""" + + model_config = ConfigDict(extra="forbid", frozen=True) + + session_state: str = Field(default="unknown", max_length=120) + stage_name: str | None = Field(default=None, max_length=240) + available_actions: tuple[AvailableAction, ...] = Field(default=(), max_length=96) + capabilities: tuple[CommandName, ...] = tuple(CommandName) + + +class CommandStep(BaseModel): + """One allowlisted operation in a bounded sequential workflow.""" + + model_config = ConfigDict(extra="forbid", frozen=True) + + command: CommandName + candidate_id: str | None = Field(default=None, max_length=160) + duration_s: float | None = Field(default=None, ge=0.05, le=120.0) + max_attempts: int = Field(default=1, ge=1, le=5) + until: StepCompletionCondition | None = None + + @model_validator(mode="before") + @classmethod + def discard_irrelevant_optional_arguments(cls, value: object) -> object: + if not isinstance(value, dict): + return value + normalized = dict(value) + if normalized.get("command") != CommandName.EXECUTE_CANDIDATE: + normalized.pop("candidate_id", None) + if normalized.get("command") != CommandName.MEASURE: + normalized.pop("duration_s", None) + if normalized.get("command") not in { + CommandName.EXECUTE_CANDIDATE, + CommandName.MEASURE, + }: + normalized.pop("max_attempts", None) + normalized.pop("until", None) + elif normalized.get("until") is not None and "max_attempts" not in normalized: + # A stop condition without a bound is unsafe. Give small local + # models a conservative host-owned bound instead of an unbounded loop. + normalized["max_attempts"] = 3 + return normalized + + @model_validator(mode="after") + def validate_arguments(self) -> CommandStep: + if self.command == CommandName.EXECUTE_CANDIDATE and not self.candidate_id: + raise ValueError("execute_candidate requires candidate_id") + return self + + +class CommandPlan(BaseModel): + """Schema-constrained output produced by the local language model.""" + + model_config = ConfigDict(extra="forbid", frozen=True) + + language: Literal["en"] = "en" + summary: str = Field(min_length=1, max_length=600) + steps: tuple[CommandStep, ...] = Field(min_length=1, max_length=24) + + +class PlanValidationError(ValueError): + """Raised when a syntactically valid model plan is unsafe for current state.""" + + +@dataclass(frozen=True, slots=True) +class ValidatedCommandPlan: + """Plan approved by deterministic host-side policy.""" + + plan: CommandPlan + requires_confirmation: bool + warnings: tuple[str, ...] = () + + +_CONFIRMATION_COMMANDS = frozenset( + { + CommandName.LOAD_DEFAULT_SCENE, + CommandName.RESET, + CommandName.EXECUTE_CANDIDATE, + CommandName.RETURN_MEASUREMENT_ROBOT, + } +) + + +def validate_command_plan( + plan: CommandPlan, + context: CommandContext, +) -> ValidatedCommandPlan: + """Resolve model output against current capabilities and candidate identifiers.""" + + capabilities = set(context.capabilities) + candidates = {action.action_id: action for action in context.available_actions} + warnings: list[str] = [] + execution_count = sum(step.max_attempts for step in plan.steps) + if execution_count > 48: + raise PlanValidationError( + f"workflow expands to {execution_count} executions; the safety limit is 48" + ) + for index, step in enumerate(plan.steps, start=1): + if step.command not in capabilities: + raise PlanValidationError( + f"step {index} requests unavailable command: {step.command.value}" + ) + if step.command == CommandName.RETURN_MEASUREMENT_ROBOT: + warnings.append("Physical action: return measurement robot to its starting position") + continue + if step.command == CommandName.MEASURE: + if step.until is not None and ( + step.until.criterion != CompletionCriterion.MEASURED_RATE_CPS_AT_MOST + ): + raise PlanValidationError( + f"step {index} uses a completion condition that does not apply to measurement" + ) + if step.max_attempts > 1: + warnings.append(f"Bounded repeated measurement: up to {step.max_attempts} attempts") + continue + if step.command != CommandName.EXECUTE_CANDIDATE: + continue + assert step.candidate_id is not None + candidate = candidates.get(step.candidate_id) + if candidate is None: + raise PlanValidationError( + f"step {index} references an action not present in the current scene: " + f"{step.candidate_id}" + ) + if not candidate.feasible: + raise PlanValidationError( + f"step {index} references an infeasible action: {step.candidate_id}" + ) + shield_retry = ( + candidate.action_type in {"place_shield", "move_shield"} + and step.until is not None + and step.until.criterion == CompletionCriterion.SHIELD_PLACEMENT_ERROR_M_AT_MOST + ) + if step.max_attempts > 1 and candidate.action_type != "decontaminate" and not shield_retry: + raise PlanValidationError( + f"step {index} repeats {candidate.action_type}; repeat one decontamination " + "candidate or use explicit ordered steps for physical repositioning" + ) + if step.until is not None: + decon_criteria = { + CompletionCriterion.DECONTAMINATION_REMOVED_FRACTION_AT_LEAST, + CompletionCriterion.DECONTAMINATION_REMAINING_FRACTION_AT_MOST, + CompletionCriterion.DECONTAMINATION_COVERAGE_FRACTION_AT_LEAST, + } + shield_criteria = { + CompletionCriterion.SHIELD_PLACEMENT_ERROR_M_AT_MOST, + } + if ( + ( + step.until.criterion in decon_criteria + and candidate.action_type != "decontaminate" + ) + or ( + step.until.criterion in shield_criteria + and candidate.action_type not in {"place_shield", "move_shield"} + ) + or step.until.criterion == CompletionCriterion.MEASURED_RATE_CPS_AT_MOST + ): + raise PlanValidationError( + f"step {index} completion condition does not apply to {candidate.action_type}" + ) + if step.max_attempts > 1: + warnings.append( + f"Bounded repeated physical action: {candidate.label} " + f"(up to {step.max_attempts} attempts)" + ) + warnings.append(f"Physical action: {candidate.label}") + return ValidatedCommandPlan( + plan=plan, + requires_confirmation=any(step.command in _CONFIRMATION_COMMANDS for step in plan.steps), + warnings=tuple(warnings), + ) diff --git a/radcounter/core/natural_language/runtime.py b/radcounter/core/natural_language/runtime.py new file mode 100644 index 0000000..51a0471 --- /dev/null +++ b/radcounter/core/natural_language/runtime.py @@ -0,0 +1,342 @@ +"""Lifecycle management for a bundled llama.cpp server and GGUF model.""" + +from __future__ import annotations + +import json +import os +import shutil +import socket +import subprocess +import threading +import time +from dataclasses import dataclass +from enum import StrEnum +from pathlib import Path +from typing import TextIO +from urllib.error import URLError +from urllib.parse import urlparse +from urllib.request import urlopen + + +class RuntimeAssetMissingError(RuntimeError): + """Raised when the packaged server binary or model is unavailable.""" + + +class RuntimeStatus(StrEnum): + STOPPED = "stopped" + STARTING = "starting" + READY = "ready" + FAILED = "failed" + EXTERNAL = "external" + + +@dataclass(frozen=True, slots=True) +class NvidiaGpu: + name: str + memory_mib: int + + +@dataclass(frozen=True, slots=True) +class LlamaCppRuntimeConfig: + """Product-owned inference configuration with environment overrides.""" + + runtime_directory: Path + server_binary: Path | None = None + model_path: Path | None = None + pid_path: Path | None = None + external_endpoint: str | None = None + model_alias: str = "radcounter-qwen3-4b" + context_size: int = 8192 + startup_timeout_s: float = 180.0 + request_parallelism: int = 1 + gpu_mode: str = "auto" + + @classmethod + def default(cls, repository_root: str | Path | None = None) -> LlamaCppRuntimeConfig: + root = ( + Path(repository_root).resolve() + if repository_root is not None + else Path(__file__).resolve().parents[3] + ) + runtime = Path( + os.environ.get("RADCOUNTER_LLM_RUNTIME_DIR", root / "runtime/llm") + ).expanduser() + binary = os.environ.get("RADCOUNTER_LLAMA_SERVER") + model = os.environ.get("RADCOUNTER_LLM_MODEL") + pid_path = os.environ.get("RADCOUNTER_LLM_PID_FILE") + return cls( + runtime_directory=runtime, + server_binary=None if not binary else Path(binary).expanduser(), + model_path=None if not model else Path(model).expanduser(), + pid_path=None if not pid_path else Path(pid_path).expanduser(), + external_endpoint=os.environ.get("RADCOUNTER_LLM_ENDPOINT"), + gpu_mode=os.environ.get("RADCOUNTER_LLM_GPU_MODE", "auto"), + ) + + +def _detect_nvidia_gpu() -> NvidiaGpu | None: + executable = shutil.which("nvidia-smi") + if executable is None: + return None + try: + completed = subprocess.run( + [ + executable, + "--query-gpu=name,memory.total", + "--format=csv,noheader,nounits", + ], + check=False, + capture_output=True, + text=True, + timeout=5, + ) + if completed.returncode != 0 or not completed.stdout.strip(): + return None + name, memory = completed.stdout.splitlines()[0].rsplit(",", 1) + return NvidiaGpu(name.strip(), int(memory.strip())) + except (OSError, ValueError, subprocess.SubprocessError): + return None + + +def _free_loopback_port() -> int: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as listener: + listener.bind(("127.0.0.1", 0)) + return int(listener.getsockname()[1]) + + +class LlamaCppRuntime: + """Start, monitor, and stop the product-bundled llama.cpp sidecar.""" + + def __init__(self, config: LlamaCppRuntimeConfig | None = None) -> None: + self.config = config or LlamaCppRuntimeConfig.default() + self.status = RuntimeStatus.STOPPED + self.endpoint: str | None = None + self.process: subprocess.Popen[str] | None = None + self.gpu = _detect_nvidia_gpu() + self._log_stream: TextIO | None = None + self._lock = threading.RLock() + + def _binary_candidates(self) -> tuple[Path, ...]: + executable = "llama-server.exe" if os.name == "nt" else "llama-server" + platform_name = "windows-x86_64" if os.name == "nt" else "linux-x86_64" + configured = () if self.config.server_binary is None else (self.config.server_binary,) + system = shutil.which("llama-server") + packaged = ( + ( + self.config.runtime_directory / "bin" / f"{platform_name}-cpu" / executable, + self.config.runtime_directory + / "bin" + / f"{platform_name}-vulkan" + / executable, + ) + if self.config.gpu_mode == "cpu" + else + ( + self.config.runtime_directory / "bin" / f"{platform_name}-cuda" / executable, + self.config.runtime_directory + / "bin" + / f"{platform_name}-vulkan" + / executable, + self.config.runtime_directory / "bin" / f"{platform_name}-cpu" / executable, + ) + if self.gpu is not None + else ( + self.config.runtime_directory / "bin" / f"{platform_name}-cpu" / executable, + self.config.runtime_directory + / "bin" + / f"{platform_name}-vulkan" + / executable, + ) + ) + return ( + *configured, + *packaged, + self.config.runtime_directory / "bin" / platform_name / executable, + self.config.runtime_directory / "bin" / executable, + *((Path(system),) if system else ()), + ) + + def resolve_binary(self) -> Path: + for candidate in self._binary_candidates(): + path = candidate.expanduser().resolve() + if path.is_file() and os.access(path, os.X_OK): + return path + raise RuntimeAssetMissingError( + "llama-server is not installed in runtime/llm; run scripts/build_llama_runtime.sh" + ) + + def resolve_model(self) -> Path: + candidates = ( + *((self.config.model_path,) if self.config.model_path is not None else ()), + self.config.runtime_directory / "models" / "Qwen3-4B-Q4_K_M.gguf", + ) + for candidate in candidates: + path = candidate.expanduser().resolve() + if path.is_file(): + return path + raise RuntimeAssetMissingError( + "Qwen3-4B GGUF is not installed in runtime/llm; run scripts/fetch_llm_model.py" + ) + + def _health(self, endpoint: str) -> bool: + try: + with urlopen(f"{endpoint}/health", timeout=2.0) as response: # noqa: S310 + if response.status != 200: + return False + payload = json.loads(response.read().decode("utf-8")) + return payload.get("status") in {"ok", "no slot available"} + except (OSError, URLError, json.JSONDecodeError): + return False + + def _command(self, binary: Path, model: Path, port: int) -> list[str]: + threads = max(2, min(16, (os.cpu_count() or 4) // 2)) + command = [ + str(binary), + "--model", + str(model), + "--alias", + self.config.model_alias, + "--host", + "127.0.0.1", + "--port", + str(port), + "--ctx-size", + str(self.config.context_size), + "--parallel", + str(self.config.request_parallelism), + "--threads", + str(threads), + "--jinja", + "--reasoning", + "off", + "--n-gpu-layers", + ] + if self.config.gpu_mode == "cpu" or self.gpu is None: + command.append("0") + elif self.config.gpu_mode in {"auto", "hybrid"}: + command.append("auto") + elif self.config.gpu_mode == "gpu": + command.append("all") + else: + raise ValueError("gpu_mode must be auto, hybrid, gpu, or cpu") + return command + + def _write_pid_file(self, pid: int) -> None: + path = self.config.pid_path + if path is None: + return + path = path.expanduser().resolve() + path.parent.mkdir(parents=True, exist_ok=True) + temporary = path.with_suffix(path.suffix + ".tmp") + temporary.write_text(f"{pid}\n", encoding="ascii") + temporary.replace(path) + + def _remove_pid_file(self, pid: int | None) -> None: + path = self.config.pid_path + if path is None: + return + path = path.expanduser().resolve() + try: + recorded = int(path.read_text(encoding="ascii").strip()) + except (FileNotFoundError, OSError, ValueError): + return + if pid is None or recorded == pid: + path.unlink(missing_ok=True) + + def start(self) -> str: + with self._lock: + if self.status in {RuntimeStatus.READY, RuntimeStatus.EXTERNAL}: + assert self.endpoint is not None + return self.endpoint + if self.config.external_endpoint: + endpoint = self.config.external_endpoint.rstrip("/") + parsed = urlparse(endpoint) + if parsed.hostname not in {"127.0.0.1", "localhost", "::1"}: + raise ValueError("the configured model endpoint must be loopback-only") + if not self._health(endpoint): + raise RuntimeError( + f"configured local model endpoint is unavailable: {endpoint}" + ) + self.endpoint = endpoint + self.status = RuntimeStatus.EXTERNAL + return endpoint + binary = self.resolve_binary() + model = self.resolve_model() + port = _free_loopback_port() + endpoint = f"http://127.0.0.1:{port}" + log_directory = self.config.runtime_directory / "logs" + log_directory.mkdir(parents=True, exist_ok=True) + self._log_stream = (log_directory / "llama-server.log").open( + "a", encoding="utf-8" + ) + self.status = RuntimeStatus.STARTING + try: + self.process = subprocess.Popen( + self._command(binary, model, port), + stdin=subprocess.DEVNULL, + stdout=self._log_stream, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + self._write_pid_file(self.process.pid) + except OSError: + self.status = RuntimeStatus.FAILED + failed_process = self.process + self.process = None + if failed_process is not None and failed_process.poll() is None: + failed_process.terminate() + failed_process.wait(timeout=8.0) + self._remove_pid_file( + None if failed_process is None else failed_process.pid + ) + self._log_stream.close() + self._log_stream = None + raise + deadline = time.monotonic() + self.config.startup_timeout_s + while time.monotonic() < deadline: + if self.process.poll() is not None: + self.status = RuntimeStatus.FAILED + stopped_pid = self.process.pid + self.process = None + self._remove_pid_file(stopped_pid) + if self._log_stream is not None: + self._log_stream.close() + self._log_stream = None + raise RuntimeError( + "llama-server stopped during startup; inspect " + "runtime/llm/logs/llama-server.log" + ) + if self._health(endpoint): + self.endpoint = endpoint + self.status = RuntimeStatus.READY + return endpoint + time.sleep(0.2) + self.stop() + self.status = RuntimeStatus.FAILED + raise TimeoutError("llama-server did not become healthy before the startup timeout") + + def stop(self) -> None: + with self._lock: + process = self.process + self.process = None + self.endpoint = None + if process is not None and process.poll() is None: + process.terminate() + try: + process.wait(timeout=8.0) + except subprocess.TimeoutExpired: + process.kill() + process.wait(timeout=3.0) + self._remove_pid_file(None if process is None else process.pid) + if self._log_stream is not None: + self._log_stream.close() + self._log_stream = None + self.status = RuntimeStatus.STOPPED + + def describe(self) -> str: + if self.gpu is None: + device = "CPU fallback" + else: + device = f"{self.gpu.name} ({self.gpu.memory_mib} MiB), {self.config.gpu_mode}" + return f"llama.cpp {self.status.value} · {device}" diff --git a/radcounter/core/rendering/camera.py b/radcounter/core/rendering/camera.py index 30fa46f..7d36f65 100644 --- a/radcounter/core/rendering/camera.py +++ b/radcounter/core/rendering/camera.py @@ -71,9 +71,7 @@ def process( permanent_count = self._update_permanent_damage(height * width) onset_ratio = max(0.0, dose_rate_gy_h / self.config.onset_dose_rate_gy_h - 1.0) - drop_probability = 1.0 - np.exp( - -self.config.drop_rate_per_s * onset_ratio * exposure_s - ) + drop_probability = 1.0 - np.exp(-self.config.drop_rate_per_s * onset_ratio * exposure_s) dropped = bool(self._rng.random() < drop_probability) if dropped: held = self._previous if self._previous is not None else np.zeros_like(source) @@ -108,9 +106,7 @@ def process( color_fraction = 1.0 / (1.0 + dose_rate_gy_h / half_rate) luminance = np.sum(rgb * np.asarray([0.2126, 0.7152, 0.0722]), axis=2, keepdims=True) rgb = luminance + color_fraction * (rgb - luminance) - bloom = self.config.bloom_gain * np.log1p( - dose_rate_gy_h / self.config.onset_dose_rate_gy_h - ) + bloom = self.config.bloom_gain * np.log1p(dose_rate_gy_h / self.config.onset_dose_rate_gy_h) rgb += bloom * np.maximum(rgb - 0.75, 0.0) noise_std = self.config.read_noise_std * np.sqrt(1.0 + onset_ratio) if noise_std: @@ -171,10 +167,7 @@ def _apply_transient_hits(self, rgb: np.ndarray, count: int) -> None: @staticmethod def _to_float(image: np.ndarray) -> tuple[np.ndarray, np.ndarray | None, float]: - if np.issubdtype(image.dtype, np.integer): - scale = float(np.iinfo(image.dtype).max) - else: - scale = 1.0 + scale = float(np.iinfo(image.dtype).max) if np.issubdtype(image.dtype, np.integer) else 1.0 rgb = image[..., :3].astype(np.float32) / scale alpha = image[..., 3:4].copy() if image.shape[2] == 4 else None return rgb, alpha, scale diff --git a/radcounter/core/rendering/evaluation.py b/radcounter/core/rendering/evaluation.py index 9c3935a..518b16c 100644 --- a/radcounter/core/rendering/evaluation.py +++ b/radcounter/core/rendering/evaluation.py @@ -51,9 +51,7 @@ def _global_ssim(reference: np.ndarray, simulated: np.ndarray) -> float: variance_left, variance_right = float(left.var()), float(right.var()) covariance = float(np.mean((left - mu_left) * (right - mu_right))) numerator = (2.0 * mu_left * mu_right + c1) * (2.0 * covariance + c2) - denominator = ( - (mu_left**2 + mu_right**2 + c1) * (variance_left + variance_right + c2) - ) + denominator = (mu_left**2 + mu_right**2 + c1) * (variance_left + variance_right + c2) scores.append(numerator / denominator) return float(np.mean(scores)) @@ -76,7 +74,9 @@ def _histogram_js(reference: np.ndarray, simulated: np.ndarray, bins: int = 64) p /= p.sum() q /= q.sum() midpoint = 0.5 * (p + q) - values.append(0.5 * np.sum(p * np.log(p / midpoint)) + 0.5 * np.sum(q * np.log(q / midpoint))) + values.append( + 0.5 * np.sum(p * np.log(p / midpoint)) + 0.5 * np.sum(q * np.log(q / midpoint)) + ) return float(np.mean(values)) diff --git a/radcounter/core/rendering/policy.py b/radcounter/core/rendering/policy.py index c898e4e..102b5c1 100644 --- a/radcounter/core/rendering/policy.py +++ b/radcounter/core/rendering/policy.py @@ -8,8 +8,8 @@ from dataclasses import dataclass from radcounter.core.rendering.models import ( - RenderMode, RendererPolicyConfig, + RenderMode, RenderPurpose, RenderQualityTier, ) @@ -175,10 +175,15 @@ def budget_for_tier( } if config.mode is not RenderMode.AUTO: mode = config.mode - elif purpose is RenderPurpose.CAPTURE and config.allow_path_tracing_for_capture and tier in { - RenderQualityTier.STRONG, - RenderQualityTier.BALANCED, - }: + elif ( + purpose is RenderPurpose.CAPTURE + and config.allow_path_tracing_for_capture + and tier + in { + RenderQualityTier.STRONG, + RenderQualityTier.BALANCED, + } + ): mode = RenderMode.PATH_TRACING elif tier is RenderQualityTier.FALLBACK: mode = RenderMode.STORM diff --git a/radcounter/core/robot_monitoring.py b/radcounter/core/robot_monitoring.py new file mode 100644 index 0000000..4564a5a --- /dev/null +++ b/radcounter/core/robot_monitoring.py @@ -0,0 +1,191 @@ +"""Simulator-independent state helpers for operator robot monitoring.""" + +from __future__ import annotations + +import math +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from typing import Any + +from radcounter.core.system_profiles import ResolvedSystemSelection + +MONITOR_UPDATE_HZ = 12.0 + +_ROBOT_COLORS = ( + (0.20, 0.78, 1.00, 0.92), + (1.00, 0.72, 0.24, 0.90), + (0.45, 0.90, 0.62, 0.90), + (0.78, 0.56, 1.00, 0.90), +) + +_REFERENCE_ROBOT_NAMES = { + "irobot-packbot-fukushima": "PackBot", + "flyability-elios3-rad": "Elios 3", +} + +_ACTION_LABELS = { + "measure": "Move to measurement station", + "decontaminate": "Decontaminate", + "place_shield": "Place shield", + "move_shield": "Move shield", + "move_object": "Move object", + "remove_object": "Remove object", + "repair_action": "Repair operation", +} + +_PHASE_LABELS = { + "idle": "Idle", + "navigating": "Navigating", + "stowing_arm": "Stowing arm", + "arm_stowed": "Arm stowed", + "approaching": "Approaching work surface", + "contact_confirmed": "Contact confirmed", + "decontaminating": "Decontaminating", + "grasping": "Grasping", + "releasing": "Releasing", + "returning_home": "Returning to start", + "complete": "Complete", + "failed": "Failed", +} + + +@dataclass(frozen=True, slots=True) +class MonitorRobot: + robot_id: str + display_name: str + prim_path: str + color_rgba: tuple[float, float, float, float] + + +def robots_for_selection(selection: ResolvedSystemSelection) -> tuple[MonitorRobot, ...]: + """Return stable operator-facing robot identities for a catalog selection.""" + + entries: list[tuple[str, str, str]] = [] + robot_set = selection.robot_set + if robot_set.kind == "reference": + entries.extend( + ( + robot.robot_id, + _REFERENCE_ROBOT_NAMES.get( + robot.reference_model_id, + robot.robot_id.replace("-", " ").title(), + ), + robot.prim_path, + ) + for robot in robot_set.reference_robots + ) + elif robot_set.kind == "fleet" and selection.robot_fleet is not None: + entries.extend( + ( + robot.id, + robot.id.replace("-", " ").title(), + str(robot.prim_path), + ) + for robot in selection.robot_fleet.robots + if robot.prim_path + ) + elif robot_set.kind == "decommissioning": + entries.extend( + ( + ("countermeasure", "Ridgeback + Franka", "/World/CountermeasureRobot"), + ("measurement", "Nova Carter", "/World/MeasurementRobot"), + ) + ) + return tuple( + MonitorRobot(robot_id, display_name, prim_path, _ROBOT_COLORS[index % len(_ROBOT_COLORS)]) + for index, (robot_id, display_name, prim_path) in enumerate(entries) + ) + + +def action_label(action_type: object) -> str: + value = str(action_type) + return _ACTION_LABELS.get(value, value.replace("_", " ")) + + +def phase_label(phase: str) -> str: + return _PHASE_LABELS.get(phase, phase.replace("_", " ")) + + +def _point3(value: object) -> tuple[float, float, float] | None: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)) or len(value) < 2: + return None + try: + point = ( + float(value[0]), + float(value[1]), + float(value[2]) if len(value) >= 3 else 0.0, + ) + except (TypeError, ValueError): + return None + return point if all(math.isfinite(item) for item in point) else None + + +def action_route_points(action: Any) -> tuple[tuple[float, float, float], ...]: + """Combine planner route fields into one de-duplicated operator route.""" + + parameters = getattr(action, "parameters", {}) + if not isinstance(parameters, Mapping): + return () + route: list[tuple[float, float, float]] = [] + for key in ("base_route_m", "pickup_base_route_m", "placement_base_route_m"): + values = parameters.get(key, ()) + if not isinstance(values, Sequence) or isinstance(values, (str, bytes)): + continue + for value in values: + point = _point3(value) + if point is not None and (not route or point != route[-1]): + route.append(point) + return tuple(route) + + +def action_target_point(action: Any) -> tuple[float, float, float] | None: + pose = getattr(action, "target_pose_world", None) + if pose is None: + return None + try: + point = (float(pose[0, 3]), float(pose[1, 3]), float(pose[2, 3])) + except (IndexError, KeyError, TypeError, ValueError): + return None + return point if all(math.isfinite(item) for item in point) else None + + +def padded_square_bounds( + lower_xy: Sequence[float], + upper_xy: Sequence[float], + *, + padding_fraction: float = 0.08, +) -> tuple[tuple[float, float], tuple[float, float]]: + """Expand a world XY range to a padded square suitable for a minimap.""" + + lower = (float(lower_xy[0]), float(lower_xy[1])) + upper = (float(upper_xy[0]), float(upper_xy[1])) + if not all(math.isfinite(value) for value in (*lower, *upper)): + raise ValueError("minimap bounds must be finite") + width = max(upper[0] - lower[0], 1.0) + height = max(upper[1] - lower[1], 1.0) + span = max(width, height) * (1.0 + 2.0 * max(0.0, padding_fraction)) + center = ((lower[0] + upper[0]) * 0.5, (lower[1] + upper[1]) * 0.5) + half = span * 0.5 + return (center[0] - half, center[1] - half), (center[0] + half, center[1] + half) + + +def project_minimap_point( + point_m: Sequence[float], + lower_xy: Sequence[float], + upper_xy: Sequence[float], +) -> tuple[float, float, float]: + """Project world XY into the SceneView's normalized top-down plane.""" + + width = max(float(upper_xy[0]) - float(lower_xy[0]), 1.0e-9) + height = max(float(upper_xy[1]) - float(lower_xy[1]), 1.0e-9) + x = -0.9 + 1.8 * (float(point_m[0]) - float(lower_xy[0])) / width + y = 0.9 - 1.8 * (float(point_m[1]) - float(lower_xy[1])) / height + return (max(-0.9, min(0.9, x)), max(-0.9, min(0.9, y)), 0.0) + + +def progress_from_remaining(remaining_m: float, initial_remaining_m: float) -> float: + if not math.isfinite(remaining_m) or not math.isfinite(initial_remaining_m): + return 0.0 + if initial_remaining_m <= 1.0e-9: + return 1.0 + return max(0.0, min(1.0, 1.0 - remaining_m / initial_remaining_m)) diff --git a/radcounter/core/robots/reference.py b/radcounter/core/robots/reference.py index bc57c6d..ba1365f 100644 --- a/radcounter/core/robots/reference.py +++ b/radcounter/core/robots/reference.py @@ -68,9 +68,21 @@ def config( maximum_speed_m_s=9.3 / 3.6, nominal_endurance_s=4.0 * 3600.0, sensor_hardpoints=( - SensorHardpoint("front_camera", "camera_link", (0.31, 0.0, 0.27), intended_sensor="RGB inspection camera"), - SensorHardpoint("survey_lidar", "lidar_link", (0.02, 0.0, 0.39), intended_sensor="3-D LiDAR"), - SensorHardpoint("radiation", "radiation_link", (0.08, 0.18, 0.40), intended_sensor="gamma dose-rate or spectroscopic detector"), + SensorHardpoint( + "front_camera", + "camera_link", + (0.31, 0.0, 0.27), + intended_sensor="RGB inspection camera", + ), + SensorHardpoint( + "survey_lidar", "lidar_link", (0.02, 0.0, 0.39), intended_sensor="3-D LiDAR" + ), + SensorHardpoint( + "radiation", + "radiation_link", + (0.08, 0.18, 0.40), + intended_sensor="gamma dose-rate or spectroscopic detector", + ), ), ) @@ -95,9 +107,18 @@ def config( ), nominal_endurance_s=7.5 * 60.0, sensor_hardpoints=( - SensorHardpoint("front_camera", "camera_link", (0.18, 0.0, -0.01), intended_sensor="inspection RGB/thermal camera group"), - SensorHardpoint("lidar", "lidar_link", (-0.02, 0.0, -0.10), intended_sensor="Ouster OS0 LiDAR"), - SensorHardpoint("radiation", "radiation_link", (-0.12, 0.0, -0.12), intended_sensor="Mirion RDS-32 WR"), + SensorHardpoint( + "front_camera", + "camera_link", + (0.18, 0.0, -0.01), + intended_sensor="inspection RGB/thermal camera group", + ), + SensorHardpoint( + "lidar", "lidar_link", (-0.02, 0.0, -0.10), intended_sensor="Ouster OS0 LiDAR" + ), + SensorHardpoint( + "radiation", "radiation_link", (-0.12, 0.0, -0.12), intended_sensor="Mirion RDS-32 WR" + ), ), ) @@ -105,7 +126,13 @@ def config( id="mhi-meister", manufacturer="Mitsubishi Heavy Industries", model="MEISTeR", - roles=("shield handling", "obstacle removal", "core sampling", "dry decontamination", "suction"), + roles=( + "shield handling", + "obstacle removal", + "core sampling", + "dry decontamination", + "suction", + ), source_urls=( "https://www.mhi.com/business/products-services/energy-environment/nuclear-power-generation/robot-mechatronics/meister", "https://www.mhi.com/news/1402201775.html", @@ -121,10 +148,27 @@ def config( maximum_speed_m_s=2.0 / 3.6, nominal_endurance_s=2.0 * 3600.0, sensor_hardpoints=( - SensorHardpoint("head_camera", "head_camera_link", (0.30, 0.0, 1.17), intended_sensor="stereo inspection camera"), - SensorHardpoint("dose_meter", "base_link", (0.12, 0.28, 0.66), intended_sensor="ion chamber"), - SensorHardpoint("left_tool", "left_tool0", (0.0, 0.0, 0.0), intended_sensor="tool camera / contact sensor"), - SensorHardpoint("right_tool", "right_tool0", (0.0, 0.0, 0.0), intended_sensor="tool camera / force sensor"), + SensorHardpoint( + "head_camera", + "head_camera_link", + (0.30, 0.0, 1.17), + intended_sensor="stereo inspection camera", + ), + SensorHardpoint( + "dose_meter", "base_link", (0.12, 0.28, 0.66), intended_sensor="ion chamber" + ), + SensorHardpoint( + "left_tool", + "left_tool0", + (0.0, 0.0, 0.0), + intended_sensor="tool camera / contact sensor", + ), + SensorHardpoint( + "right_tool", + "right_tool0", + (0.0, 0.0, 0.0), + intended_sensor="tool camera / force sensor", + ), ), ) @@ -147,16 +191,23 @@ def config( "remote pump, recovery tank, hose reel and corner rollers", ), sensor_hardpoints=( - SensorHardpoint("navigation_camera", "base_link", (0.42, 0.0, 0.82), intended_sensor="radiation-resistant navigation camera"), - SensorHardpoint("head_camera", "treatment_head", (0.08, 0.0, 0.18), intended_sensor="treatment-head camera"), + SensorHardpoint( + "navigation_camera", + "base_link", + (0.42, 0.0, 0.82), + intended_sensor="radiation-resistant navigation camera", + ), + SensorHardpoint( + "head_camera", + "treatment_head", + (0.08, 0.0, 0.18), + intended_sensor="treatment-head camera", + ), ), ) REAL_ROBOT_REFERENCES = MappingProxyType( - { - item.id: item - for item in (PACKBOT_FUKUSHIMA, ELIOS3_RAD, MHI_MEISTER, HITACHI_AROUNDER) - } + {item.id: item for item in (PACKBOT_FUKUSHIMA, ELIOS3_RAD, MHI_MEISTER, HITACHI_AROUNDER)} ) @@ -165,7 +216,9 @@ def get_real_robot_reference(model_id: str) -> RealRobotReference: return REAL_ROBOT_REFERENCES[model_id] except KeyError as exc: choices = ", ".join(sorted(REAL_ROBOT_REFERENCES)) - raise KeyError(f"Unknown real robot reference {model_id!r}; choose one of: {choices}") from exc + raise KeyError( + f"Unknown real robot reference {model_id!r}; choose one of: {choices}" + ) from exc def reference_config(model_id: str) -> RobotReferenceConfig: diff --git a/radcounter/core/surface_decontamination.py b/radcounter/core/surface_decontamination.py index d1ba72c..4792024 100644 --- a/radcounter/core/surface_decontamination.py +++ b/radcounter/core/surface_decontamination.py @@ -8,6 +8,60 @@ import numpy as np +def effective_contact_exposure_s( + dt_s: float, + surface_speed_m_s: float, + max_surface_speed_m_s: float, +) -> float: + """Return one cell's canonical exposure for one verified contact tick.""" + + if dt_s <= 0.0 or max_surface_speed_m_s <= 0.0 or surface_speed_m_s < 0.0: + raise ValueError("contact exposure inputs must be positive and speed nonnegative") + if surface_speed_m_s > max_surface_speed_m_s: + return 0.0 + speed_factor = max(0.15, 1.0 - surface_speed_m_s / max_surface_speed_m_s) + return dt_s * speed_factor + + +def irregular_deposition_field(cells_x: int = 48, cells_y: int = 28) -> np.ndarray: + """Return the canonical deterministic ragged contamination activity field. + + This is the flat-surface version used by the August 6, 2026 reference + renders. The numerical lattice is rectangular, but inactive cells remain + absent so the authored source consists of lobes, holes, and detached drops. + """ + + if cells_x < 8 or cells_y < 8: + raise ValueError("irregular deposition fields require at least 8 x 8 cells") + x = np.linspace(-1.0, 1.0, cells_x) + y = np.linspace(-1.0, 1.0, cells_y) + xx, yy = np.meshgrid(x, y, indexing="xy") + plume = ( + 2.6 * np.exp(-((xx - 0.28) ** 2 / 0.12 + (yy + 0.16) ** 2 / 0.22)) + + 1.9 * np.exp(-((xx + 0.28) ** 2 / 0.30 + (yy - 0.08) ** 2 / 0.10)) + + 1.25 * np.exp(-((xx + 0.68) ** 2 / 0.045 + (yy + 0.30) ** 2 / 0.15)) + + 0.85 * np.exp(-((xx - 0.58) ** 2 / 0.055 + (yy - 0.48) ** 2 / 0.055)) + ) + roughness = ( + 0.22 * np.sin(9.0 * xx + 2.2 * np.sin(5.0 * yy)) + + 0.18 * np.cos(11.0 * yy - 1.8 * xx) + + 0.11 * np.sin(19.0 * (xx + yy)) + ) + contaminated = plume + roughness > 0.70 + clean_holes = (((xx - 0.05) / 0.16) ** 2 + ((yy + 0.02) / 0.12) ** 2 < 1.0) | ( + ((xx + 0.48) / 0.09) ** 2 + ((yy - 0.03) / 0.08) ** 2 < 1.0 + ) + contaminated &= ~clean_holes + detached_droplets = ( + ((xx + 0.78) ** 2 + (yy - 0.58) ** 2 < 0.018) + | ((xx - 0.72) ** 2 + (yy + 0.64) ** 2 < 0.012) + | ((xx + 0.12) ** 2 + (yy - 0.72) ** 2 < 0.007) + ) + contaminated |= detached_droplets + local_activity = 92_000.0 * np.maximum(0.22, plume + 0.65 * roughness) + return np.where(contaminated, local_activity, 0.0).reshape(-1) + + @dataclass(frozen=True) class DecontaminationTool: length_m: float @@ -38,7 +92,7 @@ class DecontaminationStep: class SurfaceSourceGrid: - """A rectangular surface source discretized into independent activity cells.""" + """A surface-aligned numerical lattice of independent activity cells.""" def __init__( self, @@ -50,6 +104,8 @@ def __init__( center_world_m: tuple[float, float, float], activity_bq_per_cell: float | np.ndarray, efficiency_field: np.ndarray | None = None, + surface_u_world: tuple[float, float, float] = (1.0, 0.0, 0.0), + surface_v_world: tuple[float, float, float] = (0.0, 1.0, 0.0), ) -> None: if cells_x <= 0 or cells_y <= 0: raise ValueError("surface source dimensions must be positive") @@ -63,15 +119,29 @@ def __init__( self.cell_size_x_m = size_x_m / cells_x self.cell_size_y_m = size_y_m / cells_y + surface_u = np.asarray(surface_u_world, dtype=np.float64) + surface_v = np.asarray(surface_v_world, dtype=np.float64) + if np.linalg.norm(surface_u) <= 1e-12 or np.linalg.norm(surface_v) <= 1e-12: + raise ValueError("surface basis vectors cannot be zero") + surface_u /= np.linalg.norm(surface_u) + surface_v /= np.linalg.norm(surface_v) + if abs(float(np.dot(surface_u, surface_v))) > 1e-6: + raise ValueError("surface basis vectors must be orthogonal") + surface_normal = np.cross(surface_u, surface_v) + if np.linalg.norm(surface_normal) <= 1e-12: + raise ValueError("surface basis vectors cannot be parallel") + self.surface_u_world = surface_u + self.surface_v_world = surface_v + self.surface_normal_world = surface_normal / np.linalg.norm(surface_normal) + x = (np.arange(cells_x, dtype=np.float64) + 0.5) * self.cell_size_x_m - size_x_m * 0.5 y = (np.arange(cells_y, dtype=np.float64) + 0.5) * self.cell_size_y_m - size_y_m * 0.5 xx, yy = np.meshgrid(x, y, indexing="xy") - self.centers_world_m = np.column_stack( - ( - xx.reshape(-1) + self.center_world_m[0], - yy.reshape(-1) + self.center_world_m[1], - np.full(xx.size, self.center_world_m[2]), - ) + self.centers_surface_uv_m = np.column_stack((xx.reshape(-1), yy.reshape(-1))) + self.centers_world_m = ( + self.center_world_m[None, :] + + self.centers_surface_uv_m[:, :1] * self.surface_u_world[None, :] + + self.centers_surface_uv_m[:, 1:] * self.surface_v_world[None, :] ) initial = np.asarray(activity_bq_per_cell, dtype=np.float64) @@ -125,7 +195,7 @@ def apply_tool( if dt_s <= 0.0: raise ValueError("dt_s must be positive") center = np.asarray(tool_center_world_m, dtype=np.float64) - height_error = abs(center[2] - self.center_world_m[2]) + height_error = abs(float(np.dot(center - self.center_world_m, self.surface_normal_world))) activity_before = self.total_activity_bq if ( height_error > tool.max_contact_distance_m @@ -133,11 +203,13 @@ def apply_tool( ): return DecontaminationStep((), activity_before, activity_before, 0.0, 0.0) - relative = self.centers_world_m[:, :2] - center[:2] + relative_world = self.centers_world_m - center + relative_u = relative_world @ self.surface_u_world + relative_v = relative_world @ self.surface_v_world cosine = math.cos(tool_yaw_rad) sine = math.sin(tool_yaw_rad) - local_x = cosine * relative[:, 0] + sine * relative[:, 1] - local_y = -sine * relative[:, 0] + cosine * relative[:, 1] + local_x = cosine * relative_u + sine * relative_v + local_y = -sine * relative_u + cosine * relative_v contacted = (np.abs(local_x) <= tool.length_m * 0.5) & ( np.abs(local_y) <= tool.width_m * 0.5 ) @@ -145,8 +217,11 @@ def apply_tool( if not len(indices): return DecontaminationStep((), activity_before, activity_before, 0.0, 0.0) - speed_factor = max(0.15, 1.0 - surface_speed_m_s / tool.max_surface_speed_m_s) - effective_dwell = dt_s * speed_factor + effective_dwell = effective_contact_exposure_s( + dt_s, + surface_speed_m_s, + tool.max_surface_speed_m_s, + ) self.cumulative_exposure_s[indices] += effective_dwell exponent = ( -tool.rate_constant_s_inv diff --git a/radcounter/core/system_cli.py b/radcounter/core/system_cli.py new file mode 100644 index 0000000..0baece1 --- /dev/null +++ b/radcounter/core/system_cli.py @@ -0,0 +1,154 @@ +"""CLI for listing, validating, preparing, and activating system profiles.""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path + +from radcounter.core.environment import EnvironmentImportPipeline + +from .system_profiles import ( + default_catalog_path, + default_selection_path, + load_active_selection, + load_system_catalog, + resolve_system_selection, + save_active_selection, +) + + +def _selection_arguments(parser: argparse.ArgumentParser) -> None: + parser.add_argument("--catalog", type=Path, default=default_catalog_path()) + parser.add_argument("--profile") + parser.add_argument("--environment") + parser.add_argument("--robot-set") + parser.add_argument("--detector-set") + + +def _resolve(args: argparse.Namespace): + return resolve_system_selection( + catalog_path=args.catalog, + profile_id=args.profile, + environment_id=args.environment, + robot_set_id=args.robot_set, + detector_set_id=args.detector_set, + ) + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser( + description="Switch RadCounterSim environments, robots, and detectors" + ) + subparsers = parser.add_subparsers(dest="command", required=True) + + list_parser = subparsers.add_parser("list", help="list catalog components and profiles") + list_parser.add_argument("--catalog", type=Path, default=default_catalog_path()) + + show_parser = subparsers.add_parser("show", help="show one fully resolved selection") + _selection_arguments(show_parser) + + check_parser = subparsers.add_parser("check", help="validate paths and import prerequisites") + _selection_arguments(check_parser) + + prepare_parser = subparsers.add_parser( + "prepare", help="normalize the selected environment into the content-addressed cache" + ) + _selection_arguments(prepare_parser) + + activate_parser = subparsers.add_parser( + "activate", help="save the selection used by future radcounter-app launches" + ) + _selection_arguments(activate_parser) + activate_parser.add_argument("--selection-file", type=Path, default=default_selection_path()) + + current_parser = subparsers.add_parser("current", help="show the active saved selection") + current_parser.add_argument("--selection-file", type=Path, default=default_selection_path()) + + args = parser.parse_args(argv) + if args.command == "list": + catalog_path, catalog = load_system_catalog(args.catalog) + payload = { + "catalog": str(catalog_path), + "default_profile": catalog.default_profile, + "profiles": { + key: { + "display_name": value.display_name, + "environment": value.environment, + "robot_set": value.robot_set, + "detector_set": value.detector_set, + "application_mode": value.application_mode, + } + for key, value in catalog.profiles.items() + }, + "environments": sorted(catalog.environments), + "robot_sets": sorted(catalog.robot_sets), + "detector_sets": sorted(catalog.detector_sets), + } + print(json.dumps(payload, indent=2, sort_keys=True)) + return 0 + if args.command == "current": + print(json.dumps(load_active_selection(args.selection_file).as_dict(), indent=2)) + return 0 + + selection = _resolve(args) + if args.command == "show": + print(json.dumps(selection.as_dict(), indent=2, sort_keys=True)) + return 0 + if args.command == "check": + payload = selection.as_dict() + if not selection.environment_ready: + print(json.dumps({"valid": False, **payload}, indent=2, sort_keys=True)) + return 2 + print(json.dumps({"valid": True, **payload}, indent=2, sort_keys=True)) + return 0 + if args.command == "prepare": + if not selection.environment_ready: + source = selection.environment_source_path + raise FileNotFoundError( + f"selected environment is not ready: {source}; " + f"{selection.environment_entry.setup_hint or 'provide the configured source file'}" + ) + result = EnvironmentImportPipeline().import_environment( + selection.environment_config, + base_directory=selection.environment_descriptor_path.parent, + ) + print( + json.dumps( + { + "prepared": True, + "profile": selection.profile_id, + "environment": selection.environment_id, + "manifest": str(result.manifest_path), + "vertices": result.scene.vertex_count, + "triangles": result.scene.triangle_count, + "bounds_m": result.scene.bounds_m, + "warnings": result.scene.warnings, + }, + indent=2, + sort_keys=True, + ) + ) + return 0 + if args.command == "activate": + path = save_active_selection( + args.selection_file, + catalog_path=selection.catalog_path, + profile_id=selection.profile_id, + environment_id=args.environment, + robot_set_id=args.robot_set, + detector_set_id=args.detector_set, + ) + print( + json.dumps( + {"activated": True, "selection_file": str(path), **selection.as_dict()}, + indent=2, + sort_keys=True, + ) + ) + return 0 + raise AssertionError(args.command) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/radcounter/core/system_profiles.py b/radcounter/core/system_profiles.py new file mode 100644 index 0000000..498b6a7 --- /dev/null +++ b/radcounter/core/system_profiles.py @@ -0,0 +1,487 @@ +"""Composable environment, robot, and detector selections for RadCounterSim.""" + +from __future__ import annotations + +import json +import os +from dataclasses import dataclass +from pathlib import Path +from typing import Literal +from urllib.parse import urlparse + +import yaml +from pydantic import BaseModel, ConfigDict, Field, model_validator + +from radcounter.core.environment import EnvironmentImportConfig, load_environment_descriptor +from radcounter.core.robots import RobotFleetConfig, load_robot_fleet +from radcounter.core.sensors import DetectorDescriptor +from radcounter.core.sensors.catalog import popular_detector_catalog +from radcounter.core.sensors.plugins import load_detector_descriptor + +DEFAULT_CATALOG_RELATIVE_PATH = Path("configs/system/catalog.yaml") + + +class _FrozenModel(BaseModel): + model_config = ConfigDict(extra="forbid", frozen=True) + + +class EnvironmentSpawnAnchor(_FrozenModel): + """A curated, environment-relative pose that keeps catalog robots operable.""" + + kind: Literal["ground_robot", "aerial_robot", "work_surface", "camera"] + translation_m: tuple[float, float, float] + yaw_deg: float = 0.0 + description: str = "" + + +class EnvironmentCatalogEntry(_FrozenModel): + display_name: str = Field(min_length=1) + descriptor_uri: str = Field(min_length=1) + description: str = "" + setup_hint: str | None = None + preparation_scripts: tuple[str, ...] = () + spawn_anchors: dict[str, EnvironmentSpawnAnchor] = Field(default_factory=dict) + + +class ReferenceRobotPlacement(_FrozenModel): + robot_id: str = Field(min_length=1) + reference_model_id: str = Field(min_length=1) + prim_path: str = Field(pattern=r"^/World(?:/.*)?$") + spawn_anchor: str | None = None + translation_m: tuple[float, float, float] = (0.0, 0.0, 0.0) + yaw_deg: float = 0.0 + + +class RobotSetCatalogEntry(_FrozenModel): + display_name: str = Field(min_length=1) + kind: Literal["decommissioning", "reference", "fleet", "none"] + description: str = "" + fleet_uri: str | None = None + reference_robots: tuple[ReferenceRobotPlacement, ...] = () + + @model_validator(mode="after") + def validate_payload(self) -> RobotSetCatalogEntry: + if self.kind == "fleet" and not self.fleet_uri: + raise ValueError("fleet robot sets require fleet_uri") + if self.kind != "fleet" and self.fleet_uri is not None: + raise ValueError("fleet_uri is valid only for fleet robot sets") + if self.kind == "reference" and not self.reference_robots: + raise ValueError("reference robot sets require reference_robots") + if self.kind != "reference" and self.reference_robots: + raise ValueError("reference_robots is valid only for reference robot sets") + ids = [robot.robot_id for robot in self.reference_robots] + if len(ids) != len(set(ids)): + raise ValueError("reference robot IDs must be unique within a robot set") + return self + + +class DetectorPlacement(_FrozenModel): + detector_id: str = Field(min_length=1) + model_id: str = Field(min_length=1) + descriptor_uri: str | None = None + parent_robot_id: str | None = None + parent_sensor_link: str | None = None + parent_prim_path: str | None = None + translation_m: tuple[float, float, float] = (0.0, 0.0, 0.0) + rotation_rpy_deg: tuple[float, float, float] = (0.0, 0.0, 0.0) + + @model_validator(mode="after") + def validate_parent(self) -> DetectorPlacement: + robot_parent = self.parent_robot_id is not None + prim_parent = self.parent_prim_path is not None + if robot_parent == prim_parent: + raise ValueError( + "a detector requires exactly one of parent_robot_id or parent_prim_path" + ) + if self.parent_sensor_link is not None and not robot_parent: + raise ValueError("parent_sensor_link requires parent_robot_id") + if self.parent_prim_path is not None and not self.parent_prim_path.startswith("/World"): + raise ValueError("parent_prim_path must be rooted below /World") + return self + + +class DetectorSetCatalogEntry(_FrozenModel): + display_name: str = Field(min_length=1) + description: str = "" + detectors: tuple[DetectorPlacement, ...] + + def model_post_init(self, __context: object) -> None: + ids = [detector.detector_id for detector in self.detectors] + if len(ids) != len(set(ids)): + raise ValueError("detector IDs must be unique within a detector set") + + +class SystemProfileConfig(_FrozenModel): + display_name: str = Field(min_length=1) + environment: str = Field(min_length=1) + robot_set: str = Field(min_length=1) + detector_set: str = Field(min_length=1) + runtime_config_uri: str = Field(min_length=1) + application_mode: Literal["decommissioning", "configurable"] = "configurable" + description: str = "" + + +class SystemCatalog(_FrozenModel): + schema_version: Literal[1] + default_profile: str = Field(min_length=1) + environments: dict[str, EnvironmentCatalogEntry] + robot_sets: dict[str, RobotSetCatalogEntry] + detector_sets: dict[str, DetectorSetCatalogEntry] + profiles: dict[str, SystemProfileConfig] + + @model_validator(mode="after") + def validate_references(self) -> SystemCatalog: + if self.default_profile not in self.profiles: + raise ValueError(f"default profile is missing: {self.default_profile}") + for profile_id, profile in self.profiles.items(): + missing = [] + if profile.environment not in self.environments: + missing.append(f"environment={profile.environment}") + if profile.robot_set not in self.robot_sets: + missing.append(f"robot_set={profile.robot_set}") + if profile.detector_set not in self.detector_sets: + missing.append(f"detector_set={profile.detector_set}") + if missing: + raise ValueError(f"profile {profile_id!r} has missing references: {missing}") + return self + + +@dataclass(frozen=True) +class ResolvedDetector: + placement: DetectorPlacement + descriptor: DetectorDescriptor + + +@dataclass(frozen=True) +class ResolvedSystemSelection: + catalog_path: Path + profile_id: str + profile: SystemProfileConfig + environment_id: str + environment_entry: EnvironmentCatalogEntry + environment_descriptor_path: Path + environment_config: EnvironmentImportConfig + robot_set_id: str + robot_set: RobotSetCatalogEntry + robot_fleet_path: Path | None + robot_fleet: RobotFleetConfig | None + detector_set_id: str + detector_set: DetectorSetCatalogEntry + detectors: tuple[ResolvedDetector, ...] + runtime_config_path: Path + has_overrides: bool = False + + @property + def configurable(self) -> bool: + return self.profile.application_mode == "configurable" or self.has_overrides + + @property + def environment_source_path(self) -> Path | None: + parsed = urlparse(self.environment_config.uri) + if parsed.scheme in {"http", "https"}: + return None + raw = Path(parsed.path if parsed.scheme == "file" else self.environment_config.uri) + if raw.is_absolute(): + return raw.expanduser().resolve() + return (self.environment_descriptor_path.parent / raw).expanduser().resolve() + + @property + def environment_ready(self) -> bool: + source = self.environment_source_path + return source is None or source.is_file() + + @property + def environment_preparation_scripts(self) -> tuple[Path, ...]: + return tuple( + _catalog_path(self.catalog_path, uri) + for uri in self.environment_entry.preparation_scripts + ) + + def spawn_anchor(self, anchor_id: str) -> EnvironmentSpawnAnchor: + """Resolve a named pose from the selected environment's operator layout.""" + + try: + return self.environment_entry.spawn_anchors[anchor_id] + except KeyError as error: + raise ValueError( + f"environment {self.environment_id!r} has no spawn anchor {anchor_id!r}" + ) from error + + def as_dict(self) -> dict[str, object]: + source = self.environment_source_path + return { + "profile": self.profile_id, + "display_name": self.profile.display_name, + "application_mode": ( + "configurable" if self.configurable else self.profile.application_mode + ), + "environment": { + "id": self.environment_id, + "display_name": self.environment_entry.display_name, + "descriptor": str(self.environment_descriptor_path), + "source": str(source) if source is not None else self.environment_config.uri, + "ready": self.environment_ready, + "setup_hint": self.environment_entry.setup_hint, + "preparation_scripts": [str(path) for path in self.environment_preparation_scripts], + "spawn_anchors": { + anchor_id: anchor.model_dump(mode="json") + for anchor_id, anchor in self.environment_entry.spawn_anchors.items() + }, + }, + "robot_set": { + "id": self.robot_set_id, + "display_name": self.robot_set.display_name, + "kind": self.robot_set.kind, + "fleet": str(self.robot_fleet_path) if self.robot_fleet_path else None, + "robots": [ + { + "robot_id": robot.robot_id, + "reference_model_id": robot.reference_model_id, + "prim_path": robot.prim_path, + "spawn_anchor": robot.spawn_anchor, + } + for robot in self.robot_set.reference_robots + ], + }, + "detector_set": { + "id": self.detector_set_id, + "display_name": self.detector_set.display_name, + "detectors": [ + { + "detector_id": item.placement.detector_id, + "model_id": item.descriptor.model_id, + "display_name": item.descriptor.display_name, + "parent_robot_id": item.placement.parent_robot_id, + "parent_prim_path": item.placement.parent_prim_path, + } + for item in self.detectors + ], + }, + "runtime_config": str(self.runtime_config_path), + } + + +def repository_root() -> Path: + return Path(__file__).resolve().parents[2] + + +def default_catalog_path() -> Path: + override = os.environ.get("RADCOUNTER_SYSTEM_CATALOG") + if override: + return Path(override).expanduser().resolve() + return repository_root() / DEFAULT_CATALOG_RELATIVE_PATH + + +def default_selection_path() -> Path: + override = os.environ.get("RADCOUNTER_SELECTION_FILE") + if override: + return Path(override).expanduser().resolve() + config_home = Path(os.environ.get("XDG_CONFIG_HOME", Path.home() / ".config")) + return config_home / "radcountersim/system-selection.json" + + +def load_system_catalog(path: str | Path | None = None) -> tuple[Path, SystemCatalog]: + catalog_path = Path(path or default_catalog_path()).expanduser().resolve() + payload = yaml.safe_load(catalog_path.read_text(encoding="utf-8")) + if not isinstance(payload, dict): + raise ValueError("system catalog root must be a mapping") + return catalog_path, SystemCatalog.model_validate(payload) + + +def _catalog_path(catalog_path: Path, uri: str) -> Path: + candidate = Path(uri).expanduser() + if not candidate.is_absolute(): + candidate = catalog_path.parent / candidate + return candidate.resolve() + + +def _rebase_path(value: str, base: Path) -> str: + parsed = urlparse(value) + if parsed.scheme in {"http", "https", "file"}: + return value + candidate = Path(value).expanduser() + return str(candidate.resolve() if candidate.is_absolute() else (base / candidate).resolve()) + + +def _resolved_fleet(path: Path) -> RobotFleetConfig: + fleet = load_robot_fleet(path) + robots = [] + for robot in fleet.robots: + update: dict[str, object] = { + "uri": _rebase_path(robot.uri, path.parent), + "package_paths": tuple(_rebase_path(item, path.parent) for item in robot.package_paths), + } + if robot.lula_kinematics is not None: + update["lula_kinematics"] = robot.lula_kinematics.model_copy( + update={ + "robot_description_path": _rebase_path( + robot.lula_kinematics.robot_description_path, path.parent + ), + "urdf_path": _rebase_path(robot.lula_kinematics.urdf_path, path.parent), + } + ) + robots.append(robot.model_copy(update=update)) + return fleet.model_copy(update={"robots": tuple(robots)}) + + +def resolve_system_selection( + *, + catalog_path: str | Path | None = None, + profile_id: str | None = None, + environment_id: str | None = None, + robot_set_id: str | None = None, + detector_set_id: str | None = None, +) -> ResolvedSystemSelection: + resolved_catalog_path, catalog = load_system_catalog(catalog_path) + selected_profile_id = profile_id or catalog.default_profile + try: + profile = catalog.profiles[selected_profile_id] + except KeyError as error: + raise KeyError(f"unknown system profile: {selected_profile_id}") from error + selected_environment = environment_id or profile.environment + selected_robots = robot_set_id or profile.robot_set + selected_detectors = detector_set_id or profile.detector_set + try: + environment_entry = catalog.environments[selected_environment] + robot_set = catalog.robot_sets[selected_robots] + detector_set = catalog.detector_sets[selected_detectors] + except KeyError as error: + raise KeyError(f"unknown system component: {error.args[0]}") from error + + descriptor_path = _catalog_path(resolved_catalog_path, environment_entry.descriptor_uri) + environment_config = load_environment_descriptor(descriptor_path) + fleet_path = None + fleet = None + if robot_set.fleet_uri: + fleet_path = _catalog_path(resolved_catalog_path, robot_set.fleet_uri) + fleet = _resolved_fleet(fleet_path) + + builtins = popular_detector_catalog() + detectors = [] + for placement in detector_set.detectors: + if placement.descriptor_uri: + descriptor = load_detector_descriptor( + _catalog_path(resolved_catalog_path, placement.descriptor_uri) + ) + else: + try: + descriptor = builtins[placement.model_id] + except KeyError as error: + raise KeyError( + f"unknown built-in detector model {placement.model_id!r}; " + "set descriptor_uri for a custom detector" + ) from error + if descriptor.model_id != placement.model_id: + raise ValueError( + f"detector {placement.detector_id!r} requested model {placement.model_id!r} " + f"but its descriptor defines {descriptor.model_id!r}" + ) + detectors.append(ResolvedDetector(placement, descriptor)) + + available_robot_ids = { + "reference": {robot.robot_id for robot in robot_set.reference_robots}, + "fleet": {robot.id for robot in fleet.robots} if fleet is not None else set(), + "decommissioning": {"countermeasure", "measurement"}, + "none": set(), + }[robot_set.kind] + missing_spawn_anchors = sorted( + { + robot.spawn_anchor + for robot in robot_set.reference_robots + if robot.spawn_anchor is not None + and robot.spawn_anchor not in environment_entry.spawn_anchors + } + ) + if missing_spawn_anchors: + raise ValueError( + f"environment {selected_environment!r} does not provide spawn anchors required by " + f"robot set {selected_robots!r}: {missing_spawn_anchors}" + ) + missing_detector_robots = sorted( + { + item.placement.parent_robot_id + for item in detectors + if item.placement.parent_robot_id is not None + and item.placement.parent_robot_id not in available_robot_ids + } + ) + if missing_detector_robots: + raise ValueError( + f"detector set {selected_detectors!r} requires robots not present in " + f"{selected_robots!r}: {missing_detector_robots}" + ) + + runtime_path = _catalog_path(resolved_catalog_path, profile.runtime_config_uri) + if not runtime_path.is_file(): + raise FileNotFoundError(f"runtime configuration is missing: {runtime_path}") + return ResolvedSystemSelection( + catalog_path=resolved_catalog_path, + profile_id=selected_profile_id, + profile=profile, + environment_id=selected_environment, + environment_entry=environment_entry, + environment_descriptor_path=descriptor_path, + environment_config=environment_config, + robot_set_id=selected_robots, + robot_set=robot_set, + robot_fleet_path=fleet_path, + robot_fleet=fleet, + detector_set_id=selected_detectors, + detector_set=detector_set, + detectors=tuple(detectors), + runtime_config_path=runtime_path, + has_overrides=( + (environment_id is not None and environment_id != profile.environment) + or (robot_set_id is not None and robot_set_id != profile.robot_set) + or (detector_set_id is not None and detector_set_id != profile.detector_set) + ), + ) + + +def save_active_selection( + selection_path: str | Path, + *, + catalog_path: str | Path, + profile_id: str, + environment_id: str | None = None, + robot_set_id: str | None = None, + detector_set_id: str | None = None, +) -> Path: + target = Path(selection_path).expanduser().resolve() + payload = { + "schema_version": 1, + "catalog_path": str(Path(catalog_path).expanduser().resolve()), + "profile_id": profile_id, + "environment_id": environment_id, + "robot_set_id": robot_set_id, + "detector_set_id": detector_set_id, + } + target.parent.mkdir(parents=True, exist_ok=True) + temporary = target.with_suffix(target.suffix + ".tmp") + temporary.write_text(json.dumps(payload, indent=2, sort_keys=True) + "\n", encoding="utf-8") + temporary.replace(target) + return target + + +def load_active_selection( + selection_path: str | Path | None = None, +) -> ResolvedSystemSelection: + path = Path(selection_path or default_selection_path()).expanduser().resolve() + if not path.is_file(): + return resolve_system_selection( + catalog_path=os.environ.get("RADCOUNTER_SYSTEM_CATALOG"), + profile_id=os.environ.get("RADCOUNTER_SYSTEM_PROFILE"), + environment_id=os.environ.get("RADCOUNTER_ENVIRONMENT"), + robot_set_id=os.environ.get("RADCOUNTER_ROBOT_SET"), + detector_set_id=os.environ.get("RADCOUNTER_DETECTOR_SET"), + ) + payload = json.loads(path.read_text(encoding="utf-8")) + if payload.get("schema_version") != 1: + raise ValueError(f"unsupported active selection schema: {path}") + return resolve_system_selection( + catalog_path=os.environ.get("RADCOUNTER_SYSTEM_CATALOG") or payload["catalog_path"], + profile_id=os.environ.get("RADCOUNTER_SYSTEM_PROFILE") or payload["profile_id"], + environment_id=os.environ.get("RADCOUNTER_ENVIRONMENT") or payload.get("environment_id"), + robot_set_id=os.environ.get("RADCOUNTER_ROBOT_SET") or payload.get("robot_set_id"), + detector_set_id=os.environ.get("RADCOUNTER_DETECTOR_SET") or payload.get("detector_set_id"), + ) diff --git a/radcounter/core/water_decontamination.py b/radcounter/core/water_decontamination.py index fad959d..6a42b17 100644 --- a/radcounter/core/water_decontamination.py +++ b/radcounter/core/water_decontamination.py @@ -108,7 +108,7 @@ def __init__( state: WaterDecontaminationState, *, washability: np.ndarray | None = None, - surface_normal_world: tuple[float, float, float] = (0.0, 0.0, 1.0), + surface_normal_world: tuple[float, float, float] | None = None, runoff_direction_world_xy: tuple[float, float] = (1.0, 0.0), ) -> None: self.grid = grid @@ -116,10 +116,15 @@ def __init__( self.reference_activity_bq = ( grid.total_activity_bq + state.captured_activity_bq + state.discharged_activity_bq ) - normal = np.asarray(surface_normal_world, dtype=np.float64) + normal = np.asarray( + grid.surface_normal_world if surface_normal_world is None else surface_normal_world, + dtype=np.float64, + ) if np.linalg.norm(normal) <= 1e-12: raise ValueError("surface normal cannot be zero") self.surface_normal = normal / np.linalg.norm(normal) + if abs(float(np.dot(self.surface_normal, grid.surface_normal_world))) < 1.0 - 1e-6: + raise ValueError("surface normal must align with the source grid plane") runoff = np.asarray(runoff_direction_world_xy, dtype=np.float64) if np.linalg.norm(runoff) <= 1e-12: raise ValueError("runoff direction cannot be zero") @@ -309,20 +314,14 @@ def visual_color_rgb(self, wetness_at_full_color_l_m2: float = 0.8) -> np.ndarra def _build_runoff_targets(self) -> np.ndarray: step = min(self.grid.cell_size_x_m, self.grid.cell_size_y_m) * 1.05 targets = np.full(len(self.grid.activity_bq), -1, dtype=np.int64) - lower = ( - self.grid.center_world_m[:2] - - np.asarray([self.grid.size_x_m, self.grid.size_y_m]) * 0.5 - ) - upper = ( - self.grid.center_world_m[:2] - + np.asarray([self.grid.size_x_m, self.grid.size_y_m]) * 0.5 - ) - for index, center in enumerate(self.grid.centers_world_m[:, :2]): + lower = -np.asarray([self.grid.size_x_m, self.grid.size_y_m]) * 0.5 + upper = np.asarray([self.grid.size_x_m, self.grid.size_y_m]) * 0.5 + for index, center in enumerate(self.grid.centers_surface_uv_m): target_point = center + self.runoff_direction_xy * step if np.any(target_point < lower) or np.any(target_point > upper): continue distance = np.linalg.norm( - self.grid.centers_world_m[:, :2] - target_point, + self.grid.centers_surface_uv_m - target_point, axis=1, ) target = int(np.argmin(distance)) diff --git a/radcounter/launcher.py b/radcounter/launcher.py new file mode 100644 index 0000000..4fc5ded --- /dev/null +++ b/radcounter/launcher.py @@ -0,0 +1,29 @@ +"""Single-entry launcher for a user-owned Isaac Sim installation.""" + +from __future__ import annotations + +import os +import sys +from pathlib import Path + + +def main() -> int: + repository_root = Path( + os.environ.get("RADCOUNTER_APP_ROOT", Path(__file__).resolve().parents[1]) + ).resolve() + launcher = repository_root / "scripts/run_app.sh" + if not launcher.is_file(): + raise FileNotFoundError( + f"RadCounterSim application launcher is missing: {launcher}; " + "set RADCOUNTER_APP_ROOT to the release directory" + ) + os.execvpe( + "bash", + ["bash", str(launcher), *sys.argv[1:]], + {**os.environ, "RADCOUNTER_APP_ROOT": str(repository_root)}, + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/runtime/llm/README.md b/runtime/llm/README.md new file mode 100644 index 0000000..be5f2e1 --- /dev/null +++ b/runtime/llm/README.md @@ -0,0 +1,32 @@ +# RadCounterSim local inference runtime + +This directory is the product-owned runtime layout. Large or platform-specific +artifacts are deliberately not committed to Git. + +```text +runtime/llm/ +├── bin/ +│ ├── linux-x86_64-cpu/llama-server +│ ├── linux-x86_64-vulkan/llama-server +│ └── linux-x86_64-cuda/llama-server +├── models/Qwen3-4B-Q4_K_M.gguf +└── logs/llama-server.log +``` + +Prepare a development or release tree with: + +```bash +./scripts/build_llama_runtime.sh +uv run python scripts/fetch_llm_model.py +``` + +The build script pins llama.cpp and installs verified official CPU and Vulkan +release binaries, then optionally produces a CUDA binary when the CUDA compiler +is present. The model downloader pins the official Qwen repository revision and +verifies the complete GGUF SHA-256 before installation. + +At runtime RadCounterSim prefers CUDA, falls back to Vulkan GPU offload, and uses +the CPU binary when no GPU is available. Set +`RADCOUNTER_LLM_GPU_MODE=cpu|hybrid|auto|gpu` to override layer placement. A +developer may set `RADCOUNTER_LLM_ENDPOINT` to use an already-running loopback +server; this is not required in the packaged app. diff --git a/runtime/llm/bin/.gitkeep b/runtime/llm/bin/.gitkeep new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/runtime/llm/bin/.gitkeep @@ -0,0 +1 @@ + diff --git a/runtime/llm/licenses/Qwen3-Apache-2.0.txt b/runtime/llm/licenses/Qwen3-Apache-2.0.txt new file mode 100644 index 0000000..4921e3e --- /dev/null +++ b/runtime/llm/licenses/Qwen3-Apache-2.0.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2025 Alibaba Cloud + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. \ No newline at end of file diff --git a/runtime/llm/licenses/llama.cpp-MIT.txt b/runtime/llm/licenses/llama.cpp-MIT.txt new file mode 100644 index 0000000..e7dca55 --- /dev/null +++ b/runtime/llm/licenses/llama.cpp-MIT.txt @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023-2026 The ggml authors + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/runtime/llm/models/.gitkeep b/runtime/llm/models/.gitkeep new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/runtime/llm/models/.gitkeep @@ -0,0 +1 @@ + diff --git a/scripts/build_llama_runtime.sh b/scripts/build_llama_runtime.sh new file mode 100755 index 0000000..e226277 --- /dev/null +++ b/scripts/build_llama_runtime.sh @@ -0,0 +1,88 @@ +#!/usr/bin/env bash +set -euo pipefail + +repository_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +llama_ref="${RADCOUNTER_LLAMA_CPP_REF:-b9637}" +runtime_root="${RADCOUNTER_LLM_RUNTIME_DIR:-$repository_root/runtime/llm}" +download_root="$repository_root/.cache/llama.cpp-$llama_ref-releases" +mkdir -p "$download_root" "$runtime_root/licenses" + +install_release() { + local backend="$1" + local expected_sha256="$2" + local archive="llama-$llama_ref-bin-ubuntu-$backend-x64.tar.gz" + if [[ "$backend" == "cpu" ]]; then + archive="llama-$llama_ref-bin-ubuntu-x64.tar.gz" + fi + local url="https://github.com/ggml-org/llama.cpp/releases/download/$llama_ref/$archive" + local cached="$download_root/$archive" + local destination="$runtime_root/bin/linux-x86_64-$backend" + local extract_root + extract_root="$(mktemp -d)" + + if [[ ! -f "$cached" ]]; then + curl --fail --location --retry 3 --output "$cached.part" "$url" + mv "$cached.part" "$cached" + fi + printf '%s %s\n' "$expected_sha256" "$cached" | sha256sum --check --status + tar -xzf "$cached" -C "$extract_root" + local server + server="$(find "$extract_root" -type f -name llama-server -print -quit)" + if [[ -z "$server" ]]; then + printf '%s\n' "llama-server was not present in $archive" >&2 + exit 1 + fi + mkdir -p "$destination" + cmake -E copy "$server" "$destination/llama-server" + find "$(dirname "$server")" -maxdepth 1 \( -type f -o -type l \) -name 'lib*.so*' \ + -exec cmake -E copy {} "$destination" \; + chmod +x "$destination/llama-server" + rm -rf "$extract_root" + printf '%s\n' "Installed verified $backend llama-server in $destination" +} + +install_release cpu a50ee14f021a9d8e92e30f622f7e3be1318ee1125bb9a9ba8d2025388df48743 +install_release vulkan 6ca268d758aae9e8518afa43042678e8b60b47f0d34df7d6efff4ca622c74313 + +license_path="$runtime_root/licenses/llama.cpp-MIT.txt" +license_url="https://raw.githubusercontent.com/ggml-org/llama.cpp/$llama_ref/LICENSE" +if [[ ! -f "$license_path" ]]; then + curl --fail --location --retry 3 --output "$license_path.part" "$license_url" + mv "$license_path.part" "$license_path" +fi +printf '%s %s\n' \ + 94f29bbed6a22c35b992c5c6ebf0e7c92f13b836b90f36f461c9cf2f0f1d010d \ + "$license_path" | sha256sum --check --status + +build_backend() { + local backend="$1" + local cuda_flag="$2" + local build_root="$repository_root/.cache/llama.cpp-$llama_ref-build-$backend" + local destination="$runtime_root/bin/linux-x86_64-$backend" + + cmake -S "$source_root" -B "$build_root" \ + -DCMAKE_BUILD_TYPE=Release \ + -DGGML_CUDA="$cuda_flag" \ + -DGGML_NATIVE=OFF \ + -DLLAMA_CURL=OFF \ + -DLLAMA_BUILD_TESTS=OFF \ + -DLLAMA_BUILD_EXAMPLES=OFF + cmake --build "$build_root" --config Release --target llama-server -j + mkdir -p "$destination" + cmake -E copy "$build_root/bin/llama-server" "$destination/llama-server" + find "$build_root/bin" -maxdepth 1 \( -type f -o -type l \) -name 'lib*.so*' \ + -exec cmake -E copy {} "$destination" \; + chmod +x "$destination/llama-server" + printf '%s\n' "Installed $backend llama-server in $destination" +} + +if command -v nvcc >/dev/null 2>&1; then + source_root="$repository_root/.cache/llama.cpp-$llama_ref" + if [[ ! -d "$source_root/.git" ]]; then + git clone --depth 1 --branch "$llama_ref" \ + https://github.com/ggml-org/llama.cpp.git "$source_root" + fi + build_backend cuda ON +else + printf '%s\n' "CUDA compiler not found; using packaged Vulkan GPU offload." >&2 +fi diff --git a/scripts/convert_solidworks_to_usd.py b/scripts/convert_solidworks_to_usd.py new file mode 100755 index 0000000..89a9b9c --- /dev/null +++ b/scripts/convert_solidworks_to_usd.py @@ -0,0 +1,217 @@ +#!/usr/bin/env python3 +"""Convert a native SolidWorks assembly to USD with Isaac Sim's Linux HOOPS backend.""" + +from __future__ import annotations + +import argparse +import contextlib +import hashlib +import json +import os +import re +import shlex +import subprocess +import sys +import tempfile +from datetime import UTC, datetime +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[1] +DEFAULT_SOURCE = ( + ROOT / ".cache/external/fukushima_daiichi_solidworks/source/Building.SLDASM" +) +DEFAULT_OUTPUT = ( + ROOT / ".cache/external/fukushima_daiichi_solidworks/export/Building.usdc" +) +DEFAULT_CONFIG = ROOT / "configs/environment-converters/solidworks_to_usd.hoops.json" + + +def _arguments(argv: list[str] | None = None) -> argparse.Namespace: + parser = argparse.ArgumentParser( + description=( + "Convert .SLDASM/.SLDPRT to a monolithic USD file using the Linux " + "HOOPS Exchange converter bundled with Isaac Sim" + ) + ) + parser.add_argument("--source", type=Path, default=DEFAULT_SOURCE) + parser.add_argument("--output", type=Path, default=DEFAULT_OUTPUT) + parser.add_argument("--config", type=Path, default=DEFAULT_CONFIG) + parser.add_argument("--isaac-root", type=Path) + parser.add_argument("--force", action="store_true") + return parser.parse_args(argv) + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _version_key(path: Path) -> tuple[int, ...]: + numbers = re.findall(r"\d+", path.name.split("-standalone", maxsplit=1)[0]) + return tuple(int(value) for value in numbers) + + +def discover_isaac_root(explicit: Path | None = None) -> Path: + candidates: list[Path] = [] + if explicit is not None: + candidates.append(explicit) + environment = os.environ.get("RADCOUNTER_ISAAC_ROOT") + if environment: + candidates.append(Path(environment)) + candidates.extend(Path.home().glob(".local/isaacsim/*-standalone")) + valid = [path.expanduser().resolve() for path in candidates if (path / "kit/kit").is_file()] + if not valid: + raise FileNotFoundError( + "Isaac Sim standalone was not found; set RADCOUNTER_ISAAC_ROOT or pass --isaac-root" + ) + return max(valid, key=_version_key) + + +def _newest_extension(root: Path, prefix: str) -> Path: + matches = sorted((root / "extscache").glob(f"{prefix}-*")) + if not matches: + raise FileNotFoundError(f"Isaac Sim extension is missing: {prefix}") + return matches[-1] + + +def _conversion_command( + isaac_root: Path, + *, + source: Path, + output: Path, + config: Path, +) -> tuple[list[str], str, str]: + hoops = _newest_extension(isaac_root, "omni.kit.converter.hoops") + hoops_core = _newest_extension(isaac_root, "omni.kit.converter.hoops_core") + launch_script = hoops / "omni/kit/converter/hoops/process/launch_hoops_app.py" + if not launch_script.is_file(): + raise FileNotFoundError(f"HOOPS launch script is missing: {launch_script}") + script_arguments = " ".join( + ( + shlex.quote(str(launch_script)), + "--config-path", + shlex.quote(str(config)), + "--input-path", + shlex.quote(str(source)), + "--output-path", + shlex.quote(str(output)), + ) + ) + command = [ + str(isaac_root / "kit/kit"), + str(isaac_root / "kit/apps/omni.app.empty.kit"), + "--ext-folder", + str(isaac_root / "extscache"), + "--ext-folder", + str(isaac_root / "kit/extscore"), + "--ext-folder", + str(isaac_root / "kit/exts"), + "--enable", + "omni.kit.converter.hoops_core", + "--exec", + script_arguments, + "--no-window", + "--/app/fastShutdown=1", + "--/persistent/app/usd/muteUsdDiagnostics=false", + ] + return command, hoops.name, hoops_core.name + + +def convert(args: argparse.Namespace) -> dict[str, object]: + source = args.source.expanduser().resolve() + output = args.output.expanduser().resolve() + config = args.config.expanduser().resolve() + if not source.is_file(): + raise FileNotFoundError( + f"SolidWorks source is missing: {source}. Run scripts/fetch_fukushima_daiichi_cad.py" + ) + if source.suffix.lower() not in {".sldasm", ".sldprt"}: + raise ValueError(f"expected .SLDASM or .SLDPRT input: {source}") + if not config.is_file(): + raise FileNotFoundError(f"HOOPS converter config is missing: {config}") + if output.exists() and not args.force: + raise FileExistsError(f"output already exists (pass --force to replace it): {output}") + + isaac_root = discover_isaac_root(args.isaac_root) + output.parent.mkdir(parents=True, exist_ok=True) + with tempfile.TemporaryDirectory(prefix=".radcounter-cad-", dir=output.parent) as raw_temp: + temporary = Path(raw_temp) / output.name + command, hoops_extension, hoops_core_extension = _conversion_command( + isaac_root, + source=source, + output=temporary, + config=config, + ) + process = subprocess.run( + command, + cwd=source.parent, + check=False, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + timeout=900, + ) + log = process.stdout + success_marker = "[omni.converter.hoops_progress]*end*0*" + if process.returncode != 0 or success_marker not in log or not temporary.is_file(): + tail = "\n".join(log.splitlines()[-80:]) + raise RuntimeError( + f"HOOPS conversion failed with process code {process.returncode}:\n{tail}" + ) + if temporary.stat().st_size < 1024: + raise RuntimeError("HOOPS conversion produced an implausibly small USD file") + temporary.replace(output) + + mesh_match = re.search(r"Total Meshes in USD = (\d+)", log) + triangle_match = re.search(r"Total Triangles in USD = (\d+)", log) + converter_match = re.search(r"HOOPS Converter version: ([^\r\n]+)", log) + revision = None + with contextlib.suppress(OSError, subprocess.CalledProcessError): + revision = subprocess.run( + ["git", "-C", str(source.parent), "rev-parse", "HEAD"], + check=True, + capture_output=True, + text=True, + ).stdout.strip() + provenance = { + "schema_version": 1, + "created_at": datetime.now(UTC).isoformat(), + "source": str(source), + "source_sha256": _sha256(source), + "source_repository_revision": revision, + "output": str(output), + "output_sha256": _sha256(output), + "output_bytes": output.stat().st_size, + "mesh_count": int(mesh_match.group(1)) if mesh_match else None, + "triangle_count": int(triangle_match.group(1)) if triangle_match else None, + "converter": "NVIDIA Omniverse HOOPS Exchange", + "converter_version": converter_match.group(1).strip() if converter_match else None, + "hoops_extension": hoops_extension, + "hoops_core_extension": hoops_core_extension, + "isaac_root": str(isaac_root), + "config": str(config), + "config_sha256": _sha256(config), + } + provenance_path = output.with_suffix(output.suffix + ".provenance.json") + provenance_path.write_text( + json.dumps(provenance, indent=2, sort_keys=True) + "\n", + encoding="utf-8", + ) + return {**provenance, "provenance": str(provenance_path)} + + +def main(argv: list[str] | None = None) -> int: + try: + payload = convert(_arguments(argv)) + except Exception as error: + print(f"error: {type(error).__name__}: {error}", file=sys.stderr) + return 1 + print(json.dumps(payload, indent=2, sort_keys=True)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/export_solidworks_assembly.ps1 b/scripts/export_solidworks_assembly.ps1 new file mode 100644 index 0000000..d6e4764 --- /dev/null +++ b/scripts/export_solidworks_assembly.ps1 @@ -0,0 +1,96 @@ +[CmdletBinding()] +param( + [Parameter(Mandatory = $true)] + [string]$SourceRepository, + + [Parameter(Mandatory = $true)] + [string]$OutputStep, + + [switch]$Force, + [switch]$KeepSolidWorksOpen +) + +$ErrorActionPreference = "Stop" +$sourceRoot = (Resolve-Path -LiteralPath $SourceRepository).Path +$assemblyPath = Join-Path $sourceRoot "Building.SLDASM" +if (-not (Test-Path -LiteralPath $assemblyPath -PathType Leaf)) { + throw "Building.SLDASM was not found below $sourceRoot" +} + +$outputPath = [System.IO.Path]::GetFullPath($OutputStep) +if ([System.IO.Path]::GetExtension($outputPath).ToLowerInvariant() -notin @(".step", ".stp")) { + throw "OutputStep must end in .step or .stp" +} +if ((Test-Path -LiteralPath $outputPath) -and -not $Force) { + throw "Output already exists: $outputPath. Pass -Force to replace it." +} +[System.IO.Directory]::CreateDirectory([System.IO.Path]::GetDirectoryName($outputPath)) | Out-Null + +$solidWorks = $null +$model = $null +$openErrors = 0 +$openWarnings = 0 +$saveErrors = 0 +$saveWarnings = 0 +try { + $solidWorks = New-Object -ComObject SldWorks.Application + $solidWorks.Visible = $false + $model = $solidWorks.OpenDoc6( + $assemblyPath, + 2, + 1, + "", + [ref]$openErrors, + [ref]$openWarnings + ) + if ($null -eq $model) { + throw "SolidWorks could not open the assembly (errors=$openErrors warnings=$openWarnings)" + } + $model.ForceRebuild3($false) | Out-Null + $saved = $model.Extension.SaveAs( + $outputPath, + 0, + 1, + $null, + [ref]$saveErrors, + [ref]$saveWarnings + ) + if (-not $saved -or $saveErrors -ne 0 -or -not (Test-Path -LiteralPath $outputPath)) { + throw "STEP export failed (saved=$saved errors=$saveErrors warnings=$saveWarnings)" + } + + $sourceRevision = $null + if (Get-Command git -ErrorAction SilentlyContinue) { + $sourceRevision = (& git -C $sourceRoot rev-parse HEAD 2>$null) + } + $provenance = [ordered]@{ + schema_version = 1 + source_repository = "https://github.com/Qualot/fukushima_daiichi_solidworks" + source_revision = $sourceRevision + source_assembly = $assemblyPath + source_license = "CC BY 4.0" + source_author_attribution = "Qualot/fukushima_daiichi_solidworks contributors" + exported_step = $outputPath + exported_step_sha256 = (Get-FileHash -Algorithm SHA256 -LiteralPath $outputPath).Hash.ToLowerInvariant() + solidworks_revision = $solidWorks.RevisionNumber() + open_warnings = $openWarnings + save_warnings = $saveWarnings + } + $provenancePath = "$outputPath.provenance.json" + $provenance | ConvertTo-Json -Depth 4 | Set-Content -LiteralPath $provenancePath -Encoding utf8 + $provenance | ConvertTo-Json -Depth 4 +} +finally { + if ($null -ne $solidWorks -and $null -ne $model) { + $solidWorks.CloseDoc($model.GetTitle()) + } + if ($null -ne $solidWorks -and -not $KeepSolidWorksOpen) { + $solidWorks.ExitApp() + } + if ($null -ne $model) { + [System.Runtime.InteropServices.Marshal]::ReleaseComObject($model) | Out-Null + } + if ($null -ne $solidWorks) { + [System.Runtime.InteropServices.Marshal]::ReleaseComObject($solidWorks) | Out-Null + } +} diff --git a/scripts/fetch_fukushima_daiichi_cad.py b/scripts/fetch_fukushima_daiichi_cad.py new file mode 100755 index 0000000..7ec8b88 --- /dev/null +++ b/scripts/fetch_fukushima_daiichi_cad.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +"""Fetch the pinned CC BY 4.0 Fukushima Daiichi SolidWorks source.""" + +from __future__ import annotations + +import argparse +import hashlib +import json +import shutil +import subprocess +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[1] +DEFAULT_REMOTE = "https://github.com/Qualot/fukushima_daiichi_solidworks.git" +DEFAULT_REVISION = "f6541deb6159c5d908a4f028d021e3d2c9f7f8e8" +DEFAULT_DESTINATION = ROOT / ".cache/external/fukushima_daiichi_solidworks/source" + + +def _run(arguments: list[str], *, cwd: Path | None = None) -> str: + result = subprocess.run( + arguments, + cwd=cwd, + check=True, + text=True, + capture_output=True, + ) + return result.stdout.strip() + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser( + description="Fetch the pinned Fukushima Daiichi SolidWorks environment source" + ) + parser.add_argument("--remote", default=DEFAULT_REMOTE) + parser.add_argument("--revision", default=DEFAULT_REVISION) + parser.add_argument("--destination", type=Path, default=DEFAULT_DESTINATION) + args = parser.parse_args(argv) + if shutil.which("git") is None: + raise RuntimeError("git is required to fetch the Fukushima CAD source") + + destination = args.destination.expanduser().resolve() + if destination.exists(): + if not (destination / ".git").is_dir(): + raise FileExistsError( + f"destination exists but is not a git checkout: {destination}" + ) + dirty = _run(["git", "status", "--porcelain"], cwd=destination) + if dirty: + raise RuntimeError( + f"refusing to change a modified Fukushima CAD checkout: {destination}" + ) + _run(["git", "fetch", "--depth", "1", "origin", args.revision], cwd=destination) + _run(["git", "switch", "--detach", args.revision], cwd=destination) + else: + destination.parent.mkdir(parents=True, exist_ok=True) + _run( + [ + "git", + "clone", + "--filter=blob:none", + "--no-checkout", + args.remote, + str(destination), + ] + ) + _run(["git", "fetch", "--depth", "1", "origin", args.revision], cwd=destination) + _run(["git", "checkout", "--detach", args.revision], cwd=destination) + + actual_revision = _run(["git", "rev-parse", "HEAD"], cwd=destination) + if actual_revision != args.revision: + raise RuntimeError( + f"unexpected Fukushima CAD revision: expected {args.revision}, got {actual_revision}" + ) + assembly = destination / "Building.SLDASM" + license_path = destination / "LICENSE" + if not assembly.is_file() or not license_path.is_file(): + raise RuntimeError("checkout lacks Building.SLDASM or LICENSE") + + provenance = { + "schema_version": 1, + "source_repository": args.remote, + "source_revision": actual_revision, + "source_license": "CC BY 4.0", + "source_author_attribution": "Qualot/fukushima_daiichi_solidworks contributors", + "top_level_assembly": str(assembly), + "top_level_assembly_sha256": _sha256(assembly), + "license_sha256": _sha256(license_path), + "redistribution_note": ( + "Preserve upstream attribution and the CC BY 4.0 license when sharing exports." + ), + } + manifest = destination.parent / "source-provenance.json" + temporary = manifest.with_suffix(".json.tmp") + temporary.write_text( + json.dumps(provenance, indent=2, sort_keys=True) + "\n", encoding="utf-8" + ) + temporary.replace(manifest) + print(json.dumps({"source": str(destination), "provenance": str(manifest), **provenance})) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/fetch_llm_model.py b/scripts/fetch_llm_model.py new file mode 100755 index 0000000..6ceaf92 --- /dev/null +++ b/scripts/fetch_llm_model.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +"""Download and verify the product's pinned Qwen3 GGUF model.""" + +from __future__ import annotations + +import argparse +import hashlib +import sys +from pathlib import Path +from urllib.request import Request, urlopen + +ROOT = Path(__file__).resolve().parents[1] +MODEL_NAME = "Qwen3-4B-Q4_K_M.gguf" +MODEL_REVISION = "bc640142c66e1fdd12af0bd68f40445458f3869b" +MODEL_URL = ( + "https://huggingface.co/Qwen/Qwen3-4B-GGUF/resolve/" + f"{MODEL_REVISION}/{MODEL_NAME}?download=true" +) +MODEL_SHA256 = "7485fe6f11af29433bc51cab58009521f205840f5b4ae3a32fa7f92e8534fdf5" +LICENSE_URL = ( + "https://huggingface.co/Qwen/Qwen3-4B-GGUF/resolve/" + f"{MODEL_REVISION}/LICENSE" +) +LICENSE_SHA256 = "5de36594c10839788a8c589443a8ef9d8b8d17c65a1b5807206ae037fc36c6bd" + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + while chunk := stream.read(8 * 1024 * 1024): + digest.update(chunk) + return digest.hexdigest() + + +def _arguments() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--destination", + type=Path, + default=ROOT / "runtime/llm/models" / MODEL_NAME, + ) + return parser.parse_args() + + +def _install_license(runtime_root: Path) -> None: + destination = runtime_root / "licenses/Qwen3-Apache-2.0.txt" + if destination.is_file() and _sha256(destination) == LICENSE_SHA256: + return + destination.parent.mkdir(parents=True, exist_ok=True) + with urlopen(LICENSE_URL, timeout=30) as response: # noqa: S310 + payload = response.read() + actual = hashlib.sha256(payload).hexdigest() + if actual != LICENSE_SHA256: + raise RuntimeError( + f"Qwen license SHA-256 mismatch: expected {LICENSE_SHA256}, got {actual}" + ) + destination.write_bytes(payload) + + +def main() -> int: + args = _arguments() + destination = args.destination.expanduser().resolve() + destination.parent.mkdir(parents=True, exist_ok=True) + if destination.is_file(): + actual = _sha256(destination) + if actual == MODEL_SHA256: + _install_license(destination.parents[1]) + print(f"Model is already verified: {destination}") + return 0 + raise RuntimeError( + f"refusing to replace an existing model with SHA-256 {actual}: {destination}" + ) + + temporary = destination.with_suffix(destination.suffix + ".part") + offset = temporary.stat().st_size if temporary.is_file() else 0 + request = Request(MODEL_URL, headers={"User-Agent": "RadCounterSim/0.1"}) + if offset: + request.add_header("Range", f"bytes={offset}-") + response = urlopen(request, timeout=120) # noqa: S310 + if offset and response.status != 206: + offset = 0 + mode = "ab" if offset else "wb" + with response, temporary.open(mode) as output: + total_header = response.headers.get("Content-Range") or response.headers.get( + "Content-Length" + ) + print(f"Downloading {MODEL_NAME} ({total_header or 'size unknown'})") + received = offset + next_report = received + 128 * 1024 * 1024 + while chunk := response.read(8 * 1024 * 1024): + output.write(chunk) + received += len(chunk) + if received >= next_report: + print(f" {received / (1024**3):.2f} GiB", flush=True) + next_report += 128 * 1024 * 1024 + + actual = _sha256(temporary) + if actual != MODEL_SHA256: + temporary.unlink() + raise RuntimeError(f"model SHA-256 mismatch: expected {MODEL_SHA256}, got {actual}") + temporary.replace(destination) + _install_license(destination.parents[1]) + print(f"Installed verified model: {destination}") + return 0 + + +if __name__ == "__main__": + try: + raise SystemExit(main()) + except KeyboardInterrupt: + print("Download interrupted; the partial file can be resumed.", file=sys.stderr) + raise SystemExit(130) from None diff --git a/scripts/fetch_manchester_nuclear_assets.py b/scripts/fetch_manchester_nuclear_assets.py index f64aa03..03165c4 100644 --- a/scripts/fetch_manchester_nuclear_assets.py +++ b/scripts/fetch_manchester_nuclear_assets.py @@ -9,11 +9,10 @@ import argparse import hashlib import json -from pathlib import Path import shutil import urllib.request import zipfile - +from pathlib import Path ARTICLE_API = "https://api.figshare.com/v2/articles/25224974" DEFAULT_FILE = "500L_Drum_Store.zip" diff --git a/scripts/install_desktop_entry.py b/scripts/install_desktop_entry.py new file mode 100755 index 0000000..ead6b2a --- /dev/null +++ b/scripts/install_desktop_entry.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 +"""Install a per-user desktop entry for the RadCounterSim launcher.""" + +from __future__ import annotations + +import os +from pathlib import Path + + +def main() -> int: + root = Path(__file__).resolve().parents[1] + launcher = root / "scripts/run_app.sh" + applications = Path.home() / ".local/share/applications" + applications.mkdir(parents=True, exist_ok=True) + desktop = applications / "radcountersim.desktop" + desktop.write_text( + "\n".join( + ( + "[Desktop Entry]", + "Type=Application", + "Version=1.0", + "Name=RadCounterSim", + "Comment=Radiation measurement and countermeasure simulation", + f"Exec={launcher}", + f"Path={root}", + "Terminal=false", + "Icon=applications-science", + "Categories=Science;Education;", + "StartupNotify=true", + "", + ) + ), + encoding="utf-8", + ) + os.chmod(desktop, 0o755) + print(f"Installed desktop entry: {desktop}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/isaac_jsi_triga_validation.py b/scripts/isaac_jsi_triga_validation.py index 0cafbd6..371a78a 100755 --- a/scripts/isaac_jsi_triga_validation.py +++ b/scripts/isaac_jsi_triga_validation.py @@ -203,9 +203,11 @@ def _author_camera(stage, low: np.ndarray, high: np.ndarray) -> str: camera = UsdGeom.Camera.Define(stage, path) camera.CreateFocalLengthAttr(32.0) camera.CreateClippingRangeAttr((0.05, 10_000.0)) - matrix = Gf.Matrix4d().SetLookAt( - Gf.Vec3d(*eye), Gf.Vec3d(*target), Gf.Vec3d(0.0, 0.0, 1.0) - ).GetInverse() + matrix = ( + Gf.Matrix4d() + .SetLookAt(Gf.Vec3d(*eye), Gf.Vec3d(*target), Gf.Vec3d(0.0, 0.0, 1.0)) + .GetInverse() + ) UsdGeom.Xformable(camera.GetPrim()).AddTransformOp().Set(matrix) return path @@ -234,8 +236,10 @@ def main() -> None: gpu.free_vram_gb if gpu.free_vram_gb is not None else gpu.vram_gb, ) low_memory = available_vram_gb < 6.0 - forced_tier = RenderQualityTier.FALLBACK if available_vram_gb < 3.5 else ( - RenderQualityTier.WEAK if low_memory else None + forced_tier = ( + RenderQualityTier.FALLBACK + if available_vram_gb < 3.5 + else (RenderQualityTier.WEAK if low_memory else None) ) effective_capture = args.capture and not low_memory @@ -254,7 +258,6 @@ def main() -> None: import omni.usd from omni.kit.viewport.utility import capture_viewport_to_file, get_active_viewport from pxr import Gf, UsdGeom - from radcounter.isaac.rendering import NuclearDigitalTwinRuntime context = omni.usd.get_context() @@ -300,7 +303,6 @@ def main() -> None: timeline = omni.timeline.get_timeline_interface() started = time.monotonic() - previous = started frame_times: list[float] = [] route_distance = 0.0 route_index = 0 @@ -311,9 +313,7 @@ def main() -> None: len(route) - 2, int((now - started) / max(args.duration_s, 0.001) * (len(route) - 1)), ) - fraction = ( - (now - started) / max(args.duration_s, 0.001) * (len(route) - 1) - route_index - ) + fraction = (now - started) / max(args.duration_s, 0.001) * (len(route) - 1) - route_index position = route[route_index] * (1.0 - fraction) + route[route_index + 1] * fraction delta = route[route_index + 1] - route[route_index] robot_translate.Set(Gf.Vec3d(float(position[0]), float(position[1]), 0.02)) @@ -324,7 +324,6 @@ def main() -> None: runtime.observe_frame_time(elapsed_ms) route_distance += float(np.linalg.norm(position[:2] - previous_position[:2])) previous_position = position - previous = now timeline.pause() for _ in range(180 if effective_capture else 60): diff --git a/scripts/isaac_manchester_drum_store_validation.py b/scripts/isaac_manchester_drum_store_validation.py index f5f3dc7..4d829f6 100644 --- a/scripts/isaac_manchester_drum_store_validation.py +++ b/scripts/isaac_manchester_drum_store_validation.py @@ -7,15 +7,14 @@ import asyncio import json import math -from pathlib import Path import re import subprocess import sys import time +from pathlib import Path from isaacsim import SimulationApp - REPO_ROOT = Path(__file__).resolve().parents[1] sys.path.insert(0, str(REPO_ROOT)) @@ -47,9 +46,7 @@ def _set_pose(prim, pose, *, scale=None) -> None: xform = UsdGeom.Xformable(prim) xform.ClearXformOpOrder() xform.AddTranslateOp().Set(Gf.Vec3d(*pose.translation_m)) - xform.AddRotateXYZOp().Set( - Gf.Vec3f(*(math.degrees(value) for value in pose.rotation_rpy_rad)) - ) + xform.AddRotateXYZOp().Set(Gf.Vec3f(*(math.degrees(value) for value in pose.rotation_rpy_rad))) if scale is not None: xform.AddScaleOp().Set(Gf.Vec3f(*scale)) @@ -79,7 +76,9 @@ async def _convert_visuals(model, cache_root: Path) -> dict[Path, Path]: print(f"[convert {index}/{len(unique_meshes)}] {relative}", flush=True) task = converter.create_converter_task(str(source), str(destination), None, context) if not await task.wait_until_finished(): - error = task.get_error_message() if hasattr(task, "get_error_message") else "unknown error" + error = ( + task.get_error_message() if hasattr(task, "get_error_message") else "unknown error" + ) raise RuntimeError(f"asset conversion failed for {source}: {error}") converted[source] = destination return converted @@ -142,7 +141,7 @@ def main() -> None: output_dir.mkdir(parents=True, exist_ok=True) free_vram_before = _free_vram_gb() constrained = free_vram_before is not None and free_vram_before < 6.0 - width, height = ((960, 540) if constrained else (1600, 900)) + width, height = (960, 540) if constrained else (1600, 900) renderer = "MinimalRendering" if constrained else "RayTracedLighting" app = SimulationApp( @@ -162,12 +161,12 @@ def main() -> None: import omni.kit.app import omni.kit.viewport.utility import omni.usd - from pxr import Gf, Sdf, Usd, UsdGeom, UsdLux, UsdShade + from pxr import Gf, Usd, UsdGeom, UsdLux, UsdShade stage = omni.usd.get_context().get_stage() UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z) UsdGeom.SetStageMetersPerUnit(stage, 1.0) - world = UsdGeom.Xform.Define(stage, "/World") + UsdGeom.Xform.Define(stage, "/World") model_root = UsdGeom.Xform.Define(stage, "/World/Manchester500L") _set_pose(model_root.GetPrim(), model.pose) UsdGeom.Scope.Define(stage, "/World/Looks") diff --git a/scripts/isaac_nuclear_digital_twin.py b/scripts/isaac_nuclear_digital_twin.py index f5e431f..4b2fe4a 100644 --- a/scripts/isaac_nuclear_digital_twin.py +++ b/scripts/isaac_nuclear_digital_twin.py @@ -29,7 +29,6 @@ def main() -> None: app = SimulationApp({"headless": args.headless}) import omni.timeline import omni.usd - from radcounter.isaac.rendering import NuclearDigitalTwinRuntime context = omni.usd.get_context() diff --git a/scripts/isaac_shield_placement_video.py b/scripts/isaac_shield_placement_video.py new file mode 100644 index 0000000..a04f0a3 --- /dev/null +++ b/scripts/isaac_shield_placement_video.py @@ -0,0 +1,588 @@ +#!/usr/bin/env python3 +"""Render an articulated Ridgeback+Franka placing a shield before a surface source.""" +# ruff: noqa: E402 + +from __future__ import annotations + +import argparse +import importlib +import json +import math +import subprocess +import sys +import time +import traceback +from dataclasses import asdict, is_dataclass +from pathlib import Path +from typing import Any + +import numpy as np + +ROOT = Path(__file__).resolve().parents[1] +EXTENSION = ROOT / "source/extensions/radcounter.isaac" +for search_path in (ROOT, EXTENSION): + if str(search_path) not in sys.path: + sys.path.insert(0, str(search_path)) + +_radcounter_package = importlib.import_module("radcounter") +_extension_namespace = str(EXTENSION / "radcounter") +if _extension_namespace not in _radcounter_package.__path__: + _radcounter_package.__path__.append(_extension_namespace) + + +def arguments() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--headless", action="store_true") + parser.add_argument("--duration", type=float, default=20.0) + parser.add_argument("--capture-stride", type=int, default=3) + parser.add_argument( + "--output", + type=Path, + default=ROOT / "artifacts/shield-placement-video", + ) + args = parser.parse_args() + if args.duration <= 0.0: + parser.error("--duration must be positive") + if args.capture_stride <= 0: + parser.error("--capture-stride must be positive") + return args + + +ARGS = arguments() + +from isaacsim import SimulationApp + +APP = SimulationApp( + { + "headless": ARGS.headless, + "width": 1280, + "height": 720, + "renderer": "RaytracedLighting", + "window_title": "RadCounterSim - Physical Shield Placement", + } +) + + +def jsonable(value: Any) -> Any: + if value is None or isinstance(value, (str, int, float, bool)): + return value + if is_dataclass(value): + return jsonable(asdict(value)) + if isinstance(value, np.ndarray): + return value.tolist() + if isinstance(value, np.generic): + return value.item() + if isinstance(value, dict): + return {str(key): jsonable(item) for key, item in value.items()} + if isinstance(value, (tuple, list, set)): + return [jsonable(item) for item in value] + return str(value) + + +def world_position(stage: Any, path: str) -> np.ndarray: + from pxr import Gf, UsdGeom + + transform = UsdGeom.XformCache().GetLocalToWorldTransform(stage.GetPrimAtPath(path)) + return np.asarray(transform.Transform(Gf.Vec3d()), dtype=np.float64) + + +def set_camera(stage: Any) -> None: + from omni.kit.viewport.utility import get_active_viewport + from pxr import Gf, UsdGeom, UsdLux + + camera_path = "/World/ShieldVideoCamera" + camera = UsdGeom.Camera.Define(stage, camera_path) + camera.CreateFocalLengthAttr(23.0) + view = Gf.Matrix4d().SetLookAt( + Gf.Vec3d(-2.75, -3.45, 2.55), + Gf.Vec3d(1.15, -0.05, 0.62), + Gf.Vec3d(0.0, 0.0, 1.0), + ) + camera_xform = UsdGeom.Xformable(camera) + camera_xform.ClearXformOpOrder() + camera_xform.MakeMatrixXform().Set(view.GetInverse()) + dome = UsdLux.DomeLight.Define(stage, "/World/ShieldVideoLight") + dome.CreateIntensityAttr(920.0) + dome.CreateColorAttr(Gf.Vec3f(0.70, 0.78, 0.88)) + viewport = get_active_viewport() + if viewport is None: + raise RuntimeError("active viewport is unavailable") + viewport.set_active_camera(camera_path) + + +def author_irregular_drum_surface_source(stage: Any) -> dict[str, Any]: + from pxr import Gf, Sdf, UsdGeom + + drum_path = "/World/HiddenContaminatedDrum" + drum = stage.GetPrimAtPath(drum_path) + drum.GetAttribute("rad:role").Set("contaminated_object") + drum.GetAttribute("rad:source:enabled").Set(False) + source_path = drum_path + "/AdheredSurfaceContamination" + angular_cells = 44 + vertical_cells = 22 + theta_values = np.linspace(math.pi - 1.18, math.pi + 1.18, angular_cells) + z_values = np.linspace(-0.42, 0.42, vertical_cells) + theta, zz = np.meshgrid(theta_values, z_values, indexing="xy") + lobe_a = np.exp(-(((theta - math.pi + 0.24) / 0.48) ** 2 + ((zz + 0.05) / 0.27) ** 2)) + lobe_b = 0.78 * np.exp( + -(((theta - math.pi - 0.48) / 0.28) ** 2 + ((zz - 0.16) / 0.18) ** 2) + ) + lobe_c = 0.58 * np.exp( + -(((theta - math.pi + 0.73) / 0.20) ** 2 + ((zz - 0.25) / 0.13) ** 2) + ) + roughness = 0.17 * np.sin(11.0 * theta + 17.0 * zz) + 0.11 * np.cos( + 23.0 * theta - 8.0 * zz + ) + active = lobe_a + lobe_b + lobe_c + roughness > 0.43 + active &= ~( + ((theta - math.pi + 0.08) / 0.14) ** 2 + ((zz - 0.02) / 0.11) ** 2 < 1.0 + ) + active |= ( + ((theta - math.pi - 0.93) / 0.08) ** 2 + ((zz + 0.32) / 0.07) ** 2 < 1.0 + ) + + radius_m = 0.326 + delta_theta = float(theta_values[1] - theta_values[0]) + delta_z = float(z_values[1] - z_values[0]) + points: list[Gf.Vec3f] = [] + counts: list[int] = [] + indices: list[int] = [] + weights: list[float] = [] + colors: list[Gf.Vec3f] = [] + field = np.maximum(0.08, lobe_a + lobe_b + lobe_c + 0.45 * roughness) + maximum = max(float(field[active].max()), 1e-12) + for row, column in np.argwhere(active): + center_theta = float(theta[row, column]) + center_z = float(zz[row, column]) + base_index = len(points) + for angle, z_m in ( + (center_theta - 0.52 * delta_theta, center_z - 0.52 * delta_z), + (center_theta + 0.52 * delta_theta, center_z - 0.52 * delta_z), + (center_theta + 0.52 * delta_theta, center_z + 0.52 * delta_z), + (center_theta - 0.52 * delta_theta, center_z + 0.52 * delta_z), + ): + points.append(Gf.Vec3f(radius_m * math.cos(angle), radius_m * math.sin(angle), z_m)) + counts.append(4) + indices.extend((base_index, base_index + 1, base_index + 2, base_index + 3)) + weight = float(field[row, column]) + weights.append(weight) + fraction = weight / maximum + colors.append(Gf.Vec3f(0.34 + 0.48 * fraction, 0.045, 0.018)) + + total_activity_bq = 8.5e8 + normalized = np.asarray(weights, dtype=np.float64) + activities = total_activity_bq * normalized / normalized.sum() + mesh = UsdGeom.Mesh.Define(stage, source_path) + mesh.CreatePointsAttr(points) + mesh.CreateFaceVertexCountsAttr(counts) + mesh.CreateFaceVertexIndicesAttr(indices) + mesh.CreateSubdivisionSchemeAttr("none") + mesh.CreateDisplayColorPrimvar(UsdGeom.Tokens.uniform).Set(colors) + prim = mesh.GetPrim() + for name, value_type, value in ( + ("rad:role", Sdf.ValueTypeNames.String, "source"), + ("rad:source:type", Sdf.ValueTypeNames.String, "surface"), + ("rad:source:isotopeId", Sdf.ValueTypeNames.String, "Cs-137"), + ("rad:source:activityBq", Sdf.ValueTypeNames.Double, total_activity_bq), + ("rad:source:faceActivityBq", Sdf.ValueTypeNames.DoubleArray, activities.tolist()), + ("rad:source:irregularMask", Sdf.ValueTypeNames.Bool, True), + ("rad:source:movableWithPrim", Sdf.ValueTypeNames.Bool, True), + ("rad:source:enabled", Sdf.ValueTypeNames.Bool, True), + ): + prim.CreateAttribute(name, value_type, custom=True).Set(value) + return { + "path": source_path, + "active_faces": int(np.count_nonzero(active)), + "candidate_faces": int(active.size), + "total_activity_bq": total_activity_bq, + } + + +def simplify_scene(stage: Any) -> None: + from pxr import UsdGeom + + for path in ( + "/World/MeasurementRobot", + "/World/DeconWorkbench", + "/World/DeconWorkSurface", + "/World/ContaminatedFloor", + "/World/MovableObstacle", + "/World/DisposalZone", + ): + prim = stage.GetPrimAtPath(path) + if prim.IsValid() and prim.IsA(UsdGeom.Imageable): + UsdGeom.Imageable(prim).MakeInvisible() + # The generic task author adds stand-off grasp markers to movable props. + # They are useful for controller audits but looked like unsupported, + # floating hardware in the research render. The contaminated drum is not + # manipulated in this scene, so remove its marker and keep only the + # physical shield handle that Franka actually grasps. + drum_handle = "/World/HiddenContaminatedDrum/ManipulatorHandle" + if stage.GetPrimAtPath(drum_handle).IsValid(): + stage.RemovePrim(drum_handle) + # This scene is exclusively a shielding task. Hide the cyan + # decontamination pad before the first recorded frame instead of waiting + # until the pick-and-place controller starts. + decon_tool = "/World/CountermeasureRobot/panda_hand/RadCounterDeconTool" + if stage.GetPrimAtPath(decon_tool).IsValid(): + UsdGeom.Imageable(stage.GetPrimAtPath(decon_tool)).MakeInvisible() + + +def ground_fix_drum(stage: Any) -> dict[str, Any]: + """Turn the source drum into a floor-supported static PhysX collider.""" + + from pxr import Gf, Sdf, UsdGeom, UsdPhysics + + drum_path = "/World/HiddenContaminatedDrum" + drum = stage.GetPrimAtPath(drum_path) + if not drum.IsValid(): + raise RuntimeError("contaminated drum is missing") + xform = UsdGeom.Xformable(drum) + translate_ops = [ + operation + for operation in xform.GetOrderedXformOps() + if operation.GetOpType() == UsdGeom.XformOp.TypeTranslate + ] + if len(translate_ops) != 1: + raise RuntimeError("contaminated drum must have one translate operation") + position = translate_ops[0].Get() + # The drum is 1.0 m tall. A centre height of 0.5 m puts its collision + # cylinder directly on the z=0 floor instead of leaving a 5 cm gap. + translate_ops[0].Set(Gf.Vec3d(float(position[0]), float(position[1]), 0.5)) + UsdPhysics.RigidBodyAPI.Apply(drum).CreateRigidBodyEnabledAttr(False) + for name in ("rad:manipulation:movable", "rad:manipulation:removable"): + attribute = drum.GetAttribute(name) + if attribute: + attribute.Set(False) + drum.CreateAttribute( + "rad:manipulation:groundFixed", + Sdf.ValueTypeNames.Bool, + custom=True, + ).Set(True) + return { + "path": drum_path, + "physics_mode": "static collider", + "bottom_height_m": 0.0, + "ground_fixed": True, + } + + +class FilteredContactAudit: + """Record pair-wise PhysX forces for floor and drum collision gates.""" + + def __init__(self, contact_view: Any, *, physics_dt_s: float) -> None: + self.contact_view = contact_view + self.physics_dt_s = physics_dt_s + self.samples = 0 + self.peak_floor_force_n = 0.0 + self.peak_drum_force_n = 0.0 + self.drum_contact_samples = 0 + + @staticmethod + def _numpy(value: Any) -> np.ndarray: + if hasattr(value, "detach"): + value = value.detach() + if hasattr(value, "cpu"): + value = value.cpu() + if hasattr(value, "numpy"): + value = value.numpy() + return np.asarray(value, dtype=np.float64) + + def sample(self) -> None: + matrix = self._numpy( + self.contact_view.get_contact_force_matrix(dt=self.physics_dt_s) + ) + if matrix.ndim != 3 or matrix.shape[-2:] != (2, 3): + raise RuntimeError( + f"expected shield contact matrix (*, 2, 3), got {matrix.shape}" + ) + magnitudes = np.linalg.norm(matrix, axis=2) + floor_force = float(np.max(magnitudes[:, 0], initial=0.0)) + drum_force = float(np.max(magnitudes[:, 1], initial=0.0)) + self.samples += 1 + self.peak_floor_force_n = max(self.peak_floor_force_n, floor_force) + self.peak_drum_force_n = max(self.peak_drum_force_n, drum_force) + if drum_force > 0.5: + self.drum_contact_samples += 1 + + def summary(self) -> dict[str, Any]: + return { + "samples": self.samples, + "peak_floor_force_n": self.peak_floor_force_n, + "peak_drum_force_n": self.peak_drum_force_n, + "drum_contact_samples": self.drum_contact_samples, + "collision_free": self.drum_contact_samples == 0, + } + + +class FrameRecorder: + def __init__(self, output: Path, *, stride: int) -> None: + from omni.kit.viewport.utility import get_active_viewport + + self.frame_dir = output / ".frames" + self.frame_dir.mkdir(parents=True, exist_ok=True) + for frame in self.frame_dir.glob("frame_*.png"): + frame.unlink() + self.viewport = get_active_viewport() + if self.viewport is None: + raise RuntimeError("active viewport is unavailable") + self.stride = stride + self.physics_steps = 0 + self.frame_count = 0 + + def observe(self) -> None: + from omni.kit.viewport.utility import capture_viewport_to_file + + self.physics_steps += 1 + if self.physics_steps % self.stride: + return + path = self.frame_dir / f"frame_{self.frame_count:04d}.png" + capture_viewport_to_file(self.viewport, file_path=str(path), is_hdr=False) + self.frame_count += 1 + + def wait_for_frames(self, world: Any) -> None: + deadline = time.monotonic() + 8.0 + found = len(list(self.frame_dir.glob("frame_*.png"))) + while found < self.frame_count and time.monotonic() < deadline: + world.step(render=True) + time.sleep(0.02) + found = len(list(self.frame_dir.glob("frame_*.png"))) + if found != self.frame_count: + raise RuntimeError(f"expected {self.frame_count} frames, found {found}") + + def encode(self, output: Path, duration_s: float) -> Path: + video = output / "manipulator_surface_source_shield_placement_20s.mp4" + video.unlink(missing_ok=True) + source_fps = self.frame_count / duration_s + subprocess.run( + [ + "ffmpeg", + "-y", + "-loglevel", + "error", + "-framerate", + f"{source_fps:.9f}", + "-i", + str(self.frame_dir / "frame_%04d.png"), + "-t", + f"{duration_s:.6f}", + "-vf", + "format=yuv420p", + "-r", + "30", + "-c:v", + "libx264", + "-preset", + "slow", + "-crf", + "18", + "-movflags", + "+faststart", + str(video), + ], + check=True, + ) + for frame in self.frame_dir.glob("frame_*.png"): + frame.unlink() + self.frame_dir.rmdir() + return video + + +class RecordingStepper: + def __init__( + self, + world: Any, + recorder: FrameRecorder, + telemetry: Any, + contact_audit: FilteredContactAudit, + ) -> None: + self.world = world + self.recorder = recorder + self.telemetry = telemetry + self.contact_audit = contact_audit + + def step(self, *, render: bool = False) -> None: + del render + self.world.step(render=True) + self.telemetry.sample() + self.contact_audit.sample() + self.recorder.observe() + + +def run() -> dict[str, Any]: + import omni.usd + from isaacsim.core.api import World + from isaacsim.core.prims import RigidPrim, SingleArticulation + from radcounter.isaac.robot import ( + PhysxManipulationTelemetry, + RealRobotAssetConfig, + RidgebackFrankaController, + add_real_robot_references, + author_real_robot_task_scene, + create_decontamination_activity_map, + enable_real_robot_extensions, + ) + + ARGS.output.mkdir(parents=True, exist_ok=True) + enable_real_robot_extensions() + for _ in range(20): + APP.update() + context = omni.usd.get_context() + scene_path = ROOT / "assets/environments/radcounter_vertical_slice.usda" + if not context.open_stage(str(scene_path)): + raise RuntimeError(f"failed to open {scene_path}") + for _ in range(25): + APP.update() + stage = context.get_stage() + config = RealRobotAssetConfig() + asset_manifest = add_real_robot_references(stage, config=config) + for _ in range(100): + APP.update() + activity_map = create_decontamination_activity_map(ARGS.output / "workbench_activity.npz") + author_real_robot_task_scene(stage, activity_map, config=config) + surface_source = author_irregular_drum_surface_source(stage) + simplify_scene(stage) + drum_fixing = ground_fix_drum(stage) + set_camera(stage) + + world = World( + stage_units_in_meters=1.0, + physics_dt=1.0 / 60.0, + rendering_dt=1.0 / 60.0, + ) + articulation = world.scene.add( + SingleArticulation(config.countermeasure_root, name="shield_video_ridgeback_franka") + ) + shield_contact_view = world.scene.add( + RigidPrim( + prim_paths_expr=config.shield_path, + name="shield_floor_contact_view", + track_contact_forces=True, + contact_filter_prim_paths_expr=[ + "/World/Environment/FloorCollision", + "/World/HiddenContaminatedDrum/Drum", + ], + max_contact_count=64, + ) + ) + world.reset() + for _ in range(90): + world.step(render=True) + + recorder = FrameRecorder(ARGS.output, stride=ARGS.capture_stride) + telemetry = PhysxManipulationTelemetry( + articulation, + joint_names=RidgebackFrankaController.arm_joint_names, + rigid_contact_view=shield_contact_view, + physics_dt_s=1.0 / 60.0, + contact_threshold_n=0.5, + ) + contact_audit = FilteredContactAudit(shield_contact_view, physics_dt_s=1.0 / 60.0) + stepper = RecordingStepper(world, recorder, telemetry, contact_audit) + controller = RidgebackFrankaController( + stage, + stepper, + config=config, + articulation=articulation, + ) + for _ in range(60): + stepper.step(render=True) + stow_report = controller.stow_arm() + if not stow_report.success: + raise RuntimeError(f"Franka could not reach its transport posture: {stow_report}") + if not controller.move_base((-0.08, 0.0, 0.0)): + raise RuntimeError("Ridgeback could not establish the shield work-zone approach") + + source_position = world_position(stage, "/World/HiddenContaminatedDrum") + # Keep the 50 mm plate near the source while leaving collision margin for + # the wider shield base. At this offset the plate face is about 0.30 m + # from the contaminated surface and the base is about 0.19 m from the + # cylindrical drum collider. + destination = np.asarray( + (float(source_position[0] - 0.65), float(source_position[1]), 0.0), + dtype=np.float64, + ) + source_surface_x_m = float(source_position[0] - 0.326) + shield_source_face_x_m = float(destination[0] + 0.025) + source_shield_clearance_m = source_surface_x_m - shield_source_face_x_m + placement_route = ( + (-0.05, -1.25, 0.0), + (float(destination[0]), -1.25, 0.0), + (float(destination[0]), float(destination[1] - 0.95), math.pi / 2.0), + ) + report = controller.execute_shield_pick_and_place( + pickup_base_xy_yaw=(-0.05, -0.62, 0.0), + placement_base_route_xy_yaw=placement_route, + destination_root_position_m=destination, + # Retract the plate over the Ridgeback before driving or turning; the + # lift pose leaves the payload extended in front of the mobile base. + transport_hand_offset_from_grasp_m=(-0.25, 0.0, 0.42), + ) + for _ in range(120): + stepper.step(render=True) + if not report.success: + raise RuntimeError(f"physical shield placement failed: {report}") + contact_summary = contact_audit.summary() + if not contact_summary["collision_free"]: + raise RuntimeError(f"shield contacted the fixed drum: {contact_summary}") + telemetry_summary = telemetry.summary() + if telemetry_summary.samples <= 0: + raise RuntimeError("PhysX telemetry did not collect any physics samples") + if telemetry_summary.overall_peak_abs_joint_effort_nm <= 0.01: + raise RuntimeError("PhysX did not report a nonzero Franka joint effort") + if telemetry_summary.overall_peak_joint_reaction_force_n <= 0.1: + raise RuntimeError("PhysX did not report a nonzero Franka joint reaction force") + if telemetry_summary.peak_rigid_contact_force_n <= 0.5: + raise RuntimeError("PhysX did not report shield-to-floor contact reaction") + recorder.wait_for_frames(world) + video = recorder.encode(ARGS.output, ARGS.duration) + final_shield = world_position(stage, config.shield_path) + result = { + "passed": True, + "video": str(video), + "duration_s": ARGS.duration, + "resolution": [1280, 720], + "encoded_fps": 30, + "captured_frames": recorder.frame_count, + "physics_steps": recorder.physics_steps, + "robot": "Clearpath Ridgeback + Franka Emika Panda", + "robot_asset": asset_manifest["countermeasure_usd"], + "motion": "PhysX articulation + Lula IK + finger joints + fixed grasp constraint", + "shield_mass_kg": 2.2, + "surface_source": surface_source, + "drum_fixing": drum_fixing, + "initial_stow": stow_report, + "shield_destination_m": destination, + "shield_final_position_m": final_shield, + "source_shield_clearance_m": source_shield_clearance_m, + "shield_motion": report, + "shield_contact_audit": contact_summary, + "physx_telemetry": telemetry_summary, + } + world.stop() + return result + + +def main() -> int: + result_path = ARGS.output / "result.json" + result: dict[str, Any] + try: + result = run() + print("SHIELD_PLACEMENT_VIDEO_RESULT " + json.dumps(jsonable(result)), flush=True) + return 0 + except Exception as error: + result = { + "passed": False, + "error": f"{type(error).__name__}: {error}", + "traceback": traceback.format_exc(), + } + print("SHIELD_PLACEMENT_VIDEO_ERROR " + json.dumps(result), flush=True) + return 1 + finally: + ARGS.output.mkdir(parents=True, exist_ok=True) + result_path.write_text(json.dumps(jsonable(result), indent=2) + "\n", encoding="utf-8") + APP.close() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/isaac_surface_decon_validation.py b/scripts/isaac_surface_decon_validation.py index e422741..f1e6e15 100644 --- a/scripts/isaac_surface_decon_validation.py +++ b/scripts/isaac_surface_decon_validation.py @@ -7,6 +7,7 @@ import argparse import json import math +import subprocess import sys import time import traceback @@ -40,11 +41,40 @@ def parse_arguments() -> argparse.Namespace: type=Path, default=ROOT / ".cache/arounder-water-decon", ) + parser.add_argument( + "--render-only", + action="store_true", + help="render the decommissioning overview and process-head detail without running a sweep", + ) + parser.add_argument( + "--render-video", + action="store_true", + help="render a high-reach wall-decontamination frame sequence and encode an MP4", + ) + parser.add_argument("--video-seconds", type=float, default=20.0) + parser.add_argument("--video-fps", type=int, default=15) + parser.add_argument( + "--render-scene", + choices=("decontamination", "shield-manipulation"), + default="decontamination", + help="select the isolated research scene produced by --render-only", + ) + parser.add_argument( + "--headless", + action="store_true", + help="render off-screen without creating an X11 application window", + ) args = parser.parse_args() if args.duration <= 0.0: parser.error("--duration must be positive") if args.time_scale <= 0.0: parser.error("--time-scale must be positive") + if args.video_seconds <= 0.0: + parser.error("--video-seconds must be positive") + if args.video_fps <= 0: + parser.error("--video-fps must be positive") + if args.render_only and args.render_video: + parser.error("--render-only and --render-video are mutually exclusive") return args @@ -54,7 +84,7 @@ def parse_arguments() -> argparse.Namespace: simulation_app = SimulationApp( { - "headless": False, + "headless": ARGS.headless, "width": 1440, "height": 900, "renderer": "RaytracedLighting", @@ -72,7 +102,11 @@ def parse_arguments() -> argparse.Namespace: from radcounter.isaac.robot.input_router import IsaacRobotInputRouter from radcounter.core.robots.control import JointCommand, TwistCommand -from radcounter.core.surface_decontamination import DecontaminationTool, SurfaceSourceGrid +from radcounter.core.surface_decontamination import ( + DecontaminationTool, + SurfaceSourceGrid, + irregular_deposition_field, +) from radcounter.core.water_decontamination import ( WaterDecontaminationState, WaterJetSpec, @@ -82,6 +116,8 @@ def parse_arguments() -> argparse.Namespace: ROBOT_ID = "arounder_research_replica" ROBOT_PATH = "/World/Arounder" SOURCE_PATH = "/World/ReactorBuilding/ContaminatedFloor" +HIGH_WALL_SOURCE_PATH = "/World/ReactorBuilding/ContaminatedHighWall" +HIGH_REACH_ROBOT_PATH = "/World/HighReach10" DT_S = 1.0 / 60.0 HEAD_X_M = 0.95 HEAD_NOZZLE_Z_M = -0.17 @@ -93,6 +129,22 @@ def parse_arguments() -> argparse.Namespace: HITACHI_REFERENCE = "https://www.hitachi-hgne.co.jp/news/2013/20130308.html" IRID_REFERENCE = "https://irid.or.jp/_pdf/20150714_5.pdf" +MHI_SUPER_GIRAFFE_REFERENCE = ( + "https://www.mhi.com/jp/business/products-services/energy-environment/" + "nuclear-power-generation/applied-products/robot-mechatronics/super-giraffe" +) +IRID_HIGH_PLACE_REFERENCE = ( + "https://irid.or.jp/topics/" + "%E9%AB%98%E6%89%80%E7%94%A8%E3%83%89%E3%83%A9%E3%82%A4%E3%82%A2%E3%82%A4%E3%82%B9" + "%E3%83%96%E3%83%A9%E3%82%B9%E3%83%88%E9%99%A4%E6%9F%93%E8%A3%85%E7%BD%AE%E3%81%AE" + "%E9%96%8B%E7%99%BA%E3%83%BB%E6%B4%BB/" +) +RIDGEBACK_FRANKA_REFERENCE = ( + "https://docs.isaacsim.omniverse.nvidia.com/6.0.0/assets/usd_assets_robots.html" +) +RIDGEBACK_FRANKA_ASSET = "/Isaac/Robots/Clearpath/RidgebackFranka/ridgeback_franka.usd" +MANIPULATOR_PATH = "/World/CountermeasureRobot" +SHIELD_PATH = "/World/ShieldManipulation/ShieldCassette" @dataclass(frozen=True) @@ -138,18 +190,35 @@ def create_materials(stage) -> dict[str, object]: "yellow": define_material(stage, "IndustrialYellow", (0.91, 0.56, 0.035), roughness=0.34), "yellow_dark": define_material(stage, "IndustrialYellowDark", (0.54, 0.30, 0.02)), "track": define_material(stage, "CrawlerRubber", (0.018, 0.022, 0.024), roughness=0.92), - "steel": define_material(stage, "StainlessSteel", (0.52, 0.56, 0.57), metallic=0.82, roughness=0.24), + "steel": define_material( + stage, "StainlessSteel", (0.52, 0.56, 0.57), metallic=0.82, roughness=0.24 + ), "dark_steel": define_material(stage, "DarkSteel", (0.09, 0.11, 0.12), metallic=0.72), "pipe": define_material(stage, "PipeSteel", (0.31, 0.35, 0.36), metallic=0.65), "blue": define_material(stage, "WaterLineBlue", (0.03, 0.24, 0.62), metallic=0.18), "red": define_material(stage, "ProcessRed", (0.60, 0.055, 0.035), metallic=0.12), "green": define_material(stage, "RecoveryGreen", (0.04, 0.38, 0.17), metallic=0.10), - "glass": define_material(stage, "LensGlass", (0.025, 0.14, 0.19), metallic=0.15, roughness=0.08), + "glass": define_material( + stage, "LensGlass", (0.025, 0.14, 0.19), metallic=0.15, roughness=0.08 + ), "brush": define_material(stage, "ContainmentBrush", (0.018, 0.018, 0.014), roughness=0.98), - "water": define_material(stage, "WaterJet", (0.05, 0.40, 0.95), roughness=0.08, opacity=0.42), - "wet": define_material(stage, "WetFloor", (0.025, 0.12, 0.20), roughness=0.10, opacity=0.58), + "water": define_material( + stage, "WaterJet", (0.05, 0.40, 0.95), roughness=0.08, opacity=0.42 + ), + "wet": define_material( + stage, "WetFloor", (0.025, 0.12, 0.20), roughness=0.10, opacity=0.58 + ), "white": define_material(stage, "PaintedWhite", (0.72, 0.74, 0.70), roughness=0.54), "orange": define_material(stage, "SafetyOrange", (0.94, 0.24, 0.025), roughness=0.45), + "lead": define_material( + stage, "ShieldLeadCore", (0.13, 0.15, 0.16), metallic=0.78, roughness=0.42 + ), + "shield_skin": define_material( + stage, "ShieldStainlessJacket", (0.28, 0.31, 0.32), metallic=0.86, roughness=0.25 + ), + "corrosion": define_material( + stage, "ContaminatedCorrosion", (0.38, 0.12, 0.035), metallic=0.20, roughness=0.88 + ), } @@ -224,6 +293,26 @@ def add_link(stage, path: str, start_xz, end_xz, y_m: float, width_m: float, mat ) +def add_link_3d(stage, path: str, start_xyz, end_xyz, width_m: float, material): + start = np.asarray(start_xyz, dtype=np.float64) + end = np.asarray(end_xyz, dtype=np.float64) + delta = end - start + length = float(np.linalg.norm(delta)) + if length <= 1e-9: + raise ValueError("3-D link endpoints must be distinct") + horizontal = math.hypot(float(delta[0]), float(delta[1])) + yaw_z = math.degrees(math.atan2(float(delta[1]), float(delta[0]))) + pitch_y = -math.degrees(math.atan2(float(delta[2]), horizontal)) + return add_cube( + stage, + path, + (length, width_m, width_m), + tuple(float(value) for value in 0.5 * (start + end)), + material, + rotation_xyz=(0.0, pitch_y, yaw_z), + ) + + def add_curve(stage, path: str, points, width_m: float, color) -> object: curve = UsdGeom.BasisCurves.Define(stage, path) curve.CreateTypeAttr(UsdGeom.Tokens.linear) @@ -235,8 +324,12 @@ def add_curve(stage, path: str, points, width_m: float, color) -> object: return points_attr -def create_environment(stage, materials) -> tuple[tuple[float, float, float], ...]: +def create_environment( + stage, materials, *, high_bay: bool = False +) -> tuple[tuple[float, float, float], ...]: UsdGeom.Xform.Define(stage, "/World/ReactorBuilding") + wall_height_m = 13.0 if high_bay else 5.0 + column_height_m = 12.6 if high_bay else 4.8 add_cube( stage, "/World/ReactorBuilding/Floor", @@ -245,69 +338,290 @@ def create_environment(stage, materials) -> tuple[tuple[float, float, float], .. materials["epoxy"], collision=True, ) - add_cube(stage, "/World/ReactorBuilding/WallNorth", (16.0, 0.26, 5.0), (0.0, 5.86, 2.5), materials["concrete"], collision=True) - add_cube(stage, "/World/ReactorBuilding/WallWest", (0.26, 12.0, 5.0), (-7.86, 0.0, 2.5), materials["concrete"], collision=True) - add_cube(stage, "/World/ReactorBuilding/WallEast", (0.26, 8.0, 5.0), (7.86, 2.0, 2.5), materials["concrete"], collision=True) + add_cube( + stage, + "/World/ReactorBuilding/WallNorth", + (16.0, 0.26, wall_height_m), + (0.0, 5.86, wall_height_m * 0.5), + materials["concrete"], + collision=True, + ) + add_cube( + stage, + "/World/ReactorBuilding/WallWest", + (0.26, 12.0, wall_height_m), + (-7.86, 0.0, wall_height_m * 0.5), + materials["concrete"], + collision=True, + ) + add_cube( + stage, + "/World/ReactorBuilding/WallEast", + (0.26, 8.0, wall_height_m), + (7.86, 2.0, wall_height_m * 0.5), + materials["concrete"], + collision=True, + ) - for index, (x_m, y_m) in enumerate(((-5.4, 4.2), (0.0, 4.2), (5.4, 4.2), (-5.4, -4.1), (5.4, -4.1))): - add_cube(stage, f"/World/ReactorBuilding/Columns/C{index}", (0.52, 0.52, 4.8), (x_m, y_m, 2.4), materials["concrete"], collision=True) - add_cube(stage, f"/World/ReactorBuilding/Columns/C{index}_Foot", (0.82, 0.82, 0.16), (x_m, y_m, 0.08), materials["concrete"], collision=True) + column_positions = ( + ((-6.4, 4.2), (6.4, 4.2), (-6.4, -4.1), (6.4, -4.1)) + if high_bay + else ((-5.4, 4.2), (0.0, 4.2), (5.4, 4.2), (-5.4, -4.1), (5.4, -4.1)) + ) + for index, (x_m, y_m) in enumerate(column_positions): + add_cube( + stage, + f"/World/ReactorBuilding/Columns/C{index}", + (0.52, 0.52, column_height_m), + (x_m, y_m, column_height_m * 0.5), + materials["concrete"], + collision=True, + ) + add_cube( + stage, + f"/World/ReactorBuilding/Columns/C{index}_Foot", + (0.82, 0.82, 0.16), + (x_m, y_m, 0.08), + materials["concrete"], + collision=True, + ) - add_cylinder(stage, "/World/ReactorBuilding/BiologicalShield", 1.62, 4.3, (4.8, 2.25, 2.15), materials["concrete"], collision=True) - add_cylinder(stage, "/World/ReactorBuilding/BiologicalShieldRing", 1.82, 0.26, (4.8, 2.25, 0.18), materials["dark_steel"], collision=True) + add_cylinder( + stage, + "/World/ReactorBuilding/BiologicalShield", + 1.62, + 8.8 if high_bay else 4.3, + (4.8, 2.25, 4.4 if high_bay else 2.15), + materials["concrete"], + collision=True, + ) + add_cylinder( + stage, + "/World/ReactorBuilding/BiologicalShieldRing", + 1.82, + 0.26, + (4.8, 2.25, 0.18), + materials["dark_steel"], + collision=True, + ) pipe_specs = ( ("Condensate", 0.16, 1.25, materials["blue"]), ("FireMain", 0.13, 1.85, materials["red"]), ("Vent", 0.22, 2.55, materials["pipe"]), ) + if high_bay: + pipe_specs += ( + ("HighSteam", 0.25, 7.15, materials["pipe"]), + ("HighService", 0.18, 11.15, materials["blue"]), + ) for name, radius, z_m, material in pipe_specs: - add_cylinder(stage, f"/World/ReactorBuilding/PipeRack/{name}", radius, 13.8, (0.0, 5.25, z_m), material, axis="X") + add_cylinder( + stage, + f"/World/ReactorBuilding/PipeRack/{name}", + radius, + 13.8, + (0.0, 5.25, z_m), + material, + axis="X", + ) + rack_support_height_m = 11.8 if high_bay else 3.1 for index, x_m in enumerate((-6.2, -3.0, 0.2, 3.4, 6.3)): - add_cube(stage, f"/World/ReactorBuilding/PipeRack/Support{index}", (0.12, 0.55, 3.1), (x_m, 5.22, 1.55), materials["dark_steel"], collision=True) + add_cube( + stage, + f"/World/ReactorBuilding/PipeRack/Support{index}", + (0.12, 0.55, rack_support_height_m), + (x_m, 5.22, rack_support_height_m * 0.5), + materials["dark_steel"], + collision=True, + ) for index, (x_m, radius, height) in enumerate(((5.8, 0.48, 1.8), (6.65, 0.36, 1.45))): - add_cylinder(stage, f"/World/ReactorBuilding/ProcessSkid/Tank{index}", radius, height, (x_m, -3.7, height * 0.5 + 0.15), materials["white"], collision=True) - add_cylinder(stage, f"/World/ReactorBuilding/ProcessSkid/Tank{index}Cap", radius * 0.82, 0.08, (x_m, -3.7, height + 0.18), materials["dark_steel"]) - add_cube(stage, "/World/ReactorBuilding/ProcessSkid/Base", (2.25, 1.55, 0.18), (5.9, -3.7, 0.09), materials["dark_steel"], collision=True) + add_cylinder( + stage, + f"/World/ReactorBuilding/ProcessSkid/Tank{index}", + radius, + height, + (x_m, -3.7, height * 0.5 + 0.15), + materials["white"], + collision=True, + ) + add_cylinder( + stage, + f"/World/ReactorBuilding/ProcessSkid/Tank{index}Cap", + radius * 0.82, + 0.08, + (x_m, -3.7, height + 0.18), + materials["dark_steel"], + ) + add_cube( + stage, + "/World/ReactorBuilding/ProcessSkid/Base", + (2.25, 1.55, 0.18), + (5.9, -3.7, 0.09), + materials["dark_steel"], + collision=True, + ) for step in range(6): - add_cube(stage, f"/World/ReactorBuilding/Stairs/Step{step}", (0.75, 0.32, 0.10), (6.7, -1.65 + step * 0.28, 0.05 + step * 0.11), materials["steel"], collision=True) + add_cube( + stage, + f"/World/ReactorBuilding/Stairs/Step{step}", + (0.75, 0.32, 0.10), + (6.7, -1.65 + step * 0.28, 0.05 + step * 0.11), + materials["steel"], + collision=True, + ) drain_y = -1.42 - add_cube(stage, "/World/ReactorBuilding/Drain/Channel", (4.2, 0.30, 0.025), (0.35, drain_y, 0.012), materials["dark_steel"]) + add_cube( + stage, + "/World/ReactorBuilding/Drain/Channel", + (4.2, 0.30, 0.025), + (0.35, drain_y, 0.012), + materials["dark_steel"], + ) for index, x_m in enumerate(np.linspace(-1.65, 2.35, 24)): - add_cube(stage, f"/World/ReactorBuilding/Drain/Bar{index:02d}", (0.035, 0.29, 0.035), (float(x_m), drain_y, 0.028), materials["steel"]) - - add_cube(stage, "/World/ReactorBuilding/UtilitySkid/Base", (2.8, 1.75, 0.16), (-5.55, -3.65, 0.08), materials["dark_steel"], collision=True) - add_cylinder(stage, "/World/ReactorBuilding/UtilitySkid/RecoveryTank", 0.53, 1.55, (-6.15, -3.65, 0.92), materials["white"], collision=True) - add_cylinder(stage, "/World/ReactorBuilding/UtilitySkid/Pump", 0.25, 0.64, (-4.65, -3.80, 0.42), materials["blue"], axis="X") - add_cube(stage, "/World/ReactorBuilding/UtilitySkid/ControlCabinet", (0.58, 0.42, 1.05), (-4.75, -3.10, 0.61), materials["yellow_dark"], collision=True) - add_cylinder(stage, "/World/ReactorBuilding/HoseReel/Drum", 0.43, 0.68, (-5.15, -2.55, 0.78), materials["yellow"], axis="Y") - add_cylinder(stage, "/World/ReactorBuilding/HoseReel/FlangeL", 0.55, 0.07, (-5.15, -2.93, 0.78), materials["yellow_dark"], axis="Y") - add_cylinder(stage, "/World/ReactorBuilding/HoseReel/FlangeR", 0.55, 0.07, (-5.15, -2.17, 0.78), materials["yellow_dark"], axis="Y") - add_cube(stage, "/World/ReactorBuilding/HoseReel/StandL", (0.10, 0.10, 1.25), (-5.58, -2.85, 0.63), materials["yellow_dark"]) - add_cube(stage, "/World/ReactorBuilding/HoseReel/StandR", (0.10, 0.10, 1.25), (-4.72, -2.85, 0.63), materials["yellow_dark"]) + add_cube( + stage, + f"/World/ReactorBuilding/Drain/Bar{index:02d}", + (0.035, 0.29, 0.035), + (float(x_m), drain_y, 0.028), + materials["steel"], + ) + + add_cube( + stage, + "/World/ReactorBuilding/UtilitySkid/Base", + (2.8, 1.75, 0.16), + (-5.55, -3.65, 0.08), + materials["dark_steel"], + collision=True, + ) + add_cylinder( + stage, + "/World/ReactorBuilding/UtilitySkid/RecoveryTank", + 0.53, + 1.55, + (-6.15, -3.65, 0.92), + materials["white"], + collision=True, + ) + add_cylinder( + stage, + "/World/ReactorBuilding/UtilitySkid/Pump", + 0.25, + 0.64, + (-4.65, -3.80, 0.42), + materials["blue"], + axis="X", + ) + add_cube( + stage, + "/World/ReactorBuilding/UtilitySkid/ControlCabinet", + (0.58, 0.42, 1.05), + (-4.75, -3.10, 0.61), + materials["yellow_dark"], + collision=True, + ) + add_cylinder( + stage, + "/World/ReactorBuilding/HoseReel/Drum", + 0.43, + 0.68, + (-5.15, -2.55, 0.78), + materials["yellow"], + axis="Y", + ) + add_cylinder( + stage, + "/World/ReactorBuilding/HoseReel/FlangeL", + 0.55, + 0.07, + (-5.15, -2.93, 0.78), + materials["yellow_dark"], + axis="Y", + ) + add_cylinder( + stage, + "/World/ReactorBuilding/HoseReel/FlangeR", + 0.55, + 0.07, + (-5.15, -2.17, 0.78), + materials["yellow_dark"], + axis="Y", + ) + add_cube( + stage, + "/World/ReactorBuilding/HoseReel/StandL", + (0.10, 0.10, 1.25), + (-5.58, -2.85, 0.63), + materials["yellow_dark"], + ) + add_cube( + stage, + "/World/ReactorBuilding/HoseReel/StandR", + (0.10, 0.10, 1.25), + (-4.72, -2.85, 0.63), + materials["yellow_dark"], + ) roller_points = ((-3.75, -2.25, 0.20), (-2.55, -1.85, 0.20)) for index, point in enumerate(roller_points): - add_cylinder(stage, f"/World/ReactorBuilding/CornerRollers/R{index}", 0.12, 0.32, point, materials["orange"], axis="Z") - add_cube(stage, f"/World/ReactorBuilding/CornerRollers/R{index}Base", (0.38, 0.38, 0.04), (point[0], point[1], 0.02), materials["dark_steel"]) + add_cylinder( + stage, + f"/World/ReactorBuilding/CornerRollers/R{index}", + 0.12, + 0.32, + point, + materials["orange"], + axis="Z", + ) + add_cube( + stage, + f"/World/ReactorBuilding/CornerRollers/R{index}Base", + (0.38, 0.38, 0.04), + (point[0], point[1], 0.02), + materials["dark_steel"], + ) for index in range(5): x_m = -6.6 + index * 0.55 - add_cube(stage, f"/World/ReactorBuilding/Barrier/Post{index}", (0.06, 0.06, 0.85), (x_m, -2.65, 0.43), materials["orange"]) - add_cube(stage, "/World/ReactorBuilding/Barrier/Rail", (2.25, 0.05, 0.06), (-5.5, -2.65, 0.72), materials["orange"]) + add_cube( + stage, + f"/World/ReactorBuilding/Barrier/Post{index}", + (0.06, 0.06, 0.85), + (x_m, -2.65, 0.43), + materials["orange"], + ) + add_cube( + stage, + "/World/ReactorBuilding/Barrier/Rail", + (2.25, 0.05, 0.06), + (-5.5, -2.65, 0.72), + materials["orange"], + ) - for index, (x_m, y_m, yaw) in enumerate(((-6.5, 1.0, 17.0), (3.4, -4.3, -8.0), (6.7, 0.2, 25.0))): - add_cube(stage, f"/World/ReactorBuilding/Debris/Chunk{index}", (0.34, 0.22, 0.14), (x_m, y_m, 0.08), materials["concrete"], rotation_xyz=(0.0, 0.0, yaw), collision=True) + for index, (x_m, y_m, yaw) in enumerate( + ((-6.5, 1.0, 17.0), (3.4, -4.3, -8.0), (6.7, 0.2, 25.0)) + ): + add_cube( + stage, + f"/World/ReactorBuilding/Debris/Chunk{index}", + (0.34, 0.22, 0.14), + (x_m, y_m, 0.08), + materials["concrete"], + rotation_xyz=(0.0, 0.0, yaw), + collision=True, + ) + light_height_m = 12.35 if high_bay else 4.65 for index, x_m in enumerate((-4.8, 0.0, 4.8)): light = UsdLux.RectLight.Define(stage, f"/World/ReactorBuilding/Lights/L{index}") light.CreateIntensityAttr(18_000.0) light.CreateWidthAttr(2.2) light.CreateHeightAttr(0.25) light.CreateColorAttr(Gf.Vec3f(0.78, 0.88, 1.0)) - light.AddTranslateOp().Set(Gf.Vec3d(x_m, 0.0, 4.65)) + light.AddTranslateOp().Set(Gf.Vec3d(x_m, 0.0, light_height_m)) light.AddRotateXYZOp().Set(Gf.Vec3f(0.0, 0.0, 0.0)) return roller_points @@ -321,55 +635,212 @@ def create_robot(stage, materials, start_xyz) -> RobotVisuals: UsdPhysics.MassAPI.Apply(prim).CreateMassAttr(840.0) prim.CreateAttribute("rad:robot:id", Sdf.ValueTypeNames.String).Set(ROBOT_ID) prim.CreateAttribute("rad:robot:role", Sdf.ValueTypeNames.String).Set("mitigation") - prim.CreateAttribute("rad:robot:reference", Sdf.ValueTypeNames.String).Set("Hitachi-GE Arounder") + prim.CreateAttribute("rad:robot:reference", Sdf.ValueTypeNames.String).Set( + "Hitachi-GE Arounder" + ) prim.CreateAttribute("rad:stream:focus", Sdf.ValueTypeNames.Bool).Set(True) - add_cube(stage, f"{ROBOT_PATH}/LowerChassis", (1.28, 0.52, 0.30), (-0.05, 0.0, -0.10), materials["yellow_dark"], collision=True) - add_cube(stage, f"{ROBOT_PATH}/MainBody", (1.12, 0.54, 0.42), (-0.13, 0.0, 0.16), materials["yellow"], collision=True) - add_cube(stage, f"{ROBOT_PATH}/RearPowerPack", (0.46, 0.50, 0.55), (-0.42, 0.0, 0.52), materials["yellow"]) - add_cube(stage, f"{ROBOT_PATH}/TopServicePanel", (0.62, 0.45, 0.06), (-0.12, 0.0, 0.48), materials["dark_steel"]) + add_cube( + stage, + f"{ROBOT_PATH}/LowerChassis", + (1.28, 0.52, 0.30), + (-0.05, 0.0, -0.10), + materials["yellow_dark"], + collision=True, + ) + add_cube( + stage, + f"{ROBOT_PATH}/MainBody", + (1.12, 0.54, 0.42), + (-0.13, 0.0, 0.16), + materials["yellow"], + collision=True, + ) + add_cube( + stage, + f"{ROBOT_PATH}/RearPowerPack", + (0.46, 0.50, 0.55), + (-0.42, 0.0, 0.52), + materials["yellow"], + ) + add_cube( + stage, + f"{ROBOT_PATH}/TopServicePanel", + (0.62, 0.45, 0.06), + (-0.12, 0.0, 0.48), + materials["dark_steel"], + ) for side_name, y_m in (("Left", 0.36), ("Right", -0.36)): - add_cube(stage, f"{ROBOT_PATH}/Tracks/{side_name}/Belt", (1.50, 0.18, 0.34), (-0.08, y_m, -0.18), materials["track"], collision=True) + add_cube( + stage, + f"{ROBOT_PATH}/Tracks/{side_name}/Belt", + (1.50, 0.18, 0.34), + (-0.08, y_m, -0.18), + materials["track"], + collision=True, + ) for wheel_index, x_m in enumerate((-0.57, -0.18, 0.22, 0.57)): - add_cylinder(stage, f"{ROBOT_PATH}/Tracks/{side_name}/RoadWheel{wheel_index}", 0.135, 0.19, (x_m, y_m, -0.18), materials["steel"], axis="Y") - add_cylinder(stage, f"{ROBOT_PATH}/Tracks/{side_name}/Hub{wheel_index}", 0.055, 0.205, (x_m, y_m, -0.18), materials["yellow_dark"], axis="Y") + add_cylinder( + stage, + f"{ROBOT_PATH}/Tracks/{side_name}/RoadWheel{wheel_index}", + 0.135, + 0.19, + (x_m, y_m, -0.18), + materials["steel"], + axis="Y", + ) + add_cylinder( + stage, + f"{ROBOT_PATH}/Tracks/{side_name}/Hub{wheel_index}", + 0.055, + 0.205, + (x_m, y_m, -0.18), + materials["yellow_dark"], + axis="Y", + ) for tread_index, x_m in enumerate(np.linspace(-0.70, 0.54, 10)): for surface, z_m in (("Top", -0.005), ("Bottom", -0.355)): - add_cube(stage, f"{ROBOT_PATH}/Tracks/{side_name}/{surface}Tread{tread_index:02d}", (0.105, 0.205, 0.030), (float(x_m), y_m, z_m), materials["track"]) + add_cube( + stage, + f"{ROBOT_PATH}/Tracks/{side_name}/{surface}Tread{tread_index:02d}", + (0.105, 0.205, 0.030), + (float(x_m), y_m, z_m), + materials["track"], + ) shoulder = (-0.25, 0.46) elbow = (0.16, 0.73) wrist = (0.52, 0.39) tool_mount = (0.79, -0.10) - add_cylinder(stage, f"{ROBOT_PATH}/Arm/BaseYaw", 0.19, 0.16, (-0.25, 0.0, 0.43), materials["yellow_dark"], axis="Z") + add_cylinder( + stage, + f"{ROBOT_PATH}/Arm/BaseYaw", + 0.19, + 0.16, + (-0.25, 0.0, 0.43), + materials["yellow_dark"], + axis="Z", + ) add_link(stage, f"{ROBOT_PATH}/Arm/Boom", shoulder, elbow, 0.0, 0.13, materials["yellow"]) add_link(stage, f"{ROBOT_PATH}/Arm/Forearm", elbow, wrist, 0.0, 0.12, materials["steel"]) - add_link(stage, f"{ROBOT_PATH}/Arm/WristLink", wrist, tool_mount, 0.0, 0.105, materials["steel"]) - add_link(stage, f"{ROBOT_PATH}/Arm/HydraulicCylinderA", (-0.30, 0.34), (0.09, 0.68), 0.10, 0.052, materials["steel"]) - add_link(stage, f"{ROBOT_PATH}/Arm/HydraulicCylinderB", (0.05, 0.65), (0.47, 0.33), -0.10, 0.045, materials["yellow_dark"]) + add_link( + stage, f"{ROBOT_PATH}/Arm/WristLink", wrist, tool_mount, 0.0, 0.105, materials["steel"] + ) + add_link( + stage, + f"{ROBOT_PATH}/Arm/HydraulicCylinderA", + (-0.30, 0.34), + (0.09, 0.68), + 0.10, + 0.052, + materials["steel"], + ) + add_link( + stage, + f"{ROBOT_PATH}/Arm/HydraulicCylinderB", + (0.05, 0.65), + (0.47, 0.33), + -0.10, + 0.045, + materials["yellow_dark"], + ) for index, point in enumerate((shoulder, elbow, wrist, tool_mount)): - add_cylinder(stage, f"{ROBOT_PATH}/Arm/Joint{index}", 0.105, 0.22, (point[0], 0.0, point[1]), materials["dark_steel"], axis="Y") + add_cylinder( + stage, + f"{ROBOT_PATH}/Arm/Joint{index}", + 0.105, + 0.22, + (point[0], 0.0, point[1]), + materials["dark_steel"], + axis="Y", + ) head = UsdGeom.Xform.Define(stage, f"{ROBOT_PATH}/Arm/DeconHead") head.AddTranslateOp().Set(Gf.Vec3d(HEAD_X_M, 0.0, -0.26)) - add_cube(stage, f"{ROBOT_PATH}/Arm/DeconHead/Top", (0.42, 0.58, 0.075), (0.0, 0.0, 0.09), materials["steel"]) - add_cube(stage, f"{ROBOT_PATH}/Arm/DeconHead/FrontFrame", (0.075, 0.58, 0.18), (0.17, 0.0, 0.015), materials["steel"]) - add_cube(stage, f"{ROBOT_PATH}/Arm/DeconHead/RearFrame", (0.075, 0.58, 0.18), (-0.17, 0.0, 0.015), materials["steel"]) - add_cube(stage, f"{ROBOT_PATH}/Arm/DeconHead/BrushFront", (0.055, 0.58, 0.10), (0.19, 0.0, -0.075), materials["brush"]) - add_cube(stage, f"{ROBOT_PATH}/Arm/DeconHead/BrushRear", (0.055, 0.58, 0.10), (-0.19, 0.0, -0.075), materials["brush"]) - add_cube(stage, f"{ROBOT_PATH}/Arm/DeconHead/BrushLeft", (0.34, 0.045, 0.10), (0.0, 0.29, -0.075), materials["brush"]) - add_cube(stage, f"{ROBOT_PATH}/Arm/DeconHead/BrushRight", (0.34, 0.045, 0.10), (0.0, -0.29, -0.075), materials["brush"]) - add_cylinder(stage, f"{ROBOT_PATH}/Arm/DeconHead/SuctionPort", 0.075, 0.13, (-0.10, 0.0, 0.17), materials["green"], axis="Z") + add_cube( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/Top", + (0.42, 0.58, 0.075), + (0.0, 0.0, 0.09), + materials["steel"], + ) + add_cube( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/FrontFrame", + (0.075, 0.58, 0.18), + (0.17, 0.0, 0.015), + materials["steel"], + ) + add_cube( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/RearFrame", + (0.075, 0.58, 0.18), + (-0.17, 0.0, 0.015), + materials["steel"], + ) + add_cube( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/BrushFront", + (0.055, 0.58, 0.10), + (0.19, 0.0, -0.075), + materials["brush"], + ) + add_cube( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/BrushRear", + (0.055, 0.58, 0.10), + (-0.19, 0.0, -0.075), + materials["brush"], + ) + add_cube( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/BrushLeft", + (0.34, 0.045, 0.10), + (0.0, 0.29, -0.075), + materials["brush"], + ) + add_cube( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/BrushRight", + (0.34, 0.045, 0.10), + (0.0, -0.29, -0.075), + materials["brush"], + ) + add_cylinder( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/SuctionPort", + 0.075, + 0.13, + (-0.10, 0.0, 0.17), + materials["green"], + axis="Z", + ) carriage = UsdGeom.Xform.Define(stage, f"{ROBOT_PATH}/Arm/DeconHead/NozzleCarriage") nozzle_translate_op = carriage.AddTranslateOp() nozzle_translate_op.Set(Gf.Vec3d(0.0, 0.0, 0.0)) - add_cube(stage, f"{ROBOT_PATH}/Arm/DeconHead/NozzleCarriage/RailCar", (0.12, 0.10, 0.055), (0.0, 0.0, 0.035), materials["yellow_dark"]) - add_cylinder(stage, f"{ROBOT_PATH}/Arm/DeconHead/NozzleCarriage/Nozzle", 0.028, 0.105, (0.04, 0.0, -0.02), materials["steel"], axis="Z") + add_cube( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/NozzleCarriage/RailCar", + (0.12, 0.10, 0.055), + (0.0, 0.0, 0.035), + materials["yellow_dark"], + ) + add_cylinder( + stage, + f"{ROBOT_PATH}/Arm/DeconHead/NozzleCarriage/Nozzle", + 0.028, + 0.105, + (0.04, 0.0, -0.02), + materials["steel"], + axis="Z", + ) spray_prims = [] for index, offset_y in enumerate((-0.025, 0.0, 0.025)): - cone = UsdGeom.Cone.Define(stage, f"{ROBOT_PATH}/Arm/DeconHead/NozzleCarriage/ContainedJet{index}") + cone = UsdGeom.Cone.Define( + stage, f"{ROBOT_PATH}/Arm/DeconHead/NozzleCarriage/ContainedJet{index}" + ) cone.CreateAxisAttr("Z") cone.CreateRadiusAttr(0.035 + 0.008 * index) cone.CreateHeightAttr(0.11) @@ -379,37 +850,91 @@ def create_robot(stage, materials, start_xyz) -> RobotVisuals: spray_prims.append(cone) for camera_index, y_m in enumerate((-0.18, 0.18)): - add_cube(stage, f"{ROBOT_PATH}/Vision/Camera{camera_index}Body", (0.14, 0.12, 0.11), (0.52, y_m, 0.54), materials["dark_steel"]) - add_cylinder(stage, f"{ROBOT_PATH}/Vision/Camera{camera_index}Lens", 0.043, 0.035, (0.60, y_m, 0.54), materials["glass"], axis="X") - add_sphere(stage, f"{ROBOT_PATH}/Vision/WorkLight{camera_index}", 0.065, (0.58, y_m * 0.45, 0.38), materials["white"]) - seal_lamp = add_sphere(stage, f"{ROBOT_PATH}/Vision/SealLamp", 0.045, (0.30, 0.0, 0.66), materials["red"]) + add_cube( + stage, + f"{ROBOT_PATH}/Vision/Camera{camera_index}Body", + (0.14, 0.12, 0.11), + (0.52, y_m, 0.54), + materials["dark_steel"], + ) + add_cylinder( + stage, + f"{ROBOT_PATH}/Vision/Camera{camera_index}Lens", + 0.043, + 0.035, + (0.60, y_m, 0.54), + materials["glass"], + axis="X", + ) + add_sphere( + stage, + f"{ROBOT_PATH}/Vision/WorkLight{camera_index}", + 0.065, + (0.58, y_m * 0.45, 0.38), + materials["white"], + ) + seal_lamp = add_sphere( + stage, f"{ROBOT_PATH}/Vision/SealLamp", 0.045, (0.30, 0.0, 0.66), materials["red"] + ) seal_color_attr = seal_lamp.CreateDisplayColorAttr([Gf.Vec3f(0.88, 0.04, 0.02)]) - add_curve(stage, f"{ROBOT_PATH}/Hoses/HighPressure", [(-0.65, -0.16, 0.62), (-0.12, -0.16, 0.78), (0.42, -0.15, 0.44), (0.88, -0.12, -0.08)], 0.036, (0.06, 0.22, 0.72)) - add_curve(stage, f"{ROBOT_PATH}/Hoses/Recovery", [(-0.65, 0.16, 0.58), (-0.12, 0.18, 0.74), (0.45, 0.18, 0.40), (0.86, 0.13, -0.04)], 0.058, (0.05, 0.08, 0.07)) - add_curve(stage, f"{ROBOT_PATH}/Hoses/Control", [(-0.65, 0.05, 0.66), (-0.05, 0.05, 0.82), (0.52, 0.06, 0.38)], 0.018, (0.95, 0.34, 0.02)) + add_curve( + stage, + f"{ROBOT_PATH}/Hoses/HighPressure", + [(-0.65, -0.16, 0.62), (-0.12, -0.16, 0.78), (0.42, -0.15, 0.44), (0.88, -0.12, -0.08)], + 0.036, + (0.06, 0.22, 0.72), + ) + add_curve( + stage, + f"{ROBOT_PATH}/Hoses/Recovery", + [(-0.65, 0.16, 0.58), (-0.12, 0.18, 0.74), (0.45, 0.18, 0.40), (0.86, 0.13, -0.04)], + 0.058, + (0.05, 0.08, 0.07), + ) + add_curve( + stage, + f"{ROBOT_PATH}/Hoses/Control", + [(-0.65, 0.05, 0.66), (-0.05, 0.05, 0.82), (0.52, 0.06, 0.38)], + 0.018, + (0.95, 0.34, 0.02), + ) UsdGeom.Xform.Define(stage, "/World/Utilities") tether_points_attrs = ( - add_curve(stage, "/World/Utilities/Tether/Pressure", [(0.0, 0.0, 0.0)] * 5, 0.040, (0.04, 0.16, 0.60)), - add_curve(stage, "/World/Utilities/Tether/Recovery", [(0.0, 0.0, 0.0)] * 5, 0.064, (0.025, 0.035, 0.03)), - add_curve(stage, "/World/Utilities/Tether/PowerControl", [(0.0, 0.0, 0.0)] * 5, 0.020, (0.94, 0.28, 0.02)), + add_curve( + stage, + "/World/Utilities/Tether/Pressure", + [(0.0, 0.0, 0.0)] * 5, + 0.040, + (0.04, 0.16, 0.60), + ), + add_curve( + stage, + "/World/Utilities/Tether/Recovery", + [(0.0, 0.0, 0.0)] * 5, + 0.064, + (0.025, 0.035, 0.03), + ), + add_curve( + stage, + "/World/Utilities/Tether/PowerControl", + [(0.0, 0.0, 0.0)] * 5, + 0.020, + (0.94, 0.28, 0.02), + ), + ) + return RobotVisuals( + nozzle_translate_op, tuple(spray_prims), seal_color_attr, tether_points_attrs ) - return RobotVisuals(nozzle_translate_op, tuple(spray_prims), seal_color_attr, tether_points_attrs) def activity_field(cells_x: int, cells_y: int) -> np.ndarray: - x = np.linspace(-1.0, 1.0, cells_x) - y = np.linspace(-1.0, 1.0, cells_y) - xx, yy = np.meshgrid(x, y, indexing="xy") - hotspot = 2.8 * np.exp(-((xx - 0.30) ** 2 + (yy + 0.18) ** 2) / 0.12) - pipe_drip = 1.4 * np.exp(-((xx + 0.48) ** 2) / 0.045) * np.exp(-((yy - 0.22) ** 2) / 0.25) - mottling = 0.22 * (np.sin(7.0 * xx) * np.cos(5.0 * yy) + 1.0) - return (115_000.0 * (1.0 + hotspot + pipe_drip + mottling)).reshape(-1) + return irregular_deposition_field(cells_x, cells_y) def create_surface_source(stage) -> tuple[SurfaceSourceGrid, list, list]: - cells_x, cells_y = 32, 16 + cells_x, cells_y = 48, 28 efficiency = np.ones(cells_x * cells_y, dtype=np.float64) field = efficiency.reshape(cells_y, cells_x) field[3:8, 17:23] = 0.52 @@ -427,19 +952,27 @@ def create_surface_source(stage) -> tuple[SurfaceSourceGrid, list, list]: root_prim = root.GetPrim() root_prim.CreateAttribute("rad:role", Sdf.ValueTypeNames.String).Set("contaminated_surface") root_prim.CreateAttribute("rad:source:type", Sdf.ValueTypeNames.String).Set("surface") - root_prim.CreateAttribute("rad:surface:coating", Sdf.ValueTypeNames.String).Set("radiation-resistant epoxy") - root_prim.CreateAttribute("rad:source:initialActivityBq", Sdf.ValueTypeNames.Double).Set(grid.initial_total_activity_bq) + root_prim.CreateAttribute("rad:surface:coating", Sdf.ValueTypeNames.String).Set( + "radiation-resistant epoxy" + ) + root_prim.CreateAttribute("rad:source:initialActivityBq", Sdf.ValueTypeNames.Double).Set( + grid.initial_total_activity_bq + ) color_attributes = [] activity_attributes = [] raw_colors = grid.color_rgb() epoxy = np.asarray([0.19, 0.27, 0.25]) - colors = 0.62 * raw_colors + 0.38 * epoxy + # Radiation contamination is not a physical red mat. Keep this as a muted + # scientific overlay on the epoxy floor, with inactive grid cells hidden. + colors = 0.34 * raw_colors + 0.66 * epoxy for index, center in enumerate(grid.centers_world_m): cell = UsdGeom.Cube.Define(stage, f"{SOURCE_PATH}/Cell_{index:03d}") cell.CreateSizeAttr(1.0) cell.AddTranslateOp().Set(Gf.Vec3d(*center)) - cell.AddScaleOp().Set(Gf.Vec3f(grid.cell_size_x_m * 0.985, grid.cell_size_y_m * 0.985, 0.012)) + cell.AddScaleOp().Set(Gf.Vec3f(grid.cell_size_x_m * 1.02, grid.cell_size_y_m * 1.02, 0.004)) color_attribute = cell.CreateDisplayColorAttr([Gf.Vec3f(*colors[index])]) + if grid.activity_bq[index] <= 0.0: + cell.GetVisibilityAttr().Set(UsdGeom.Tokens.invisible) color_attributes.append(color_attribute) prim = cell.GetPrim() prim.CreateAttribute("rad:role", Sdf.ValueTypeNames.String).Set("source") @@ -451,6 +984,349 @@ def create_surface_source(stage) -> tuple[SurfaceSourceGrid, list, list]: return grid, color_attributes, activity_attributes +def create_high_wall_surface_source(stage) -> SurfaceSourceGrid: + cells_x, cells_y = 48, 28 + efficiency = np.ones(cells_x * cells_y, dtype=np.float64) + efficiency.reshape(cells_y, cells_x)[8:15, 20:29] = 0.48 + grid = SurfaceSourceGrid( + cells_x=cells_x, + cells_y=cells_y, + size_x_m=3.2, + size_y_m=2.2, + center_world_m=(0.0, 5.724, 9.35), + activity_bq_per_cell=activity_field(cells_x, cells_y), + efficiency_field=efficiency, + surface_u_world=(1.0, 0.0, 0.0), + surface_v_world=(0.0, 0.0, 1.0), + ) + root = UsdGeom.Xform.Define(stage, HIGH_WALL_SOURCE_PATH) + root_prim = root.GetPrim() + for name, value_type, value in ( + ("rad:role", Sdf.ValueTypeNames.String, "contaminated_surface"), + ("rad:source:type", Sdf.ValueTypeNames.String, "surface"), + ("rad:source:isotopeId", Sdf.ValueTypeNames.String, "Cs-137"), + ("rad:source:surfaceKind", Sdf.ValueTypeNames.String, "high_wall"), + ("rad:decon:enabled", Sdf.ValueTypeNames.Bool, True), + ("rad:source:initialActivityBq", Sdf.ValueTypeNames.Double, grid.initial_total_activity_bq), + ("rad:source:maximumHeightM", Sdf.ValueTypeNames.Double, 10.45), + ): + root_prim.CreateAttribute(name, value_type).Set(value) + + raw_colors = grid.color_rgb() + concrete = np.asarray([0.38, 0.40, 0.39]) + colors = 0.38 * raw_colors + 0.62 * concrete + for index, center in enumerate(grid.centers_world_m): + if grid.activity_bq[index] <= 0.0: + continue + cell = UsdGeom.Cube.Define(stage, f"{HIGH_WALL_SOURCE_PATH}/Cell_{index:04d}") + cell.CreateSizeAttr(1.0) + cell.AddTranslateOp().Set(Gf.Vec3d(*center)) + cell.AddScaleOp().Set( + Gf.Vec3f(grid.cell_size_x_m * 1.025, 0.004, grid.cell_size_y_m * 1.025) + ) + cell.CreateDisplayColorAttr([Gf.Vec3f(*colors[index])]) + prim = cell.GetPrim() + prim.CreateAttribute("rad:role", Sdf.ValueTypeNames.String).Set("source") + prim.CreateAttribute("rad:source:type", Sdf.ValueTypeNames.String).Set("surface_cell") + prim.CreateAttribute("rad:source:activityBq", Sdf.ValueTypeNames.Double).Set( + float(grid.activity_bq[index]) + ) + prim.CreateAttribute("rad:decon:enabled", Sdf.ValueTypeNames.Bool).Set(True) + return grid + + +def create_high_reach_robot(stage, materials) -> dict[str, object]: + """Build a 10 m research high-reach decontamination mechanism. + + The architecture is derived from SUPER-Giraffe's telescopic-ladder, + outrigger, and distal-manipulator layout, but the sixth ladder stage and + 10 m reach envelope are RadCounterSim research extensions rather than a + claim about the manufacturer's 8 m machine. + """ + + root = UsdGeom.Xform.Define(stage, HIGH_REACH_ROBOT_PATH) + root.AddTranslateOp().Set(Gf.Vec3d(0.0, 0.70, 0.0)) + prim = root.GetPrim() + for name, value_type, value in ( + ("rad:role", Sdf.ValueTypeNames.String, "countermeasure_robot"), + ("rad:robot:model", Sdf.ValueTypeNames.String, "RadCounter HighReach-10"), + ("rad:robot:referenceArchitecture", Sdf.ValueTypeNames.String, "MHI SUPER-Giraffe"), + ("rad:robot:geometryFidelity", Sdf.ValueTypeNames.String, "reference_procedural"), + ( + "rad:robot:driveArchitecture", + Sdf.ValueTypeNames.String, + "four_wheel_drive_four_wheel_steering", + ), + ("rad:robot:maximumReachHeightM", Sdf.ValueTypeNames.Double, 10.0), + ("rad:robot:telescopicStages", Sdf.ValueTypeNames.Int, 6), + ("rad:robot:distalManipulatorDofs", Sdf.ValueTypeNames.Int, 7), + ("rad:robot:stabilityInterlock", Sdf.ValueTypeNames.Bool, True), + ("rad:robot:transportRequiresBoomStowed", Sdf.ValueTypeNames.Bool, True), + ("rad:robot:driveInterlockedWithOutriggers", Sdf.ValueTypeNames.Bool, True), + ("rad:decon:method", Sdf.ValueTypeNames.String, "dry_ice_blast_with_suction"), + ): + prim.CreateAttribute(name, value_type).Set(value) + + # A 10 m ladder cannot credibly sit on the compact cart used by the first + # concept render. This is a road-mobile 4WD/4WS carrier: the ladder folds + # into the rear cradle for transport, and the four jacks must be retracted + # before the wheel drives can be enabled. + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Underbody", + (2.18, 3.18, 0.22), + (0.0, -0.10, 0.31), + materials["dark_steel"], + collision=True, + ) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Chassis", + (2.50, 3.60, 0.62), + (0.0, -0.10, 0.62), + materials["yellow_dark"], + collision=True, + ) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/EquipmentDeck", + (2.14, 2.72, 0.34), + (0.0, -0.30, 1.02), + materials["yellow"], + collision=True, + ) + for y_m, axle_name in ((-1.27, "Rear"), (1.20, "Front")): + for x_m, side_name in ((-1.27, "Left"), (1.27, "Right")): + name = f"{side_name}_{axle_name}" + wheel = add_cylinder( + stage, + f"{HIGH_REACH_ROBOT_PATH}/SteeredWheels/{name}/Tire", + 0.46, + 0.34, + (x_m, y_m, 0.47), + materials["track"], + axis="X", + ) + wheel.GetPrim().CreateAttribute("rad:drive:powered", Sdf.ValueTypeNames.Bool).Set(True) + wheel.GetPrim().CreateAttribute("rad:drive:steered", Sdf.ValueTypeNames.Bool).Set(True) + add_cylinder( + stage, + f"{HIGH_REACH_ROBOT_PATH}/SteeredWheels/{name}/Hub", + 0.18, + 0.38, + (x_m, y_m, 0.47), + materials["steel"], + axis="X", + ) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/SteeredWheels/{name}/SteeringKnuckle", + (0.18, 0.32, 0.25), + (math.copysign(1.05, x_m), y_m, 0.62), + materials["dark_steel"], + ) + + for y_m, name in ((-1.94, "Rear"), (1.74, "Front")): + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Bumpers/{name}", + (2.40, 0.16, 0.28), + (0.0, y_m, 0.48), + materials["steel"], + collision=True, + ) + + for x_m in (-1.78, 1.78): + for y_m in (-1.48, 1.48): + name = f"{'L' if x_m < 0.0 else 'R'}_{'Rear' if y_m < 0.0 else 'Front'}" + beam_start = (math.copysign(0.98, x_m), y_m, 0.62) + beam_end = (x_m, y_m, 0.36) + add_link_3d( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Outriggers/{name}Beam", + beam_start, + beam_end, + 0.14, + materials["steel"], + ) + add_cylinder( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Outriggers/{name}Jack", + 0.11, + 0.52, + (x_m, y_m, 0.30), + materials["dark_steel"], + axis="Z", + ) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Outriggers/{name}Pad", + (0.44, 0.44, 0.10), + (x_m, y_m, 0.06), + materials["dark_steel"], + collision=True, + ) + + add_cylinder( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Turntable", + 0.62, + 0.30, + (0.0, 0.35, 1.31), + materials["dark_steel"], + ) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Counterweight", + (1.55, 0.90, 0.78), + (0.0, -0.78, 1.47), + materials["yellow"], + collision=True, + ) + + for x_m in (-0.53, 0.53): + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/TransportCradle/Side{'L' if x_m < 0 else 'R'}", + (0.16, 0.22, 0.72), + (x_m, -1.25, 1.48), + materials["dark_steel"], + ) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/TransportCradle/Saddle", + (1.22, 0.25, 0.18), + (0.0, -1.25, 1.80), + materials["steel"], + ) + + boom_start = np.asarray((0.0, 0.35, 1.57), dtype=np.float64) + boom_tip = np.asarray((0.0, 4.12, 9.40), dtype=np.float64) + fractions = (0.0, 0.23, 0.43, 0.61, 0.76, 0.89, 1.0) + widths = (0.48, 0.43, 0.38, 0.33, 0.28, 0.23) + for index, (low, high, width_m) in enumerate( + zip(fractions[:-1], fractions[1:], widths, strict=True), start=1 + ): + start = boom_start + low * (boom_tip - boom_start) + end = boom_start + high * (boom_tip - boom_start) + add_link_3d( + stage, + f"{HIGH_REACH_ROBOT_PATH}/TelescopicLadder/Stage{index}", + start, + end, + width_m, + materials["yellow"] if index % 2 else materials["steel"], + ) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/TelescopicLadder/Collar{index}", + (width_m + 0.09, width_m + 0.09, width_m + 0.09), + tuple(float(value) for value in end), + materials["dark_steel"], + ) + + arm_points = ( + tuple(boom_tip), + (0.18, 4.22, 9.55), + (0.33, 4.34, 9.61), + (0.28, 4.46, 9.49), + (0.15, 4.58, 9.39), + (0.06, 4.69, 9.38), + (0.01, 4.79, 9.38), + (0.00, 4.88, 9.38), + ) + for index, (start, end) in enumerate(zip(arm_points[:-1], arm_points[1:], strict=True)): + add_link_3d( + stage, + f"{HIGH_REACH_ROBOT_PATH}/DistalManipulator/Link{index + 1}", + start, + end, + 0.14 - 0.008 * index, + materials["steel"] if index % 2 else materials["orange"], + ) + add_sphere( + stage, + f"{HIGH_REACH_ROBOT_PATH}/DistalManipulator/Joint{index + 1}", + 0.10 - 0.005 * index, + end, + materials["dark_steel"], + ) + + head_center = (0.0, 4.96, 9.38) + head = UsdGeom.Xform.Define(stage, f"{HIGH_REACH_ROBOT_PATH}/DeconHead") + head.GetPrim().CreateAttribute("rad:role", Sdf.ValueTypeNames.String).Set("decon_tool") + head.GetPrim().CreateAttribute("rad:decon:contactHeightM", Sdf.ValueTypeNames.Double).Set(9.38) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/DeconHead/RecoveryHousing", + (0.64, 0.16, 0.54), + head_center, + materials["steel"], + ) + add_cube( + stage, + f"{HIGH_REACH_ROBOT_PATH}/DeconHead/ContainmentBrush", + (0.60, 0.06, 0.50), + (0.0, 5.07, 9.38), + materials["brush"], + ) + add_cylinder( + stage, + f"{HIGH_REACH_ROBOT_PATH}/DeconHead/SuctionPort", + 0.085, + 0.18, + (0.0, 4.84, 9.38), + materials["green"], + axis="Y", + ) + add_curve( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Hoses/BlastSupply", + [ + (0.38, -0.35, 0.85), + (0.28, 1.15, 3.0), + (0.20, 2.35, 5.7), + (0.12, 3.55, 8.0), + (0.08, 4.82, 9.30), + ], + 0.045, + (0.10, 0.28, 0.72), + ) + add_curve( + stage, + f"{HIGH_REACH_ROBOT_PATH}/Hoses/DustRecovery", + [ + (-0.38, -0.30, 0.82), + (-0.30, 1.10, 2.9), + (-0.22, 2.30, 5.6), + (-0.15, 3.50, 7.9), + (-0.10, 4.82, 9.32), + ], + 0.075, + (0.04, 0.05, 0.05), + ) + + return { + "robot_path": HIGH_REACH_ROBOT_PATH, + "model": "RadCounter HighReach-10", + "drive_architecture": "four-wheel drive / four-wheel steering", + "wheel_diameter_m": 0.92, + "chassis_dimensions_m": [2.50, 3.60, 0.62], + "rendered_configuration": "working: outriggers deployed, wheel drive interlocked", + "transport_transition": [ + "retract distal manipulator", + "retract and lower telescopic ladder into transport cradle", + "raise four outrigger jacks", + "enable four wheel steering and drive", + ], + "maximum_reach_height_m": 10.0, + "telescopic_stages": 6, + "distal_manipulator_dofs": 7, + "reference_architecture": "MHI SUPER-Giraffe (8 m); extended research design", + "reference_urls": [MHI_SUPER_GIRAFFE_REFERENCE, IRID_HIGH_PLACE_REFERENCE], + } + + def create_work_zone(stage, materials, grid: SurfaceSourceGrid) -> None: half_x = grid.size_x_m * 0.5 half_y = grid.size_y_m * 0.5 @@ -461,9 +1337,276 @@ def create_work_zone(stage, materials, grid: SurfaceSourceGrid) -> None: ("West", (0.055, grid.size_y_m + 0.14, 0.025), (cx - half_x - 0.06, cy, 0.026)), ("East", (0.055, grid.size_y_m + 0.14, 0.025), (cx + half_x + 0.06, cy, 0.026)), ): - add_cube(stage, f"/World/ReactorBuilding/WorkZone/{name}", size, position, materials["orange"]) + add_cube( + stage, f"/World/ReactorBuilding/WorkZone/{name}", size, position, materials["orange"] + ) for index, x_m in enumerate(np.linspace(cx - half_x, cx + half_x, 8)): - add_cube(stage, f"/World/ReactorBuilding/WorkZone/SurveyMark{index}", (0.025, 0.16, 0.02), (float(x_m), cy + half_y + 0.18, 0.022), materials["white"]) + add_cube( + stage, + f"/World/ReactorBuilding/WorkZone/SurveyMark{index}", + (0.025, 0.16, 0.02), + (float(x_m), cy + half_y + 0.18, 0.022), + materials["white"], + ) + + +def create_irregular_cylindrical_surface_source( + stage, + path: str, + *, + center_world_m: tuple[float, float, float], + radius_m: float, + height_m: float, + total_activity_bq: float, +) -> dict[str, object]: + """Create a sparse face-activity field conforming to a cylindrical object.""" + + angular_cells = 48 + vertical_cells = 24 + angles = np.linspace(-math.pi, math.pi, angular_cells, endpoint=False) + heights = np.linspace(-1.0, 1.0, vertical_cells) + theta, zz = np.meshgrid(angles, heights, indexing="xy") + + def wrapped_delta(value, center): + return np.angle(np.exp(1j * (value - center))) + + field = ( + 2.5 * np.exp(-(wrapped_delta(theta, -2.45) ** 2 / 0.48 + (zz - 0.18) ** 2 / 0.22)) + + 1.7 * np.exp(-(wrapped_delta(theta, -1.75) ** 2 / 0.24 + (zz + 0.38) ** 2 / 0.12)) + + 1.1 * np.exp(-(wrapped_delta(theta, 2.85) ** 2 / 0.14 + (zz - 0.62) ** 2 / 0.08)) + ) + roughness = 0.22 * np.sin(7.0 * theta + 4.0 * zz) + 0.17 * np.cos(13.0 * zz - 2.0 * theta) + active = field + roughness > 0.62 + active &= ~((wrapped_delta(theta, -2.30) / 0.22) ** 2 + ((zz + 0.02) / 0.16) ** 2 < 1.0) + active |= (wrapped_delta(theta, -0.95) ** 2 + (zz - 0.52) ** 2 < 0.025) | ( + wrapped_delta(theta, 2.15) ** 2 + (zz + 0.55) ** 2 < 0.018 + ) + + weights = np.where(active, np.maximum(0.1, field + 0.45 * roughness), 0.0) + weights /= max(float(weights.sum()), 1e-12) + face_activity = total_activity_bq * weights + points: list[Gf.Vec3f] = [] + counts: list[int] = [] + indices: list[int] = [] + activities: list[float] = [] + colors: list[Gf.Vec3f] = [] + center_x, center_y, center_z = center_world_m + delta_theta = 2.0 * math.pi / angular_cells + delta_z = height_m / (vertical_cells - 1) + visual_radius = radius_m + 0.004 + maximum = max(float(face_activity.max()), 1e-12) + for row, column in np.argwhere(active): + theta_center = float(theta[row, column]) + z_center = center_z + 0.5 * height_m * float(zz[row, column]) + theta_low = theta_center - 0.52 * delta_theta + theta_high = theta_center + 0.52 * delta_theta + z_low = z_center - 0.52 * delta_z + z_high = z_center + 0.52 * delta_z + base_index = len(points) + for angle, z_m in ( + (theta_low, z_low), + (theta_high, z_low), + (theta_high, z_high), + (theta_low, z_high), + ): + points.append( + Gf.Vec3f( + center_x + visual_radius * math.cos(angle), + center_y + visual_radius * math.sin(angle), + z_m, + ) + ) + counts.append(4) + indices.extend((base_index, base_index + 1, base_index + 2, base_index + 3)) + activity = float(face_activity[row, column]) + activities.append(activity) + fraction = activity / maximum + colors.append(Gf.Vec3f(0.34 + 0.46 * fraction, 0.07 + 0.10 * fraction, 0.025)) + + mesh = UsdGeom.Mesh.Define(stage, path) + mesh.CreatePointsAttr(points) + mesh.CreateFaceVertexCountsAttr(counts) + mesh.CreateFaceVertexIndicesAttr(indices) + mesh.CreateSubdivisionSchemeAttr("none") + color_primvar = mesh.CreateDisplayColorPrimvar(UsdGeom.Tokens.uniform) + color_primvar.Set(colors) + mesh_prim = mesh.GetPrim() + for name, value_type, value in ( + ("rad:role", Sdf.ValueTypeNames.String, "source"), + ("rad:source:type", Sdf.ValueTypeNames.String, "surface"), + ("rad:source:isotopeId", Sdf.ValueTypeNames.String, "Cs-137"), + ("rad:source:activityBq", Sdf.ValueTypeNames.Double, total_activity_bq), + ("rad:source:surfaceKind", Sdf.ValueTypeNames.String, "adhered_cylindrical_patch"), + ("rad:source:faceActivityBq", Sdf.ValueTypeNames.DoubleArray, activities), + ("rad:source:irregularMask", Sdf.ValueTypeNames.Bool, True), + ("rad:decon:enabled", Sdf.ValueTypeNames.Bool, True), + ): + mesh_prim.CreateAttribute(name, value_type).Set(value) + return { + "path": path, + "active_faces": len(activities), + "candidate_faces": angular_cells * vertical_cells, + "total_activity_bq": total_activity_bq, + } + + +def create_shield_manipulation_scene(stage, materials) -> dict[str, object]: + """Author an isolated, reactor-building shield-handling research scene.""" + + from isaacsim.storage.native import get_assets_root_path + + assets_root = get_assets_root_path() + if not assets_root: + raise RuntimeError("Isaac Sim assets root is unavailable") + + UsdGeom.Xform.Define(stage, "/World/ShieldManipulation") + robot = UsdGeom.Xform.Define(stage, MANIPULATOR_PATH) + robot.GetPrim().GetReferences().AddReference(assets_root + RIDGEBACK_FRANKA_ASSET) + robot_prim = robot.GetPrim() + robot_prim.CreateAttribute("rad:role", Sdf.ValueTypeNames.String).Set("countermeasure_robot") + robot_prim.CreateAttribute("rad:robot:model", Sdf.ValueTypeNames.String).Set( + "Clearpath Ridgeback + Franka Emika Panda" + ) + robot_prim.CreateAttribute("rad:robot:geometryFidelity", Sdf.ValueTypeNames.String).Set( + "manufacturer_asset" + ) + robot_prim.CreateAttribute("rad:robot:controller", Sdf.ValueTypeNames.String).Set( + "holonomic base + seven-axis arm IK + parallel gripper" + ) + + shield = UsdGeom.Xform.Define(stage, SHIELD_PATH) + shield.AddTranslateOp().Set(Gf.Vec3d(0.78, -0.62, 0.0)) + shield_prim = shield.GetPrim() + UsdPhysics.RigidBodyAPI.Apply(shield_prim).CreateRigidBodyEnabledAttr(True) + UsdPhysics.MassAPI.Apply(shield_prim).CreateMassAttr(2.8) + for name, value_type, value in ( + ("rad:role", Sdf.ValueTypeNames.String, "shield"), + ("rad:material:id", Sdf.ValueTypeNames.String, "tungsten_composite"), + ("rad:material:mode", Sdf.ValueTypeNames.String, "solid"), + ("rad:shield:movable", Sdf.ValueTypeNames.Bool, True), + ("rad:shield:massKg", Sdf.ValueTypeNames.Double, 2.8), + ("rad:manipulation:movable", Sdf.ValueTypeNames.Bool, True), + ("rad:manipulation:graspFrame", Sdf.ValueTypeNames.String, "GraspFrame"), + ): + shield_prim.CreateAttribute(name, value_type).Set(value) + + add_cube( + stage, + f"{SHIELD_PATH}/BaseFoot", + (0.30, 0.52, 0.065), + (0.0, 0.0, 0.035), + materials["dark_steel"], + collision=True, + ) + core = add_cube( + stage, + f"{SHIELD_PATH}/TungstenCore", + (0.025, 0.42, 0.62), + (0.0, 0.0, 0.37), + materials["lead"], + collision=True, + ) + core.GetPrim().CreateAttribute("rad:material:id", Sdf.ValueTypeNames.String).Set( + "tungsten_composite" + ) + for side_name, x_m in (("RobotSide", -0.020), ("SourceSide", 0.020)): + add_cube( + stage, + f"{SHIELD_PATH}/Jacket{side_name}", + (0.012, 0.46, 0.66), + (x_m, 0.0, 0.37), + materials["shield_skin"], + collision=True, + ) + for y_m in (-0.235, 0.235): + add_cube( + stage, + f"{SHIELD_PATH}/Edge_{'L' if y_m > 0.0 else 'R'}", + (0.060, 0.025, 0.68), + (0.0, y_m, 0.37), + materials["yellow_dark"], + ) + for y_m in (-0.10, 0.10): + add_cube( + stage, + f"{SHIELD_PATH}/HandleStand_{'L' if y_m > 0.0 else 'R'}", + (0.12, 0.025, 0.025), + (-0.075, y_m, 0.54), + materials["orange"], + ) + add_cube( + stage, + f"{SHIELD_PATH}/Handle", + (0.025, 0.23, 0.030), + (-0.14, 0.0, 0.54), + materials["orange"], + ) + grasp = UsdGeom.Xform.Define(stage, f"{SHIELD_PATH}/GraspFrame") + grasp.AddTranslateOp().Set(Gf.Vec3d(-0.14, 0.0, 0.54)) + + # A corroded process-pipe spool carries an adhered, spatially varying + # surface source. The object itself is not a homogeneous volume source. + source_path = "/World/ShieldManipulation/ContaminatedValveSpool" + source = UsdGeom.Xform.Define(stage, source_path) + source_prim = source.GetPrim() + source_prim.CreateAttribute("rad:role", Sdf.ValueTypeNames.String).Set("contaminated_object") + source_prim.CreateAttribute("rad:manipulation:movable", Sdf.ValueTypeNames.Bool).Set(True) + add_cylinder( + stage, + f"{source_path}/VerticalPipe", + 0.20, + 0.88, + (1.95, -0.62, 0.46), + materials["pipe"], + collision=True, + ) + for z_m in (0.10, 0.80): + add_cylinder( + stage, + f"{source_path}/Flange_{int(z_m * 100):02d}", + 0.31, + 0.10, + (1.95, -0.62, z_m), + materials["dark_steel"], + collision=True, + ) + add_cylinder( + stage, + f"{source_path}/BranchPipe", + 0.13, + 0.72, + (1.62, -0.62, 0.56), + materials["pipe"], + axis="X", + collision=True, + ) + add_cylinder( + stage, + f"{source_path}/ValveBody", + 0.23, + 0.28, + (1.30, -0.62, 0.56), + materials["corrosion"], + axis="X", + collision=True, + ) + surface_source = create_irregular_cylindrical_surface_source( + stage, + f"{source_path}/AdheredSurfaceContamination", + center_world_m=(1.95, -0.62, 0.46), + radius_m=0.20, + height_m=0.76, + total_activity_bq=8.5e8, + ) + + return { + "robot_asset": assets_root + RIDGEBACK_FRANKA_ASSET, + "robot_path": MANIPULATOR_PATH, + "shield_path": SHIELD_PATH, + "contaminated_object_path": source_path, + "surface_source": surface_source, + "shield_mass_kg": 2.8, + } def build_route(grid: SurfaceSourceGrid) -> tuple[tuple[float, float, float], list[RouteSegment]]: @@ -478,18 +1621,36 @@ def build_route(grid: SurfaceSourceGrid) -> tuple[tuple[float, float, float], li segments: list[RouteSegment] = [] for row_index, row in enumerate(rows): if row_index % 2 == 0: - segments.append(RouteSegment((forward_end_x, float(row)), True, f"sealed wash lane {row_index + 1}")) + segments.append( + RouteSegment((forward_end_x, float(row)), True, f"sealed wash lane {row_index + 1}") + ) if row_index + 1 < len(rows): - segments.append(RouteSegment((reverse_start_x, float(rows[row_index + 1])), False, "head closed / reposition")) + segments.append( + RouteSegment( + (reverse_start_x, float(rows[row_index + 1])), + False, + "head closed / reposition", + ) + ) else: - segments.append(RouteSegment((reverse_end_x, float(row)), True, f"sealed wash lane {row_index + 1}")) + segments.append( + RouteSegment((reverse_end_x, float(row)), True, f"sealed wash lane {row_index + 1}") + ) if row_index + 1 < len(rows): - segments.append(RouteSegment((forward_start_x, float(rows[row_index + 1])), False, "head closed / reposition")) + segments.append( + RouteSegment( + (forward_start_x, float(rows[row_index + 1])), + False, + "head closed / reposition", + ) + ) return start, segments def transform_and_pose(stage) -> tuple[Gf.Matrix4d, tuple[float, float, float], float]: - transform = UsdGeom.Xformable(stage.GetPrimAtPath(ROBOT_PATH)).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + transform = UsdGeom.Xformable(stage.GetPrimAtPath(ROBOT_PATH)).ComputeLocalToWorldTransform( + Usd.TimeCode.Default() + ) position = transform.ExtractTranslation() x_axis = transform.TransformDir(Gf.Vec3d(1.0, 0.0, 0.0)).GetNormalized() yaw = math.atan2(x_axis[1], x_axis[0]) @@ -598,21 +1759,33 @@ def __init__(self, initial_activity_bq: float, pressure_mpa: float) -> None: def update(self, grid, auto, sealed, state, physical_elapsed_s, pressure_mpa) -> None: self.status.text = f"Phase: {auto.phase}" - self.seal.text = "Head seal: CLOSED | jet + vacuum ON" if sealed else "Head seal: OPEN | jet interlocked" + self.seal.text = ( + "Head seal: CLOSED | jet + vacuum ON" if sealed else "Head seal: OPEN | jet interlocked" + ) self.process.text = f"Process: {pressure_mpa:.0f} MPa | 450 mm internal nozzle scan" self.activity.text = f"Surface activity: {grid.total_activity_bq:,.0f} Bq" df = grid.initial_total_activity_bq / max(grid.total_activity_bq, 1e-12) self.df.text = f"Decontamination factor: {df:.2f}" self.treated.text = f"Treated area: {grid.treated_fraction * 100.0:.1f}%" - self.route.text = f"Route segment: {min(auto.index + 1, len(auto.route))} / {len(auto.route)}" - self.water.text = f"Water: {state.applied_water_l:.1f} L supplied / {state.recovered_water_l:.1f} L recovered" + self.route.text = ( + f"Route segment: {min(auto.index + 1, len(auto.route))} / {len(auto.route)}" + ) + self.water.text = ( + f"Water: {state.applied_water_l:.1f} L supplied / " + f"{state.recovered_water_l:.1f} L recovered" + ) recovery = state.recovered_water_l / max(state.applied_water_l, 1e-12) * 100.0 - self.recovery.text = f"Recovery: {recovery:.1f}% | discharge: {state.discharged_water_l:.1f} L" + self.recovery.text = ( + f"Recovery: {recovery:.1f}% | discharge: {state.discharged_water_l:.1f} L" + ) self.waste.text = f"Captured activity: {state.captured_activity_bq:,.0f} Bq" hours = int(physical_elapsed_s // 3600) minutes = int((physical_elapsed_s % 3600) // 60) seconds = int(physical_elapsed_s % 60) - self.clock.text = f"Plant operation time: {hours:02d}:{minutes:02d}:{seconds:02d} ({ARGS.time_scale:.0f}x view)" + self.clock.text = ( + f"Plant operation time: {hours:02d}:{minutes:02d}:{seconds:02d} " + f"({ARGS.time_scale:.0f}x view)" + ) self.progress_model.set_value(float(min(grid.removed_fraction, 1.0))) @@ -669,14 +1842,18 @@ def update_process_visuals(visuals: RobotVisuals, scan_y_m: float, sealed: bool) visibility = UsdGeom.Tokens.inherited if sealed else UsdGeom.Tokens.invisible for cone in visuals.spray_prims: cone.GetVisibilityAttr().Set(visibility) - visuals.seal_color_attr.Set([Gf.Vec3f(0.04, 0.86, 0.18) if sealed else Gf.Vec3f(0.88, 0.04, 0.02)]) + visuals.seal_color_attr.Set( + [Gf.Vec3f(0.04, 0.86, 0.18) if sealed else Gf.Vec3f(0.88, 0.04, 0.02)] + ) def update_surface_visuals(grid, water_process, color_attributes, activity_attributes) -> None: colors = water_process.visual_color_rgb() epoxy = np.asarray([0.19, 0.27, 0.25]) - colors = 0.68 * colors + 0.32 * epoxy - for index, (color_attr, activity_attr) in enumerate(zip(color_attributes, activity_attributes, strict=True)): + colors = 0.40 * colors + 0.60 * epoxy + for index, (color_attr, activity_attr) in enumerate( + zip(color_attributes, activity_attributes, strict=True) + ): color_attr.Set([Gf.Vec3f(*colors[index])]) activity_attr.Set(float(grid.activity_bq[index])) @@ -689,10 +1866,253 @@ def request_capture(path: Path) -> None: viewport_utility.capture_viewport_to_file(viewport, str(path)) +def _set_link_geometry(stage, path: str, start_xyz, end_xyz, width_m: float) -> None: + start = np.asarray(start_xyz, dtype=np.float64) + end = np.asarray(end_xyz, dtype=np.float64) + delta = end - start + length = float(np.linalg.norm(delta)) + if length <= 1e-9: + raise ValueError("animated link endpoints must be distinct") + horizontal = math.hypot(float(delta[0]), float(delta[1])) + rotation = Gf.Vec3f( + 0.0, + -math.degrees(math.atan2(float(delta[2]), horizontal)), + math.degrees(math.atan2(float(delta[1]), float(delta[0]))), + ) + xformable = UsdGeom.Xformable(stage.GetPrimAtPath(path)) + operations = xformable.GetOrderedXformOps() + if len(operations) != 3: + raise RuntimeError(f"expected translate/rotate/scale operations on {path}") + operations[0].Set(Gf.Vec3d(*(0.5 * (start + end)))) + operations[1].Set(rotation) + operations[2].Set(Gf.Vec3f(length, width_m, width_m)) + + +def _solve_fabrik_chain(base_xyz, target_xyz, lengths_m: tuple[float, ...]) -> np.ndarray: + """Solve a small fixed-length serial chain for the rendered distal arm.""" + + base = np.asarray(base_xyz, dtype=np.float64) + target = np.asarray(target_xyz, dtype=np.float64) + reach = float(sum(lengths_m)) + separation = float(np.linalg.norm(target - base)) + if separation >= reach: + direction = (target - base) / max(separation, 1e-12) + points = [base] + for length_m in lengths_m: + points.append(points[-1] + length_m * direction) + return np.asarray(points) + + interpolation = np.linspace(0.0, 1.0, len(lengths_m) + 1)[:, None] + bend = np.asarray((0.20, -0.10, 0.34), dtype=np.float64) + points = base + interpolation * (target - base) + points += np.sin(math.pi * interpolation) * bend + points[0] = base + points[-1] = target + for _ in range(24): + points[-1] = target + for index in range(len(lengths_m) - 1, -1, -1): + delta = points[index] - points[index + 1] + distance = float(np.linalg.norm(delta)) + points[index] = points[index + 1] + lengths_m[index] * delta / max(distance, 1e-12) + points[0] = base + for index, length_m in enumerate(lengths_m): + delta = points[index + 1] - points[index] + distance = float(np.linalg.norm(delta)) + points[index + 1] = points[index] + length_m * delta / max(distance, 1e-12) + if float(np.linalg.norm(points[-1] - target)) <= 1e-5: + break + return points + + +def _animate_high_reach_arm(stage, target_local_xyz: tuple[float, float, float]) -> None: + base = (0.0, 4.12, 9.40) + lengths = (0.30, 0.28, 0.26, 0.24, 0.22, 0.20, 0.18) + points = _solve_fabrik_chain(base, target_local_xyz, lengths) + for index, (start, end) in enumerate(zip(points[:-1], points[1:], strict=True), start=1): + _set_link_geometry( + stage, + f"{HIGH_REACH_ROBOT_PATH}/DistalManipulator/Link{index}", + start, + end, + 0.148 - 0.008 * index, + ) + joint = UsdGeom.Xformable( + stage.GetPrimAtPath(f"{HIGH_REACH_ROBOT_PATH}/DistalManipulator/Joint{index}") + ) + joint.GetOrderedXformOps()[0].Set(Gf.Vec3d(*end)) + + head = UsdGeom.Xformable(stage.GetPrimAtPath(f"{HIGH_REACH_ROBOT_PATH}/DeconHead")) + operations = head.GetOrderedXformOps() + translate = operations[0] if operations else head.AddTranslateOp() + initial = np.asarray((0.0, 4.96, 9.38), dtype=np.float64) + translate.Set(Gf.Vec3d(*(np.asarray(target_local_xyz) - initial))) + + +def _high_reach_scan_target(video_time_s: float, duration_s: float) -> tuple[float, float, float]: + lead_s = min(1.5, duration_s * 0.10) + tail_s = min(2.0, duration_s * 0.12) + scan_duration_s = max(duration_s - lead_s - tail_s, 1e-6) + if video_time_s < lead_s: + progress = 0.0 + elif video_time_s >= duration_s - tail_s: + progress = 1.0 + else: + progress = (video_time_s - lead_s) / scan_duration_s + rows = 6 + row_position = min(progress, 1.0 - 1e-9) * rows + row = min(int(row_position), rows - 1) + phase = row_position - row + if row % 2: + phase = 1.0 - phase + x_m = -1.05 + 2.10 * phase + z_m = 8.70 + row * (1.32 / (rows - 1)) + return (x_m, 4.96, z_m) + + +def _update_high_wall_visuals(stage, grid: SurfaceSourceGrid) -> None: + concrete = np.asarray([0.38, 0.40, 0.39]) + fraction = np.divide( + grid.activity_bq, + grid.initial_activity_bq, + out=np.zeros_like(grid.activity_bq), + where=grid.initial_activity_bq > 0.0, + ) + overlay_colors = 0.38 * grid.color_rgb() + 0.62 * concrete + colors = concrete + fraction[:, None] * (overlay_colors - concrete) + for index in np.flatnonzero(grid.initial_activity_bq > 0.0): + prim = stage.GetPrimAtPath(f"{HIGH_WALL_SOURCE_PATH}/Cell_{index:04d}") + if not prim.IsValid(): + continue + UsdGeom.Imageable(prim).GetVisibilityAttr().Set( + UsdGeom.Tokens.invisible if fraction[index] < 0.10 else UsdGeom.Tokens.inherited + ) + UsdGeom.Gprim(prim).GetDisplayColorAttr().Set([Gf.Vec3f(*colors[index])]) + prim.GetAttribute("rad:source:activityBq").Set(float(grid.activity_bq[index])) + + +def render_high_reach_decontamination_video(world, stage, grid: SurfaceSourceGrid) -> dict: + duration_s = float(ARGS.video_seconds) + fps = int(ARGS.video_fps) + frame_count = int(round(duration_s * fps)) + frame_dir = ARGS.output / ".high_reach_video_frames" + frame_dir.mkdir(parents=True, exist_ok=True) + for stale_frame in frame_dir.glob("frame_*.png"): + stale_frame.unlink() + video_path = ARGS.output / "high_reach_wall_decontamination_20s.mp4" + video_path.unlink(missing_ok=True) + + set_camera_view( + eye=np.asarray([-7.25, -12.8, 7.35]), + target=np.asarray([0.0, 3.55, 5.50]), + camera_prim_path="/OmniverseKit_Persp", + ) + for _ in range(90): + world.step(render=True) + + tool = DecontaminationTool( + length_m=0.72, + width_m=0.58, + rate_constant_s_inv=0.48, + max_contact_distance_m=0.09, + max_surface_speed_m_s=3.0, + ) + initial_activity_bq = grid.total_activity_bq + previous_target = np.asarray(_high_reach_scan_target(0.0, duration_s), dtype=np.float64) + active_start_s = min(1.5, duration_s * 0.10) + active_end_s = duration_s - min(2.0, duration_s * 0.12) + for frame_index in range(frame_count): + video_time_s = frame_index / fps + target_local = np.asarray( + _high_reach_scan_target(video_time_s, duration_s), dtype=np.float64 + ) + _animate_high_reach_arm(stage, tuple(float(value) for value in target_local)) + if active_start_s <= video_time_s < active_end_s: + speed_m_s = float(np.linalg.norm(target_local - previous_target) * fps) + grid.apply_tool( + tool, + tool_center_world_m=( + float(target_local[0]), + float(target_local[1] + 0.70), + float(target_local[2]), + ), + tool_yaw_rad=0.0, + surface_speed_m_s=speed_m_s, + dt_s=5.0 / fps, + ) + _update_high_wall_visuals(stage, grid) + previous_target = target_local + world.step(render=True) + request_capture(frame_dir / f"frame_{frame_index:04d}.png") + world.step(render=True) + if frame_index % fps == 0: + print( + f"HIGH_REACH_VIDEO t={video_time_s:.1f}s " + f"removed={grid.removed_fraction * 100.0:.1f}%", + flush=True, + ) + + for _ in range(30): + world.step(render=True) + capture_deadline = time.monotonic() + 5.0 + rendered_frames = len(list(frame_dir.glob("frame_*.png"))) + while rendered_frames < frame_count and time.monotonic() < capture_deadline: + world.step(render=True) + time.sleep(0.02) + rendered_frames = len(list(frame_dir.glob("frame_*.png"))) + if rendered_frames != frame_count: + raise RuntimeError(f"expected {frame_count} video frames, found {rendered_frames}") + subprocess.run( + [ + "ffmpeg", + "-y", + "-loglevel", + "error", + "-framerate", + str(fps), + "-i", + str(frame_dir / "frame_%04d.png"), + "-vf", + "crop=1440:810:0:45,scale=1280:720,format=yuv420p", + "-r", + "30", + "-c:v", + "libx264", + "-preset", + "slow", + "-crf", + "18", + "-movflags", + "+faststart", + str(video_path), + ], + check=True, + ) + for rendered_frame in frame_dir.glob("frame_*.png"): + rendered_frame.unlink() + frame_dir.rmdir() + return { + "passed": True, + "video": str(video_path), + "duration_s": duration_s, + "capture_fps": fps, + "encoded_fps": 30, + "resolution": [1280, 720], + "frames": frame_count, + "initial_activity_bq": initial_activity_bq, + "final_activity_bq": grid.total_activity_bq, + "removed_fraction": grid.removed_fraction, + "surface_source": HIGH_WALL_SOURCE_PATH, + "decontamination_model": "contact-footprint cumulative-exposure decay", + } + + def main() -> int: ARGS.output.mkdir(parents=True, exist_ok=True) - result_path = ARGS.output / "result.json" + result_path = ARGS.output / ("video_result.json" if ARGS.render_video else "result.json") before_path = ARGS.output / "before.png" + tool_detail_path = ARGS.output / "decontamination_head_detail.png" + high_reach_path = ARGS.output / "high_reach_decontamination.png" + shield_manipulator_path = ARGS.output / "shield_manipulator.png" during_path = ARGS.output / "during.png" after_path = ARGS.output / "after.png" result: dict[str, object] = {"passed": False} @@ -702,16 +2122,89 @@ def main() -> int: world = World(stage_units_in_meters=1.0, physics_dt=DT_S, rendering_dt=DT_S) stage = omni.usd.get_context().get_stage() materials = create_materials(stage) - roller_points = create_environment(stage, materials) - grid, color_attributes, activity_attributes = create_surface_source(stage) - create_work_zone(stage, materials, grid) - start_xyz, route = build_route(grid) - visuals = create_robot(stage, materials, start_xyz) + high_reach_render = ARGS.render_video or ( + ARGS.render_only and ARGS.render_scene == "decontamination" + ) + roller_points = create_environment(stage, materials, high_bay=high_reach_render) dome = UsdLux.DomeLight.Define(stage, "/World/ReactorBuilding/Ambient") dome.CreateIntensityAttr(340.0) dome.CreateColorAttr(Gf.Vec3f(0.56, 0.66, 0.76)) + if high_reach_render: + grid = create_high_wall_surface_source(stage) + robot_manifest = create_high_reach_robot(stage, materials) + world.reset() + if ARGS.render_video: + video_result = render_high_reach_decontamination_video(world, stage, grid) + result = { + **video_result, + "scene": "13 m reactor-building high-wall decontamination", + "reference_robot": "RadCounter HighReach-10 research design", + **robot_manifest, + } + print("HIGH_REACH_VIDEO_RESULT " + json.dumps(result), flush=True) + return 0 + set_camera_view( + eye=np.asarray([-8.0, -15.2, 7.1]), + target=np.asarray([0.0, 3.00, 4.85]), + camera_prim_path="/OmniverseKit_Persp", + ) + for _ in range(120): + world.step(render=True) + request_capture(high_reach_path) + for _ in range(30): + world.step(render=True) + result = { + "passed": True, + "render_only": True, + "scene": "13 m reactor-building high-bay decontamination", + "reference_robot": "RadCounter HighReach-10 research design", + "design_disclosure": ( + "six-stage 10 m research extension of the documented 8 m " + "MHI SUPER-Giraffe architecture" + ), + **robot_manifest, + "surface_source": HIGH_WALL_SOURCE_PATH, + "surface_orientation": "vertical wall", + "active_surface_cells": int(np.count_nonzero(grid.activity_bq)), + "maximum_contamination_height_m": 10.45, + "image": str(high_reach_path), + } + print("HIGH_REACH_RENDER_RESULT " + json.dumps(result), flush=True) + return 0 + + if ARGS.render_scene == "shield-manipulation": + if not ARGS.render_only: + raise ValueError("shield-manipulation is a render-only scene") + scene_manifest = create_shield_manipulation_scene(stage, materials) + world.reset() + set_camera_view( + eye=np.asarray([4.35, -5.35, 2.65]), + target=np.asarray([0.70, -0.55, 0.57]), + camera_prim_path="/OmniverseKit_Persp", + ) + for _ in range(150): + world.step(render=True) + request_capture(shield_manipulator_path) + for _ in range(30): + world.step(render=True) + result = { + "passed": True, + "render_only": True, + "scene": "reactor-building manipulator shield placement", + "reference_robot": "Clearpath Ridgeback + Franka Emika Panda", + "reference_urls": [RIDGEBACK_FRANKA_REFERENCE], + **scene_manifest, + "image": str(shield_manipulator_path), + } + print("SHIELD_MANIPULATOR_RENDER_RESULT " + json.dumps(result), flush=True) + return 0 + + grid, color_attributes, activity_attributes = create_surface_source(stage) + start_xyz, route = build_route(grid) + visuals = create_robot(stage, materials, start_xyz) + world.reset() controller = RigidArounderController(stage) auto = ArounderAutoController(stage, route, ARGS.time_scale) @@ -721,7 +2214,9 @@ def main() -> int: pressure_mpa, _coefficient = operation_parameters(ARGS.operation_mode) panel = ArounderWindow(grid.initial_total_activity_bq, pressure_mpa) - water_state = WaterDecontaminationState(supply_remaining_l=1000.0, wastewater_capacity_l=1000.0) + water_state = WaterDecontaminationState( + supply_remaining_l=1000.0, wastewater_capacity_l=1000.0 + ) water_process = WaterSurfaceDecontaminator( grid, water_state, @@ -749,6 +2244,47 @@ def main() -> int: for _ in range(20): world.step(render=True) + set_camera_view( + eye=np.asarray([-0.10, -2.30, 1.55]), + target=np.asarray([-1.12, -0.58, 0.20]), + camera_prim_path="/OmniverseKit_Persp", + ) + for _ in range(35): + world.step(render=True) + request_capture(tool_detail_path) + for _ in range(20): + world.step(render=True) + + if ARGS.render_only: + result = { + "passed": True, + "render_only": True, + "reference_robot": ( + "Hitachi-GE / IRID Arounder low-section high-pressure-water " + "decontamination machine" + ), + "reference_urls": [HITACHI_REFERENCE, IRID_REFERENCE], + "scene": "reactor-building floor decontamination", + "surface_source": SOURCE_PATH, + "surface_cells": len(grid.activity_bq), + "active_surface_cells": int(np.count_nonzero(grid.activity_bq)), + "surface_shape": "irregular spill/runoff field with detached droplets", + "rectangular_work_zone_frame": False, + "initial_activity_bq": grid.initial_total_activity_bq, + "overview_image": str(before_path), + "decontamination_head_detail_image": str(tool_detail_path), + } + print("AROUNDER_RENDER_RESULT " + json.dumps(result), flush=True) + return 0 + + set_camera_view( + eye=np.asarray([6.8, -7.6, 5.15]), + target=np.asarray([0.15, 0.15, 0.48]), + camera_prim_path="/OmniverseKit_Persp", + ) + for _ in range(20): + world.step(render=True) + initial_activity = grid.total_activity_bq start_position = transform_and_pose(stage)[1] previous_position = start_position @@ -773,7 +2309,9 @@ def main() -> int: path_length_m += frame_distance scan_phase = 2.0 * math.pi * 0.52 * elapsed scan_y_m = 0.5 * HEAD_STROKE_M * math.sin(scan_phase) - scan_visual_speed_m_s = abs(0.5 * HEAD_STROKE_M * 2.0 * math.pi * 0.52 * math.cos(scan_phase)) + scan_visual_speed_m_s = abs( + 0.5 * HEAD_STROKE_M * 2.0 * math.pi * 0.52 * math.cos(scan_phase) + ) brush_point = transform.Transform(Gf.Vec3d(HEAD_X_M, scan_y_m, HEAD_BRUSH_Z_M)) seal_gap_m = abs(float(brush_point[2]) - float(grid.center_world_m[2])) sealed = auto.water_requested and seal_gap_m <= 0.055 @@ -827,9 +2365,11 @@ def main() -> int: print( f"AROUNDER_TELEMETRY t={elapsed:.1f} plant_t={physical_elapsed_s:.0f} " f"phase={auto.phase!r} seal={sealed} pressure_mpa={pressure_mpa:.0f} " - f"activity_bq={grid.total_activity_bq:.0f} df={initial_activity / max(grid.total_activity_bq, 1e-12):.2f} " + f"activity_bq={grid.total_activity_bq:.0f} " + f"df={initial_activity / max(grid.total_activity_bq, 1e-12):.2f} " f"treated_pct={grid.treated_fraction * 100.0:.1f} path_m={path_length_m:.2f} " - f"water_l={water_state.applied_water_l:.1f} recovery_pct={recovery * 100.0:.1f}", + f"water_l={water_state.applied_water_l:.1f} " + f"recovery_pct={recovery * 100.0:.1f}", flush=True, ) frame_index += 1 @@ -859,7 +2399,9 @@ def main() -> int: ) result = { "passed": passed, - "reference_robot": "Hitachi-GE / IRID Arounder low-section high-pressure-water decontamination machine", + "reference_robot": ( + "Hitachi-GE / IRID Arounder low-section high-pressure-water decontamination machine" + ), "reference_urls": [HITACHI_REFERENCE, IRID_REFERENCE], "isaac_sim_target": "6.0.1", "operation_mode": ARGS.operation_mode, diff --git a/scripts/run_app.sh b/scripts/run_app.sh new file mode 100755 index 0000000..f8f6cd7 --- /dev/null +++ b/scripts/run_app.sh @@ -0,0 +1,70 @@ +#!/usr/bin/env bash +set -euo pipefail + +repository_root="${RADCOUNTER_APP_ROOT:-$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)}" +export RADCOUNTER_HOST_ENV_NO_ROS=1 +source "$repository_root/scripts/host_env.sh" + +if ! command -v uv >/dev/null 2>&1; then + printf '%s\n' "RadCounterSim requires uv: https://docs.astral.sh/uv/" >&2 + exit 2 +fi +if [[ ! -f "$RADCOUNTER_ISAAC_ROOT/uv.lock" ]]; then + printf '%s\n' "Isaac Sim 6.0.1 was not found at: $RADCOUNTER_ISAAC_ROOT" >&2 + printf '%s\n' "Install your own Isaac Sim copy or set RADCOUNTER_ISAAC_ROOT." >&2 + exit 2 +fi + +eula_marker="$RADCOUNTER_ISAAC_ROOT/.venv/lib/python3.12/site-packages/isaacsim/kit/EULA_ACCEPTED" +if [[ "${OMNI_KIT_ACCEPT_EULA:-}" != "YES" && ! -f "$eula_marker" ]]; then + printf '%s\n' "Review and accept NVIDIA's Omniverse EULA before first launch." >&2 + printf '%s\n' "Then launch once with OMNI_KIT_ACCEPT_EULA=YES." >&2 + exit 2 +fi + +runtime_root="${RADCOUNTER_LLM_RUNTIME_DIR:-$repository_root/runtime/llm}" +model_path="${RADCOUNTER_LLM_MODEL:-$runtime_root/models/Qwen3-4B-Q4_K_M.gguf}" +llm_pid_file="$runtime_root/logs/llama-server-launcher-$$.pid" +export RADCOUNTER_LLM_PID_FILE="$llm_pid_file" +if [[ -z "${RADCOUNTER_LLM_ENDPOINT:-}" ]]; then + if [[ ! -f "$model_path" ]]; then + printf '%s\n' "The local command model is not installed: $model_path" >&2 + printf '%s\n' "Run: uv run python scripts/fetch_llm_model.py" >&2 + exit 2 + fi + if [[ ! -x "$runtime_root/bin/linux-x86_64-cpu/llama-server" && \ + ! -x "$runtime_root/bin/linux-x86_64-vulkan/llama-server" && \ + ! -x "$runtime_root/bin/linux-x86_64-cuda/llama-server" && \ + -z "${RADCOUNTER_LLAMA_SERVER:-}" ]]; then + printf '%s\n' "The bundled llama.cpp runtime is not installed." >&2 + printf '%s\n' "Run: ./scripts/build_llama_runtime.sh" >&2 + exit 2 + fi +fi + +application_pid="" +cleanup_application() { + trap - INT TERM HUP + if [[ -n "$application_pid" ]] && kill -0 "$application_pid" 2>/dev/null; then + kill -TERM "$application_pid" 2>/dev/null || true + fi + if [[ -f "$llm_pid_file" ]]; then + read -r llm_pid < "$llm_pid_file" || true + if [[ "$llm_pid" =~ ^[0-9]+$ && -r "/proc/$llm_pid/cmdline" ]]; then + llm_command="$(tr '\0' ' ' < "/proc/$llm_pid/cmdline")" + if [[ "$llm_command" == *llama-server* ]]; then + kill -TERM "$llm_pid" 2>/dev/null || true + fi + fi + rm -f "$llm_pid_file" + fi +} +trap cleanup_application EXIT INT TERM HUP + +uv run --project "$RADCOUNTER_ISAAC_ROOT" --locked \ + python "$repository_root/scripts/run_gui.py" "$@" & +application_pid="$!" +wait "$application_pid" +application_status="$?" +application_pid="" +exit "$application_status" diff --git a/scripts/run_gui.py b/scripts/run_gui.py index c08edfe..371c87e 100644 --- a/scripts/run_gui.py +++ b/scripts/run_gui.py @@ -1,16 +1,19 @@ -"""Isaac Sim launcher guard.""" +#!/usr/bin/env python3 +"""Launch the complete articulated RadCounterSim GUI workflow.""" +from __future__ import annotations -def main() -> int: - try: - import isaacsim # type: ignore[import-not-found] # noqa: F401 - except ModuleNotFoundError as exc: - raise SystemExit( - "Isaac Sim 6.0.1 is not available. " - "Launch this script with the Isaac Sim Python runtime." - ) from exc - raise SystemExit("Merge the local official Isaac Sim 6.0.1 UI template before GUI use.") +import sys +from pathlib import Path +from run_gui_validation import main if __name__ == "__main__": - raise SystemExit(main()) + root = Path(__file__).resolve().parents[1] + arguments = [ + "--interactive", + "--artifact", + str(root / "artifacts/app/latest.json"), + *sys.argv[1:], + ] + raise SystemExit(main(arguments)) diff --git a/scripts/run_gui_validation.py b/scripts/run_gui_validation.py index 0c662a0..177e34c 100644 --- a/scripts/run_gui_validation.py +++ b/scripts/run_gui_validation.py @@ -4,18 +4,19 @@ from __future__ import annotations import argparse -import hashlib +import asyncio import importlib import json -import shutil +import math import sys import time import traceback -from collections.abc import Iterable, Mapping +from collections.abc import Callable, Iterable, Mapping from dataclasses import asdict, is_dataclass, replace from enum import Enum from pathlib import Path from typing import Any +from urllib.parse import urlparse import numpy as np @@ -31,19 +32,132 @@ _radcounter_package.__path__.append(_extension_namespace) -def _arguments() -> argparse.Namespace: +DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION = ( + "Decontaminate the entire irregular wall-mounted Cs-137 surface source with a " + "serpentine pass until the remaining fraction is no more than 60%, using at most " + "3 passes. Then place the same primary lead shield, LeadShield, at " + "25% of the source-to-protected-area line and move that same shield to 65%. " + "Move to the Protected measurement station, measure for 2 seconds, return the " + "measurement robot to its starting position, and finally show the current status." +) +DEFAULT_GUI_MAX_FPS = 60.0 + + +class ComplexNaturalLanguageValidationError(RuntimeError): + """Preserve a failed complex-run audit in the top-level JSON artifact.""" + + def __init__(self, message: str, audit: Mapping[str, Any]) -> None: + super().__init__(message) + self.audit = dict(audit) + + +class DecontaminationSmokeValidationError(RuntimeError): + """Preserve contact and motion evidence when the CAD smoke gate fails.""" + + def __init__(self, message: str, audit: Mapping[str, Any]) -> None: + super().__init__(message) + self.audit = dict(audit) + + +def _arguments(argv: list[str] | None = None) -> argparse.Namespace: parser = argparse.ArgumentParser() + parser.add_argument("--interactive", action=argparse.BooleanOptionalAction, default=False) + parser.add_argument("--initial-command") + parser.add_argument( + "--confirm-initial-command", + action=argparse.BooleanOptionalAction, + default=False, + ) + parser.add_argument( + "--complex-natural-language-validation", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "run the visible, locally interpreted multi-pass decontamination and " + "same-shield repositioning gate" + ), + ) + parser.add_argument( + "--natural-language-timeout-s", + type=float, + default=1800.0, + help="wall-clock timeout for the bounded complex natural-language workflow", + ) parser.add_argument("--keep-open", action=argparse.BooleanOptionalAction, default=True) parser.add_argument("--headless", action=argparse.BooleanOptionalAction, default=False) parser.add_argument("--phase-hold-s", type=float, default=0.35) parser.add_argument("--frame-delay-s", type=float, default=0.0) + parser.add_argument( + "--max-fps", + type=float, + default=DEFAULT_GUI_MAX_FPS, + help="cap the open GUI refresh rate; use 0 to disable the limit", + ) parser.add_argument("--decon-duration-s", type=float, default=1.5) + parser.add_argument("--system-catalog", type=Path) + parser.add_argument("--profile") + parser.add_argument("--environment") + parser.add_argument("--robot-set") + parser.add_argument("--detector-set") + parser.add_argument("--selection-file", type=Path) + parser.add_argument( + "--robot-monitor-smoke-test", + action=argparse.BooleanOptionalAction, + default=False, + help="exercise follow, onboard, work, overview, and manual-cancel camera modes", + ) + parser.add_argument( + "--decontamination-smoke-test", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "compose the articulated decontamination task on the selected " + "environment and execute one contact-verified raster" + ), + ) parser.add_argument( "--artifact", type=Path, default=ROOT / "artifacts/gui-validation/latest.json", ) - return parser.parse_args() + arguments = parser.parse_args(argv) + if arguments.interactive and arguments.complex_natural_language_validation: + parser.error("--interactive and --complex-natural-language-validation are separate modes") + if arguments.complex_natural_language_validation and arguments.headless: + parser.error( + "--complex-natural-language-validation requires a visible GUI; remove --headless" + ) + if arguments.natural_language_timeout_s <= 0.0: + parser.error("--natural-language-timeout-s must be positive") + if not math.isfinite(arguments.max_fps) or arguments.max_fps < 0.0: + parser.error("--max-fps must be a finite non-negative number") + return arguments + + +def _selected_system(args: argparse.Namespace): + from radcounter.core.system_profiles import ( + load_active_selection, + resolve_system_selection, + ) + + explicit = any( + ( + args.system_catalog, + args.profile, + args.environment, + args.robot_set, + args.detector_set, + ) + ) + if not explicit: + return load_active_selection(args.selection_file) + return resolve_system_selection( + catalog_path=args.system_catalog, + profile_id=args.profile, + environment_id=args.environment, + robot_set_id=args.robot_set, + detector_set_id=args.detector_set, + ) def _jsonable(value: Any) -> Any: @@ -84,18 +198,67 @@ def _complete(coroutine: Any) -> Any: raise RuntimeError("workflow coroutine unexpectedly yielded to the Kit event loop") +def _complete_with_updates( + app: Any, + coroutine: Any, + *, + before_update: Any | None = None, + timeout_s: float = 180.0, +) -> Any: + """Drive Kit's event loop until an asynchronous application command completes.""" + + task = asyncio.ensure_future(coroutine) + deadline = time.monotonic() + timeout_s + while not task.done() and time.monotonic() < deadline: + if before_update is not None: + before_update() + app.update() + time.sleep(0.01) + if not task.done(): + task.cancel() + raise TimeoutError("natural-language application command timed out") + return task.result() + + class _GuiStepper: - def __init__(self, app: Any, frame_delay_s: float) -> None: + def __init__(self, app: Any, frame_delay_s: float, world: Any | None = None) -> None: self.app = app + self.world = world self.frame_delay_s = max(0.0, frame_delay_s) def step(self, *, render: bool = False) -> None: - del render - self.app.update() + if self.world is None: + self.app.update() + else: + self.world.step(render=render) if self.frame_delay_s: time.sleep(self.frame_delay_s) +class _GuiFrameRateLimiter: + """Bound the persistent GUI loop without slowing validation physics.""" + + def __init__( + self, + max_fps: float, + *, + clock: Callable[[], float] = time.perf_counter, + sleeper: Callable[[float], None] = time.sleep, + ) -> None: + self._frame_period_s = 0.0 if max_fps == 0.0 else 1.0 / max_fps + self._clock = clock + self._sleeper = sleeper + self._previous_frame_s = clock() + + def wait(self) -> None: + if self._frame_period_s == 0.0: + return + remaining_s = self._frame_period_s - (self._clock() - self._previous_frame_s) + if remaining_s > 0.0: + self._sleeper(remaining_s) + self._previous_frame_s = self._clock() + + class _ValidationPanel: def __init__(self) -> None: import omni.ui as ui @@ -105,8 +268,21 @@ def __init__(self) -> None: self.progress = ui.SimpleStringModel("0 / 9 operations") self.result = ui.SimpleStringModel("No operation has completed yet") self.audit = ui.SimpleStringModel("Validation is running") - self.window = ui.Window("RadCounterSim Full Validation", width=520, height=690) + self.window = ui.Window( + "RadCounterSim Full Validation", + width=520, + height=690, + dockPreference=ui.DockPreference.RIGHT, + ) self.window.frame.set_build_fn(self._build) + # Keep both RadCounterSim panels together from startup. Operations is + # the normal operator-facing tab, so leave it selected after docking. + target_active = getattr( + ui.DockPolicy, + "TARGET_WINDOW_IS_ACTIVE", + ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE, + ) + self.window.deferred_dock_in("RadCounterSim Operations", target_active) def _build(self) -> None: ui = self._ui @@ -148,6 +324,9 @@ def finish(self, success: bool, artifact: Path) -> None: else: self.audit.set_value(f"FAIL: inspect the latest result and {artifact}") + def hide(self) -> None: + self.window.visible = False + def _hold(app: Any, duration_s: float) -> None: deadline = time.monotonic() + max(0.0, duration_s) @@ -165,114 +344,24 @@ def _world_position(stage: Any, prim_path: str) -> np.ndarray: return np.asarray(transform.Transform(Gf.Vec3d()), dtype=np.float64) -def _surface_center(stage: Any, prim_path: str) -> np.ndarray: - from pxr import Gf, UsdGeom - - prim = stage.GetPrimAtPath(prim_path) - mesh = UsdGeom.Mesh(prim) - points = mesh.GetPointsAttr().Get() or [] - if not points: - raise RuntimeError(f"decontamination surface has no vertices: {prim_path}") - transform = UsdGeom.XformCache().GetLocalToWorldTransform(prim) - vertices = np.asarray( - [transform.Transform(Gf.Vec3d(point)) for point in points], dtype=np.float64 - ) - return np.asarray( - ( - float(np.mean(vertices[:, 0])), - float(np.mean(vertices[:, 1])), - float(np.max(vertices[:, 2])), - ) - ) - - -def _position_contact_tool( - stage: Any, - app: Any, - surface_position_m: np.ndarray, - *, - standoff_m: float = 0.025, - frames: int = 24, -) -> None: - from pxr import Gf, UsdGeom - - robot = stage.GetPrimAtPath("/World/CountermeasureRobot") - tool = stage.GetPrimAtPath("/World/CountermeasureRobot/DeconContactTool") - robot_world = UsdGeom.XformCache().GetLocalToWorldTransform(robot) - target_world = np.asarray(surface_position_m, dtype=np.float64).copy() - target_world[2] += standoff_m - target_local = np.asarray( - robot_world.GetInverse().Transform(Gf.Vec3d(*map(float, target_world))), - dtype=np.float64, - ) - translate = tool.GetAttribute("xformOp:translate") - start_local = np.asarray(translate.Get(), dtype=np.float64) - for fraction in np.linspace(0.0, 1.0, max(frames, 2)): - position = (1.0 - fraction) * start_local + fraction * target_local - translate.Set(Gf.Vec3d(*map(float, position))) - app.update() - - # The parent base can continue settling while the child tool is animated. - # Resolve the exact local contact pose against the final parent transform. - final_robot_world = UsdGeom.XformCache().GetLocalToWorldTransform(robot) - final_target_local = final_robot_world.GetInverse().Transform( - Gf.Vec3d(*map(float, target_world)) - ) - translate.Set(Gf.Vec3d(*map(float, final_target_local))) - - -def _probe_decon_contacts(decontaminator: Any) -> dict[str, Any]: - import carb - - samples, center, axis = decontaminator._tool_samples() - hits = [] - for origin in samples: - hit = decontaminator._query.raycast_closest( - carb.Float3(*map(float, origin)), - carb.Float3(*map(float, axis)), - decontaminator.config.max_contact_distance_m, - True, - ) - normal = hit.get("normal", (0.0, 0.0, 0.0)) - hits.append( - { - "origin_m": origin.tolist(), - "hit": bool(hit.get("hit", False)), - "collision": str(hit.get("collision", "")), - "distance_m": float(hit.get("distance", -1.0)), - "face_index": int(hit.get("faceIndex", -1)), - "normal": [float(normal[index]) for index in range(3)], - } - ) - return {"center_m": center.tolist(), "axis": axis.tolist(), "hits": hits} - - def _path_blockers( probe: Any, start_m: np.ndarray, target_m: np.ndarray, *, excluded_paths: tuple[str, ...] = (), + moving_robot_path: str | None = None, ) -> list[str]: start = np.asarray(start_m, dtype=np.float64) target = np.asarray(target_m, dtype=np.float64) distance = float(np.linalg.norm(target[:2] - start[:2])) samples = np.linspace(start, target, max(2, int(np.ceil(distance / 0.12)) + 1)) samples[:, 2] = np.maximum(samples[:, 2], 0.32) - ignored = ( - probe.config.countermeasure_robot_path, - probe.config.measurement_robot_path, - *excluded_paths, - ) - clearance = probe.config.mobile_clearance_m blockers: list[str] = [] - for path, lower, upper in probe._collision_bounds(): - if any(probe._is_descendant(path, item) for item in ignored): - continue - if upper[2] <= 0.08: - continue - expanded_lower = lower - np.asarray((clearance, clearance, 0.05)) - expanded_upper = upper + np.asarray((clearance, clearance, 0.05)) + for path, expanded_lower, expanded_upper in probe._navigation_obstacles( + excluded_paths=excluded_paths, + moving_robot_path=moving_robot_path, + ): inside = np.all((samples >= expanded_lower) & (samples <= expanded_upper), axis=1) if bool(np.any(inside)): blockers.append(path) @@ -286,10 +375,12 @@ def _configure_camera(stage: Any) -> None: path = "/World/ValidationCamera" camera = UsdGeom.Camera.Define(stage, path) - camera.CreateFocalLengthAttr(24.0) + camera.CreateFocalLengthAttr(22.0) view = Gf.Matrix4d().SetLookAt( - Gf.Vec3d(10.5, -13.5, 10.0), - Gf.Vec3d(0.0, 0.0, 0.5), + # Frame the original cell, connecting corridor, and separate + # wall-decontamination room in one high oblique facility view. + Gf.Vec3d(18.5, -19.5, 21.0), + Gf.Vec3d(4.5, 0.0, 0.5), Gf.Vec3d(0.0, 0.0, 1.0), ) UsdGeom.Xformable(camera).MakeMatrixXform().Set(view.GetInverse()) @@ -300,38 +391,1234 @@ def _configure_camera(stage: Any) -> None: print(f"camera configuration warning: {type(error).__name__}: {error}", flush=True) -def _prepare_activity_copy(stage: Any, artifact_root: Path) -> tuple[Path, float]: - source = ROOT / "assets/contaminated_objects/floor_activity.npz" - destination = artifact_root / "runtime_floor_activity.npz" - destination.parent.mkdir(parents=True, exist_ok=True) - shutil.copy2(source, destination) - digest = hashlib.sha256(destination.read_bytes()).hexdigest() - surface = stage.GetPrimAtPath("/World/ContaminatedFloor") - for name in ("rad:source:activityMapUri", "rad:decon:activityMapUri"): - surface.GetAttribute(name).Set(str(destination)) - for name in ("rad:source:activityMapSha256", "rad:decon:activityMapSha256"): - surface.GetAttribute(name).Set(digest) - with np.load(destination, allow_pickle=False) as payload: - activity = float(np.sum(payload["activity_bq"])) - return destination, activity +def _configure_decon_room_camera( + stage: Any, + *, + eye_m: tuple[float, float, float] = (10.25, -2.10, 2.75), + target_m: tuple[float, float, float] = (14.30, 0.80, 1.12), +) -> None: + """Leave interactive runs focused on the irregular source and real tool.""" + + try: + from omni.kit.viewport.utility import get_active_viewport + from pxr import Gf, UsdGeom + + path = "/World/DeconRoomInspectionCamera" + camera = UsdGeom.Camera.Define(stage, path) + camera.CreateFocalLengthAttr(25.0) + view = Gf.Matrix4d().SetLookAt( + Gf.Vec3d(*eye_m), + Gf.Vec3d(*target_m), + Gf.Vec3d(0.0, 0.0, 1.0), + ) + UsdGeom.Xformable(camera).MakeMatrixXform().Set(view.GetInverse()) + viewport = get_active_viewport() + if viewport is not None: + viewport.set_active_camera(path) + except Exception as error: + print(f"inspection camera warning: {type(error).__name__}: {error}", flush=True) + + +def _configure_complex_facility_camera(stage: Any) -> None: + """Frame all four connected rooms and the shield-staging branch.""" + + try: + from omni.kit.viewport.utility import get_active_viewport + from pxr import Gf, UsdGeom + + path = "/World/ComplexFacilityValidationCamera" + camera = UsdGeom.Camera.Define(stage, path) + camera.CreateFocalLengthAttr(21.0) + view = Gf.Matrix4d().SetLookAt( + Gf.Vec3d(28.0, -25.0, 28.0), + Gf.Vec3d(8.0, 3.0, 0.55), + Gf.Vec3d(0.0, 0.0, 1.0), + ) + UsdGeom.Xformable(camera).MakeMatrixXform().Set(view.GetInverse()) + viewport = get_active_viewport() + if viewport is not None: + viewport.set_active_camera(path) + except Exception as error: + print(f"complex facility camera warning: {type(error).__name__}: {error}", flush=True) + + +def _activity_total(path: Path) -> float: + with np.load(path, allow_pickle=False) as payload: + return float(np.sum(payload["activity_bq"])) def _measurement_rows(items: Iterable[Any]) -> list[dict[str, Any]]: return [_jsonable(item) for item in items] -def _run_validation(app: Any, args: argparse.Namespace, panel: _ValidationPanel) -> dict: - import omni.timeline +def _attribute_value(prim: Any, name: str, default: Any = None) -> Any: + """Read one authored USD attribute without leaking invalid attribute handles.""" + + if prim is None or not prim or not prim.IsValid(): + return default + attribute = prim.GetAttribute(name) + if not attribute or not attribute.IsValid() or not attribute.HasAuthoredValueOpinion(): + return default + value = attribute.Get() + return default if value is None else value + + +def _usd_string_array(value: Any) -> list[str]: + if value is None: + return [] + if isinstance(value, str): + return [value] + try: + return [str(item) for item in value] + except TypeError: + return [str(value)] + + +def _environment_audit( + stage: Any, + *, + surface_path: str, + activity_path: Path, + facility_root_path: str = "/World/RemoteDeconFacility", +) -> dict[str, Any]: + """Audit the visible facility and the exact activity-bearing source geometry.""" + + from pxr import UsdGeom + + facility = stage.GetPrimAtPath(facility_root_path) + surface = stage.GetPrimAtPath(surface_path) + room_prefix = f"{facility_root_path}/Rooms/" + corridor_prefix = f"{facility_root_path}/Corridors/" + rooms: list[dict[str, Any]] = [] + corridors: list[dict[str, Any]] = [] + equipment: list[dict[str, Any]] = [] + shields: list[dict[str, Any]] = [] + for prim in stage.Traverse(): + path = str(prim.GetPath()) + room_id = _attribute_value(prim, "rad:facility:roomId") + corridor_id = _attribute_value(prim, "rad:facility:corridorId") + role = str(_attribute_value(prim, "rad:role", "")) + if path.startswith(room_prefix) and room_id: + rooms.append({"path": path, "room_id": str(room_id)}) + if path.startswith(corridor_prefix) and corridor_id: + corridors.append( + { + "path": path, + "corridor_id": str(corridor_id), + "from_room_id": str(_attribute_value(prim, "rad:facility:fromRoomId", "")), + "to_room_id": str(_attribute_value(prim, "rad:facility:toRoomId", "")), + } + ) + if role == "facility_equipment": + equipment.append( + { + "path": path, + "equipment_id": str(_attribute_value(prim, "rad:facility:equipmentId", "")), + "equipment_type": str(_attribute_value(prim, "rad:facility:equipmentType", "")), + "room_id": str(room_id or ""), + "fixed": bool(_attribute_value(prim, "rad:facility:fixed", False)), + } + ) + if role == "shield": + shields.append( + { + "path": path, + "inventory_id": str(_attribute_value(prim, "rad:shield:inventoryId", "")), + "movable": bool(_attribute_value(prim, "rad:shield:movable", False)), + "staged": bool(_attribute_value(prim, "rad:shield:staged", False)), + "deployed": bool(_attribute_value(prim, "rad:shield:deployed", False)), + "placement_fraction": _attribute_value(prim, "rad:shield:placementFraction"), + "room_id": str(room_id or ""), + } + ) + + source_map_uri = str(_attribute_value(surface, "rad:source:activityMapUri", "")) + decon_map_uri = str(_attribute_value(surface, "rad:decon:activityMapUri", "")) + face_count = 0 + if surface and surface.IsValid() and surface.IsA(UsdGeom.Mesh): + face_count = len(UsdGeom.Mesh(surface).GetFaceVertexCountsAttr().Get() or []) + map_path = Path(source_map_uri).expanduser() if source_map_uri else activity_path + map_triangle_count = 0 + map_activity_count = 0 + map_activity_bq = 0.0 + map_load_error: str | None = None + try: + with np.load(map_path, allow_pickle=False) as payload: + triangle_indices = np.asarray(payload["triangle_indices"]) + activity = np.asarray(payload["activity_bq"], dtype=np.float64) + map_triangle_count = int(triangle_indices.size) + map_activity_count = int(activity.size) + map_activity_bq = float(np.sum(activity)) + except Exception as error: + map_load_error = f"{type(error).__name__}: {error}" + + candidate_cells = int(_attribute_value(surface, "rad:source:candidateCellCount", 0)) + active_cells = int(_attribute_value(surface, "rad:source:activeCellCount", 0)) + active_faces = int(_attribute_value(surface, "rad:source:activeFaceCount", 0)) + declared = { + "layout_id": str(_attribute_value(facility, "rad:facility:layoutId", "")), + "deterministic": bool(_attribute_value(facility, "rad:facility:deterministic", False)), + "room_count": int(_attribute_value(facility, "rad:facility:roomCount", 0)), + "corridor_count": int(_attribute_value(facility, "rad:facility:corridorCount", 0)), + "equipment_count": int(_attribute_value(facility, "rad:facility:equipmentCount", 0)), + "obstacle_count": int(_attribute_value(facility, "rad:facility:obstacleCount", 0)), + "reserved_route_ids": _usd_string_array( + _attribute_value(facility, "rad:facility:reservedRouteIds", ()) + ), + "decon_surface_geometry": str( + _attribute_value(facility, "rad:facility:deconSurfaceGeometry", "") + ), + } + source = { + "path": surface_path, + "role": str(_attribute_value(surface, "rad:role", "")), + "source_type": str(_attribute_value(surface, "rad:source:type", "")), + "source_enabled": bool(_attribute_value(surface, "rad:source:enabled", False)), + "decon_enabled": bool(_attribute_value(surface, "rad:decon:enabled", False)), + "irregular_mask": bool(_attribute_value(surface, "rad:source:irregularMask", False)), + "candidate_cell_count": candidate_cells, + "active_cell_count": active_cells, + "active_face_count": active_faces, + "mesh_face_count": face_count, + "deposition_model": str(_attribute_value(surface, "rad:source:depositionModel", "")), + "geometry": str(_attribute_value(surface, "rad:source:geometry", "")), + "surface_orientation": str(_attribute_value(surface, "rad:decon:surfaceOrientation", "")), + "raster_rows": int(_attribute_value(surface, "rad:decon:rasterRows", 0)), + "collision_enabled": bool(_attribute_value(surface, "physics:collisionEnabled", False)), + "source_activity_map_uri": source_map_uri, + "decon_activity_map_uri": decon_map_uri, + "requested_activity_map_uri": str(activity_path.resolve()), + "map_triangle_count": map_triangle_count, + "map_activity_count": map_activity_count, + "map_activity_bq": map_activity_bq, + "map_load_error": map_load_error, + } + discovered = { + "room_count": len(rooms), + "corridor_count": len(corridors), + "equipment_count": len(equipment), + "shield_count": len(shields), + } + invariants = { + "facility_root_exists": bool(facility and facility.IsValid()), + "deterministic_facility": declared["deterministic"], + "at_least_four_rooms": (declared["room_count"] >= 4 and discovered["room_count"] >= 4), + "room_declaration_matches_stage": (declared["room_count"] == discovered["room_count"]), + "at_least_three_corridors": ( + declared["corridor_count"] >= 3 and discovered["corridor_count"] >= 3 + ), + "corridor_declaration_matches_stage": ( + declared["corridor_count"] == discovered["corridor_count"] + ), + "at_least_ten_equipment_obstacles": ( + declared["equipment_count"] >= 10 + and declared["obstacle_count"] >= 10 + and discovered["equipment_count"] >= 10 + ), + "equipment_declaration_matches_stage": ( + declared["equipment_count"] == discovered["equipment_count"] + ), + "multiple_reserved_routes": len(declared["reserved_route_ids"]) >= 2, + "at_least_two_physical_shields": discovered["shield_count"] >= 2, + "primary_and_staging_shields_exist": { + "/World/LeadShield", + "/World/StagingLeadShield", + }.issubset({row["path"] for row in shields}), + "surface_source_and_decon_enabled": (source["source_enabled"] and source["decon_enabled"]), + "surface_is_irregular_dense_geometry": ( + source["irregular_mask"] + and candidate_cells >= 48 * 28 + and active_cells > 0 + and source["deposition_model"] + == "gaussian_lobes_correlated_roughness_holes_satellite_droplets" + and source["geometry"] == "irregular_masked_triangle_activity_map" + ), + "surface_is_six_lane_vertical_raster": ( + source["surface_orientation"] == "vertical_x" and source["raster_rows"] == 6 + ), + "visible_collision_activity_geometry_is_identical": ( + declared["decon_surface_geometry"] == "same_visible_irregular_collision_activity_mesh" + and source["collision_enabled"] + and active_faces == 2 * active_cells + and active_faces == face_count + and active_faces == map_triangle_count + and map_triangle_count == map_activity_count + and map_load_error is None + ), + "source_and_decon_use_same_activity_map": ( + bool(source_map_uri) + and source_map_uri == decon_map_uri + and Path(source_map_uri).expanduser().resolve() == activity_path.resolve() + ), + } + return { + "facility_root": facility_root_path, + "declared": declared, + "discovered": discovered, + "rooms": rooms, + "corridors": corridors, + "equipment": equipment, + "shields": shields, + "surface_source": source, + "invariants": invariants, + "failed_invariants": [name for name, passed in invariants.items() if not passed], + } + + +def _enum_tail(value: Any) -> str: + return str(value).strip().lower().rsplit(".", 1)[-1] + + +def _completed_action_result(row: Mapping[str, Any]) -> bool: + return _enum_tail(row.get("status", "")) in {"completed", "partial"} + + +def _complex_process_audit( + plan_payload: Mapping[str, Any], + raw_results: Iterable[Mapping[str, Any]], + environment_before: Mapping[str, Any], + environment_after: Mapping[str, Any], + physical_state: Mapping[str, Any], + *, + expected_surface_path: str, + expected_shield_path: str = "/World/LeadShield", + expected_protected_path: str = "/World/DetectorStations/Protected", +) -> dict[str, Any]: + """Verify ordered process semantics using only artifact-safe public results.""" + + plan = _jsonable(plan_payload) + results = [dict(_jsonable(row)) for row in raw_results] + plan_steps = [dict(step) for step in plan.get("steps", []) if isinstance(step, Mapping)] + + def action_rows(action_type: str) -> list[tuple[int, dict[str, Any]]]: + return [ + (index, row) + for index, row in enumerate(results) + if _enum_tail(row.get("action_type", "")) == action_type + ] + + decon_rows = action_rows("decontaminate") + place_rows = action_rows("place_shield") + move_rows = action_rows("move_shield") + protected_moves = [ + (index, row) + for index, row in action_rows("measure") + if str(dict(row.get("public_details", {})).get("detector_path", "")) + == expected_protected_path + ] + measurement_rows = [ + (index, row) + for index, row in enumerate(results) + if _enum_tail(row.get("command", "")) == "measure" and "action_type" not in row + ] + return_rows = [ + (index, row) + for index, row in enumerate(results) + if _enum_tail(row.get("command", "")) == "return_measurement_robot" + ] + status_rows = [ + (index, row) + for index, row in enumerate(results) + if _enum_tail(row.get("command", "")) == "show_status" + ] + + decon_plan_steps = [ + step + for step in plan_steps + if _enum_tail(step.get("command", "")) == "execute_candidate" + and "decon" in str(step.get("candidate_id", "")).lower() + ] + decon_plan = decon_plan_steps[0] if decon_plan_steps else {} + decon_max_attempts = int(decon_plan.get("max_attempts", 0) or 0) + decon_until = decon_plan.get("until") + if not isinstance(decon_until, Mapping): + decon_until = {} + completion_rows = [ + dict(row.get("completion_condition", {})) + for _, row in decon_rows + if isinstance(row.get("completion_condition"), Mapping) + ] + final_completion = completion_rows[-1] if completion_rows else {} + final_decon_attempt = int(decon_rows[-1][1].get("attempt", 0)) if decon_rows else 0 + + decon_public_details = [dict(row.get("public_details", {})) for _, row in decon_rows] + decon_motion = [dict(details.get("motion_audit", {})) for details in decon_public_details] + activity_before_values = [float(row.get("activity_before_bq", 0.0)) for row in decon_motion] + activity_after_values = [float(row.get("activity_after_bq", 0.0)) for row in decon_motion] + + shield_events: list[tuple[int, dict[str, Any], str]] = [] + for index, row in (*place_rows, *move_rows): + shield_events.append((index, row, _enum_tail(row.get("action_type", "")))) + shield_events.sort(key=lambda item: item[0]) + + mitigation_rows = [*decon_rows, *place_rows, *move_rows] + collateral_rows: list[dict[str, Any]] = [] + missing_collateral_audit: list[str] = [] + for _, row in mitigation_rows: + details = row.get("public_details", {}) + details = dict(details) if isinstance(details, Mapping) else {} + rows = details.get("collateral_motion_audit") + if not isinstance(rows, list): + missing_collateral_audit.append(str(row.get("action_id", "unknown"))) + continue + for collateral in rows: + if isinstance(collateral, Mapping): + collateral_rows.append(dict(collateral)) + maximum_collateral_displacement_m = max( + (float(row.get("displacement_m", float("inf"))) for row in collateral_rows), + default=0.0, + ) + + place_details = dict(place_rows[0][1].get("public_details", {})) if place_rows else {} + move_details = dict(move_rows[0][1].get("public_details", {})) if move_rows else {} + place_motion = dict(place_details.get("motion_audit", {})) + move_motion = dict(move_details.get("motion_audit", {})) + protected_motion = ( + dict(protected_moves[0][1].get("public_details", {})).get("motion_audit", {}) + if protected_moves + else {} + ) + protected_motion = dict(protected_motion) if isinstance(protected_motion, Mapping) else {} + measurement = measurement_rows[0][1] if measurement_rows else {} + return_result = return_rows[0][1] if return_rows else {} + return_details = return_result.get("public_details", {}) + return_details = dict(return_details) if isinstance(return_details, Mapping) else {} + return_motion = return_details.get("motion_audit", {}) + return_motion = dict(return_motion) if isinstance(return_motion, Mapping) else {} + + position_before = np.asarray( + physical_state.get("measurement_robot_initial_position_m", ()), + dtype=np.float64, + ) + position_after = np.asarray( + physical_state.get("measurement_robot_final_position_m", ()), + dtype=np.float64, + ) + returned_home_distance_m = ( + float(np.linalg.norm(position_after - position_before)) + if position_before.shape == (3,) and position_after.shape == (3,) + else float("inf") + ) + shield_before = np.asarray( + physical_state.get("primary_shield_initial_position_m", ()), dtype=np.float64 + ) + shield_after = np.asarray( + physical_state.get("primary_shield_final_position_m", ()), dtype=np.float64 + ) + shield_displacement_m = ( + float(np.linalg.norm(shield_after - shield_before)) + if shield_before.shape == (3,) and shield_after.shape == (3,) + else 0.0 + ) + final_primary_shield = next( + ( + row + for row in environment_after.get("shields", []) + if isinstance(row, Mapping) and row.get("path") == expected_shield_path + ), + {}, + ) + expected_plan_commands = [ + "execute_candidate", + "execute_candidate", + "execute_candidate", + "execute_candidate", + "measure", + "return_measurement_robot", + "show_status", + ] + result_rows_by_workflow_step = { + step_number: [ + row for row in results if int(row.get("workflow_step", 0) or 0) == step_number + ] + for step_number in range(1, len(plan_steps) + 1) + } + physical_results_match_plan = all( + result_rows_by_workflow_step.get(step_number) + and all( + row.get("action_id") == plan_steps[step_number - 1].get("candidate_id") + for row in result_rows_by_workflow_step[step_number] + ) + for step_number in range(1, 5) + ) + + ordered_indices = ( + decon_rows[-1][0] if decon_rows else -1, + place_rows[0][0] if place_rows else -1, + move_rows[0][0] if move_rows else -1, + protected_moves[0][0] if protected_moves else -1, + measurement_rows[0][0] if measurement_rows else -1, + return_rows[0][0] if return_rows else -1, + status_rows[0][0] if status_rows else -1, + ) + invariants = { + "environment_before_passed": not environment_before.get("failed_invariants"), + "environment_after_passed": not environment_after.get("failed_invariants"), + "plan_has_exact_seven_step_complex_process": ( + len(plan_steps) == 7 + and [_enum_tail(step.get("command", "")) for step in plan_steps] + == expected_plan_commands + ), + "physical_results_match_confirmed_plan_candidates": physical_results_match_plan, + "plan_has_bounded_multi_pass_decontamination": ( + bool(decon_plan_steps) + and decon_max_attempts == 3 + and bool(decon_until) + and _enum_tail(decon_until.get("criterion", "")) + == "decontamination_remaining_fraction_at_most" + and abs(float(decon_until.get("threshold", -1.0)) - 0.60) <= 1.0e-6 + ), + "decontamination_attempt_metadata_complete": ( + bool(decon_rows) + and len(completion_rows) == len(decon_rows) + and [int(row.get("attempt", 0)) for _, row in decon_rows] + == list(range(1, len(decon_rows) + 1)) + and all(int(row.get("max_attempts", 0)) == decon_max_attempts for _, row in decon_rows) + and all( + _enum_tail(condition.get("criterion", "")) + == _enum_tail(decon_until.get("criterion", "")) + and abs( + float(condition.get("threshold", -1.0)) + - float(decon_until.get("threshold", -2.0)) + ) + <= 1.0e-6 + for condition in completion_rows + ) + ), + "bounded_decontamination_condition_met_after_repeated_passes": ( + bool(decon_rows) + and len(decon_rows) <= decon_max_attempts + and len(decon_rows) >= 2 + and bool(final_completion.get("met", False)) + and final_decon_attempt < decon_max_attempts + 1 + and _enum_tail(final_completion.get("criterion", "")) + == "decontamination_remaining_fraction_at_most" + and float(final_completion.get("observed", float("inf"))) + <= float(final_completion.get("threshold", -1.0)) + ), + "decontamination_completed_on_visible_surface": ( + bool(decon_rows) + and all(_completed_action_result(row) for _, row in decon_rows) + and all( + details.get("surface_path") == expected_surface_path + for details in decon_public_details + ) + and all( + float(details.get("accepted_contacts", 0)) > 0 for details in decon_public_details + ) + and all(bool(motion.get("success", False)) for motion in decon_motion) + and all(float(motion.get("coverage_fraction", 0.0)) >= 0.35 for motion in decon_motion) + ), + "decontamination_reduced_activity": ( + bool(activity_before_values) + and all( + after < before + for before, after in zip(activity_before_values, activity_after_values, strict=True) + ) + and activity_after_values[-1] < activity_before_values[0] + and float(environment_after.get("surface_source", {}).get("map_activity_bq", 0.0)) + < float(environment_before.get("surface_source", {}).get("map_activity_bq", 0.0)) + ), + "same_primary_shield_placed_at_25_then_moved_to_65": ( + bool(place_rows) + and bool(move_rows) + and _completed_action_result(place_rows[0][1]) + and _completed_action_result(move_rows[0][1]) + and place_details.get("object_path") == expected_shield_path + and move_details.get("object_path") == expected_shield_path + and place_details.get("deployment_state") == "deployed" + and move_details.get("deployment_state") == "deployed" + and bool(place_motion.get("success", False)) + and bool(move_motion.get("success", False)) + and abs(float(place_details.get("placement_fraction", -1.0)) - 0.25) <= 1.0e-6 + and abs(float(move_details.get("placement_fraction", -1.0)) - 0.65) <= 1.0e-6 + and shield_events[:2] + and [event[2] for event in shield_events[:2]] == ["place_shield", "move_shield"] + ), + "primary_shield_final_metadata_is_65_percent": ( + bool(final_primary_shield) + and bool(final_primary_shield.get("deployed", False)) + and abs(float(final_primary_shield.get("placement_fraction", -1.0)) - 0.65) <= 1.0e-6 + and shield_displacement_m > 0.25 + ), + "protected_navigation_completed": ( + bool(protected_moves) + and _completed_action_result(protected_moves[0][1]) + and bool(protected_motion.get("success", False)) + ), + "two_second_measurement_completed": ( + bool(measurement_rows) + and abs(float(measurement.get("duration_s", -1.0)) - 2.0) <= 1.0e-6 + and int(measurement.get("detector_count", 0)) > 0 + and bool(measurement.get("measurements")) + and any( + isinstance(row, Mapping) + and row.get("detector_path") == expected_protected_path + and abs(float(row.get("duration_s", -1.0)) - 2.0) <= 1.0e-6 + for row in measurement.get("measurements", []) + ) + ), + "measurement_robot_returned_home": ( + bool(return_rows) + and _completed_action_result(return_result) + and bool(return_motion.get("success", False)) + and returned_home_distance_m <= 0.25 + ), + "final_status_reported": bool(status_rows), + "process_order_is_decon_place_move_measure_return_status": ( + all(index >= 0 for index in ordered_indices) + and list(ordered_indices) == sorted(ordered_indices) + ), + "all_results_have_workflow_attempt_metadata": ( + bool(results) + and all( + int(row.get("workflow_step", 0)) >= 1 + and int(row.get("attempt", 0)) >= 1 + and int(row.get("max_attempts", 0)) >= 1 + for row in results + ) + ), + "collateral_motion_audited_and_bounded": ( + bool(mitigation_rows) + and not missing_collateral_audit + and bool(collateral_rows) + and maximum_collateral_displacement_m <= 0.05 + ), + } + return { + "plan": plan, + "results": results, + "expected_paths": { + "surface": expected_surface_path, + "primary_shield": expected_shield_path, + "protected_station": expected_protected_path, + }, + "decontamination": { + "planned_max_attempts": decon_max_attempts, + "planned_completion_condition": dict(decon_until), + "executed_attempts": len(decon_rows), + "completion_rows": completion_rows, + "first_activity_before_bq": ( + activity_before_values[0] if activity_before_values else None + ), + "final_activity_after_bq": ( + activity_after_values[-1] if activity_after_values else None + ), + }, + "shield_events": [ + { + "result_index": index, + "action_type": action_type, + "action_id": row.get("action_id"), + "object_path": dict(row.get("public_details", {})).get("object_path"), + "placement_fraction": dict(row.get("public_details", {})).get("placement_fraction"), + } + for index, row, action_type in shield_events + ], + "ordered_result_indices": ordered_indices, + "returned_home_distance_m": returned_home_distance_m, + "primary_shield_displacement_m": shield_displacement_m, + "collateral_motion": { + "audited_rows": collateral_rows, + "missing_action_ids": missing_collateral_audit, + "maximum_displacement_m": maximum_collateral_displacement_m, + "limit_m": 0.05, + }, + "invariants": invariants, + "failed_invariants": [name for name, passed in invariants.items() if not passed], + } + + +def _local_llm_audit(dashboard: Any) -> dict[str, Any]: + controller = getattr(dashboard, "_natural_language", None) + runtime = getattr(controller, "runtime", None) + interpreter = getattr(controller, "interpreter", None) + endpoint = str(getattr(interpreter, "endpoint", "") or getattr(runtime, "endpoint", "")) + parsed = urlparse(endpoint) if endpoint else None + runtime_status = _enum_tail(getattr(runtime, "status", "")) + process = getattr(runtime, "process", None) + audit = { + "controller_class": type(controller).__name__ if controller is not None else None, + "runtime_class": type(runtime).__name__ if runtime is not None else None, + "runtime_status": runtime_status, + "runtime_process_pid": getattr(process, "pid", None), + "interpreter_class": (type(interpreter).__name__ if interpreter is not None else None), + "model": getattr(interpreter, "model", None), + "endpoint": endpoint, + "endpoint_host": None if parsed is None else parsed.hostname, + } + invariants = { + "openai_compatible_interpreter_was_instantiated": ( + audit["interpreter_class"] == "OpenAICompatibleCommandInterpreter" + ), + "llama_runtime_is_ready": runtime_status in {"ready", "external"}, + "inference_endpoint_is_loopback_only": ( + audit["endpoint_host"] in {"127.0.0.1", "localhost", "::1"} + ), + "qwen_model_alias_was_used": audit["model"] == "radcounter-qwen3-4b", + } + audit["invariants"] = invariants + audit["failed_invariants"] = [name for name, passed in invariants.items() if not passed] + return audit + + +def _capture_viewport_evidence(app: Any, path: Path) -> dict[str, Any]: + """Request a fresh PNG and drive rendering long enough to flush it.""" + + from omni.kit.viewport.utility import capture_viewport_to_file, get_active_viewport + + path.parent.mkdir(parents=True, exist_ok=True) + started_ns = time.time_ns() + viewport = get_active_viewport() + if viewport is None: + return {"path": str(path), "captured": False, "error": "no active viewport"} + capture_viewport_to_file(viewport, file_path=str(path), is_hdr=False) + deadline = time.monotonic() + 15.0 + while time.monotonic() < deadline and app.is_running(): + app.update() + if path.is_file() and path.stat().st_size > 0 and path.stat().st_mtime_ns >= started_ns: + break + return { + "path": str(path.resolve()), + "captured": ( + path.is_file() and path.stat().st_size > 0 and path.stat().st_mtime_ns >= started_ns + ), + "bytes": path.stat().st_size if path.is_file() else 0, + "modified_ns": path.stat().st_mtime_ns if path.is_file() else None, + } + + +def _run_complex_natural_language_validation( + app: Any, + args: argparse.Namespace, + panel: _ValidationPanel, + dashboard: Any, + *, + stage: Any, + stage_path: Path, + asset_manifest: Mapping[str, Any], + robot_config: Any, + activity_path: Path, + generator: Any, + countermeasure_controller: Any, + measurement_controller: Any, + simulation: Any, +) -> dict[str, Any]: + """Run the production NL controller through confirmation and its physical queue.""" + + instruction = args.initial_command or DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + total_steps = 7 + screenshot_root = args.artifact.parent + initial_screenshot_path = screenshot_root / ( + f"{args.artifact.stem}-complex-natural-language-initial.png" + ) + final_screenshot_path = screenshot_root / ( + f"{args.artifact.stem}-complex-natural-language-final.png" + ) + _configure_complex_facility_camera(stage) + for _ in range(30): + app.update() + + environment_before = _environment_audit( + stage, + surface_path=robot_config.decon_surface_path, + activity_path=activity_path, + ) + physical_state: dict[str, Any] = { + "measurement_robot_initial_position_m": _world_position( + stage, robot_config.measurement_articulation + ).tolist(), + "primary_shield_initial_position_m": _world_position(stage, "/World/LeadShield").tolist(), + } + context_before = dashboard.natural_language_context() + context_payload = context_before.model_dump(mode="json") + initial_screenshot = _capture_viewport_evidence(app, initial_screenshot_path) + panel.update( + "Complex facility and irregular source preflight", + 0, + total_steps, + { + "instruction": instruction, + "available_actions": len(context_before.available_actions), + "environment_failed_invariants": environment_before["failed_invariants"], + "initial_screenshot": initial_screenshot, + }, + ) + if environment_before["failed_invariants"]: + payload = { + "success": False, + "mode": "complex_natural_language_validation", + "instruction": instruction, + "stage": str(stage_path), + "environment_before": environment_before, + "public_context_before": context_payload, + "screenshots": {"initial": initial_screenshot}, + "failed_invariants": [ + f"environment_before.{name}" for name in environment_before["failed_invariants"] + ], + } + raise ComplexNaturalLanguageValidationError("complex facility preflight failed", payload) + + physical_queue_dispatches: list[dict[str, Any]] = [] + + def process_physical_queue() -> None: + queue = getattr(dashboard, "_physical_command_queue", ()) + if queue: + step = queue[0][0] + physical_queue_dispatches.append( + { + "sequence": len(physical_queue_dispatches) + 1, + "command": _enum_tail(getattr(step, "command", "")), + "candidate_id": getattr(step, "candidate_id", None), + } + ) + generator.probe.invalidate_collision_cache() + dashboard.process_pending_natural_language_actions() + + panel.update( + "Local LLM interpretation before explicit confirmation", + 0, + total_steps, + {"instruction": instruction, "confirm_physical": False}, + ) + plan_payload: dict[str, Any] = {} + plan_preflight: dict[str, Any] = {} + try: + interpreted = _complete_with_updates( + app, + dashboard.submit_natural_language_instruction( + instruction, + confirm_physical=False, + ), + timeout_s=args.natural_language_timeout_s, + ) + plan_payload = interpreted.validated.plan.model_dump(mode="json") + context_by_id = {action.action_id: action for action in context_before.available_actions} + planned_steps = list(interpreted.validated.plan.steps) + planned_actions = [context_by_id.get(step.candidate_id or "") for step in planned_steps] + expected_commands = ( + "execute_candidate", + "execute_candidate", + "execute_candidate", + "execute_candidate", + "measure", + "return_measurement_robot", + "show_status", + ) + preflight_invariants = { + "seven_ordered_logical_steps": ( + len(planned_steps) == len(expected_commands) + and tuple(_enum_tail(step.command) for step in planned_steps) == expected_commands + ), + "bounded_decontamination_is_first": ( + len(planned_actions) >= 1 + and planned_actions[0] is not None + and planned_actions[0].action_type == "decontaminate" + and planned_steps[0].max_attempts == 3 + and planned_steps[0].until is not None + and _enum_tail(planned_steps[0].until.criterion) + == "decontamination_remaining_fraction_at_most" + and abs(planned_steps[0].until.threshold - 0.60) <= 1.0e-6 + ), + "primary_shield_has_feasible_25_then_65_slots": ( + len(planned_actions) >= 3 + and all(action is not None for action in planned_actions[1:3]) + and all(action.feasible for action in planned_actions[1:3] if action) + and all( + action.target == "/World/LeadShield" + for action in planned_actions[1:3] + if action + ) + and [ + round(float(action.placement_fraction or -1.0), 6) + for action in planned_actions[1:3] + if action is not None + ] + == [0.25, 0.65] + ), + "protected_navigation_is_fourth": ( + len(planned_actions) >= 4 + and planned_actions[3] is not None + and planned_actions[3].action_type == "measure" + and planned_actions[3].target == "/World/DetectorStations/Protected" + ), + "physical_plan_is_waiting_for_confirmation": ( + interpreted.validated.requires_confirmation and not interpreted.executed + ), + } + plan_preflight = { + "invariants": preflight_invariants, + "failed_invariants": [ + name for name, passed in preflight_invariants.items() if not passed + ], + "shield_candidates": [ + { + **action.model_dump(mode="json"), + "feasibility_facts": _jsonable( + getattr( + getattr(dashboard, "_command_candidates", {}).get(action.action_id), + "feasibility", + None, + ) + ), + } + for action in context_before.available_actions + if action.action_type in {"place_shield", "move_shield"} + ], + } + if plan_preflight["failed_invariants"]: + raise RuntimeError( + f"natural-language plan preflight failed: {plan_preflight['failed_invariants']}" + ) + panel.update( + "Plan verified; executing explicit confirmation through the physical queue", + 0, + total_steps, + {"plan": plan_payload, "plan_preflight": plan_preflight}, + ) + submission = _complete_with_updates( + app, + dashboard.confirm_natural_language_instruction(), + before_update=process_physical_queue, + timeout_s=args.natural_language_timeout_s, + ) + except Exception as error: + partial_results = list(getattr(error, "results", ())) + environment_after = _environment_audit( + stage, + surface_path=robot_config.decon_surface_path, + activity_path=activity_path, + ) + physical_state.update( + { + "measurement_robot_final_position_m": _world_position( + stage, robot_config.measurement_articulation + ).tolist(), + "primary_shield_final_position_m": _world_position( + stage, "/World/LeadShield" + ).tolist(), + } + ) + local_llm = _local_llm_audit(dashboard) + partial_audit = _complex_process_audit( + plan_payload, + partial_results, + environment_before, + environment_after, + physical_state, + expected_surface_path=robot_config.decon_surface_path, + ) + payload = { + "success": False, + "mode": "complex_natural_language_validation", + "instruction": instruction, + "stage": str(stage_path), + "error": f"{type(error).__name__}: {error}", + "local_llm": local_llm, + "confirmation_requested": True, + "plan_preflight": plan_preflight, + "physical_queue_dispatches": physical_queue_dispatches, + "partial_results": _jsonable(partial_results), + "environment_before": environment_before, + "environment_after": environment_after, + "physical_state": physical_state, + "process_audit": partial_audit, + "public_context_before": context_payload, + "screenshots": {"initial": initial_screenshot}, + "failed_invariants": [ + "natural_language_workflow_completed", + *local_llm["failed_invariants"], + *partial_audit["failed_invariants"], + ], + } + raise ComplexNaturalLanguageValidationError( + "complex natural-language workflow failed", payload + ) from error + + plan_payload = submission.validated.plan.model_dump(mode="json") + results = list(submission.results) + environment_after = _environment_audit( + stage, + surface_path=robot_config.decon_surface_path, + activity_path=activity_path, + ) + physical_state.update( + { + "measurement_robot_final_position_m": _world_position( + stage, robot_config.measurement_articulation + ).tolist(), + "measurement_robot_home_position_m": _jsonable( + getattr(measurement_controller, "home_position_m", None) + ), + "primary_shield_final_position_m": _world_position(stage, "/World/LeadShield").tolist(), + } + ) + process_audit = _complex_process_audit( + plan_payload, + results, + environment_before, + environment_after, + physical_state, + expected_surface_path=robot_config.decon_surface_path, + ) + local_llm = _local_llm_audit(dashboard) + final_screenshot = _capture_viewport_evidence(app, final_screenshot_path) + physical_result_count = sum( + 1 + for row in results + if _enum_tail(row.get("command", "")) in {"execute_candidate", "return_measurement_robot"} + ) + runner_invariants = { + "visible_gui_mode_was_enforced": not args.headless, + "fresh_initial_and_final_viewport_evidence": ( + bool(initial_screenshot.get("captured")) and bool(final_screenshot.get("captured")) + ), + "physical_plan_required_confirmation": bool(submission.validated.requires_confirmation), + "confirmed_plan_executed": bool(submission.executed), + "physical_actions_used_dashboard_queue": ( + bool(physical_queue_dispatches) + and len(physical_queue_dispatches) == physical_result_count + and any(row["command"] == "execute_candidate" for row in physical_queue_dispatches) + and any( + row["command"] == "return_measurement_robot" for row in physical_queue_dispatches + ) + ), + "actual_loopback_local_llm_used": not local_llm["failed_invariants"], + "complex_process_invariants_passed": not process_audit["failed_invariants"], + } + failed = [name for name, passed in runner_invariants.items() if not passed] + payload = { + "success": not failed, + "mode": "complex_natural_language_validation", + "stage": str(stage_path), + "assets": _jsonable(asset_manifest), + "robot_models": { + "countermeasure": "Clearpath Ridgeback + Franka Panda", + "measurement": "NVIDIA Nova Carter", + }, + "instruction": instruction, + "natural_language": { + "plan": plan_payload, + "warnings": list(submission.validated.warnings), + "requires_confirmation": submission.validated.requires_confirmation, + "confirmation_requested": True, + "executed": submission.executed, + "results": _jsonable(results), + }, + "plan_preflight": plan_preflight, + "local_llm": local_llm, + "physical_queue_dispatches": physical_queue_dispatches, + "public_context_before": context_payload, + "environment_before": environment_before, + "environment_after": environment_after, + "physical_state": physical_state, + "process_audit": process_audit, + "screenshots": { + "initial": initial_screenshot, + "final": final_screenshot, + }, + "runner_invariants": runner_invariants, + "failed_invariants": failed, + "transport_statistics": _jsonable(simulation.transport.statistics), + "controller_audit": { + "countermeasure_dof_count": len(countermeasure_controller.dof_names), + "measurement_dof_count": len(measurement_controller.robot.dof_names), + "franka_arm_joint_excursion_rad": (countermeasure_controller.arm_joint_excursion_rad), + }, + } + panel.update( + "Complex natural-language validation completed", + total_steps if not failed else 0, + total_steps, + payload, + ) + if failed: + raise ComplexNaturalLanguageValidationError( + f"complex validation invariants failed: {failed}", payload + ) + return payload + + +def _monitor_update(app: Any) -> None: + for _ in range(4): + app.update() + time.sleep(0.10) + app.update() + + +def _exercise_robot_monitor(app: Any, dashboard: Any) -> dict[str, object]: + """Exercise camera state without authoring a second rendered viewport.""" + + from pxr import Gf + + monitor = dashboard._robot_monitor + if not monitor.robots: + raise RuntimeError("robot monitor smoke test requires at least one catalog robot") + robot_id = monitor.robots[0].robot_id + + monitor.follow_robot(robot_id) + _monitor_update(app) + follow = monitor.audit() + if follow["camera_mode"] != "follow": + raise RuntimeError(f"follow camera did not activate: {follow}") + + monitor.onboard_robot(robot_id) + _monitor_update(app) + onboard = monitor.audit() + if onboard["camera_mode"] != "onboard": + raise RuntimeError(f"onboard camera did not activate: {onboard}") + + monitor.begin_operation( + robot_id=robot_id, + operation="Decontamination", + phase="navigating", + route_m=((0.0, 0.0, 0.0), (1.0, 0.5, 0.0)), + target_m=(1.0, 0.5, 1.0), + target_path="/World/RobotMonitorSmokeTarget", + auto_work_view=True, + ) + _monitor_update(app) + monitor.update_countermeasure_progress( + { + "phase": "decontaminating", + "progress": 0.63, + "coverage_fraction": 0.57, + } + ) + _monitor_update(app) + work = monitor.audit() + if work["camera_mode"] != "work" or not math.isclose(work["progress"], 0.63): + raise RuntimeError(f"work camera/progress did not activate: {work}") + + camera_op = monitor._camera_op + if camera_op is None or camera_op.Get() is None: + raise RuntimeError("operator camera did not author a transform") + camera_op.Set(Gf.Matrix4d(1.0)) + _monitor_update(app) + manual_cancel = monitor.audit() + if manual_cancel["camera_mode"] != "free": + raise RuntimeError(f"manual camera edit did not cancel tracking: {manual_cancel}") + + monitor.overview() + _monitor_update(app) + overview = monitor.audit() + if overview["camera_mode"] != "overview": + raise RuntimeError(f"overview camera did not activate: {overview}") + monitor.stop_follow(manual=False) + return { + "follow": follow, + "onboard": onboard, + "work": work, + "manual_cancel": manual_cancel, + "overview": overview, + } + + +def _run_configurable_system( + app: Any, + args: argparse.Namespace, + panel: _ValidationPanel, + dashboard: Any, + selection: Any, +) -> dict[str, object]: + """Load a catalog selection without assuming the vertical-slice task layout.""" + + import omni.usd + from radcounter.isaac.runtime import IsaacRadiationSimulation + from radcounter.isaac.system_profile import ( + compose_selected_system, + prepare_environment_stage, + ) + + panel.update( + "Preparing selected environment", + 0, + 3, + selection.as_dict(), + ) + stage_path, environment_manifest = prepare_environment_stage(selection) + context = omni.usd.get_context() + if not context.open_stage(str(stage_path)): + raise RuntimeError(f"failed to open selected environment stage: {stage_path}") + for _ in range(30): + app.update() + stage = context.get_stage() + panel.update("Composing selected robots and detectors", 1, 3, selection.as_dict()) + composed = compose_selected_system(stage, selection, stage_path=stage_path) + for _ in range(30): + app.update() + simulation = IsaacRadiationSimulation.from_config(stage, composed.runtime_config_path) + dashboard.configure_system_paths( + stage_path=composed.stage_path, + config_path=composed.runtime_config_path, + display_name=selection.profile.display_name, + simulation=simulation, + selection=selection, + ) + panel.update("Selected system is ready", 3, 3, composed.profile_manifest) + robot_monitor_validation = ( + _exercise_robot_monitor(app, dashboard) if args.robot_monitor_smoke_test else None + ) + if args.interactive: + panel.hide() + return { + "success": True, + "mode": "configurable", + "selection": selection.as_dict(), + "stage": str(composed.stage_path), + "environment_manifest": str(environment_manifest), + "runtime_config": str(composed.runtime_config_path), + "robot_paths": composed.robot_paths, + "detector_paths": composed.detector_paths, + "disabled_detector_paths": list(composed.disabled_detector_paths), + "rebased_asset_paths": composed.rebased_asset_paths, + "source_count": len(simulation.sources), + "detector_count": len(simulation.detectors), + "robot_monitor": dashboard.robot_monitor_audit(), + "robot_monitor_validation": robot_monitor_validation, + "note": ( + "The configurable session composes selected assets without adding the " + "vertical-slice-only decontamination task layout." + ), + } + + +def _run_validation( + app: Any, + args: argparse.Namespace, + panel: _ValidationPanel, + dashboard: Any, +) -> dict: + selection = _selected_system(args) + if args.decontamination_smoke_test and selection.robot_set.kind != "decommissioning": + raise RuntimeError( + "--decontamination-smoke-test requires the explicit central-catalog " + "robot set 'articulated-decommissioning'; do not inject an unselected " + "task robot into the operator roster" + ) + if selection.configurable and not args.decontamination_smoke_test: + return _run_configurable_system(app, args, panel, dashboard, selection) + import omni.usd - from radcounter.isaac.planning import IsaacActionCandidateGenerator + from isaacsim.core.api import World + from isaacsim.core.prims import SingleArticulation + from isaacsim.robot.wheeled_robots.robots import WheeledRobot + from radcounter.isaac.planning import ( + IsaacActionCandidateGenerator, + SceneCandidateConfig, + ) from radcounter.isaac.robot import ( ContactDrivenDecontaminator, DecontaminationConfig, - IsaacPhysicsRobotController, - PhysicsControllerConfig, + NovaCarterController, + RealRobotAssetConfig, + RidgebackFrankaController, + add_real_robot_references, + author_real_robot_task_scene, + create_decontamination_activity_map, + enable_real_robot_extensions, ) from radcounter.isaac.runtime import IsaacRadiationSimulation - from radcounter.isaac.ui.dashboard import RadCounterDashboard from radcounter.isaac.workflow import IsaacWorkflowServices from radcounter.core.actions import ResourceState @@ -339,54 +1626,175 @@ def _run_validation(app: Any, args: argparse.Namespace, panel: _ValidationPanel) from radcounter.core.models.actions import ActionStatus, ActionType from radcounter.core.planning import DeterministicFeasibilityChecker + enable_real_robot_extensions() + for _ in range(20): + app.update() context = omni.usd.get_context() - stage_path = ROOT / "assets/environments/radcounter_vertical_slice.usda" + environment_manifest = None + if args.decontamination_smoke_test: + from radcounter.isaac.system_profile import prepare_environment_stage + + environment_operation_offset_m = selection.environment_config.translation_world_m + stage_path, environment_manifest = prepare_environment_stage(selection) + else: + environment_operation_offset_m = None + stage_path = ROOT / "assets/environments/radcounter_vertical_slice.usda" if not context.open_stage(str(stage_path)): raise RuntimeError(f"failed to open {stage_path}") for _ in range(24): app.update() stage = context.get_stage() - _configure_camera(stage) - - dashboard = RadCounterDashboard("radcounter.gui.validation") + if args.decontamination_smoke_test: + decon_anchor = selection.spawn_anchor("decon-surface") + ground_primary = selection.spawn_anchor("ground-primary") + ground_secondary = selection.spawn_anchor("ground-secondary") + camera_eye = selection.spawn_anchor("inspection-camera-eye") + camera_target = selection.spawn_anchor("inspection-camera-target") + robot_config = replace( + RealRobotAssetConfig(), + decon_workbench_center_m=decon_anchor.translation_m, + shield_initial_position_m=(0.0, 3.0, ground_primary.translation_m[2]), + include_validation_facility=False, + ) + else: + decon_anchor = None + ground_primary = None + ground_secondary = None + camera_eye = None + camera_target = None + robot_config = replace( + RealRobotAssetConfig(), + decon_workbench_center_m=(14.39, 0.80, 1.15), + shield_initial_position_m=(4.80, 2.80, 0.0), + ) + asset_manifest = add_real_robot_references(stage, config=robot_config) + panel.update("Loading articulated robot assets", 0, 8, asset_manifest) + for _ in range(120): + app.update() artifact_root = args.artifact.parent - activity_path, floor_activity_before = _prepare_activity_copy(stage, artifact_root) - - timeline = omni.timeline.get_timeline_interface() - timeline.play() - for _ in range(30): + activity_path = create_decontamination_activity_map( + artifact_root / "runtime_workbench_activity.npz" + ) + floor_activity_before = _activity_total(activity_path) + author_real_robot_task_scene(stage, activity_path, config=robot_config) + if args.decontamination_smoke_test: + # This run selects only contact decontamination. Do not leave an + # unrelated movable validation shield in the imported CAD where its + # settling motion could be mistaken for task collateral. + shield = stage.GetPrimAtPath(robot_config.shield_path) + if shield.IsValid(): + stage.RemovePrim(robot_config.shield_path) + if args.decontamination_smoke_test: + assert camera_eye is not None and camera_target is not None + _configure_decon_room_camera( + stage, + eye_m=camera_eye.translation_m, + target_m=camera_target.translation_m, + ) + else: + _configure_camera(stage) + for _ in range(60): app.update() - simulation = IsaacRadiationSimulation.from_config( - stage, - ROOT / "configs/scenarios/vertical_slice.runtime.json", + world = World( + stage_units_in_meters=1.0, + physics_dt=1.0 / 60.0, + rendering_dt=1.0 / 60.0, ) - dashboard.simulation = simulation - stepper = _GuiStepper(app, args.frame_delay_s) - controller_config = PhysicsControllerConfig( - grasp_distance_m=0.24, - maximum_navigation_steps=3200, - maximum_settle_steps=240, + franka_articulation = world.scene.add( + SingleArticulation( + robot_config.countermeasure_root, + name="radcounter_gui_ridgeback_franka", + ) ) - countermeasure_controller = IsaacPhysicsRobotController( + carter_articulation = world.scene.add( + WheeledRobot( + prim_path=robot_config.measurement_articulation, + name="radcounter_gui_nova_carter", + wheel_dof_names=list(NovaCarterController.wheel_names), + ) + ) + world.reset() + spawn_pose_before_settle = _world_position(stage, robot_config.panda_base_path) + for _ in range(90): + world.step(render=False) + spawn_pose_after_settle = _world_position(stage, robot_config.panda_base_path) + spawn_vertical_drop_m = max( + 0.0, + float(spawn_pose_before_settle[2] - spawn_pose_after_settle[2]), + ) + if args.decontamination_smoke_test and spawn_vertical_drop_m > 0.15: + raise RuntimeError( + "catalog robot spawn is unsupported by the selected CAD floor: " + f"vertical drop={spawn_vertical_drop_m:.3f} m" + ) + stepper = _GuiStepper(app, args.frame_delay_s, world) + countermeasure_controller = RidgebackFrankaController( stage, - "/World/CountermeasureRobot", - "/World/CountermeasureRobot/DeconTool", stepper, - config=controller_config, + config=robot_config, + articulation=franka_articulation, ) - measurement_controller = IsaacPhysicsRobotController( + measurement_controller = NovaCarterController( stage, - "/World/MeasurementRobot", - "/World/MeasurementRobot/Detector", stepper, - config=controller_config, + config=robot_config, + articulation=carter_articulation, + ) + countermeasure_controller.progress_callback = dashboard.update_countermeasure_progress + measurement_controller.progress_callback = dashboard.update_navigation_progress + if args.decontamination_smoke_test: + assert ground_primary is not None and ground_secondary is not None + measurement_controller.set_initial_pose(ground_secondary.translation_m) + surface_x, surface_y, _ = robot_config.decon_workbench_center_m + staging = countermeasure_controller.navigate_route( + ( + ground_primary.translation_m, + (surface_x - 0.98, surface_y - 0.55, ground_primary.translation_m[2]), + ), + final_yaw_rad=0.0, + ) + else: + measurement_controller.set_initial_pose((-4.1, -2.5, 0.0)) + staging = countermeasure_controller.navigate_route( + ((0.0, -1.00, 0.0), (1.20, -1.00, 0.0)), + final_yaw_rad=0.0, + ) + if not staging.success: + raise RuntimeError(f"countermeasure staging motion failed: {staging.message}") + if args.interactive: + # Keep later operator-triggered physics motion observable instead of + # advancing hundreds of rendered steps as fast as the GPU allows. + stepper.frame_delay_s = max(stepper.frame_delay_s, 1.0 / 120.0) + simulation = IsaacRadiationSimulation.from_config( + stage, + ROOT / "configs/scenarios/vertical_slice.runtime.json", + ) + dashboard.simulation = simulation + candidate_config = SceneCandidateConfig( + countermeasure_pose_path=robot_config.panda_base_path, + measurement_pose_path=robot_config.measurement_articulation, + end_effector_offset_m=(0.72, 0.0, 0.0), + decon_end_effector_offset_m=(0.90, 0.0, 0.0), + # Live contact validation shows that less than 0.90 m lets the + # Ridgeback envelope push the drum before the gripper reaches its + # stand-off handle. Preserve this physical separation; the planner + # changes approach side when a later task needs a different workspace. + object_end_effector_offset_m=(0.90, 0.0, 0.0), + manipulator_workspace_m=0.95, + mobile_clearance_m=0.55, + ignored_collision_paths=( + ("/World/Environment",) if args.decontamination_smoke_test else () + ), ) generator = IsaacActionCandidateGenerator( - stage, simulation, controller=countermeasure_controller + stage, + simulation, + controller=countermeasure_controller, + config=candidate_config, ) belief = BeliefState( - ("/World/ContaminatedFloor",), + (robot_config.decon_surface_path,), np.asarray([2.4e7]), np.asarray([[4.0e12]]), RevisionState(), @@ -404,9 +1812,20 @@ def _run_validation(app: Any, args: argparse.Namespace, panel: _ValidationPanel) ) decontaminator = ContactDrivenDecontaminator( stage, - "/World/CountermeasureRobot/DeconContactTool", - "/World/ContaminatedFloor", - DecontaminationConfig(max_contact_distance_m=0.06), + robot_config.decon_tool_path, + robot_config.decon_surface_path, + DecontaminationConfig( + footprint_points_local_m=tuple( + (float(local_x), float(local_y), 0.0) + for local_x in np.linspace(-0.085, 0.085, 9) + for local_y in np.linspace(-0.065, 0.065, 9) + ), + treatment_axis_local=(0.0, 0.0, 1.0), + max_contact_distance_m=0.045, + max_surface_speed_m_s=0.35, + rate_constant_s_inv=1.1, + transfer_mode="transfer_to_waste", + ), ) def estimator(measurement: object, previous: BeliefState | None) -> BeliefState: @@ -419,17 +1838,201 @@ def estimator(measurement: object, previous: BeliefState | None) -> BeliefState: estimator, controller=countermeasure_controller, measurement_controller=measurement_controller, - decontaminators={"/World/ContaminatedFloor": decontaminator}, + decontaminators={robot_config.decon_surface_path: decontaminator}, resources=resources, artifact_path=ROOT / "artifacts/ui/latest_workflow.json", ) checker = DeterministicFeasibilityChecker() _complete(services.initialize()) + dashboard.configure_system_paths( + stage_path=stage_path, + config_path=ROOT / "configs/scenarios/vertical_slice.runtime.json", + display_name=selection.profile.display_name, + simulation=simulation, + selection=selection, + ) + dashboard.bind_workflow(services, belief) + + if args.decontamination_smoke_test: + candidates = generator.generate_decon_actions(belief) + if not candidates: + raise RuntimeError("the selected environment generated no decontamination action") + candidate = candidates[0] + feasibility = checker.evaluate(candidate, resources) + if not feasibility.feasible: + raise RuntimeError( + "decontamination action is infeasible in the selected environment: " + f"{feasibility.reasons}; facts={_jsonable(candidate.feasibility)}" + ) + action = replace( + candidate.action, + predicted_duration_s=args.decon_duration_s, + parameters={ + **candidate.action.parameters, + "duration_s": args.decon_duration_s, + "decon_media": args.decon_duration_s, + }, + ) + activity_before = _activity_total(activity_path) + robot_before = _world_position(stage, robot_config.countermeasure_root) + panel.update( + "Executing Fukushima articulated contact decontamination", + 0, + 1, + _jsonable(candidate.feasibility), + ) + dashboard._robot_monitor.begin_action(action) + try: + result = _complete(services.execute(action)) + except Exception: + dashboard._robot_monitor.finish_action(success=False) + raise + dashboard._robot_monitor.finish_action( + success=True, + public_details=result.public_details, + ) + activity_after = _activity_total(activity_path) + robot_after = _world_position(stage, robot_config.countermeasure_root) + public_details = dict(result.public_details) + motion = dict(public_details.get("motion_audit", {})) + accepted_contacts = int(public_details.get("accepted_contacts", 0)) + coverage_fraction = float(motion.get("coverage_fraction", 0.0)) + invariants = { + "articulated_action_completed": result.status == ActionStatus.COMPLETED, + "visible_irregular_surface_reduced": activity_after < activity_before, + "verified_contacts_recorded": accepted_contacts > 0, + "meaningful_surface_coverage": coverage_fraction >= 0.35, + "franka_arm_moved": countermeasure_controller.arm_joint_excursion_rad > 0.2, + "fukushima_geometry_in_transport": len(simulation.transport.geometry_paths) >= 995, + "catalog_spawn_stayed_supported": spawn_vertical_drop_m <= 0.15, + "separate_validation_facility_absent": not stage.GetPrimAtPath( + "/World/RemoteDeconFacility" + ).IsValid(), + } + failed = [name for name, passed in invariants.items() if not passed] + payload = { + "success": not failed, + "mode": "fukushima_decontamination_smoke_test", + "selection": selection.as_dict(), + "stage": str(stage_path), + "environment_manifest": ( + None if environment_manifest is None else str(environment_manifest) + ), + "environment_operation_offset_m": environment_operation_offset_m, + "spawn_anchors": { + "countermeasure": "ground-primary", + "measurement": "ground-secondary", + "decontamination_surface": "decon-surface", + }, + "spawn_vertical_drop_m": spawn_vertical_drop_m, + "robot_model": "Clearpath Ridgeback + Franka Panda", + "surface_path": robot_config.decon_surface_path, + "activity_before_bq": activity_before, + "activity_after_bq": activity_after, + "removed_fraction": ( + (activity_before - activity_after) / activity_before + if activity_before > 0.0 + else 0.0 + ), + "accepted_contacts": accepted_contacts, + "coverage_fraction": coverage_fraction, + "robot_displacement_m": float(np.linalg.norm(robot_after - robot_before)), + "arm_joint_excursion_rad": countermeasure_controller.arm_joint_excursion_rad, + "transport_geometries": len(simulation.transport.geometry_paths), + "result": _jsonable(result.public_view()), + "robot_monitor": dashboard.robot_monitor_audit(), + "invariants": invariants, + "failed_invariants": failed, + } + dashboard.set_workflow_view(services.workflow_view()) + assert camera_eye is not None and camera_target is not None + _configure_decon_room_camera( + stage, + eye_m=camera_eye.translation_m, + target_m=camera_target.translation_m, + ) + panel.update( + "Completed Fukushima articulated contact decontamination", + 1, + 1, + payload, + ) + if failed: + raise DecontaminationSmokeValidationError( + f"Fukushima decontamination invariants failed: {failed}", payload + ) + return payload + + if args.complex_natural_language_validation: + return _run_complex_natural_language_validation( + app, + args, + panel, + dashboard, + stage=stage, + stage_path=stage_path, + asset_manifest=asset_manifest, + robot_config=robot_config, + activity_path=activity_path, + generator=generator, + countermeasure_controller=countermeasure_controller, + measurement_controller=measurement_controller, + simulation=simulation, + ) + + if args.interactive: + context_view = dashboard.natural_language_context() + command_result = None + if args.initial_command: + submission = _complete_with_updates( + app, + dashboard.submit_natural_language_instruction( + args.initial_command, + confirm_physical=args.confirm_initial_command, + ), + before_update=dashboard.process_pending_natural_language_actions, + ) + command_result = { + "instruction": args.initial_command, + "plan": submission.validated.plan.model_dump(mode="json"), + "executed": submission.executed, + "results": list(submission.results), + } + if submission.executed: + _configure_decon_room_camera(stage) + panel.update( + "Ready for natural-language operation", + 0, + 0, + { + "languages": ["en"], + "available_actions": len(context_view.available_actions), + "local_inference": "llama.cpp + Qwen3-4B GGUF", + }, + ) + panel.hide() + return { + "success": True, + "mode": "interactive", + "stage": str(stage_path), + "robot_models": { + "countermeasure": "Clearpath Ridgeback + Franka Panda", + "measurement": "NVIDIA Nova Carter", + }, + "natural_language": { + "languages": ["en"], + "backend": "bundled llama.cpp", + "model": "Qwen3-4B-Q4_K_M.gguf", + "available_actions": len(context_view.available_actions), + "confirmation_required_for_physical_actions": True, + "initial_command": command_result, + }, + } total_operations = 8 completed = 0 records: list[dict[str, Any]] = [] - initial_robot_position = _world_position(stage, "/World/MeasurementRobot") + initial_robot_position = _world_position(stage, robot_config.measurement_articulation) initial_shield_position = _world_position(stage, "/World/LeadShield") initial_drum_position = _world_position(stage, "/World/HiddenContaminatedDrum") initial_obstacle_position = _world_position(stage, "/World/MovableObstacle") @@ -455,7 +2058,8 @@ def execute_candidate( } if "pickup_base_position_m" in parameters: robot_position = generator.probe.world_position( - generator.config.countermeasure_robot_path + generator.config.countermeasure_pose_path + or generator.config.countermeasure_robot_path ) pickup_position = np.asarray(parameters["pickup_base_position_m"]) placement_position = np.asarray(parameters["placement_base_position_m"]) @@ -508,8 +2112,22 @@ def execute_candidate( measurement_candidates = generator.generate_measurement_actions(belief) if len(measurement_candidates) < 4: raise RuntimeError("the scene did not generate all four measurement stations") - panel.update("Moving measurement robot through four stations", completed, total_operations) - for candidate in measurement_candidates: + measurement_station_paths = tuple( + candidate.action.target_prim_path for candidate in measurement_candidates + ) + executed_measurement_action_ids: list[str] = [] + panel.update("Moving measurement robot through all stations", completed, total_operations) + for station_path in measurement_station_paths: + # Each route must begin at Nova Carter's live pose. Reusing the routes + # generated before the first station can send a later leg through an + # obstacle because those routes all share the original start position. + # Rebuild collision bounds too: movable props may have settled since + # the dashboard's initial candidate preview populated the cache. + generator.probe.invalidate_collision_cache() + live_candidates = generator.generate_measurement_actions(belief) + candidate = next( + item for item in live_candidates if item.action.target_prim_path == station_path + ) report = checker.evaluate(candidate, resources) if not report.feasible: raise RuntimeError( @@ -517,27 +2135,32 @@ def execute_candidate( ) result = _complete(services.execute(candidate.action)) if result.status != ActionStatus.COMPLETED: - position, orientation = measurement_controller._base_pose() - velocity = measurement_controller._numpy( - measurement_controller._base.get_linear_velocities() - )[0] + position = _world_position(stage, robot_config.measurement_articulation) + target = candidate.action.target_pose_world[:3, 3] + direct_blockers = _path_blockers( + generator.probe, + position, + target, + moving_robot_path=robot_config.measurement_root, + ) raise RuntimeError( "measurement move failed: " f"details={result.public_details}, position_m={position.tolist()}, " - f"orientation_wxyz={orientation.tolist()}, velocity_m_s={velocity.tolist()}, " - f"target_m={candidate.action.target_pose_world[:3, 3].tolist()}" + f"target_m={target.tolist()}, " + f"direct_blockers={direct_blockers}" ) + executed_measurement_action_ids.append(candidate.action.action_id) initial_measurement = _complete(services.measure()) belief = services.estimate(initial_measurement, None) completed += 1 records.append( { - "label": "four-station measurement", - "station_actions": [item.action.action_id for item in measurement_candidates], + "label": "all-station measurement", + "station_actions": executed_measurement_action_ids, "measurement": _measurement_rows(initial_measurement), } ) - panel.update("Completed: four-station measurement", completed, total_operations, records[-1]) + panel.update("Completed: all-station measurement", completed, total_operations, records[-1]) _hold(app, args.phase_hold_s) decon_candidate = generator.generate_decon_actions(belief)[0] @@ -550,25 +2173,8 @@ def execute_candidate( "decon_media": args.decon_duration_s, }, ) - precontact = countermeasure_controller.navigate_to( - decon_action.parameters["pickup_base_position_m"] - ) - if not precontact.success: - raise RuntimeError(f"decontamination pre-positioning failed: {precontact.message}") - _position_contact_tool( - stage, - app, - _surface_center(stage, "/World/ContaminatedFloor"), - ) - contact_probe = _probe_decon_contacts(decontaminator) - print(json.dumps({"decon_contact_probe": contact_probe}), flush=True) - # Pre-positioning above established and probed the physical contact pose. - # Do not let the service navigate the parent base a second time. - decon_parameters = dict(decon_action.parameters) - decon_parameters.pop("pickup_base_position_m", None) - decon_action = replace(decon_action, parameters=decon_parameters) execute_candidate( - "contact decontamination", + "IK raster contact decontamination", decon_candidate, action_override=decon_action, ) @@ -586,7 +2192,10 @@ def execute_candidate( "reasons": checker.evaluate(item, resources).reasons, "to_pickup_blockers": _path_blockers( generator.probe, - generator.probe.world_position(generator.config.countermeasure_robot_path), + generator.probe.world_position( + generator.config.countermeasure_pose_path + or generator.config.countermeasure_robot_path + ), np.asarray(item.action.parameters["pickup_base_position_m"]), excluded_paths=(str(item.action.target_prim_path),), ), @@ -606,7 +2215,11 @@ def execute_candidate( current_shield = _world_position(stage, "/World/LeadShield") generated_corrections = generator.generate_shield_actions(belief) correction_candidates = [ - item for item in generated_corrections if checker.evaluate(item, resources).feasible + item + for item in generated_corrections + if item.action.action_type == ActionType.MOVE_SHIELD + and item.action.target_prim_path == shield_candidate.action.target_prim_path + and checker.evaluate(item, resources).feasible ] if not correction_candidates: diagnostics = [ @@ -625,8 +2238,7 @@ def execute_candidate( np.linalg.norm(item.action.target_pose_world[:3, 3] - current_shield) ), ) - correction_action = replace(correction.action, action_type=ActionType.MOVE_SHIELD) - execute_candidate("shield pose correction", correction, action_override=correction_action) + execute_candidate("shield pose correction", correction) object_candidates = generator.generate_move_remove_actions(belief) drum_move = next( @@ -674,9 +2286,8 @@ def execute_candidate( dashboard.set_workflow_view(services.workflow_view()) panel.update("Completed: final verification", completed, total_operations, records[-1]) - with np.load(activity_path, allow_pickle=False) as payload: - floor_activity_after = float(np.sum(payload["activity_bq"])) - final_robot_position = _world_position(stage, "/World/MeasurementRobot") + floor_activity_after = _activity_total(activity_path) + final_robot_position = _world_position(stage, robot_config.measurement_articulation) final_shield_position = _world_position(stage, "/World/LeadShield") final_obstacle_position = _world_position(stage, "/World/MovableObstacle") drum = stage.GetPrimAtPath("/World/HiddenContaminatedDrum") @@ -712,6 +2323,9 @@ def execute_candidate( "post_action_measurement_available": bool(final_measurement), "residual_available": final_diagnosis is not None, "native_transport_used": simulation.transport.statistics["native_trace_calls"] > 0, + "franka_arm_moved": countermeasure_controller.arm_joint_excursion_rad > 0.2, + "articulated_countermeasure_robot": len(countermeasure_controller.dof_names) >= 12, + "articulated_measurement_robot": len(measurement_controller.robot.dof_names) >= 7, } failed = [name for name, passed in invariants.items() if not passed] if failed: @@ -720,6 +2334,23 @@ def execute_candidate( return { "success": True, "stage": str(stage_path), + "assets": asset_manifest, + "robot_models": { + "countermeasure": "Clearpath Ridgeback + Franka Panda", + "measurement": "NVIDIA Nova Carter", + }, + "dof_audit": { + "countermeasure_dofs": list(countermeasure_controller.dof_names), + "measurement_dofs": list(measurement_controller.robot.dof_names), + "franka_arm_joint_excursion_rad": (countermeasure_controller.arm_joint_excursion_rad), + }, + "motion_policy": { + "teleport_during_operations": False, + "base_control": "articulation joints / differential wheel joints", + "arm_control": "Lula IK to seven Franka joint targets", + "grasp_control": "finger closure plus hand-attached PhysX joint", + "decon_control": "IK raster with live PhysX contact acceptance", + }, "operations": records, "invariants": invariants, "initial": { @@ -741,11 +2372,12 @@ def execute_candidate( "transport_statistics": simulation.transport.statistics, "controller_trace": countermeasure_controller.trace, "measurement_controller_trace": measurement_controller.trace, + "robot_monitor": dashboard.robot_monitor_audit(), } -def main() -> int: - args = _arguments() +def main(argv: list[str] | None = None) -> int: + args = _arguments(argv) from isaacsim import SimulationApp app = SimulationApp( @@ -758,14 +2390,27 @@ def main() -> int: } ) panel = None + dashboard = None payload: dict[str, Any] exit_code = 0 try: + from radcounter.isaac.ui.dashboard import RadCounterDashboard + + dashboard = RadCounterDashboard("radcounter.gui.validation") panel = _ValidationPanel() - payload = _run_validation(app, args, panel) + payload = _run_validation(app, args, panel, dashboard) except Exception as error: exit_code = 1 + preserved = ( + dict(error.audit) + if isinstance( + error, + (ComplexNaturalLanguageValidationError, DecontaminationSmokeValidationError), + ) + else {} + ) payload = { + **preserved, "success": False, "error": f"{type(error).__name__}: {error}", "traceback": traceback.format_exc(), @@ -775,20 +2420,30 @@ def main() -> int: print( json.dumps({"validation_artifact": str(args.artifact), **payload}, default=str), flush=True ) - if panel is not None: + if panel is not None and not args.interactive: panel.finish(exit_code == 0, args.artifact) + total = 7 if args.complex_natural_language_validation else 8 + disposition = "remains open for inspection" if args.keep_open else "will close" panel.update( - "PASS - GUI remains open for inspection" - if exit_code == 0 - else "FAIL - GUI remains open for inspection", - 8 if exit_code == 0 else 0, - 8, + f"PASS - GUI {disposition}" if exit_code == 0 else f"FAIL - GUI {disposition}", + total if exit_code == 0 else 0, + total, payload, ) - if args.keep_open: - while app.is_running(): - app.update() - app.close() + elif panel is not None and exit_code != 0: + panel.finish(False, args.artifact) + try: + if args.keep_open: + frame_limiter = _GuiFrameRateLimiter(args.max_fps) + while app.is_running(): + if dashboard is not None: + dashboard.process_pending_natural_language_actions() + app.update() + frame_limiter.wait() + finally: + if dashboard is not None: + dashboard.shutdown() + app.close() return exit_code diff --git a/scripts/run_host_gates.sh b/scripts/run_host_gates.sh index 67c7690..eae4a7c 100755 --- a/scripts/run_host_gates.sh +++ b/scripts/run_host_gates.sh @@ -20,11 +20,16 @@ uv run ruff check . physics_actions_gate.py \ articulation_ik_gate.py \ real_robot_gate.py \ + articulated_object_gate.py \ dashboard_gate.py \ workflow_gate.py do uv run --project "$RADCOUNTER_ISAAC_ROOT" --locked python "tests/isaac/$gate" done + uv run --project "$RADCOUNTER_ISAAC_ROOT" --locked \ + python scripts/run_gui_validation.py \ + --headless --no-keep-open --phase-hold-s 0 \ + --artifact artifacts/gui-validation/host-gate.json ) ( diff --git a/scripts/run_real_robot_validation.py b/scripts/run_real_robot_validation.py index 0cd1fda..9580ce0 100644 --- a/scripts/run_real_robot_validation.py +++ b/scripts/run_real_robot_validation.py @@ -122,17 +122,6 @@ def update(self, phase: str, detail: str) -> None: self.detail.set_value(detail) -def _create_activity_map(path: Path) -> None: - path.parent.mkdir(parents=True, exist_ok=True) - np.savez_compressed( - path, - triangle_indices=np.arange(8, dtype=np.int64), - activity_bq=np.full(8, 2.5e6, dtype=np.float64), - cumulative_treatment_exposure=np.zeros(8, dtype=np.float64), - last_treated_step=np.full(8, -1, dtype=np.int64), - ) - - def _configure_camera( stage: Any, *, @@ -198,6 +187,7 @@ def _run(app: Any, args: argparse.Namespace, panel: _Panel) -> dict[str, Any]: RidgebackFrankaController, add_real_robot_references, author_real_robot_task_scene, + create_decontamination_activity_map, enable_real_robot_extensions, ) from radcounter.isaac.runtime import IsaacRadiationSimulation @@ -218,7 +208,7 @@ def _run(app: Any, args: argparse.Namespace, panel: _Panel) -> dict[str, Any]: for _ in range(120): app.update() activity_path = args.artifact.parent / "workbench_activity.npz" - _create_activity_map(activity_path) + create_decontamination_activity_map(activity_path) author_real_robot_task_scene(stage, activity_path, config=config) _configure_camera(stage) for _ in range(60): @@ -298,24 +288,36 @@ def _run(app: Any, args: argparse.Namespace, panel: _Panel) -> dict[str, Any]: "Contact-driven decontamination", "Franka is lowering a physical pad and sweeping the contaminated mesh.", ) - if not franka.move_base((-0.08, 0.0, 0.0)): + travel_posture = franka.stow_arm() + if not travel_posture.success: + raise AssertionError( + f"Franka could not enter its travel posture: {travel_posture}" + ) + decon_base_x = config.decon_workbench_center_m[0] - 0.90 + decon_base_y = config.decon_workbench_center_m[1] + decon_route = ( + (0.0, -1.50, 0.0), + (5.20, -1.50, 0.0), + (8.90, -1.50, 0.0), + (10.30, -1.50, 0.0), + (10.30, decon_base_y, 0.0), + (decon_base_x, decon_base_y, 0.0), + ) + navigation_report = franka.navigate_route(decon_route, final_yaw_rad=0.0) + if not navigation_report.success: raise AssertionError( - "Ridgeback could not establish workbench clearance: " - f"target={franka.last_base_target.tolist()}, " - f"actual={franka.last_base_positions.tolist()}" + "Ridgeback could not reach the remote decontamination wall: " + f"{navigation_report}" ) decontaminator = ContactDrivenDecontaminator( stage, config.decon_tool_path, config.decon_surface_path, DecontaminationConfig( - footprint_points_local_m=( - (-0.06, -0.045, 0.0), - (0.0, -0.045, 0.0), - (0.06, -0.045, 0.0), - (-0.06, 0.045, 0.0), - (0.0, 0.045, 0.0), - (0.06, 0.045, 0.0), + footprint_points_local_m=tuple( + (float(local_x), float(local_y), 0.0) + for local_x in np.linspace(-0.085, 0.085, 9) + for local_y in np.linspace(-0.065, 0.065, 9) ), treatment_axis_local=(0.0, 0.0, 1.0), max_contact_distance_m=0.045, @@ -324,25 +326,26 @@ def _run(app: Any, args: argparse.Namespace, panel: _Panel) -> dict[str, Any]: transfer_mode="transfer_to_waste", ), ) - decon_waypoints = ( - (0.54, -0.15, 0.790), - (0.68, -0.15, 0.790), - (0.82, -0.15, 0.790), - (0.82, 0.15, 0.790), - (0.68, 0.15, 0.790), - (0.54, 0.15, 0.790), - ) - decon_report = franka.execute_decontamination( - decontaminator, - decon_waypoints, - dwell_frames=36, - ) + decon_report = franka.execute_surface_decontamination(decontaminator, 1.5) captures["decontamination"] = _capture( app, args.artifact.parent / "frames", "02_decontamination", args.capture ) if not decon_report.success: raise AssertionError(f"physical decontamination failed: {decon_report}") + return_route = ( + (10.30, decon_base_y, 0.0), + (10.30, -1.50, 0.0), + (8.90, -1.50, 0.0), + (5.20, -1.50, 0.0), + (0.0, -1.25, 0.0), + ) + return_report = franka.navigate_route(return_route, final_yaw_rad=0.0) + if not return_report.success: + raise AssertionError( + f"Ridgeback could not return from the decontamination room: {return_report}" + ) + panel.update( "Shield pick-and-place", "Franka is opening, grasping, lifting, carrying, and releasing a 2.2 kg shield.", diff --git a/source/extensions/radcounter.isaac/config/extension.toml b/source/extensions/radcounter.isaac/config/extension.toml index 167d79f..36b1b04 100644 --- a/source/extensions/radcounter.isaac/config/extension.toml +++ b/source/extensions/radcounter.isaac/config/extension.toml @@ -7,6 +7,9 @@ keywords = ["isaac", "radiation", "robotics", "countermeasure"] [dependencies] "omni.kit.uiapp" = {} +"omni.ui.scene" = {} +"omni.kit.viewport.utility" = {} +"omni.kit.viewport.window" = {} "omni.appwindow" = {} "omni.usd" = {} "omni.timeline" = {} diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/natural_language/__init__.py b/source/extensions/radcounter.isaac/radcounter/isaac/natural_language/__init__.py new file mode 100644 index 0000000..6d69b80 --- /dev/null +++ b/source/extensions/radcounter.isaac/radcounter/isaac/natural_language/__init__.py @@ -0,0 +1,13 @@ +"""Natural-language application control for the Isaac host.""" + +from .controller import ( + CommandSubmission, + NaturalLanguageCommandController, + NaturalLanguageCommandHost, +) + +__all__ = [ + "CommandSubmission", + "NaturalLanguageCommandController", + "NaturalLanguageCommandHost", +] diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/natural_language/controller.py b/source/extensions/radcounter.isaac/radcounter/isaac/natural_language/controller.py new file mode 100644 index 0000000..9cf258d --- /dev/null +++ b/source/extensions/radcounter.isaac/radcounter/isaac/natural_language/controller.py @@ -0,0 +1,320 @@ +"""Application controller joining local inference to allowlisted host commands.""" + +from __future__ import annotations + +import asyncio +import json +from collections.abc import Mapping +from dataclasses import dataclass +from datetime import UTC, datetime +from pathlib import Path +from typing import Protocol + +from radcounter.core.natural_language import ( + CommandContext, + CommandInterpreter, + CommandStep, + CompletionCriterion, + LlamaCppRuntime, + OpenAICompatibleCommandInterpreter, + ValidatedCommandPlan, + validate_command_plan, +) + + +class NaturalLanguageCommandHost(Protocol): + def natural_language_context(self) -> CommandContext: ... + + async def execute_natural_language_step( + self, step: CommandStep + ) -> Mapping[str, object]: ... + + +@dataclass(frozen=True, slots=True) +class CommandSubmission: + validated: ValidatedCommandPlan + executed: bool + results: tuple[Mapping[str, object], ...] = () + + +class WorkflowExecutionError(RuntimeError): + """A bounded workflow failed after one or more auditable operations.""" + + def __init__( + self, + message: str, + *, + results: tuple[Mapping[str, object], ...], + step_index: int, + attempt: int, + ) -> None: + super().__init__(message) + self.results = results + self.step_index = step_index + self.attempt = attempt + + +class NaturalLanguageCommandController: + """Interpret, confirm, execute, and audit one bounded instruction at a time.""" + + def __init__( + self, + host: NaturalLanguageCommandHost, + *, + runtime: LlamaCppRuntime | None = None, + interpreter: CommandInterpreter | None = None, + audit_path: str | Path | None = None, + ) -> None: + self.host = host + self.runtime = runtime if runtime is not None else LlamaCppRuntime() + self.interpreter = interpreter + self.audit_path = None if audit_path is None else Path(audit_path) + self.pending: ValidatedCommandPlan | None = None + self._pending_instruction = "" + self._busy = False + + @property + def busy(self) -> bool: + return self._busy + + async def _ensure_interpreter(self) -> CommandInterpreter: + if self.interpreter is not None: + return self.interpreter + endpoint = await asyncio.to_thread(self.runtime.start) + self.interpreter = OpenAICompatibleCommandInterpreter( + endpoint, + model=self.runtime.config.model_alias, + ) + return self.interpreter + + async def submit(self, instruction: str) -> CommandSubmission: + if self._busy: + raise RuntimeError("another natural-language command is already running") + self._busy = True + try: + interpreter = await self._ensure_interpreter() + context = self.host.natural_language_context() + plan = await interpreter.interpret(instruction, context) + validated = validate_command_plan(plan, context) + if validated.requires_confirmation: + self.pending = validated + self._pending_instruction = instruction + self._write_audit(instruction, validated, "awaiting_confirmation", ()) + return CommandSubmission(validated, False) + results = await self._execute(validated) + self._write_audit(instruction, validated, "completed", results) + return CommandSubmission(validated, True, results) + except Exception as error: + self._write_failure(instruction, error) + raise + finally: + self._busy = False + + async def confirm(self) -> CommandSubmission: + if self._busy: + raise RuntimeError("another natural-language command is already running") + if self.pending is None: + raise RuntimeError("there is no command awaiting confirmation") + self._busy = True + validated = self.pending + instruction = self._pending_instruction + self.pending = None + self._pending_instruction = "" + try: + # Revalidate against the live scene immediately before execution. + validated = validate_command_plan(validated.plan, self.host.natural_language_context()) + results = await self._execute(validated) + self._write_audit(instruction, validated, "completed", results) + return CommandSubmission(validated, True, results) + except Exception as error: + self._write_failure(instruction, error) + raise + finally: + self._busy = False + + def cancel(self) -> None: + if self.pending is not None: + self._write_audit( + self._pending_instruction, + self.pending, + "cancelled", + (), + ) + self.pending = None + self._pending_instruction = "" + + async def _execute( + self, validated: ValidatedCommandPlan + ) -> tuple[Mapping[str, object], ...]: + results: list[Mapping[str, object]] = [] + for step_index, step in enumerate(validated.plan.steps, start=1): + initial_activity_bq: float | None = None + for attempt in range(1, step.max_attempts + 1): + # Earlier physical steps and earlier attempts may change + # candidates, feasibility, action type, or capabilities. + live_step_plan = validated.plan.model_copy(update={"steps": (step,)}) + try: + validate_command_plan( + live_step_plan, self.host.natural_language_context() + ) + raw_result = await self.host.execute_natural_language_step(step) + except Exception as error: + raise WorkflowExecutionError( + f"workflow step {step_index} attempt {attempt} failed: " + f"{type(error).__name__}: {error}", + results=tuple(results), + step_index=step_index, + attempt=attempt, + ) from error + result = dict(raw_result) + result["workflow_step"] = step_index + result["attempt"] = attempt + result["max_attempts"] = step.max_attempts + if step.until is not None: + value, initial_activity_bq = self._completion_value( + step.until.criterion, + result, + initial_activity_bq=initial_activity_bq, + ) + condition_met = self._condition_met( + step.until.criterion, + value, + step.until.threshold, + ) + result["completion_condition"] = { + "criterion": step.until.criterion.value, + "threshold": step.until.threshold, + "observed": value, + "met": condition_met, + } + results.append(result) + if condition_met: + break + else: + results.append(result) + return tuple(results) + + @staticmethod + def _public_metric(result: Mapping[str, object], *path: str) -> float: + value: object = result + for component in path: + if not isinstance(value, Mapping) or component not in value: + dotted = ".".join(path) + raise RuntimeError( + f"public result does not provide completion metric: {dotted}" + ) + value = value[component] + if not isinstance(value, (int, float)): + raise RuntimeError( + f"public completion metric is not numeric: {'.'.join(path)}" + ) + return float(value) + + @classmethod + def _completion_value( + cls, + criterion: CompletionCriterion, + result: Mapping[str, object], + *, + initial_activity_bq: float | None, + ) -> tuple[float, float | None]: + if criterion == CompletionCriterion.DECONTAMINATION_REMOVED_FRACTION_AT_LEAST: + before = cls._public_metric( + result, "public_details", "motion_audit", "activity_before_bq" + ) + after = cls._public_metric( + result, "public_details", "motion_audit", "activity_after_bq" + ) + baseline = before if initial_activity_bq is None else initial_activity_bq + removed_fraction = 1.0 - after / baseline if baseline > 0.0 else 1.0 + return (max(0.0, min(1.0, removed_fraction)), baseline) + if criterion == CompletionCriterion.DECONTAMINATION_COVERAGE_FRACTION_AT_LEAST: + return ( + cls._public_metric( + result, "public_details", "motion_audit", "coverage_fraction" + ), + initial_activity_bq, + ) + if criterion == CompletionCriterion.DECONTAMINATION_REMAINING_FRACTION_AT_MOST: + before = cls._public_metric( + result, "public_details", "motion_audit", "activity_before_bq" + ) + after = cls._public_metric( + result, "public_details", "motion_audit", "activity_after_bq" + ) + baseline = before if initial_activity_bq is None else initial_activity_bq + return (after / baseline if baseline > 0.0 else 0.0, baseline) + if criterion == CompletionCriterion.MEASURED_RATE_CPS_AT_MOST: + return ( + cls._public_metric(result, "maximum_measured_rate_cps"), + initial_activity_bq, + ) + if criterion == CompletionCriterion.SHIELD_PLACEMENT_ERROR_M_AT_MOST: + return ( + cls._public_metric( + result, "public_details", "motion_audit", "placement_error_m" + ), + initial_activity_bq, + ) + raise RuntimeError(f"unsupported completion criterion: {criterion}") + + @staticmethod + def _condition_met( + criterion: CompletionCriterion, + observed: float, + threshold: float, + ) -> bool: + at_least = { + CompletionCriterion.DECONTAMINATION_REMOVED_FRACTION_AT_LEAST, + CompletionCriterion.DECONTAMINATION_COVERAGE_FRACTION_AT_LEAST, + } + return observed >= threshold if criterion in at_least else observed <= threshold + + def _append_audit(self, payload: Mapping[str, object]) -> None: + if self.audit_path is None: + return + self.audit_path.parent.mkdir(parents=True, exist_ok=True) + with self.audit_path.open("a", encoding="utf-8") as stream: + stream.write(json.dumps(payload, ensure_ascii=False, sort_keys=True, default=str)) + stream.write("\n") + + def _write_audit( + self, + instruction: str, + validated: ValidatedCommandPlan, + status: str, + results: tuple[Mapping[str, object], ...], + ) -> None: + self._append_audit( + { + "timestamp": datetime.now(UTC).isoformat(), + "instruction": instruction, + "plan": validated.plan.model_dump(mode="json"), + "requires_confirmation": validated.requires_confirmation, + "warnings": list(validated.warnings), + "status": status, + "results": list(results), + } + ) + + def _write_failure(self, instruction: str, error: Exception) -> None: + payload: dict[str, object] = { + "timestamp": datetime.now(UTC).isoformat(), + "instruction": instruction, + "status": "failed", + "error_type": type(error).__name__, + "error": str(error), + } + if isinstance(error, WorkflowExecutionError): + payload.update( + { + "failed_step": error.step_index, + "failed_attempt": error.attempt, + "partial_results": list(error.results), + } + ) + self._append_audit(payload) + + def shutdown(self) -> None: + self.cancel() + self.runtime.stop() diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py b/source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py index d2869f3..4e80074 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py @@ -17,12 +17,21 @@ class SceneCandidateConfig: countermeasure_robot_path: str = "/World/CountermeasureRobot" measurement_robot_path: str = "/World/MeasurementRobot" + countermeasure_pose_path: str | None = None + measurement_pose_path: str | None = None task_detector_path: str = "/World/DetectorStations/Protected" disposal_zone_path: str = "/World/DisposalZone" end_effector_offset_m: tuple[float, float, float] = (1.2, 0.0, 0.0) + decon_end_effector_offset_m: tuple[float, float, float] | None = None + object_end_effector_offset_m: tuple[float, float, float] | None = None mobile_clearance_m: float = 0.28 manipulator_workspace_m: float = 1.55 - shield_line_fractions: tuple[float, ...] = (0.35, 0.5, 0.65) + manipulator_vertical_range_m: tuple[float, float] = (-0.35, 1.15) + # Include a near-source slot inside the remote room as well as the + # original-cell slots. In a multi-room facility the straight line to the + # protected station crosses the west wall around 35%, so 25% is the first + # physically reachable host-owned placement slot for a staged panel. + shield_line_fractions: tuple[float, ...] = (0.25, 0.35, 0.5, 0.65) measurement_duration_s: float = 2.0 shield_duration_s: float = 45.0 decon_duration_s: float = 20.0 @@ -34,6 +43,10 @@ class SceneCandidateConfig: (3.4, -1.8), ) dose_proxy_to_sv_h: float = 1.0e-12 + # A compound CAD building may expose one collision mesh whose AABB covers + # all of its traversable rooms. Curated spawn anchors can opt that root + # out of the coarse AABB planner while PhysX still enforces its triangles. + ignored_collision_paths: tuple[str, ...] = () class IsaacSceneFeasibilityProbe: @@ -48,6 +61,7 @@ def __init__( self.stage = stage self.config = config self.controller = controller + self._collision_bounds_cache: tuple[tuple[str, np.ndarray, np.ndarray], ...] | None = None @staticmethod def _attribute(prim: Any, name: str, default: object = None) -> object: @@ -89,16 +103,28 @@ def bounds(self, prim_or_path: Any) -> tuple[np.ndarray, np.ndarray] | None: return minimum, maximum def _collision_bounds(self) -> tuple[tuple[str, np.ndarray, np.ndarray], ...]: - from pxr import UsdPhysics + from pxr import Usd, UsdGeom, UsdPhysics + + if self._collision_bounds_cache is not None: + return self._collision_bounds_cache result: list[tuple[str, np.ndarray, np.ndarray]] = [] + cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) for prim in self.stage.Traverse(): if not prim.HasAPI(UsdPhysics.CollisionAPI): continue - bounds = self.bounds(prim) - if bounds is not None: - result.append((str(prim.GetPath()), bounds[0], bounds[1])) - return tuple(result) + aligned = cache.ComputeWorldBound(prim).ComputeAlignedRange() + lower = np.asarray(aligned.GetMin(), dtype=np.float64) + upper = np.asarray(aligned.GetMax(), dtype=np.float64) + if np.all(np.isfinite(lower)) and np.all(np.isfinite(upper)): + result.append((str(prim.GetPath()), lower, upper)) + self._collision_bounds_cache = tuple(result) + return self._collision_bounds_cache + + def invalidate_collision_cache(self) -> None: + """Refresh collision geometry before a new live-scene planning pass.""" + + self._collision_bounds_cache = None @staticmethod def _is_descendant(path: str, parent: str) -> bool: @@ -108,11 +134,19 @@ def _navigation_obstacles( self, *, excluded_paths: tuple[str, ...] = (), + moving_robot_path: str | None = None, + carried_object_path: str | None = None, + carried_base_position_m: np.ndarray | None = None, ) -> tuple[tuple[str, np.ndarray, np.ndarray], ...]: + ignored_robots = ( + (self.config.countermeasure_robot_path, self.config.measurement_robot_path) + if moving_robot_path is None + else (moving_robot_path,) + ) ignored = ( - self.config.countermeasure_robot_path, - self.config.measurement_robot_path, + *ignored_robots, *excluded_paths, + *getattr(self.config, "ignored_collision_paths", ()), ) clearance = self.config.mobile_clearance_m obstacles: list[tuple[str, np.ndarray, np.ndarray]] = [] @@ -124,6 +158,48 @@ def _navigation_obstacles( expanded_lower = lower - np.asarray((clearance, clearance, 0.05)) expanded_upper = upper + np.asarray((clearance, clearance, 0.05)) obstacles.append((path, expanded_lower, expanded_upper)) + # Some referenced manufacturer robot assets expose their PhysX shapes + # through schemas that are not returned by the generic + # UsdPhysics.CollisionAPI traversal above. Always include the other + # articulated robot's composed bounds so one robot can never plan + # straight through the other. + for robot_path in ( + self.config.countermeasure_robot_path, + self.config.measurement_robot_path, + ): + if any(self._is_descendant(robot_path, item) for item in ignored): + continue + robot_bounds = self.bounds(robot_path) + if robot_bounds is None: + continue + robot_lower, robot_upper = robot_bounds + expanded_lower = robot_lower - np.asarray((clearance, clearance, 0.05)) + expanded_upper = robot_upper + np.asarray((clearance, clearance, 0.05)) + obstacles.append((f"{robot_path}:articulated_bounds", expanded_lower, expanded_upper)) + if carried_object_path is not None: + if carried_base_position_m is None: + raise ValueError("carried_base_position_m is required with carried_object_path") + carried_bounds = self.bounds(carried_object_path) + if carried_bounds is None: + raise ValueError(f"carried object has no bounds: {carried_object_path}") + base = np.asarray(carried_base_position_m, dtype=np.float64) + relative_lower = carried_bounds[0] - base + relative_upper = carried_bounds[1] - base + swept_margin = np.asarray((0.12, 0.12, 0.05), dtype=np.float64) + for path, lower, upper in self._collision_bounds(): + if any(self._is_descendant(path, item) for item in ignored): + continue + if upper[2] <= 0.08: + continue + # Configuration-space obstacle for the carried object's full + # AABB. A base position inside this box would make the carried + # payload overlap the scene obstacle even when the base itself + # remains clear. + forbidden_lower = lower - relative_upper - swept_margin + forbidden_upper = upper - relative_lower + swept_margin + obstacles.append( + (f"{path}:carried:{carried_object_path}", forbidden_lower, forbidden_upper) + ) return tuple(obstacles) @staticmethod @@ -138,7 +214,21 @@ def _segment_available( samples[:, 2] = np.maximum(samples[:, 2], 0.32) for _, expanded_lower, expanded_upper in obstacles: inside = np.all((samples >= expanded_lower) & (samples <= expanded_upper), axis=1) - if bool(np.any(inside)): + # A manipulation base pose is intentionally close to the serviced + # object and may sit inside its *inflated* clearance box without + # touching its real collision geometry. Permit a monotonic exit + # from, or entry into, that endpoint margin; never permit crossing + # through the obstacle in the middle of a route. + first_outside = 0 + while first_outside < len(inside) and inside[first_outside]: + first_outside += 1 + last_outside = len(inside) - 1 + while last_outside >= 0 and inside[last_outside]: + last_outside -= 1 + if first_outside == len(inside): + return False + middle = inside[first_outside : last_outside + 1] + if bool(np.any(middle)): return False return True @@ -148,10 +238,18 @@ def plan_mobile_route( target_m: np.ndarray, *, excluded_paths: tuple[str, ...] = (), + moving_robot_path: str | None = None, + carried_object_path: str | None = None, + carried_base_position_m: np.ndarray | None = None, ) -> tuple[np.ndarray, ...] | None: start = np.asarray(start_m, dtype=np.float64) target = np.asarray(target_m, dtype=np.float64) - obstacles = self._navigation_obstacles(excluded_paths=excluded_paths) + obstacles = self._navigation_obstacles( + excluded_paths=excluded_paths, + moving_robot_path=moving_robot_path, + carried_object_path=carried_object_path, + carried_base_position_m=carried_base_position_m, + ) if self._segment_available(start, target, obstacles): return (target.copy(),) @@ -215,25 +313,80 @@ def mobile_path_available( target_m: np.ndarray, *, excluded_paths: tuple[str, ...] = (), + moving_robot_path: str | None = None, + carried_object_path: str | None = None, + carried_base_position_m: np.ndarray | None = None, + ) -> bool: + return ( + self.plan_mobile_route( + start_m, + target_m, + excluded_paths=excluded_paths, + moving_robot_path=moving_robot_path, + carried_object_path=carried_object_path, + carried_base_position_m=carried_base_position_m, + ) + is not None + ) + + def mobile_base_pose_available( + self, + position_m: np.ndarray, + *, + excluded_paths: tuple[str, ...] = (), ) -> bool: - return self.plan_mobile_route(start_m, target_m, excluded_paths=excluded_paths) is not None + """Check that a manipulation base pose itself clears scene geometry. + + Route segments may deliberately enter an inflated endpoint margin so a + manipulator can service the target object. That exception must not + apply to unrelated obstacles at the final base pose: doing so can put + the chassis directly through a nearby prop even though the route is + reported as available. + """ + + position = np.asarray(position_m, dtype=np.float64) + ignored = ( + self.config.countermeasure_robot_path, + self.config.measurement_robot_path, + *excluded_paths, + *getattr(self.config, "ignored_collision_paths", ()), + ) + clearance = float(self.config.mobile_clearance_m) + for path, lower, upper in self._collision_bounds(): + if any(self._is_descendant(path, item) for item in ignored): + continue + if upper[2] <= 0.08: + continue + inside_xy = bool( + lower[0] - clearance <= position[0] <= upper[0] + clearance + and lower[1] - clearance <= position[1] <= upper[1] + clearance + ) + if inside_xy: + return False + return True def manipulator_reachable( self, target_m: np.ndarray, planned_base_m: np.ndarray | None = None ) -> bool: target = np.asarray(target_m, dtype=np.float64) - robot = self.world_position(self.config.countermeasure_robot_path) + robot = self.world_position( + self.config.countermeasure_pose_path or self.config.countermeasure_robot_path + ) planned_base = robot if planned_base_m is None else np.asarray(planned_base_m) - if ( - self.controller is not None - and hasattr(self.controller, "check_reachability") - and np.linalg.norm(planned_base - robot) <= 0.05 - ): + if self.controller is not None and hasattr(self.controller, "check_reachability"): try: - return bool(self.controller.check_reachability(target)) + return bool( + self.controller.check_reachability(target, base_position_m=planned_base) + ) + except TypeError: + if np.linalg.norm(planned_base - robot) <= 0.05: + return bool(self.controller.check_reachability(target)) except NotImplementedError: pass - return float(np.linalg.norm(target - planned_base)) <= self.config.manipulator_workspace_m + radial = float(np.linalg.norm(target[:2] - planned_base[:2])) + vertical = float(target[2] - planned_base[2]) + minimum_z, maximum_z = self.config.manipulator_vertical_range_m + return radial <= self.config.manipulator_workspace_m and minimum_z <= vertical <= maximum_z def grasp_frame_available(self, object_path: str | None) -> bool: if not object_path: @@ -268,8 +421,24 @@ def collision_free_placement(self, object_path: str | None, target_m: np.ndarray continue if upper[2] <= candidate_lower[2] + 0.035: continue - overlap = np.all(candidate_lower < upper - 0.01) and np.all( - candidate_upper > lower + 0.01 + prim = self.stage.GetPrimAtPath(path) + movable_neighbor = False + while prim and prim.IsValid() and str(prim.GetPath()) != "/": + movable = prim.GetAttribute("rad:manipulation:movable") + if movable and movable.HasAuthoredValueOpinion() and bool(movable.Get()): + movable_neighbor = True + break + prim = prim.GetParent() + # A payload can be geometrically separate from another prop while + # leaving too little room for the gripper and final arm descent. + # Preserve a manipulation envelope around movable neighbors. + manipulation_margin = 0.22 if movable_neighbor else 0.0 + margin = np.asarray( + (manipulation_margin, manipulation_margin, 0.0), + dtype=np.float64, + ) + overlap = np.all(candidate_lower - margin < upper - 0.01) and np.all( + candidate_upper + margin > lower + 0.01 ) if bool(overlap): return False @@ -310,25 +479,52 @@ def facts( pickup_base_m: np.ndarray, placement_base_m: np.ndarray, requires_grasp: bool, + pickup_manipulator_target_m: np.ndarray | None = None, + placement_manipulator_target_m: np.ndarray | None = None, requires_disposal: bool = False, ) -> FeasibilityFacts: - robot_position = self.world_position(self.config.countermeasure_robot_path) + robot_position = self.world_position( + self.config.countermeasure_pose_path or self.config.countermeasure_robot_path + ) path_to_pickup = self.mobile_path_available( robot_position, pickup_base_m, - excluded_paths=(() if object_path is None else (object_path,)), + moving_robot_path=self.config.countermeasure_robot_path, ) path_to_placement = self.mobile_path_available( pickup_base_m, placement_base_m, excluded_paths=(() if object_path is None else (object_path,)), + moving_robot_path=self.config.countermeasure_robot_path, + carried_object_path=object_path if requires_grasp else None, + carried_base_position_m=pickup_base_m if requires_grasp else None, + ) + pickup_pose_available = self.mobile_base_pose_available( + pickup_base_m, + excluded_paths=(() if object_path is None else (object_path,)), + ) + placement_pose_available = self.mobile_base_pose_available( + placement_base_m, + excluded_paths=(() if object_path is None else (object_path,)), + ) + pickup_target = ( + target_m if pickup_manipulator_target_m is None else pickup_manipulator_target_m + ) + placement_target = ( + target_m if placement_manipulator_target_m is None else placement_manipulator_target_m ) return FeasibilityFacts( - mobile_path_available=path_to_pickup and path_to_placement, + mobile_path_available=( + path_to_pickup + and path_to_placement + and pickup_pose_available + and placement_pose_available + ), manipulator_reachable=( True if not requires_grasp - else self.manipulator_reachable(target_m, placement_base_m) + else self.manipulator_reachable(pickup_target, pickup_base_m) + and self.manipulator_reachable(placement_target, placement_base_m) ), collision_free=self.collision_free_placement(object_path, target_m), grasp_frame_available=( @@ -370,8 +566,11 @@ def _attribute(prim: Any, name: str, default: object = None) -> object: return default if value is None else value @staticmethod - def _pose(position_m: np.ndarray) -> np.ndarray: + def _pose(position_m: np.ndarray, *, yaw_rad: float = 0.0) -> np.ndarray: pose = np.eye(4, dtype=np.float64) + cosine = math.cos(yaw_rad) + sine = math.sin(yaw_rad) + pose[:2, :2] = np.asarray(((cosine, -sine), (sine, cosine))) pose[:3, 3] = np.asarray(position_m, dtype=np.float64) return pose @@ -386,7 +585,12 @@ def _center(self, prim_or_path: Any) -> np.ndarray: return self.probe.world_position(prim_or_path) def _robot_position(self, robot_path: str) -> np.ndarray: - return self.probe.world_position(robot_path) + pose_path = robot_path + if robot_path == self.config.countermeasure_robot_path: + pose_path = self.config.countermeasure_pose_path or robot_path + elif robot_path == self.config.measurement_robot_path: + pose_path = self.config.measurement_pose_path or robot_path + return self.probe.world_position(pose_path) def _public_source_samples(self, belief: BeliefState) -> tuple[np.ndarray, np.ndarray]: strengths_by_path: dict[str, float] = {} @@ -451,6 +655,8 @@ def _facts( pickup_base_m: np.ndarray, placement_base_m: np.ndarray, requires_grasp: bool, + pickup_manipulator_target_m: np.ndarray | None = None, + placement_manipulator_target_m: np.ndarray | None = None, requires_disposal: bool = False, ) -> FeasibilityFacts: return self.probe.facts( @@ -459,6 +665,8 @@ def _facts( pickup_base_m=pickup_base_m, placement_base_m=placement_base_m, requires_grasp=requires_grasp, + pickup_manipulator_target_m=pickup_manipulator_target_m, + placement_manipulator_target_m=placement_manipulator_target_m, requires_disposal=requires_disposal, ) @@ -511,8 +719,12 @@ def _base_for_end_effector( *, robot_z: float, yaw_rad: float = 0.0, + offset_m: tuple[float, float, float] | None = None, ) -> np.ndarray: - offset = np.asarray(self.config.end_effector_offset_m, dtype=np.float64) + offset = np.asarray( + self.config.end_effector_offset_m if offset_m is None else offset_m, + dtype=np.float64, + ) cosine = math.cos(yaw_rad) sine = math.sin(yaw_rad) rotated_offset = np.asarray( @@ -528,12 +740,20 @@ def _base_for_end_effector( return result def _route( - self, start_m: np.ndarray, target_m: np.ndarray, object_path: str + self, + start_m: np.ndarray, + target_m: np.ndarray, + object_path: str, + *, + carrying: bool = False, ) -> list[list[float]]: route = self.probe.plan_mobile_route( start_m, target_m, - excluded_paths=(object_path,), + excluded_paths=((object_path,) if carrying else ()), + moving_robot_path=self.config.countermeasure_robot_path, + carried_object_path=object_path if carrying else None, + carried_base_position_m=start_m if carrying else None, ) return [] if route is None else [waypoint.tolist() for waypoint in route] @@ -544,15 +764,24 @@ def _measurement_candidates(self, belief: BeliefState) -> list[ActionCandidate]: if self._attribute(prim, "rad:role", "") != "detector_station": continue target = self.probe.world_position(prim) - path_available = self.probe.mobile_path_available(robot, target) - facts = FeasibilityFacts(mobile_path_available=path_available) + route = self.probe.plan_mobile_route( + robot, + target, + moving_robot_path=self.config.measurement_robot_path, + ) + facts = FeasibilityFacts(mobile_path_available=route is not None) action = CountermeasureAction( action_id=f"measure-{self._safe_name(str(prim.GetPath()))}", action_type=ActionType.MEASURE, robot_id=self.config.measurement_robot_path, target_prim_path=str(prim.GetPath()), target_pose_world=self._pose(target), - parameters={"detector_path": str(prim.GetPath())}, + parameters={ + "detector_path": str(prim.GetPath()), + "base_route_m": ( + [] if route is None else [waypoint.tolist() for waypoint in route] + ), + }, predicted_duration_s=self.config.measurement_duration_s, ) candidates.append( @@ -575,7 +804,11 @@ def _decon_candidates(self, belief: BeliefState) -> list[ActionCandidate]: if not bool(self._attribute(prim, "rad:decon:enabled", False)): continue target = self._center(prim) - base = self._base_for_end_effector(target, robot_z=float(robot[2])) + base = self._base_for_end_effector( + target, + robot_z=float(robot[2]), + offset_m=self.config.decon_end_effector_offset_m, + ) efficiency = float(self._attribute(prim, "rad:decon:efficiencyMean", 0.8)) facts = self._facts( object_path=None, @@ -584,6 +817,11 @@ def _decon_candidates(self, belief: BeliefState) -> list[ActionCandidate]: placement_base_m=base, requires_grasp=False, ) + route = self.probe.plan_mobile_route( + robot, + base, + moving_robot_path=self.config.countermeasure_robot_path, + ) action = CountermeasureAction( action_id=f"decon-{self._safe_name(str(prim.GetPath()))}", action_type=ActionType.DECONTAMINATE, @@ -593,9 +831,21 @@ def _decon_candidates(self, belief: BeliefState) -> list[ActionCandidate]: target_pose_world=self._pose(target), parameters={ "surface_path": str(prim.GetPath()), + "decon_profile": str( + self._attribute( + prim, + "rad:decon:profile", + "full_coverage_serpentine_raster", + ) + ), + "raster_rows": int(self._attribute(prim, "rad:decon:rasterRows", 6)), "duration_s": self.config.decon_duration_s, "pickup_base_position_m": base.tolist(), "placement_base_position_m": base.tolist(), + "pickup_base_route_m": ( + [] if route is None else [waypoint.tolist() for waypoint in route] + ), + "pickup_base_yaw_rad": 0.0, "decon_media": self.config.decon_duration_s, }, predicted_duration_s=self.config.decon_duration_s, @@ -634,39 +884,87 @@ def _shield_candidates(self, belief: BeliefState) -> list[ActionCandidate]: if not bool(self._attribute(shield, "rad:shield:movable", False)): continue shield_path = str(shield.GetPath()) + deployed = bool(self._attribute(shield, "rad:shield:deployed", False)) + deployment_state = "deployed" if deployed else "available" pickup = self._center(shield) - pickup_base = self._base_for_end_effector(pickup, robot_z=float(robot[2])) + shield_root = self.probe.world_position(shield) + root_from_center = shield_root - pickup + frame_name = str(self._attribute(shield, "rad:manipulation:graspFrame", "")) + grasp_position = self.probe.world_position(f"{shield_path.rstrip('/')}/{frame_name}") + grasp_from_root = grasp_position - shield_root + pickup_base = self._base_for_end_effector(grasp_position, robot_z=float(robot[2])) for fraction in self.config.shield_line_fractions: target = source + fraction * (protected - source) target[2] = pickup[2] - placement_base = self._base_for_end_effector(target, robot_z=float(robot[2])) - facts = self._facts( - object_path=shield_path, - target_m=target, - pickup_base_m=pickup_base, - placement_base_m=placement_base, - requires_grasp=True, + target_root = target + root_from_center + placement_options: list[tuple[float, np.ndarray, np.ndarray, FeasibilityFacts]] = [] + # This panel has one physical service handle on its west face. + # The controller preserves the payload's world orientation, so + # a pi-yaw fallback would put the Ridgeback east of the plate + # while its arm still reaches through to the west handle. That + # overlaps the chassis and payload at release. Keep the base on + # the authored handle side until orientation-aware payload + # rotation or a verified second grasp frame is implemented. + for placement_yaw in (0.0,): + target_grasp = target_root + grasp_from_root + placement_base = self._base_for_end_effector( + target_grasp, + robot_z=float(robot[2]), + yaw_rad=placement_yaw, + ) + option_facts = self._facts( + object_path=shield_path, + target_m=target, + pickup_base_m=pickup_base, + placement_base_m=placement_base, + requires_grasp=True, + pickup_manipulator_target_m=grasp_position, + placement_manipulator_target_m=target_grasp, + ) + placement_options.append( + (placement_yaw, target_grasp, placement_base, option_facts) + ) + placement_yaw, target_grasp, placement_base, facts = next( + ( + option + for option in placement_options + if option[3].mobile_path_available and option[3].manipulator_reachable + ), + placement_options[0], ) action = CountermeasureAction( action_id=( f"shield-{self._safe_name(shield_path)}-{int(round(fraction * 100)):02d}" ), - action_type=ActionType.PLACE_SHIELD, + # The identifier intentionally remains stable across this + # state transition. A confirmed multi-step workflow can + # therefore re-resolve the same placement option against + # the live scene and receive MOVE_SHIELD after deployment. + action_type=(ActionType.MOVE_SHIELD if deployed else ActionType.PLACE_SHIELD), robot_id=self.config.countermeasure_robot_path, target_prim_path=shield_path, - target_pose_world=self._pose(target), + target_pose_world=self._pose(target_root), parameters={ "object_path": shield_path, "pickup_base_position_m": pickup_base.tolist(), "placement_base_position_m": placement_base.tolist(), "pickup_base_route_m": self._route(robot, pickup_base, shield_path), "placement_base_route_m": self._route( - pickup_base, placement_base, shield_path + pickup_base, + placement_base, + shield_path, + carrying=True, ), "pickup_base_yaw_rad": 0.0, - "placement_base_yaw_rad": 0.0, + "placement_base_yaw_rad": placement_yaw, "shield_type": str(self._attribute(shield, "rad:material:id", "default")), - "shield_units": int(self._attribute(shield, "rad:shield:resourceUnits", 1)), + "shield_units": ( + 0 + if deployed + else int(self._attribute(shield, "rad:shield:resourceUnits", 1)) + ), + "placement_fraction": float(fraction), + "deployment_state": deployment_state, }, predicted_duration_s=self.config.shield_duration_s, ) @@ -702,7 +1000,16 @@ def _object_candidates(self, belief: BeliefState) -> list[ActionCandidate]: continue object_path = str(prim.GetPath()) pickup = self._center(prim) - pickup_base = self._base_for_end_effector(pickup, robot_z=float(robot[2])) + object_root = self.probe.world_position(prim) + root_from_center = object_root - pickup + frame_name = str(self._attribute(prim, "rad:manipulation:graspFrame", "")) + grasp_position = self.probe.world_position(f"{object_path.rstrip('/')}/{frame_name}") + grasp_from_root = grasp_position - object_root + pickup_base = self._base_for_end_effector( + grasp_position, + robot_z=float(robot[2]), + offset_m=self.config.object_end_effector_offset_m, + ) parking_slot = parking_paths.index(object_path) configured_offsets = self.config.object_parking_offsets_m offset_index = parking_slot % len(configured_offsets) @@ -711,26 +1018,38 @@ def _object_candidates(self, belief: BeliefState) -> list[ActionCandidate]: offset_xy[0] += 1.8 * offset_ring parking = zone_position + np.asarray((offset_xy[0], offset_xy[1], 0.0)) parking[2] = pickup[2] - placement_base = self._base_for_end_effector(parking, robot_z=float(robot[2])) + parking_root = parking + root_from_center + placement_base = self._base_for_end_effector( + parking_root + grasp_from_root, + robot_z=float(robot[2]), + offset_m=self.config.object_end_effector_offset_m, + ) facts = self._facts( object_path=object_path, target_m=parking, pickup_base_m=pickup_base, placement_base_m=placement_base, requires_grasp=True, + pickup_manipulator_target_m=grasp_position, + placement_manipulator_target_m=parking_root + grasp_from_root, ) action = CountermeasureAction( action_id=f"move-{self._safe_name(object_path)}", action_type=ActionType.MOVE_OBJECT, robot_id=self.config.countermeasure_robot_path, target_prim_path=object_path, - target_pose_world=self._pose(parking), + target_pose_world=self._pose(parking_root), parameters={ "object_path": object_path, "pickup_base_position_m": pickup_base.tolist(), "placement_base_position_m": placement_base.tolist(), "pickup_base_route_m": self._route(robot, pickup_base, object_path), - "placement_base_route_m": self._route(pickup_base, placement_base, object_path), + "placement_base_route_m": self._route( + pickup_base, + placement_base, + object_path, + carrying=True, + ), "pickup_base_yaw_rad": 0.0, "placement_base_yaw_rad": 0.0, }, @@ -748,19 +1067,42 @@ def _object_candidates(self, belief: BeliefState) -> list[ActionCandidate]: ) if not bool(self._attribute(prim, "rad:manipulation:removable", False)): continue + # A removable object may first have been parked next to the + # disposal zone. Re-approaching that grasp from yaw zero places + # the chassis between the payload and the zone and leaves the + # elevated pre-grasp outside Franka's collision-free workspace. + # Approach from the opposite side while preserving the validated + # 0.90 m object stand-off. + removal_pickup_yaw = math.pi + removal_pickup_base = self._base_for_end_effector( + grasp_position, + robot_z=float(robot[2]), + yaw_rad=removal_pickup_yaw, + offset_m=self.config.object_end_effector_offset_m, + ) removal_target = zone_position.copy() removal_target[2] = pickup[2] + removal_root = removal_target + root_from_center + removal_yaw = math.pi + # The controller keeps the payload's world orientation fixed + # while the base turns. Preserve the authored grasp offset too; + # rotating it here would send the carried drum one metre north + # before asking the arm to move it back at the disposal zone. + removal_grasp = removal_root + grasp_from_root removal_base = self._base_for_end_effector( - removal_target, + removal_grasp, robot_z=float(robot[2]), - yaw_rad=math.pi, + yaw_rad=removal_yaw, + offset_m=self.config.object_end_effector_offset_m, ) removal_facts = self._facts( object_path=object_path, target_m=removal_target, - pickup_base_m=pickup_base, + pickup_base_m=removal_pickup_base, placement_base_m=removal_base, requires_grasp=True, + pickup_manipulator_target_m=grasp_position, + placement_manipulator_target_m=removal_grasp, requires_disposal=True, ) remove = CountermeasureAction( @@ -768,16 +1110,25 @@ def _object_candidates(self, belief: BeliefState) -> list[ActionCandidate]: action_type=ActionType.REMOVE_OBJECT, robot_id=self.config.countermeasure_robot_path, target_prim_path=object_path, - target_pose_world=self._pose(removal_target), + target_pose_world=self._pose(removal_root, yaw_rad=removal_yaw), parameters={ "object_path": object_path, "disposal_zone_path": self.config.disposal_zone_path, - "pickup_base_position_m": pickup_base.tolist(), + "pickup_base_position_m": removal_pickup_base.tolist(), "placement_base_position_m": removal_base.tolist(), - "pickup_base_route_m": self._route(robot, pickup_base, object_path), - "placement_base_route_m": self._route(pickup_base, removal_base, object_path), - "pickup_base_yaw_rad": 0.0, - "placement_base_yaw_rad": math.pi, + "pickup_base_route_m": self._route(robot, removal_pickup_base, object_path), + "placement_base_route_m": self._route( + removal_pickup_base, + removal_base, + object_path, + carrying=True, + ), + "pickup_base_yaw_rad": removal_pickup_yaw, + "placement_base_yaw_rad": removal_yaw, + # Removal ends inside a bounded disposal zone. Allow the + # released rigid body to settle naturally, then apply the + # stricter zone-containment check before disabling it. + "placement_settle_tolerance_m": 0.25, }, predicted_duration_s=self.config.object_duration_s, ) @@ -797,6 +1148,7 @@ def generate_all( self, belief: BeliefState, diagnosis: object | None = None ) -> tuple[ActionCandidate, ...]: del diagnosis + self.probe.invalidate_collision_cache() self._latest.clear() candidates = [ *self._measurement_candidates(belief), @@ -807,15 +1159,19 @@ def generate_all( return tuple(candidates) def generate_measurement_actions(self, belief: BeliefState) -> tuple[ActionCandidate, ...]: + self.probe.invalidate_collision_cache() return tuple(self._measurement_candidates(belief)) def generate_decon_actions(self, belief: BeliefState) -> tuple[ActionCandidate, ...]: + self.probe.invalidate_collision_cache() return tuple(self._decon_candidates(belief)) def generate_shield_actions(self, belief: BeliefState) -> tuple[ActionCandidate, ...]: + self.probe.invalidate_collision_cache() return tuple(self._shield_candidates(belief)) def generate_move_remove_actions(self, belief: BeliefState) -> tuple[ActionCandidate, ...]: + self.probe.invalidate_collision_cache() return tuple(self._object_candidates(belief)) def preview(self, action: CountermeasureAction, belief: BeliefState) -> dict[str, object]: diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/rendering/effects.py b/source/extensions/radcounter.isaac/radcounter/isaac/rendering/effects.py index 422be2f..9ef896f 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/rendering/effects.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/rendering/effects.py @@ -35,9 +35,7 @@ def author_static(self) -> dict[str, int | bool]: ) settings.set_bool("/rtx/raytracing/fog/enabled", fog_enabled) settings.set_float("/rtx/raytracing/fog/density", self.config.fog_density) - settings.set_float_array( - "/rtx/raytracing/fog/color", list(self.config.fog_color_rgb) - ) + settings.set_float_array("/rtx/raytracing/fog/color", list(self.config.fog_color_rgb)) dust_count = int(self.config.dust_particle_count * self.budget.effect_particle_scale) if dust_count: low = np.asarray(self.config.bounds_min_m) @@ -130,7 +128,9 @@ def author_wet_trace( for index in range(len(values) - 1): start = index * 2 faces.extend((start, start + 1, start + 3, start + 2)) - path = f"{self.root_path}/WetTraces/trace_{len(values)}_{abs(hash(values.tobytes())) % 100000}" + path = ( + f"{self.root_path}/WetTraces/trace_{len(values)}_{abs(hash(values.tobytes())) % 100000}" + ) mesh = UsdGeom.Mesh.Define(self.stage, path) mesh.CreatePointsAttr([Gf.Vec3f(*value) for value in vertices]) mesh.CreateFaceVertexCountsAttr([4] * (len(values) - 1)) diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/rendering/ingestion.py b/source/extensions/radcounter.isaac/radcounter/isaac/rendering/ingestion.py index 4c7aab7..716683d 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/rendering/ingestion.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/rendering/ingestion.py @@ -4,7 +4,6 @@ import fnmatch import hashlib -import json import urllib.parse from dataclasses import dataclass from pathlib import Path @@ -136,9 +135,7 @@ def _mount_manifest( authored.CreateFaceVertexIndicesAttr(mesh.triangles.reshape(-1).tolist()) authored.CreateSubdivisionSchemeAttr("none") prim = authored.GetPrim() - prim.CreateAttribute("rad:material:id", Sdf.ValueTypeNames.String).Set( - mesh.material_id - ) + prim.CreateAttribute("rad:material:id", Sdf.ValueTypeNames.String).Set(mesh.material_id) prim.CreateAttribute("rad:visual:normalizedFallback", Sdf.ValueTypeNames.Bool).Set(True) UsdGeom.Imageable(prim).CreatePurposeAttr("render") return IngestionReport( diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/rendering/lighting.py b/source/extensions/radcounter.isaac/radcounter/isaac/rendering/lighting.py index 7ddb230..e32d559 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/rendering/lighting.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/rendering/lighting.py @@ -47,12 +47,12 @@ def author(self, config: FacilityLightingConfig) -> tuple[str, ...]: Gf.Vec3f(*item.color_rgb) ) if item.color_temperature_k is not None: - prim.CreateAttribute( - "inputs:enableColorTemperature", Sdf.ValueTypeNames.Bool - ).Set(True) - prim.CreateAttribute( - "inputs:colorTemperature", Sdf.ValueTypeNames.Float - ).Set(item.color_temperature_k) + prim.CreateAttribute("inputs:enableColorTemperature", Sdf.ValueTypeNames.Bool).Set( + True + ) + prim.CreateAttribute("inputs:colorTemperature", Sdf.ValueTypeNames.Float).Set( + item.color_temperature_k + ) if item.light_type is LightType.RECT: prim.CreateAttribute("inputs:width", Sdf.ValueTypeNames.Float).Set(item.width_m) prim.CreateAttribute("inputs:height", Sdf.ValueTypeNames.Float).Set(item.height_m) @@ -64,7 +64,9 @@ def author(self, config: FacilityLightingConfig) -> tuple[str, ...]: prim.CreateAttribute("inputs:texture:file", Sdf.ValueTypeNames.Asset).Set( Sdf.AssetPath(item.hdri_uri) ) - prim.CreateAttribute("inputs:texture:format", Sdf.ValueTypeNames.Token).Set("latlong") + prim.CreateAttribute("inputs:texture:format", Sdf.ValueTypeNames.Token).Set( + "latlong" + ) xformable = UsdGeom.Xformable(prim) xformable.ClearXformOpOrder() xformable.AddTranslateOp().Set(Gf.Vec3d(*item.translation_m)) diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/rendering/runtime.py b/source/extensions/radcounter.isaac/radcounter/isaac/rendering/runtime.py index 7e0c7ab..5a2f1a2 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/rendering/runtime.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/rendering/runtime.py @@ -4,11 +4,6 @@ from dataclasses import asdict, dataclass -from radcounter.core.rendering import ( - DigitalTwinRenderingConfig, - RenderPurpose, - nuclear_facility_materials, -) from radcounter.isaac.rendering.effects import FacilityEffectsAuthor from radcounter.isaac.rendering.ingestion import DigitalTwinIngestor, IngestionReport from radcounter.isaac.rendering.lighting import FacilityLightingAuthor @@ -16,6 +11,12 @@ from radcounter.isaac.rendering.products import IsaacRenderProductManager from radcounter.isaac.rendering.renderer import IsaacRenderController +from radcounter.core.rendering import ( + DigitalTwinRenderingConfig, + RenderPurpose, + nuclear_facility_materials, +) + @dataclass(frozen=True) class DigitalTwinRuntimeReport: diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/robot/__init__.py b/source/extensions/radcounter.isaac/radcounter/isaac/robot/__init__.py index 8713975..840cd26 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/robot/__init__.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/robot/__init__.py @@ -1,19 +1,69 @@ -"""Generic robot import and control for the Isaac Sim adapter.""" +"""Robot import, sensing, and high-fidelity Isaac Sim controllers.""" +from .decontamination import ( + ContactDrivenDecontaminator, + DecontaminationConfig, + TreatmentTickResult, +) from .generic import ( GenericArticulationController, GenericRobotImporter, RobotFleetManager, ) -from .sensor_rig import IsaacRobotSensorRigManager, MountedIsaacSensor +from .physics_controller import ( + IsaacPhysicsRobotController, + PhysicsActionReport, + PhysicsControllerConfig, + RobotExecutionState, +) +from .physx_telemetry import ( + PhysxManipulationTelemetry, + PhysxManipulationTelemetrySummary, +) +from .real_robots import ( + ArticulatedTaskReport, + DecontaminationMotionReport, + HandMotionResult, + MeasurementMotionReport, + NovaCarterController, + RealRobotAssetConfig, + RidgebackFrankaController, + ShieldMotionReport, + add_real_robot_references, + author_real_robot_task_scene, + create_decontamination_activity_map, + enable_real_robot_extensions, +) from .reference_models import SpawnedReferenceRobot, spawn_reference_robot +from .sensor_rig import IsaacRobotSensorRigManager, MountedIsaacSensor __all__ = [ + "ArticulatedTaskReport", + "ContactDrivenDecontaminator", + "DecontaminationConfig", + "DecontaminationMotionReport", "GenericArticulationController", "GenericRobotImporter", + "HandMotionResult", + "IsaacPhysicsRobotController", "IsaacRobotSensorRigManager", + "MeasurementMotionReport", "MountedIsaacSensor", + "NovaCarterController", + "PhysicsActionReport", + "PhysicsControllerConfig", + "PhysxManipulationTelemetry", + "PhysxManipulationTelemetrySummary", + "RealRobotAssetConfig", + "RidgebackFrankaController", + "RobotExecutionState", "RobotFleetManager", + "ShieldMotionReport", "SpawnedReferenceRobot", + "TreatmentTickResult", + "add_real_robot_references", + "author_real_robot_task_scene", + "create_decontamination_activity_map", + "enable_real_robot_extensions", "spawn_reference_robot", ] diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/robot/decontamination.py b/source/extensions/radcounter.isaac/radcounter/isaac/robot/decontamination.py index aa51bf4..1a65e2e 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/robot/decontamination.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/robot/decontamination.py @@ -14,6 +14,8 @@ import numpy as np from numpy.typing import NDArray +from radcounter.core.surface_decontamination import effective_contact_exposure_s + FloatArray = NDArray[np.float64] IntArray = NDArray[np.int64] @@ -102,6 +104,14 @@ def __init__( self.activity_bq = np.asarray(payload["activity_bq"], dtype=np.float64) self.exposure = np.asarray(payload["cumulative_treatment_exposure"], dtype=np.float64) self.last_treated_step = np.asarray(payload["last_treated_step"], dtype=np.int64) + self.initial_activity_bq = self.activity_bq.copy() + from pxr import UsdGeom + + authored_colors = UsdGeom.Gprim(surface).GetDisplayColorAttr().Get() or () + initial_colors = np.asarray(authored_colors, dtype=np.float64) + self._initial_display_colors = ( + initial_colors if initial_colors.shape == (len(self.activity_bq), 3) else None + ) common = self._rng.normal() local = self._rng.normal(size=len(self.activity_bq)) correlation = 0.75 @@ -141,6 +151,35 @@ def _tool_samples(self) -> tuple[FloatArray, FloatArray, FloatArray]: axis /= np.linalg.norm(axis) return samples, center, axis + def _update_surface_visuals(self) -> None: + """Fade each rendered face using the same remaining truth activity.""" + + if self._initial_display_colors is None: + return + from pxr import Gf, UsdGeom + + surface = self.stage.GetPrimAtPath(self.surface_path) + gprim = UsdGeom.Gprim(surface) + fraction = np.divide( + self.activity_bq, + self.initial_activity_bq, + out=np.zeros_like(self.activity_bq), + where=self.initial_activity_bq > 0.0, + ) + fraction = np.clip(fraction, 0.0, 1.0) + host_color = np.asarray((0.20, 0.27, 0.32), dtype=np.float64) + colors = host_color + fraction[:, None] * ( + self._initial_display_colors - host_color + ) + color_attr = gprim.GetDisplayColorAttr() + color_attr.Set([Gf.Vec3f(*color) for color in colors]) + color_attr.SetMetadata("interpolation", UsdGeom.Tokens.uniform) + opacity_attr = gprim.GetDisplayOpacityAttr() + if not opacity_attr: + opacity_attr = gprim.CreateDisplayOpacityAttr() + opacity_attr.Set([0.0 if value < 0.10 else 1.0 for value in fraction]) + opacity_attr.SetMetadata("interpolation", UsdGeom.Tokens.uniform) + def tick(self, dt_s: float, simulation_step: int) -> TreatmentTickResult: import carb @@ -180,10 +219,17 @@ def tick(self, dt_s: float, simulation_step: int) -> TreatmentTickResult: accepted += 1 hit_counts[triangle_index] = hit_counts.get(triangle_index, 0) + 1 removed_total = 0.0 - footprint_count = max(len(self.config.footprint_points_local_m), 1) - for triangle_index, count in hit_counts.items(): + effective_exposure_s = effective_contact_exposure_s( + dt_s, + speed, + self.config.max_surface_speed_m_s, + ) + for triangle_index in hit_counts: row = self._row_by_triangle[triangle_index] - incremental_exposure = dt_s * count / footprint_count + # Ray count is spatial sampling density, not elapsed time. Apply + # one contact tick per hit face so densifying the pad footprint + # cannot dilute treatment, matching SurfaceSourceGrid.apply_tool(). + incremental_exposure = effective_exposure_s removal_fraction = 1.0 - math.exp( -self.config.rate_constant_s_inv * incremental_exposure * self.truth_efficiency[row] ) @@ -197,6 +243,8 @@ def tick(self, dt_s: float, simulation_step: int) -> TreatmentTickResult: self._cumulative_removed += removed_total if self.config.transfer_mode == "transfer_to_waste": self._transfer_to_waste(removed_total) + if simulation_step % 10 == 0: + self._update_surface_visuals() recontaminated = self.inject_recontamination(self.config.recontamination_rate_bq_s * dt_s) return TreatmentTickResult( accepted_contacts=accepted, @@ -264,6 +312,7 @@ def _transfer_to_waste(self, removed_activity_bq: float) -> None: def flush(self) -> str: if not self._dirty: return hashlib.sha256(self.activity_path.read_bytes()).hexdigest() + self._update_surface_visuals() self.activity_path.parent.mkdir(parents=True, exist_ok=True) file_descriptor, temporary_name = tempfile.mkstemp( prefix=f".{self.activity_path.stem}.", diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/robot/input_router.py b/source/extensions/radcounter.isaac/radcounter/isaac/robot/input_router.py index b64388d..899fd7f 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/robot/input_router.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/robot/input_router.py @@ -104,6 +104,14 @@ def detach_devices(self) -> None: self._keyboard_subscription = None self._gamepad_subscription = None + def close(self) -> None: + """Release Kit input subscriptions and an optional command server.""" + + self.detach_devices() + if self.command_server is not None: + self.command_server.close() + self.command_server = None + def register_auto_controller( self, robot_id: str, diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/robot/physics_controller.py b/source/extensions/radcounter.isaac/radcounter/isaac/robot/physics_controller.py index 9414035..996ac93 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/robot/physics_controller.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/robot/physics_controller.py @@ -508,7 +508,10 @@ def execute_pick_and_place( placement_base_yaw_rad: float | None = None, pickup_base_route_m: Sequence[Sequence[float]] | None = None, placement_base_route_m: Sequence[Sequence[float]] | None = None, + target_root_position_m: Sequence[float] | None = None, + placement_settle_tolerance_m: float = 0.15, ) -> PhysicsActionReport: + del target_root_position_m, placement_settle_tolerance_m pickup_route = ( (pickup_base_position_m,) if pickup_base_route_m is None else pickup_base_route_m ) diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/robot/physx_telemetry.py b/source/extensions/radcounter.isaac/radcounter/isaac/robot/physx_telemetry.py new file mode 100644 index 0000000..4ef2103 --- /dev/null +++ b/source/extensions/radcounter.isaac/radcounter/isaac/robot/physx_telemetry.py @@ -0,0 +1,152 @@ +"""PhysX joint-effort, joint-reaction, and rigid-contact telemetry.""" + +from __future__ import annotations + +from collections.abc import Sequence +from dataclasses import dataclass +from typing import Any + +import numpy as np + + +def _numpy(value: Any) -> np.ndarray: + if value is None: + return np.asarray([], dtype=np.float64) + if hasattr(value, "detach"): + value = value.detach() + if hasattr(value, "cpu"): + value = value.cpu() + if hasattr(value, "numpy"): + value = value.numpy() + return np.asarray(value, dtype=np.float64) + + +@dataclass(frozen=True, slots=True) +class PhysxManipulationTelemetrySummary: + samples: int + joint_names: tuple[str, ...] + peak_abs_joint_effort_nm: tuple[float, ...] + rms_joint_effort_nm: tuple[float, ...] + peak_joint_reaction_force_n: tuple[float, ...] + peak_joint_reaction_torque_nm: tuple[float, ...] + peak_rigid_contact_force_n: float + contact_samples: int + + @property + def overall_peak_abs_joint_effort_nm(self) -> float: + return max(self.peak_abs_joint_effort_nm, default=0.0) + + @property + def overall_peak_joint_reaction_force_n(self) -> float: + return max(self.peak_joint_reaction_force_n, default=0.0) + + @property + def overall_peak_joint_reaction_torque_nm(self) -> float: + return max(self.peak_joint_reaction_torque_nm, default=0.0) + + +class PhysxManipulationTelemetry: + """Accumulate solver measurements without coupling to a particular robot wrapper.""" + + def __init__( + self, + articulation: Any, + *, + joint_names: Sequence[str], + rigid_contact_view: Any | None = None, + physics_dt_s: float = 1.0 / 60.0, + contact_threshold_n: float = 0.1, + ) -> None: + if physics_dt_s <= 0.0: + raise ValueError("physics_dt_s must be positive") + if contact_threshold_n < 0.0: + raise ValueError("contact_threshold_n must be nonnegative") + self.articulation = articulation + self.joint_names = tuple(joint_names) + if not self.joint_names: + raise ValueError("at least one joint name is required") + self.joint_indices = np.asarray( + [articulation.get_dof_index(name) for name in self.joint_names], + dtype=np.int64, + ) + if np.any(self.joint_indices < 0): + raise ValueError("all requested joints must exist in the articulation") + # The measured-effort vector is indexed by DOF, while the 6-D reaction + # matrix is indexed by incoming *joint*. Robot assets contain fixed + # joints that have no DOF, so using dof_index + 1 for reactions reads + # unrelated rows and can produce nonsensical forces. Isaac's API + # explicitly exposes the required name-to-joint-index map in metadata. + view = getattr(articulation, "_articulation_view", None) + metadata = getattr(view, "_metadata", None) + joint_index_map = getattr(metadata, "joint_indices", None) + if joint_index_map is None: + raise ValueError("articulation joint metadata is required for reaction telemetry") + try: + self.reaction_rows = np.asarray( + [int(joint_index_map[name]) + 1 for name in self.joint_names], + dtype=np.int64, + ) + except KeyError as error: + raise ValueError(f"joint metadata is missing {error.args[0]}") from error + self.rigid_contact_view = rigid_contact_view + self.physics_dt_s = float(physics_dt_s) + self.contact_threshold_n = float(contact_threshold_n) + count = len(self.joint_names) + self._samples = 0 + self._effort_square_sum = np.zeros(count, dtype=np.float64) + self._peak_abs_effort = np.zeros(count, dtype=np.float64) + self._peak_reaction_force = np.zeros(count, dtype=np.float64) + self._peak_reaction_torque = np.zeros(count, dtype=np.float64) + self._peak_contact_force = 0.0 + self._contact_samples = 0 + + def sample(self) -> None: + efforts = _numpy(self.articulation.get_measured_joint_efforts()).reshape(-1) + reaction = _numpy(self.articulation.get_measured_joint_forces()) + if efforts.size <= int(self.joint_indices.max()): + raise RuntimeError("PhysX joint-effort vector does not cover the requested joints") + if reaction.ndim != 2 or reaction.shape[1] != 6: + raise RuntimeError("PhysX measured joint reactions must have shape (N, 6)") + if reaction.shape[0] <= int(self.reaction_rows.max()): + raise RuntimeError("PhysX joint-reaction matrix does not cover the requested joints") + + selected_efforts = efforts[self.joint_indices] + selected_reaction = reaction[self.reaction_rows] + reaction_force = np.linalg.norm(selected_reaction[:, :3], axis=1) + reaction_torque = np.linalg.norm(selected_reaction[:, 3:], axis=1) + self._samples += 1 + self._effort_square_sum += selected_efforts * selected_efforts + self._peak_abs_effort = np.maximum(self._peak_abs_effort, np.abs(selected_efforts)) + self._peak_reaction_force = np.maximum(self._peak_reaction_force, reaction_force) + self._peak_reaction_torque = np.maximum(self._peak_reaction_torque, reaction_torque) + + if self.rigid_contact_view is None: + return + # A contact-filtered view must be read through the pair-wise force + # matrix. ``get_net_contact_forces`` also includes contacts with + # unfiltered bodies, which makes a floor-only audit count nearby props. + pairwise = getattr(self.rigid_contact_view, "get_contact_force_matrix", None) + if pairwise is None: + contact_vectors = _numpy( + self.rigid_contact_view.get_net_contact_forces(dt=self.physics_dt_s) + ).reshape(-1, 3) + else: + contact_vectors = _numpy(pairwise(dt=self.physics_dt_s)).reshape(-1, 3) + contact_force = float(np.max(np.linalg.norm(contact_vectors, axis=1), initial=0.0)) + self._peak_contact_force = max(self._peak_contact_force, contact_force) + if contact_force >= self.contact_threshold_n: + self._contact_samples += 1 + + def summary(self) -> PhysxManipulationTelemetrySummary: + divisor = max(self._samples, 1) + rms = np.sqrt(self._effort_square_sum / divisor) + return PhysxManipulationTelemetrySummary( + samples=self._samples, + joint_names=self.joint_names, + peak_abs_joint_effort_nm=tuple(map(float, self._peak_abs_effort)), + rms_joint_effort_nm=tuple(map(float, rms)), + peak_joint_reaction_force_n=tuple(map(float, self._peak_reaction_force)), + peak_joint_reaction_torque_nm=tuple(map(float, self._peak_reaction_torque)), + peak_rigid_contact_force_n=float(self._peak_contact_force), + contact_samples=self._contact_samples, + ) diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/robot/real_robots.py b/source/extensions/radcounter.isaac/radcounter/isaac/robot/real_robots.py index 69fd975..3d7d777 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/robot/real_robots.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/robot/real_robots.py @@ -10,7 +10,7 @@ import hashlib import math -from collections.abc import Callable, Sequence +from collections.abc import Callable, Mapping, Sequence from dataclasses import dataclass from pathlib import Path from typing import Any @@ -18,13 +18,15 @@ import numpy as np FrameCallback = Callable[[float, int], None] +NavigationProgressCallback = Callable[ + [int, tuple[float, float, float], tuple[float, float], float], None +] +OperationProgressCallback = Callable[[Mapping[str, object]], None] @dataclass(frozen=True, slots=True) class RealRobotAssetConfig: - countermeasure_asset: str = ( - "/Isaac/Robots/Clearpath/RidgebackFranka/ridgeback_franka.usd" - ) + countermeasure_asset: str = "/Isaac/Robots/Clearpath/RidgebackFranka/ridgeback_franka.usd" measurement_asset: str = "/Isaac/Robots/NVIDIA/NovaCarter/nova_carter.usd" countermeasure_root: str = "/World/CountermeasureRobot" measurement_root: str = "/World/MeasurementRobot" @@ -32,13 +34,86 @@ class RealRobotAssetConfig: detector_path: str = "/World/MeasurementRobot/chassis_link/Detector" panda_base_path: str = "/World/CountermeasureRobot/panda_link0" panda_hand_path: str = "/World/CountermeasureRobot/panda_hand" - decon_tool_path: str = ( - "/World/CountermeasureRobot/panda_hand/RadCounterDeconTool/ContactPad" - ) + decon_tool_path: str = "/World/CountermeasureRobot/panda_hand/RadCounterDeconTool/ContactPad" shield_path: str = "/World/LeadShield" shield_grasp_path: str = "/World/LeadShield/ShieldGraspFrame" - shield_grasp_offset_m: tuple[float, float, float] = (-0.12, 0.0, 0.59) + shield_grasp_offset_m: tuple[float, float, float] = (-0.24, 0.0, 0.59) decon_surface_path: str = "/World/DeconWorkSurface" + decon_workbench_center_m: tuple[float, float, float] = (14.39, 0.80, 1.15) + shield_initial_position_m: tuple[float, float, float] = (0.78, -0.62, 0.0) + include_validation_facility: bool = True + + +@dataclass(frozen=True, slots=True) +class FacilityRoomSpec: + """Auditable room bounds for the deterministic validation facility.""" + + room_id: str + display_name: str + center_m: tuple[float, float, float] + half_extent_m: tuple[float, float, float] + + +@dataclass(frozen=True, slots=True) +class FacilityCorridorSpec: + """One physically connected passage between two declared rooms.""" + + corridor_id: str + from_room_id: str + to_room_id: str + center_m: tuple[float, float, float] + half_extent_m: tuple[float, float, float] + + +@dataclass(frozen=True, slots=True) +class FacilityPrimitiveSpec: + """Pure-data primitive used to author and regression-test the facility.""" + + path: str + category: str + center_m: tuple[float, float, float] + half_extent_m: tuple[float, float, float] + color: tuple[float, float, float] + shape: str = "cube" + axis: str = "Z" + collision: bool = True + room_id: str | None = None + corridor_id: str | None = None + equipment_id: str | None = None + equipment_type: str | None = None + display_name: str | None = None + + +@dataclass(frozen=True, slots=True) +class FacilityRouteSpec: + """Reserved robot-base centerline kept clear by the authored layout.""" + + route_id: str + waypoints_m: tuple[tuple[float, float, float], ...] + + +@dataclass(frozen=True, slots=True) +class FacilityLayoutManifest: + """Isaac-independent manifest for a deterministic decommissioning layout.""" + + layout_id: str + root_path: str + rooms: tuple[FacilityRoomSpec, ...] + corridors: tuple[FacilityCorridorSpec, ...] + primitives: tuple[FacilityPrimitiveSpec, ...] + reserved_routes: tuple[FacilityRouteSpec, ...] + secondary_shield_path: str + secondary_shield_position_m: tuple[float, float, float] + + @property + def equipment_ids(self) -> tuple[str, ...]: + return tuple( + dict.fromkeys( + primitive.equipment_id + for primitive in self.primitives + if primitive.equipment_id is not None + ) + ) @dataclass(frozen=True, slots=True) @@ -58,7 +133,9 @@ class DecontaminationMotionReport: removed_activity_bq: float activity_before_bq: float activity_after_bq: float + removed_fraction: float treated_triangle_indices: tuple[int, ...] + coverage_fraction: float waypoint_errors_m: tuple[float, ...] tool_path_length_m: float @@ -87,6 +164,38 @@ class MeasurementMotionReport: initial_position_m: tuple[float, float, float] final_position_m: tuple[float, float, float] + @property + def message(self) -> str: + return ( + "wheel-joint navigation completed" if self.success else "wheel-joint navigation failed" + ) + + @property + def state(self) -> str: + return "complete" if self.success else "failed" + + +@dataclass(frozen=True, slots=True) +class ArticulatedTaskReport: + """Workflow-compatible audit for an arm, gripper, and base task.""" + + state: str + success: bool + steps: int + message: str + object_path: str | None = None + phases: tuple[str, ...] = () + grasp_distance_m: float | None = None + placement_error_m: float | None = None + finger_aperture_open_m: float | None = None + finger_aperture_closed_m: float | None = None + arm_joint_excursion_rad: float = 0.0 + target_position_m: tuple[float, float, float] | None = None + release_position_m: tuple[float, float, float] | None = None + final_position_m: tuple[float, float, float] | None = None + release_orientation_wxyz: tuple[float, float, float, float] | None = None + final_orientation_wxyz: tuple[float, float, float, float] | None = None + def enable_real_robot_extensions() -> None: """Enable the two optional Isaac extensions used by the real controllers.""" @@ -97,6 +206,782 @@ def enable_real_robot_extensions() -> None: enable_extension("isaacsim.robot.wheeled_robots") +def create_decontamination_activity_map( + path: str | Path, + *, + triangle_count: int | None = None, + total_activity_bq: float = 2.0e7, +) -> Path: + """Create a writable surface map whose rows match the authored work surface.""" + + from radcounter.core.surface_decontamination import irregular_deposition_field + + destination = Path(path).expanduser().resolve() + if triangle_count is not None and triangle_count <= 0: + raise ValueError("triangle_count must be positive") + if not math.isfinite(total_activity_bq) or total_activity_bq < 0.0: + raise ValueError("total_activity_bq must be finite and nonnegative") + destination.parent.mkdir(parents=True, exist_ok=True) + if triangle_count is None: + active_cell_activity = irregular_deposition_field() + active_cell_activity = active_cell_activity[active_cell_activity > 0.0] + # The rendered collision mesh has two triangles per active cell. Split + # the cell activity between them so truth and visible geometry match. + raw_weights = np.repeat(active_cell_activity * 0.5, 2) + triangle_count = len(raw_weights) + else: + triangle_indices_for_weights = np.arange(triangle_count, dtype=np.int64) + raw_weights = 0.12 + np.square( + np.sin(0.83 + triangle_indices_for_weights.astype(np.float64) * 1.71) + ) + triangle_indices = np.arange(triangle_count, dtype=np.int64) + activity_bq = total_activity_bq * raw_weights / float(np.sum(raw_weights)) + np.savez_compressed( + destination, + triangle_indices=triangle_indices, + activity_bq=activity_bq, + cumulative_treatment_exposure=np.zeros(triangle_count, dtype=np.float64), + last_treated_step=np.full(triangle_count, -1, dtype=np.int64), + ) + return destination + + +def decommissioning_facility_layout() -> FacilityLayoutManifest: + """Return the deterministic, Isaac-independent complex facility manifest.""" + + wall_color = (0.20, 0.27, 0.32) + branch_color = (0.27, 0.34, 0.38) + floor_color = (0.18, 0.19, 0.20) + steel_color = (0.22, 0.27, 0.29) + equipment_blue = (0.12, 0.31, 0.39) + service_yellow = (0.90, 0.56, 0.08) + + rooms = ( + FacilityRoomSpec( + "original_cell", + "Original reactor service cell", + (0.0, 0.0, 1.50), + (6.0, 4.0, 1.50), + ), + FacilityRoomSpec( + "remote_decon_room", + "Remote wall-decontamination room", + (12.0, 0.50, 1.50), + (2.50, 3.0, 1.50), + ), + FacilityRoomSpec( + "reactor_service_room", + "Reactor auxiliary service room", + (11.0, 7.50, 1.50), + (3.50, 2.50, 1.50), + ), + FacilityRoomSpec( + "shield_staging_room", + "Shield and waste staging room", + (19.50, 7.50, 1.50), + (2.50, 2.50, 1.50), + ), + ) + corridors = ( + FacilityCorridorSpec( + "original_to_decon_transfer", + "original_cell", + "remote_decon_room", + (7.75, -1.50, 1.50), + (1.75, 1.0, 1.50), + ), + FacilityCorridorSpec( + "decon_to_reactor_service", + "remote_decon_room", + "reactor_service_room", + (11.20, 4.25, 1.50), + (1.0, 0.75, 1.50), + ), + FacilityCorridorSpec( + "reactor_to_shield_staging", + "reactor_service_room", + "shield_staging_room", + (15.75, 8.0, 1.50), + (1.25, 1.0, 1.50), + ), + ) + + def equipment( + equipment_id: str, + equipment_type: str, + part_name: str, + center_m: tuple[float, float, float], + half_extent_m: tuple[float, float, float], + color: tuple[float, float, float], + *, + room_id: str, + shape: str = "cube", + axis: str = "Z", + display_name: str | None = None, + ) -> FacilityPrimitiveSpec: + return FacilityPrimitiveSpec( + path=f"Equipment/{equipment_id}/{part_name}", + category="equipment", + center_m=center_m, + half_extent_m=half_extent_m, + color=color, + shape=shape, + axis=axis, + room_id=room_id, + equipment_id=equipment_id, + equipment_type=equipment_type, + display_name=display_name, + ) + + primitives = ( + # Floors retain the original remote-room geometry and add two rooms and + # two turning passages. Their upper faces stay at z=0, matching the + # original concrete floor without creating a wheel-catching step. + FacilityPrimitiveSpec( + "CorridorFloor", + "floor", + (7.75, -1.50, -0.10), + (1.75, 1.0, 0.10), + floor_color, + corridor_id="original_to_decon_transfer", + ), + FacilityPrimitiveSpec( + "DeconRoomFloor", + "floor", + (12.0, 0.50, -0.10), + (2.50, 3.0, 0.10), + floor_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "ShieldServiceAlcoveFloor", + "floor", + (9.25, 0.50, -0.10), + (0.25, 0.90, 0.10), + floor_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "ServiceAccessFloor", + "floor", + (11.20, 4.25, -0.10), + (1.0, 0.75, 0.10), + floor_color, + corridor_id="decon_to_reactor_service", + ), + FacilityPrimitiveSpec( + "ReactorServiceRoomFloor", + "floor", + (11.0, 7.50, -0.10), + (3.50, 2.50, 0.10), + floor_color, + room_id="reactor_service_room", + ), + FacilityPrimitiveSpec( + "ShieldTransferFloor", + "floor", + (15.75, 8.0, -0.10), + (1.25, 1.0, 0.10), + floor_color, + corridor_id="reactor_to_shield_staging", + ), + FacilityPrimitiveSpec( + "ShieldStagingRoomFloor", + "floor", + (19.50, 7.50, -0.10), + (2.50, 2.50, 0.10), + floor_color, + room_id="shield_staging_room", + ), + # Original doorway, transfer corridor, and remote decon room. The + # remote north wall is split only to expose the new service-room door; + # its east wall and irregular contamination geometry remain unchanged. + FacilityPrimitiveSpec( + "OriginalEastWallNorth", + "wall", + (6.0, 1.75, 1.50), + (0.10, 2.25, 1.50), + wall_color, + ), + FacilityPrimitiveSpec( + "OriginalEastWallSouth", + "wall", + (6.0, -3.25, 1.50), + (0.10, 0.75, 1.50), + wall_color, + ), + FacilityPrimitiveSpec( + "CorridorNorthWall", + "wall", + (7.75, -0.50, 1.50), + (1.75, 0.10, 1.50), + branch_color, + corridor_id="original_to_decon_transfer", + ), + FacilityPrimitiveSpec( + "CorridorSouthWall", + "wall", + (7.75, -2.50, 1.50), + (1.75, 0.10, 1.50), + branch_color, + corridor_id="original_to_decon_transfer", + ), + FacilityPrimitiveSpec( + "DeconRoomWestWallSouthJamb", + "wall", + (9.50, -0.35, 1.50), + (0.10, 0.15, 1.50), + wall_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "DeconRoomWestWallNorth", + "wall", + (9.50, 2.35, 1.50), + (0.10, 1.15, 1.50), + wall_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "ShieldServiceAlcoveWestWall", + "wall", + (9.0, 0.50, 1.50), + (0.10, 0.90, 1.50), + branch_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "ShieldServiceAlcoveSouthWall", + "wall", + (9.25, -0.40, 1.50), + (0.25, 0.10, 1.50), + branch_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "ShieldServiceAlcoveNorthWall", + "wall", + (9.25, 1.40, 1.50), + (0.25, 0.10, 1.50), + branch_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "DeconRoomNorthWallWest", + "wall", + (9.85, 3.50, 1.50), + (0.35, 0.10, 1.50), + wall_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "DeconRoomNorthWallEast", + "wall", + (13.35, 3.50, 1.50), + (1.15, 0.10, 1.50), + wall_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "DeconRoomSouthWall", + "wall", + (12.0, -2.50, 1.50), + (2.50, 0.10, 1.50), + wall_color, + room_id="remote_decon_room", + ), + FacilityPrimitiveSpec( + "DeconRoomEastWall", + "wall", + (14.50, 0.50, 1.50), + (0.10, 3.0, 1.50), + wall_color, + room_id="remote_decon_room", + ), + # North service corridor and reactor auxiliary room. + FacilityPrimitiveSpec( + "ServiceAccessWestWall", + "wall", + (10.20, 4.25, 1.50), + (0.10, 0.75, 1.50), + branch_color, + corridor_id="decon_to_reactor_service", + ), + FacilityPrimitiveSpec( + "ServiceAccessEastWall", + "wall", + (12.20, 4.25, 1.50), + (0.10, 0.75, 1.50), + branch_color, + corridor_id="decon_to_reactor_service", + ), + FacilityPrimitiveSpec( + "ReactorServiceWestWall", + "wall", + (7.50, 7.50, 1.50), + (0.10, 2.50, 1.50), + wall_color, + room_id="reactor_service_room", + ), + FacilityPrimitiveSpec( + "ReactorServiceNorthWall", + "wall", + (11.0, 10.0, 1.50), + (3.50, 0.10, 1.50), + wall_color, + room_id="reactor_service_room", + ), + FacilityPrimitiveSpec( + "ReactorServiceSouthWallWest", + "wall", + (8.85, 5.0, 1.50), + (1.35, 0.10, 1.50), + wall_color, + room_id="reactor_service_room", + ), + FacilityPrimitiveSpec( + "ReactorServiceSouthWallEast", + "wall", + (13.35, 5.0, 1.50), + (1.15, 0.10, 1.50), + wall_color, + room_id="reactor_service_room", + ), + FacilityPrimitiveSpec( + "ReactorServiceEastWallSouth", + "wall", + (14.50, 6.0, 1.50), + (0.10, 1.0, 1.50), + wall_color, + room_id="reactor_service_room", + ), + FacilityPrimitiveSpec( + "ReactorServiceEastWallNorth", + "wall", + (14.50, 9.50, 1.50), + (0.10, 0.50, 1.50), + wall_color, + room_id="reactor_service_room", + ), + # Turning passage and shield-staging room. + FacilityPrimitiveSpec( + "ShieldTransferSouthWall", + "wall", + (15.75, 7.0, 1.50), + (1.25, 0.10, 1.50), + branch_color, + corridor_id="reactor_to_shield_staging", + ), + FacilityPrimitiveSpec( + "ShieldTransferNorthWall", + "wall", + (15.75, 9.0, 1.50), + (1.25, 0.10, 1.50), + branch_color, + corridor_id="reactor_to_shield_staging", + ), + FacilityPrimitiveSpec( + "ShieldStagingWestWallSouth", + "wall", + (17.0, 6.0, 1.50), + (0.10, 1.0, 1.50), + wall_color, + room_id="shield_staging_room", + ), + FacilityPrimitiveSpec( + "ShieldStagingWestWallNorth", + "wall", + (17.0, 9.50, 1.50), + (0.10, 0.50, 1.50), + wall_color, + room_id="shield_staging_room", + ), + FacilityPrimitiveSpec( + "ShieldStagingSouthWall", + "wall", + (19.50, 5.0, 1.50), + (2.50, 0.10, 1.50), + wall_color, + room_id="shield_staging_room", + ), + FacilityPrimitiveSpec( + "ShieldStagingNorthWall", + "wall", + (19.50, 10.0, 1.50), + (2.50, 0.10, 1.50), + wall_color, + room_id="shield_staging_room", + ), + FacilityPrimitiveSpec( + "ShieldStagingEastWall", + "wall", + (22.0, 7.50, 1.50), + (0.10, 2.50, 1.50), + wall_color, + room_id="shield_staging_room", + ), + # Reactor-service equipment. Composite entries share one equipment_id, + # so audit counts represent actual equipment, not the number of meshes. + equipment( + "reactor_service_vessel", + "reactor_service_vessel", + "Body", + (8.50, 8.45, 1.15), + (0.55, 0.55, 1.15), + steel_color, + room_id="reactor_service_room", + shape="cylinder", + display_name="Drained reactor service vessel", + ), + equipment( + "reactor_service_vessel", + "reactor_service_vessel", + "Pedestal", + (8.50, 8.45, 0.12), + (0.72, 0.72, 0.12), + (0.12, 0.14, 0.15), + room_id="reactor_service_room", + ), + equipment( + "primary_loop_pipe_rack", + "process_pipe_rack", + "Header", + (9.0, 6.35, 2.15), + (0.10, 0.78, 0.10), + equipment_blue, + room_id="reactor_service_room", + shape="cylinder", + axis="Y", + display_name="Isolated primary-loop pipe rack", + ), + equipment( + "primary_loop_pipe_rack", + "process_pipe_rack", + "SupportWest", + (9.0, 5.72, 1.08), + (0.08, 0.08, 1.08), + steel_color, + room_id="reactor_service_room", + ), + equipment( + "primary_loop_pipe_rack", + "process_pipe_rack", + "SupportEast", + (9.0, 6.98, 1.08), + (0.08, 0.08, 1.08), + steel_color, + room_id="reactor_service_room", + ), + equipment( + "valve_manifold", + "isolated_valve_manifold", + "Cabinet", + (13.55, 6.05, 0.72), + (0.42, 0.34, 0.72), + equipment_blue, + room_id="reactor_service_room", + display_name="Tagged isolation-valve manifold", + ), + equipment( + "valve_manifold", + "isolated_valve_manifold", + "ValveStem", + (13.55, 6.05, 1.58), + (0.10, 0.10, 0.18), + service_yellow, + room_id="reactor_service_room", + shape="cylinder", + ), + equipment( + "coolant_pump_skid", + "drained_coolant_pump", + "Skid", + (8.35, 5.55, 0.12), + (0.62, 0.36, 0.12), + (0.11, 0.12, 0.13), + room_id="reactor_service_room", + display_name="Drained coolant pump skid", + ), + equipment( + "coolant_pump_skid", + "drained_coolant_pump", + "Pump", + (8.35, 5.55, 0.52), + (0.31, 0.31, 0.40), + steel_color, + room_id="reactor_service_room", + shape="cylinder", + ), + equipment( + "heat_exchanger", + "isolated_heat_exchanger", + "Shell", + (13.15, 9.25, 1.05), + (0.72, 0.30, 0.30), + steel_color, + room_id="reactor_service_room", + shape="cylinder", + axis="X", + display_name="Isolated residual-heat exchanger", + ), + equipment( + "heat_exchanger", + "isolated_heat_exchanger", + "Saddle", + (13.15, 9.25, 0.48), + (0.62, 0.22, 0.18), + (0.12, 0.14, 0.15), + room_id="reactor_service_room", + ), + equipment( + "electrical_switchgear", + "legacy_switchgear", + "Cabinet", + (8.0, 9.48, 0.85), + (0.30, 0.28, 0.85), + (0.16, 0.25, 0.20), + room_id="reactor_service_room", + display_name="De-energized legacy switchgear", + ), + equipment( + "cable_tray", + "overhead_cable_tray", + "Tray", + (11.25, 9.62, 2.55), + (1.55, 0.10, 0.08), + (0.30, 0.24, 0.14), + room_id="reactor_service_room", + display_name="Overhead legacy cable tray", + ), + equipment( + "scaffold_tower", + "maintenance_scaffold", + "Platform", + (12.35, 5.55, 1.72), + (0.40, 0.34, 0.08), + service_yellow, + room_id="reactor_service_room", + display_name="Maintenance scaffold tower", + ), + equipment( + "scaffold_tower", + "maintenance_scaffold", + "LegWest", + (12.05, 5.55, 0.86), + (0.05, 0.05, 0.86), + steel_color, + room_id="reactor_service_room", + ), + equipment( + "scaffold_tower", + "maintenance_scaffold", + "LegEast", + (12.65, 5.55, 0.86), + (0.05, 0.05, 0.86), + steel_color, + room_id="reactor_service_room", + ), + # Staging-room inventory, waste, and dismantling debris remain along + # the room perimeter, leaving the center aisle and shield pickup clear. + equipment( + "sealed_waste_drums", + "sealed_waste_drum_bank", + "DrumA", + (17.75, 5.75, 0.45), + (0.25, 0.25, 0.45), + (0.43, 0.31, 0.08), + room_id="shield_staging_room", + shape="cylinder", + display_name="Sealed low-level waste drum bank", + ), + equipment( + "sealed_waste_drums", + "sealed_waste_drum_bank", + "DrumB", + (18.35, 5.75, 0.45), + (0.25, 0.25, 0.45), + (0.38, 0.28, 0.07), + room_id="shield_staging_room", + shape="cylinder", + ), + equipment( + "debris_pallet", + "segmented_pipe_debris_pallet", + "Pallet", + (19.65, 5.65, 0.18), + (0.70, 0.40, 0.18), + (0.29, 0.19, 0.10), + room_id="shield_staging_room", + display_name="Segmented pipe debris pallet", + ), + equipment( + "debris_pallet", + "segmented_pipe_debris_pallet", + "Pipe", + (19.65, 5.65, 0.46), + (0.62, 0.09, 0.09), + steel_color, + room_id="shield_staging_room", + shape="cylinder", + axis="X", + ), + equipment( + "ventilation_duct", + "temporary_ventilation_duct", + "Duct", + (19.45, 9.62, 2.45), + (1.20, 0.16, 0.16), + (0.36, 0.38, 0.40), + room_id="shield_staging_room", + display_name="Temporary filtered ventilation duct", + ), + equipment( + "sampling_cabinet", + "radiological_sampling_cabinet", + "Cabinet", + (21.35, 5.80, 0.82), + (0.32, 0.30, 0.82), + equipment_blue, + room_id="shield_staging_room", + display_name="Radiological sampling cabinet", + ), + equipment( + "tool_crib", + "remote_tool_crib", + "Crib", + (17.75, 9.50, 0.72), + (0.42, 0.30, 0.72), + (0.18, 0.22, 0.24), + room_id="shield_staging_room", + display_name="Remote tooling crib", + ), + equipment( + "shield_storage_rack", + "shield_storage_rack", + "Shelf", + (21.30, 9.55, 1.05), + (0.42, 0.25, 0.08), + service_yellow, + room_id="shield_staging_room", + display_name="Empty shield storage rack", + ), + equipment( + "shield_storage_rack", + "shield_storage_rack", + "PostWest", + (20.98, 9.55, 0.53), + (0.05, 0.05, 0.53), + steel_color, + room_id="shield_staging_room", + ), + equipment( + "shield_storage_rack", + "shield_storage_rack", + "PostEast", + (21.62, 9.55, 0.53), + (0.05, 0.05, 0.53), + steel_color, + room_id="shield_staging_room", + ), + ) + reserved_routes = ( + FacilityRouteSpec( + "primary_decon_route", + ( + (0.0, -1.50, 0.28), + (5.20, -1.50, 0.28), + (8.90, -1.50, 0.28), + (10.30, -1.50, 0.28), + (10.30, 0.80, 0.28), + (13.49, 0.80, 0.28), + ), + ), + FacilityRouteSpec( + "staging_room_service_route", + ( + (10.30, 0.80, 0.28), + (11.20, 4.25, 0.28), + (11.20, 7.95, 0.28), + (15.75, 8.0, 0.28), + (18.20, 8.20, 0.28), + (20.15, 8.45, 0.28), + ), + ), + FacilityRouteSpec( + "primary_shield_25_service_route", + ( + (3.84, 2.80, 0.28), + (5.20, -1.50, 0.28), + (8.90, -1.50, 0.28), + (10.30, -1.50, 0.28), + (10.30, 0.42755624655, 0.28), + (9.8825, 0.42755624655, 0.28), + ), + ), + FacilityRouteSpec( + "primary_shield_65_service_route", + ( + (9.8825, 0.42755624655, 0.28), + (10.30, 0.42755624655, 0.28), + (10.30, -1.50, 0.28), + (8.90, -1.50, 0.28), + (5.20, -1.50, 0.28), + (4.1265, 0.19952624839, 0.28), + ), + ), + ) + return FacilityLayoutManifest( + layout_id="four_room_decommissioning_cell_v1", + root_path="/World/RemoteDeconFacility", + rooms=rooms, + corridors=corridors, + primitives=primitives, + reserved_routes=reserved_routes, + secondary_shield_path="/World/StagingLeadShield", + secondary_shield_position_m=(21.15, 8.45, 0.0), + ) + + +def facility_route_clearance_m( + layout: FacilityLayoutManifest, + route_id: str, + *, + sample_spacing_m: float = 0.05, +) -> float: + """Return planar clearance from a reserved route to walls and equipment.""" + + if not math.isfinite(sample_spacing_m) or sample_spacing_m <= 0.0: + raise ValueError("sample_spacing_m must be finite and positive") + route = next( + (item for item in layout.reserved_routes if item.route_id == route_id), + None, + ) + if route is None: + raise KeyError(f"unknown facility route: {route_id}") + obstacles = tuple( + primitive + for primitive in layout.primitives + if primitive.collision and primitive.category in {"wall", "equipment"} + ) + minimum = math.inf + for start, end in zip(route.waypoints_m[:-1], route.waypoints_m[1:], strict=True): + distance = math.dist(start[:2], end[:2]) + sample_count = max(1, int(math.ceil(distance / sample_spacing_m))) + for sample_index in range(sample_count + 1): + fraction = sample_index / sample_count + x = start[0] + fraction * (end[0] - start[0]) + y = start[1] + fraction * (end[1] - start[1]) + for primitive in obstacles: + lower_x = primitive.center_m[0] - primitive.half_extent_m[0] + upper_x = primitive.center_m[0] + primitive.half_extent_m[0] + lower_y = primitive.center_m[1] - primitive.half_extent_m[1] + upper_y = primitive.center_m[1] + primitive.half_extent_m[1] + dx = max(lower_x - x, 0.0, x - upper_x) + dy = max(lower_y - y, 0.0, y - upper_y) + minimum = min(minimum, math.hypot(dx, dy)) + return minimum + + def _custom_attribute(prim: Any, name: str, value_type: Any, value: object) -> None: attribute = prim.GetAttribute(name) if not attribute: @@ -148,6 +1033,355 @@ def _cube( return cube.GetPrim() +def _cylinder( + stage: Any, + path: str, + *, + translate: Sequence[float], + half_extent: Sequence[float], + axis: str, + color: Sequence[float], + collision: bool = True, +) -> Any: + from pxr import Gf, UsdGeom, UsdPhysics + + half_x, half_y, half_z = map(float, half_extent) + dimensions = { + "X": (max(half_y, half_z), 2.0 * half_x), + "Y": (max(half_x, half_z), 2.0 * half_y), + "Z": (max(half_x, half_y), 2.0 * half_z), + } + try: + radius, height = dimensions[axis] + except KeyError as error: + raise ValueError(f"unsupported cylinder axis: {axis}") from error + cylinder = UsdGeom.Cylinder.Define(stage, path) + cylinder.CreateAxisAttr(axis) + cylinder.CreateRadiusAttr(radius) + cylinder.CreateHeightAttr(height) + UsdGeom.Xformable(cylinder).AddTranslateOp().Set(Gf.Vec3d(*map(float, translate))) + cylinder.CreateDisplayColorAttr([Gf.Vec3f(*map(float, color))]) + if collision: + UsdPhysics.CollisionAPI.Apply(cylinder.GetPrim()).CreateCollisionEnabledAttr(True) + return cylinder.GetPrim() + + +def _author_remote_decon_facility( + stage: Any, +) -> FacilityLayoutManifest: + """Author four connected rooms while preserving the remote decon geometry.""" + + from pxr import Gf, Sdf, UsdGeom + + layout = decommissioning_facility_layout() + root_path = layout.root_path + for stale_path in ( + "/World/ComplexFacility", + "/World/MazeFacility", + layout.secondary_shield_path, + root_path, + ): + if stage.GetPrimAtPath(stale_path).IsValid(): + stage.RemovePrim(stale_path) + original_east_wall = "/World/Environment/WallEast" + if stage.GetPrimAtPath(original_east_wall).IsValid(): + stage.RemovePrim(original_east_wall) + root = UsdGeom.Xform.Define(stage, root_path).GetPrim() + root.SetDisplayName("Four-room decommissioning validation facility") + for name, value_type, value in ( + ("rad:role", Sdf.ValueTypeNames.String, "decommissioning_facility"), + ("rad:facility:layoutId", Sdf.ValueTypeNames.String, layout.layout_id), + ("rad:facility:deterministic", Sdf.ValueTypeNames.Bool, True), + ("rad:facility:roomCount", Sdf.ValueTypeNames.Int, len(layout.rooms)), + ("rad:facility:corridorCount", Sdf.ValueTypeNames.Int, len(layout.corridors)), + ( + "rad:facility:equipmentCount", + Sdf.ValueTypeNames.Int, + len(layout.equipment_ids), + ), + ( + "rad:facility:obstacleCount", + Sdf.ValueTypeNames.Int, + len(layout.equipment_ids), + ), + ( + "rad:facility:reservedRouteIds", + Sdf.ValueTypeNames.StringArray, + [route.route_id for route in layout.reserved_routes], + ), + ( + "rad:facility:deconSurfaceGeometry", + Sdf.ValueTypeNames.String, + "same_visible_irregular_collision_activity_mesh", + ), + ): + _custom_attribute(root, name, value_type, value) + + UsdGeom.Scope.Define(stage, f"{root_path}/Rooms") + for room in layout.rooms: + marker = UsdGeom.Xform.Define(stage, f"{root_path}/Rooms/{room.room_id}").GetPrim() + marker.SetDisplayName(room.display_name) + for name, value_type, value in ( + ("rad:facility:roomId", Sdf.ValueTypeNames.String, room.room_id), + ( + "rad:facility:centerM", + Sdf.ValueTypeNames.Double3, + Gf.Vec3d(*room.center_m), + ), + ( + "rad:facility:halfExtentM", + Sdf.ValueTypeNames.Double3, + Gf.Vec3d(*room.half_extent_m), + ), + ): + _custom_attribute(marker, name, value_type, value) + + UsdGeom.Scope.Define(stage, f"{root_path}/Corridors") + for corridor in layout.corridors: + marker = UsdGeom.Xform.Define( + stage, f"{root_path}/Corridors/{corridor.corridor_id}" + ).GetPrim() + marker.SetDisplayName(corridor.corridor_id.replace("_", " ").title()) + for name, value_type, value in ( + ( + "rad:facility:corridorId", + Sdf.ValueTypeNames.String, + corridor.corridor_id, + ), + ( + "rad:facility:fromRoomId", + Sdf.ValueTypeNames.String, + corridor.from_room_id, + ), + ( + "rad:facility:toRoomId", + Sdf.ValueTypeNames.String, + corridor.to_room_id, + ), + ( + "rad:facility:centerM", + Sdf.ValueTypeNames.Double3, + Gf.Vec3d(*corridor.center_m), + ), + ( + "rad:facility:halfExtentM", + Sdf.ValueTypeNames.Double3, + Gf.Vec3d(*corridor.half_extent_m), + ), + ): + _custom_attribute(marker, name, value_type, value) + + equipment_roots: dict[str, Any] = {} + for primitive in layout.primitives: + if primitive.equipment_id is not None: + equipment_root = equipment_roots.get(primitive.equipment_id) + if equipment_root is None: + equipment_root = UsdGeom.Xform.Define( + stage, + f"{root_path}/Equipment/{primitive.equipment_id}", + ).GetPrim() + equipment_roots[primitive.equipment_id] = equipment_root + equipment_root.SetDisplayName( + primitive.display_name or primitive.equipment_id.replace("_", " ").title() + ) + for name, value_type, value in ( + ("rad:role", Sdf.ValueTypeNames.String, "facility_equipment"), + ( + "rad:facility:equipmentId", + Sdf.ValueTypeNames.String, + primitive.equipment_id, + ), + ( + "rad:facility:equipmentType", + Sdf.ValueTypeNames.String, + primitive.equipment_type or "decommissioning_obstacle", + ), + ( + "rad:facility:roomId", + Sdf.ValueTypeNames.String, + primitive.room_id or "", + ), + ("rad:facility:fixed", Sdf.ValueTypeNames.Bool, True), + ): + _custom_attribute(equipment_root, name, value_type, value) + + path = f"{root_path}/{primitive.path}" + if primitive.shape == "cube": + authored = _cube( + stage, + path, + translate=primitive.center_m, + half_scale=primitive.half_extent_m, + color=primitive.color, + collision=primitive.collision, + ) + elif primitive.shape == "cylinder": + authored = _cylinder( + stage, + path, + translate=primitive.center_m, + half_extent=primitive.half_extent_m, + axis=primitive.axis, + color=primitive.color, + collision=primitive.collision, + ) + else: + raise ValueError(f"unsupported facility primitive shape: {primitive.shape}") + _custom_attribute( + authored, + "rad:facility:category", + Sdf.ValueTypeNames.String, + primitive.category, + ) + _custom_attribute( + authored, + "rad:material:id", + Sdf.ValueTypeNames.String, + "steel" if primitive.category == "equipment" else "concrete", + ) + if primitive.room_id is not None: + _custom_attribute( + authored, + "rad:facility:roomId", + Sdf.ValueTypeNames.String, + primitive.room_id, + ) + if primitive.corridor_id is not None: + _custom_attribute( + authored, + "rad:facility:corridorId", + Sdf.ValueTypeNames.String, + primitive.corridor_id, + ) + + # Non-colliding stripes expose all three corridor centerlines in the GUI. + stripe_specs = ( + ( + "Transfer", + ((6.45, -1.50), (7.20, -1.50), (7.95, -1.50), (8.70, -1.50), (9.25, -1.50)), + (0.18, 0.025), + ), + ( + "Service", + ((11.20, 3.78), (11.20, 4.25), (11.20, 4.72)), + (0.025, 0.14), + ), + ( + "Shield", + ((14.85, 8.0), (15.45, 8.0), (16.05, 8.0), (16.65, 8.0)), + (0.14, 0.025), + ), + ) + for group, points, half_xy in stripe_specs: + for index, (x_position, y_position) in enumerate(points, start=1): + stripe = _cube( + stage, + f"{root_path}/{group}CorridorStripe{index}", + translate=(x_position, y_position, 0.006), + half_scale=(*half_xy, 0.006), + color=(0.90, 0.66, 0.05), + collision=False, + ) + _custom_attribute( + stripe, + "rad:facility:category", + Sdf.ValueTypeNames.String, + "route_marking", + ) + + remote_station = UsdGeom.Xform.Define(stage, "/World/DetectorStations/RemoteDeconRoom") + remote_station.ClearXformOpOrder() + remote_station.AddTranslateOp().Set(Gf.Vec3d(11.10, 1.65, 0.80)) + station_prim = remote_station.GetPrim() + station_prim.SetDisplayName("Remote decontamination room") + _custom_attribute(station_prim, "rad:role", Sdf.ValueTypeNames.String, "detector_station") + _custom_attribute(station_prim, "rad:detector:id", Sdf.ValueTypeNames.String, "gamma-counter") + _custom_attribute( + station_prim, + "rad:facility:roomId", + Sdf.ValueTypeNames.String, + "remote_decon_room", + ) + return layout + + +def _author_staging_lead_shield( + stage: Any, + layout: FacilityLayoutManifest, + *, + grasp_offset_m: tuple[float, float, float], +) -> None: + """Author a second movable physical shield without touching the primary.""" + + from pxr import Gf, Sdf, UsdGeom, UsdPhysics + + path = layout.secondary_shield_path + shield = UsdGeom.Xform.Define(stage, path) + shield.AddTranslateOp().Set(Gf.Vec3d(*layout.secondary_shield_position_m)) + prim = shield.GetPrim() + prim.SetDisplayName("Staged lead service shield 02") + UsdPhysics.RigidBodyAPI.Apply(prim).CreateRigidBodyEnabledAttr(True) + mass = UsdPhysics.MassAPI.Apply(prim) + mass.CreateMassAttr(2.2) + # The service panel is explicitly ballasted at its wheeled base. Without + # the authored low centre of mass, PhysX derives it mostly from the tall + # plate volume and the panel can topple after a correct gripper release. + mass.CreateCenterOfMassAttr(Gf.Vec3f(0.0, 0.0, 0.10)) + for name, value_type, value in ( + ("rad:role", Sdf.ValueTypeNames.String, "shield"), + ("rad:material:id", Sdf.ValueTypeNames.String, "lead"), + ("rad:material:mode", Sdf.ValueTypeNames.String, "solid"), + ("rad:shield:movable", Sdf.ValueTypeNames.Bool, True), + ("rad:shield:resourceUnits", Sdf.ValueTypeNames.Int, 1), + ("rad:shield:staged", Sdf.ValueTypeNames.Bool, True), + ("rad:shield:inventoryId", Sdf.ValueTypeNames.String, "staging-shield-02"), + ("rad:manipulation:movable", Sdf.ValueTypeNames.Bool, True), + ("rad:manipulation:removable", Sdf.ValueTypeNames.Bool, False), + ( + "rad:manipulation:graspFrame", + Sdf.ValueTypeNames.String, + "ShieldGraspFrame", + ), + ("rad:facility:roomId", Sdf.ValueTypeNames.String, "shield_staging_room"), + ): + _custom_attribute(prim, name, value_type, value) + _cube( + stage, + path + "/Base", + translate=(0.0, 0.0, 0.035), + half_scale=(0.18, 0.30, 0.035), + color=(0.11, 0.12, 0.13), + ) + plate = _cube( + stage, + path + "/Plate", + translate=(0.0, 0.0, 0.55), + half_scale=(0.025, 0.22, 0.35), + color=(0.20, 0.22, 0.24), + ) + _custom_attribute(plate, "rad:material:id", Sdf.ValueTypeNames.String, "lead") + _custom_attribute(plate, "rad:material:mode", Sdf.ValueTypeNames.String, "solid") + handle_inner_x = -0.025 + handle_outer_x = grasp_offset_m[0] - 0.04 + _cube( + stage, + path + "/Handle", + translate=( + 0.5 * (handle_inner_x + handle_outer_x), + grasp_offset_m[1], + grasp_offset_m[2], + ), + half_scale=( + 0.5 * abs(handle_inner_x - handle_outer_x), + 0.015, + 0.018, + ), + color=(0.92, 0.56, 0.08), + ) + grasp = UsdGeom.Xform.Define(stage, path + "/ShieldGraspFrame") + grasp.AddTranslateOp().Set(Gf.Vec3d(*grasp_offset_m)) + + def add_real_robot_references( stage: Any, *, @@ -197,18 +1431,51 @@ def author_real_robot_task_scene( if missing: raise RuntimeError(f"real robot references are not composed: {missing}") - # The legacy vertical slice authors both a floor cube and a second collision - # mesh 15 mm above it. Keep that mesh as a radiation source, but remove the - # duplicate contact plane so wheeled robots are not trapped between surfaces. + # The legacy floor patch is replaced by the reachable workbench below. It + # remains in the stage for provenance, but cannot participate in treatment + # or radiation after the high-fidelity task scene is composed. legacy_surface = stage.GetPrimAtPath("/World/ContaminatedFloor") - legacy_collision = legacy_surface.GetAttribute("physics:collisionEnabled") - if legacy_collision: - legacy_collision.Set(False) + if legacy_surface.IsValid(): + legacy_collision = legacy_surface.GetAttribute("physics:collisionEnabled") + if legacy_collision: + legacy_collision.Set(False) + for attribute_name in ("rad:source:enabled", "rad:decon:enabled"): + attribute = legacy_surface.GetAttribute(attribute_name) + if attribute: + attribute.Set(False) countermeasure = stage.GetPrimAtPath(cfg.countermeasure_root) measurement = stage.GetPrimAtPath(cfg.measurement_root) _custom_attribute(countermeasure, "rad:role", Sdf.ValueTypeNames.String, "countermeasure_robot") _custom_attribute(measurement, "rad:role", Sdf.ValueTypeNames.String, "measurement_robot") + for prim, model, dofs, controller in ( + ( + countermeasure, + "Clearpath Ridgeback + Franka Emika Panda", + 12, + "holonomic articulation + Lula IK + finger joints", + ), + ( + measurement, + "NVIDIA Nova Carter", + 7, + "differential wheel articulation", + ), + ): + _custom_attribute(prim, "rad:robot:model", Sdf.ValueTypeNames.String, model) + _custom_attribute(prim, "rad:robot:dofCount", Sdf.ValueTypeNames.Int, dofs) + _custom_attribute( + prim, + "rad:robot:controller", + Sdf.ValueTypeNames.String, + controller, + ) + _custom_attribute( + prim, + "rad:robot:geometryFidelity", + Sdf.ValueTypeNames.String, + "manufacturer_asset", + ) # The Ridgeback asset represents its holonomic base with x/y/yaw joints. # Increase their physical drives before the timeline starts so the base can @@ -291,48 +1558,58 @@ def author_real_robot_task_scene( collision=False, ) - for path in ("/World/DeconWorkbench", cfg.decon_surface_path, cfg.shield_path): + for path in ( + "/World/DeconWorkbench", + cfg.decon_surface_path, + cfg.shield_path, + ): if stage.GetPrimAtPath(path).IsValid(): stage.RemovePrim(path) - workbench = UsdGeom.Xform.Define(stage, "/World/DeconWorkbench") - _cube( - stage, - "/World/DeconWorkbench/Top", - translate=(0.68, 0.0, 0.60), - half_scale=(0.23, 0.30, 0.04), - color=(0.16, 0.18, 0.19), - ) - for index, (x, y) in enumerate( - ((0.50, -0.23), (0.86, -0.23), (0.50, 0.23), (0.86, 0.23)) - ): - _cube( - stage, - f"/World/DeconWorkbench/Leg{index}", - translate=(x, y, 0.29), - half_scale=(0.035, 0.035, 0.29), - color=(0.10, 0.11, 0.12), - ) - workbench.GetPrim().SetDisplayName("Contaminated work surface") - - x_values = (0.48, 0.68, 0.88) - y_values = (-0.24, 0.0, 0.24) - points = [Gf.Vec3f(x, y, 0.651) for y in y_values for x in x_values] + from radcounter.core.surface_decontamination import irregular_deposition_field + + surface_x, surface_y, surface_z = cfg.decon_workbench_center_m + cells_y, cells_z = 48, 28 + source_width_m, source_height_m = 0.72, 0.56 + field = irregular_deposition_field(cells_y, cells_z).reshape(cells_z, cells_y) + active_cells = np.argwhere(field > 0.0) + cell_width_m = source_width_m / cells_y + cell_height_m = source_height_m / cells_z + points: list[Gf.Vec3f] = [] indices: list[int] = [] - for row in range(2): - for column in range(2): - lower_left = row * 3 + column - lower_right = lower_left + 1 - upper_left = lower_left + 3 - upper_right = upper_left + 1 - indices.extend( - (lower_left, lower_right, upper_right, lower_left, upper_right, upper_left) + colors: list[Gf.Vec3f] = [] + maximum_activity = max(float(field.max()), 1e-12) + wall_color = np.asarray((0.20, 0.27, 0.32), dtype=np.float64) + for row, column in active_cells: + y_center = surface_y + ((float(column) + 0.5) * cell_width_m - source_width_m * 0.5) + z_center = surface_z + ((float(row) + 0.5) * cell_height_m - source_height_m * 0.5) + half_y = cell_width_m * 0.515 + half_z = cell_height_m * 0.515 + start = len(points) + points.extend( + ( + Gf.Vec3f(surface_x, y_center - half_y, z_center - half_z), + Gf.Vec3f(surface_x, y_center + half_y, z_center - half_z), + Gf.Vec3f(surface_x, y_center + half_y, z_center + half_z), + Gf.Vec3f(surface_x, y_center - half_y, z_center + half_z), ) + ) + # Clockwise winding from the room gives both triangles a -X normal, + # matching a tool that approaches the east wall from inside the room. + indices.extend((start, start + 2, start + 1, start, start + 3, start + 2)) + fraction = float(field[row, column]) / maximum_activity + hot_overlay = np.asarray( + (0.44 + 0.42 * fraction, 0.055 + 0.08 * fraction, 0.018), + dtype=np.float64, + ) + color = 0.28 * hot_overlay + 0.72 * wall_color + colors.extend((Gf.Vec3f(*color), Gf.Vec3f(*color))) surface = UsdGeom.Mesh.Define(stage, cfg.decon_surface_path) surface.CreatePointsAttr(points) - surface.CreateFaceVertexCountsAttr([3] * 8) + surface.CreateFaceVertexCountsAttr([3] * (2 * len(active_cells))) surface.CreateFaceVertexIndicesAttr(indices) surface.CreateSubdivisionSchemeAttr("none") - surface.CreateDisplayColorAttr([Gf.Vec3f(0.78, 0.18, 0.08)]) + display_color = surface.CreateDisplayColorAttr(colors) + display_color.SetMetadata("interpolation", UsdGeom.Tokens.uniform) UsdPhysics.CollisionAPI.Apply(surface.GetPrim()).CreateCollisionEnabledAttr(True) surface_prim = surface.GetPrim() activity_path = Path(activity_map_path).resolve() @@ -348,14 +1625,56 @@ def author_real_robot_task_scene( ("rad:decon:enabled", Sdf.ValueTypeNames.Bool, True), ("rad:decon:activityMapUri", Sdf.ValueTypeNames.String, str(activity_path)), ("rad:decon:activityMapSha256", Sdf.ValueTypeNames.String, digest), + ("rad:decon:efficiencyMean", Sdf.ValueTypeNames.Double, 0.86), + ("rad:decon:efficiencyStd", Sdf.ValueTypeNames.Double, 0.08), + ("rad:decon:minToolDwellS", Sdf.ValueTypeNames.Double, 0.4), + ("rad:source:irregularMask", Sdf.ValueTypeNames.Bool, True), + ("rad:source:candidateCellCount", Sdf.ValueTypeNames.Int, cells_y * cells_z), + ("rad:source:activeCellCount", Sdf.ValueTypeNames.Int, len(active_cells)), + ("rad:source:activeFaceCount", Sdf.ValueTypeNames.Int, 2 * len(active_cells)), + ( + "rad:source:depositionModel", + Sdf.ValueTypeNames.String, + "gaussian_lobes_correlated_roughness_holes_satellite_droplets", + ), + ("rad:decon:rasterRows", Sdf.ValueTypeNames.Int, 6), ): _custom_attribute(surface_prim, name, value_type, value) + surface_prim.SetDisplayName("Cs-137 wall-mounted planar contamination source") + _custom_attribute( + surface_prim, + "rad:source:geometry", + Sdf.ValueTypeNames.String, + "irregular_masked_triangle_activity_map", + ) + _custom_attribute( + surface_prim, + "rad:decon:surfaceOrientation", + Sdf.ValueTypeNames.String, + "vertical_x", + ) + facility_layout = ( + _author_remote_decon_facility(stage) if cfg.include_validation_facility else None + ) + if facility_layout is None: + # Imported environments retain their own geometry. The canonical + # validation cell remains available as the separate vertical-slice + # environment and must never be silently overlaid on a selected CAD. + for validation_path in ( + "/World/RemoteDeconFacility", + "/World/StagingLeadShield", + "/World/DetectorStations/RemoteDeconRoom", + ): + if stage.GetPrimAtPath(validation_path).IsValid(): + stage.RemovePrim(validation_path) shield = UsdGeom.Xform.Define(stage, cfg.shield_path) - shield.AddTranslateOp().Set(Gf.Vec3d(0.78, -0.62, 0.0)) + shield.AddTranslateOp().Set(Gf.Vec3d(*cfg.shield_initial_position_m)) shield_prim = shield.GetPrim() UsdPhysics.RigidBodyAPI.Apply(shield_prim).CreateRigidBodyEnabledAttr(True) - UsdPhysics.MassAPI.Apply(shield_prim).CreateMassAttr(2.2) + mass = UsdPhysics.MassAPI.Apply(shield_prim) + mass.CreateMassAttr(2.2) + mass.CreateCenterOfMassAttr(Gf.Vec3f(0.0, 0.0, 0.10)) for name, value_type, value in ( ("rad:role", Sdf.ValueTypeNames.String, "shield"), ("rad:material:id", Sdf.ValueTypeNames.String, "lead"), @@ -371,7 +1690,7 @@ def author_real_robot_task_scene( stage, cfg.shield_path + "/Base", translate=(0.0, 0.0, 0.035), - half_scale=(0.14, 0.20, 0.035), + half_scale=(0.18, 0.30, 0.035), color=(0.11, 0.12, 0.13), ) plate = _cube( @@ -383,17 +1702,92 @@ def author_real_robot_task_scene( ) _custom_attribute(plate, "rad:material:id", Sdf.ValueTypeNames.String, "lead") _custom_attribute(plate, "rad:material:mode", Sdf.ValueTypeNames.String, "solid") + # Extend the service handle from the west face of the plate far enough for + # the Franka wrist to descend without striking the plate. The grasp frame + # sits near the outer end while the same rigid handle remains connected to + # the shield body. + handle_inner_x = -0.025 + handle_outer_x = cfg.shield_grasp_offset_m[0] - 0.04 + handle_center_x = 0.5 * (handle_inner_x + handle_outer_x) + handle_half_length_x = 0.5 * abs(handle_inner_x - handle_outer_x) _cube( stage, cfg.shield_path + "/Handle", - translate=cfg.shield_grasp_offset_m, - half_scale=(0.10, 0.015, 0.018), + translate=( + handle_center_x, + cfg.shield_grasp_offset_m[1], + cfg.shield_grasp_offset_m[2], + ), + half_scale=(handle_half_length_x, 0.015, 0.018), color=(0.92, 0.56, 0.08), ) grasp = UsdGeom.Xform.Define(stage, cfg.shield_grasp_path) grasp.AddTranslateOp().Set(Gf.Vec3d(*cfg.shield_grasp_offset_m)) + if facility_layout is not None: + _author_staging_lead_shield( + stage, + facility_layout, + grasp_offset_m=cfg.shield_grasp_offset_m, + ) UsdGeom.Xform.Define(stage, "/World/DeconCleanTrace") + # Give every generic manipulation prop a stand-off handle that the Franka + # can approach from above without intersecting the prop body. The props + # stay inside the arm's rated payload; their large visual envelopes + # exercise collision-aware routing. + payloads = ( + ( + "/World/HiddenContaminatedDrum", + "GraspFrame", + (0.0, -0.50, 0.25), + 2.7, + ), + ( + "/World/MovableObstacle", + "ObstacleGraspFrame", + (-0.72, 0.0, 0.12), + 2.8, + ), + ) + for object_path, frame_name, frame_offset, payload_kg in payloads: + object_prim = stage.GetPrimAtPath(object_path) + if not object_prim or not object_prim.IsValid(): + # Environment catalogs may supply only the selected facility and + # articulated decontamination task. Optional relocation/disposal + # props are augmented when present and simply remain unavailable + # as actions when absent. + continue + UsdPhysics.MassAPI.Apply(object_prim).CreateMassAttr(payload_kg) + _custom_attribute( + object_prim, + "rad:manipulation:payloadKg", + Sdf.ValueTypeNames.Double, + payload_kg, + ) + frame_path = f"{object_path}/{frame_name}" + frame = UsdGeom.Xform.Define(stage, frame_path) + frame.ClearXformOpOrder() + frame.AddTranslateOp().Set(Gf.Vec3d(*frame_offset)) + _cube( + stage, + object_path + "/ManipulatorHandle", + translate=frame_offset, + half_scale=(0.10, 0.015, 0.018), + color=(0.92, 0.56, 0.08), + collision=False, + ) + if object_path == "/World/HiddenContaminatedDrum": + # Join the outer crossbar to the drum while keeping the Franka + # wrist clear of the 0.32 m drum radius during vertical approach. + _cube( + stage, + object_path + "/ManipulatorHandleStem", + translate=(0.0, -0.41, 0.25), + half_scale=(0.018, 0.09, 0.018), + color=(0.92, 0.56, 0.08), + collision=False, + ) + class RidgebackFrankaController: """Control every Ridgeback+Franka DOF through one Isaac articulation.""" @@ -414,6 +1808,7 @@ def __init__( *, config: RealRobotAssetConfig | None = None, articulation: Any | None = None, + progress_callback: OperationProgressCallback | None = None, ) -> None: from isaacsim.core.prims import SingleArticulation from isaacsim.robot_motion.motion_generation import ( @@ -450,14 +1845,17 @@ def __init__( arm = self._arm_positions() self._arm_min = arm.copy() self._arm_max = arm.copy() - self._grasp_joint_path = ( - self.config.panda_hand_path + "/RadCounterShieldGraspJoint" - ) + self._grasp_joint_path = self.config.panda_hand_path + "/RadCounterShieldGraspJoint" self._handle_collision_enabled = True + self._grasp_collision_state: dict[str, bool] = {} + self._grasped_object_path: str | None = None self.last_base_target = np.zeros(3, dtype=np.float64) self.last_base_positions = self._positions()[ [self.indices[name] for name in self.base_joint_names] ].copy() + self.last_base_motion_steps = 0 + self.trace: list[str] = ["idle"] + self.progress_callback = progress_callback @staticmethod def _smoothstep(value: float) -> float: @@ -479,6 +1877,14 @@ def _observe_arm(self) -> None: def arm_joint_excursion_rad(self) -> float: return float(np.max(self._arm_max - self._arm_min)) + def _transition(self, state: str) -> None: + self.trace.append(state) + self._emit_progress(phase=state) + + def _emit_progress(self, **event: object) -> None: + if self.progress_callback is not None: + self.progress_callback(event) + def _step(self, callback: FrameCallback | None, frame: int) -> None: self.stepper.step(render=True) self._observe_arm() @@ -510,24 +1916,124 @@ def move_base( indices = np.asarray([self.indices[name] for name in self.base_joint_names], dtype=np.int32) self.last_base_target = target.copy() start = self._positions()[indices] + self.last_base_motion_steps = 0 for frame in range(1, interpolation_steps + 1): + self.last_base_motion_steps += 1 fraction = self._smoothstep(frame / interpolation_steps) command = start + fraction * (target - start) self.controller.apply_action( ArticulationAction(joint_positions=command, joint_indices=indices) ) self._step(None, frame) + if frame == 1 or frame % 6 == 0 or frame == interpolation_steps: + self._emit_progress( + phase="navigating", + progress=float(frame / interpolation_steps), + target_m=tuple(map(float, target)), + ) for frame in range(1, settle_steps + 1): + self.last_base_motion_steps += 1 self.controller.apply_action( ArticulationAction(joint_positions=target, joint_indices=indices) ) self._step(None, interpolation_steps + frame) if np.linalg.norm(self._positions()[indices] - target) <= tolerance: self.last_base_positions = self._positions()[indices].copy() + self._emit_progress( + phase="navigating", + progress=1.0, + target_m=tuple(map(float, target)), + ) return True self.last_base_positions = self._positions()[indices].copy() return False + def navigate_to( + self, + target_position_m: Sequence[float], + target_yaw_rad: float | None = None, + ) -> ArticulatedTaskReport: + """Move the holonomic base through articulation targets, never teleports.""" + + target = np.asarray(target_position_m, dtype=np.float64) + if target.shape not in {(2,), (3,)}: + raise ValueError("target_position_m must contain x/y or x/y/z") + yaw = ( + float(self.last_base_positions[2]) if target_yaw_rad is None else float(target_yaw_rad) + ) + self._transition("navigating") + distance = float(np.linalg.norm(target[:2] - self.last_base_positions[:2])) + interpolation_steps = max(90, min(720, int(math.ceil(distance / 0.025)))) + success = self.move_base( + (float(target[0]), float(target[1]), yaw), + interpolation_steps=interpolation_steps, + ) + state = "complete" if success else "failed" + self._transition(state) + return ArticulatedTaskReport( + state, + success, + self.last_base_motion_steps, + "articulated base target reached" if success else "articulated base target failed", + phases=("navigate",), + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + + def navigate_route( + self, + route_m: Sequence[Sequence[float]], + final_yaw_rad: float | None = None, + ) -> ArticulatedTaskReport: + phases: list[str] = [] + total_steps = 0 + route = tuple(route_m) + if not route: + return ArticulatedTaskReport( + "failed", False, 0, "articulated base route is empty", phases=() + ) + self._emit_progress( + phase="navigating", + progress=0.0, + path_m=tuple(tuple(map(float, point[:3])) for point in route), + ) + for index, waypoint in enumerate(route): + yaw = final_yaw_rad if index == len(route) - 1 else None + result = self.navigate_to(waypoint, yaw) + total_steps += result.steps + phases.append(f"navigate_{index + 1}") + if not result.success: + return ArticulatedTaskReport( + "failed", + False, + total_steps, + f"base route failed at waypoint {index + 1}", + phases=tuple(phases), + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + return ArticulatedTaskReport( + "complete", + True, + total_steps, + "articulated base route completed", + phases=tuple(phases), + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + + def check_reachability( + self, + target_position_m: Sequence[float], + *, + base_position_m: Sequence[float] | None = None, + ) -> bool: + target = np.asarray(target_position_m, dtype=np.float64) + if base_position_m is None: + base, _ = _world_pose(self.stage, self.config.panda_base_path) + else: + base = np.asarray(base_position_m, dtype=np.float64) + radial = float(np.linalg.norm(target[:2] - base[:2])) + vertical = float(target[2] - base[2]) + return radial <= 0.95 and -0.35 <= vertical <= 1.15 + def move_hand( self, target_position_m: Sequence[float], @@ -542,9 +2048,7 @@ def move_hand( target = np.asarray(target_position_m, dtype=np.float64) orientation = np.asarray( - self.downward_orientation_wxyz - if orientation_wxyz is None - else orientation_wxyz, + self.downward_orientation_wxyz if orientation_wxyz is None else orientation_wxyz, dtype=np.float64, ) self._sync_kinematics_base() @@ -613,6 +2117,56 @@ def set_gripper(self, finger_position_m: float, *, steps: int = 45) -> float: self._step(None, frame) return float(np.sum(self._positions()[indices])) + def stow_arm( + self, + *, + interpolation_steps: int = 75, + settle_steps: int = 120, + tolerance_rad: float = 0.035, + ) -> ArticulatedTaskReport: + """Fold the Franka into a repeatable collision-safe travel posture.""" + + from isaacsim.core.utils.types import ArticulationAction + + target = np.asarray((0.0, -0.60, 0.0, -2.0, 0.0, 1.50, 0.75)) + indices = np.asarray([self.indices[name] for name in self.arm_joint_names], dtype=np.int32) + start = self._positions()[indices] + self._transition("stowing_arm") + for frame in range(1, interpolation_steps + 1): + fraction = self._smoothstep(frame / interpolation_steps) + command = start + fraction * (target - start) + self.controller.apply_action( + ArticulationAction(joint_positions=command, joint_indices=indices) + ) + self._step(None, frame) + steps = interpolation_steps + for frame in range(1, settle_steps + 1): + error = float(np.max(np.abs(self._positions()[indices] - target))) + if error <= tolerance_rad: + self._transition("arm_stowed") + return ArticulatedTaskReport( + "complete", + True, + steps, + "arm folded into travel posture", + phases=("stow_arm",), + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + self.controller.apply_action( + ArticulationAction(joint_positions=target, joint_indices=indices) + ) + self._step(None, interpolation_steps + frame) + steps += 1 + self._transition("failed") + return ArticulatedTaskReport( + "failed", + False, + steps, + "arm failed to reach travel posture", + phases=("stow_arm",), + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + def set_decon_tool_visible(self, visible: bool) -> None: from pxr import UsdGeom @@ -640,14 +2194,36 @@ def execute_decontamination( hand_waypoints_m: Sequence[Sequence[float]], *, dwell_frames: int = 32, + orientation_wxyz: Sequence[float] | None = None, + approach_offset_m: Sequence[float] = (0.0, 0.0, 0.16), + retreat_offset_m: Sequence[float] = (0.0, 0.0, 0.18), + author_trace_patches: bool = True, + approach_tolerance_m: float = 0.025, + waypoint_tolerance_m: float = 0.025, + required_coverage_fraction: float = 0.0, + max_waypoint_speed_m_s: float | None = None, ) -> DecontaminationMotionReport: if not hand_waypoints_m: raise ValueError("at least one decontamination waypoint is required") + planned_path = tuple( + tuple(map(float, waypoint[:3])) for waypoint in hand_waypoints_m + ) + self._emit_progress( + phase="approaching", + progress=0.0, + coverage_fraction=0.0, + path_m=planned_path, + target_m=planned_path[0], + ) self.set_decon_tool_visible(True) activity_before = float(np.sum(decontaminator.activity_bq)) first = np.asarray(hand_waypoints_m[0], dtype=np.float64) - approach = first + np.asarray((0.0, 0.0, 0.16)) - approach_result = self.move_hand(approach) + approach = first + np.asarray(approach_offset_m, dtype=np.float64) + approach_result = self.move_hand( + approach, + orientation_wxyz, + tolerance_m=approach_tolerance_m, + ) if not approach_result.success: return DecontaminationMotionReport( False, @@ -656,10 +2232,14 @@ def execute_decontamination( 0.0, activity_before, activity_before, + 0.0, (), + 0.0, (approach_result.position_error_m,), 0.0, ) + self._transition("contact_confirmed") + self._transition("decontaminating") accepted = 0 rejected = 0 removed = 0.0 @@ -681,7 +2261,26 @@ def treatment_tick(dt_s: float, _: int) -> None: for index, waypoint in enumerate(hand_waypoints_m): contact_before = accepted - motion = self.move_hand(waypoint, callback=treatment_tick) + interpolation_steps = 75 + if max_waypoint_speed_m_s is not None: + if max_waypoint_speed_m_s <= 0.0: + raise ValueError("max_waypoint_speed_m_s must be positive") + distance_m = float( + np.linalg.norm( + np.asarray(waypoint, dtype=np.float64) - self.end_effector_position() + ) + ) + interpolation_steps = max( + interpolation_steps, + int(math.ceil(distance_m * 60.0 / max_waypoint_speed_m_s)), + ) + motion = self.move_hand( + waypoint, + orientation_wxyz, + interpolation_steps=interpolation_steps, + tolerance_m=waypoint_tolerance_m, + callback=treatment_tick, + ) errors.append(motion.position_error_m) if not motion.success: decontaminator.flush() @@ -692,17 +2291,28 @@ def treatment_tick(dt_s: float, _: int) -> None: removed, activity_before, float(np.sum(decontaminator.activity_bq)), + max(0.0, removed / activity_before) if activity_before > 0.0 else 0.0, tuple(sorted(triangles)), + len(triangles) / max(len(decontaminator.triangle_indices), 1), tuple(errors), 0.0, ) self.hold(dwell_frames, treatment_tick) - if accepted > contact_before: + if author_trace_patches and accepted > contact_before: self._clean_trace_patch(index) + self._emit_progress( + phase="decontaminating", + progress=float((index + 1) / len(hand_waypoints_m)), + coverage_fraction=( + len(triangles) / max(len(decontaminator.triangle_indices), 1) + ), + accepted_contacts=accepted, + target_m=planned_path[index], + ) retreat = np.asarray(hand_waypoints_m[-1], dtype=np.float64) + np.asarray( - (0.0, 0.0, 0.18) + retreat_offset_m, dtype=np.float64 ) - self.move_hand(retreat) + self.move_hand(retreat, orientation_wxyz) decontaminator.flush() path_length = 0.0 for first_position, second_position in zip( @@ -710,18 +2320,559 @@ def treatment_tick(dt_s: float, _: int) -> None: ): path_length += float(np.linalg.norm(second_position - first_position)) activity_after = float(np.sum(decontaminator.activity_bq)) + removed_fraction = ( + max(0.0, (activity_before - activity_after) / activity_before) + if activity_before > 0.0 + else 0.0 + ) + coverage_fraction = len(triangles) / max(len(decontaminator.triangle_indices), 1) return DecontaminationMotionReport( - accepted > 0 and removed > 0.0 and activity_after < activity_before, + accepted > 0 + and removed > 0.0 + and activity_after < activity_before + and coverage_fraction >= required_coverage_fraction, accepted, rejected, removed, activity_before, activity_after, + removed_fraction, tuple(sorted(triangles)), + coverage_fraction, tuple(errors), path_length, ) + def execute_surface_decontamination( + self, + decontaminator: Any, + duration_s: float, + ) -> DecontaminationMotionReport: + """Raster a reachable surface with Lula IK and live contact feedback.""" + + from pxr import Usd, UsdGeom + + surface = self.stage.GetPrimAtPath(decontaminator.surface_path) + bounds = ( + UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + .ComputeWorldBound(surface) + .ComputeAlignedRange() + ) + lower = np.asarray(bounds.GetMin(), dtype=np.float64) + upper = np.asarray(bounds.GetMax(), dtype=np.float64) + if not np.all(np.isfinite(lower)) or not np.all(np.isfinite(upper)): + raise RuntimeError("decontamination surface has invalid world bounds") + orientation_name = str( + surface.GetAttribute("rad:decon:surfaceOrientation").Get() or "horizontal" + ) + orientation_wxyz: tuple[float, float, float, float] | None = None + approach_offset = (0.0, 0.0, 0.16) + retreat_offset = (0.0, 0.0, 0.18) + author_trace_patches = True + approach_tolerance_m = 0.025 + waypoint_tolerance_m = 0.025 + required_coverage_fraction = 0.0 + max_waypoint_speed_m_s: float | None = None + if orientation_name == "vertical_x": + # Rotate the tool's local +Z treatment axis toward world +X so the + # pad presses against the far room's east wall. + half_sqrt = math.sqrt(0.5) + orientation_wxyz = (half_sqrt, 0.0, half_sqrt, 0.0) + hand_x = float(lower[0] - 0.139) + # Match the six-lane boustrophedon scan in the August 6 high-wall + # reference. Continuous motion between endpoints treats the real + # irregular collision faces; it does not raster a hidden rectangle. + footprint = np.asarray( + decontaminator.config.footprint_points_local_m, + dtype=np.float64, + ) + # With the tool rotated toward +X, local Y spans wall Y and local + # X spans wall Z. Keep the pad center one projected half-extent + # inside the surface: the physical pad still covers the boundary, + # while Lula is not asked to reach an unnecessarily extreme + # center pose at the final high-wall corner. + y_margin = min( + float(np.max(np.abs(footprint[:, 1]))), + max(0.0, (upper[1] - lower[1]) * 0.25), + ) + z_margin = min( + float(np.max(np.abs(footprint[:, 0]))), + max(0.0, (upper[2] - lower[2]) * 0.25), + ) + y_limits = (float(lower[1] + y_margin), float(upper[1] - y_margin)) + z_rows = np.linspace(lower[2] + z_margin, upper[2] - z_margin, 6) + waypoints = tuple( + (hand_x, y_position, float(z_position)) + for row, z_position in enumerate(z_rows) + for y_position in (y_limits if row % 2 == 0 else y_limits[::-1]) + ) + approach_offset = (-0.08, 0.0, 0.0) + retreat_offset = (-0.12, 0.0, 0.0) + author_trace_patches = False + approach_tolerance_m = 0.055 + waypoint_tolerance_m = 0.055 + required_coverage_fraction = 0.35 + max_waypoint_speed_m_s = 0.20 + else: + margin = np.minimum((upper - lower) * 0.18, np.asarray((0.07, 0.07, 0.0))) + x_values = np.linspace(lower[0] + margin[0], upper[0] - margin[0], 3) + y_values = (lower[1] + margin[1], upper[1] - margin[1]) + # The proven contact hand height is 0.139 m above a horizontal + # surface with the downward Lula orientation. + hand_z = float(upper[2] + 0.139) + waypoints = tuple( + (float(x), float(y), hand_z) + for row, y in enumerate(y_values) + for x in (x_values if row % 2 == 0 else x_values[::-1]) + ) + dwell_frames = max(6, int(round(max(duration_s, 0.1) * 60.0 / len(waypoints)))) + self._transition("approaching") + self._emit_progress( + phase="approaching", + progress=0.0, + path_m=tuple(tuple(map(float, waypoint)) for waypoint in waypoints), + target_m=tuple(map(float, waypoints[0])), + ) + report = self.execute_decontamination( + decontaminator, + waypoints, + dwell_frames=dwell_frames, + orientation_wxyz=orientation_wxyz, + approach_offset_m=approach_offset, + retreat_offset_m=retreat_offset, + author_trace_patches=author_trace_patches, + approach_tolerance_m=approach_tolerance_m, + waypoint_tolerance_m=waypoint_tolerance_m, + required_coverage_fraction=required_coverage_fraction, + max_waypoint_speed_m_s=max_waypoint_speed_m_s, + ) + self._transition("complete" if report.success else "failed") + return report + + @staticmethod + def _grasp_frame_name(target: Any) -> str: + attribute = target.GetAttribute("rad:manipulation:graspFrame") + return str(attribute.Get() or "") if attribute else "" + + def _attach_object(self, object_path: str) -> float: + """Latch a verified hand/object contact after visible finger closure.""" + + from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics + + target = self.stage.GetPrimAtPath(object_path) + if not target or not target.IsValid(): + raise RuntimeError(f"grasp target does not exist: {object_path}") + movable = target.GetAttribute("rad:manipulation:movable") + if not movable or not bool(movable.Get()): + raise RuntimeError(f"grasp target is not movable: {object_path}") + payload = target.GetAttribute("rad:manipulation:payloadKg") + if payload and float(payload.Get() or 0.0) > 3.0: + raise RuntimeError(f"grasp target exceeds the Franka payload: {payload.Get()} kg") + frame_name = self._grasp_frame_name(target) + if not frame_name: + raise RuntimeError(f"grasp target has no grasp frame: {object_path}") + grasp_path = f"{object_path.rstrip('/')}/{frame_name}" + grasp_prim = self.stage.GetPrimAtPath(grasp_path) + if not grasp_prim or not grasp_prim.IsValid(): + raise RuntimeError(f"grasp frame does not exist: {grasp_path}") + hand_position = self.end_effector_position() + grasp_position, _ = _world_pose(self.stage, grasp_path) + distance = float(np.linalg.norm(hand_position - grasp_position)) + if distance > 0.055: + raise RuntimeError(f"grasp frame is {distance:.3f} m from the gripper") + + hand = self.stage.GetPrimAtPath(self.config.panda_hand_path) + cache = UsdGeom.XformCache() + hand_world = cache.GetLocalToWorldTransform(hand) + target_world = cache.GetLocalToWorldTransform(target) + grasp_world = cache.GetLocalToWorldTransform(grasp_prim) + anchor_world = grasp_world.Transform(Gf.Vec3d()) + hand_anchor = hand_world.GetInverse().Transform(anchor_world) + target_anchor = target_world.GetInverse().Transform(anchor_world) + local_rotation = hand_world.ExtractRotationQuat().GetInverse() * ( + target_world.ExtractRotationQuat() + ) + imaginary = local_rotation.GetImaginary() + if self.stage.GetPrimAtPath(self._grasp_joint_path).IsValid(): + self.stage.RemovePrim(self._grasp_joint_path) + joint = UsdPhysics.FixedJoint.Define(self.stage, self._grasp_joint_path) + joint.CreateBody0Rel().SetTargets([Sdf.Path(self.config.panda_hand_path)]) + joint.CreateBody1Rel().SetTargets([Sdf.Path(object_path)]) + joint.CreateLocalPos0Attr().Set(Gf.Vec3f(*hand_anchor)) + joint.CreateLocalPos1Attr().Set(Gf.Vec3f(*target_anchor)) + joint.CreateLocalRot0Attr().Set( + Gf.Quatf(float(local_rotation.GetReal()), Gf.Vec3f(*map(float, imaginary))) + ) + joint.CreateLocalRot1Attr().Set(Gf.Quatf(1.0)) + + self._grasp_collision_state.clear() + for descendant in Usd.PrimRange(target): + if descendant.GetName() not in {"Handle", "ManipulatorHandle"}: + continue + collision = descendant.GetAttribute("physics:collisionEnabled") + if collision and collision.HasAuthoredValueOpinion(): + path = str(descendant.GetPath()) + self._grasp_collision_state[path] = bool(collision.Get()) + collision.Set(False) + self._grasped_object_path = object_path + self._step(None, 1) + return distance + + def _restore_grasp_collisions(self) -> None: + """Restore handle collisions after the open fingers have retracted.""" + + for path, enabled in self._grasp_collision_state.items(): + collision = self.stage.GetPrimAtPath(path).GetAttribute("physics:collisionEnabled") + if collision: + collision.Set(enabled) + self._grasp_collision_state.clear() + + def _release_object(self, *, restore_collisions: bool = True) -> None: + if self._grasped_object_path is None: + raise RuntimeError("no object is grasped") + # A fixed joint can retain the last interpolation velocity on the + # payload even after the hand has reached its final target. Clear + # that residual motion before detaching so a tall service shield is + # released from rest instead of receiving a repeatable tipping + # impulse. Gravity and contacts remain active after the joint is + # removed; this is not a pose teleport or a kinematic placement. + target = self.stage.GetPrimAtPath(self._grasped_object_path) + from pxr import Gf + + for attribute_name in ("physics:velocity", "physics:angularVelocity"): + attribute = target.GetAttribute(attribute_name) + if attribute: + attribute.Set(Gf.Vec3f(0.0)) + self.stage.RemovePrim(self._grasp_joint_path) + if restore_collisions: + self._restore_grasp_collisions() + self._grasped_object_path = None + self._step(None, 1) + + def execute_pick_and_place( + self, + object_path: str, + pickup_base_position_m: Sequence[float], + placement_base_position_m: Sequence[float], + pickup_tool_position_m: Sequence[float] | None = None, + placement_tool_position_m: Sequence[float] | None = None, + tool_orientation_xyzw: Sequence[float] | None = None, + before_release: Callable[[], None] | None = None, + after_release: Callable[[], None] | None = None, + pickup_base_yaw_rad: float | None = None, + placement_base_yaw_rad: float | None = None, + pickup_base_route_m: Sequence[Sequence[float]] | None = None, + placement_base_route_m: Sequence[Sequence[float]] | None = None, + target_root_position_m: Sequence[float] | None = None, + placement_settle_tolerance_m: float = 0.15, + ) -> ArticulatedTaskReport: + """Execute a complete base/arm/finger pick-and-place with pose audit.""" + + del pickup_tool_position_m, placement_tool_position_m, tool_orientation_xyzw + self.set_decon_tool_visible(False) + phases: list[str] = [] + total_steps = 0 + + stow = self.stow_arm() + total_steps += stow.steps + phases.extend(stow.phases) + if not stow.success: + return ArticulatedTaskReport( + "failed", + False, + total_steps, + stow.message, + object_path, + tuple(phases), + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + + pickup_route = ( + (pickup_base_position_m,) + if pickup_base_route_m is None or not tuple(pickup_base_route_m) + else tuple(pickup_base_route_m) + ) + pickup = self.navigate_route(pickup_route, pickup_base_yaw_rad) + total_steps += pickup.steps + phases.extend(pickup.phases) + if not pickup.success: + return ArticulatedTaskReport( + "failed", + False, + total_steps, + "pickup base motion failed", + object_path, + tuple(phases), + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + + target = self.stage.GetPrimAtPath(object_path) + frame_name = self._grasp_frame_name(target) + grasp_path = f"{object_path.rstrip('/')}/{frame_name}" + object_initial, _ = _world_pose(self.stage, object_path) + grasp_position, _ = _world_pose(self.stage, grasp_path) + grasp_offset = grasp_position - object_initial + open_aperture = self.set_gripper(0.035) + phases.append("open_gripper") + approach = grasp_position + np.asarray((0.0, 0.0, 0.14)) + for phase, target_position in (("approach", approach), ("grasp_pose", grasp_position)): + self._transition(phase) + motion = self.move_hand(target_position) + total_steps += motion.steps + phases.append(phase) + if not motion.success: + return ArticulatedTaskReport( + "failed", + False, + total_steps, + ( + f"{phase} IK motion failed: target={motion.target_position_m}, " + f"actual={motion.final_position_m}, " + f"error={motion.position_error_m:.4f} m" + ), + object_path, + tuple(phases), + finger_aperture_open_m=open_aperture, + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + # The authored service handle is 30 mm thick. Command a 32 mm total + # aperture so the physical fingers establish contact without trying to + # crush the handle into an 8 mm gap and launching the rigid payload. + closed_aperture = self.set_gripper(0.016) + phases.append("close_gripper") + self._transition("grasping") + grasp_distance = self._attach_object(object_path) + phases.append("attach_at_hand") + pickup_base = np.asarray(pickup_base_position_m, dtype=np.float64) + retract_xy = pickup_base[:2] - grasp_position[:2] + retract_norm = float(np.linalg.norm(retract_xy)) + if retract_norm <= 1.0e-9: + self._release_object() + return ArticulatedTaskReport( + "failed", + False, + total_steps, + "pickup base and grasp frame have no horizontal separation", + object_path, + tuple(phases), + grasp_distance, + finger_aperture_open_m=open_aperture, + finger_aperture_closed_m=closed_aperture, + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + retract_world = np.asarray( + ( + 0.14 * retract_xy[0] / retract_norm, + 0.14 * retract_xy[1] / retract_norm, + 0.22, + ) + ) + lift_targets = ( + ("lift_clearance", grasp_position + np.asarray((0.0, 0.0, 0.08))), + # Retract toward the actual pickup base while raising the load. + # The sign changes for opposite-side grasps; a fixed world-X + # offset would extend the loaded arm farther at yaw pi. + ("lift_transport", grasp_position + retract_world), + ) + for lift_phase, lift_target in lift_targets: + lift = self.move_hand(lift_target, tolerance_m=0.025) + total_steps += lift.steps + phases.append(lift_phase) + if not lift.success: + self._release_object() + return ArticulatedTaskReport( + "failed", + False, + total_steps, + f"loaded {lift_phase} IK motion failed", + object_path, + tuple(phases), + grasp_distance, + finger_aperture_open_m=open_aperture, + finger_aperture_closed_m=closed_aperture, + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + + placement_route = ( + (placement_base_position_m,) + if placement_base_route_m is None or not tuple(placement_base_route_m) + else tuple(placement_base_route_m) + ) + carried = self.navigate_route(placement_route, placement_base_yaw_rad) + total_steps += carried.steps + phases.extend(f"carry_{phase}" for phase in carried.phases) + if not carried.success: + self._release_object() + return ArticulatedTaskReport( + "failed", + False, + total_steps, + "loaded base route failed", + object_path, + tuple(phases), + grasp_distance, + finger_aperture_open_m=open_aperture, + finger_aperture_closed_m=closed_aperture, + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + if target_root_position_m is None: + destination_root = np.asarray(placement_base_position_m, dtype=np.float64).copy() + destination_root[:2] += ( + object_initial[:2] - np.asarray(pickup_base_position_m, dtype=np.float64)[:2] + ) + destination_root[2] = object_initial[2] + else: + destination_root = np.asarray(target_root_position_m, dtype=np.float64) + # move_hand() commands a fixed world-space tool orientation. Turning + # the mobile base only changes the side from which the arm approaches; + # the attached object returns to its original world orientation during + # the placement IK motion, so its grasp offset must not follow base yaw. + destination_grasp = destination_root + grasp_offset + preplace = destination_grasp + np.asarray((0.0, 0.0, 0.16)) + placement_targets = ( + (preplace, 0.025), + (destination_grasp + np.asarray((0.0, 0.0, 0.08)), 0.025), + # Finish 5 mm above the authored support plane. The former + # 40 mm release height made a correctly routed tall panel free- + # fall onto one edge and topple during the settle audit. + (destination_grasp + np.asarray((0.0, 0.0, 0.005)), 0.012), + ) + for index, (target_position, tolerance_m) in enumerate(placement_targets, start=1): + motion = self.move_hand(target_position, tolerance_m=tolerance_m) + total_steps += motion.steps + phases.append(f"place_{index}") + if not motion.success: + self._release_object() + return ArticulatedTaskReport( + "failed", + False, + total_steps, + ( + "loaded placement IK motion failed: " + f"target={motion.target_position_m}, " + f"actual={motion.final_position_m}, " + f"error={motion.position_error_m:.4f} m" + ), + object_path, + tuple(phases), + grasp_distance, + finger_aperture_open_m=open_aperture, + finger_aperture_closed_m=closed_aperture, + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + if before_release is not None: + before_release() + # Let the closed-loop arm and attached payload come fully to rest at + # the low release pose before opening the fingers. + self.hold(30) + phases.append("stabilize_before_release") + release_position, release_orientation = _world_pose(self.stage, object_path) + self.set_gripper(0.035) + self._transition("releasing") + # Keep the service-handle collision disabled for the few frames in + # which the open fingers retract. Re-enabling it while the gripper is + # still co-located with the handle can apply a separation impulse and + # topple an otherwise stable shield after an accurate placement. + self._release_object(restore_collisions=False) + phases.extend(("open_gripper", "release", "retract_after_release", "settle")) + self.move_hand(preplace) + self._restore_grasp_collisions() + self.hold(90) + if after_release is not None: + after_release() + final_position, final_orientation = _world_pose(self.stage, object_path) + placement_error = float(np.linalg.norm(final_position - destination_root)) + success = placement_error <= placement_settle_tolerance_m + state = "complete" if success else "failed" + self._transition(state) + return ArticulatedTaskReport( + state, + success, + total_steps, + "articulated pick-and-place completed" + if success + else ( + "object did not settle at the commanded pose: " + f"error={placement_error:.4f} m, " + f"tolerance={placement_settle_tolerance_m:.4f} m" + ), + object_path, + tuple(phases), + grasp_distance, + placement_error, + open_aperture, + closed_aperture, + self.arm_joint_excursion_rad, + tuple(map(float, destination_root)), + tuple(map(float, release_position)), + tuple(map(float, final_position)), + tuple(map(float, release_orientation)), + tuple(map(float, final_orientation)), + ) + + def remove_to_disposal_zone( + self, + object_path: str, + disposal_zone_path: str, + ) -> ArticulatedTaskReport: + """Secure a physically delivered object and deactivate its source.""" + + from pxr import Sdf, UsdGeom + + target = self.stage.GetPrimAtPath(object_path) + zone = self.stage.GetPrimAtPath(disposal_zone_path) + cache = UsdGeom.BBoxCache( + 0.0, + [UsdGeom.Tokens.default_, UsdGeom.Tokens.render], + useExtentsHint=True, + ) + zone_range = cache.ComputeWorldBound(zone).ComputeAlignedRange() + object_range = cache.ComputeWorldBound(target).ComputeAlignedRange() + zone_lower = np.asarray(zone_range.GetMin(), dtype=np.float64) + zone_upper = np.asarray(zone_range.GetMax(), dtype=np.float64) + object_lower = np.asarray(object_range.GetMin(), dtype=np.float64) + object_upper = np.asarray(object_range.GetMax(), dtype=np.float64) + horizontal_margin_m = 0.03 + contained_xy = bool( + np.all(object_lower[:2] >= zone_lower[:2] - horizontal_margin_m) + and np.all(object_upper[:2] <= zone_upper[:2] + horizontal_margin_m) + ) + supported = bool(-0.08 <= object_lower[2] - zone_upper[2] <= 0.18) + if not contained_xy or not supported: + return ArticulatedTaskReport( + "failed", + False, + 0, + ( + "object is not contained by the disposal zone: " + f"contained_xy={contained_xy}, supported={supported}" + ), + object_path, + ) + source_enabled = target.GetAttribute("rad:source:enabled") + if source_enabled: + source_enabled.Set(False) + disposed = target.GetAttribute("rad:disposal:disposed") + if not disposed: + disposed = target.CreateAttribute( + "rad:disposal:disposed", Sdf.ValueTypeNames.Bool, custom=True + ) + disposed.Set(True) + for name in ("rad:manipulation:movable", "rad:manipulation:removable"): + attribute = target.GetAttribute(name) + if attribute: + attribute.Set(False) + self._transition("secured_in_disposal") + return ArticulatedTaskReport( + "complete", + True, + 0, + "object secured and source disabled in disposal zone", + object_path, + ("secure_disposal",), + arm_joint_excursion_rad=self.arm_joint_excursion_rad, + ) + def _attach_shield(self) -> float: from pxr import Gf, Sdf, UsdGeom, UsdPhysics @@ -776,13 +2927,22 @@ def execute_shield_pick_and_place( pickup_base_xy_yaw: Sequence[float], placement_base_route_xy_yaw: Sequence[Sequence[float]], destination_root_position_m: Sequence[float], + transport_hand_offset_from_grasp_m: Sequence[float] | None = None, + placement_staging_offsets_m: Sequence[Sequence[float]] = (), ) -> ShieldMotionReport: self.set_decon_tool_visible(False) initial, _ = _world_pose(self.stage, self.config.shield_path) if not self.move_base(pickup_base_xy_yaw): return ShieldMotionReport( - False, "pickup_base", math.inf, 0.0, 0.0, 0.0, math.inf, - tuple(map(float, initial)), tuple(map(float, initial)), + False, + "pickup_base", + math.inf, + 0.0, + 0.0, + 0.0, + math.inf, + tuple(map(float, initial)), + tuple(map(float, initial)), (float(np.linalg.norm(self.last_base_positions - self.last_base_target)),), (), (), @@ -790,29 +2950,79 @@ def execute_shield_pick_and_place( open_aperture = self.set_gripper(0.035) grasp_position, _ = _world_pose(self.stage, self.config.shield_grasp_path) approach = grasp_position + np.asarray((0.0, 0.0, 0.16)) - if not self.move_hand(approach).success or not self.move_hand(grasp_position).success: + approach_motion = self.move_hand(approach) + grasp_motion = self.move_hand(grasp_position) if approach_motion.success else None + if not approach_motion.success or grasp_motion is None or not grasp_motion.success: return ShieldMotionReport( - False, "pickup_hand", math.inf, open_aperture, 0.0, 0.0, math.inf, - tuple(map(float, initial)), tuple(map(float, initial)), () - , (), () + False, + "pickup_hand", + math.inf, + open_aperture, + 0.0, + 0.0, + math.inf, + tuple(map(float, initial)), + tuple(map(float, initial)), + (), + (), + (), ) - closed_aperture = self.set_gripper(0.004) + # Match the 30 mm handle thickness before creating the verified fixed + # joint; an 8 mm commanded gap causes a large PhysX separation impulse. + closed_aperture = self.set_gripper(0.016) grasp_distance = self._attach_shield() lift_target = grasp_position + np.asarray((0.0, 0.0, 0.24)) if not self.move_hand(lift_target).success: self._release_shield() return ShieldMotionReport( - False, "lift", grasp_distance, open_aperture, closed_aperture, 0.0, math.inf, - tuple(map(float, initial)), tuple(map(float, initial)), () - , (), () + False, + "lift", + grasp_distance, + open_aperture, + closed_aperture, + 0.0, + math.inf, + tuple(map(float, initial)), + tuple(map(float, initial)), + (), + (), + (), + ) + if transport_hand_offset_from_grasp_m is not None: + transport_offset = np.asarray( + transport_hand_offset_from_grasp_m, + dtype=np.float64, ) + if transport_offset.shape != (3,): + raise ValueError("transport hand offset must contain x/y/z") + transport_target = grasp_position + transport_offset + transport = self.move_hand( + transport_target, + interpolation_steps=105, + tolerance_m=0.025, + ) + if not transport.success: + self._release_shield() + final, _ = _world_pose(self.stage, self.config.shield_path) + return ShieldMotionReport( + False, + "transport_posture", + grasp_distance, + open_aperture, + closed_aperture, + float(final[2] - initial[2]), + math.inf, + tuple(map(float, initial)), + tuple(map(float, final)), + (), + (transport.steps,), + (transport.position_error_m,), + ) lifted, _ = _world_pose(self.stage, self.config.shield_path) base_errors: list[float] = [] for waypoint in placement_base_route_xy_yaw: if not self.move_base(waypoint): - base_error = float( - np.linalg.norm(self.last_base_positions - self.last_base_target) - ) + base_error = float(np.linalg.norm(self.last_base_positions - self.last_base_target)) base_errors.append(base_error) self._release_shield() final, _ = _world_pose(self.stage, self.config.shield_path) @@ -840,14 +3050,34 @@ def execute_shield_pick_and_place( ) preplace = destination_grasp + np.asarray((0.0, 0.0, 0.12)) loaded_tolerance_m = 0.025 - placement_motions = [ - self.move_hand(preplace, tolerance_m=loaded_tolerance_m) - ] - if placement_motions[0].success: + placement_motions = [] + for offset_m in placement_staging_offsets_m: + offset = np.asarray(offset_m, dtype=np.float64) + if offset.shape != (3,): + raise ValueError("placement staging offsets must contain x/y/z") + placement_motions.append( + self.move_hand( + destination_grasp + offset, + interpolation_steps=105, + tolerance_m=loaded_tolerance_m, + ) + ) + if not placement_motions[-1].success: + break + if not placement_motions or placement_motions[-1].success: + placement_motions.append( + self.move_hand( + preplace, + interpolation_steps=105, + tolerance_m=loaded_tolerance_m, + ) + ) + if placement_motions[-1].success: for height_offset in (0.08, 0.04, 0.0): placement_motions.append( self.move_hand( destination_grasp + np.asarray((0.0, 0.0, height_offset)), + interpolation_steps=105, tolerance_m=loaded_tolerance_m, ) ) @@ -906,6 +3136,7 @@ def __init__( wheel_radius_m: float = 0.14, wheel_base_m: float = 0.413, articulation: Any | None = None, + progress_callback: NavigationProgressCallback | None = None, ) -> None: from isaacsim.robot.wheeled_robots.robots import WheeledRobot @@ -914,6 +3145,7 @@ def __init__( self.config = config or RealRobotAssetConfig() self.wheel_radius_m = wheel_radius_m self.wheel_base_m = wheel_base_m + self.progress_callback = progress_callback self.robot = articulation or WheeledRobot( prim_path=self.config.measurement_articulation, name="radcounter_nova_carter", @@ -925,6 +3157,9 @@ def __init__( missing = [name for name in self.wheel_names if name not in self.robot.dof_names] if missing: raise RuntimeError(f"Nova Carter wheel DOFs are missing: {missing}") + self.trace: list[str] = ["idle"] + self.home_position_m: tuple[float, float, float] | None = None + self._home_return_route_m: tuple[tuple[float, float, float], ...] = () @staticmethod def _yaw(orientation_wxyz: np.ndarray) -> float: @@ -949,16 +3184,25 @@ def set_initial_pose( self.stop() for _ in range(20): self.stepper.step(render=True) + home = np.asarray(position_m, dtype=np.float64) + self.home_position_m = tuple(map(float, home[:3])) + self._home_return_route_m = () + + def return_home(self) -> MeasurementMotionReport: + """Drive back to the explicitly recorded initial position.""" + + if self.home_position_m is None: + raise RuntimeError("measurement robot starting position is not recorded") + self.trace.append("returning_home") + if self._home_return_route_m: + return self.navigate_route(self._home_return_route_m) + return self.navigate_to(self.home_position_m) def _command(self, linear_m_s: float, angular_rad_s: float) -> None: from isaacsim.core.utils.types import ArticulationAction - left = ( - linear_m_s - angular_rad_s * self.wheel_base_m / 2.0 - ) / self.wheel_radius_m - right = ( - linear_m_s + angular_rad_s * self.wheel_base_m / 2.0 - ) / self.wheel_radius_m + left = (linear_m_s - angular_rad_s * self.wheel_base_m / 2.0) / self.wheel_radius_m + right = (linear_m_s + angular_rad_s * self.wheel_base_m / 2.0) / self.wheel_radius_m self.robot.apply_wheel_actions( ArticulationAction(joint_velocities=np.asarray((left, right), dtype=np.float64)) ) @@ -971,19 +3215,36 @@ def navigate_to( target_xy_m: Sequence[float], *, tolerance_m: float = 0.12, - maximum_steps: int = 1200, + maximum_steps: int | None = None, ) -> MeasurementMotionReport: target = np.asarray(target_xy_m, dtype=np.float64) initial, _ = _world_pose(self.stage, self.config.measurement_articulation) + if maximum_steps is None: + initial_distance_m = float(np.linalg.norm(target[:2] - initial[:2])) + # Budget for 0.25 m/s average progress plus turns and settling. + # The previous fixed 1,200-frame cap could only cover about 11 m + # and stopped Nova Carter midway through the connecting corridor. + maximum_steps = max( + 1200, + int(math.ceil(initial_distance_m / 0.25 * 60.0)) + 300, + ) + if maximum_steps <= 0: + raise ValueError("maximum_steps must be positive") + self.trace.append("navigating") success = False steps = 0 for step in range(1, maximum_steps + 1): steps = step - position, orientation = _world_pose( - self.stage, self.config.measurement_articulation - ) + position, orientation = _world_pose(self.stage, self.config.measurement_articulation) error = target[:2] - position[:2] distance = float(np.linalg.norm(error)) + if self.progress_callback is not None and (step == 1 or step % 15 == 0): + self.progress_callback( + step, + tuple(map(float, position)), + tuple(map(float, target[:2])), + distance, + ) if distance <= tolerance_m: success = True break @@ -1000,6 +3261,14 @@ def navigate_to( self.stepper.step(render=True) final, _ = _world_pose(self.stage, self.config.measurement_articulation) displacement = float(np.linalg.norm(final[:2] - initial[:2])) + if self.progress_callback is not None: + self.progress_callback( + steps, + tuple(map(float, final)), + tuple(map(float, target[:2])), + float(np.linalg.norm(target[:2] - final[:2])), + ) + self.trace.append("complete" if success else "failed") return MeasurementMotionReport( success, steps, @@ -1008,9 +3277,62 @@ def navigate_to( tuple(map(float, final)), ) + def navigate_route( + self, + waypoints_m: Sequence[Sequence[float]], + ) -> MeasurementMotionReport: + """Drive a collision-planned polyline and report the aggregate motion.""" + + waypoints = tuple(waypoints_m) + initial, _ = _world_pose(self.stage, self.config.measurement_articulation) + if not waypoints: + return MeasurementMotionReport( + False, + 0, + 0.0, + tuple(map(float, initial)), + tuple(map(float, initial)), + ) + if ( + self.home_position_m is not None + and np.linalg.norm(initial[:2] - np.asarray(self.home_position_m[:2], dtype=np.float64)) + <= 0.20 + ): + outbound_nodes = (initial, *(np.asarray(item, dtype=np.float64) for item in waypoints)) + self._home_return_route_m = tuple( + tuple(map(float, node[:3])) for node in reversed(outbound_nodes[:-1]) + ) + total_steps = 0 + for waypoint in waypoints: + report = self.navigate_to(waypoint) + total_steps += report.steps + if not report.success: + final, _ = _world_pose(self.stage, self.config.measurement_articulation) + return MeasurementMotionReport( + False, + total_steps, + float(np.linalg.norm(final[:2] - initial[:2])), + tuple(map(float, initial)), + tuple(map(float, final)), + ) + final, _ = _world_pose(self.stage, self.config.measurement_articulation) + return MeasurementMotionReport( + True, + total_steps, + float(np.linalg.norm(final[:2] - initial[:2])), + tuple(map(float, initial)), + tuple(map(float, final)), + ) + __all__ = [ + "ArticulatedTaskReport", "DecontaminationMotionReport", + "FacilityCorridorSpec", + "FacilityLayoutManifest", + "FacilityPrimitiveSpec", + "FacilityRoomSpec", + "FacilityRouteSpec", "HandMotionResult", "MeasurementMotionReport", "NovaCarterController", @@ -1019,5 +3341,8 @@ def navigate_to( "ShieldMotionReport", "add_real_robot_references", "author_real_robot_task_scene", + "create_decontamination_activity_map", + "decommissioning_facility_layout", "enable_real_robot_extensions", + "facility_route_clearance_m", ] diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/robot/reference_models.py b/source/extensions/radcounter.isaac/radcounter/isaac/robot/reference_models.py index c1f1061..c8ba5c6 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/robot/reference_models.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/robot/reference_models.py @@ -30,7 +30,14 @@ def _color(prim: Any, rgb: tuple[float, float, float]) -> None: UsdGeom.Gprim(prim).CreateDisplayColorAttr([Gf.Vec3f(*rgb)]) -def _cube(stage: Any, path: str, size: tuple[float, float, float], xyz=(0.0, 0.0, 0.0), rgb=(0.3, 0.3, 0.3), rpy=(0.0, 0.0, 0.0)) -> Any: +def _cube( + stage: Any, + path: str, + size: tuple[float, float, float], + xyz=(0.0, 0.0, 0.0), + rgb=(0.3, 0.3, 0.3), + rpy=(0.0, 0.0, 0.0), +) -> Any: from pxr import Gf, UsdGeom cube = UsdGeom.Cube.Define(stage, path) @@ -44,7 +51,16 @@ def _cube(stage: Any, path: str, size: tuple[float, float, float], xyz=(0.0, 0.0 return cube.GetPrim() -def _cylinder(stage: Any, path: str, radius: float, height: float, xyz=(0.0, 0.0, 0.0), rgb=(0.2, 0.2, 0.2), axis="Z", rpy=(0.0, 0.0, 0.0)) -> Any: +def _cylinder( + stage: Any, + path: str, + radius: float, + height: float, + xyz=(0.0, 0.0, 0.0), + rgb=(0.2, 0.2, 0.2), + axis="Z", + rpy=(0.0, 0.0, 0.0), +) -> Any: from pxr import Gf, UsdGeom cylinder = UsdGeom.Cylinder.Define(stage, path) @@ -70,17 +86,63 @@ def _sphere(stage: Any, path: str, radius: float, xyz=(0.0, 0.0, 0.0), rgb=(0.2, def _camera(stage: Any, path: str, xyz: tuple[float, float, float], scale=1.0) -> None: - _cube(stage, path + "/Housing", (0.08 * scale, 0.10 * scale, 0.07 * scale), xyz, (0.08, 0.09, 0.10)) - _cylinder(stage, path + "/Lens", 0.027 * scale, 0.018 * scale, (xyz[0] + 0.047 * scale, xyz[1], xyz[2]), (0.03, 0.10, 0.16), axis="X") - _cylinder(stage, path + "/LampL", 0.017 * scale, 0.012 * scale, (xyz[0] + 0.047 * scale, xyz[1] + 0.037 * scale, xyz[2]), (0.95, 0.90, 0.65), axis="X") - _cylinder(stage, path + "/LampR", 0.017 * scale, 0.012 * scale, (xyz[0] + 0.047 * scale, xyz[1] - 0.037 * scale, xyz[2]), (0.95, 0.90, 0.65), axis="X") + _cube( + stage, + path + "/Housing", + (0.08 * scale, 0.10 * scale, 0.07 * scale), + xyz, + (0.08, 0.09, 0.10), + ) + _cylinder( + stage, + path + "/Lens", + 0.027 * scale, + 0.018 * scale, + (xyz[0] + 0.047 * scale, xyz[1], xyz[2]), + (0.03, 0.10, 0.16), + axis="X", + ) + _cylinder( + stage, + path + "/LampL", + 0.017 * scale, + 0.012 * scale, + (xyz[0] + 0.047 * scale, xyz[1] + 0.037 * scale, xyz[2]), + (0.95, 0.90, 0.65), + axis="X", + ) + _cylinder( + stage, + path + "/LampR", + 0.017 * scale, + 0.012 * scale, + (xyz[0] + 0.047 * scale, xyz[1] - 0.037 * scale, xyz[2]), + (0.95, 0.90, 0.65), + axis="X", + ) -def _track(stage: Any, path: str, xyz: tuple[float, float, float], length: float, width: float, height: float, wheel_count: int = 4) -> None: +def _track( + stage: Any, + path: str, + xyz: tuple[float, float, float], + length: float, + width: float, + height: float, + wheel_count: int = 4, +) -> None: _cube(stage, path + "/Belt", (length, width, height), xyz, (0.035, 0.040, 0.042)) for index in range(wheel_count): x = xyz[0] - 0.36 * length + index * (0.72 * length / max(1, wheel_count - 1)) - _cylinder(stage, f"{path}/RoadWheel{index}", height * 0.32, width * 1.04, (x, xyz[1], xyz[2]), (0.20, 0.22, 0.20), axis="Y") + _cylinder( + stage, + f"{path}/RoadWheel{index}", + height * 0.32, + width * 1.04, + (x, xyz[1], xyz[2]), + (0.20, 0.22, 0.20), + axis="Y", + ) def _root(stage: Any, prim_path: str, model_id: str) -> tuple[Any, Any, str]: @@ -121,15 +183,36 @@ def _packbot(stage: Any, prim_path: str) -> SpawnedReferenceRobot: _track(stage, base + "/LeftTrack", (0.0, 0.205, 0.095), 0.52, 0.12, 0.17) _track(stage, base + "/RightTrack", (0.0, -0.205, 0.095), 0.52, 0.12, 0.17) _cube(stage, base + "/Chassis", (0.43, 0.30, 0.12), (-0.01, 0.0, 0.16), deck) - _cube(stage, base + "/ElectronicsDeck", (0.28, 0.25, 0.09), (-0.07, 0.0, 0.255), (0.24, 0.25, 0.23)) + _cube( + stage, + base + "/ElectronicsDeck", + (0.28, 0.25, 0.09), + (-0.07, 0.0, 0.255), + (0.24, 0.25, 0.23), + ) for side, y in (("L", 0.205), ("R", -0.205)): - _cube(stage, base + f"/FrontFlipper{side}", (0.23, 0.09, 0.075), (0.30, y, 0.115), black, (0.0, -14.0, 0.0)) + _cube( + stage, + base + f"/FrontFlipper{side}", + (0.23, 0.09, 0.075), + (0.30, y, 0.115), + black, + (0.0, -14.0, 0.0), + ) _cylinder(stage, base + f"/FlipperHub{side}", 0.060, 0.125, (0.22, y, 0.11), deck, axis="Y") _cylinder(stage, base + "/MastPan", 0.045, 0.10, (0.02, 0.0, 0.35), black) _cube(stage, base + "/Mast", (0.045, 0.045, 0.25), (0.02, 0.0, 0.47), deck) - _cube(stage, base + "/ManipulatorBoom", (0.28, 0.055, 0.055), (0.145, 0.0, 0.52), deck, (0.0, -22.0, 0.0)) + _cube( + stage, + base + "/ManipulatorBoom", + (0.28, 0.055, 0.055), + (0.145, 0.0, 0.52), + deck, + (0.0, -22.0, 0.0), + ) camera_link = base + "/camera_link" from pxr import UsdGeom + UsdGeom.Xform.Define(stage, camera_link) _camera(stage, camera_link, (0.30, 0.0, 0.61), 0.9) lidar_link = base + "/lidar_link" @@ -137,9 +220,22 @@ def _packbot(stage: Any, prim_path: str) -> SpawnedReferenceRobot: _cylinder(stage, lidar_link + "/Lidar", 0.045, 0.075, (0.02, 0.0, 0.68), (0.08, 0.12, 0.14)) radiation_link = base + "/radiation_link" UsdGeom.Xform.Define(stage, radiation_link) - _cube(stage, radiation_link + "/GammaProbe", (0.16, 0.075, 0.10), (0.03, 0.16, 0.41), (0.93, 0.64, 0.08)) + _cube( + stage, + radiation_link + "/GammaProbe", + (0.16, 0.075, 0.10), + (0.03, 0.16, 0.41), + (0.93, 0.64, 0.08), + ) _cylinder(stage, base + "/Antenna", 0.007, 0.36, (-0.17, -0.09, 0.48), black) - return SpawnedReferenceRobot(model_id, prim_path, base, translate, yaw, {"camera": camera_link, "lidar": lidar_link, "radiation": radiation_link}) + return SpawnedReferenceRobot( + model_id, + prim_path, + base, + translate, + yaw, + {"camera": camera_link, "lidar": lidar_link, "radiation": radiation_link}, + ) def _cage_curves(stage: Any, path: str, radius: float) -> None: @@ -174,8 +270,17 @@ def _elios3(stage: Any, prim_path: str) -> SpawnedReferenceRobot: _cage_curves(stage, base + "/CollisionTolerantCage", 0.235) _cube(stage, base + "/AvionicsBody", (0.25, 0.11, 0.085), (0.0, 0.0, 0.0), (0.10, 0.11, 0.12)) _cube(stage, base + "/Battery", (0.14, 0.12, 0.10), (-0.10, 0.0, 0.035), (0.20, 0.21, 0.22)) - for index, (x, y) in enumerate(((0.105, 0.105), (0.105, -0.105), (-0.105, 0.105), (-0.105, -0.105))): - _cube(stage, base + f"/RotorArm{index}", (0.18, 0.025, 0.025), (0.5 * x, 0.5 * y, 0.0), (0.16, 0.17, 0.18), (0.0, 0.0, math.degrees(math.atan2(y, x)))) + for index, (x, y) in enumerate( + ((0.105, 0.105), (0.105, -0.105), (-0.105, 0.105), (-0.105, -0.105)) + ): + _cube( + stage, + base + f"/RotorArm{index}", + (0.18, 0.025, 0.025), + (0.5 * x, 0.5 * y, 0.0), + (0.16, 0.17, 0.18), + (0.0, 0.0, math.degrees(math.atan2(y, x))), + ) _cylinder(stage, base + f"/Rotor{index}", 0.078, 0.012, (x, y, 0.0), (0.04, 0.045, 0.05)) _cylinder(stage, base + f"/Motor{index}", 0.025, 0.035, (x, y, 0.0), (0.48, 0.50, 0.49)) camera_link = base + "/camera_link" @@ -183,36 +288,112 @@ def _elios3(stage: Any, prim_path: str) -> SpawnedReferenceRobot: _camera(stage, camera_link, (0.155, 0.0, -0.015), 0.72) lidar_link = base + "/lidar_link" UsdGeom.Xform.Define(stage, lidar_link) - _cylinder(stage, lidar_link + "/OusterOS0", 0.043, 0.065, (-0.015, 0.0, -0.095), (0.10, 0.12, 0.13)) + _cylinder( + stage, lidar_link + "/OusterOS0", 0.043, 0.065, (-0.015, 0.0, -0.095), (0.10, 0.12, 0.13) + ) radiation_link = base + "/radiation_link" UsdGeom.Xform.Define(stage, radiation_link) - _cube(stage, radiation_link + "/MirionRDS32", (0.14, 0.062, 0.052), (-0.105, 0.0, -0.115), (0.95, 0.82, 0.10)) - _cube(stage, radiation_link + "/RDS32Display", (0.045, 0.064, 0.030), (-0.095, 0.0, -0.148), (0.08, 0.24, 0.20)) - return SpawnedReferenceRobot(model_id, prim_path, base, translate, yaw, {"camera": camera_link, "lidar": lidar_link, "radiation": radiation_link}) + _cube( + stage, + radiation_link + "/MirionRDS32", + (0.14, 0.062, 0.052), + (-0.105, 0.0, -0.115), + (0.95, 0.82, 0.10), + ) + _cube( + stage, + radiation_link + "/RDS32Display", + (0.045, 0.064, 0.030), + (-0.095, 0.0, -0.148), + (0.08, 0.24, 0.20), + ) + return SpawnedReferenceRobot( + model_id, + prim_path, + base, + translate, + yaw, + {"camera": camera_link, "lidar": lidar_link, "radiation": radiation_link}, + ) def _arm_visual(stage: Any, base: str, name: str, side: float, abrasive: bool) -> str: yellow = (0.88, 0.62, 0.06) - joints = ((0.02, side * 0.22, 0.74), (0.12, side * 0.28, 0.92), (0.32, side * 0.30, 1.04), (0.49, side * 0.27, 0.91)) + joints = ( + (0.02, side * 0.22, 0.74), + (0.12, side * 0.28, 0.92), + (0.32, side * 0.30, 1.04), + (0.49, side * 0.27, 0.91), + ) for index, point in enumerate(joints): - _sphere(stage, f"{base}/{name}/Joint{index + 1}", 0.055 if index < 2 else 0.043, point, (0.16, 0.17, 0.16)) + _sphere( + stage, + f"{base}/{name}/Joint{index + 1}", + 0.055 if index < 2 else 0.043, + point, + (0.16, 0.17, 0.16), + ) for index, (start, end) in enumerate(zip(joints, joints[1:], strict=True)): midpoint = tuple(0.5 * (a + b) for a, b in zip(start, end, strict=True)) length = math.dist(start, end) yaw = math.degrees(math.atan2(end[1] - start[1], end[0] - start[0])) - pitch = -math.degrees(math.atan2(end[2] - start[2], math.hypot(end[0] - start[0], end[1] - start[1]))) - _cube(stage, f"{base}/{name}/Link{index + 1}", (length, 0.065, 0.065), midpoint, yellow, (0.0, pitch, yaw)) + pitch = -math.degrees( + math.atan2(end[2] - start[2], math.hypot(end[0] - start[0], end[1] - start[1])) + ) + _cube( + stage, + f"{base}/{name}/Link{index + 1}", + (length, 0.065, 0.065), + midpoint, + yellow, + (0.0, pitch, yaw), + ) tool_path = base + ("/left_tool0" if side > 0 else "/right_tool0") from pxr import UsdGeom + UsdGeom.Xform.Define(stage, tool_path) end = joints[-1] if abrasive: - _cylinder(stage, tool_path + "/BlastNozzle", 0.032, 0.22, (end[0] + 0.10, end[1], end[2] - 0.02), (0.20, 0.22, 0.23), axis="X") - _cylinder(stage, tool_path + "/RecoveryShroud", 0.075, 0.05, (end[0] + 0.21, end[1], end[2] - 0.02), (0.08, 0.09, 0.09), axis="X") + _cylinder( + stage, + tool_path + "/BlastNozzle", + 0.032, + 0.22, + (end[0] + 0.10, end[1], end[2] - 0.02), + (0.20, 0.22, 0.23), + axis="X", + ) + _cylinder( + stage, + tool_path + "/RecoveryShroud", + 0.075, + 0.05, + (end[0] + 0.21, end[1], end[2] - 0.02), + (0.08, 0.09, 0.09), + axis="X", + ) else: - _cube(stage, tool_path + "/GripperPalm", (0.11, 0.10, 0.055), (end[0] + 0.07, end[1], end[2]), (0.18, 0.19, 0.19)) - _cube(stage, tool_path + "/FingerA", (0.12, 0.025, 0.025), (end[0] + 0.15, end[1] + 0.045, end[2]), (0.10, 0.11, 0.11)) - _cube(stage, tool_path + "/FingerB", (0.12, 0.025, 0.025), (end[0] + 0.15, end[1] - 0.045, end[2]), (0.10, 0.11, 0.11)) + _cube( + stage, + tool_path + "/GripperPalm", + (0.11, 0.10, 0.055), + (end[0] + 0.07, end[1], end[2]), + (0.18, 0.19, 0.19), + ) + _cube( + stage, + tool_path + "/FingerA", + (0.12, 0.025, 0.025), + (end[0] + 0.15, end[1] + 0.045, end[2]), + (0.10, 0.11, 0.11), + ) + _cube( + stage, + tool_path + "/FingerB", + (0.12, 0.025, 0.025), + (end[0] + 0.15, end[1] - 0.045, end[2]), + (0.10, 0.11, 0.11), + ) return tool_path @@ -223,18 +404,36 @@ def _meister(stage: Any, prim_path: str) -> SpawnedReferenceRobot: for longitudinal, x in (("Front", 0.34), ("Rear", -0.34)): for side, y in (("Left", 0.285), ("Right", -0.285)): _track(stage, f"{base}/{longitudinal}{side}Crawler", (x, y, 0.15), 0.48, 0.13, 0.22, 3) - _cylinder(stage, f"{base}/{longitudinal}{side}Pivot", 0.07, 0.15, (x, y, 0.23), yellow, axis="Y") + _cylinder( + stage, + f"{base}/{longitudinal}{side}Pivot", + 0.07, + 0.15, + (x, y, 0.23), + yellow, + axis="Y", + ) _cube(stage, base + "/LowerChassis", (0.72, 0.46, 0.20), (0.0, 0.0, 0.28), yellow) _cube(stage, base + "/TiltingUpperBody", (0.50, 0.45, 0.38), (-0.05, 0.0, 0.53), yellow) _cube(stage, base + "/ElectronicsCabinet", (0.30, 0.34, 0.36), (-0.24, 0.0, 0.77), yellow) left_tool = _arm_visual(stage, base, "LeftArm7Axis", 1.0, abrasive=False) right_tool = _arm_visual(stage, base, "RightArm7Axis", -1.0, abrasive=True) from pxr import UsdGeom + camera_link = base + "/head_camera_link" UsdGeom.Xform.Define(stage, camera_link) _camera(stage, camera_link, (0.12, 0.0, 1.16), 1.0) - _cylinder(stage, base + "/CableReel", 0.15, 0.10, (-0.47, 0.0, 0.58), (0.08, 0.09, 0.09), axis="Y") - return SpawnedReferenceRobot(model_id, prim_path, base, translate, yaw, {"camera": camera_link, "left_tool": left_tool, "right_tool": right_tool}) + _cylinder( + stage, base + "/CableReel", 0.15, 0.10, (-0.47, 0.0, 0.58), (0.08, 0.09, 0.09), axis="Y" + ) + return SpawnedReferenceRobot( + model_id, + prim_path, + base, + translate, + yaw, + {"camera": camera_link, "left_tool": left_tool, "right_tool": right_tool}, + ) def _arounder(stage: Any, prim_path: str) -> SpawnedReferenceRobot: @@ -244,21 +443,52 @@ def _arounder(stage: Any, prim_path: str) -> SpawnedReferenceRobot: _track(stage, base + "/LeftCrawler", (0.0, 0.235, 0.18), 1.05, 0.13, 0.25, 5) _track(stage, base + "/RightCrawler", (0.0, -0.235, 0.18), 1.05, 0.13, 0.25, 5) _cube(stage, base + "/PowerPack", (0.56, 0.43, 0.55), (-0.20, 0.0, 0.51), yellow) - for index, point in enumerate(((0.05, 0.0, 0.72), (0.27, 0.0, 0.87), (0.49, 0.0, 0.61), (0.63, 0.0, 0.32))): + for index, point in enumerate( + ((0.05, 0.0, 0.72), (0.27, 0.0, 0.87), (0.49, 0.0, 0.61), (0.63, 0.0, 0.32)) + ): _sphere(stage, base + f"/TreatmentArm/Joint{index + 1}", 0.055, point, (0.13, 0.14, 0.14)) - _cube(stage, base + "/TreatmentArm/Upper", (0.34, 0.07, 0.07), (0.17, 0.0, 0.80), yellow, (0.0, -33.0, 0.0)) - _cube(stage, base + "/TreatmentArm/Fore", (0.36, 0.07, 0.07), (0.38, 0.0, 0.74), yellow, (0.0, 43.0, 0.0)) + _cube( + stage, + base + "/TreatmentArm/Upper", + (0.34, 0.07, 0.07), + (0.17, 0.0, 0.80), + yellow, + (0.0, -33.0, 0.0), + ) + _cube( + stage, + base + "/TreatmentArm/Fore", + (0.36, 0.07, 0.07), + (0.38, 0.0, 0.74), + yellow, + (0.0, 43.0, 0.0), + ) head = base + "/treatment_head" from pxr import UsdGeom + UsdGeom.Xform.Define(stage, head) _cube(stage, head + "/RecoveryHead", (0.34, 0.52, 0.13), (0.66, 0.0, 0.10), (0.22, 0.24, 0.24)) - _cube(stage, head + "/BrushSkirt", (0.35, 0.53, 0.035), (0.66, 0.0, 0.025), (0.025, 0.030, 0.030)) - _cylinder(stage, head + "/ReciprocatingNozzle", 0.015, 0.42, (0.66, 0.0, 0.13), (0.12, 0.42, 0.68), axis="Y") - _cylinder(stage, head + "/SuctionPort", 0.04, 0.08, (0.59, -0.18, 0.17), (0.08, 0.09, 0.09), axis="X") + _cube( + stage, head + "/BrushSkirt", (0.35, 0.53, 0.035), (0.66, 0.0, 0.025), (0.025, 0.030, 0.030) + ) + _cylinder( + stage, + head + "/ReciprocatingNozzle", + 0.015, + 0.42, + (0.66, 0.0, 0.13), + (0.12, 0.42, 0.68), + axis="Y", + ) + _cylinder( + stage, head + "/SuctionPort", 0.04, 0.08, (0.59, -0.18, 0.17), (0.08, 0.09, 0.09), axis="X" + ) camera_link = base + "/camera_link" UsdGeom.Xform.Define(stage, camera_link) _camera(stage, camera_link, (0.24, 0.0, 0.94), 0.9) - return SpawnedReferenceRobot(model_id, prim_path, base, translate, yaw, {"camera": camera_link, "treatment_head": head}) + return SpawnedReferenceRobot( + model_id, prim_path, base, translate, yaw, {"camera": camera_link, "treatment_head": head} + ) _BUILDERS = { diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/runtime/simulation.py b/source/extensions/radcounter.isaac/radcounter/isaac/runtime/simulation.py index 144858c..865a5d2 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/runtime/simulation.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/runtime/simulation.py @@ -145,6 +145,22 @@ class _CachedRayPaths: path_lengths: FloatArray +def _ensure_material_columns(paths: FloatArray, material_count: int) -> FloatArray: + """Pad native path lengths for configured materials absent from the current stage.""" + + values = np.asarray(paths, dtype=np.float64) + if values.ndim != 2: + raise ValueError("native path lengths must be a two-dimensional array") + if values.shape[1] > material_count: + raise ValueError( + f"native scene returned {values.shape[1]} material columns for " + f"{material_count} configured materials" + ) + if values.shape[1] == material_count: + return values + return np.pad(values, ((0, 0), (0, material_count - values.shape[1]))) + + class NativeStageTransport: """Mirror attenuation geometry from USD into the native Embree scene.""" @@ -404,6 +420,7 @@ def path_lengths(self, origins: FloatArray, targets: FloatArray) -> FloatArray: paths = self._trace_paths(origins, targets) if paths.ndim == 1: paths = paths.reshape(len(origins), -1) + paths = _ensure_material_columns(paths, len(self._material_ids)) self._ray_cache[key] = _CachedRayPaths(origins.copy(), targets.copy(), paths) while len(self._ray_cache) > 4096: self._ray_cache.pop(next(iter(self._ray_cache))) diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/system_profile.py b/source/extensions/radcounter.isaac/radcounter/isaac/system_profile.py new file mode 100644 index 0000000..5765db1 --- /dev/null +++ b/source/extensions/radcounter.isaac/radcounter/isaac/system_profile.py @@ -0,0 +1,337 @@ +"""Compose a selected environment, robot set, and detector set in Isaac Sim.""" + +from __future__ import annotations + +import hashlib +import json +from dataclasses import dataclass +from pathlib import Path +from typing import Any +from urllib.parse import urlparse + +from radcounter.core.environment import EnvironmentImportPipeline +from radcounter.core.system_profiles import ResolvedSystemSelection + +from .robot import ( + RealRobotAssetConfig, + RobotFleetManager, + add_real_robot_references, + enable_real_robot_extensions, + spawn_reference_robot, +) +from .usd import EnvironmentUsdWriter + + +@dataclass(frozen=True) +class ComposedSystem: + stage_path: Path + environment_manifest_path: Path + runtime_config_path: Path + robot_paths: dict[str, str] + detector_paths: dict[str, str] + disabled_detector_paths: tuple[str, ...] + rebased_asset_paths: dict[str, str] + profile_manifest: dict[str, object] + fleet_manager: RobotFleetManager | None = None + + +@dataclass(frozen=True) +class _RobotHandle: + root_path: str + sensor_links: dict[str, str] + + +def prepare_environment_stage(selection: ResolvedSystemSelection) -> tuple[Path, Path]: + if not selection.environment_ready: + raise FileNotFoundError( + f"selected environment is not ready: {selection.environment_source_path}; " + f"{selection.environment_entry.setup_hint or 'provide the configured source file'}" + ) + result = EnvironmentImportPipeline().import_environment( + selection.environment_config, + base_directory=selection.environment_descriptor_path.parent, + ) + stage_path = EnvironmentUsdWriter().write( + result.scene, + result.output_directory / "environment.usda", + ) + return stage_path, result.manifest_path + + +def compose_selected_system( + stage: Any, + selection: ResolvedSystemSelection, + *, + stage_path: Path, +) -> ComposedSystem: + disabled_detector_paths = _disable_existing_detectors(stage) + handles, fleet_manager = _compose_robots(stage, selection) + detector_paths = _compose_detectors(stage, selection, handles) + rebased_assets = _rebase_referenced_asset_paths(stage) + runtime_path = _write_runtime_config(selection, stage_path) + stage.GetRootLayer().Save() + manifest = { + **selection.as_dict(), + "stage": str(stage_path), + "runtime_config": str(runtime_path), + "robot_paths": {key: value.root_path for key, value in handles.items()}, + "detector_paths": detector_paths, + "disabled_detector_paths": list(disabled_detector_paths), + "rebased_asset_paths": rebased_assets, + } + return ComposedSystem( + stage_path=stage_path, + environment_manifest_path=stage_path.parent / "manifest.json", + runtime_config_path=runtime_path, + robot_paths={key: value.root_path for key, value in handles.items()}, + detector_paths=detector_paths, + disabled_detector_paths=disabled_detector_paths, + rebased_asset_paths=rebased_assets, + profile_manifest=manifest, + fleet_manager=fleet_manager, + ) + + +def _disable_existing_detectors(stage: Any) -> tuple[str, ...]: + """Deactivate authored detectors so a selected detector set is replacement, not additive.""" + + from pxr import Sdf + + disabled = [] + for prim in stage.Traverse(): + role = prim.GetAttribute("rad:role") + detector_id = prim.GetAttribute("rad:detector:id") + if not role or not detector_id or not detector_id.Get(): + continue + if str(role.Get()) not in {"detector", "detector_station"}: + continue + role.Set("inactive_detector") + prim.CreateAttribute( + "rad:detector:disabledByProfile", Sdf.ValueTypeNames.Bool, custom=True + ).Set(True) + disabled.append(str(prim.GetPath())) + return tuple(disabled) + + +def _rebase_referenced_asset_paths(stage: Any) -> dict[str, str]: + """Make relative RadCounter sidecars stable in a generated composition layer.""" + + rebased: dict[str, str] = {} + root_layer_path = Path(stage.GetRootLayer().realPath).resolve() + for prim in stage.Traverse(): + for attribute in prim.GetAttributes(): + name = attribute.GetName() + if not name.startswith("rad:") or not name.lower().endswith("uri"): + continue + value = attribute.Get() + if not isinstance(value, str) or not value: + continue + parsed = urlparse(value) + candidate = Path(parsed.path if parsed.scheme == "file" else value).expanduser() + if parsed.scheme in {"http", "https"} or candidate.is_absolute(): + continue + resolved = None + for specification in attribute.GetPropertyStack(): + layer_path = Path(specification.layer.realPath or "") + if not layer_path.is_file() or layer_path.resolve() == root_layer_path: + continue + layer_candidate = (layer_path.parent / candidate).resolve() + if layer_candidate.is_file(): + resolved = layer_candidate + break + if resolved is None: + continue + attribute.Set(str(resolved)) + rebased[f"{prim.GetPath()}.{name}"] = str(resolved) + return rebased + + +def _compose_robots( + stage: Any, + selection: ResolvedSystemSelection, +) -> tuple[dict[str, _RobotHandle], RobotFleetManager | None]: + kind = selection.robot_set.kind + handles: dict[str, _RobotHandle] = {} + if kind == "none": + return handles, None + if kind == "reference": + from pxr import Gf, Sdf + + for placement in selection.robot_set.reference_robots: + spawned = spawn_reference_robot( + stage, + placement.reference_model_id, + placement.prim_path, + ) + anchor = ( + None + if placement.spawn_anchor is None + else selection.spawn_anchor(placement.spawn_anchor) + ) + translation = placement.translation_m if anchor is None else anchor.translation_m + yaw_deg = placement.yaw_deg if anchor is None else anchor.yaw_deg + spawned.translation_op.Set(Gf.Vec3d(*translation)) + spawned.yaw_op.Set(float(yaw_deg)) + if anchor is not None: + prim = stage.GetPrimAtPath(spawned.prim_path) + prim.CreateAttribute( + "rad:spawn:anchorId", Sdf.ValueTypeNames.String, custom=True + ).Set(placement.spawn_anchor) + prim.CreateAttribute( + "rad:spawn:environmentId", Sdf.ValueTypeNames.String, custom=True + ).Set(selection.environment_id) + handles[placement.robot_id] = _RobotHandle( + spawned.prim_path, + dict(spawned.sensor_links), + ) + return handles, None + if kind == "fleet": + if selection.robot_fleet is None: + raise RuntimeError("resolved fleet robot set has no fleet configuration") + manager = RobotFleetManager(stage, selection.robot_fleet) + imported = manager.import_all() + for robot in selection.robot_fleet.robots: + root = robot.prim_path or imported[robot.id] + handles[robot.id] = _RobotHandle(root, {}) + return handles, manager + if kind == "decommissioning": + from pxr import Gf, Sdf, UsdGeom + + enable_real_robot_extensions() + config = RealRobotAssetConfig() + add_real_robot_references(stage, config=config) + for path, anchor_id in ( + (config.countermeasure_root, "ground-primary"), + (config.measurement_root, "ground-secondary"), + ): + anchor = selection.spawn_anchor(anchor_id) + prim = stage.GetPrimAtPath(path) + xformable = UsdGeom.Xformable(prim) + xformable.ClearXformOpOrder() + xformable.AddTranslateOp().Set(Gf.Vec3d(*anchor.translation_m)) + xformable.AddRotateZOp().Set(float(anchor.yaw_deg)) + prim.CreateAttribute("rad:spawn:anchorId", Sdf.ValueTypeNames.String, custom=True).Set( + anchor_id + ) + prim.CreateAttribute( + "rad:spawn:environmentId", Sdf.ValueTypeNames.String, custom=True + ).Set(selection.environment_id) + handles["countermeasure"] = _RobotHandle(config.countermeasure_root, {}) + handles["measurement"] = _RobotHandle( + config.measurement_articulation, + {"radiation": config.detector_path}, + ) + return handles, None + raise AssertionError(kind) + + +def _find_robot_link(stage: Any, handle: _RobotHandle, link_name: str) -> str: + explicit = handle.sensor_links.get(link_name) + if explicit: + return explicit + root = stage.GetPrimAtPath(handle.root_path) + if not root.IsValid(): + raise ValueError(f"robot root does not exist: {handle.root_path}") + candidates = {link_name, f"{link_name}_link"} + for prim in stage.Traverse(): + path = str(prim.GetPath()) + if not path.startswith(handle.root_path.rstrip("/") + "/"): + continue + if prim.GetName() in candidates: + return path + raise ValueError(f"robot {handle.root_path} has no sensor link {link_name!r}") + + +def _compose_detectors( + stage: Any, + selection: ResolvedSystemSelection, + handles: dict[str, _RobotHandle], +) -> dict[str, str]: + from pxr import Gf, Sdf, UsdGeom + + paths: dict[str, str] = {} + for resolved in selection.detectors: + placement = resolved.placement + if placement.parent_robot_id is not None: + try: + handle = handles[placement.parent_robot_id] + except KeyError as error: + raise ValueError( + f"detector {placement.detector_id!r} requires robot " + f"{placement.parent_robot_id!r}, which is not selected" + ) from error + parent_path = ( + _find_robot_link(stage, handle, placement.parent_sensor_link) + if placement.parent_sensor_link + else handle.root_path + ) + else: + parent_path = str(placement.parent_prim_path) + parent = stage.GetPrimAtPath(parent_path) + if not parent.IsValid(): + raise ValueError( + f"detector {placement.detector_id!r} parent does not exist: {parent_path}" + ) + safe_id = "".join( + character if character.isalnum() else "_" for character in placement.detector_id + ) + path = f"{parent_path.rstrip('/')}/RadCounterDetectors/{safe_id}" + xform = UsdGeom.Xform.Define(stage, path) + xformable = UsdGeom.Xformable(xform.GetPrim()) + xformable.ClearXformOpOrder() + xformable.AddTranslateOp().Set(Gf.Vec3d(*placement.translation_m)) + xformable.AddRotateXYZOp().Set(Gf.Vec3f(*placement.rotation_rpy_deg)) + prim = xform.GetPrim() + prim.CreateAttribute("rad:role", Sdf.ValueTypeNames.String, custom=True).Set("detector") + prim.CreateAttribute("rad:detector:id", Sdf.ValueTypeNames.String, custom=True).Set( + placement.detector_id + ) + prim.CreateAttribute("rad:detector:modelId", Sdf.ValueTypeNames.String, custom=True).Set( + resolved.descriptor.model_id + ) + body = UsdGeom.Cylinder.Define(stage, path + "/Body") + body.CreateAxisAttr("X") + body.CreateRadiusAttr(0.045) + body.CreateHeightAttr(0.12) + body.CreateDisplayColorAttr([Gf.Vec3f(0.94, 0.67, 0.08)]) + paths[placement.detector_id] = path + return paths + + +def _write_runtime_config(selection: ResolvedSystemSelection, stage_path: Path) -> Path: + payload = json.loads(selection.runtime_config_path.read_text(encoding="utf-8")) + detectors = {} + for resolved in selection.detectors: + descriptor = resolved.descriptor + response = next( + ( + item + for item in descriptor.particle_responses + if item.radiation_type.value == "gamma" + ), + descriptor.particle_responses[0], + ) + detectors[resolved.placement.detector_id] = { + "efficiency_energy_keV": list(response.intrinsic_efficiency.energies_kev), + "intrinsic_efficiency": list(response.intrinsic_efficiency.values), + "background_cps": descriptor.background_cps, + "dead_time_s": descriptor.dead_time_s, + } + payload["scene"] = str(stage_path) + payload["detectors"] = detectors + payload["selected_system"] = { + "profile": selection.profile_id, + "environment": selection.environment_id, + "robot_set": selection.robot_set_id, + "detector_set": selection.detector_set_id, + } + serialized = json.dumps(payload, indent=2, sort_keys=True) + "\n" + digest = hashlib.sha256(serialized.encode("utf-8")).hexdigest()[:24] + environment_cache = Path(selection.environment_config.cache_directory).expanduser().resolve() + target = environment_cache.parent / "system_profiles" / digest + target.mkdir(parents=True, exist_ok=True) + path = target / "runtime.json" + temporary = path.with_suffix(".json.tmp") + temporary.write_text(serialized, encoding="utf-8") + temporary.replace(path) + return path diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/ui/dashboard.py b/source/extensions/radcounter.isaac/radcounter/isaac/ui/dashboard.py index acc38ca..d07e6a6 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/ui/dashboard.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/ui/dashboard.py @@ -5,6 +5,7 @@ import asyncio import json import math +import sys from collections.abc import Mapping from pathlib import Path from typing import Any @@ -14,7 +15,40 @@ import omni.ui as ui import omni.usd +from radcounter.core.natural_language import ( + AvailableAction, + CommandContext, + CommandName, + CommandStep, + normalize_operator_instruction, +) +from radcounter.core.system_profiles import ( + default_catalog_path, + default_selection_path, + load_active_selection, + load_system_catalog, + resolve_system_selection, + save_active_selection, +) + from ..runtime.simulation import IsaacRadiationSimulation, measurement_payload +from .robot_monitor import RobotMonitorOverlay + + +def _bound_label( + model: ui.AbstractValueModel, + subscriptions: list[Any], + **kwargs: object, +) -> ui.Label: + """Create a Label whose text follows a value model on current Omni UI.""" + + label = ui.Label(model.get_value_as_string(), **kwargs) + + def update(changed: ui.AbstractValueModel) -> None: + label.text = changed.get_value_as_string() + + subscriptions.append(model.subscribe_value_changed_fn(update)) + return label class RadCounterDashboard: @@ -23,8 +57,11 @@ def __init__(self, ext_id: str) -> None: self.root = Path(__file__).resolve().parents[6] self.stage_path = self.root / "assets/environments/radcounter_vertical_slice.usda" self.config_path = self.root / "configs/scenarios/vertical_slice.runtime.json" + self._system_display_name = "vertical slice" + self._compose_default_scene = True self.artifact_path = self.root / "artifacts/ui/latest_measurement.json" self.workflow_artifact_path = self.root / "artifacts/ui/latest_workflow.json" + self._label_subscriptions: list[Any] = [] self.simulation: IsaacRadiationSimulation | None = None self._tasks: set[asyncio.Task[Any]] = set() self._status = ui.SimpleStringModel("Load the vertical-slice scene to begin.") @@ -38,10 +75,67 @@ def __init__(self, ext_id: str) -> None: self._estimate = ui.SimpleStringModel("No estimator result supplied") self._residual = ui.SimpleStringModel("No verification residual supplied") self._plan = ui.SimpleStringModel("No countermeasure plan supplied") - self._window = ui.Window("RadCounterSim Operations", width=460, height=880) + self._selection_file = default_selection_path() + try: + self._active_system_selection = load_active_selection(self._selection_file) + self._system_catalog_path = self._active_system_selection.catalog_path + except Exception: + self._system_catalog_path = default_catalog_path() + self._active_system_selection = resolve_system_selection( + catalog_path=self._system_catalog_path + ) + self._system_catalog_path, self._system_catalog = load_system_catalog( + self._system_catalog_path + ) + self._profile_ids = tuple(self._system_catalog.profiles) + self._environment_ids = tuple(self._system_catalog.environments) + self._robot_set_ids = tuple(self._system_catalog.robot_sets) + self._detector_set_ids = tuple(self._system_catalog.detector_sets) + self._system_summary = ui.SimpleStringModel("") + self._system_controls: list[Any] = [] + self._updating_system_controls = False + self._command_input = ui.SimpleStringModel("") + self._command_status = ui.SimpleStringModel( + "Local command model starts when the first instruction is submitted." + ) + self._command_preview = ui.SimpleStringModel( + "Example: Move to the protected area and measure for 2 seconds." + ) + self._workflow_services: Any | None = None + self._workflow_belief: Any | None = None + self._command_candidates: dict[str, Any] = {} + self._physical_command_queue: list[ + tuple[CommandStep, asyncio.Future[dict[str, object]]] + ] = [] + self._navigation_status_prefix = "Measurement robot moving" + from ..natural_language import NaturalLanguageCommandController + + self._natural_language = NaturalLanguageCommandController( + self, + audit_path=self.root / "artifacts/ui/natural_language_commands.jsonl", + ) + self._robot_monitor = RobotMonitorOverlay(ext_id) + self._robot_monitor.set_robot_list_changed_callback(self._rebuild_robot_list) + self._robot_monitor.configure(self._active_system_selection) + self._window = ui.Window( + "RadCounterSim Operations", + width=540, + height=1000, + dockPreference=ui.DockPreference.RIGHT, + ) self._window.frame.set_build_fn(self._build) + self._window.deferred_dock_in("Stage", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE) + + def _operator_style(self, **properties: object) -> dict[str, object]: + return properties def _build(self) -> None: + with ui.ScrollingFrame( + horizontal_scrollbar_policy=ui.ScrollBarPolicy.SCROLLBAR_ALWAYS_OFF, + ): + self._build_content() + + def _build_content(self) -> None: with ui.VStack(spacing=10, height=0): ui.Label("RADCOUNTER / OPERATIONS", style={"font_size": 18, "color": 0xFFE3B341}) ui.Label( @@ -49,18 +143,82 @@ def _build(self) -> None: style={"font_size": 12, "color": 0xFF9FA6AD}, ) ui.Separator(height=4) + self._build_robot_monitor_controls() + ui.Separator(height=4) + ui.Label( + "NATURAL LANGUAGE COMMAND / ROBOT LLM", + style={"font_size": 11, "color": 0xFFE3B341}, + ) + ui.Label( + "Describe the robot task in English. Change the environment, robot, " + "and detector with the selectors below.", + word_wrap=True, + style=self._operator_style(font_size=12, color=0xFFB8BDC3), + ) + ui.StringField( + self._command_input, + height=46, + style=self._operator_style(font_size=14), + ) with ui.HStack(height=34, spacing=8): - ui.Button( - "Load vertical slice", clicked_fn=lambda: self._schedule(self._load_scene()) + self._command_run_button = ui.Button( + "1. Interpret", + width=150, + clicked_fn=self._submit_natural_language, + style=self._operator_style(font_size=12), + ) + self._command_confirm_button = ui.Button( + "2. Confirm & run", + width=250, + clicked_fn=self._confirm_natural_language, + style=self._operator_style( + font_size=12, + background_color=0xFFE3B341, + color=0xFF181B1F, + ), + ) + self._command_cancel_button = ui.Button( + "Cancel", + width=80, + clicked_fn=self._cancel_natural_language, + ) + self._command_confirm_button.enabled = False + self._command_cancel_button.enabled = False + _bound_label( + self._command_status, + self._label_subscriptions, + word_wrap=True, + height=62, + style=self._operator_style(font_size=12), + ) + _bound_label( + self._command_preview, + self._label_subscriptions, + word_wrap=True, + height=90, + style=self._operator_style(font_size=12, color=0xFFD3D7DC), + ) + ui.Separator(height=4) + self._build_system_selector() + ui.Separator(height=4) + with ui.HStack(height=34, spacing=8): + self._load_scene_button = ui.Button( + "Load vertical slice", + clicked_fn=lambda: self._schedule(self._load_scene()), ) ui.Button("Play / Pause", clicked_fn=self._toggle_timeline) with ui.HStack(height=34, spacing=8): ui.Button("Initialize radiation", clicked_fn=self._initialize_runtime) ui.Button("Sync USD -> Embree", clicked_fn=self._synchronize) ui.Label("STATUS", style={"font_size": 11, "color": 0xFF6CB6FF}) - ui.Label("", model=self._status, word_wrap=True, height=52) - ui.Label("", model=self._sources, word_wrap=True) - ui.Label("", model=self._revisions, word_wrap=True) + _bound_label( + self._status, + self._label_subscriptions, + word_wrap=True, + height=52, + ) + _bound_label(self._sources, self._label_subscriptions, word_wrap=True) + _bound_label(self._revisions, self._label_subscriptions, word_wrap=True) ui.Separator(height=4) ui.Label("SOURCE AUTHORING", style={"font_size": 11, "color": 0xFF6CB6FF}) with ui.HStack(height=28, spacing=8): @@ -81,15 +239,35 @@ def _build(self) -> None: with ui.HStack(height=34, spacing=8): ui.Button("Measure stations", clicked_fn=self._measure) ui.Button("Render dose proxy", clicked_fn=self._render_dose_map) - ui.Label("", model=self._measurement, word_wrap=True, height=150) + _bound_label( + self._measurement, + self._label_subscriptions, + word_wrap=True, + height=150, + ) ui.Separator(height=4) ui.Label("WORKFLOW OUTPUT", style={"font_size": 11, "color": 0xFF6CB6FF}) ui.Label("ESTIMATE", style={"font_size": 10, "color": 0xFF9FA6AD}) - ui.Label("", model=self._estimate, word_wrap=True, height=54) + _bound_label( + self._estimate, + self._label_subscriptions, + word_wrap=True, + height=54, + ) ui.Label("RESIDUAL", style={"font_size": 10, "color": 0xFF9FA6AD}) - ui.Label("", model=self._residual, word_wrap=True, height=64) + _bound_label( + self._residual, + self._label_subscriptions, + word_wrap=True, + height=64, + ) ui.Label("PLAN", style={"font_size": 10, "color": 0xFF9FA6AD}) - ui.Label("", model=self._plan, word_wrap=True, height=64) + _bound_label( + self._plan, + self._label_subscriptions, + word_wrap=True, + height=64, + ) ui.Button( "Load latest workflow artifact", height=32, @@ -111,20 +289,659 @@ def _build(self) -> None: style={"font_size": 11, "color": 0xFFDD7A6B}, ) + def _build_robot_monitor_controls(self) -> None: + ui.Label( + "ROBOT MONITOR", + style={"font_size": 11, "color": 0xFF6CB6FF}, + ) + ui.Label( + "Select View to follow a robot or Onboard for its camera. Manual camera " + "movement disables tracking.", + word_wrap=True, + style=self._operator_style(font_size=12, color=0xFFB8BDC3), + ) + with ui.HStack(height=30, spacing=8): + ui.Button("Building overview", clicked_fn=self._robot_monitor.overview) + ui.Button( + "Free camera", + clicked_fn=self._robot_monitor.stop_follow, + ) + self._robot_list_frame = ui.Frame(height=0) + self._robot_list_frame.set_build_fn(self._build_robot_rows) + + def _build_robot_rows(self) -> None: + robots = self._robot_monitor.robots + if not robots: + ui.Label( + "The current configuration has no robots.", + style=self._operator_style(font_size=12, color=0xFF8E989F), + ) + return + with ui.VStack(spacing=4, height=0): + for robot in robots: + active = robot.robot_id == self._robot_monitor.active_robot_id + with ui.HStack(height=30, spacing=6): + ui.Rectangle( + width=5, + style={"background_color": (0xFFF4BD55 if active else 0xFF48545C)}, + ) + ui.Label( + ("● " if active else "○ ") + robot.display_name, + width=250, + style=self._operator_style( + font_size=12, + color=0xFFF4E2B9 if active else 0xFFD2D8DC, + ), + ) + ui.Button( + "View", + width=84, + clicked_fn=lambda robot_id=robot.robot_id: self._robot_monitor.follow_robot( + robot_id + ), + ) + ui.Button( + "Onboard", + width=84, + clicked_fn=lambda robot_id=robot.robot_id: ( + self._robot_monitor.onboard_robot(robot_id) + ), + ) + + def _rebuild_robot_list(self) -> None: + frame = getattr(self, "_robot_list_frame", None) + if frame is not None: + frame.rebuild() + + @staticmethod + def _choice_index(values: tuple[str, ...], selected: str) -> int: + try: + return values.index(selected) + except ValueError: + return 0 + + @staticmethod + def _combo_index(combo: Any) -> int: + value_model = combo.model.get_item_value_model() + getter = getattr(value_model, "get_value_as_int", None) + return int(getter() if getter is not None else value_model.as_int) + + @staticmethod + def _set_combo_index(combo: Any, values: tuple[str, ...], selected: str) -> None: + value_model = combo.model.get_item_value_model() + value_model.set_value(RadCounterDashboard._choice_index(values, selected)) + + @staticmethod + def _choice_labels(entries: Mapping[str, Any]) -> tuple[str, ...]: + return tuple(entry.display_name for entry in entries.values()) + + def _build_system_selector(self) -> None: + selection = self._active_system_selection + ui.Label( + "SYSTEM CONFIGURATION", + style={"font_size": 11, "color": 0xFF6CB6FF}, + ) + ui.Label( + "Choose a preset or select the environment, robot, and detector independently.", + word_wrap=True, + style=self._operator_style(font_size=12, color=0xFFB8BDC3), + ) + rows = ( + ( + "Preset", + self._profile_ids, + self._system_catalog.profiles, + selection.profile_id, + "_profile_combo", + ), + ( + "Environment", + self._environment_ids, + self._system_catalog.environments, + selection.environment_id, + "_environment_combo", + ), + ( + "Robot", + self._robot_set_ids, + self._system_catalog.robot_sets, + selection.robot_set_id, + "_robot_set_combo", + ), + ( + "Detector", + self._detector_set_ids, + self._system_catalog.detector_sets, + selection.detector_set_id, + "_detector_set_combo", + ), + ) + for label, values, entries, selected, attribute in rows: + with ui.HStack(height=28, spacing=8): + ui.Label(label, width=92) + combo = ui.ComboBox( + self._choice_index(values, selected), + *self._choice_labels(entries), + ) + setattr(self, attribute, combo) + self._system_controls.append(combo) + self._profile_combo.model.add_item_changed_fn(self._on_profile_choice_changed) + for combo in ( + self._environment_combo, + self._robot_set_combo, + self._detector_set_combo, + ): + combo.model.add_item_changed_fn(self._on_component_choice_changed) + with ui.HStack(height=34, spacing=8): + self._system_apply_button = ui.Button( + "Apply selected configuration", + clicked_fn=self._apply_system_selection, + style=self._operator_style( + font_size=12, + background_color=0xFF4F86C6, + color=0xFFF6F8FA, + ), + ) + ui.Button("Restore current", width=110, clicked_fn=self._restore_system_selection) + _bound_label( + self._system_summary, + self._label_subscriptions, + word_wrap=True, + height=48, + style=self._operator_style(font_size=11, color=0xFFD3D7DC), + ) + self._update_system_preview() + + def _selected_system_ids(self) -> tuple[str, str, str, str]: + return ( + self._profile_ids[self._combo_index(self._profile_combo)], + self._environment_ids[self._combo_index(self._environment_combo)], + self._robot_set_ids[self._combo_index(self._robot_set_combo)], + self._detector_set_ids[self._combo_index(self._detector_set_combo)], + ) + + def _resolve_system_draft(self) -> Any: + profile, environment, robot_set, detector_set = self._selected_system_ids() + return resolve_system_selection( + catalog_path=self._system_catalog_path, + profile_id=profile, + environment_id=environment, + robot_set_id=robot_set, + detector_set_id=detector_set, + ) + + def _update_system_preview(self) -> None: + try: + selection = self._resolve_system_draft() + except Exception as exc: + self._system_summary.set_value(f"Choose a different combination: {exc}") + if hasattr(self, "_system_apply_button"): + self._system_apply_button.enabled = False + return + readiness = "Ready" if selection.environment_ready else "Environment data unavailable" + self._system_summary.set_value( + f"{selection.environment_entry.display_name} · " + f"{selection.robot_set.display_name} · {selection.detector_set.display_name}\n" + f"{readiness}" + ) + if hasattr(self, "_system_apply_button"): + can_prepare = bool(selection.environment_preparation_scripts) + self._system_apply_button.enabled = selection.environment_ready or can_prepare + self._system_apply_button.text = ( + "Apply selected configuration" + if selection.environment_ready + else "Prepare environment and apply" + ) + + def _on_profile_choice_changed(self, *_args: Any) -> None: + if self._updating_system_controls: + return + profile_id = self._profile_ids[self._combo_index(self._profile_combo)] + profile = self._system_catalog.profiles[profile_id] + self._updating_system_controls = True + try: + self._set_combo_index( + self._environment_combo, self._environment_ids, profile.environment + ) + self._set_combo_index(self._robot_set_combo, self._robot_set_ids, profile.robot_set) + self._set_combo_index( + self._detector_set_combo, self._detector_set_ids, profile.detector_set + ) + finally: + self._updating_system_controls = False + self._update_system_preview() + + def _on_component_choice_changed(self, *_args: Any) -> None: + if not self._updating_system_controls: + self._update_system_preview() + + def _sync_system_controls(self, selection: Any) -> None: + self._updating_system_controls = True + try: + self._set_combo_index(self._profile_combo, self._profile_ids, selection.profile_id) + self._set_combo_index( + self._environment_combo, self._environment_ids, selection.environment_id + ) + self._set_combo_index( + self._robot_set_combo, self._robot_set_ids, selection.robot_set_id + ) + self._set_combo_index( + self._detector_set_combo, self._detector_set_ids, selection.detector_set_id + ) + finally: + self._updating_system_controls = False + self._active_system_selection = selection + self._update_system_preview() + + def _restore_system_selection(self) -> None: + self._sync_system_controls(self._active_system_selection) + + def _set_system_controls_enabled(self, enabled: bool) -> None: + for control in self._system_controls: + control.enabled = enabled + self._system_apply_button.enabled = enabled + + def _apply_system_selection(self) -> None: + self._schedule(self._apply_system_selection_async()) + + async def _prepare_selected_environment(self, selection: Any) -> Any: + scripts = selection.environment_preparation_scripts + if not scripts: + raise FileNotFoundError( + selection.environment_entry.setup_hint or "Environment data is unavailable" + ) + project_python = self.root / ".venv/bin/python" + python = project_python if project_python.is_file() else Path(sys.executable) + for index, script in enumerate(scripts, start=1): + if script.suffix != ".py" or self.root not in script.parents or not script.is_file(): + raise ValueError(f"Environment preparation script is not allowed: {script}") + self._status.set_value(f"Preparing environment ({index}/{len(scripts)}): {script.stem}") + process = await asyncio.create_subprocess_exec( + str(python), + str(script), + cwd=str(self.root), + stdout=asyncio.subprocess.PIPE, + stderr=asyncio.subprocess.STDOUT, + ) + stdout, _ = await asyncio.wait_for(process.communicate(), timeout=900.0) + if process.returncode != 0: + detail = stdout.decode(errors="replace")[-3000:] + raise RuntimeError(f"{script.name} failed:\n{detail}") + prepared = self._resolve_system_draft() + if not prepared.environment_ready: + raise FileNotFoundError( + "Environment data is still unavailable after preparation: " + f"{prepared.environment_source_path}" + ) + return prepared + + async def _apply_system_selection_async(self) -> None: + try: + selection = self._resolve_system_draft() + except Exception as exc: + self._status.set_value(f"Cannot apply configuration: {type(exc).__name__}: {exc}") + self._update_system_preview() + return + self._set_system_controls_enabled(False) + self._natural_language.cancel() + omni.timeline.get_timeline_interface().pause() + self._status.set_value(f"Configuring {selection.profile.display_name}...") + try: + if not selection.environment_ready: + selection = await self._prepare_selected_environment(selection) + if selection.configurable: + from ..system_profile import ( + compose_selected_system, + prepare_environment_stage, + ) + + stage_path, _ = prepare_environment_stage(selection) + context = omni.usd.get_context() + result, error = await context.open_stage_async(str(stage_path)) + if not result: + raise RuntimeError(f"stage load failed: {error}") + composed = compose_selected_system( + context.get_stage(), selection, stage_path=stage_path + ) + simulation = IsaacRadiationSimulation.from_config( + context.get_stage(), composed.runtime_config_path + ) + self.configure_system_paths( + stage_path=composed.stage_path, + config_path=composed.runtime_config_path, + display_name=selection.profile.display_name, + simulation=simulation, + selection=selection, + ) + self._sources.set_value( + f"Estimator-visible sources: {len(simulation.belief_source_paths)}" + ) + self._revisions.set_value( + f"Embree geometries: {len(simulation.transport.geometry_paths)}" + ) + else: + source = selection.environment_source_path + if source is None: + raise RuntimeError("default environment must be a local USD stage") + self.stage_path = source + self.config_path = selection.runtime_config_path + self._system_display_name = selection.profile.display_name + self._compose_default_scene = True + self._load_scene_button.text = f"Reload {selection.profile.display_name}" + await self._load_scene() + self._initialize_runtime() + if self.simulation is None: + raise RuntimeError("radiation runtime did not initialize") + + save_active_selection( + self._selection_file, + catalog_path=selection.catalog_path, + profile_id=selection.profile_id, + environment_id=selection.environment_id, + robot_set_id=selection.robot_set_id, + detector_set_id=selection.detector_set_id, + ) + self._sync_system_controls(selection) + if not selection.configurable: + self._robot_monitor.configure(selection) + self._status.set_value(f"Configuration applied: {selection.profile.display_name}") + except Exception as exc: + self._status.set_value(f"Failed to apply configuration: {type(exc).__name__}: {exc}") + finally: + self._set_system_controls_enabled(True) + self._update_system_preview() + def _schedule(self, coroutine) -> None: task = asyncio.ensure_future(coroutine) self._tasks.add(task) task.add_done_callback(self._tasks.discard) + def _set_command_busy(self, busy: bool) -> None: + self._command_run_button.enabled = not busy + if busy: + self._command_confirm_button.enabled = False + self._command_cancel_button.enabled = False + + def configure_system_paths( + self, + *, + stage_path: str | Path, + config_path: str | Path, + display_name: str, + simulation: IsaacRadiationSimulation | None = None, + selection: Any | None = None, + ) -> None: + """Bind the dashboard reload action to a generated system-profile stage.""" + + self.stage_path = Path(stage_path).expanduser().resolve() + self.config_path = Path(config_path).expanduser().resolve() + self._system_display_name = display_name + self._compose_default_scene = False + self.simulation = simulation + self._workflow_services = None + self._workflow_belief = None + self._command_candidates.clear() + self._load_scene_button.text = f"Reload {display_name}" + self._status.set_value(f"Loaded {display_name}.") + if selection is not None: + self._sync_system_controls(selection) + self._robot_monitor.configure(selection) + + @staticmethod + def _format_command_plan(submission) -> str: + lines = [submission.validated.plan.summary] + for index, step in enumerate(submission.validated.plan.steps, start=1): + detail = step.command.value + if step.candidate_id: + detail += f" · {step.candidate_id}" + if step.duration_s is not None: + detail += f" · {step.duration_s:g} s" + if step.max_attempts > 1: + detail += f" · max {step.max_attempts} attempts" + if step.until is not None: + detail += f" · until {step.until.criterion.value}={step.until.threshold:g}" + lines.append(f"{index}. {detail}") + return "\n".join(lines) + + @staticmethod + def _format_command_completion(submission: Any) -> str: + motions: list[Mapping[str, object]] = [] + measurement_duration_s: float | None = None + returned_home = False + unmet_conditions: list[Mapping[str, object]] = [] + for result in submission.results: + if not isinstance(result, Mapping): + continue + completion = result.get("completion_condition") + if ( + isinstance(completion, Mapping) + and completion.get("met") is False + and result.get("attempt") == result.get("max_attempts") + ): + unmet_conditions.append(completion) + command = result.get("command") + if command == CommandName.MEASURE.value: + duration = result.get("duration_s") + if isinstance(duration, (int, float)): + measurement_duration_s = float(duration) + if command == CommandName.RETURN_MEASUREMENT_ROBOT.value: + returned_home = True + public_details = result.get("public_details") + if not isinstance(public_details, Mapping): + continue + motion = public_details.get("motion_audit") + if isinstance(motion, Mapping): + motions.append(motion) + if unmet_conditions: + last = unmet_conditions[-1] + return ( + "The bounded workflow stopped at its attempt limit without meeting " + f"the completion condition: {last.get('criterion')} " + f"observed={last.get('observed')} target={last.get('threshold')}." + ) + if motions: + distances = [ + float(motion["displacement_m"]) + for motion in motions + if isinstance(motion.get("displacement_m"), (int, float)) + ] + final = motions[-1].get("final_position_m") + measurement = ( + "" + if measurement_duration_s is None + else f" -> measured for {measurement_duration_s:g} seconds" + ) + if returned_home and len(distances) >= 2: + final_text = "" + if isinstance(final, (tuple, list)) and len(final) >= 2: + final_text = f" Final position ({float(final[0]):.2f}, {float(final[1]):.2f})." + return ( + f"Three steps complete: moved {distances[0]:.2f} m{measurement}, " + f"then returned {distances[-1]:.2f} m to the start.{final_text}" + ) + initial = motions[0].get("initial_position_m") + if ( + isinstance(initial, (tuple, list)) + and len(initial) >= 2 + and isinstance(final, (tuple, list)) + and len(final) >= 2 + and distances + ): + return ( + f"Moved {distances[0]:.2f} m: " + f"({float(initial[0]):.2f}, {float(initial[1]):.2f}) → " + f"({float(final[0]):.2f}, {float(final[1]):.2f}){measurement}." + ) + return "Movement and measurement completed." + + def update_navigation_progress( + self, + step: int, + position_m: tuple[float, float, float], + target_xy_m: tuple[float, float], + remaining_m: float, + ) -> None: + """Show observable articulation progress while the robot is moving.""" + + self._command_status.set_value( + f"{self._navigation_status_prefix}: step {step} · " + f"position ({position_m[0]:.2f}, {position_m[1]:.2f}) -> " + f"target ({target_xy_m[0]:.2f}, {target_xy_m[1]:.2f}) · " + f"{remaining_m:.2f} m remaining" + ) + self._robot_monitor.update_navigation_progress( + position_m=position_m, + target_xy_m=target_xy_m, + remaining_m=remaining_m, + ) + + def update_countermeasure_progress(self, event: Mapping[str, object]) -> None: + """Forward physical countermeasure phases to the viewport monitor.""" + + self._robot_monitor.update_countermeasure_progress(event) + + def robot_monitor_audit(self) -> dict[str, object]: + return self._robot_monitor.audit() + + def _submit_natural_language(self) -> None: + raw_instruction = self._command_input.get_value_as_string() + instruction = normalize_operator_instruction(raw_instruction) + if not instruction: + self._command_status.set_value("Enter an instruction in English.") + return + self._command_input.set_value("") + self._natural_language.cancel() + self._schedule( + self._interpret_natural_language( + instruction, + duplicate_removed=instruction != raw_instruction.strip(), + ) + ) + + async def _interpret_natural_language( + self, + instruction: str, + *, + duplicate_removed: bool = False, + ) -> None: + self._set_command_busy(True) + prefix = "Duplicate input was reduced to one copy. " if duplicate_removed else "" + self._command_status.set_value(f"{prefix}Interpreting with the local model...") + self._command_preview.set_value(instruction) + try: + submission = await self._natural_language.submit(instruction) + self._command_preview.set_value(self._format_command_plan(submission)) + if submission.executed: + self._command_status.set_value("Command completed.") + else: + self._command_status.set_value( + "Interpretation complete. Select the yellow '2. Confirm & run' " + "button to move the robot." + ) + self._command_confirm_button.enabled = True + self._command_cancel_button.enabled = True + except Exception as exc: + self._command_status.set_value(f"Command rejected: {type(exc).__name__}: {exc}") + finally: + self._command_run_button.enabled = True + + def _confirm_natural_language(self) -> None: + self._schedule(self._execute_confirmed_natural_language()) + + async def _execute_confirmed_natural_language(self) -> None: + self._set_command_busy(True) + self._command_status.set_value("Executing confirmed operation...") + try: + submission = await self._natural_language.confirm() + self._command_preview.set_value(self._format_command_plan(submission)) + self._command_status.set_value(self._format_command_completion(submission)) + except Exception as exc: + self._command_status.set_value(f"Execution failed: {type(exc).__name__}: {exc}") + finally: + self._command_run_button.enabled = True + self._command_confirm_button.enabled = False + self._command_cancel_button.enabled = False + + def _cancel_natural_language(self) -> None: + self._natural_language.cancel() + self._command_confirm_button.enabled = False + self._command_cancel_button.enabled = False + self._command_status.set_value("Pending operation cancelled.") + + async def submit_natural_language_instruction( + self, + instruction: str, + *, + confirm_physical: bool = False, + ): + """Submit an instruction through the same controller used by the UI.""" + + submission = await self._natural_language.submit(instruction) + if not submission.executed and confirm_physical: + submission = await self._natural_language.confirm() + return submission + + async def confirm_natural_language_instruction(self): + """Confirm a previously interpreted plan through the dashboard controller.""" + + return await self._natural_language.confirm() + async def _load_scene(self) -> None: - self._status.set_value("Loading USD stage...") + self._status.set_value(f"Loading {self._system_display_name}...") context = omni.usd.get_context() result, error = await context.open_stage_async(str(self.stage_path)) if not result: self._status.set_value(f"Stage load failed: {error}") return + if not self._compose_default_scene: + self._workflow_services = None + self._workflow_belief = None + self._command_candidates.clear() + try: + self.simulation = IsaacRadiationSimulation.from_config( + context.get_stage(), self.config_path + ) + except Exception as exc: + self.simulation = None + self._status.set_value(f"Profile reload failed: {type(exc).__name__}: {exc}") + return + self._status.set_value(f"Reloaded {self._system_display_name}.") + return + try: + from omni.kit.app import get_app + + from ..robot.real_robots import ( + RealRobotAssetConfig, + add_real_robot_references, + author_real_robot_task_scene, + create_decontamination_activity_map, + enable_real_robot_extensions, + ) + + enable_real_robot_extensions() + for _ in range(20): + await get_app().next_update_async() + stage = context.get_stage() + config = RealRobotAssetConfig() + add_real_robot_references(stage, config=config) + for _ in range(120): + await get_app().next_update_async() + activity_path = create_decontamination_activity_map( + self.root / "artifacts/ui/runtime_dashboard_activity.npz" + ) + author_real_robot_task_scene(stage, activity_path, config=config) + for _ in range(20): + await get_app().next_update_async() + except Exception as exc: + self._status.set_value( + f"Articulated scene composition failed: {type(exc).__name__}: {exc}" + ) + return self.simulation = None - self._status.set_value(f"Loaded {self.stage_path.name}") + self._workflow_services = None + self._workflow_belief = None + self._command_candidates.clear() + self._status.set_value("Loaded Ridgeback+Franka and Nova Carter with IK task tooling.") @staticmethod def _toggle_timeline() -> None: @@ -301,9 +1118,337 @@ def _load_workflow_artifact(self) -> None: self.set_workflow_view(payload) self._status.set_value(f"Loaded {self.workflow_artifact_path.relative_to(self.root)}") + def bind_workflow(self, services: Any, belief: Any) -> None: + """Expose a live, truth-free workflow to natural-language commands.""" + + self._workflow_services = services + self._workflow_belief = belief + self._refresh_command_candidates() + self._command_status.set_value("English natural-language control is ready") + + @staticmethod + def _candidate_label(candidate: Any) -> str: + action = candidate.action + target = "scene target" + if action.target_prim_path: + target = Path(str(action.target_prim_path)).name + labels = { + "measure": "move to station; measuring requires a separate step", + "decontaminate": "decontaminate", + "place_shield": "place shield", + "move_shield": "move shield", + "move_object": "move object", + "remove_object": "remove object", + "repair_action": "repair action", + } + action_type = str(action.action_type) + details: list[str] = [] + if action_type == "decontaminate": + details.append(str(action.parameters.get("decon_profile", "full serpentine raster"))) + if action_type in {"place_shield", "move_shield"}: + fraction = action.parameters.get("placement_fraction") + if isinstance(fraction, (int, float)): + details.append(f"source→protected line {100.0 * float(fraction):g}%") + suffix = "" if not details else " · " + " · ".join(details) + return f"{labels.get(action_type, action_type)} · {target}{suffix}" + + def _refresh_command_candidates(self) -> tuple[AvailableAction, ...]: + services = self._workflow_services + belief = self._workflow_belief + if services is None or belief is None: + self._command_candidates.clear() + return () + from radcounter.core.planning import DeterministicFeasibilityChecker + + checker = DeterministicFeasibilityChecker() + candidates = services.candidate_generator.generate_all(belief, services.last_diagnosis) + self._command_candidates = {item.action.action_id: item for item in candidates} + stage = omni.usd.get_context().get_stage() + + def target_label(item: Any) -> str | None: + path = item.action.target_prim_path + if stage is None or path is None: + return None + prim = stage.GetPrimAtPath(str(path)) + if not prim or not prim.IsValid(): + return None + display_name = str(prim.GetDisplayName() or "").strip() + return display_name or prim.GetName() + + return tuple( + AvailableAction( + action_id=item.action.action_id, + action_type=str(item.action.action_type), + label=self._candidate_label(item), + target=( + None + if item.action.target_prim_path is None + else str(item.action.target_prim_path) + ), + target_label=target_label(item), + robot_id=str(item.action.robot_id), + execution_mode=( + str(item.action.parameters["decon_profile"]) + if "decon_profile" in item.action.parameters + else None + ), + placement_fraction=( + float(item.action.parameters["placement_fraction"]) + if "placement_fraction" in item.action.parameters + else None + ), + predicted_duration_s=float(item.action.predicted_duration_s), + feasible=checker.evaluate(item, services.resources).feasible, + ) + for item in candidates + ) + + def natural_language_context(self) -> CommandContext: + """Return only current public state and generated action identifiers.""" + + timeline = omni.timeline.get_timeline_interface() + stage = omni.usd.get_context().get_stage() + if stage is None: + session_state = "no_stage" + stage_name = None + else: + session_state = "playing" if timeline.is_playing() else "paused" + identifier = stage.GetRootLayer().identifier + stage_name = Path(identifier).name if identifier else "untitled" + capabilities = { + CommandName.LOAD_DEFAULT_SCENE, + CommandName.PLAY, + CommandName.PAUSE, + CommandName.STEP, + CommandName.RESET, + CommandName.SHOW_STATUS, + } + if stage is not None: + capabilities.add(CommandName.INITIALIZE_RADIATION) + if self.simulation is not None: + capabilities.update( + { + CommandName.SYNC_SCENE, + CommandName.MEASURE, + CommandName.RENDER_DOSE_MAP, + } + ) + if getattr(self, "_latest_records", None): + capabilities.add(CommandName.EXPORT_MEASUREMENT) + services = self._workflow_services + measurement_controller = ( + None if services is None else getattr(services, "measurement_controller", None) + ) + if ( + measurement_controller is not None + and getattr(measurement_controller, "home_position_m", None) is not None + ): + capabilities.add(CommandName.RETURN_MEASUREMENT_ROBOT) + available_actions = self._refresh_command_candidates() + if available_actions: + capabilities.add(CommandName.EXECUTE_CANDIDATE) + # Reloading a bound stage would invalidate articulation controllers. + capabilities.discard(CommandName.LOAD_DEFAULT_SCENE) + capabilities.discard(CommandName.RESET) + return CommandContext( + session_state=session_state, + stage_name=stage_name, + available_actions=available_actions, + capabilities=tuple(sorted(capabilities, key=str)), + ) + + @staticmethod + def _complete_immediate(coroutine: Any) -> Any: + try: + coroutine.send(None) + except StopIteration as completed: + return completed.value + coroutine.close() + raise RuntimeError("physical workflow unexpectedly yielded") + + def _execute_candidate_command_sync(self, candidate_id: str) -> dict[str, object]: + from radcounter.core.models.actions import ActionStatus, ActionType + from radcounter.core.planning import DeterministicFeasibilityChecker + + services = self._workflow_services + belief = self._workflow_belief + if services is None or belief is None: + raise RuntimeError("physical workflow is not initialized") + self._refresh_command_candidates() + candidate = self._command_candidates.get(candidate_id) + if candidate is None: + raise RuntimeError(f"action is no longer available: {candidate_id}") + report = DeterministicFeasibilityChecker().evaluate(candidate, services.resources) + if not report.feasible: + raise RuntimeError( + f"action became infeasible: {candidate_id}: {', '.join(report.reasons)}" + ) + action = candidate.action + if action.action_type == ActionType.MEASURE: + self._navigation_status_prefix = "Moving to measurement station" + self._robot_monitor.begin_action(action) + try: + prediction = services.preview(action, belief) + result = self._complete_immediate(services.execute(action)) + if result.status not in {ActionStatus.COMPLETED, ActionStatus.PARTIAL}: + raise RuntimeError(f"{candidate_id} failed: {result.public_details}") + diagnosis = None + if action.action_type != ActionType.MEASURE: + verification = self._complete_immediate(services.verify(action)) + diagnosis = services.diagnose(prediction, verification) + belief = services.update(belief, diagnosis) + self._workflow_belief = belief + self.set_workflow_view(services.workflow_view()) + except Exception: + self._robot_monitor.finish_action(success=False) + raise + self._robot_monitor.finish_action( + success=True, + public_details=result.public_details, + ) + return { + "command": CommandName.EXECUTE_CANDIDATE.value, + "action_id": candidate_id, + "action_type": str(action.action_type), + "status": str(result.status), + "public_details": dict(result.public_details), + "diagnosed": diagnosis is not None, + } + + def _return_measurement_robot_command_sync(self) -> dict[str, object]: + services = self._workflow_services + controller = None if services is None else getattr(services, "measurement_controller", None) + if controller is None: + raise RuntimeError("measurement robot controller is not initialized") + self._navigation_status_prefix = "Measurement robot returning to start" + self._robot_monitor.begin_operation( + robot_id="measurement", + operation="Return to start", + phase="returning_home", + ) + try: + report = controller.return_home() + except Exception: + self._robot_monitor.finish_action(success=False) + raise + if not report.success: + self._robot_monitor.finish_action(success=False) + raise RuntimeError(report.message) + self._robot_monitor.finish_action(success=True) + return { + "command": CommandName.RETURN_MEASUREMENT_ROBOT.value, + "status": "completed", + "public_details": { + "message": report.message, + "motion_audit": { + "success": bool(report.success), + "steps": int(report.steps), + "displacement_m": float(report.displacement_m), + "initial_position_m": list(report.initial_position_m), + "final_position_m": list(report.final_position_m), + }, + }, + } + + def process_pending_natural_language_actions(self) -> None: + """Execute one physical action outside Kit's asyncio callback context.""" + + if not self._physical_command_queue: + return + step, future = self._physical_command_queue.pop(0) + if future.cancelled(): + return + try: + if step.command == CommandName.EXECUTE_CANDIDATE: + assert step.candidate_id is not None + result = self._execute_candidate_command_sync(step.candidate_id) + elif step.command == CommandName.RETURN_MEASUREMENT_ROBOT: + result = self._return_measurement_robot_command_sync() + else: + raise RuntimeError(f"unsupported queued physical command: {step.command}") + except Exception as exc: + future.set_exception(exc) + else: + future.set_result(result) + + async def execute_natural_language_step(self, step: CommandStep) -> dict[str, object]: + """Execute one previously validated command on the Kit main thread.""" + + from omni.kit.app import get_app + + command = step.command + if command == CommandName.LOAD_DEFAULT_SCENE: + await self._load_scene() + elif command == CommandName.PLAY: + omni.timeline.get_timeline_interface().play() + elif command == CommandName.PAUSE: + omni.timeline.get_timeline_interface().pause() + elif command == CommandName.STEP: + timeline = omni.timeline.get_timeline_interface() + timeline.play() + await get_app().next_update_async() + timeline.pause() + elif command == CommandName.RESET: + await self._load_scene() + elif command == CommandName.INITIALIZE_RADIATION: + self._initialize_runtime() + elif command == CommandName.SYNC_SCENE: + self._synchronize() + elif command == CommandName.MEASURE: + if step.duration_s is not None: + self._duration.set_value(float(step.duration_s)) + duration_s = float(self._duration.get_value_as_float()) + self._measure() + records = tuple(getattr(self, "_latest_records", ())) + public_measurements = [ + { + "detector_path": str(record.detector_path), + "counts": int(record.counts), + "measured_rate_cps": float(record.measured_rate_cps), + "duration_s": float(record.duration_s), + } + for record in records + ] + return { + "command": command.value, + "duration_s": duration_s, + "detector_count": len(records), + "measurements": public_measurements, + "maximum_measured_rate_cps": max( + (row["measured_rate_cps"] for row in public_measurements), + default=0.0, + ), + "status": self._status.get_value_as_string(), + } + elif command == CommandName.RENDER_DOSE_MAP: + self._render_dose_map() + elif command == CommandName.EXPORT_MEASUREMENT: + self._export_measurement() + elif command in { + CommandName.EXECUTE_CANDIDATE, + CommandName.RETURN_MEASUREMENT_ROBOT, + }: + future = asyncio.get_running_loop().create_future() + self._physical_command_queue.append((step, future)) + return await future + elif command != CommandName.SHOW_STATUS: + raise RuntimeError(f"unhandled natural-language command: {command}") + return { + "command": command.value, + "status": self._status.get_value_as_string(), + } + def shutdown(self) -> None: for task in tuple(self._tasks): task.cancel() self._tasks.clear() + for _, future in self._physical_command_queue: + future.cancel() + self._physical_command_queue.clear() + self._natural_language.shutdown() + self._robot_monitor.destroy() + self._workflow_services = None + self._workflow_belief = None + self._command_candidates.clear() self.simulation = None self._window = None diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/ui/robot_monitor.py b/source/extensions/radcounter.isaac/radcounter/isaac/ui/robot_monitor.py new file mode 100644 index 0000000..63ee8a0 --- /dev/null +++ b/source/extensions/radcounter.isaac/radcounter/isaac/ui/robot_monitor.py @@ -0,0 +1,966 @@ +"""Low-cost robot tracking, minimap, camera, and viewport operation overlays.""" + +from __future__ import annotations + +import math +import time +from collections.abc import Callable, Mapping, Sequence +from typing import Any + +import numpy as np +import omni.kit.app +import omni.ui as ui +import omni.usd +from omni.ui import color as cl +from omni.ui import scene as sc +from pxr import Gf, Usd, UsdGeom + +from radcounter.core.robot_monitoring import ( + MONITOR_UPDATE_HZ, + MonitorRobot, + action_label, + action_route_points, + action_target_point, + padded_square_bounds, + phase_label, + progress_from_remaining, + project_minimap_point, + robots_for_selection, +) + + +def _rgba(value: Sequence[float]) -> Any: + return cl(float(value[0]), float(value[1]), float(value[2]), float(value[3])) + + +def _bind_label( + model: ui.AbstractValueModel, + subscriptions: list[Any], + **kwargs: object, +) -> ui.Label: + label = ui.Label(model.get_value_as_string(), **kwargs) + + def update(changed: ui.AbstractValueModel) -> None: + label.text = changed.get_value_as_string() + + subscriptions.append(model.subscribe_value_changed_fn(update)) + return label + + +class _RobotViewportManipulator(sc.Manipulator): + """Draw operator guidance over the viewport, including through-wall beacons.""" + + def __init__(self) -> None: + super().__init__() + self.robots: tuple[dict[str, object], ...] = () + self.active_robot_id: str | None = None + self.route_m: tuple[tuple[float, float, float], ...] = () + self.target_m: tuple[float, float, float] | None = None + self.target_label = "" + self.operation_label = "" + self.camera_position_m: tuple[float, float, float] | None = None + + def update( + self, + *, + robots: tuple[dict[str, object], ...], + active_robot_id: str | None, + route_m: tuple[tuple[float, float, float], ...], + target_m: tuple[float, float, float] | None, + target_label: str, + operation_label: str, + camera_position_m: tuple[float, float, float] | None, + ) -> None: + self.robots = robots + self.active_robot_id = active_robot_id + self.route_m = route_m + self.target_m = target_m + self.target_label = target_label + self.operation_label = operation_label + self.camera_position_m = camera_position_m + self.invalidate() + + @staticmethod + def _outline(lower: Sequence[float], upper: Sequence[float], color: Any) -> None: + low = tuple(map(float, lower)) + high = tuple(map(float, upper)) + corners = ( + (low[0], low[1], low[2]), + (high[0], low[1], low[2]), + (high[0], high[1], low[2]), + (low[0], high[1], low[2]), + (low[0], low[1], high[2]), + (high[0], low[1], high[2]), + (high[0], high[1], high[2]), + (low[0], high[1], high[2]), + ) + for first, second in ( + (0, 1), + (1, 2), + (2, 3), + (3, 0), + (4, 5), + (5, 6), + (6, 7), + (7, 4), + (0, 4), + (1, 5), + (2, 6), + (3, 7), + ): + sc.Line(corners[first], corners[second], color=color, thickness=2.2) + + def on_build(self) -> None: + if len(self.route_m) >= 2: + elevated = [(x, y, z + 0.18) for x, y, z in self.route_m] + sc.Curve( + elevated, + curve_type=sc.Curve.CurveType.LINEAR, + colors=[cl(0.20, 0.78, 1.00, 0.78)], + thicknesses=[3.0], + ) + if self.target_m is not None: + target = tuple(self.target_m) + sc.Points([target], colors=[cl(1.0, 0.35, 0.28, 0.92)], sizes=[16.0]) + with ( + sc.Transform( + look_at=sc.Transform.LookAt.CAMERA, + transform=sc.Matrix44.get_translation_matrix( + target[0], target[1], target[2] + 0.45 + ), + ), + sc.Transform(scale_to=sc.Space.NDC), + ): + sc.Label( + self.target_label or "Work target", + alignment=ui.Alignment.CENTER_BOTTOM, + color=cl(1.0, 0.55, 0.42, 0.95), + size=15, + ) + for item in self.robots: + position = tuple(item["position_m"]) + lower = tuple(item["lower_m"]) + upper = tuple(item["upper_m"]) + color = _rgba(item["color_rgba"]) + active = item["robot_id"] == self.active_robot_id + self._outline(lower, upper, color) + marker_position = (position[0], position[1], upper[2] + (0.28 if active else 0.16)) + sc.Points( + [marker_position], + colors=[color], + sizes=[17.0 if active else 10.0], + ) + distance = "" + if self.camera_position_m is not None: + distance_m = math.dist(position, self.camera_position_m) + distance = f" · {distance_m:.1f} m" + suffix = f"\n{self.operation_label}" if active and self.operation_label else "" + with ( + sc.Transform( + look_at=sc.Transform.LookAt.CAMERA, + transform=sc.Matrix44.get_translation_matrix(*marker_position), + ), + sc.Transform(scale_to=sc.Space.NDC), + ): + sc.Label( + f"{item['display_name']}{distance}{suffix}", + alignment=ui.Alignment.CENTER_BOTTOM, + color=color, + size=17 if active else 13, + ) + + +class RobotMonitorOverlay: + """Own the operator monitor without creating additional rendered viewports.""" + + CAMERA_PATH = "/World/RadCounterMonitoring/OperatorCamera" + + def __init__(self, ext_id: str) -> None: + self.ext_id = ext_id + self.robots: tuple[MonitorRobot, ...] = () + self.active_robot_id: str | None = None + self.active_action = "Idle" + self.active_phase = "idle" + self.progress = 0.0 + self.coverage_fraction: float | None = None + self.route_m: tuple[tuple[float, float, float], ...] = () + self.target_m: tuple[float, float, float] | None = None + self.target_path: str | None = None + self.camera_mode = "free" + self._camera_eye: np.ndarray | None = None + self._camera_target: np.ndarray | None = None + self._camera_op: Any | None = None + self._authored_camera_matrix: np.ndarray | None = None + self._authored_camera_world_matrix: np.ndarray | None = None + self._skip_manual_check = False + self._initial_remaining_m: float | None = None + self._auto_work_view = False + self._work_view_triggered = False + self._last_tick_s = 0.0 + self._map_lower_xy = (-5.0, -5.0) + self._map_upper_xy = (5.0, 5.0) + self._structure_rectangles: tuple[tuple[tuple[float, float, float], ...], ...] = () + self._contamination_points: tuple[tuple[float, float, float], ...] = () + self._overlay_error: str | None = None + self._on_robot_list_changed: Callable[[], None] | None = None + self._subscriptions: list[Any] = [] + self._viewport_window: Any | None = None + self._viewport_api: Any | None = None + self._overlay_frame: Any | None = None + self._viewport_scene: Any | None = None + self._viewport_manipulator: _RobotViewportManipulator | None = None + self._minimap_scene: Any | None = None + self._minimap_dynamic: Any | None = None + self._active_model = ui.SimpleStringModel("NO ACTIVE ROBOT") + self._position_model = ui.SimpleStringModel("Position --") + self._action_model = ui.SimpleStringModel("Idle") + self._camera_model = ui.SimpleStringModel("FREE CAMERA") + self._progress_model = ui.SimpleFloatModel(0.0) + self._progress_text_model = ui.SimpleStringModel("0%") + self._update_subscription = ( + omni.kit.app.get_app() + .get_update_event_stream() + .create_subscription_to_pop(self._on_update, name=f"{ext_id}.robot_monitor") + ) + self._ensure_overlay() + + def set_robot_list_changed_callback(self, callback: Callable[[], None]) -> None: + self._on_robot_list_changed = callback + + def _ensure_overlay(self) -> bool: + if self._overlay_frame is not None: + return True + try: + from omni.kit.viewport.utility import get_active_viewport_window + + self._viewport_window = get_active_viewport_window() + if self._viewport_window is None: + return False + self._viewport_api = self._viewport_window.viewport_api + self._overlay_frame = self._viewport_window.get_frame(f"{self.ext_id}.robot_monitor") + with self._overlay_frame, ui.ZStack(): + self._viewport_scene = sc.SceneView() + with self._viewport_scene.scene: + self._viewport_manipulator = _RobotViewportManipulator() + self._viewport_api.add_scene_view(self._viewport_scene) + with ui.VStack(): + ui.Spacer(height=12) + with ui.HStack(height=214): + ui.Spacer(width=14) + self._build_status_bar() + ui.Spacer() + self._build_minimap() + ui.Spacer(width=14) + ui.Spacer() + self._overlay_error = None + return True + except Exception as exc: + self._overlay_error = f"{type(exc).__name__}: {exc}" + self._overlay_frame = None + return False + + def _build_status_bar(self) -> None: + with ui.ZStack(width=670, height=82): + ui.Rectangle( + style={ + "background_color": 0xE620252A, + "border_color": 0xFF3B5668, + "border_width": 1, + "border_radius": 5, + } + ) + with ui.VStack(spacing=3): + ui.Spacer(height=8) + with ui.HStack(height=20): + ui.Spacer(width=12) + _bind_label( + self._active_model, + self._subscriptions, + width=260, + style={"font_size": 15, "color": 0xFFFFC66D}, + ) + _bind_label( + self._position_model, + self._subscriptions, + width=230, + style={"font_size": 13, "color": 0xFFDCE5EB}, + ) + _bind_label( + self._camera_model, + self._subscriptions, + alignment=ui.Alignment.RIGHT, + style={"font_size": 11, "color": 0xFF73C9F5}, + ) + ui.Spacer(width=12) + with ui.HStack(height=22): + ui.Spacer(width=12) + _bind_label( + self._action_model, + self._subscriptions, + width=560, + style={"font_size": 13, "color": 0xFFF1F4F6}, + ) + _bind_label( + self._progress_text_model, + self._subscriptions, + alignment=ui.Alignment.RIGHT, + style={"font_size": 13, "color": 0xFF73C9F5}, + ) + ui.Spacer(width=12) + with ui.HStack(height=8): + ui.Spacer(width=12) + ui.ProgressBar( + self._progress_model, + height=5, + style={"color": 0xFF4CC7F2, "background_color": 0xFF303941}, + ) + ui.Spacer(width=12) + + def _build_minimap(self) -> None: + with ui.ZStack(width=252, height=202): + ui.Rectangle( + style={ + "background_color": 0xEA151A1E, + "border_color": 0xFF3B5668, + "border_width": 1, + "border_radius": 5, + } + ) + with ui.VStack(spacing=2): + ui.Spacer(height=7) + ui.Label( + "BUILDING OVERVIEW", + height=18, + alignment=ui.Alignment.CENTER, + style={"font_size": 11, "color": 0xFF9BAAB4}, + ) + self._minimap_scene = sc.SceneView( + height=168, + aspect_ratio_policy=sc.AspectRatioPolicy.PRESERVE_ASPECT_FIT, + ) + self._minimap_scene.cache_draw_buffer = False + self._rebuild_minimap_scene() + + def configure(self, selection: Any) -> None: + self.robots = robots_for_selection(selection) + self.active_robot_id = self.robots[0].robot_id if self.robots else None + self.active_action = "Idle" + self.active_phase = "idle" + self.progress = 0.0 + self.coverage_fraction = None + self.route_m = () + self.target_m = None + self.target_path = None + self.stop_follow(manual=False) + self._compute_minimap_geometry(selection) + self._ensure_overlay() + self._rebuild_minimap_scene() + self._update_models(()) + if self._on_robot_list_changed is not None: + self._on_robot_list_changed() + + def _stage_bounds(self, stage: Any) -> tuple[np.ndarray, np.ndarray] | None: + cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + for path in ("/World/Environment", "/World/RemoteDeconFacility", "/World"): + prim = stage.GetPrimAtPath(path) + if not prim or not prim.IsValid(): + continue + aligned = cache.ComputeWorldBound(prim).ComputeAlignedRange() + lower = np.asarray(aligned.GetMin(), dtype=np.float64) + upper = np.asarray(aligned.GetMax(), dtype=np.float64) + if np.all(np.isfinite(lower)) and np.all(np.isfinite(upper)): + return lower, upper + return None + + def _compute_minimap_geometry(self, selection: Any) -> None: + stage = omni.usd.get_context().get_stage() + if stage is None: + return + bounds = self._stage_bounds(stage) + if bounds is None: + anchors = [ + anchor.translation_m + for anchor in selection.environment_entry.spawn_anchors.values() + ] + if anchors: + points = np.asarray(anchors, dtype=np.float64) + lower, upper = points.min(axis=0), points.max(axis=0) + else: + lower, upper = np.asarray((-5.0, -5.0, 0.0)), np.asarray((5.0, 5.0, 2.0)) + else: + lower, upper = bounds + self._map_lower_xy, self._map_upper_xy = padded_square_bounds(lower[:2], upper[:2]) + + cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + rectangles: list[tuple[float, tuple[tuple[float, float, float], ...]]] = [] + contamination: list[tuple[float, float, float]] = [] + for prim in stage.Traverse(): + role = prim.GetAttribute("rad:role") + role_value = str(role.Get() or "") if role else "" + decon = prim.GetAttribute("rad:decon:enabled") + if role_value == "contaminated_surface" or (decon and bool(decon.Get())): + aligned = cache.ComputeWorldBound(prim).ComputeAlignedRange() + center = (np.asarray(aligned.GetMin()) + np.asarray(aligned.GetMax())) * 0.5 + points = None + if prim.IsA(UsdGeom.Mesh): + points = UsdGeom.Mesh(prim).GetPointsAttr().Get() + if points: + transform = UsdGeom.XformCache(Usd.TimeCode.Default()).GetLocalToWorldTransform( + prim + ) + stride = max(1, int(math.ceil(len(points) / 96))) + for point in points[::stride]: + world = transform.Transform(Gf.Vec3d(*point)) + contamination.append( + project_minimap_point( + world, + self._map_lower_xy, + self._map_upper_xy, + ) + ) + elif np.all(np.isfinite(center)): + contamination.append( + project_minimap_point( + center, + self._map_lower_xy, + self._map_upper_xy, + ) + ) + if not (prim.IsA(UsdGeom.Mesh) or prim.IsA(UsdGeom.Cube)): + continue + path = str(prim.GetPath()) + if not ( + path.startswith("/World/Environment") + or path.startswith("/World/RemoteDeconFacility") + ): + continue + aligned = cache.ComputeWorldBound(prim).ComputeAlignedRange() + item_lower = np.asarray(aligned.GetMin(), dtype=np.float64) + item_upper = np.asarray(aligned.GetMax(), dtype=np.float64) + extent = item_upper - item_lower + if not np.all(np.isfinite(extent)) or extent[0] < 0.15 or extent[1] < 0.15: + continue + corners = tuple( + project_minimap_point(point, self._map_lower_xy, self._map_upper_xy) + for point in ( + (item_lower[0], item_lower[1], 0.0), + (item_upper[0], item_lower[1], 0.0), + (item_upper[0], item_upper[1], 0.0), + (item_lower[0], item_upper[1], 0.0), + (item_lower[0], item_lower[1], 0.0), + ) + ) + rectangles.append((float(extent[0] * extent[1]), corners)) + rectangles.sort(key=lambda item: item[0], reverse=True) + self._structure_rectangles = tuple(item[1] for item in rectangles[:90]) + self._contamination_points = tuple(contamination[:128]) + + def _rebuild_minimap_scene(self) -> None: + if self._minimap_scene is None: + return + self._minimap_scene.scene.clear() + with self._minimap_scene.scene: + for value in (-0.45, 0.0, 0.45): + grid_color = cl(0.22, 0.28, 0.32, 0.55) + sc.Line( + (-0.9, value, 0.0), + (0.9, value, 0.0), + color=grid_color, + thickness=1.0, + ) + sc.Line( + (value, -0.9, 0.0), + (value, 0.9, 0.0), + color=grid_color, + thickness=1.0, + ) + for rectangle in self._structure_rectangles: + sc.Curve( + rectangle, + curve_type=sc.Curve.CurveType.LINEAR, + colors=[cl(0.40, 0.48, 0.53, 0.48)], + thicknesses=[1.0], + ) + if self._contamination_points: + sc.Points( + self._contamination_points, + colors=[cl(1.0, 0.28, 0.22, 0.78)] * len(self._contamination_points), + sizes=[8.0], + ) + self._minimap_dynamic = sc.Transform() + self._update_minimap_dynamic(()) + + def _robot_by_id(self, robot_id: str | None) -> MonitorRobot | None: + return next((robot for robot in self.robots if robot.robot_id == robot_id), None) + + def _robot_for_action(self, action_robot_id: object) -> MonitorRobot | None: + value = str(action_robot_id) + return next( + ( + robot + for robot in self.robots + if value == robot.robot_id + or value == robot.prim_path + or value.startswith(robot.prim_path.rstrip("/") + "/") + ), + None, + ) + + def begin_action(self, action: Any) -> None: + robot = self._robot_for_action(getattr(action, "robot_id", "")) + self.begin_operation( + robot_id=None if robot is None else robot.robot_id, + operation=action_label(getattr(action, "action_type", "operation")), + phase="navigating", + route_m=action_route_points(action), + target_m=action_target_point(action), + target_path=getattr(action, "target_prim_path", None), + auto_work_view=str(getattr(action, "action_type", "")) == "decontaminate", + ) + + def begin_operation( + self, + *, + robot_id: str | None, + operation: str, + phase: str, + route_m: Sequence[Sequence[float]] = (), + target_m: Sequence[float] | None = None, + target_path: str | None = None, + auto_work_view: bool = False, + ) -> None: + if self._robot_by_id(robot_id) is not None: + self.active_robot_id = robot_id + self.active_action = operation + self.active_phase = phase + self.progress = 0.0 + self.coverage_fraction = None + self._initial_remaining_m = None + self._auto_work_view = auto_work_view + self._work_view_triggered = False + self.route_m = tuple(tuple(float(item) for item in point[:3]) for point in route_m) + self.target_m = None if target_m is None else tuple(float(item) for item in target_m[:3]) + self.target_path = target_path + self.follow_robot(self.active_robot_id) + self._update_models(self._robot_snapshots()) + + def finish_action( + self, + *, + success: bool, + public_details: Mapping[str, object] | None = None, + ) -> None: + self.active_phase = "complete" if success else "failed" + self.progress = 1.0 if success else self.progress + details = public_details or {} + motion = details.get("motion_audit") + if isinstance(motion, Mapping): + coverage = motion.get("coverage_fraction") + if isinstance(coverage, (int, float)): + self.coverage_fraction = float(coverage) + self._update_models(self._robot_snapshots()) + + def update_countermeasure_progress(self, event: Mapping[str, object]) -> None: + phase = event.get("phase") + if isinstance(phase, str): + self.active_phase = phase + if ( + phase == "decontaminating" + and self._auto_work_view + and not self._work_view_triggered + ): + self._work_view_triggered = True + self.work_view(self.active_robot_id) + progress = event.get("progress") + if isinstance(progress, (int, float)) and math.isfinite(float(progress)): + self.progress = max(0.0, min(1.0, float(progress))) + coverage = event.get("coverage_fraction") + if isinstance(coverage, (int, float)): + self.coverage_fraction = max(0.0, min(1.0, float(coverage))) + route = event.get("path_m") + if isinstance(route, Sequence) and not isinstance(route, (str, bytes)): + points = [] + for value in route: + if isinstance(value, Sequence) and len(value) >= 3: + points.append(tuple(float(item) for item in value[:3])) + if points: + self.route_m = tuple(points) + target = event.get("target_m") + if ( + isinstance(target, Sequence) + and not isinstance(target, (str, bytes)) + and len(target) >= 3 + ): + self.target_m = tuple(float(item) for item in target[:3]) + + def update_navigation_progress( + self, + *, + position_m: tuple[float, float, float], + target_xy_m: tuple[float, float], + remaining_m: float, + ) -> None: + measurement = next((item for item in self.robots if item.robot_id == "measurement"), None) + if measurement is not None: + self.active_robot_id = measurement.robot_id + self.active_phase = "navigating" + if self._initial_remaining_m is None or remaining_m > self._initial_remaining_m: + self._initial_remaining_m = max(remaining_m, 1.0e-9) + self.progress = progress_from_remaining(remaining_m, self._initial_remaining_m) + self.target_m = (float(target_xy_m[0]), float(target_xy_m[1]), float(position_m[2])) + + def _set_active_robot(self, robot_id: str | None) -> MonitorRobot | None: + robot = self._robot_by_id(robot_id) + if robot is not None: + self.active_robot_id = robot.robot_id + if self._on_robot_list_changed is not None: + self._on_robot_list_changed() + return robot + + def follow_robot(self, robot_id: str | None) -> None: + if self._set_active_robot(robot_id) is None: + return + self._activate_camera("follow") + + def onboard_robot(self, robot_id: str | None) -> None: + if self._set_active_robot(robot_id) is None: + return + self._activate_camera("onboard") + + def work_view(self, robot_id: str | None) -> None: + if self._set_active_robot(robot_id) is None: + return + self._activate_camera("work") + + def overview(self) -> None: + self._activate_camera("overview") + + def _activate_camera(self, mode: str) -> None: + stage = omni.usd.get_context().get_stage() + if stage is None: + return + camera = UsdGeom.Camera.Define(stage, self.CAMERA_PATH) + camera.CreateFocalLengthAttr(24.0 if mode != "onboard" else 18.0) + xformable = UsdGeom.Xformable(camera) + xformable.ClearXformOpOrder() + self._camera_op = xformable.AddTransformOp() + self.camera_mode = mode + self._camera_eye = None + self._camera_target = None + self._authored_camera_matrix = None + self._authored_camera_world_matrix = None + self._skip_manual_check = True + self._ensure_overlay() + if self._viewport_api is not None: + self._viewport_api.set_active_camera(self.CAMERA_PATH) + self._update_models(self._robot_snapshots()) + + def stop_follow(self, *, manual: bool = True) -> None: + self.camera_mode = "free" + self._camera_op = None + self._authored_camera_matrix = None + self._authored_camera_world_matrix = None + if manual: + self._auto_work_view = False + self._update_models(self._robot_snapshots()) + + @staticmethod + def _matrix_array(matrix: Any) -> np.ndarray: + return np.asarray([[float(matrix[row][column]) for column in range(4)] for row in range(4)]) + + @staticmethod + def _world_transform(stage: Any, path: str) -> Any | None: + prim = stage.GetPrimAtPath(path) + if not prim or not prim.IsValid(): + return None + return UsdGeom.XformCache(Usd.TimeCode.Default()).GetLocalToWorldTransform(prim) + + def _desired_camera_pose( + self, snapshots: tuple[dict[str, object], ...] + ) -> tuple[np.ndarray, np.ndarray] | None: + if self.camera_mode == "overview": + lower = np.asarray(self._map_lower_xy) + upper = np.asarray(self._map_upper_xy) + center = (lower + upper) * 0.5 + span = float(np.max(upper - lower)) + return ( + np.asarray((center[0], center[1] - 0.15 * span, max(8.0, 0.95 * span))), + np.asarray((center[0], center[1], 0.0)), + ) + active = next( + (item for item in snapshots if item["robot_id"] == self.active_robot_id), None + ) + if active is None: + return None + position = np.asarray(active["position_m"], dtype=np.float64) + forward = np.asarray(active["forward_m"], dtype=np.float64) + forward[2] = 0.0 + length = float(np.linalg.norm(forward)) + forward = np.asarray((1.0, 0.0, 0.0)) if length < 1.0e-6 else forward / length + side = np.asarray((-forward[1], forward[0], 0.0)) + if self.camera_mode == "onboard": + eye = position + 0.38 * forward + np.asarray((0.0, 0.0, 0.82)) + return eye, eye + 5.0 * forward + np.asarray((0.0, 0.0, 0.05)) + if self.camera_mode == "work" and self.target_m is not None: + target = np.asarray(self.target_m, dtype=np.float64) + center = (position + target) * 0.5 + separation = max(2.2, float(np.linalg.norm(target[:2] - position[:2]))) + eye = center - 0.7 * separation * forward - 0.75 * separation * side + eye[2] = max(position[2], target[2]) + max(2.2, 0.65 * separation) + return eye, center + np.asarray((0.0, 0.0, 0.35)) + eye = position - 3.4 * forward - 1.45 * side + np.asarray((0.0, 0.0, 2.35)) + target = position + 0.75 * forward + np.asarray((0.0, 0.0, 0.65)) + return eye, target + + def _update_camera( + self, + snapshots: tuple[dict[str, object], ...], + dt_s: float, + ) -> None: + if self.camera_mode == "free" or self._camera_op is None: + return + if self._viewport_api is None or str(self._viewport_api.camera_path) != self.CAMERA_PATH: + self.stop_follow() + return + current_matrix = self._camera_op.Get() + stage = omni.usd.get_context().get_stage() + current_world = None if stage is None else self._world_transform(stage, self.CAMERA_PATH) + if ( + not self._skip_manual_check + and self._authored_camera_matrix is not None + and current_matrix is not None + and float( + np.max(np.abs(self._matrix_array(current_matrix) - self._authored_camera_matrix)) + ) + > 1.0e-3 + ): + self.stop_follow() + return + if ( + not self._skip_manual_check + and self._authored_camera_world_matrix is not None + and current_world is not None + and float( + np.max( + np.abs(self._matrix_array(current_world) - self._authored_camera_world_matrix) + ) + ) + > 1.0e-3 + ): + self.stop_follow() + return + desired = self._desired_camera_pose(snapshots) + if desired is None: + return + desired_eye, desired_target = desired + if self._camera_eye is None or self._camera_target is None: + self._camera_eye = desired_eye.copy() + self._camera_target = desired_target.copy() + else: + blend = 1.0 - math.exp(-4.5 * max(dt_s, 1.0 / MONITOR_UPDATE_HZ)) + self._camera_eye += blend * (desired_eye - self._camera_eye) + self._camera_target += blend * (desired_target - self._camera_target) + view = Gf.Matrix4d().SetLookAt( + Gf.Vec3d(*self._camera_eye), + Gf.Vec3d(*self._camera_target), + Gf.Vec3d(0.0, 0.0, 1.0), + ) + matrix = view.GetInverse() + self._camera_op.Set(matrix) + self._authored_camera_matrix = self._matrix_array(matrix) + stage = omni.usd.get_context().get_stage() + authored_world = None if stage is None else self._world_transform(stage, self.CAMERA_PATH) + self._authored_camera_world_matrix = ( + None if authored_world is None else self._matrix_array(authored_world) + ) + self._skip_manual_check = False + + def _robot_snapshots(self) -> tuple[dict[str, object], ...]: + stage = omni.usd.get_context().get_stage() + if stage is None: + return () + cache = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + snapshots = [] + for robot in self.robots: + prim = stage.GetPrimAtPath(robot.prim_path) + if not prim or not prim.IsValid(): + continue + transform = UsdGeom.XformCache(Usd.TimeCode.Default()).GetLocalToWorldTransform(prim) + position = np.asarray(transform.ExtractTranslation(), dtype=np.float64) + forward = np.asarray(transform.TransformDir(Gf.Vec3d(1.0, 0.0, 0.0)), dtype=np.float64) + aligned = cache.ComputeWorldBound(prim).ComputeAlignedRange() + lower = np.asarray(aligned.GetMin(), dtype=np.float64) + upper = np.asarray(aligned.GetMax(), dtype=np.float64) + if not np.all(np.isfinite(lower)) or not np.all(np.isfinite(upper)): + lower = position - np.asarray((0.35, 0.35, 0.05)) + upper = position + np.asarray((0.35, 0.35, 0.75)) + snapshots.append( + { + "robot_id": robot.robot_id, + "display_name": robot.display_name, + "prim_path": robot.prim_path, + "color_rgba": robot.color_rgba, + "position_m": tuple(map(float, position)), + "forward_m": tuple(map(float, forward)), + "lower_m": tuple(map(float, lower)), + "upper_m": tuple(map(float, upper)), + } + ) + return tuple(snapshots) + + def _camera_position(self, stage: Any) -> tuple[float, float, float] | None: + if self._viewport_api is None: + return None + path = str(self._viewport_api.camera_path) + transform = self._world_transform(stage, path) + if transform is None: + return None + return tuple(map(float, transform.ExtractTranslation())) + + def _update_models(self, snapshots: tuple[dict[str, object], ...]) -> None: + active = next( + (item for item in snapshots if item["robot_id"] == self.active_robot_id), None + ) + robot = self._robot_by_id(self.active_robot_id) + name = robot.display_name if robot is not None else "NO ACTIVE ROBOT" + self._active_model.set_value(f"ACTIVE ROBOT: {name}") + if active is None: + self._position_model.set_value("Position --") + else: + x, y, z = active["position_m"] + self._position_model.set_value(f"Position X {x:.2f} · Y {y:.2f} · Z {z:.2f} m") + phase = phase_label(self.active_phase) + coverage = ( + "" + if self.coverage_fraction is None + else f" · processed {100.0 * self.coverage_fraction:.0f}%" + ) + self._action_model.set_value(f"{self.active_action} · {phase}{coverage}") + camera_names = { + "free": "FREE CAMERA", + "follow": "FOLLOW", + "work": "WORK VIEW", + "onboard": "ONBOARD", + "overview": "OVERVIEW", + } + self._camera_model.set_value(camera_names.get(self.camera_mode, self.camera_mode.upper())) + self._progress_model.set_value(float(self.progress)) + self._progress_text_model.set_value(f"{100.0 * self.progress:.0f}%") + + def _update_minimap_dynamic(self, snapshots: tuple[dict[str, object], ...]) -> None: + if self._minimap_dynamic is None: + return + self._minimap_dynamic.clear() + with self._minimap_dynamic: + if len(self.route_m) >= 2: + route = tuple( + project_minimap_point(point, self._map_lower_xy, self._map_upper_xy) + for point in self.route_m + ) + sc.Curve( + route, + curve_type=sc.Curve.CurveType.LINEAR, + colors=[cl(0.20, 0.78, 1.00, 0.88)], + thicknesses=[2.5], + ) + if self.target_m is not None: + target = project_minimap_point( + self.target_m, self._map_lower_xy, self._map_upper_xy + ) + sc.Points([target], colors=[cl(1.0, 0.35, 0.28, 0.95)], sizes=[11.0]) + for item in snapshots: + position = project_minimap_point( + item["position_m"], self._map_lower_xy, self._map_upper_xy + ) + color = _rgba(item["color_rgba"]) + active = item["robot_id"] == self.active_robot_id + sc.Points([position], colors=[color], sizes=[14.0 if active else 9.0]) + forward = np.asarray(item["forward_m"], dtype=np.float64) + heading_world = np.asarray(item["position_m"], dtype=np.float64) + 0.9 * forward + heading = project_minimap_point( + heading_world, self._map_lower_xy, self._map_upper_xy + ) + sc.Line(position, heading, color=color, thickness=2.5) + with sc.Transform( + transform=sc.Matrix44.get_translation_matrix( + position[0], position[1] + 0.07, 0.0 + ) + ): + sc.Label( + str(item["display_name"]), + alignment=ui.Alignment.CENTER_BOTTOM, + color=color, + size=11 if active else 9, + ) + + def _on_update(self, _event: object) -> None: + now = time.perf_counter() + interval = 1.0 / MONITOR_UPDATE_HZ + if now - self._last_tick_s < interval: + return + dt_s = interval if self._last_tick_s == 0.0 else now - self._last_tick_s + self._last_tick_s = now + if not self._ensure_overlay(): + return + snapshots = self._robot_snapshots() + self._update_camera(snapshots, dt_s) + self._update_models(snapshots) + self._update_minimap_dynamic(snapshots) + stage = omni.usd.get_context().get_stage() + camera_position = None if stage is None else self._camera_position(stage) + if self._viewport_manipulator is not None: + if "measure" in self.active_action.casefold(): + target_label = "Detector measurement position" + elif self.target_path: + target_label = self.target_path.rsplit("/", 1)[-1] + else: + target_label = "Work target" + self._viewport_manipulator.update( + robots=snapshots, + active_robot_id=self.active_robot_id, + route_m=self.route_m, + target_m=self.target_m, + target_label=target_label, + operation_label=f"{self.active_action} · {phase_label(self.active_phase)}", + camera_position_m=camera_position, + ) + + def audit(self) -> dict[str, object]: + return { + "enabled": self._overlay_frame is not None, + "update_rate_hz": MONITOR_UPDATE_HZ, + "active_robot_id": self.active_robot_id, + "active_action": self.active_action, + "phase": self.active_phase, + "progress": self.progress, + "camera_mode": self.camera_mode, + "robot_count": len(self.robots), + "robots": [ + { + "robot_id": robot.robot_id, + "display_name": robot.display_name, + "prim_path": robot.prim_path, + } + for robot in self.robots + ], + "minimap_structure_count": len(self._structure_rectangles), + "contamination_marker_count": len(self._contamination_points), + "planned_route_point_count": len(self.route_m), + "overlay_error": self._overlay_error, + "single_rendered_viewport": True, + } + + def destroy(self) -> None: + self._update_subscription = None + if self._viewport_scene is not None and self._viewport_api is not None: + self._viewport_api.remove_scene_view(self._viewport_scene) + self._viewport_scene.scene.clear() + if self._minimap_scene is not None: + self._minimap_scene.scene.clear() + self._subscriptions.clear() + self._viewport_manipulator = None + self._viewport_scene = None + self._minimap_scene = None + self._overlay_frame = None + self._viewport_window = None + self._viewport_api = None diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/usd/environment.py b/source/extensions/radcounter.isaac/radcounter/isaac/usd/environment.py index 0dd2d3f..fc7a617 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/usd/environment.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/usd/environment.py @@ -51,6 +51,12 @@ def _material_for_name(name: str, config: EnvironmentImportConfig) -> str: return config.default_material_id +def _usd_row_matrix_values(matrix: Any) -> tuple[tuple[float, ...], ...]: + """Transpose the core column-vector transform for USD's row-vector convention.""" + + return tuple(tuple(float(matrix[column][row]) for column in range(4)) for row in range(4)) + + class EnvironmentUsdWriter: """Create a stable generated stage while preserving the source as immutable input.""" @@ -166,8 +172,8 @@ def _reference_native( ) matrix = normalization_matrix(config, defaults) gf_matrix = gf.Matrix4d() - for row in range(4): - gf_matrix.SetRow(row, gf.Vec4d(*[float(value) for value in matrix[row]])) + for row, values in enumerate(_usd_row_matrix_values(matrix)): + gf_matrix.SetRow(row, gf.Vec4d(*values)) usd_geom.Xformable(root).AddTransformOp().Set(gf_matrix) for prim in stage.Traverse(): if prim == root or not prim.IsA(usd_geom.Gprim): diff --git a/source/extensions/radcounter.isaac/radcounter/isaac/workflow/services.py b/source/extensions/radcounter.isaac/radcounter/isaac/workflow/services.py index 0b1c26d..06c1b78 100644 --- a/source/extensions/radcounter.isaac/radcounter/isaac/workflow/services.py +++ b/source/extensions/radcounter.isaac/radcounter/isaac/workflow/services.py @@ -8,7 +8,7 @@ import tempfile from collections.abc import Callable, Mapping from contextlib import suppress -from dataclasses import dataclass +from dataclasses import asdict, dataclass, is_dataclass from pathlib import Path from typing import Any @@ -73,6 +73,23 @@ class _ExecutionReport: public_details: Mapping[str, object] +def _motion_audit(report: object) -> Mapping[str, object]: + if is_dataclass(report): + return asdict(report) + return { + name: getattr(report, name) + for name in ( + "state", + "steps", + "message", + "phases", + "grasp_distance_m", + "placement_error_m", + ) + if hasattr(report, name) + } + + EstimatorCallback = Callable[[tuple[PublicMeasurement, ...], BeliefState | None], BeliefState] PreviewCallback = Callable[[CountermeasureAction, BeliefState], object] DiagnosisCallback = Callable[[object, tuple[PublicMeasurement, ...]], object] @@ -201,17 +218,71 @@ def preview(self, action: CountermeasureAction, belief: BeliefState) -> object: ) return self.last_prediction + def _collateral_positions( + self, excluded_object_path: str | None = None + ) -> dict[str, np.ndarray]: + """Snapshot non-target movable props for post-motion safety auditing.""" + + positions: dict[str, np.ndarray] = {} + stage = getattr(self.controller, "stage", None) + if stage is None: + return positions + for prim in stage.Traverse(): + path = str(prim.GetPath()) + if excluded_object_path and ( + path == excluded_object_path + or path.startswith(excluded_object_path.rstrip("/") + "/") + ): + continue + movable = prim.GetAttribute("rad:manipulation:movable") + if not movable or not movable.HasAuthoredValueOpinion() or not bool(movable.Get()): + continue + positions[path] = self.candidate_generator.probe.world_position(prim) + return positions + + def _collateral_motion_audit( + self, + before: Mapping[str, np.ndarray], + ) -> tuple[list[dict[str, object]], list[dict[str, object]]]: + audit: list[dict[str, object]] = [] + violations: list[dict[str, object]] = [] + for path, initial in before.items(): + final = self.candidate_generator.probe.world_position(path) + displacement = float(np.linalg.norm(final - initial)) + row: dict[str, object] = { + "object_path": path, + "displacement_m": displacement, + } + audit.append(row) + if displacement > 0.05: + violations.append(row) + return audit, violations + def _pick_and_place(self, action: CountermeasureAction) -> _ExecutionReport: if self.controller is None: return _ExecutionReport(False, "countermeasure controller is not configured", {}) parameters = action.parameters object_path = str(parameters.get("object_path", action.target_prim_path or "")) + collateral_before = self._collateral_positions(object_path) pickup = np.asarray(parameters["pickup_base_position_m"], dtype=np.float64) placement = np.asarray(parameters["placement_base_position_m"], dtype=np.float64) pickup_yaw = parameters.get("pickup_base_yaw_rad") placement_yaw = parameters.get("placement_base_yaw_rad") pickup_route = parameters.get("pickup_base_route_m") placement_route = parameters.get("placement_base_route_m") + plan_audit = { + "pickup_base_position_m": pickup.tolist(), + "placement_base_position_m": placement.tolist(), + "pickup_base_route_m": pickup_route, + "placement_base_route_m": placement_route, + "pickup_base_yaw_rad": pickup_yaw, + "placement_base_yaw_rad": placement_yaw, + "target_root_position_m": ( + None + if action.target_pose_world is None + else action.target_pose_world[:3, 3].tolist() + ), + } report = self.controller.execute_pick_and_place( object_path, pickup, @@ -220,18 +291,83 @@ def _pick_and_place(self, action: CountermeasureAction) -> _ExecutionReport: placement_base_yaw_rad=(None if placement_yaw is None else float(placement_yaw)), pickup_base_route_m=pickup_route, placement_base_route_m=placement_route, + target_root_position_m=( + None + if action.target_pose_world is None + else action.target_pose_world[:3, 3] + ), + placement_settle_tolerance_m=float( + parameters.get("placement_settle_tolerance_m", 0.15) + ), ) - if not report.success: - return _ExecutionReport(False, report.message, {"steps": report.steps}) - if action.action_type == ActionType.REMOVE_OBJECT: + released_into_disposal = ( + action.action_type == ActionType.REMOVE_OBJECT + and "release" in tuple(getattr(report, "phases", ())) + ) + if action.action_type == ActionType.REMOVE_OBJECT and ( + report.success or released_into_disposal + ): disposal = self.controller.remove_to_disposal_zone( object_path, str(parameters["disposal_zone_path"]), ) - if not disposal.success: + if disposal.success: return _ExecutionReport( - False, disposal.message, {"steps": report.steps + disposal.steps} + True, + disposal.message, + { + "object_path": object_path, + "steps": report.steps + disposal.steps, + "motion_audit": _motion_audit(report), + "disposal_audit": _motion_audit(disposal), + }, ) + if report.success: + return _ExecutionReport( + False, + disposal.message, + { + "steps": report.steps + disposal.steps, + "motion_audit": _motion_audit(report), + "disposal_audit": _motion_audit(disposal), + }, + ) + if not report.success: + collateral_audit, collateral_violations = self._collateral_motion_audit( + collateral_before + ) + message = report.message + if collateral_violations: + paths = ", ".join( + str(row["object_path"]) for row in collateral_violations + ) + message = f"{message}; unintended object motion detected: {paths}" + return _ExecutionReport( + False, + message, + { + "steps": report.steps, + "motion_audit": _motion_audit(report), + "collateral_motion_audit": collateral_audit, + "plan_audit": plan_audit, + }, + ) + collateral_audit, collateral_violations = self._collateral_motion_audit( + collateral_before + ) + if collateral_violations: + paths = ", ".join(str(row["object_path"]) for row in collateral_violations) + return _ExecutionReport( + False, + f"unintended object motion detected: {paths}", + { + "object_path": object_path, + "steps": report.steps, + "motion_audit": _motion_audit(report), + "collateral_motion_audit": collateral_audit, + "plan_audit": plan_audit, + }, + ) return _ExecutionReport( True, report.message, @@ -239,6 +375,9 @@ def _pick_and_place(self, action: CountermeasureAction) -> _ExecutionReport: "object_path": object_path, "steps": report.steps, "physical_state": str(report.state), + "motion_audit": _motion_audit(report), + "collateral_motion_audit": collateral_audit, + "plan_audit": plan_audit, }, ) @@ -246,11 +385,21 @@ def _execute_measurement_move(self, action: CountermeasureAction) -> _ExecutionR if self.measurement_controller is None: return _ExecutionReport(False, "measurement robot controller is not configured", {}) assert action.target_pose_world is not None - report = self.measurement_controller.navigate_to(action.target_pose_world[:3, 3]) + route = action.parameters.get("base_route_m") + report = ( + self.measurement_controller.navigate_route(route) + if route and hasattr(self.measurement_controller, "navigate_route") + else self.measurement_controller.navigate_to(action.target_pose_world[:3, 3]) + ) return _ExecutionReport( report.success, report.message, - {"steps": report.steps, "detector_path": action.target_prim_path}, + { + "steps": report.steps, + "detector_path": action.target_prim_path, + "base_route_m": route, + "motion_audit": _motion_audit(report), + }, ) def _execute_decontamination(self, action: CountermeasureAction) -> _ExecutionReport: @@ -258,11 +407,74 @@ def _execute_decontamination(self, action: CountermeasureAction) -> _ExecutionRe decontaminator = self.decontaminators.get(surface_path) if decontaminator is None: return _ExecutionReport(False, f"no contact decontaminator for {surface_path}", {}) + collateral_before = self._collateral_positions() + stow = None + if self.controller is not None and hasattr(self.controller, "stow_arm"): + stow = self.controller.stow_arm() + if not stow.success: + return _ExecutionReport( + False, + stow.message, + {"steps": stow.steps, "stow_audit": _motion_audit(stow)}, + ) + navigation = None if self.controller is not None and "pickup_base_position_m" in action.parameters: - navigation = self.controller.navigate_to(action.parameters["pickup_base_position_m"]) + route = action.parameters.get("pickup_base_route_m") + navigation = ( + self.controller.navigate_route( + route, + action.parameters.get("pickup_base_yaw_rad"), + ) + if route and hasattr(self.controller, "navigate_route") + else self.controller.navigate_to(action.parameters["pickup_base_position_m"]) + ) if not navigation.success: - return _ExecutionReport(False, navigation.message, {"steps": navigation.steps}) + return _ExecutionReport( + False, + navigation.message, + { + "steps": navigation.steps + (0 if stow is None else stow.steps), + "stow_audit": None if stow is None else _motion_audit(stow), + "navigation_audit": _motion_audit(navigation), + "base_route_m": route, + }, + ) duration = float(action.parameters.get("duration_s", action.predicted_duration_s)) + if self.controller is not None and hasattr( + self.controller, "execute_surface_decontamination" + ): + report = self.controller.execute_surface_decontamination( + decontaminator, + duration, + ) + digest = decontaminator.flush() + self.simulation.refresh_scene_state() + collateral_audit, collateral_violations = self._collateral_motion_audit( + collateral_before + ) + success = bool(report.success) and not collateral_violations + return _ExecutionReport( + success, + "articulated contact decontamination completed" + if success + else ( + "unintended object motion detected during decontamination" + if collateral_violations + else "articulated contact decontamination failed" + ), + { + "surface_path": surface_path, + "accepted_contacts": int(report.accepted_contacts), + "removed_activity_bq": float(report.removed_activity_bq), + "activity_map_sha256": digest, + "stow_audit": None if stow is None else _motion_audit(stow), + "navigation_audit": ( + None if navigation is None else _motion_audit(navigation) + ), + "motion_audit": _motion_audit(report), + "collateral_motion_audit": collateral_audit, + }, + ) ticks = max(1, int(math.ceil(duration / self.physics_dt_s))) removed = 0.0 contacts = 0 @@ -301,6 +513,57 @@ def _physical_execute(self, action: CountermeasureAction) -> _ExecutionReport: return self._pick_and_place(action) return _ExecutionReport(False, f"unsupported action type: {action.action_type}", {}) + def _update_shield_deployment_metadata( + self, action: CountermeasureAction + ) -> Mapping[str, object]: + """Commit public shield state only after physical placement succeeds.""" + + if action.action_type not in {ActionType.PLACE_SHIELD, ActionType.MOVE_SHIELD}: + return {} + shield_path = str(action.target_prim_path or "") + if not shield_path: + raise RuntimeError("a successful shield action has no target prim path") + placement_fraction = action.parameters.get("placement_fraction") + if not isinstance(placement_fraction, (int, float)): + raise RuntimeError("a successful shield action has no placement fraction") + placement_fraction = float(placement_fraction) + if not math.isfinite(placement_fraction) or not 0.0 <= placement_fraction <= 1.0: + raise RuntimeError("shield placement fraction must be finite and in [0, 1]") + + stage = getattr(self.candidate_generator, "stage", None) + if stage is None: + stage = getattr(self.controller, "stage", None) + if stage is None: + raise RuntimeError("shield deployment metadata requires a live USD stage") + shield = stage.GetPrimAtPath(shield_path) + if not shield or not shield.IsValid(): + raise RuntimeError(f"shield prim is unavailable after placement: {shield_path}") + + deployed_attribute = shield.GetAttribute("rad:shield:deployed") + fraction_attribute = shield.GetAttribute("rad:shield:placementFraction") + if ( + not deployed_attribute + or not deployed_attribute.IsValid() + or not fraction_attribute + or not fraction_attribute.IsValid() + ): + from pxr import Sdf + + if not deployed_attribute or not deployed_attribute.IsValid(): + deployed_attribute = shield.CreateAttribute( + "rad:shield:deployed", Sdf.ValueTypeNames.Bool, custom=True + ) + deployed_attribute.Set(True) + if not fraction_attribute or not fraction_attribute.IsValid(): + fraction_attribute = shield.CreateAttribute( + "rad:shield:placementFraction", Sdf.ValueTypeNames.Double, custom=True + ) + fraction_attribute.Set(placement_fraction) + return { + "deployment_state": "deployed", + "placement_fraction": placement_fraction, + } + def _consume(self, action: CountermeasureAction) -> None: self.resources.consume(action.resource_cost) runtime = self.resources.remaining_robot_runtime_s.get(action.robot_id) @@ -318,7 +581,9 @@ def _consume(self, action: CountermeasureAction) -> None: self.resources.remaining_countermeasure_count = max( 0, self.resources.remaining_countermeasure_count - 1 ) - if action.action_type in {ActionType.PLACE_SHIELD, ActionType.MOVE_SHIELD}: + # Deployment consumes one inventory unit. Repositioning an already + # deployed physical panel consumes time, but never another panel. + if action.action_type == ActionType.PLACE_SHIELD: shield_type = str(action.parameters.get("shield_type", "default")) if shield_type in self.resources.remaining_shield_units: units = int(action.parameters.get("shield_units", 1)) @@ -357,11 +622,16 @@ async def execute(self, action: CountermeasureAction) -> ActionResult: except Exception as exc: report = _ExecutionReport(False, f"{type(exc).__name__}: {exc}", {}) if report.success: + shield_state = self._update_shield_deployment_metadata(action) self._consume(action) self._bump_revision(action) changed = self.simulation.synchronize() status = ActionStatus.COMPLETED - details = {**report.public_details, "changed_paths": list(changed)} + details = { + **report.public_details, + **shield_state, + "changed_paths": list(changed), + } else: status = ActionStatus.FAILED details = dict(report.public_details) diff --git a/tests/isaac/articulated_object_gate.py b/tests/isaac/articulated_object_gate.py new file mode 100644 index 0000000..c392659 --- /dev/null +++ b/tests/isaac/articulated_object_gate.py @@ -0,0 +1,126 @@ +"""Live gate for generic Franka object and obstacle IK approach poses.""" + +from __future__ import annotations + +import importlib +import json +import shutil +import sys +import tempfile +from dataclasses import replace +from pathlib import Path + +import numpy as np + +ROOT = Path(__file__).resolve().parents[2] +EXTENSION = ROOT / "source/extensions/radcounter.isaac" +for path in (ROOT, EXTENSION): + if str(path) not in sys.path: + sys.path.insert(0, str(path)) +_radcounter_package = importlib.import_module("radcounter") +_extension_namespace = str(EXTENSION / "radcounter") +if _extension_namespace not in _radcounter_package.__path__: + _radcounter_package.__path__.append(_extension_namespace) + + +class _Stepper: + def __init__(self, world) -> None: + self.world = world + + def step(self, *, render: bool = False) -> None: + self.world.step(render=render) + + +def _position(stage, path: str) -> np.ndarray: + from pxr import Gf, UsdGeom + + prim = stage.GetPrimAtPath(path) + matrix = UsdGeom.XformCache().GetLocalToWorldTransform(prim) + return np.asarray(matrix.Transform(Gf.Vec3d()), dtype=np.float64) + + +def main() -> int: + from isaacsim import SimulationApp + + app = SimulationApp({"headless": True}) + temporary_root = Path(tempfile.mkdtemp(prefix="radcounter-object-gate-")) + try: + import omni.usd + from isaacsim.core.api import World + from isaacsim.core.prims import SingleArticulation + from radcounter.isaac.robot import ( + RealRobotAssetConfig, + RidgebackFrankaController, + add_real_robot_references, + author_real_robot_task_scene, + create_decontamination_activity_map, + enable_real_robot_extensions, + ) + + enable_real_robot_extensions() + for _ in range(20): + app.update() + context = omni.usd.get_context() + assert context.open_stage( + str(ROOT / "assets/environments/radcounter_vertical_slice.usda") + ) + for _ in range(20): + app.update() + stage = context.get_stage() + config = replace( + RealRobotAssetConfig(), + decon_workbench_center_m=(14.39, 0.80, 1.15), + shield_initial_position_m=(4.80, 2.80, 0.0), + ) + add_real_robot_references(stage, config=config) + for _ in range(120): + app.update() + activity = create_decontamination_activity_map( + temporary_root / "activity.npz" + ) + author_real_robot_task_scene(stage, activity, config=config) + world = World(stage_units_in_meters=1.0, physics_dt=1.0 / 60.0) + articulation = world.scene.add( + SingleArticulation(config.countermeasure_root, name="object_gate_franka") + ) + world.reset() + for _ in range(90): + world.step(render=False) + controller = RidgebackFrankaController( + stage, + _Stepper(world), + config=config, + articulation=articulation, + ) + audit = {} + for object_path in ( + "/World/HiddenContaminatedDrum", + "/World/MovableObstacle", + ): + target = stage.GetPrimAtPath(object_path) + frame_name = str(target.GetAttribute("rad:manipulation:graspFrame").Get()) + grasp = _position(stage, f"{object_path}/{frame_name}") + root = _position(stage, object_path) + stow = controller.stow_arm() + assert stow.success, stow + assert controller.move_base((grasp[0] - 0.90, grasp[1], 0.0)) + approach = controller.move_hand(grasp + np.asarray((0.0, 0.0, 0.14))) + root_after_approach = _position(stage, object_path) + approach_displacement_m = float(np.linalg.norm(root_after_approach - root)) + audit[object_path] = { + "root_m": root.tolist(), + "grasp_m": grasp.tolist(), + "approach": approach, + "approach_displacement_m": approach_displacement_m, + } + assert approach.success, audit[object_path] + assert approach_displacement_m <= 0.03, audit[object_path] + print(json.dumps(audit, default=str), flush=True) + return 0 + finally: + app.close() + shutil.rmtree(temporary_root) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/isaac/fukushima_profile_gate.py b/tests/isaac/fukushima_profile_gate.py new file mode 100644 index 0000000..c1757e9 --- /dev/null +++ b/tests/isaac/fukushima_profile_gate.py @@ -0,0 +1,103 @@ +"""Live Kit gate for the optional Fukushima CAD system profile.""" + +from __future__ import annotations + +import json +import os +import sys +import traceback +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +sys.path[:0] = [ + str(ROOT), + str(ROOT / "source/extensions/radcounter.isaac"), + str(ROOT / "build/native/python"), +] + + +def main() -> int: + from isaacsim import SimulationApp + + app = SimulationApp({"headless": True}) + exit_code = 0 + try: + import omni.usd + from pxr import Gf, Usd, UsdGeom + from radcounter.isaac.runtime import IsaacRadiationSimulation + from radcounter.isaac.system_profile import ( + compose_selected_system, + prepare_environment_stage, + ) + + from radcounter.core.system_profiles import resolve_system_selection + + selection = resolve_system_selection(profile_id="fukushima-packbot") + assert selection.environment_ready, selection.environment_source_path + stage_path, environment_manifest = prepare_environment_stage(selection) + context = omni.usd.get_context() + assert context.open_stage(str(stage_path)) + for _ in range(12): + app.update() + stage = context.get_stage() + composed = compose_selected_system(stage, selection, stage_path=stage_path) + simulation = IsaacRadiationSimulation.from_config(stage, composed.runtime_config_path) + mesh_count = sum(prim.IsA(UsdGeom.Mesh) for prim in stage.Traverse()) + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + def world_position(path: str) -> list[float]: + matrix = xform_cache.GetLocalToWorldTransform(stage.GetPrimAtPath(path)) + return [float(value) for value in matrix.Transform(Gf.Vec3d())] + + packbot_position = world_position(composed.robot_paths["packbot"]) + elios_position = world_position(composed.robot_paths["elios3"]) + environment_range = ( + UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_]) + .ComputeWorldBound(stage.GetPrimAtPath("/World/Environment")) + .ComputeAlignedRange() + ) + + assert mesh_count >= 995 + assert set(composed.robot_paths) == {"packbot", "elios3"} + assert set(composed.detector_paths) == {"packbot-gm", "elios-imager"} + assert len(simulation.detectors) == 2 + assert len(simulation.transport.geometry_paths) >= 995 + assert packbot_position == list(selection.spawn_anchor("ground-primary").translation_m) + assert elios_position == list(selection.spawn_anchor("aerial-primary").translation_m) + assert not stage.GetPrimAtPath("/World/RemoteDeconFacility").IsValid() + os.write( + 1, + ( + json.dumps( + { + "profile": selection.profile_id, + "stage": str(stage_path), + "environment_manifest": str(environment_manifest), + "meshes": mesh_count, + "robots": sorted(composed.robot_paths), + "detectors": sorted(composed.detector_paths), + "transport_geometries": len(simulation.transport.geometry_paths), + "robot_positions_m": { + "packbot": packbot_position, + "elios3": elios_position, + }, + "environment_bounds_m": { + "minimum": [float(value) for value in environment_range.GetMin()], + "maximum": [float(value) for value in environment_range.GetMax()], + }, + "separate_validation_facility_absent": True, + } + ) + + "\n" + ).encode(), + ) + except BaseException: + os.write(2, traceback.format_exc().encode()) + exit_code = 1 + finally: + app.close() + return exit_code + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/unit/test_english_only_repository.py b/tests/unit/test_english_only_repository.py new file mode 100644 index 0000000..fc5172b --- /dev/null +++ b/tests/unit/test_english_only_repository.py @@ -0,0 +1,33 @@ +from __future__ import annotations + +import re +import subprocess +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +_CJK_TEXT = re.compile(r"[\u3000-\u30ff\u3400-\u4dbf\u4e00-\u9fff\uf900-\ufaff\uff00-\uffef]") + + +def test_all_tracked_text_is_english_only() -> None: + tracked = subprocess.run( + ["git", "ls-files", "-z"], + cwd=ROOT, + check=True, + capture_output=True, + ).stdout.split(b"\0") + violations: list[str] = [] + for encoded_path in tracked: + if not encoded_path: + continue + relative_path = encoded_path.decode("utf-8") + data = (ROOT / relative_path).read_bytes() + if b"\0" in data: + continue + try: + text = data.decode("utf-8") + except UnicodeDecodeError: + continue + for line_number, line in enumerate(text.splitlines(), start=1): + if _CJK_TEXT.search(line): + violations.append(f"{relative_path}:{line_number}") + assert not violations, "Non-English CJK text found in:\n" + "\n".join(violations) diff --git a/tests/unit/test_gui_complex_validation.py b/tests/unit/test_gui_complex_validation.py new file mode 100644 index 0000000..63a8e87 --- /dev/null +++ b/tests/unit/test_gui_complex_validation.py @@ -0,0 +1,401 @@ +from __future__ import annotations + +from copy import deepcopy + +import pytest + +from radcounter.core.natural_language import ( + AvailableAction, + CommandContext, + CommandName, + CommandPlan, + CommandStep, +) +from radcounter.core.natural_language.client import _normalize_plan +from scripts.run_gui_validation import ( + DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION, + DEFAULT_GUI_MAX_FPS, + _arguments, + _complex_process_audit, + _GuiFrameRateLimiter, +) + +SURFACE = "/World/RemoteDeconFacility/DeconWorkSurface" +SHIELD = "/World/LeadShield" +PROTECTED = "/World/DetectorStations/Protected" + + +def _environment(activity_bq: float, *, deployed: bool) -> dict[str, object]: + return { + "failed_invariants": [], + "surface_source": {"map_activity_bq": activity_bq}, + "shields": [ + { + "path": SHIELD, + "deployed": deployed, + "placement_fraction": 0.65 if deployed else None, + }, + { + "path": "/World/StagingLeadShield", + "deployed": False, + "placement_fraction": None, + }, + ], + } + + +def _workflow_row(step: int, **values: object) -> dict[str, object]: + return { + "workflow_step": step, + "attempt": 1, + "max_attempts": 1, + **values, + } + + +def _decon_row( + attempt: int, + before_bq: float, + after_bq: float, + observed: float, + *, + condition_met: bool, +) -> dict[str, object]: + return { + "command": "execute_candidate", + "action_id": "decon-irregular-wall", + "action_type": "decontaminate", + "status": "completed", + "public_details": { + "surface_path": SURFACE, + "accepted_contacts": 120, + "motion_audit": { + "success": True, + "activity_before_bq": before_bq, + "activity_after_bq": after_bq, + "coverage_fraction": 0.94, + }, + "collateral_motion_audit": [ + {"object_path": "/World/MovableObstacle", "displacement_m": 0.002} + ], + }, + "workflow_step": 1, + "attempt": attempt, + "max_attempts": 3, + "completion_condition": { + "criterion": "decontamination_remaining_fraction_at_most", + "threshold": 0.60, + "observed": observed, + "met": condition_met, + }, + } + + +def _passing_payload() -> tuple[dict[str, object], list[dict[str, object]]]: + plan: dict[str, object] = { + "language": "en", + "summary": "Combined decontamination, shielding, and measurement process", + "steps": [ + { + "command": "execute_candidate", + "candidate_id": "decon-irregular-wall", + "max_attempts": 3, + "until": { + "criterion": "decontamination_remaining_fraction_at_most", + "threshold": 0.60, + }, + }, + { + "command": "execute_candidate", + "candidate_id": "shield-world-leadshield-25", + "max_attempts": 1, + }, + { + "command": "execute_candidate", + "candidate_id": "shield-world-leadshield-65", + "max_attempts": 1, + }, + { + "command": "execute_candidate", + "candidate_id": "measure-world-detectorstations-protected", + "max_attempts": 1, + }, + {"command": "measure", "duration_s": 2.0, "max_attempts": 1}, + {"command": "return_measurement_robot", "max_attempts": 1}, + {"command": "show_status", "max_attempts": 1}, + ], + } + collateral = [{"object_path": "/World/MovableObstacle", "displacement_m": 0.003}] + results = [ + _decon_row(1, 100.0, 79.0, 0.79, condition_met=False), + _decon_row(2, 79.0, 63.0, 0.63, condition_met=False), + _decon_row(3, 63.0, 50.0, 0.50, condition_met=True), + _workflow_row( + 2, + command="execute_candidate", + action_id="shield-world-leadshield-25", + action_type="place_shield", + status="completed", + public_details={ + "object_path": SHIELD, + "deployment_state": "deployed", + "placement_fraction": 0.25, + "motion_audit": {"success": True, "placement_error_m": 0.01}, + "collateral_motion_audit": collateral, + }, + ), + _workflow_row( + 3, + command="execute_candidate", + action_id="shield-world-leadshield-65", + action_type="move_shield", + status="completed", + public_details={ + "object_path": SHIELD, + "deployment_state": "deployed", + "placement_fraction": 0.65, + "motion_audit": {"success": True, "placement_error_m": 0.01}, + "collateral_motion_audit": collateral, + }, + ), + _workflow_row( + 4, + command="execute_candidate", + action_id="measure-world-detectorstations-protected", + action_type="measure", + status="completed", + public_details={ + "detector_path": PROTECTED, + "motion_audit": {"success": True, "displacement_m": 4.0}, + }, + ), + _workflow_row( + 5, + command="measure", + duration_s=2.0, + detector_count=1, + measurements=[ + { + "detector_path": PROTECTED, + "duration_s": 2.0, + "counts": 42, + "measured_rate_cps": 21.0, + } + ], + status="Integrated 1 detector", + ), + _workflow_row( + 6, + command="return_measurement_robot", + status="completed", + public_details={"motion_audit": {"success": True, "displacement_m": 4.0}}, + ), + _workflow_row(7, command="show_status", status="ready"), + ] + return plan, results + + +def _audit(plan: dict[str, object], results: list[dict[str, object]]) -> dict[str, object]: + return _complex_process_audit( + plan, + results, + _environment(100.0, deployed=False), + _environment(50.0, deployed=True), + { + "measurement_robot_initial_position_m": [0.0, 0.0, 0.0], + "measurement_robot_final_position_m": [0.02, 0.01, 0.0], + "primary_shield_initial_position_m": [4.8, 2.8, 0.0], + "primary_shield_final_position_m": [1.8, 0.8, 0.0], + }, + expected_surface_path=SURFACE, + ) + + +def test_complex_validation_cli_requires_visible_separate_mode() -> None: + args = _arguments(["--complex-natural-language-validation", "--no-keep-open"]) + assert args.complex_natural_language_validation is True + assert args.keep_open is False + assert args.headless is False + assert args.natural_language_timeout_s == 1800.0 + assert args.max_fps == DEFAULT_GUI_MAX_FPS + + with pytest.raises(SystemExit): + _arguments(["--complex-natural-language-validation", "--headless"]) + with pytest.raises(SystemExit): + _arguments(["--complex-natural-language-validation", "--interactive"]) + + +def test_gui_fps_limit_is_configurable_and_rejects_negative_values() -> None: + assert _arguments(["--max-fps", "30"]).max_fps == 30.0 + assert _arguments(["--max-fps", "0"]).max_fps == 0.0 + with pytest.raises(SystemExit): + _arguments(["--max-fps", "-1"]) + with pytest.raises(SystemExit): + _arguments(["--max-fps", "nan"]) + + +def test_gui_frame_limiter_accounts_for_update_time() -> None: + class FakeClock: + def __init__(self) -> None: + self.now = 0.0 + self.sleeps: list[float] = [] + + def __call__(self) -> float: + return self.now + + def sleep(self, duration_s: float) -> None: + self.sleeps.append(duration_s) + self.now += duration_s + + clock = FakeClock() + limiter = _GuiFrameRateLimiter(50.0, clock=clock, sleeper=clock.sleep) + clock.now += 0.006 + limiter.wait() + assert clock.sleeps == pytest.approx([0.014]) + + clock.now += 0.025 + limiter.wait() + assert clock.sleeps == pytest.approx([0.014]) + + unlimited = _GuiFrameRateLimiter(0.0, clock=clock, sleeper=clock.sleep) + unlimited.wait() + assert clock.sleeps == pytest.approx([0.014]) + + +def test_default_instruction_requests_achievable_bounded_complex_process() -> None: + assert "irregular wall-mounted Cs-137 surface source" in ( + DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + ) + assert "remaining fraction is no more than 60%" in ( + DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + ) + assert "at most 3 passes" in DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + assert "LeadShield" in DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + assert "25%" in DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + assert "65%" in DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + assert "Protected" in DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + assert "measure for 2 seconds" in DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION + + +def test_default_instruction_repairs_small_model_refusal_to_primary_shield_process() -> None: + context = CommandContext( + session_state="ready", + available_actions=( + AvailableAction( + action_id="decon-irregular-wall", + action_type="decontaminate", + label="Irregular wall full raster", + target=SURFACE, + ), + *tuple( + AvailableAction( + action_id=f"shield-world-leadshield-{percentage}", + action_type="place_shield", + label=f"LeadShield {percentage}%", + target=SHIELD, + target_label="LeadShield", + placement_fraction=fraction, + ) + for percentage, fraction in ((25, 0.25), (65, 0.65)) + ), + *tuple( + AvailableAction( + action_id=f"shield-world-stagingleadshield-{percentage}", + action_type="place_shield", + label=f"Staged shield {percentage}%", + target="/World/StagingLeadShield", + target_label="Staged lead service shield 02", + placement_fraction=fraction, + ) + for percentage, fraction in ((25, 0.25), (65, 0.65)) + ), + AvailableAction( + action_id="measure-world-detectorstations-protected", + action_type="measure", + label="Move to Protected", + target=PROTECTED, + target_label="Protected", + ), + ), + capabilities=( + CommandName.EXECUTE_CANDIDATE, + CommandName.MEASURE, + CommandName.RETURN_MEASUREMENT_ROBOT, + CommandName.SHOW_STATUS, + ), + ) + refusal = CommandPlan( + language="en", + summary="Show status only", + steps=(CommandStep(command=CommandName.SHOW_STATUS),), + ) + repaired = _normalize_plan( + refusal, + DEFAULT_COMPLEX_NATURAL_LANGUAGE_INSTRUCTION, + context, + ) + assert [step.command for step in repaired.steps] == [ + CommandName.EXECUTE_CANDIDATE, + CommandName.EXECUTE_CANDIDATE, + CommandName.EXECUTE_CANDIDATE, + CommandName.EXECUTE_CANDIDATE, + CommandName.MEASURE, + CommandName.RETURN_MEASUREMENT_ROBOT, + CommandName.SHOW_STATUS, + ] + assert repaired.steps[0].max_attempts == 3 + assert repaired.steps[0].until is not None + assert repaired.steps[0].until.threshold == pytest.approx(0.60) + assert [step.candidate_id for step in repaired.steps[1:3]] == [ + "shield-world-leadshield-25", + "shield-world-leadshield-65", + ] + assert repaired.steps[4].duration_s == pytest.approx(2.0) + + +def test_complex_process_audit_accepts_ordered_bounded_public_results() -> None: + plan, results = _passing_payload() + audit = _audit(plan, results) + assert audit["failed_invariants"] == [] + assert audit["decontamination"]["executed_attempts"] == 3 + assert audit["collateral_motion"]["maximum_displacement_m"] == pytest.approx(0.003) + + +@pytest.mark.parametrize( + ("mutation", "failed_invariant"), + [ + ( + lambda rows: rows[4]["public_details"].update(object_path="/World/StagingLeadShield"), + "same_primary_shield_placed_at_25_then_moved_to_65", + ), + ( + lambda rows: rows[4]["public_details"].update(placement_fraction=0.50), + "same_primary_shield_placed_at_25_then_moved_to_65", + ), + ( + lambda rows: rows[5]["public_details"].update( + detector_path="/World/DetectorStations/RemoteDeconRoom" + ), + "protected_navigation_completed", + ), + ( + lambda rows: rows[6].update(duration_s=3.0), + "two_second_measurement_completed", + ), + ( + lambda rows: rows[3]["public_details"]["collateral_motion_audit"][0].update( + displacement_m=0.050001 + ), + "collateral_motion_audited_and_bounded", + ), + ], +) +def test_complex_process_audit_rejects_wrong_target_or_collateral_motion( + mutation: object, + failed_invariant: str, +) -> None: + plan, results = _passing_payload() + mutated = deepcopy(results) + mutation(mutated) + audit = _audit(plan, mutated) + assert failed_invariant in audit["failed_invariants"] diff --git a/tests/unit/test_isaac_extension_artifacts.py b/tests/unit/test_isaac_extension_artifacts.py index 73ffe77..f076078 100644 --- a/tests/unit/test_isaac_extension_artifacts.py +++ b/tests/unit/test_isaac_extension_artifacts.py @@ -9,6 +9,9 @@ def test_isaac_extension_declares_direct_host_dependencies() -> None: assert config["python"]["module"][0]["name"] == "radcounter.isaac" assert set(config["dependencies"]) == { "omni.kit.uiapp", + "omni.ui.scene", + "omni.kit.viewport.utility", + "omni.kit.viewport.window", "omni.appwindow", "omni.usd", "omni.timeline", diff --git a/tests/unit/test_isaac_runtime_transport.py b/tests/unit/test_isaac_runtime_transport.py new file mode 100644 index 0000000..38b0aaa --- /dev/null +++ b/tests/unit/test_isaac_runtime_transport.py @@ -0,0 +1,43 @@ +import importlib +from pathlib import Path + +import numpy as np +import pytest + +import radcounter + +ROOT = Path(__file__).resolve().parents[2] +EXTENSION_NAMESPACE = str(ROOT / "source/extensions/radcounter.isaac/radcounter") +if EXTENSION_NAMESPACE not in radcounter.__path__: + radcounter.__path__.append(EXTENSION_NAMESPACE) + +_ensure_material_columns = importlib.import_module( + "radcounter.isaac.runtime.simulation" +)._ensure_material_columns +_usd_row_matrix_values = importlib.import_module( + "radcounter.isaac.usd.environment" +)._usd_row_matrix_values + + +def test_absent_configured_materials_are_zero_padded() -> None: + paths = _ensure_material_columns(np.asarray([[0.4, 0.2]]), material_count=3) + np.testing.assert_allclose(paths, [[0.4, 0.2, 0.0]]) + + +def test_native_scene_cannot_return_more_materials_than_configured() -> None: + with pytest.raises(ValueError, match="4 material columns"): + _ensure_material_columns(np.ones((2, 4)), material_count=3) + + +def test_native_usd_reference_transposes_column_vector_transform() -> None: + matrix = np.asarray( + [ + [0.0, -0.001, 0.0, -35.0], + [0.001, 0.0, 0.0, 2.0], + [0.0, 0.0, 0.001, 3.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) + rows = np.asarray(_usd_row_matrix_values(matrix)) + np.testing.assert_allclose(rows, matrix.T) + np.testing.assert_allclose(rows[3, :3], (-35.0, 2.0, 3.0)) diff --git a/tests/unit/test_natural_language.py b/tests/unit/test_natural_language.py new file mode 100644 index 0000000..d513dd1 --- /dev/null +++ b/tests/unit/test_natural_language.py @@ -0,0 +1,940 @@ +from __future__ import annotations + +import json +import threading +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path + +import pytest +from pydantic import ValidationError + +from radcounter.core.natural_language import ( + AvailableAction, + CommandContext, + CommandName, + CommandPlan, + CommandStep, + CompletionCriterion, + LlamaCppRuntime, + LlamaCppRuntimeConfig, + OpenAICompatibleCommandInterpreter, + PlanValidationError, + RuntimeAssetMissingError, + StepCompletionCondition, + validate_command_plan, +) +from radcounter.core.natural_language.client import ( + _inlined_command_schema, + _normalize_plan, + normalize_operator_instruction, +) +from radcounter.core.natural_language.runtime import NvidiaGpu + +ROOT = Path(__file__).resolve().parents[2] + + +def _controller_class(): + import radcounter + + namespace = str(ROOT / "source/extensions/radcounter.isaac/radcounter") + if namespace not in radcounter.__path__: + radcounter.__path__.append(namespace) + from radcounter.isaac.natural_language import NaturalLanguageCommandController + + return NaturalLanguageCommandController + + +def _context() -> CommandContext: + return CommandContext( + session_state="ready", + stage_name="vertical_slice", + available_actions=( + AvailableAction( + action_id="measure-protected", + action_type="measure", + label="Measure at Protected station", + target="/World/DetectorStations/Protected", + ), + AvailableAction( + action_id="remove-drum", + action_type="remove_object", + label="Move contaminated drum to disposal", + target="/World/HiddenContaminatedDrum", + feasible=False, + ), + ), + ) + + +def _shield_context() -> CommandContext: + return CommandContext( + session_state="ready", + stage_name="vertical_slice", + available_actions=( + AvailableAction( + action_id="shield-world-leadshield-35", + action_type="place_shield", + label="Place lead shield", + target="/World/LeadShield", + ), + AvailableAction( + action_id="shield-world-leadshield-50", + action_type="place_shield", + label="Place lead shield", + target="/World/LeadShield", + feasible=False, + ), + ), + ) + + +def _shield_clearance_context() -> CommandContext: + return CommandContext( + session_state="ready", + stage_name="vertical_slice", + available_actions=( + AvailableAction( + action_id="shield-world-leadshield-35", + action_type="place_shield", + label="Place lead shield at 35 percent", + feasible=False, + ), + AvailableAction( + action_id="shield-world-leadshield-65", + action_type="place_shield", + label="Place lead shield at 65 percent", + feasible=True, + ), + ), + ) + + +def test_command_plan_rejects_free_form_arguments() -> None: + normalized = CommandStep.model_validate( + {"command": "pause", "candidate_id": "measure-protected", "duration_s": 0.1} + ) + assert normalized.candidate_id is None + assert normalized.duration_s is None + with pytest.raises(ValidationError): + CommandPlan.model_validate( + { + "language": "en", + "summary": "Run arbitrary code", + "steps": [{"command": "run_python", "code": "print('unsafe')"}], + } + ) + + +def test_host_validation_resolves_scene_candidates_and_confirmation() -> None: + plan = CommandPlan( + language="en", + summary="Measure in the protected area", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + CommandStep(command=CommandName.MEASURE, duration_s=2.0), + ), + ) + validated = validate_command_plan(plan, _context()) + assert validated.requires_confirmation is True + assert validated.warnings == ("Physical action: Measure at Protected station",) + + +def test_host_validation_rejects_invented_or_infeasible_candidate() -> None: + for candidate_id in ("invented-action", "remove-drum"): + plan = CommandPlan( + language="en", + summary="Do it", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id=candidate_id, + ), + ), + ) + with pytest.raises(PlanValidationError): + validate_command_plan(plan, _context()) + + +def test_host_validation_allows_confirmed_sequential_physical_actions() -> None: + plan = CommandPlan( + language="en", + summary="Move, measure, and return", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + CommandStep(command=CommandName.MEASURE, duration_s=5.0), + CommandStep(command=CommandName.RETURN_MEASUREMENT_ROBOT), + ), + ) + validated = validate_command_plan(plan, _context()) + assert validated.requires_confirmation is True + assert validated.warnings == ( + "Physical action: Measure at Protected station", + "Physical action: return measurement robot to its starting position", + ) + + +def test_command_plan_supports_twenty_four_step_workflows_but_remains_bounded() -> None: + steps = tuple(CommandStep(command=CommandName.SHOW_STATUS) for _ in range(24)) + assert len(CommandPlan(language="en", summary="Inspect repeatedly", steps=steps).steps) == 24 + with pytest.raises(ValidationError): + CommandPlan(language="en", summary="Too long", steps=(*steps, steps[0])) + + +def test_bounded_workflow_rejects_more_than_forty_eight_executions() -> None: + context = CommandContext( + session_state="ready", + available_actions=( + AvailableAction( + action_id="decon-surface", + action_type="decontaminate", + label="Full serpentine surface decontamination", + ), + ), + ) + repeated = CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="decon-surface", + max_attempts=5, + ) + plan = CommandPlan(language="en", summary="Too many passes", steps=(repeated,) * 10) + with pytest.raises(PlanValidationError, match="48"): + validate_command_plan(plan, context) + + +def test_llama_schema_is_inlined_and_does_not_expose_truth() -> None: + encoded = json.dumps(_inlined_command_schema(), sort_keys=True).lower() + assert "$ref" not in encoded + assert "$defs" not in encoded + assert "truth" not in encoded + assert "execute_candidate" in encoded + + +def test_explicit_english_measurement_is_not_lost_after_navigation() -> None: + incomplete = CommandPlan( + language="en", + summary="Move to the station", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + ), + ) + concise = _normalize_plan( + incomplete, + "Move the measurement robot to the protected area and measure for 2 seconds", + _context(), + ) + assert concise.language == "en" + assert concise.steps[-1] == CommandStep(command=CommandName.MEASURE, duration_s=2.0) + + english = _normalize_plan( + incomplete, + "Move to the protected area and measure for 3 seconds", + _context(), + ) + assert english.language == "en" + assert english.steps[-1] == CommandStep(command=CommandName.MEASURE, duration_s=3.0) + + model_kept_measure_but_lost_duration = CommandPlan( + language="en", + summary="Move and measure", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + CommandStep(command=CommandName.MEASURE), + ), + ) + repaired_duration = _normalize_plan( + model_kept_measure_but_lost_duration, + "Move to the protected area and measure for 2 seconds", + _context(), + ) + assert repaired_duration.steps[-1].duration_s == 2.0 + + +def test_explicit_move_measure_return_is_repaired_to_three_ordered_steps() -> None: + small_model_plan = CommandPlan( + language="en", + summary="Move, measure, and return", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + CommandStep(command=CommandName.MEASURE), + # A small model can incorrectly encode "return" as another station move. + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + ), + ) + normalized = _normalize_plan( + small_model_plan, + ( + "Move the measurement robot to the protected area, measure the radiation " + "level for 5 seconds, then return it to its starting position" + ), + _context(), + ) + assert normalized.steps == ( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + CommandStep(command=CommandName.MEASURE, duration_s=5.0), + CommandStep(command=CommandName.RETURN_MEASUREMENT_ROBOT), + ) + + +def test_operator_instruction_collapses_only_triple_or_more_exact_repetitions() -> None: + instruction = "Move the measurement robot to the protected area and measure for 2 seconds." + assert normalize_operator_instruction(instruction * 3) == instruction + assert normalize_operator_instruction(instruction * 5) == instruction + assert normalize_operator_instruction(instruction * 2) == instruction * 2 + assert normalize_operator_instruction("Pause the simulation") == "Pause the simulation" + + +def test_operator_instruction_repairs_fuzzy_repetition_and_quote_marker() -> None: + instruction = ( + "Use the countermeasure robot's manipulator to pick up the lead shield, " + "place it between the source and protected area, then measure for 5 seconds " + "to > verify the effec" + ) + corrupted = instruction * 3 + instruction.replace(" > ", " ") + assert normalize_operator_instruction(corrupted) == instruction.replace(" > ", " ") + + +def test_explicit_shield_manipulation_repairs_small_model_refusal() -> None: + refusal = CommandPlan( + language="en", + summary="The requested placement lacks public coordinates.", + steps=(CommandStep(command=CommandName.SHOW_STATUS),), + ) + normalized = _normalize_plan( + refusal, + ( + "Use the countermeasure robot's manipulator to pick up the lead shield, " + "place it between the radiation source and the protected area, then measure " + "the radiation level for 5 seconds to verify the effect." + ), + _shield_context(), + ) + assert normalized.steps == ( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="shield-world-leadshield-35", + ), + CommandStep(command=CommandName.MEASURE, duration_s=5.0), + ) + assert "Place the lead shield" in normalized.summary + assert validate_command_plan(normalized, _shield_context()).requires_confirmation is True + + +def test_infeasible_shield_choice_is_replaced_by_host_verified_safe_candidate() -> None: + plan = CommandPlan( + language="en", + summary="Place the shield", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="shield-world-leadshield-35", + ), + ), + ) + normalized = _normalize_plan( + plan, + "Use the manipulator to place the lead shield between the source and protected area", + _shield_clearance_context(), + ) + assert normalized.steps[0].candidate_id == "shield-world-leadshield-65" + assert validate_command_plan(normalized, _shield_clearance_context()).requires_confirmation + + +def test_explicit_decontamination_repairs_small_model_refusal() -> None: + context = CommandContext( + session_state="ready", + stage_name="complex_work_cell", + available_actions=( + AvailableAction( + action_id="decon-world-deconworksurface", + action_type="decontaminate", + label="Decontaminate planar Cs-137 source", + target="/World/DeconWorkSurface", + ), + ), + ) + refusal = CommandPlan( + language="en", + summary="The target could not be identified", + steps=(CommandStep(command=CommandName.SHOW_STATUS),), + ) + normalized = _normalize_plan( + refusal, + "Avoid the obstacle, move to the contaminated work surface, and decontaminate " + "the entire surface source", + context, + ) + assert normalized.steps == ( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="decon-world-deconworksurface", + ), + ) + assert validate_command_plan(normalized, context).requires_confirmation + + +def test_decontamination_verification_collapses_redundant_small_model_steps() -> None: + context = CommandContext( + session_state="ready", + stage_name="complex_work_cell", + available_actions=( + AvailableAction( + action_id="decon-world-deconworksurface", + action_type="decontaminate", + label="Decontaminate planar source", + ), + AvailableAction( + action_id="measure-world-detectorstations-protected", + action_type="measure", + label="Move to protected detector", + ), + ), + ) + redundant = CommandPlan( + language="en", + summary="Decontaminate and measure", + steps=( + CommandStep(command=CommandName.SHOW_STATUS), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-world-detectorstations-protected", + ), + CommandStep(command=CommandName.MEASURE), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-world-detectorstations-protected", + ), + CommandStep(command=CommandName.SHOW_STATUS), + ), + ) + normalized = _normalize_plan( + redundant, + "Decontaminate the surface source, move to the protected area, and measure for 5 seconds", + context, + ) + assert normalized.steps == ( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="decon-world-deconworksurface", + ), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-world-detectorstations-protected", + ), + CommandStep(command=CommandName.MEASURE, duration_s=5.0), + ) + + +def test_complex_decon_measure_return_status_is_ordered_by_operator_semantics() -> None: + context = CommandContext( + session_state="ready", + stage_name="maze", + available_actions=( + AvailableAction( + action_id="decon-surface", + action_type="decontaminate", + label="Decontaminate surface", + ), + AvailableAction( + action_id="measure-protected", + action_type="measure", + label="Move to protected detector", + ), + AvailableAction( + action_id="measure-remote-decon-room", + action_type="measure", + label="Move to remote decontamination room", + ), + AvailableAction( + action_id="move-obstacle", + action_type="move_object", + label="Move unrelated obstacle", + ), + ), + ) + out_of_order = CommandPlan( + language="en", + summary="Combined workflow", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="move-obstacle", + ), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="move-obstacle", + ), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="decon-surface", + ), + CommandStep(command=CommandName.SHOW_STATUS), + CommandStep(command=CommandName.MEASURE), + CommandStep(command=CommandName.RETURN_MEASUREMENT_ROBOT), + ), + ) + normalized = _normalize_plan( + out_of_order, + "Decontaminate the surface source, then measure for 5 seconds in the remote " + "decontamination room, return to the starting position, and show status", + context, + ) + assert normalized.steps == ( + CommandStep(command=CommandName.EXECUTE_CANDIDATE, candidate_id="decon-surface"), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-remote-decon-room", + ), + CommandStep(command=CommandName.MEASURE, duration_s=5.0), + CommandStep(command=CommandName.RETURN_MEASUREMENT_ROBOT), + CommandStep(command=CommandName.SHOW_STATUS), + ) + + +def test_complex_bounded_decon_and_ordered_shield_reposition_are_repaired() -> None: + context = CommandContext( + session_state="ready", + stage_name="multi_room_decommissioning_facility", + available_actions=( + AvailableAction( + action_id="decon-wall-source", + action_type="decontaminate", + label="Full six-lane serpentine raster · irregular wall source", + target="/World/DeconWorkSurface", + execution_mode="full_coverage_serpentine_raster", + ), + AvailableAction( + action_id="shield-primary-35", + action_type="place_shield", + label="Place primary shield at 35 percent", + target="/World/LeadShield", + placement_fraction=0.35, + ), + AvailableAction( + action_id="shield-primary-65", + action_type="place_shield", + label="Place primary shield at 65 percent", + target="/World/LeadShield", + placement_fraction=0.65, + ), + AvailableAction( + action_id="measure-protected", + action_type="measure", + label="Move to protected detector", + target="/World/DetectorStations/Protected", + target_label="Protected area", + ), + ), + ) + refusal = CommandPlan( + language="en", + summary="The workflow is too complex to execute", + steps=(CommandStep(command=CommandName.SHOW_STATUS),), + ) + normalized = _normalize_plan( + refusal, + ( + "Decontaminate the irregular wall source up to three times until the removal " + "fraction reaches at least 70%, place the lead shield at 35% of the line from " + "the source to the protected area and then move it to 65%, move to the protected " + "area and measure for 5 seconds, return to the start, and show status" + ), + context, + ) + assert normalized.steps == ( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="decon-wall-source", + max_attempts=3, + until=StepCompletionCondition( + criterion=CompletionCriterion.DECONTAMINATION_REMOVED_FRACTION_AT_LEAST, + threshold=0.70, + ), + ), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="shield-primary-35", + ), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="shield-primary-65", + ), + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + CommandStep(command=CommandName.MEASURE, duration_s=5.0), + CommandStep(command=CommandName.RETURN_MEASUREMENT_ROBOT), + CommandStep(command=CommandName.SHOW_STATUS), + ) + validated = validate_command_plan(normalized, context) + assert validated.requires_confirmation + assert any("up to 3 attempts" in warning for warning in validated.warnings) + + +def test_all_station_instruction_expands_to_navigation_and_measurement_pairs() -> None: + context = CommandContext( + session_state="ready", + available_actions=tuple( + AvailableAction( + action_id=f"measure-{name}", + action_type="measure", + label=f"Move to {name}", + target=f"/World/DetectorStations/{name}", + ) + for name in ("north", "protected", "remote") + ), + ) + small_model = CommandPlan( + language="en", + summary="Measure", + steps=(CommandStep(command=CommandName.MEASURE),), + ) + normalized = _normalize_plan( + small_model, + "Visit every measurement station in order and measure for 2 seconds at each station", + context, + ) + assert len(normalized.steps) == 6 + assert [step.command for step in normalized.steps] == [ + CommandName.EXECUTE_CANDIDATE, + CommandName.MEASURE, + ] * 3 + assert all( + step.duration_s == 2.0 for step in normalized.steps if step.command == CommandName.MEASURE + ) + + +def test_builtin_command_misreported_as_candidate_is_normalized() -> None: + context = CommandContext( + session_state="playing", + capabilities=("pause", "show_status"), + ) + small_model_plan = CommandPlan( + language="en", + summary="Pause the simulation", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="pause", + ), + ), + ) + normalized = _normalize_plan(small_model_plan, "Pause the simulation", context) + assert normalized.steps == (CommandStep(command=CommandName.PAUSE),) + + +def test_openai_compatible_client_accepts_english_structured_plan() -> None: + requests: list[dict[str, object]] = [] + + class Handler(BaseHTTPRequestHandler): + def do_POST(self) -> None: # noqa: N802 + length = int(self.headers["Content-Length"]) + requests.append(json.loads(self.rfile.read(length))) + content = json.dumps( + { + "language": "en", + "summary": "Measure in the protected area", + "steps": [ + { + "command": "execute_candidate", + "candidate_id": "measure-protected", + }, + {"command": "measure", "duration_s": 2.0}, + ], + }, + ensure_ascii=False, + ) + body = json.dumps( + {"choices": [{"message": {"role": "assistant", "content": content}}]} + ).encode() + self.send_response(200) + self.send_header("Content-Type", "application/json") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + + def log_message(self, format: str, *args: object) -> None: + del format, args + + server = ThreadingHTTPServer(("127.0.0.1", 0), Handler) + thread = threading.Thread(target=server.serve_forever, daemon=True) + thread.start() + try: + interpreter = OpenAICompatibleCommandInterpreter(f"http://127.0.0.1:{server.server_port}") + import asyncio + + plan = asyncio.run( + interpreter.interpret( + "Move the measurement robot to the protected area and measure for 2 seconds", + _context(), + ) + ) + finally: + server.shutdown() + server.server_close() + thread.join(timeout=2) + assert plan.language == "en" + assert [step.command for step in plan.steps] == [ + CommandName.EXECUTE_CANDIDATE, + CommandName.MEASURE, + ] + response_format = requests[0]["response_format"] + assert isinstance(response_format, dict) + assert response_format["type"] == "json_schema" + assert "measurement robot" in str(requests[0]["messages"]) + + +def test_client_refuses_non_loopback_endpoint() -> None: + with pytest.raises(ValueError, match="loopback"): + OpenAICompatibleCommandInterpreter("https://example.com/v1") + + runtime = LlamaCppRuntime( + LlamaCppRuntimeConfig( + runtime_directory=ROOT / "runtime/llm", + external_endpoint="https://example.com", + ) + ) + with pytest.raises(ValueError, match="loopback"): + runtime.start() + + +def test_runtime_selects_packaged_cuda_and_cpu_binaries(tmp_path: Path) -> None: + runtime_root = tmp_path / "runtime" + cpu = runtime_root / "bin/linux-x86_64-cpu/llama-server" + cuda = runtime_root / "bin/linux-x86_64-cuda/llama-server" + vulkan = runtime_root / "bin/linux-x86_64-vulkan/llama-server" + model = runtime_root / "models/Qwen3-4B-Q4_K_M.gguf" + for path in (cpu, cuda, vulkan, model): + path.parent.mkdir(parents=True, exist_ok=True) + path.write_bytes(b"runtime-test") + cpu.chmod(0o755) + cuda.chmod(0o755) + vulkan.chmod(0o755) + + runtime = LlamaCppRuntime(LlamaCppRuntimeConfig(runtime_directory=runtime_root)) + runtime.gpu = NvidiaGpu("Test GPU", 10_240) + assert runtime.resolve_binary() == cuda + command = runtime._command(cuda, model, 12345) + assert command[-1] == "auto" + + cuda.unlink() + assert runtime.resolve_binary() == vulkan + + cpu_runtime = LlamaCppRuntime( + LlamaCppRuntimeConfig(runtime_directory=runtime_root, gpu_mode="cpu") + ) + cpu_runtime.gpu = NvidiaGpu("Test GPU", 10_240) + assert cpu_runtime.resolve_binary() == cpu + + runtime.gpu = None + assert runtime.resolve_binary() == cpu + assert runtime._command(cpu, model, 12345)[-1] == "0" + + +def test_runtime_reports_missing_product_assets(tmp_path: Path) -> None: + runtime = LlamaCppRuntime( + LlamaCppRuntimeConfig( + runtime_directory=tmp_path, + server_binary=tmp_path / "missing-llama-server", + ) + ) + with pytest.raises(RuntimeAssetMissingError, match="build_llama_runtime"): + runtime.resolve_binary() + + +def test_controller_auto_runs_safe_commands_and_confirms_physical_actions( + tmp_path: Path, +) -> None: + import asyncio + + class Interpreter: + def __init__(self) -> None: + self.plan = CommandPlan( + language="en", + summary="Pause", + steps=(CommandStep(command=CommandName.PAUSE),), + ) + + async def interpret(self, instruction: str, context: CommandContext) -> CommandPlan: + del instruction, context + return self.plan + + class Host: + def __init__(self) -> None: + self.steps: list[CommandStep] = [] + + def natural_language_context(self) -> CommandContext: + return _context() + + async def execute_natural_language_step(self, step: CommandStep) -> dict[str, object]: + self.steps.append(step) + return {"command": step.command.value, "ok": True} + + host = Host() + interpreter = Interpreter() + audit = tmp_path / "commands.jsonl" + NaturalLanguageCommandController = _controller_class() + controller = NaturalLanguageCommandController( + host, + interpreter=interpreter, + audit_path=audit, + ) + safe = asyncio.run(controller.submit("Pause the simulation")) + assert safe.executed is True + assert host.steps[-1].command == CommandName.PAUSE + + interpreter.plan = CommandPlan( + language="en", + summary="Move to the protected area", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="measure-protected", + ), + ), + ) + physical = asyncio.run(controller.submit("Move to the protected area")) + assert physical.executed is False + assert controller.pending is not None + assert host.steps[-1].command == CommandName.PAUSE + confirmed = asyncio.run(controller.confirm()) + assert confirmed.executed is True + assert host.steps[-1].candidate_id == "measure-protected" + + rows = [json.loads(line) for line in audit.read_text(encoding="utf-8").splitlines()] + assert [row["status"] for row in rows] == [ + "completed", + "awaiting_confirmation", + "completed", + ] + controller.shutdown() + + +def test_controller_stops_bounded_decontamination_when_public_condition_is_met( + tmp_path: Path, +) -> None: + import asyncio + + context = CommandContext( + session_state="ready", + available_actions=( + AvailableAction( + action_id="decon-irregular-wall", + action_type="decontaminate", + label="Full irregular-wall serpentine raster", + ), + ), + ) + condition = StepCompletionCondition( + criterion=CompletionCriterion.DECONTAMINATION_REMAINING_FRACTION_AT_MOST, + threshold=0.30, + ) + + class Interpreter: + async def interpret(self, instruction: str, supplied: CommandContext) -> CommandPlan: + del instruction, supplied + return CommandPlan( + language="en", + summary="Decontaminate up to five times until no more than 30% remains", + steps=( + CommandStep( + command=CommandName.EXECUTE_CANDIDATE, + candidate_id="decon-irregular-wall", + max_attempts=5, + until=condition, + ), + ), + ) + + class Host: + def __init__(self) -> None: + self.calls = 0 + + def natural_language_context(self) -> CommandContext: + return context + + async def execute_natural_language_step(self, step: CommandStep) -> dict[str, object]: + del step + before_after = ((100.0, 80.0), (80.0, 55.0), (55.0, 25.0)) + before, after = before_after[self.calls] + self.calls += 1 + return { + "command": "execute_candidate", + "public_details": { + "motion_audit": { + "activity_before_bq": before, + "activity_after_bq": after, + "removed_fraction": (before - after) / before, + "coverage_fraction": 0.94, + } + }, + } + + host = Host() + NaturalLanguageCommandController = _controller_class() + controller = NaturalLanguageCommandController( + host, + interpreter=Interpreter(), + audit_path=tmp_path / "bounded.jsonl", + ) + pending = asyncio.run( + controller.submit("Decontaminate up to five times until no more than 30% remains") + ) + assert pending.executed is False + completed = asyncio.run(controller.confirm()) + assert host.calls == 3 + assert len(completed.results) == 3 + assert completed.results[-1]["completion_condition"] == { + "criterion": "decontamination_remaining_fraction_at_most", + "threshold": 0.30, + "observed": 0.25, + "met": True, + } + controller.shutdown() + + +def test_product_runtime_and_command_surface_artifacts_exist() -> None: + dashboard = ( + ROOT / "source/extensions/radcounter.isaac/radcounter/isaac/ui/dashboard.py" + ).read_text(encoding="utf-8") + run_gui = (ROOT / "scripts/run_gui.py").read_text(encoding="utf-8") + run_gui_validation = (ROOT / "scripts/run_gui_validation.py").read_text(encoding="utf-8") + notices = (ROOT / "THIRD_PARTY_NOTICES.md").read_text(encoding="utf-8") + assert "NATURAL LANGUAGE COMMAND" in dashboard + assert "def bind_workflow" in dashboard + assert "execute_natural_language_step" in dashboard + assert '"--interactive"' in run_gui + assert '"extra_args"' not in run_gui_validation + assert "subscribe_value_changed_fn" in dashboard + assert "model=self._command_status" not in dashboard + assert "update_navigation_progress" in dashboard + assert "motion_audit" in dashboard + assert "llama.cpp" in notices + assert "Qwen3-4B" in notices + assert (ROOT / "scripts/build_llama_runtime.sh").is_file() + assert (ROOT / "scripts/fetch_llm_model.py").is_file() diff --git a/tests/unit/test_physx_telemetry.py b/tests/unit/test_physx_telemetry.py new file mode 100644 index 0000000..df56d22 --- /dev/null +++ b/tests/unit/test_physx_telemetry.py @@ -0,0 +1,80 @@ +# ruff: noqa: E402 +from __future__ import annotations + +import sys +from pathlib import Path + +import numpy as np + +import radcounter + +ROOT = Path(__file__).resolve().parents[2] +EXTENSION = ROOT / "source/extensions/radcounter.isaac" +sys.path.insert(0, str(EXTENSION)) +extension_namespace = str(EXTENSION / "radcounter") +if extension_namespace not in radcounter.__path__: + radcounter.__path__.append(extension_namespace) + +from radcounter.isaac.robot.physx_telemetry import PhysxManipulationTelemetry + + +class FakeArticulation: + def __init__(self) -> None: + self.names = {"joint_a": 1, "joint_b": 3} + metadata = type("Metadata", (), {"joint_indices": self.names})() + self._articulation_view = type("ArticulationView", (), {"_metadata": metadata})() + self.sample_index = 0 + + def get_dof_index(self, name: str) -> int: + return self.names.get(name, -1) + + def get_measured_joint_efforts(self) -> np.ndarray: + self.sample_index += 1 + scale = float(self.sample_index) + return np.asarray((0.0, -2.0 * scale, 0.0, 3.0 * scale)) + + def get_measured_joint_forces(self) -> np.ndarray: + reactions = np.zeros((5, 6), dtype=np.float64) + reactions[2, :3] = (3.0, 4.0, 0.0) + reactions[2, 3:] = (0.0, 0.0, 2.0) + reactions[4, :3] = (0.0, 0.0, 12.0) + reactions[4, 3:] = (0.0, 5.0, 12.0) + return reactions + + +class FakeContactView: + def get_contact_force_matrix(self, *, dt: float) -> np.ndarray: + assert dt == 0.02 + return np.asarray((((0.0, 0.0, 9.0),),)) + + def get_net_contact_forces(self, *, dt: float) -> np.ndarray: + assert dt == 0.02 + return np.asarray(((0.0, 0.0, 9.0),)) + + +def test_physx_telemetry_accumulates_effort_reaction_and_contact() -> None: + telemetry = PhysxManipulationTelemetry( + FakeArticulation(), + joint_names=("joint_a", "joint_b"), + rigid_contact_view=FakeContactView(), + physics_dt_s=0.02, + ) + telemetry.sample() + telemetry.sample() + summary = telemetry.summary() + assert summary.samples == 2 + assert summary.peak_abs_joint_effort_nm == (4.0, 6.0) + assert np.allclose(summary.rms_joint_effort_nm, (np.sqrt(10.0), np.sqrt(22.5))) + assert summary.peak_joint_reaction_force_n == (5.0, 12.0) + assert summary.peak_joint_reaction_torque_nm == (2.0, 13.0) + assert summary.peak_rigid_contact_force_n == 9.0 + assert summary.contact_samples == 2 + + +def test_physx_telemetry_rejects_unknown_joint() -> None: + try: + PhysxManipulationTelemetry(FakeArticulation(), joint_names=("missing",)) + except ValueError as error: + assert "must exist" in str(error) + else: + raise AssertionError("unknown joint should be rejected") diff --git a/tests/unit/test_real_robot_artifacts.py b/tests/unit/test_real_robot_artifacts.py index b963ca9..2e9e594 100644 --- a/tests/unit/test_real_robot_artifacts.py +++ b/tests/unit/test_real_robot_artifacts.py @@ -1,16 +1,31 @@ +import sys from pathlib import Path +from types import SimpleNamespace +import numpy as np import yaml +import radcounter + +ROOT = Path(__file__).resolve().parents[2] +EXTENSION = ROOT / "source/extensions/radcounter.isaac" +if str(EXTENSION) not in sys.path: + sys.path.insert(0, str(EXTENSION)) +extension_namespace = str(EXTENSION / "radcounter") +if extension_namespace not in radcounter.__path__: + radcounter.__path__.append(extension_namespace) + def test_real_robot_assets_and_motion_gate_are_declared() -> None: root = Path(__file__).resolve().parents[2] module = root / "source/extensions/radcounter.isaac/radcounter/isaac/robot/real_robots.py" runner = root / "scripts/run_real_robot_validation.py" gate = root / "tests/isaac/real_robot_gate.py" + object_gate = root / "tests/isaac/articulated_object_gate.py" assert module.is_file() assert runner.is_file() assert gate.is_file() + assert object_gate.is_file() source = module.read_text(encoding="utf-8") assert "RidgebackFranka/ridgeback_franka.usd" in source assert "NVIDIA/NovaCarter/nova_carter.usd" in source @@ -27,10 +42,447 @@ def test_robot_configs_select_real_isaac_controllers() -> None: measurement = yaml.safe_load( (root / "configs/robots/measurement_robot.yaml").read_text(encoding="utf-8") ) - assert countermeasure["reference_model_id"] == "mhi-meister" - assert countermeasure["controller"] == "mhi_meister_dual_arm_crawler" - assert countermeasure["arm_dofs"] == 14 - assert countermeasure["geometry_fidelity"] == "reference_procedural" - assert measurement["reference_model_id"] == "irobot-packbot-fukushima" - assert measurement["controller"] == "packbot_differential_crawler" - assert measurement["wheel_dof_names"] == ["left_track_joint", "right_track_joint"] + assert countermeasure["reference_model_id"] == "clearpath-ridgeback-franka" + assert countermeasure["controller"] == "ridgeback_franka_lula_ik" + assert countermeasure["arm_dofs"] == 7 + assert countermeasure["geometry_fidelity"] == "manufacturer_asset" + assert measurement["reference_model_id"] == "nvidia-nova-carter" + assert measurement["controller"] == "nova_carter_differential_wheels" + assert measurement["wheel_dof_names"] == ["joint_wheel_left", "joint_wheel_right"] + + +def test_gui_validation_uses_articulated_motion_without_tool_teleport() -> None: + root = Path(__file__).resolve().parents[2] + source = (root / "scripts/run_gui_validation.py").read_text(encoding="utf-8") + assert "RidgebackFrankaController" in source + assert "NovaCarterController" in source + assert "create_decontamination_activity_map" in source + assert "_position_contact_tool" not in source + assert "IsaacPhysicsRobotController" not in source + + controller = ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/robot/real_robots.py" + ).read_text(encoding="utf-8") + assert "_author_remote_decon_facility" in controller + assert "CorridorNorthWall" in controller + assert "DeconRoomEastWall" in controller + assert "Cs-137 wall-mounted planar contamination source" in controller + assert "irregular_deposition_field" in controller + assert "gaussian_lobes_correlated_roughness_holes_satellite_droplets" in controller + assert "surface.CreateFaceVertexCountsAttr([3] * (2 * len(active_cells)))" in controller + assert "z_rows = np.linspace" in controller + assert "float(np.max(np.abs(footprint[:, 1])))" in controller + assert "float(np.max(np.abs(footprint[:, 0])))" in controller + assert "required_coverage_fraction = 0.35" in controller + assert "max_waypoint_speed_m_s = 0.20" in controller + assert "compute_inverse_kinematics" in controller + assert "def execute_pick_and_place" in controller + assert "SetTargets([Sdf.Path(self.config.panda_hand_path)])" in controller + assert "def execute_surface_decontamination" in controller + assert "progress_callback" in controller + assert "OperationProgressCallback" in controller + assert 'phase="decontaminating"' in controller + assert "coverage_fraction=(" in controller + assert "path_m=planned_path" in controller + assert "def return_home" in controller + assert "def navigate_route" in controller + assert "initial_distance_m / 0.25 * 60.0" in controller + assert "destination_grasp = destination_root + grasp_offset" in controller + assert "yaw_delta = placement_yaw - pickup_yaw" not in controller + assert controller.count("self.set_gripper(0.016)") == 2 + assert "shield_grasp_offset_m: tuple[float, float, float] = (-0.24, 0.0, 0.59)" in controller + assert "handle_outer_x = cfg.shield_grasp_offset_m[0] - 0.04" in controller + assert "(0.0, -0.50, 0.25)" in controller + assert 'object_path + "/ManipulatorHandleStem"' in controller + assert '"lift_clearance"' in controller + assert '"lift_transport"' in controller + assert "retract_xy = pickup_base[:2] - grasp_position[:2]" in controller + assert "grasp_position + retract_world" in controller + assert "self._release_object(restore_collisions=False)" in controller + release_index = controller.index("self._release_object(restore_collisions=False)") + retract_index = controller.index("self.move_hand(preplace)", release_index) + restore_index = controller.index("self._restore_grasp_collisions()", retract_index) + assert release_index < retract_index < restore_index + + rules = (root / "docs/decontamination-authoring-rules.md").read_text(encoding="utf-8") + assert "Gaussian lobes plus correlated sinusoidal roughness" in rules + assert "continuous boustrophedon/serpentine raster" in rules + assert "Sample the full physical pad footprint densely enough" in rules + assert "ray count is spatial sampling density, not elapsed time" in rules + assert "planned sweep" in rules + assert "same irregular activity-bearing mesh" in rules + assert "Never show a regular rectangular coverage proxy" in rules + assert "np.linspace(-0.085, 0.085, 9)" in source + + contact_model = ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/robot/decontamination.py" + ).read_text(encoding="utf-8") + assert "effective_contact_exposure_s" in contact_model + assert "def _update_surface_visuals" in contact_model + assert "opacity_attr.Set([0.0 if value < 0.10" in contact_model + assert "count / footprint_count" not in contact_model + assert "implementation, regression tests" in (root / "AGENTS.md").read_text(encoding="utf-8") + + workflow = ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/workflow/services.py" + ).read_text(encoding="utf-8") + assert '"motion_audit": _motion_audit(report)' in workflow + assert "collateral_motion_audit" in workflow + assert '"pickup_base_route_m"' in ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py" + ).read_text(encoding="utf-8") + assert '"base_route_m"' in ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py" + ).read_text(encoding="utf-8") + assert "carried_object_path" in ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py" + ).read_text(encoding="utf-8") + assert "live_candidates = generator.generate_measurement_actions(belief)" in source + assert "generator.probe.invalidate_collision_cache()" in source + assert "object_end_effector_offset_m=(0.90, 0.0, 0.0)" in source + planner_source = ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py" + ).read_text(encoding="utf-8") + assert "removal_pickup_yaw = math.pi" in planner_source + assert "removal_grasp = removal_root + grasp_from_root" in planner_source + assert "removal_grasp_offset" not in planner_source + assert "create_decontamination_activity_map(activity_path)" in ( + root / "scripts/run_real_robot_validation.py" + ).read_text(encoding="utf-8") + + +def test_complex_decommissioning_facility_manifest_is_deterministic_and_clear() -> None: + from radcounter.isaac.robot.real_robots import ( + RealRobotAssetConfig, + decommissioning_facility_layout, + facility_route_clearance_m, + ) + + layout = decommissioning_facility_layout() + assert layout == decommissioning_facility_layout() + assert layout.layout_id == "four_room_decommissioning_cell_v1" + + room_ids = {room.room_id for room in layout.rooms} + assert room_ids == { + "original_cell", + "remote_decon_room", + "reactor_service_room", + "shield_staging_room", + } + assert len(layout.corridors) >= 3 + connected = {"original_cell"} + while True: + expanded = ( + connected + | { + corridor.to_room_id + for corridor in layout.corridors + if corridor.from_room_id in connected + } + | { + corridor.from_room_id + for corridor in layout.corridors + if corridor.to_room_id in connected + } + ) + if expanded == connected: + break + connected = expanded + assert connected == room_ids + + assert len(layout.equipment_ids) >= 10 + assert len(layout.equipment_ids) == len(set(layout.equipment_ids)) + assert {primitive.shape for primitive in layout.primitives} >= {"cube", "cylinder"} + assert all( + primitive.room_id in room_ids + for primitive in layout.primitives + if primitive.equipment_id is not None + ) + + route_ids = {route.route_id for route in layout.reserved_routes} + assert route_ids == { + "primary_decon_route", + "staging_room_service_route", + "primary_shield_25_service_route", + "primary_shield_65_service_route", + } + for route_id in route_ids: + assert facility_route_clearance_m(layout, route_id) >= 0.65 + primary_route = next( + route for route in layout.reserved_routes if route.route_id == "primary_decon_route" + ) + config = RealRobotAssetConfig() + assert np.allclose( + primary_route.waypoints_m[-1][:2], + (config.decon_workbench_center_m[0] - 0.90, config.decon_workbench_center_m[1]), + ) + + staging_room = next(room for room in layout.rooms if room.room_id == "shield_staging_room") + shield_position = np.asarray(layout.secondary_shield_position_m) + room_lower = np.asarray(staging_room.center_m) - np.asarray(staging_room.half_extent_m) + room_upper = np.asarray(staging_room.center_m) + np.asarray(staging_room.half_extent_m) + assert np.all(shield_position[:2] > room_lower[:2]) + assert np.all(shield_position[:2] < room_upper[:2]) + assert layout.secondary_shield_path == "/World/StagingLeadShield" + assert layout.secondary_shield_path != config.shield_path + + primitive_paths = {primitive.path for primitive in layout.primitives} + assert { + "ShieldServiceAlcoveFloor", + "ShieldServiceAlcoveWestWall", + "DeconRoomWestWallSouthJamb", + "DeconRoomWestWallNorth", + } <= primitive_paths + + +def test_primary_shield_25_and_65_routes_use_the_west_handle_service_side() -> None: + from radcounter.isaac.planning.scene_candidates import ( + IsaacActionCandidateGenerator, + ) + from radcounter.isaac.robot.real_robots import ( + RealRobotAssetConfig, + decommissioning_facility_layout, + facility_route_clearance_m, + ) + + layout = decommissioning_facility_layout() + route_by_id = {route.route_id: route for route in layout.reserved_routes} + generator = object.__new__(IsaacActionCandidateGenerator) + generator.config = SimpleNamespace(end_effector_offset_m=(0.72, 0.0, 0.0)) + shield_config = RealRobotAssetConfig() + public_source = np.asarray((14.39, 0.5700749954, 0.45)) + protected = np.asarray((0.0, 0.0, 0.45)) + root_from_center = np.asarray((0.05, 0.0, -0.45)) + + for percentage, fraction in ((25, 0.25), (65, 0.65)): + target_root = public_source + fraction * (protected - public_source) + target_root += root_from_center + target_grasp = target_root + np.asarray(shield_config.shield_grasp_offset_m) + placement_base = generator._base_for_end_effector( + target_grasp, + robot_z=0.28, + yaw_rad=0.0, + ) + route_id = f"primary_shield_{percentage}_service_route" + route_endpoint = np.asarray(route_by_id[route_id].waypoints_m[-1]) + + assert np.allclose(route_endpoint, placement_base, atol=1.0e-9) + assert np.isclose(target_root[0] - placement_base[0], 0.96) + assert np.isclose(target_root[1], placement_base[1]) + assert placement_base[0] < target_grasp[0] < target_root[0] + assert facility_route_clearance_m(layout, route_id) >= 0.65 + + +def test_complex_facility_authoring_declares_auditable_metadata() -> None: + source = ( + ROOT / "source/extensions/radcounter.isaac/radcounter/isaac/robot/real_robots.py" + ).read_text(encoding="utf-8") + + for attribute_name in ( + "rad:facility:layoutId", + "rad:facility:roomCount", + "rad:facility:corridorCount", + "rad:facility:equipmentCount", + "rad:facility:obstacleCount", + "rad:facility:reservedRouteIds", + "rad:facility:equipmentType", + ): + assert attribute_name in source + assert "same_visible_irregular_collision_activity_mesh" in source + assert "def _author_staging_lead_shield" in source + assert source.count("CreateCenterOfMassAttr(Gf.Vec3f(0.0, 0.0, 0.10))") >= 2 + assert source.count("half_scale=(0.18, 0.30, 0.035)") >= 2 + assert '"rad:shield:inventoryId"' in source + assert '"staging-shield-02"' in source + assert "if cfg.include_validation_facility else None" in source + assert '"/World/RemoteDeconFacility"' in source + assert '"/World/DetectorStations/RemoteDeconRoom"' in source + assert "if legacy_surface.IsValid():" in source + assert "Optional relocation/disposal" in source + + planner_source = ( + ROOT / "source/extensions/radcounter.isaac/radcounter/isaac/planning/scene_candidates.py" + ).read_text(encoding="utf-8") + assert "for placement_yaw in (0.0,):" in planner_source + assert "overlaps the chassis and payload at release" in planner_source + + +def test_carried_shield_route_avoids_drum_swept_volume() -> None: + from radcounter.isaac.planning.scene_candidates import IsaacSceneFeasibilityProbe + + probe = object.__new__(IsaacSceneFeasibilityProbe) + probe.config = SimpleNamespace( + countermeasure_robot_path="/World/CountermeasureRobot", + measurement_robot_path="/World/MeasurementRobot", + mobile_clearance_m=0.42, + ) + probe._collision_bounds = lambda: ( # type: ignore[method-assign] + ( + "/World/HiddenContaminatedDrum/Drum", + np.asarray((2.08, -0.32, 0.05)), + np.asarray((2.72, 0.32, 1.05)), + ), + ) + probe.bounds = lambda path: ( # type: ignore[method-assign] + np.asarray((4.42, 2.0, 0.0)), + np.asarray((5.18, 3.6, 1.8)), + ) + pickup_base = np.asarray((3.6, 2.8, 0.015)) + placement_base = np.asarray((-0.7667, -0.325, 0.015)) + + base_only = probe.plan_mobile_route( + pickup_base, + placement_base, + excluded_paths=("/World/LeadShield",), + ) + carrying = probe.plan_mobile_route( + pickup_base, + placement_base, + excluded_paths=("/World/LeadShield",), + carried_object_path="/World/LeadShield", + carried_base_position_m=pickup_base, + ) + + assert base_only is not None and len(base_only) == 1 + assert carrying is not None and len(carrying) >= 2 + assert not np.allclose(carrying[0], placement_base) + + +def test_manipulation_base_endpoint_cannot_overlap_unrelated_prop() -> None: + from radcounter.isaac.planning.scene_candidates import IsaacSceneFeasibilityProbe + + probe = object.__new__(IsaacSceneFeasibilityProbe) + probe.config = SimpleNamespace( + countermeasure_robot_path="/World/CountermeasureRobot", + measurement_robot_path="/World/MeasurementRobot", + mobile_clearance_m=0.42, + ) + probe._collision_bounds = lambda: ( # type: ignore[method-assign] + ( + "/World/HiddenContaminatedDrum/Drum", + np.asarray((2.08, -0.32, 0.05)), + np.asarray((2.72, 0.32, 1.05)), + ), + ) + + assert not probe.mobile_base_pose_available( + np.asarray((2.215, -0.26, 0.015)), + excluded_paths=("/World/LeadShield",), + ) + assert probe.mobile_base_pose_available( + np.asarray((3.895, -0.26, 0.015)), + excluded_paths=("/World/LeadShield",), + ) + + +def test_curated_cad_spawn_can_ignore_only_the_compound_environment_aabb() -> None: + from radcounter.isaac.planning.scene_candidates import IsaacSceneFeasibilityProbe + + probe = object.__new__(IsaacSceneFeasibilityProbe) + probe.config = SimpleNamespace( + countermeasure_robot_path="/World/CountermeasureRobot", + measurement_robot_path="/World/MeasurementRobot", + mobile_clearance_m=0.55, + ignored_collision_paths=("/World/Environment",), + ) + probe._collision_bounds = lambda: ( # type: ignore[method-assign] + ( + "/World/Environment/Building/Mesh", + np.asarray((-25.0, -25.0, -2.0)), + np.asarray((2.2, 25.0, 58.0)), + ), + ) + probe.bounds = lambda path: None # type: ignore[method-assign] + + start = np.asarray((0.0, -1.0, 0.0)) + target = np.asarray((1.2, -0.55, 0.0)) + route = probe.plan_mobile_route( + start, + target, + moving_robot_path="/World/CountermeasureRobot", + ) + + assert route is not None + assert np.allclose(route[-1], target) + assert probe.mobile_base_pose_available(target) + + +def test_measurement_route_treats_countermeasure_robot_as_obstacle() -> None: + from radcounter.isaac.planning.scene_candidates import IsaacSceneFeasibilityProbe + + probe = object.__new__(IsaacSceneFeasibilityProbe) + probe.config = SimpleNamespace( + countermeasure_robot_path="/World/CountermeasureRobot", + measurement_robot_path="/World/MeasurementRobot", + mobile_clearance_m=0.55, + ) + probe._collision_bounds = lambda: () # type: ignore[method-assign] + probe.bounds = lambda path: ( # type: ignore[method-assign] + ( + np.asarray((0.65, -1.45, 0.0)), + np.asarray((1.75, -0.55, 1.1)), + ) + if path == "/World/CountermeasureRobot" + else None + ) + start = np.asarray((-3.2, 2.4, 0.0)) + target = np.asarray((3.2, -2.4, 0.8)) + + route = probe.plan_mobile_route( + start, + target, + moving_robot_path="/World/MeasurementRobot", + ) + + assert route is not None and len(route) >= 2 + assert not np.allclose(route[0], target) + + +def test_pickup_route_does_not_cross_target_object() -> None: + from radcounter.isaac.planning.scene_candidates import IsaacSceneFeasibilityProbe + + probe = object.__new__(IsaacSceneFeasibilityProbe) + probe.config = SimpleNamespace( + countermeasure_robot_path="/World/CountermeasureRobot", + measurement_robot_path="/World/MeasurementRobot", + mobile_clearance_m=0.55, + ) + probe._collision_bounds = lambda: ( # type: ignore[method-assign] + ( + "/World/HiddenContaminatedDrum/Drum", + np.asarray((2.08, -0.32, 0.05)), + np.asarray((2.72, 0.32, 1.05)), + ), + ) + probe.bounds = lambda path: None # type: ignore[method-assign] + start = np.asarray((3.8, 0.0, 0.28)) + pickup_base = np.asarray((1.5, -0.38, 0.28)) + + route = probe.plan_mobile_route( + start, + pickup_base, + moving_robot_path="/World/CountermeasureRobot", + ) + + assert route is not None and len(route) >= 2 + assert not np.allclose(route[0], pickup_base) + + +def test_opposite_side_removal_base_preserves_object_standoff() -> None: + from radcounter.isaac.planning.scene_candidates import ( + IsaacActionCandidateGenerator, + ) + + generator = object.__new__(IsaacActionCandidateGenerator) + generator.config = SimpleNamespace(end_effector_offset_m=(0.90, 0.0, 0.0)) + grasp = np.asarray((2.4, -0.5, 0.8)) + + base = generator._base_for_end_effector( + grasp, + robot_z=0.28, + yaw_rad=np.pi, + offset_m=(0.90, 0.0, 0.0), + ) + + assert np.allclose(base, np.asarray((3.3, -0.5, 0.28))) + assert np.isclose(np.linalg.norm(base[:2] - grasp[:2]), 0.90) diff --git a/tests/unit/test_robot_monitoring.py b/tests/unit/test_robot_monitoring.py new file mode 100644 index 0000000..cf12be4 --- /dev/null +++ b/tests/unit/test_robot_monitoring.py @@ -0,0 +1,98 @@ +from __future__ import annotations + +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from radcounter.core.robot_monitoring import ( + action_route_points, + action_target_point, + padded_square_bounds, + progress_from_remaining, + project_minimap_point, + robots_for_selection, +) +from radcounter.core.system_profiles import resolve_system_selection + + +def test_monitor_lists_reference_and_decommissioning_robots() -> None: + reference = robots_for_selection(resolve_system_selection(profile_id="fukushima-packbot")) + assert [(robot.robot_id, robot.display_name, robot.prim_path) for robot in reference] == [ + ("packbot", "PackBot", "/World/Robots/PackBot"), + ("elios3", "Elios 3", "/World/Robots/Elios3"), + ] + decommissioning = robots_for_selection(resolve_system_selection(profile_id="vertical-slice")) + assert [robot.robot_id for robot in decommissioning] == ["countermeasure", "measurement"] + assert len({robot.color_rgba for robot in reference}) == 2 + + +def test_monitor_combines_planner_routes_and_target() -> None: + action = SimpleNamespace( + parameters={ + "pickup_base_route_m": [[0.0, 0.0, 0.0], [1.0, 2.0, 0.0]], + "placement_base_route_m": [[1.0, 2.0, 0.0], [3.0, 4.0, 0.0]], + }, + target_pose_world=np.asarray( + [ + [1.0, 0.0, 0.0, 4.0], + [0.0, 1.0, 0.0, 5.0], + [0.0, 0.0, 1.0, 6.0], + [0.0, 0.0, 0.0, 1.0], + ] + ), + ) + assert action_route_points(action) == ( + (0.0, 0.0, 0.0), + (1.0, 2.0, 0.0), + (3.0, 4.0, 0.0), + ) + assert action_target_point(action) == (4.0, 5.0, 6.0) + + +def test_minimap_projection_uses_padded_square_and_inverts_world_y() -> None: + lower, upper = padded_square_bounds((-2.0, -1.0), (2.0, 1.0), padding_fraction=0.0) + assert lower == (-2.0, -2.0) + assert upper == (2.0, 2.0) + assert project_minimap_point((0.0, 0.0, 0.0), lower, upper) == pytest.approx((0.0, 0.0, 0.0)) + assert project_minimap_point((2.0, 2.0, 0.0), lower, upper) == pytest.approx((0.9, -0.9, 0.0)) + + +def test_progress_from_remaining_is_bounded() -> None: + assert progress_from_remaining(7.5, 10.0) == pytest.approx(0.25) + assert progress_from_remaining(-1.0, 10.0) == 1.0 + assert progress_from_remaining(12.0, 10.0) == 0.0 + + +def test_isaac_monitor_declares_all_operator_views_and_single_viewport_policy() -> None: + root = Path(__file__).resolve().parents[2] + monitor = ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/ui/robot_monitor.py" + ).read_text(encoding="utf-8") + dashboard = ( + root / "source/extensions/radcounter.isaac/radcounter/isaac/ui/dashboard.py" + ).read_text(encoding="utf-8") + + assert "ACTIVE ROBOT" in monitor + assert "BUILDING OVERVIEW" in monitor + assert 'self._activate_camera("follow")' in monitor + assert 'self._activate_camera("onboard")' in monitor + assert 'self._activate_camera("work")' in monitor + assert 'phase == "decontaminating"' in monitor + assert "and not self._work_view_triggered" in monitor + assert '"single_rendered_viewport": True' in monitor + assert "MONITOR_UPDATE_HZ = 12.0" in (root / "radcounter/core/robot_monitoring.py").read_text( + encoding="utf-8" + ) + assert '"View"' in dashboard + assert "self._robot_monitor.follow_robot" in dashboard + assert '"Onboard"' in dashboard + assert "self._robot_monitor.onboard_robot" in dashboard + assert "update_countermeasure_progress" in dashboard + assert "with ui.ScrollingFrame(" in dashboard + runner = (root / "scripts/run_gui_validation.py").read_text(encoding="utf-8") + assert '"--robot-monitor-smoke-test"' in runner + assert "def _exercise_robot_monitor" in runner + assert 'manual_cancel["camera_mode"] != "free"' in runner + assert "do not inject an unselected" in runner diff --git a/tests/unit/test_shield_deployment_state.py b/tests/unit/test_shield_deployment_state.py new file mode 100644 index 0000000..49a1cfb --- /dev/null +++ b/tests/unit/test_shield_deployment_state.py @@ -0,0 +1,229 @@ +from __future__ import annotations + +import asyncio +import sys +from pathlib import Path +from types import SimpleNamespace + +import numpy as np + +import radcounter +from radcounter.core.actions import ResourceState +from radcounter.core.models.actions import ActionStatus, ActionType +from radcounter.core.planning import DeterministicFeasibilityChecker +from radcounter.core.planning.models import FeasibilityFacts + +ROOT = Path(__file__).resolve().parents[2] +EXTENSION = ROOT / "source/extensions/radcounter.isaac" +if str(EXTENSION) not in sys.path: + sys.path.insert(0, str(EXTENSION)) +extension_namespace = str(EXTENSION / "radcounter") +if extension_namespace not in radcounter.__path__: + radcounter.__path__.append(extension_namespace) + +from radcounter.isaac.planning import ( # noqa: E402 + IsaacActionCandidateGenerator, + SceneCandidateConfig, +) +from radcounter.isaac.workflow.services import ( # noqa: E402 + IsaacWorkflowServices, + _ExecutionReport, +) + + +class _Attribute: + def __init__(self, value: object) -> None: + self.value = value + + def __bool__(self) -> bool: + return True + + def HasAuthoredValueOpinion(self) -> bool: # noqa: N802 + return True + + def IsValid(self) -> bool: # noqa: N802 + return True + + def Get(self) -> object: # noqa: N802 + return self.value + + def Set(self, value: object) -> None: # noqa: N802 + self.value = value + + +class _MissingAttribute: + def __bool__(self) -> bool: + return False + + def IsValid(self) -> bool: # noqa: N802 + return False + + +class _ShieldPrim: + path = "/World/LeadShield" + + def __init__(self) -> None: + self.attributes = { + "rad:role": _Attribute("shield"), + "rad:material:id": _Attribute("lead"), + "rad:shield:movable": _Attribute(True), + "rad:shield:resourceUnits": _Attribute(1), + "rad:shield:deployed": _Attribute(False), + "rad:shield:placementFraction": _Attribute(0.0), + "rad:manipulation:graspFrame": _Attribute("ShieldGraspFrame"), + } + + def __bool__(self) -> bool: + return True + + def GetPath(self) -> str: # noqa: N802 + return self.path + + def GetAttribute(self, name: str) -> _Attribute | _MissingAttribute: # noqa: N802 + return self.attributes.get(name, _MissingAttribute()) + + def IsValid(self) -> bool: # noqa: N802 + return True + + +class _Stage: + def __init__(self, shield: _ShieldPrim) -> None: + self.shield = shield + + def Traverse(self) -> tuple[_ShieldPrim, ...]: # noqa: N802 + return (self.shield,) + + def GetPrimAtPath(self, path: str) -> _ShieldPrim | None: # noqa: N802 + return self.shield if path == self.shield.path else None + + +class _Simulation: + def __init__(self) -> None: + self.synchronize_calls = 0 + + def synchronize(self) -> tuple[str, ...]: + self.synchronize_calls += 1 + return ("/World/LeadShield",) + + +def _generator(stage: _Stage, simulation: _Simulation) -> IsaacActionCandidateGenerator: + generator = object.__new__(IsaacActionCandidateGenerator) + generator.stage = stage + generator.simulation = simulation + generator.config = SceneCandidateConfig(shield_line_fractions=(0.35, 0.65)) + generator.probe = SimpleNamespace( + world_position=lambda value: ( + np.asarray((0.0, 0.0, 0.6), dtype=np.float64) + if isinstance(value, str) + else np.asarray((0.0, 0.0, 0.0), dtype=np.float64) + ) + ) + generator._public_source_samples = lambda belief: ( # type: ignore[method-assign] + np.asarray(((0.0, 0.0, 0.5),), dtype=np.float64), + np.asarray((1.0,), dtype=np.float64), + ) + generator._task_position = lambda: np.asarray( # type: ignore[method-assign] + (10.0, 0.0, 0.5), dtype=np.float64 + ) + generator._robot_position = lambda path: np.asarray( # type: ignore[method-assign] + (-1.0, 0.0, 0.0), dtype=np.float64 + ) + generator._center = lambda prim: np.asarray( # type: ignore[method-assign] + (0.0, 0.0, 0.5), dtype=np.float64 + ) + generator._base_for_end_effector = ( # type: ignore[method-assign] + lambda target, **kwargs: np.asarray(target, dtype=np.float64) + ) + generator._facts = lambda **kwargs: FeasibilityFacts() # type: ignore[method-assign] + generator._route = lambda *args, **kwargs: [] # type: ignore[method-assign] + generator._shield_reduction = lambda shield: 0.1 # type: ignore[method-assign] + generator._candidate = ( # type: ignore[method-assign] + lambda action, belief, facts, **kwargs: SimpleNamespace(action=action, feasibility=facts) + ) + return generator + + +def test_successful_shield_deployment_transitions_stable_candidates_to_move() -> None: + shield = _ShieldPrim() + stage = _Stage(shield) + simulation = _Simulation() + generator = _generator(stage, simulation) + resources = ResourceState( + remaining_work_time_s=300.0, + remaining_robot_runtime_s={"/World/CountermeasureRobot": 300.0}, + remaining_shield_units={"lead": 1}, + remaining_countermeasure_count=4, + ) + services = IsaacWorkflowServices( + simulation, + generator, + lambda measurement, previous: previous, + resources=resources, + ) + services._physical_execute = ( # type: ignore[method-assign] + lambda action: _ExecutionReport(True, "physical placement completed", {}) + ) + + initial = generator._shield_candidates(None) # type: ignore[arg-type] + place_35 = initial[0].action + assert place_35.action_type == ActionType.PLACE_SHIELD + assert place_35.parameters["shield_units"] == 1 + assert place_35.parameters["placement_fraction"] == 0.35 + assert place_35.parameters["deployment_state"] == "available" + + placed = asyncio.run(services.execute(place_35)) + + assert placed.status == ActionStatus.COMPLETED + assert shield.attributes["rad:shield:deployed"].Get() is True + assert shield.attributes["rad:shield:placementFraction"].Get() == 0.35 + assert resources.remaining_shield_units["lead"] == 0 + assert placed.public_details["deployment_state"] == "deployed" + assert placed.public_details["placement_fraction"] == 0.35 + + deployed = generator._shield_candidates(None) # type: ignore[arg-type] + move_35 = deployed[0].action + move_65_candidate = deployed[1] + move_65 = move_65_candidate.action + assert move_35.action_id == place_35.action_id + assert all(candidate.action.action_type == ActionType.MOVE_SHIELD for candidate in deployed) + assert all(candidate.action.parameters["shield_units"] == 0 for candidate in deployed) + assert DeterministicFeasibilityChecker().evaluate(move_65_candidate, resources).feasible + assert all( + candidate.action.parameters["deployment_state"] == "deployed" for candidate in deployed + ) + + moved = asyncio.run(services.execute(move_65)) + + assert moved.status == ActionStatus.COMPLETED + assert shield.attributes["rad:shield:placementFraction"].Get() == 0.65 + assert resources.remaining_shield_units["lead"] == 0 + assert simulation.synchronize_calls == 2 + + +def test_failed_shield_placement_does_not_commit_metadata_or_inventory() -> None: + shield = _ShieldPrim() + stage = _Stage(shield) + simulation = _Simulation() + generator = _generator(stage, simulation) + resources = ResourceState( + remaining_shield_units={"lead": 2}, + remaining_countermeasure_count=2, + ) + services = IsaacWorkflowServices( + simulation, + generator, + lambda measurement, previous: previous, + resources=resources, + ) + services._physical_execute = ( # type: ignore[method-assign] + lambda action: _ExecutionReport(False, "physical placement failed", {}) + ) + action = generator._shield_candidates(None)[0].action # type: ignore[arg-type] + + result = asyncio.run(services.execute(action)) + + assert result.status == ActionStatus.FAILED + assert shield.attributes["rad:shield:deployed"].Get() is False + assert shield.attributes["rad:shield:placementFraction"].Get() == 0.0 + assert resources.remaining_shield_units["lead"] == 2 + assert simulation.synchronize_calls == 0 diff --git a/tests/unit/test_surface_decontamination.py b/tests/unit/test_surface_decontamination.py index f87650c..7d636dd 100644 --- a/tests/unit/test_surface_decontamination.py +++ b/tests/unit/test_surface_decontamination.py @@ -3,6 +3,8 @@ from radcounter.core.surface_decontamination import ( DecontaminationTool, SurfaceSourceGrid, + effective_contact_exposure_s, + irregular_deposition_field, ) @@ -71,3 +73,76 @@ def test_activity_is_monotonic_and_visual_color_tracks_cleaning() -> None: colors = grid.color_rgb() treated = grid.cumulative_exposure_s > 0.0 assert colors[treated, 1].mean() > colors[~treated, 1].mean() + + +def test_vertical_wall_grid_uses_surface_basis_for_contact() -> None: + grid = SurfaceSourceGrid( + cells_x=4, + cells_y=4, + size_x_m=2.0, + size_y_m=2.0, + center_world_m=(0.0, 1.0, 5.0), + activity_bq_per_cell=100.0, + surface_u_world=(1.0, 0.0, 0.0), + surface_v_world=(0.0, 0.0, 1.0), + ) + + step = grid.apply_tool( + _tool(), + tool_center_world_m=(0.5, 0.98, 5.5), + tool_yaw_rad=0.0, + surface_speed_m_s=0.0, + dt_s=1.0, + ) + + assert len(step.contacted_cells) == 4 + assert step.removed_activity_bq > 0.0 + assert np.allclose(grid.surface_normal_world, (0.0, -1.0, 0.0)) + + +def test_canonical_irregular_field_retains_reference_mask_invariants() -> None: + field = irregular_deposition_field().reshape(28, 48) + active = field > 0.0 + + assert int(np.count_nonzero(active)) == 497 + assert int(active.size) == 1_344 + assert field[active].std() > 60_000.0 + assert not active[13, 21] # clean hole inside the main deposition + + seen: set[tuple[int, int]] = set() + component_sizes: list[int] = [] + for row, column in np.argwhere(active): + start = (int(row), int(column)) + if start in seen: + continue + pending = [start] + seen.add(start) + size = 0 + while pending: + current_row, current_column = pending.pop() + size += 1 + for row_delta in (-1, 0, 1): + for column_delta in (-1, 0, 1): + if row_delta == column_delta == 0: + continue + neighbor = ( + current_row + row_delta, + current_column + column_delta, + ) + if ( + 0 <= neighbor[0] < active.shape[0] + and 0 <= neighbor[1] < active.shape[1] + and active[neighbor] + and neighbor not in seen + ): + seen.add(neighbor) + pending.append(neighbor) + component_sizes.append(size) + + assert sorted(component_sizes, reverse=True) == [477, 13, 7] + + +def test_contact_exposure_is_one_speed_adjusted_tick_not_ray_count() -> None: + exposure = effective_contact_exposure_s(0.5, 0.1, 0.3) + assert np.isclose(exposure, 1.0 / 3.0) + assert effective_contact_exposure_s(0.5, 0.31, 0.3) == 0.0 diff --git a/tests/unit/test_system_profiles.py b/tests/unit/test_system_profiles.py new file mode 100644 index 0000000..23dd200 --- /dev/null +++ b/tests/unit/test_system_profiles.py @@ -0,0 +1,159 @@ +import json +from pathlib import Path + +import pytest + +from radcounter.core.system_profiles import ( + default_catalog_path, + load_active_selection, + load_system_catalog, + resolve_system_selection, + save_active_selection, +) +from scripts.convert_solidworks_to_usd import _conversion_command, discover_isaac_root + + +def test_repository_catalog_resolves_default_decommissioning_profile() -> None: + catalog_path, catalog = load_system_catalog() + assert catalog_path == default_catalog_path() + assert catalog.default_profile == "vertical-slice" + + selection = resolve_system_selection(profile_id="vertical-slice") + assert selection.environment_ready + assert not selection.configurable + assert selection.environment_config.environment_id == "vertical-slice-external" + assert selection.robot_set.kind == "decommissioning" + assert [item.descriptor.model_id for item in selection.detectors] == ["nai_tl"] + + +def test_fukushima_profile_is_pinned_to_expected_conversion_target() -> None: + selection = resolve_system_selection(profile_id="fukushima-packbot") + assert selection.configurable + assert selection.environment_config.format.value == "usd" + assert selection.environment_config.coordinate_system.up_axis.value == "auto" + assert selection.environment_config.translation_world_m == (2.2, 0.0, 18.0) + assert selection.spawn_anchor("ground-primary").translation_m == (0.0, -1.0, 0.0) + assert selection.spawn_anchor("decon-surface").translation_m == (2.18, 0.0, 1.15) + assert selection.environment_source_path is not None + assert selection.environment_source_path.as_posix().endswith( + ".cache/external/fukushima_daiichi_solidworks/export/Building.usdc" + ) + assert [path.name for path in selection.environment_preparation_scripts] == [ + "fetch_fukushima_daiichi_cad.py", + "convert_solidworks_to_usd.py", + ] + assert selection.robot_set.kind == "reference" + assert {item.robot_id for item in selection.robot_set.reference_robots} == { + "packbot", + "elios3", + } + assert {item.spawn_anchor for item in selection.robot_set.reference_robots} == { + "ground-primary", + "aerial-primary", + } + assert {item.placement.detector_id for item in selection.detectors} == { + "packbot-gm", + "elios-imager", + } + + +def test_component_overrides_are_independent_and_validate_robot_dependencies() -> None: + selection = resolve_system_selection( + profile_id="vertical-slice", + robot_set_id="fukushima-response-reference", + detector_set_id="fukushima-survey", + ) + assert selection.configurable + assert selection.environment_id == "vertical-slice" + assert selection.robot_set_id == "fukushima-response-reference" + + with pytest.raises(ValueError, match="requires robots not present"): + resolve_system_selection( + profile_id="vertical-slice", + robot_set_id="no-robots", + detector_set_id="fukushima-survey", + ) + + +def test_fleet_paths_are_rebased_from_the_fleet_descriptor() -> None: + selection = resolve_system_selection( + profile_id="vertical-slice", + robot_set_id="packbot-articulated", + detector_set_id="no-detectors", + ) + assert selection.robot_fleet is not None + robot = selection.robot_fleet.robots[0] + assert Path(robot.uri).is_absolute() + assert Path(robot.uri).name == "packbot_fukushima.urdf" + assert Path(robot.uri).is_file() + + +def test_active_selection_round_trip_uses_explicit_state_file(tmp_path: Path) -> None: + state = tmp_path / "selection.json" + selected = resolve_system_selection(profile_id="vertical-slice-packbot") + save_active_selection( + state, + catalog_path=selected.catalog_path, + profile_id=selected.profile_id, + ) + payload = json.loads(state.read_text(encoding="utf-8")) + assert payload["profile_id"] == "vertical-slice-packbot" + + loaded = load_active_selection(state) + assert loaded.profile_id == "vertical-slice-packbot" + assert loaded.robot_set_id == "fukushima-response-reference" + assert loaded.detector_set_id == "fukushima-survey" + + +def test_linux_solidworks_converter_uses_minimal_headless_kit(tmp_path: Path) -> None: + isaac = tmp_path / "isaacsim" / "6.0.1-standalone" + (isaac / "kit/apps").mkdir(parents=True) + (isaac / "kit/kit").touch() + hoops = isaac / "extscache/omni.kit.converter.hoops-510.3.0" + core = isaac / "extscache/omni.kit.converter.hoops_core-511.3.2" + launch = hoops / "omni/kit/converter/hoops/process/launch_hoops_app.py" + launch.parent.mkdir(parents=True) + launch.touch() + core.mkdir(parents=True) + + assert discover_isaac_root(isaac) == isaac.resolve() + command, hoops_name, core_name = _conversion_command( + isaac, + source=tmp_path / "Building.SLDASM", + output=tmp_path / "Building.usdc", + config=tmp_path / "options.json", + ) + assert "omni.app.empty.kit" in command[1] + assert "omni.kit.converter.hoops_core" in command + assert "--no-window" in command + assert "Building.SLDASM" in " ".join(command) + assert hoops_name == hoops.name + assert core_name == core.name + + +def test_operator_gui_has_separate_explicit_system_and_llm_controls() -> None: + dashboard = ( + Path(__file__).resolve().parents[2] + / "source/extensions/radcounter.isaac/radcounter/isaac/ui/dashboard.py" + ).read_text(encoding="utf-8") + assert "SYSTEM CONFIGURATION" in dashboard + assert '"Environment"' in dashboard + assert '"Robot"' in dashboard + assert '"Detector"' in dashboard + assert "Apply selected configuration" in dashboard + assert "NATURAL LANGUAGE COMMAND / ROBOT LLM" in dashboard + assert "Change the environment, robot" in dashboard + assert dashboard.index("NATURAL LANGUAGE COMMAND / ROBOT LLM") < dashboard.index( + "self._build_system_selector()" + ) + assert "save_active_selection(" in dashboard + + +def test_decommissioning_assets_use_environment_ground_anchors() -> None: + compositor = ( + Path(__file__).resolve().parents[2] + / "source/extensions/radcounter.isaac/radcounter/isaac/system_profile.py" + ).read_text(encoding="utf-8") + assert '(config.countermeasure_root, "ground-primary")' in compositor + assert '(config.measurement_root, "ground-secondary")' in compositor + assert '"rad:spawn:environmentId"' in compositor diff --git a/tests/unit/test_water_decontamination.py b/tests/unit/test_water_decontamination.py index 79c2b2b..600d913 100644 --- a/tests/unit/test_water_decontamination.py +++ b/tests/unit/test_water_decontamination.py @@ -111,3 +111,35 @@ def test_wetness_adds_blue_component_to_visualization() -> None: ) after = process.visual_color_rgb() assert np.any(after[:, 2] > before[:, 2]) + + +def test_water_jet_treats_vertical_wall_surface() -> None: + grid = SurfaceSourceGrid( + cells_x=6, + cells_y=4, + size_x_m=1.2, + size_y_m=0.8, + center_world_m=(0.0, 1.0, 5.0), + activity_bq_per_cell=1000.0, + surface_u_world=(1.0, 0.0, 0.0), + surface_v_world=(0.0, 0.0, 1.0), + ) + state = WaterDecontaminationState(10.0, 10.0) + process = WaterSurfaceDecontaminator( + grid, + state, + runoff_direction_world_xy=(0.0, -1.0), + ) + + step = process.apply( + _spec(), + nozzle_world_m=(0.0, 0.5, 5.0), + jet_direction_world=(0.0, 1.0, 0.0), + surface_speed_m_s=0.0, + dt_s=1.0, + ) + + assert step.blocked_reason is None + assert step.removed_activity_bq > 0.0 + assert step.impact_world_m is not None + assert np.allclose(step.impact_world_m, (0.0, 1.0, 5.0))