src/fpgm/physics/scene.py

"""Assembles a :class:`~fpgm.physics.types.SimSpec`/:class:`~fpgm.physics.types.ObservedTrack`
from artifacts S6/S7 already wrote to disk -- no new perception, no new fitting.

Everything this module reads was already measured by an earlier stage: the object's pose
track and its measured noise floor (S6, ``poses.npz`` / ``object_poses/meta.json``), the
drawer's prismatic axis (S7, ``events.json``, via :class:`fpgm.datagen.events.PrismaticFit`),
the static scene's own observed depth (S3, ``background_depth.h5`` -- see
:func:`_build_heightfield_spec`), and the robot's joint trajectory (DROID's own
``trajectory.h5``). :func:`build_sim_spec` and :func:`build_observed_track`
exist to turn that pile of already-measured artifacts into the two wire-format objects
:mod:`fpgm.physics.worker_mujoco`/``likelihood.py`` need, doing exactly one NEW derivation
along the way -- convex-hull export and finite-difference velocity -- and refusing to
fabricate anything that was not actually measured (see the ``PhysicsError`` raise sites
below, especially the noise floor: it is the one number the whole calibration rests on
being real).

**One non-obvious, EMPIRICALLY VERIFIED fact this module works around**: ``poses.npz``'s
``T_world_obj`` rotation block carries a baked-in *scale* factor for MESH-representation
objects (reconstructed from an arbitrary-CAD-scale ``.glb``, aligned via a scaled SE3), but
NOT for OBSERVED_SURFACE-representation objects (built directly from metric depth points).
Verified directly on the demo episode (``AUTOLab+0d4edc83+2023-10-21-19h-07m-04s``,
``22008760``): the brick's (mesh) rotation-block column norms are ``0.0573`` (its alignment
scale), the drawer's (observed-surface) are ``1.0000`` to 7 decimal places. Both
:class:`~fpgm.physics.types.SimSpec`'s ``BodySpec.init_pose`` and
:mod:`fpgm.physics.worker_mujoco`'s MJCF construction require a PURE rotation (unit-norm
columns; ``_mat_to_quat`` there is not scale-robust) -- so this module always measures the
per-object scale from the anchor frame's own column norms (never assumes 1.0, never assumes
the alignment scale from some other stage) and strips it before writing ``init_pose``,
folding it into the exported convex-hull STL's vertex scale instead so the physical size is
preserved.

**Scenes are multi-body; identification is still single-body.** ``SimSpec.bodies`` is a
list, and :func:`build_sim_spec` now populates it with every object S6 tracked in the
episode -- but :class:`~fpgm.physics.types.SimResult`/``fpgm.physics.worker_mujoco`` still
only ever return (and score) ONE body's trajectory (see that worker's module docstring on
"the body of interest" convention, ``spec.bodies[0]``). ``bodies[0]`` is always the
requested ``label``, a ``"free"``/``"prismatic"`` body whose parameters the returned
:class:`~fpgm.physics.types.ParamPosterior` is actually about. Every OTHER label
``poses.npz`` records for this episode is appended as ``kind="kinematic"`` -- a body driven
exactly along its own measured pose track (:class:`~fpgm.physics.types.BodySpec.pose_track`),
never a hypothesis, never scored on its own. This single change is what makes "the brick
rests on the drawer" fall out of the physics for free: **before** this, the brick's scene
contained no drawer at all, so a brick anchored above the (wrong, independently-fitted)
support plane just fell through empty space to the numerical safety floor (measured on the
demo episode: 59/64 particles diverged, ``ok=False``) -- there was nothing at the drawer's
actual height for it to land on. **After**, the drawer is present at its own measured pose,
so contact with it is possible regardless of which surface S6's plane-fitter happened to
find. No per-episode "brick supports on drawer" logic is written anywhere for this to work;
it is a consequence of both objects' real measured tracks being in the same scene, and it
works symmetrically -- when the drawer is the requested `label`, the brick becomes the
kinematic passenger instead. Only ``bodies[0]``'s parameters are ever inferred by this
stage; a kinematic body's own physical parameters (mass, its own friction independent of
the body of interest) are NOT separately identified -- see
``fpgm.physics.worker_mujoco._apply_kinematic_friction``'s docstring for why that would not
even be identifiable from one episode's evidence.

**Interface assumption -- finger poses via ``RobotModel.link_poses``, not the
``ArmKinematics.fk`` fast path.** ``ArmKinematics.fk``/``link7_to_tcp_transform`` give the
TCP (fingertip-midpoint) pose, not each finger individually -- there is no documented axis
convention anywhere in this codebase for deriving two separate finger-pad poses from that
single TCP pose without guessing which local axis the jaws open along. ``RobotModel.link_poses``
(the same call ``fpgm.robot.kinematics._build_flange_table`` already makes to build
``ArmKinematics``'s own flange-offset table) returns the real URDF ``left_inner_finger`` /
``right_inner_finger`` link poses directly, with no guessed offset. It costs ~0.7 ms/call
(vs. ``ArmKinematics.fk``'s 31 microseconds) -- irrelevant here: S9 builds a spec once per
episode-object offline, not once per IK-solve iteration, so the ~0.7 ms x (frames) cost this
trades away is a rounding error next to the MuJoCo simulation this spec feeds into.

**Where each field is a MEASUREMENT vs. an ESTIMATE vs. a fixed ASSUMPTION** (do not blur
these -- it is the whole point of this stage):

* MEASURED: the object's pose track and its ``pose_noise_mm`` translation noise floor (S6),
  the drawer's prismatic axis/origin (S7's ``PrismaticFit``), the static scene's own observed
  depth (S3's ``background_depth.h5``), the robot's joint trajectory (DROID), the object's own
  convex-hull geometry.
* DERIVED (a defensible computation from a measurement, not a second independent
  measurement): initial linear/angular velocity (finite difference of the measured track);
  ``ObservedTrack.sigma_rot_rad`` (see :func:`_derive_sigma_rot_rad` -- there is no rotational
  analogue anywhere in this codebase to ``pose_noise_mm``, so it is derived from the same
  measured translation noise via a small-angle lever-arm argument, and that derivation is
  never silently presented as an independent measurement); the static-scene heightfield (see
  :func:`_build_heightfield_spec` -- a top-down raster of MEASURED depth, not a fit).
* ASSUMED (a fixed, documented modelling choice with no data behind it): the heightfield's
  cell size and extent margin (see :data:`_HFIELD_CELL_SIZE_M`,
  :data:`_HFIELD_EXTENT_MARGIN_M`), how far below the observed minimum an unobserved cell is
  placed (:data:`_HFIELD_HOLE_DROP_M`), the prismatic joint-range padding fraction.

**No more fitted support plane.** Earlier versions of this module gave the scene exactly one
static surface: a plane fitted (by S6/S7, ``fpgm.datagen.geometry_ops.fit_support_plane``)
from ``object_poses/meta.json``'s ``support_plane``. That plane is a fit to wherever an
object's *own* observed footprint happened to sit -- on the demo episode
(``AUTOLab+0d4edc83+2023-10-21-19h-07m-04s``, ``22008760``) that is the brick's *destination*
surface, not the surface it starts on: the brick is measured **+167 mm above** that plane at
its first PNP frame. The table the brick actually starts on was never given collision geometry
at all, so a simulated brick free-fell through empty space to the numerical safety floor
(measured: 59/64 particles diverged, every one ``ok=False``). The table *was* present in the
data -- S3's ``background_depth.h5`` -- it was simply never turned into geometry. That is the
whole bug :func:`_build_heightfield_spec` exists to fix: build ONE static surface from the
observed depth itself, with no object-identity, no plane-fitting, no per-episode judgement
call about which surface is "the" support. See that function's docstring for the heightfield
representation's own honest limitation (no overhangs) and unknown-cell policy (never a
fabricated resting surface).

**Nor a heightfield with a tracked object baked into it.** Fixing the plane bug above
introduced a second, subtler one: ``background_depth.h5`` is S3's TEMPORAL MEDIAN of the
scene, and "static" there means "the median-of-clips merge produced a stable depth value
here" -- NOT "not a tracked object". An object that sits mostly still for the episode is
*itself* what the median depth records at its own footprint, because its own body occludes
whatever is really behind/beneath it from the camera's one viewpoint. On the demo episode
this measurably hurt the DRAWER specifically (it barely moves) while leaving the BRICK
(moves through most of its own footprint) fine: comparing ``outputs/real2sim_demo/summary.json``
(the old fitted-plane run) against a heightfield-only run before the fix below, the brick's
divergence went 58/64 -> 0/64 and its median translation error 133.3 -> 10.5 mm exactly as
expected, but the drawer's median translation error got WORSE, 37.2 -> 64.0 mm --
``build_sim_spec``'s own ``diagnostics["anchor_height_above_heightfield_m"]`` diagnosed it
directly: -11.6 mm for the brick (a plausible, shallow contact resolution) vs. -50.4 mm for
the drawer, over 4x deeper, the signature of a body resting on a copy of itself. Directly
verified (not just inferred from that gap): rasterising with vs. without the drawer's own S5
mask excluded showed 76.2% of the cells under the drawer's own footprint that had ANY
directly-observed support lost that support entirely once the drawer's own pixels were
removed -- almost nothing else was ever visible there across the whole episode. The fix
(:func:`_tracked_object_carve_mask`) carves out every pixel ANY tracked object's own S5 mask
covers, in ANY frame, before rasterising -- keeping the same "no object identity, no
semantics" design intent as the heightfield itself: everything that is not a tracked moving
object becomes one static block, and everything that IS one is excluded regardless of
whether it happens to move in this particular episode. A carved cell is handled by the exact
same :func:`_fill_unknown_cells` unknown-cell policy as any other occlusion -- never
interpolated into a plausible surface, which is also why this raises the heightfield's hole
fraction rather than lowering it.

**Nor a permanent, ANY-frame carve.** The carve above (call it v2; the no-carve heightfield
above it is v1) turned out to be ITS OWN version of the same self-embedding bug, just in the
opposite direction. It unions a tracked object's own mask over EVERY video frame and drops
that whole footprint unconditionally -- but "everywhere an object was ever seen" is a much
bigger region than "where it rests", for any object that actually moves. MEASURED on the
demo episode: the brick moves 206 px mean displacement through the episode, so v2 carved away
not just the brick's body but the TABLE SURFACE it started on and was later moved off of --
a surface plainly visible, cleanly, once the brick is gone. Directly verified: take the
brick's own mask at frame 4 (99 native-resolution background-depth pixels); the SAME 99
pixels are no longer covered by frame 40 (12 of them already revealed) and are fully revealed
by frame 70, staying revealed through frames 100 and 126 -- ``build_sim_spec``'s own
``anchor_height_above_heightfield_m`` diagnosed the damage directly: **+134.5 mm** for the
brick (free-falls the whole episode, never lands) where v1's no-carve heightfield had
(coincidentally -- see above) gotten this particular number approximately right.

The natural next idea -- read ``depth_dense.h5`` (S3's ``(T, H, W)`` per-frame depth) at each
pixel and take a genuine temporal median restricted to the frames a tracked object doesn't
cover it -- turned out to be a red herring, not a fix, once actually checked. MEASURED: at
those same 99 pixels, ``depth_dense.h5``'s value is IDENTICAL to 4+ decimal places across
frames 70/100/126. That is not coincidence: ``fpgm.datagen.dense_depth.compose_dense_depth``
broadcasts ``background_depth.h5`` UNCHANGED across every output frame and then z-buffers a
per-frame ROBOT RENDER on top wherever one is supplied (``source=SPLAT``) -- and DIRECTLY
CHECKED, every ``dense_depth/meta.json`` currently under ``outputs/datagen/`` in this repo has
``robot_composited: false`` (S2's render was never wired into these runs -- "expected for an
S3-only run", per :mod:`fpgm.datagen.dense_depth`'s own docstring). Confirmed at the array
level for the demo episode: ``depth_dense.h5``'s ``depth_mm``/``source`` are BIT-IDENTICAL,
every one of the 127 frames, to ``background_depth.h5`` broadcast across time
(``np.array_equal`` true for every frame checked). So today, across this whole repo,
``depth_dense.h5`` carries ZERO information ``background_depth.h5`` does not already have --
there is no live per-frame measurement of the static surface anywhere in this pipeline, only
ONE measurement (S3's anchor-median plate) replayed at every frame. This also cannot change
for the better by reading more of it: wherever a future run DOES composite a robot render
(``source=SPLAT``), that value is the ROBOT's own rendered depth, not the static scene's --
exactly as untrustworthy as a tracked object's own body (the very bug this section is about),
so it could never legitimately count as a "clean" surface observation either. Building on a
number nobody has actually verified varies would be exactly the mistake "Nor a heightfield
with a tracked object baked into it" above is about -- so :func:`_tracked_object_carve_mask`
never reads ``depth_dense.h5`` at all.

**v3 (this fix): carve only where EVERY frame is covered.** "Median over the frames not
covered by a tracked object" collapses algebraically, given the fact just established, to:
keep ``background_depth.h5``'s own already-measured value at a pixel UNLESS every single
video frame shows some tracked object's mask covering it -- the median of a constant sampled
over any non-empty subset of frames is that same constant, so there is nothing left to
average once the "which frames are clean" question is answered. :func:`_tracked_object_carve_mask`
therefore computes exactly that boolean question from the per-frame masks (union over every
tracked object's own role, per frame; intersection over every frame) and nothing else -- an
excluded pixel has genuinely never once been seen free of a tracked object this episode (the
drawer's own footprint, mostly), and keeps being handled by :func:`_fill_unknown_cells`'s
unchanged unknown-cell policy. A pixel that DOES have at least one clean frame is no longer
excluded at all, regardless of how many other frames a tracked object happened to cover it --
recovering exactly the brick's-starting-table-surface case the ANY-frame carve destroyed,
without reintroducing v1's self-embedding bug for an object (the drawer) that truly is
covered every frame. Both resulting fractions -- cells with no observation anywhere at all,
and cells with an observation that a tracked object nonetheless permanently occludes -- are
reported separately in ``diagnostics`` (``hole_frac_no_observation`` /
``hole_frac_no_clean_observation``), since they call for different responses from a reader.

**MEASURED end-to-end result, this fix, demo episode.** The brick recovers exactly v1's own
``anchor_height_above_heightfield_m`` (-11.6 mm, from +134.5 mm broken under v2) and
``scripts/render_real2sim_demo.py``'s simulated-vs-observed translation error recovers v1's
own 10.5 mm median (from v2's 121.6 mm) -- because literally ZERO native-resolution pixels in
the brick's own footprint are covered in all 127 video frames (it moves too much), this fix's
carve is a complete, measured no-op for the brick specifically, exactly as intended. The
drawer's own ALWAYS-covered fraction is smaller than a naive "the drawer never moves" mental
model predicts -- only 12.2% of its ever-covered VIDEO-resolution pixels are covered in
literally all 127 frames (measured), the rest failing the ALL-frames test by at least one
frame despite the object not physically moving. This is SAM3 mask boundary flicker under
partial robot-arm occlusion, not object motion -- the SAME phenomenon
:mod:`fpgm.datagen.static_span`'s own module docstring already measured and named for this
exact object ("the drawer's SAM mask boundary flickers as the robot arm occludes and reveals
parts of a large fixture"), here showing up as a strict-intersection carve missing most of a
genuinely-static object's own footprint rather than as a noisy centroid. Measured consequence:
the drawer's own simulated translation error (64.0 mm) lands almost exactly on v1's number
(64.0 mm), not v2's slightly better one (60.7 mm) -- this fix trades away a few mm on the
object v2 happened to help in order to stop destroying the object v2 broke by over two orders
of magnitude. A stricter, flicker-robust carve (e.g. a coverage-fraction threshold rather than
a literal ``all()``) is a plausible follow-up but is NOT implemented here: it was not asked
for, has no measured evidence it would improve the actual simulated outcome (only the native-
pixel carve fraction), and this fix already meets the task's own target (the brick no longer
free-falls) without it.
"""

from __future__ import annotations

import contextlib
import json
from pathlib import Path
from typing import Any

import cv2
import h5py
import numpy as np
import trimesh

from fpgm.config_datagen import DatagenProfile
from fpgm.data.droid_raw import read_mp4_properties
from fpgm.data.pointworld import FlowsReader
from fpgm.datagen.frame_index import EpisodeFrameIndex
from fpgm.datagen.object_masks import read_masks_h5
from fpgm.datagen.robot_buffers import (
    TRAJECTORY_GRIPPER_POSITION_KEY,
    TRAJECTORY_JOINT_POSITIONS_KEY,
    load_extrinsics_candidates,
    read_native_camera_intrinsics,
)
from fpgm.datagen.types import DepthSourceCode, PoseSource
from fpgm.geometry.camera import Camera
from fpgm.geometry.transforms import matrix_to_rotvec
from fpgm.physics.types import (
    BodySpec,
    GripperSpec,
    HeightfieldSpec,
    ObservedTrack,
    PhysicsError,
    SimSpec,
)
from fpgm.robot.urdf import RobotModel
from fpgm.utils.logging import get_logger
from fpgm.utils.timing import StepTimer

logger = get_logger(__name__)

__all__ = ["build_sim_spec", "build_observed_track", "load_observed_tracks", "object_crop"]

REPO_ROOT = Path(__file__).resolve().parents[3]
_DEFAULT_PROFILE_YAML = REPO_ROOT / "configs" / "datagen_droid.yaml"

#: S6's own "is this frame's pose trustworthy enough to score" gate --
#: ``fpgm.datagen.object_poses.ObjectPoseConfig.visible_frac_gap_threshold``. REUSED, not
#: re-tuned: that value is what S6 itself used to decide whether to demote a PNP frame to
#: GAP (object_poses.py:525, applied at object_poses.py:1590-1592). Duplicated as a literal
#: here (not imported) only because importing ``ObjectPoseConfig`` would pull in the whole
#: S6 estimation module for one float; the value itself is the single source of truth this
#: module trusts, not a re-derivation.
VISIBLE_FRAC_GAP_THRESHOLD = 0.25

#: Heightfield grid vertex spacing, both axes. ASSUMPTION, not measured -- chosen to
#: roughly match the native-resolution background-depth pixel's own footprint at a typical
#: DROID tabletop range. Measured on the demo episode: the background-depth camera's native
#: intrinsics (``fpgm.datagen.robot_buffers.read_native_camera_intrinsics``) give fx ~= 131 px
#: at 320x180, and the observed tabletop sits ~0.9-1.1 m from the camera, so one native pixel
#: already spans roughly (0.9 to 1.1) / 131 ~= 6.9-8.4 mm of world XY there -- 5 mm is close
#: to, and slightly finer than, that native footprint: fine enough not to throw away real
#: structure the depth resolves, without inventing resolution the sensor never had (going
#: much finer would just mean several adjacent cells are fed by the same source pixel).
_HFIELD_CELL_SIZE_M = 0.005

#: How far the heightfield's rasterised XY extent is padded beyond the bounding box of
#: every label ``poses.npz`` tracks in this episode (every frame, not just valid ones --
#: see :func:`_build_heightfield_spec`). ASSUMPTION: generous enough that a typical DROID
#: push/place excursion beyond the tracked bodies' own recorded footprint still lands on
#: real geometry, small enough that the grid stays a local tabletop patch rather than the
#: whole room a background camera happens to see (measured on the demo episode: the raw
#: background-depth point cloud spans a y-range of *4.4 m* end to end -- walls and floor far
#: outside the workspace -- while the tracked bodies' own bounding box is under 30 cm across;
#: rasterising the *whole* observed cloud would build a >300k-cell grid dominated by
#: geometry no simulated body in this episode could ever reach).
_HFIELD_EXTENT_MARGIN_M = 0.35

#: How far below the lowest DIRECTLY OBSERVED cell an unobserved ("hole") cell is placed.
#: ASSUMPTION, not measured. Must be large enough that no plausible object in this stage's
#: scenes (drawer/brick-scale, a few cm) could ever come to rest spanning a hole and a real
#: cell without the discontinuity being obvious in the rollout, small enough that it does
#: not need an unreasonably large `elevation_z` dynamic range on scenes that are mostly
#: holes. See :func:`_build_heightfield_spec`'s docstring for why a hole is dropped rather
#: than interpolated.
_HFIELD_HOLE_DROP_M = 0.15

#: Frames (in chronological PNP-frame order, not necessarily contiguous video-frame
#: indices) used to both anchor the initial pose and finite-difference the initial
#: velocity -- same window size as scripts/settle_after_release.py's own
#: `--n-vel-frames` default.
_DEFAULT_N_VEL_FRAMES = 5

#: Joint-range padding beyond the observed excursion -- a fraction of the observed
#: travel, floored at a fixed minimum so a near-static object's tiny observed range
#: doesn't produce an unrealistically tight prismatic limit. Not measured; a modelling
#: margin so a particle's simulated drawer is not artificially wall-stopped at exactly
#: the historical extremes.
_JOINT_RANGE_PAD_FRAC = 0.2
_JOINT_RANGE_PAD_MIN_M = 0.02

#: Physics substeps per output frame -- overrides SimSpec's dataclass default of 4.
#: Chosen, not measured: `_mujoco_settle_worker.py`-style contact (a rigid body settling
#: onto a support under gravity) was found DURING TESTING to tunnel through a support
#: box at substeps=4 for realistic DROID fall speeds/support thicknesses (see
#: fpgm.physics.worker_mujoco's module docstring for the mechanism -- discrete
#: per-step collision detection, not continuous, can miss a thin support if a body
#: crosses it within one physics step). 40 substeps at a ~1/60 s DROID frame interval
#: gives a ~0.42 ms physics step, comfortably finer than the few-cm support thicknesses
#: and sub-1 m/s object speeds this stage's episodes involve.
DEFAULT_SUBSTEPS = 40


def _load_profile(profile: DatagenProfile | None) -> DatagenProfile:
    return profile if profile is not None else DatagenProfile.from_yaml(_DEFAULT_PROFILE_YAML)


def _null_step(timer: StepTimer | None, label: str, **fields: Any):
    if timer is None:
        return contextlib.nullcontext()
    return timer.step(label, **fields)


def _load_master_artifacts(master_dir: Path, label: str) -> dict[str, Any]:
    """Read ``poses.npz``, ``events.json``, ``object_poses/meta.json`` for one label.

    Raises:
        PhysicsError: If any file is missing, or ``label`` is not tracked in this episode.
    """
    poses_path = master_dir / "poses.npz"
    events_path = master_dir / "events.json"
    meta_path = master_dir / "object_poses" / "meta.json"
    for p in (poses_path, events_path, meta_path):
        if not p.exists():
            raise PhysicsError(f"{label}: required S6/S7 artifact missing: {p}")

    with np.load(poses_path) as npz:
        labels = [str(x) for x in npz["labels"]]
        if label not in labels:
            raise PhysicsError(f"{label!r} not in poses.npz labels {labels} ({poses_path})")
        T_world_obj = np.asarray(npz[f"{label}__T_world_obj"], dtype=np.float64)
        pose_source = np.asarray(npz[f"{label}__pose_source"])
        visible_frac = np.asarray(npz[f"{label}__visible_frac"], dtype=np.float64)

    events = json.loads(events_path.read_text())
    if label not in events:
        raise PhysicsError(f"{label!r} not in events.json ({events_path})")
    meta = json.loads(meta_path.read_text())

    return {
        "T_world_obj": T_world_obj,
        "pose_source": pose_source,
        "visible_frac": visible_frac,
        "prismatic": events[label].get("prismatic"),
        "meta": meta,
    }


def _valid_mask(pose_source: np.ndarray, visible_frac: np.ndarray) -> np.ndarray:
    return (pose_source == PoseSource.PNP) & (visible_frac >= VISIBLE_FRAC_GAP_THRESHOLD)


def _pose_noise_mm(meta: dict, label: str) -> float:
    objects = meta.get("payload", {}).get("objects", {})
    value = objects.get(label, {}).get("pose_noise_mm")
    if value is None or not np.isfinite(value) or value <= 0:
        raise PhysicsError(
            f"{label!r}: no usable measured pose_noise_mm in object_poses/meta.json "
            f"(got {value!r}). This is the noise floor the whole likelihood calibration "
            "rests on being MEASURED (see fpgm.datagen.object_poses._estimate_pose_noise_mm) "
            "-- refusing to substitute a guessed value rather than raise."
        )
    return float(value)


def _pure_scale(rot: np.ndarray) -> float:
    """Mean column norm of a (3, 3) block -- see module docstring's baked-in-scale note."""
    return float(np.mean(np.linalg.norm(rot, axis=0)))


def _canonical_geometry_points(geometry_path: Path, label: str) -> np.ndarray:
    """``(N, 3)`` points in the object's own canonical/local frame, from whichever
    representation S6 actually wrote (see ``fpgm.datagen.geometry_ops.write_object_mesh_glb``
    / ``write_object_point_cloud_npz`` -- both representations are handled here, dispatched
    purely by file extension since ``object_poses/meta.json``'s ``geometry_paths`` already
    points at the right one regardless of which it is).
    """
    if geometry_path.suffix == ".glb":
        mesh = trimesh.load(str(geometry_path), force="mesh", process=False)
        return np.asarray(mesh.vertices, dtype=np.float64)
    if geometry_path.suffix == ".npz":
        with np.load(geometry_path) as npz:
            return np.asarray(npz["points_canonical"], dtype=np.float64)
    raise PhysicsError(f"{label}: unrecognised geometry file {geometry_path}")


def _geometry_path(meta: dict, label: str) -> Path:
    paths = meta.get("payload", {}).get("geometry_paths", {})
    if label not in paths:
        raise PhysicsError(f"{label!r}: no geometry_paths entry in object_poses/meta.json")
    path = Path(paths[label])
    if not path.exists():
        raise PhysicsError(f"{label!r}: geometry file listed but missing on disk: {path}")
    return path


def _first_pnp_window(pose_source: np.ndarray, n_vel_frames: int) -> np.ndarray:
    idx = np.where(pose_source == PoseSource.PNP)[0][:n_vel_frames]
    if idx.size < 2:
        raise PhysicsError(
            f"need >=2 PNP frames to anchor an initial pose/velocity, found {idx.size} "
            f"in the first {n_vel_frames} requested"
        )
    return idx


def _finite_difference_velocity(
    T_world_obj: np.ndarray, indices: np.ndarray, scale: float, dt: float
) -> tuple[np.ndarray, np.ndarray]:
    """Linear velocity (least-squares slope) + angular velocity (averaged rotvec/dt),
    both WORLD frame -- same method as scripts/settle_after_release.py's own
    `estimate_release_velocity`, generalised to a possibly non-contiguous frame window
    (unlike that script, S9's PNP frames are not guaranteed consecutive) by using each
    consecutive PAIR's own true index gap for its own dt, rather than assuming a fixed
    stride.
    """
    t = (indices - indices[0]).astype(np.float64) * dt
    pos = T_world_obj[indices, :3, 3]
    lin_vel = np.array([np.polyfit(t, pos[:, k], 1)[0] for k in range(3)], dtype=np.float64)

    omega_samples = []
    for i in range(len(indices) - 1):
        dt_pair = float(indices[i + 1] - indices[i]) * dt
        r0 = T_world_obj[indices[i], :3, :3] / scale
        r1 = T_world_obj[indices[i + 1], :3, :3] / scale
        r_rel = r1 @ r0.T
        omega_samples.append(matrix_to_rotvec(r_rel) / dt_pair)
    ang_vel_world = np.mean(np.asarray(omega_samples), axis=0) if omega_samples else np.zeros(3)
    return lin_vel, ang_vel_world


def _derive_sigma_rot_rad(sigma_trans_m: float, bounding_radius_m: float) -> float:
    """Small-angle lever-arm estimate: NOT an independent measurement (see module docstring).

    No rotational analogue to `pose_noise_mm` exists anywhere in this codebase (S6's
    `_estimate_pose_noise_mm` only ever computed a translation residual). Treating the
    measured translation noise as if it were a point-tracking error at the object's own
    bounding radius from its centroid gives `sigma_rot ~= sigma_trans / radius` -- a
    standard order-of-magnitude estimate for a rigid PnP solve, defensible but explicitly
    NOT a second measurement, which is why this function exists rather than the derivation
    being inlined and blurred into "the" noise floor.
    """
    if bounding_radius_m <= 0:
        raise PhysicsError(f"non-positive bounding radius {bounding_radius_m} -- degenerate mesh?")
    return sigma_trans_m / bounding_radius_m


def _bounding_radius_m(points_canonical: np.ndarray, scale: float) -> float:
    """Max distance from the (scaled) points' own centroid to any point.

    Same definition MuJoCo's `geom_rbound` uses internally for a mesh geom (max vertex
    distance from the body's own origin) -- recomputed independently here from the
    canonical points directly since this module has no MuJoCo/compiled-model access
    (fpgm env has no mujoco, see the top-level docstrings on env isolation). The two
    numbers are not required to match exactly (they are never compared to each other);
    this copy is only used for :func:`_derive_sigma_rot_rad`.
    """
    pts = points_canonical * scale
    centroid = pts.mean(axis=0)
    return float(np.max(np.linalg.norm(pts - centroid, axis=1)))


def _s2_camera(
    profile: DatagenProfile, uuid: str, camera_serial: str, master_dir: Path,
    video_width: int, video_height: int,
) -> Camera:
    """Reconstruct S2's own VALIDATED camera (intrinsics + extrinsic) for this episode/camera.

    Mirrors ``fpgm.datagen.pipeline.EpisodePipeline._run_s6``'s "camera + extrinsics: mirror
    S2's own construction" block EXACTLY (same three calls, same order, same source of
    truth for which extrinsics candidate won): :func:`~fpgm.datagen.robot_buffers.
    read_native_camera_intrinsics` for the intrinsics, :func:`~fpgm.datagen.robot_buffers.
    load_extrinsics_candidates` for both extrinsics HYPOTHESES, and S2's own
    ``robot_buffers/meta.json`` (written by ``run_robot_buffer_stage``) for which of those
    two hypotheses actually passed ``RobotAlignmentGate`` (see that class's module
    docstring). This is deliberately NOT a re-derivation: S2 measured which extrinsic
    candidate agrees with an independent SAM3 detection and which does not (on the demo
    episode the two disagree; using the wrong one silently would misplace every unprojected
    depth point without any obvious symptom -- exactly the failure mode
    ``RobotAlignmentGate`` exists to catch). Every downstream stage that unprojects depth
    (S6's fixture-geometry build, this function) uses this exact same reconstruction so
    there is one, and only one, camera pose for the whole pipeline to ever disagree about.

    Returns:
        A :class:`~fpgm.geometry.camera.Camera` valid at ``(video_width, video_height)`` --
        callers unprojecting a DIFFERENT resolution's depth (e.g. S3's native-resolution
        ``background_depth.h5``) must call :meth:`~fpgm.geometry.camera.Camera.rescaled`
        themselves, same as :func:`fpgm.datagen.geometry_ops._lift_masked_points` already
        does for S3's dense depth.

    Raises:
        PhysicsError: if S2's own ``robot_buffers/meta.json`` is missing (S2 never ran for
            this episode/camera) or its recorded ``chosen_extrinsics`` name is not among the
            current extrinsics candidates (the candidate set changed since S2 ran) -- this
            function never guesses a camera pose S2 itself did not already validate.
    """
    s2_meta_path = master_dir / "robot_buffers" / "meta.json"
    if not s2_meta_path.exists():
        raise PhysicsError(
            f"S2 output not found: {s2_meta_path} -- S9 needs the camera extrinsic S2 already "
            "validated via RobotAlignmentGate, not a re-derivation"
        )
    camera_intrinsics_native = read_native_camera_intrinsics(
        profile.paths.flows_h5(uuid), camera_serial
    )
    trajectory_path = profile.paths.episode_dir(uuid) / "trajectory.h5"
    extrinsics_candidates = load_extrinsics_candidates(
        profile.paths.cameras_json(uuid), trajectory_path, camera_serial
    )
    s2_meta = json.loads(s2_meta_path.read_text())
    chosen_name = s2_meta.get("payload", {}).get("chosen_extrinsics")
    chosen = next((c for c in extrinsics_candidates if c.name == chosen_name), None)
    if chosen is None:
        raise PhysicsError(
            f"{s2_meta_path}: chosen_extrinsics={chosen_name!r} is not among the current "
            f"extrinsics candidates {[c.name for c in extrinsics_candidates]!r} -- the "
            "candidate set changed since S2 ran"
        )
    return Camera(camera_intrinsics_native.scaled(video_width, video_height), chosen.world_to_cam)


def _load_background_depth(master_dir: Path) -> tuple[np.ndarray, np.ndarray, int, int]:
    """``(depth_mm, source, height, width)`` from S3's ``background_depth.h5``.

    Raises:
        PhysicsError: file missing, required datasets missing, or a shape/attribute
            mismatch -- never silently substitutes a smaller/reshaped array.
    """
    path = master_dir / "background_depth.h5"
    if not path.exists():
        raise PhysicsError(
            f"background_depth.h5 not found: {path} -- S3 (background depth) must run "
            "before S9 can build a static-scene heightfield"
        )
    with h5py.File(path, "r") as f:
        if "depth_mm" not in f or "source" not in f:
            raise PhysicsError(f"{path}: missing depth_mm/source dataset")
        depth_mm = np.asarray(f["depth_mm"])
        source = np.asarray(f["source"])
        height = int(f.attrs.get("height", depth_mm.shape[0] if depth_mm.ndim else -1))
        width = int(f.attrs.get("width", depth_mm.shape[1] if depth_mm.ndim == 2 else -1))
    if depth_mm.shape != (height, width) or source.shape != (height, width):
        raise PhysicsError(
            f"{path}: depth_mm/source shape {depth_mm.shape}/{source.shape} disagrees with "
            f"recorded (height, width) = ({height}, {width})"
        )
    return depth_mm, source, height, width


def _rasterize_top_down(
    points_xyz: np.ndarray,
    *,
    cell_size_m: float,
    x_range: tuple[float, float],
    y_range: tuple[float, float],
) -> tuple[np.ndarray, np.ndarray, np.ndarray, int, int]:
    """Top-down MAX-Z raster of a world-frame point cloud onto a fixed vertex grid.

    Pure and data-free (no file I/O, no PhysicsError-worthy business logic beyond a
    degenerate-extent check) so it is directly unit-testable with a synthetic point cloud --
    see ``tests/test_physics_scene.py``.

    **Max, not mean or median.** A falling or sliding body contacts whatever is physically
    on TOP within a cell's footprint; a mean or median would blend that top surface with
    anything glimpsed through a gap beside/behind it (e.g. a sliver of floor visible past a
    table edge at a grazing angle), fabricating a surface height that was never the true
    contact height at that cell. Max is also the max ``geom_rbound``-style convention MuJoCo
    itself is agnostic to -- the choice is about fidelity to "what does a body dropped here
    actually land on", not a MuJoCo requirement.

    **Vertex grid, not a pixel grid.** ``nx``/``ny`` are VERTEX counts (``(nx-1)``/``(ny-1)``
    cell spans between them), matching MuJoCo's own ``<hfield>`` convention (see
    ``fpgm.physics.worker_mujoco._heightfield_mujoco_params``'s docstring for the empirically
    -verified mapping from ``(row, col)`` to world position) -- a point falling in the half
    -open cell ``[x0 + i*cell, x0 + (i+1)*cell)`` is assigned to vertex ``i``, i.e. the LOWER
    corner of its cell, not rounded to the nearest vertex. This under- rather than
    over-estimates a rising surface's true edge position by up to one cell width, a bias in
    the "safe" direction (never claims a body could rest slightly further out than the data
    supports).

    Args:
        points_xyz: ``(N, 3)`` world-frame points, e.g. from unprojecting the background
            depth's ANCHOR pixels.
        cell_size_m: Uniform vertex spacing, both axes.
        x_range, y_range: ``(min, max)`` world extent to rasterise -- points outside are
            clipped to the nearest edge vertex column/row rather than dropped, since the
            caller (:func:`_build_heightfield_spec`) already restricts this range and a
            point landing exactly on the boundary must not be silently lost to a rounding
            edge case.

    Returns:
        ``(height_m, valid, origin_xy, nx, ny)``. ``height_m``/``valid`` are ``(ny, nx)``;
        ``height_m`` is ``NaN`` wherever ``valid`` is ``False`` (zero points fell in that
        cell) -- filling that in with a real, non-fabricated sentinel is the CALLER's job
        (:func:`_fill_unknown_cells`), kept separate so this rasteriser stays a pure
        geometric operation with no "what should an unknown cell be" policy baked in.

    Raises:
        PhysicsError: if ``x_range``/``y_range`` is degenerate (max <= min).
    """
    x0, x1 = x_range
    y0, y1 = y_range
    if not (x1 > x0 and y1 > y0):
        raise PhysicsError(f"degenerate heightfield rasterisation extent: x={x_range} y={y_range}")
    points_xyz = np.asarray(points_xyz, dtype=np.float64)

    nx = int(np.ceil((x1 - x0) / cell_size_m)) + 1
    ny = int(np.ceil((y1 - y0) / cell_size_m)) + 1
    ix = np.clip(np.floor((points_xyz[:, 0] - x0) / cell_size_m).astype(np.int64), 0, nx - 1)
    iy = np.clip(np.floor((points_xyz[:, 1] - y0) / cell_size_m).astype(np.int64), 0, ny - 1)
    flat_idx = iy * nx + ix

    height_flat = np.full(nx * ny, -np.inf, dtype=np.float64)
    np.maximum.at(height_flat, flat_idx, points_xyz[:, 2])
    valid_flat = np.isfinite(height_flat)
    height_flat = np.where(valid_flat, height_flat, np.nan)

    origin_xy = np.array([x0, y0], dtype=np.float64)
    return height_flat.reshape(ny, nx), valid_flat.reshape(ny, nx), origin_xy, nx, ny


def _fill_unknown_cells(height_m: np.ndarray, valid: np.ndarray) -> tuple[np.ndarray, float]:
    """Replace unobserved cells with a fixed sentinel BELOW the observed minimum.

    **Why a fixed drop, not interpolation.** Push-pull interpolation is exactly what S3's
    own ``background_depth.h5`` already does for ITS holes (see
    ``fpgm.datagen.background_depth``'s module docstring, step 3) -- and this function
    deliberately does NOT reuse that: a smoothed-in pixel is a defensible choice for a video
    frame a human or a VLM looks at (a plausible-looking hole beats a visible black gap), but
    an invented Z value becoming a simulated RESTING surface is a different, worse failure
    mode -- a particle would silently receive contact evidence at a height nobody ever
    measured. This module already uses that same reasoning once, upstream of this function
    (see :func:`_build_heightfield_spec`: only ``DepthSourceCode.ANCHOR`` pixels are
    unprojected at all, never S3's own ``FILLED`` ones). Dropping the sentinel well below
    the real minimum instead means: a body that ends up over a hole falls through an honest,
    reportable gap (in the worst case reaching the numerical safety floor and coming back
    ``ok=False``) rather than resting on geometry nobody observed.

    Returns:
        ``(filled_height_m, hole_frac)`` -- ``hole_frac`` is ``1 - valid.mean()``, the
        fraction of cells that were unobserved (recorded in
        ``build_sim_spec``'s ``diagnostics["limitations"]``).

    Raises:
        PhysicsError: if EVERY cell is unobserved -- nothing to build a floor from at all
            (a real capture/extent failure, not something to paper over with an arbitrary
            default height).
    """
    hole_frac = float(1.0 - valid.mean()) if valid.size else 1.0
    if not valid.any():
        raise PhysicsError(
            "heightfield: zero observed cells anywhere in the rasterised extent -- nothing "
            "measured to build a static-scene collision surface from"
        )
    z_floor = float(np.nanmin(height_m[valid])) - _HFIELD_HOLE_DROP_M
    filled = np.where(valid, height_m, z_floor).astype(np.float64)
    return filled, hole_frac


#: Half-width, in CELLS, of the neighbourhood :func:`_surface_height_under_xy` medians
#: over. NOT a single-vertex lookup -- see that function's docstring for why: MEASURED
#: directly on the demo episode, one bare vertex sampled at the brick's own footprint
#: centroid read 0.048 m while a median over the brick's whole ~140-cell footprint read
#: 0.195 m, a 15 cm disagreement over one 5 mm cell. That is real per-pixel noise/residual
#: contamination surviving background_depth.py's own per-clip median at an isolated pixel,
#: not a bug in this function -- but it means a single-cell query is not a reliable summary
#: of "the surface here", and a caller asking a human-facing "is this plausible" question
#: needs the more robust neighbourhood statistic instead.
_SURFACE_QUERY_HALF_WINDOW_CELLS = 3

def _surface_height_under_xy(
    height_m: np.ndarray, valid: np.ndarray, origin_xy: np.ndarray, cell_size_m: float,
    xy: np.ndarray,
) -> float | None:
    """MEDIAN heightfield sample in a small neighbourhood under a world ``(x, y)`` -- a
    diagnostic lookup, not what the simulator itself uses (MuJoCo bilinearly interpolates
    the FULL grid; this is a coarse, human-facing "is this plausible" summary, never fed
    into the physics).

    **Median over a window, not a single nearest vertex.** See
    :data:`_SURFACE_QUERY_HALF_WINDOW_CELLS`'s docstring for the measured reason: at this
    module's 5 mm cell size, ONE cell can disagree with its neighbours by well over a
    centimetre (real sensor noise/an isolated contaminated pixel surviving S3's own
    per-clip merge), so a bare nearest-vertex lookup is not a representative "what does the
    data say the surface height is here" answer. A window median is: individual noisy
    cells get outvoted, while an unobserved region still correctly returns ``None`` rather
    than silently substituting a distant cell's value (see below).

    Returns:
        The median height over the ``(2*half_window+1)^2``-cell window centred on the
        nearest vertex to ``xy``, restricted to cells where ``valid`` is ``True`` (the
        ORIGINAL observed mask, not the hole-filled array -- so this can never report a
        fabricated hole height as if it meant something). ``None`` if ``xy`` falls outside
        the grid entirely, or if EVERY cell in the window is unobserved.
    """
    ny, nx = height_m.shape
    ix = int(round((float(xy[0]) - float(origin_xy[0])) / cell_size_m))
    iy = int(round((float(xy[1]) - float(origin_xy[1])) / cell_size_m))
    if not (0 <= ix < nx and 0 <= iy < ny):
        return None
    hw = _SURFACE_QUERY_HALF_WINDOW_CELLS
    x0, x1 = max(0, ix - hw), min(nx, ix + hw + 1)
    y0, y1 = max(0, iy - hw), min(ny, iy + hw + 1)
    window_valid = valid[y0:y1, x0:x1]
    if not window_valid.any():
        return None
    return float(np.median(height_m[y0:y1, x0:x1][window_valid]))


def _tracked_object_carve_mask(
    camera_dir: Path, native_h: int, native_w: int,
) -> tuple[np.ndarray, dict[str, Any]]:
    """Pixels, at S3's background-depth NATIVE resolution, that get NO clean (object-free)
    observation in ANY video frame -- the cells :func:`_build_heightfield_spec` must treat as
    unknown rather than trust ``background_depth.h5``'s value at.

    **v1 -> v2 -> v3.** See this module's own docstring ("Nor a heightfield with a tracked
    object baked into it" / "Nor a permanent, ANY-frame carve") for the full history and the
    measurements behind each step; summary: v1 (no carve at all) let a mostly-still tracked
    object (measured here: the drawer) become its own resting surface. v2 (carve every pixel
    covered in ANY frame, permanently) overcorrected -- a MOVING tracked object (measured
    here: the brick, 206 px mean displacement) carved away real, later-revealed surface it
    once merely passed over, including the table it started on (MEASURED:
    ``anchor_height_above_heightfield_m`` = +134.5 mm, free-falls). v3 (this function) carves
    a pixel only if EVERY frame shows a tracked object covering it -- equivalent to a
    per-frame temporal median restricted to clean frames (see the module docstring for why
    that reduces to exactly this boolean question, and why ``depth_dense.h5`` is deliberately
    never read to compute it).

    **Combine rule: union over ROLES per frame, intersection over FRAMES.** For each video
    frame, a pixel is "covered" if ANY tracked object's own S5 mask covers it that frame (no
    object identity, no semantics -- same as v2: every label
    :class:`~fpgm.datagen.object_masks.PromptMaskStage` segmented for this episode/camera
    counts, whether or not it is the body of interest, a kinematic passenger, or one that
    never actually moved). A pixel is carved (returned ``True``) only if it is "covered" in
    EVERY frame -- i.e. ``combined_covered_video.all(axis=0)``, not ``.any(axis=0)`` (that
    would be v2 again). The union-over-roles step must happen BEFORE the intersection-over-
    frames step (not the reverse) because a pixel can be permanently occluded by the UNION of
    two objects taking turns covering it without either one covering it alone in every frame
    -- see this function's own tests for a concrete case.

    **Label mapping: read, never guessed.** The masks live at
    ``camera_dir/master/prompt_<object_id>_masks.h5`` where ``object_id`` is an internal
    enumeration index with no semantic meaning on its own -- the only trustworthy
    ``{label: masks_path}`` mapping is ``camera_dir/prompt_masks/meta.json``'s own
    ``payload["roles"]`` list, written by :class:`~fpgm.datagen.object_masks.PromptMaskStage.
    run` and already reused for exactly this purpose by
    ``fpgm.datagen.pipeline.EpisodePipeline._collect_gates`` (``prompt_meta_path =
    master_dir.parent / "prompt_masks" / "meta.json"``). This function reads the identical
    file the same way rather than inventing a second label-recovery heuristic -- e.g. assuming
    ``object_id`` order agrees with ``poses.npz``'s own label order, which happens to be true
    for the demo episode but is not guaranteed by anything.

    Args:
        camera_dir: ``profile.paths.camera_dir(uuid, camera_serial)`` -- the directory
            ``master/`` and ``prompt_masks/`` both live under.
        native_h, native_w: S3's ``background_depth.h5`` native resolution -- the resolution
            the returned mask is returned at, since it is ANDed directly against
            ``background_depth.h5``'s own ``source`` array in :func:`_build_heightfield_spec`.
            The intersection-over-time reduction happens FIRST, at the SAM3 video resolution
            (a ``(H, W)`` array); the resize to native resolution happens once afterwards,
            nearest-neighbour (never bilinear/area, same convention
            :func:`fpgm.datagen.geometry_ops._lift_masked_points` already uses) -- so the
            resize cost is independent of ``T``, same as v2 had it.

    Returns:
        ``(carve_mask, diagnostics)``. ``carve_mask`` is ``(native_h, native_w)`` bool,
        all-``False`` in the degenerate case every role's own mask was unreadable.
        ``diagnostics`` carries ``n_roles_total``, ``roles_carved`` (labels successfully
        combined), ``roles_skipped`` (``{label: reason}`` for a role whose own mask file could
        not be read -- an artifact-unreadable case, not a filtering decision), the
        intersection carve's own ``carve_frac_native`` (fraction of the native grid this
        function excludes), and ``ever_covered_frac_native`` (what v2's rejected ANY-frame
        carve would have excluded instead -- kept only as a documented point of comparison;
        always ``>= carve_frac_native``, with equality exactly when every carved pixel's
        covering object(s) never once revealed it, e.g. the demo episode's drawer).

    Raises:
        PhysicsError: If ``camera_dir/prompt_masks/meta.json`` itself is missing, corrupt, or
            lists zero roles -- there is then no reliable label mapping to carve from at all,
            and this function refuses to guess one (e.g. by falling back to the unlabelled
            S4/S5 discovery output, :class:`~fpgm.datagen.object_masks.ObjectMaskStage`'s
            ``object_masks`` stage, whose ``object_id`` carries no semantic label -- see that
            module's own "S4+S5 replacement" section for why that path was superseded).
    """
    meta_path = camera_dir / "prompt_masks" / "meta.json"
    if not meta_path.exists():
        raise PhysicsError(
            f"heightfield carve: {meta_path} not found -- S9 needs the label -> own-S5-mask "
            "mapping fpgm.datagen.object_masks.PromptMaskStage wrote to know which pixels "
            "belong to a TRACKED OBJECT rather than the static scene; refusing to guess that "
            "mapping from object_id order or fall back to the unlabelled motion-clustering "
            "(object_masks) stage instead"
        )
    try:
        meta = json.loads(meta_path.read_text())
    except json.JSONDecodeError as exc:
        raise PhysicsError(f"heightfield carve: {meta_path} is corrupt: {exc}") from exc
    roles = meta.get("payload", {}).get("roles", [])
    if not roles:
        raise PhysicsError(
            f"heightfield carve: {meta_path} lists zero roles -- no label mapping to carve from"
        )

    combined_covered_video: np.ndarray | None = None  # (T, H, W) bool, OR over every role.
    roles_carved: list[str] = []
    roles_skipped: dict[str, str] = {}
    for role in roles:
        label = str(role.get("label") or f"<unlabelled object_id={role.get('object_id')}>")
        masks_path_str = role.get("masks_path")
        if not masks_path_str:
            roles_skipped[label] = "role has no masks_path in prompt_masks/meta.json"
            continue
        masks_path = Path(masks_path_str)
        if not masks_path.exists():
            roles_skipped[label] = f"own S5 mask file missing on disk: {masks_path}"
            continue
        try:
            masks = read_masks_h5(masks_path)  # (T, H, W) bool, SAM3 video resolution.
        except OSError as exc:
            roles_skipped[label] = f"own S5 mask file unreadable: {exc}"
            continue
        if combined_covered_video is None:
            combined_covered_video = masks.copy()
        elif masks.shape != combined_covered_video.shape:
            roles_skipped[label] = (
                f"own S5 mask shape {masks.shape} disagrees with another already-combined "
                f"role's {combined_covered_video.shape} -- cannot union onto one grid"
            )
            continue
        else:
            combined_covered_video |= masks
        roles_carved.append(label)

    if combined_covered_video is None:
        carve_native = np.zeros((native_h, native_w), dtype=bool)
        ever_covered_frac_native = 0.0
    else:
        always_covered_video = combined_covered_video.all(axis=0)  # this fix's carve.
        # v2's rejected ANY-frame carve, kept only as a documented comparison diagnostic.
        ever_covered_video = combined_covered_video.any(axis=0)
        carve_native = cv2.resize(
            always_covered_video.astype(np.uint8), (native_w, native_h),
            interpolation=cv2.INTER_NEAREST,
        ).astype(bool)
        ever_covered_native = cv2.resize(
            ever_covered_video.astype(np.uint8), (native_w, native_h),
            interpolation=cv2.INTER_NEAREST,
        ).astype(bool)
        ever_covered_frac_native = (
            float(ever_covered_native.mean()) if ever_covered_native.size else 0.0
        )

    diagnostics: dict[str, Any] = {
        "n_roles_total": len(roles),
        "roles_carved": roles_carved,
        "roles_skipped": roles_skipped,
        "carve_frac_native": float(carve_native.mean()) if carve_native.size else 0.0,
        "ever_covered_frac_native": ever_covered_frac_native,
    }
    return carve_native, diagnostics


def _build_heightfield_spec(
    profile: DatagenProfile,
    uuid: str,
    camera_serial: str,
    master_dir: Path,
    scratch_dir: Path,
    *,
    video_width: int,
    video_height: int,
    timer: StepTimer | None = None,
) -> tuple[HeightfieldSpec, dict[str, Any]]:
    """The scene's single static collision surface, built from S3's observed depth.

    Replaces the old fitted-``support_plane`` entirely (see this module's docstring for the
    measured bug that motivated the change) -- see
    :class:`~fpgm.physics.types.HeightfieldSpec`'s own docstring for the representation's
    honest limitation (no overhangs) and the unknown-cell policy (never fabricated).

    Pipeline, each step delegated to a pure, independently-tested helper:

    1. Load S3's ``background_depth.h5`` (:func:`_load_background_depth`).
    2. Keep ONLY ``DepthSourceCode.ANCHOR`` pixels -- i.e. pixels a real merged clip anchor
       covered, per :mod:`fpgm.datagen.background_depth`'s own median-of-anchors merge --
       and drop every ``FILLED`` (push-pull interpolated) pixel. This is a SECOND,
       independent "do not trust an already-invented value" decision on top of
       :func:`_fill_unknown_cells`'s own -- S3's fill is fine for a video frame, not for
       collision geometry a body might rest on (see that function's docstring).
    3. Also exclude every pixel that gets NO clean (object-free) observation in ANY video
       frame (:func:`_tracked_object_carve_mask`) -- ``background_depth.h5`` is a TEMPORAL
       MEDIAN, so "static" there does not mean "not a tracked object": an object that sits
       mostly still (measured here: the drawer) is itself what the median depth records at
       its own footprint, and simulating it would then mean colliding it with a baked-in copy
       of itself. This is deliberately NOT a permanent ANY-frame carve (that was tried and
       measured to break a MOVING tracked object instead -- see this module's own docstring's
       "Nor a permanent, ANY-frame carve" section, and that function's own docstring, for the
       full history and the depth-provenance investigation that motivated this).
    4. Reconstruct S2's validated camera (:func:`_s2_camera`) and unproject the surviving
       pixels to world-frame points.
    5. Restrict the rasterised extent to a margin around every label ``poses.npz`` tracks in
       this episode (every frame, valid or not -- extent sizing is not a quality gate) --
       NOT the raw point cloud's own extent, which spans a background camera's whole
       field of view (measured on the demo episode: ~4.4 m of world Y, dominated by a wall/
       floor no simulated body in this episode's rollout could ever reach). See
       :data:`_HFIELD_EXTENT_MARGIN_M`.
    6. Rasterise (:func:`_rasterize_top_down`) and hole-fill (:func:`_fill_unknown_cells`) --
       the carve in step 3 means a permanently-occluded pixel now hole-fills exactly like any
       other occluded region, per that function's own unknown-cell policy (never a fabricated
       resting surface). A SECOND, diagnostic-only rasterisation over the pre-carve point set
       (never written to ``heightfield.npz``, never fed to the simulator) then splits
       ``hole_frac`` into its two structurally different causes -- see the ``Returns`` entry
       below.
    7. Write ``height_m``/``valid`` to ``scratch_dir/heightfield.npz`` (see
       :class:`~fpgm.physics.types.HeightfieldSpec`'s docstring for why a sidecar file, not
       inline JSON).

    Returns:
        ``(spec, diagnostics)`` -- ``diagnostics`` carries ``hole_frac`` (fraction of cells
        in the final grid that were NOT directly observed), split into
        ``hole_frac_no_observation`` (cells with zero ANCHOR-sourced support anywhere, at ANY
        time, independent of any tracked object -- a capture-coverage limitation) and
        ``hole_frac_no_clean_observation`` (cells that DO have ANCHOR-sourced support
        somewhere, but every one of those pixels is covered by a tracked object in every
        single frame -- this stage's own honest occlusion residual; on the demo episode, the
        drawer's own never-revealed footprint). ``hole_frac == hole_frac_no_observation +
        hole_frac_no_clean_observation`` always (the post-carve point set is a strict subset
        of the pre-carve one, so a cell the carve empties was necessarily non-empty before
        it). Also carries ``n_anchor_points``, ``n_cells``, the rasterised ``extent_m``, and
        ``carve`` (:func:`_tracked_object_carve_mask`'s own diagnostics -- which labels were
        combined, which were skipped and why, and the native-resolution carve fraction).

    Raises:
        PhysicsError: missing/malformed ``background_depth.h5``, no finite tracked-body
            position to size the extent from, no usable ``prompt_masks/meta.json`` label
            mapping to carve from (see :func:`_tracked_object_carve_mask`), zero ANCHOR
            pixels surviving the carve within the extent, or (degenerate) zero valid cells
            anywhere in the rasterised grid -- see the helpers above for exactly which
            condition raises which message.
    """
    with _null_step(timer, "heightfield_extent", uuid=uuid):
        with np.load(master_dir / "poses.npz") as npz:
            labels = [str(x) for x in npz["labels"]]
            xy_by_label = [np.asarray(npz[f"{lbl}__T_world_obj"])[:, :2, 3] for lbl in labels]
        all_xy = np.concatenate(xy_by_label, axis=0) if xy_by_label else np.zeros((0, 2))
        all_xy = all_xy[np.all(np.isfinite(all_xy), axis=1)]
        if all_xy.shape[0] == 0:
            raise PhysicsError(
                "heightfield: no finite tracked-body position anywhere in poses.npz to size "
                "the heightfield's extent from"
            )
        x_range = (
            float(all_xy[:, 0].min()) - _HFIELD_EXTENT_MARGIN_M,
            float(all_xy[:, 0].max()) + _HFIELD_EXTENT_MARGIN_M,
        )
        y_range = (
            float(all_xy[:, 1].min()) - _HFIELD_EXTENT_MARGIN_M,
            float(all_xy[:, 1].max()) + _HFIELD_EXTENT_MARGIN_M,
        )

    with _null_step(timer, "heightfield_unproject", uuid=uuid):
        depth_mm, source, native_h, native_w = _load_background_depth(master_dir)
        camera = _s2_camera(
            profile, uuid, camera_serial, master_dir,
            video_width=video_width, video_height=video_height,
        ).rescaled(native_w, native_h)

        anchor_mask = source == DepthSourceCode.ANCHOR
        n_anchor_precarve = int(anchor_mask.sum())
        if n_anchor_precarve == 0:
            raise PhysicsError(
                "heightfield: zero ANCHOR-sourced pixels anywhere in background_depth.h5 -- "
                "nothing directly measured to build a collision surface from, regardless of "
                "any tracked-object carve"
            )

        with _null_step(timer, "heightfield_carve_objects", uuid=uuid):
            carve_native, carve_diag = _tracked_object_carve_mask(
                profile.paths.camera_dir(uuid, camera_serial), native_h, native_w,
            )
        carve_diag["n_anchor_px_precarve"] = n_anchor_precarve
        carve_diag["n_anchor_px_postcarve"] = int((anchor_mask & ~carve_native).sum())

        # PRE-carve pixel set (every ANCHOR pixel, whether or not a tracked object ever
        # covers it) is unprojected ONCE; the carve is then applied as a boolean select on
        # these same points below, so the hole_frac_no_observation / hole_frac_no_clean_
        # observation split (see this function's own docstring) needs no second unprojection.
        ys, xs = np.nonzero(anchor_mask)
        carved_here = carve_native[ys, xs]
        uv = np.stack([xs + 0.5, ys + 0.5], axis=1).astype(np.float64)
        depth_m = depth_mm[ys, xs].astype(np.float64) / 1000.0
        points_world = camera.unproject(uv, depth_m)

        in_extent = (
            (points_world[:, 0] >= x_range[0]) & (points_world[:, 0] <= x_range[1]) &
            (points_world[:, 1] >= y_range[0]) & (points_world[:, 1] <= y_range[1])
        )
        points_world = points_world[in_extent]
        carved_in_extent = carved_here[in_extent]
        if points_world.shape[0] == 0:
            raise PhysicsError(
                "heightfield: zero ANCHOR-sourced background points fall within the "
                f"tracked-body extent x={x_range} y={y_range} -- nothing to rasterise"
            )

        points_world_clean = points_world[~carved_in_extent]
        if points_world_clean.shape[0] == 0:
            raise PhysicsError(
                "heightfield: every ANCHOR-sourced background point within the tracked-body "
                f"extent is covered by a tracked object in EVERY frame (roles carved: "
                f"{carve_diag['roles_carved']}) -- nothing directly measured and ever "
                "object-free to build a collision surface from"
            )

    with _null_step(timer, "heightfield_rasterize", uuid=uuid, n=int(points_world_clean.shape[0])):
        height_m, valid, origin_xy, nx, ny = _rasterize_top_down(
            points_world_clean, cell_size_m=_HFIELD_CELL_SIZE_M, x_range=x_range, y_range=y_range,
        )
        height_m, hole_frac = _fill_unknown_cells(height_m, valid)

        grid_path = scratch_dir / "heightfield.npz"
        np.savez(grid_path, height_m=height_m, valid=valid)

    with _null_step(timer, "heightfield_hole_diagnostics", uuid=uuid, n=int(points_world.shape[0])):
        # Diagnostic-only: rasterise the PRE-carve point set onto the SAME grid to learn which
        # holes are "never observed at all" vs. "observed, but a tracked object permanently
        # occludes it" -- see this function's own docstring for the exact accounting identity.
        # Never written to heightfield.npz, never influences the simulator.
        _height_no_carve, valid_no_carve, _origin2, _nx2, _ny2 = _rasterize_top_down(
            points_world, cell_size_m=_HFIELD_CELL_SIZE_M, x_range=x_range, y_range=y_range,
        )
        hole_frac_no_observation = (
            float(1.0 - valid_no_carve.mean()) if valid_no_carve.size else 1.0
        )
        hole_frac_no_clean_observation = float(np.mean(valid_no_carve & ~valid))

    spec = HeightfieldSpec(
        grid_path=grid_path, nx=nx, ny=ny, cell_size_m=_HFIELD_CELL_SIZE_M, origin_xy=origin_xy,
    )
    diagnostics: dict[str, Any] = {
        "n_anchor_points_in_extent": int(points_world_clean.shape[0]),
        "n_cells": int(nx * ny),
        "hole_frac": hole_frac,
        "hole_frac_no_observation": hole_frac_no_observation,
        "hole_frac_no_clean_observation": hole_frac_no_clean_observation,
        "extent_m": {"x": list(x_range), "y": list(y_range)},
        "cell_size_m": _HFIELD_CELL_SIZE_M,
        "carve": carve_diag,
    }
    return spec, diagnostics


def _prismatic_kind_and_range(
    prismatic: dict | None, T_world_obj: np.ndarray, valid: np.ndarray, anchor_idx: int
) -> tuple[str, np.ndarray | None, np.ndarray | None, tuple[float, float]]:
    """``(kind, axis_world, origin_world, joint_range)``.

    `joint_range` is expressed relative to the SAME zero-point as the simulation's own
    qpos=0 reference (the anchor frame's world pose, see build_sim_spec) -- NOT relative
    to `PrismaticFit.origin_world` (its PCA centroid) and NOT indexed by
    `PrismaticFit.displacement` directly, because that array is only defined over the
    fit's own (gated, `visible_frac`-filtered) frame subset, whose indices do not align
    1:1 with the full video-frame axis `valid`/`anchor_idx` are indexed by (measured
    directly: on the demo episode, `n_frames` in the gated fit is 101 of 127 total video
    frames). The axis/origin ARE reused as-is (measured by
    `fpgm.datagen.events.PrismaticFit`, never re-fit here); only the displacement is
    recomputed, as a plain projection of the already-tracked `T_world_obj` onto that
    same measured axis, over every valid frame -- not a second measurement of the axis
    itself.
    """
    if prismatic is None or not prismatic.get("is_prismatic", False):
        return "free", None, None, (-1.0, 1.0)

    axis_world = np.asarray(prismatic["axis_world"], dtype=np.float64)
    origin_world = np.asarray(prismatic["origin_world"], dtype=np.float64)
    axis_world = axis_world / np.linalg.norm(axis_world)

    full_displacement = (T_world_obj[:, :3, 3] - origin_world[None, :]) @ axis_world
    valid_disp = full_displacement[valid]
    if valid_disp.size == 0:
        return "prismatic", axis_world, origin_world, (-1.0, 1.0)

    lo, hi = float(valid_disp.min()), float(valid_disp.max())
    anchor_disp = float(full_displacement[anchor_idx])
    pad = max(_JOINT_RANGE_PAD_MIN_M, _JOINT_RANGE_PAD_FRAC * (hi - lo))
    joint_range = (lo - anchor_disp - pad, hi - anchor_disp + pad)
    return "prismatic", axis_world, origin_world, joint_range


def _finger_geometry(robot: RobotModel) -> np.ndarray:
    """``(3,)`` half-extents for both finger-pad mocap boxes -- MEASURED from the URDF's
    own finger-pad visual mesh AABB (in each link's own local frame), not a guessed
    constant. Both `left_inner_finger`/`right_inner_finger` are measured and averaged
    (component-wise) since the Robotiq 2F-85 pads are nominally mirror-symmetric; any
    residual difference is small numerical/authoring noise in the URDF meshes.
    """
    meshes = robot.visual_meshes()
    half_extents = []
    for link_name in ("left_inner_finger", "right_inner_finger"):
        if link_name not in meshes:
            raise PhysicsError(f"URDF has no visual geometry for {link_name!r}")
        pts = []
        for mesh, transform in meshes[link_name]:
            verts = np.asarray(mesh.vertices, dtype=np.float64)
            verts_link = (transform[:3, :3] @ verts.T).T + transform[:3, 3]
            pts.append(verts_link)
        pts = np.concatenate(pts, axis=0)
        half_extents.append((pts.max(axis=0) - pts.min(axis=0)) / 2.0)
    return np.mean(np.asarray(half_extents), axis=0)


def build_observed_track(
    uuid: str,
    camera_serial: str,
    label: str,
    *,
    profile: DatagenProfile | None = None,
    timer: StepTimer | None = None,
) -> ObservedTrack:
    """The measured pose track + measured noise floor for one episode-object.

    Args:
        uuid, camera_serial, label: Which episode/camera/object -- must already have a
            completed S6 run (``poses.npz``, ``object_poses/meta.json``).
        profile: Defaults to ``configs/datagen_droid.yaml`` (the same profile every real
            S6 caller uses -- see ``fpgm.datagen.pipeline``).
        timer: See module-level Timing note; ``None`` is a no-op.

    Raises:
        PhysicsError: Missing artifact, unknown label, or (see module docstring)
            unavailable measured noise floor -- never substitutes a guess.
    """
    profile = _load_profile(profile)
    master_dir = profile.paths.master_dir(uuid, camera_serial)

    with _null_step(timer, "load_observed_track", uuid=uuid, body_label=label):
        art = _load_master_artifacts(master_dir, label)
        T_world_obj = art["T_world_obj"]
        pose_source = art["pose_source"]
        visible_frac = art["visible_frac"]
        meta = art["meta"]

        valid = _valid_mask(pose_source, visible_frac)
        sigma_trans_m = _pose_noise_mm(meta, label) / 1000.0

        anchor_idx = int(_first_pnp_window(pose_source, _DEFAULT_N_VEL_FRAMES)[-1])
        scale = _pure_scale(T_world_obj[anchor_idx, :3, :3])
        geometry_path = _geometry_path(meta, label)
        points_canonical = _canonical_geometry_points(geometry_path, label)
        bounding_radius_m = _bounding_radius_m(points_canonical, scale)
        sigma_rot_rad = _derive_sigma_rot_rad(sigma_trans_m, bounding_radius_m)

        episode_dir = profile.paths.episode_dir(uuid)
        mp4_path = episode_dir / "recordings" / "MP4" / f"{camera_serial}.mp4"
        mp4_fps, n_video_frames, _res = read_mp4_properties(mp4_path)
        if n_video_frames != T_world_obj.shape[0]:
            raise PhysicsError(
                f"{label}: poses.npz has {T_world_obj.shape[0]} frames but the mp4 reports "
                f"{n_video_frames} -- video-frame-axis artifacts have gone out of sync"
            )

    return ObservedTrack(
        uuid=uuid, camera_serial=camera_serial, label=label,
        T_world_obj=T_world_obj, valid=valid,
        sigma_trans_m=sigma_trans_m, sigma_rot_rad=sigma_rot_rad, fps=float(mp4_fps),
    )


def load_observed_tracks(
    uuid: str,
    camera_serial: str,
    *,
    profile: DatagenProfile | None = None,
    labels: list[str] | None = None,
    timer: StepTimer | None = None,
) -> tuple[dict[str, ObservedTrack], dict[str, str]]:
    """Every object in this episode that has a usable measured track.

    :func:`build_observed_track` is deliberately single-object and strict -- it
    raises rather than substituting a guessed noise floor. That strictness is
    right for one object and wrong for an episode: one object whose mesh is
    missing must not cost the caller the other object in the same clip.

    So this wrapper keeps the strictness and downgrades it to a *per-object*
    outcome, returning the objects that loaded plus a ``{label: reason}`` map
    for the ones that did not. The reasons are returned, never logged-and
    -dropped, because "S9 found no objects here" and "S9 found two objects and
    both failed for the same missing-mesh reason" call for completely different
    responses, and only the second one is actionable.

    Note what this is *not*: a quality filter. Nothing here inspects how much
    the object moved, and nothing may -- an object that sat still is loaded
    exactly like one that was thrown across the table, and the arithmetic in
    :mod:`fpgm.physics.inference` makes the still one a no-op on its own. The
    only thing that excludes an object here is the artifact being unreadable.

    Args:
        uuid, camera_serial: Which episode/camera.
        profile: Defaults to ``configs/datagen_droid.yaml``.
        labels: Restrict to these labels. ``None`` (default) takes every label
            ``poses.npz`` records, which is what makes this work unchanged for
            motion-discovered objects (``obj0``/``obj1``...) that no instruction
            ever named.
        timer: See module-level Timing note; ``None`` is a no-op.

    Returns:
        ``(tracks, skipped)``.

    Raises:
        PhysicsError: Only if ``poses.npz`` itself is missing/unreadable -- that
            is an episode-level fault (S6 never ran), not an object-level one.
    """
    profile = _load_profile(profile)
    master_dir = profile.paths.master_dir(uuid, camera_serial)
    poses_path = master_dir / "poses.npz"
    if not poses_path.exists():
        raise PhysicsError(f"{uuid}/{camera_serial}: no S6 output at {poses_path}")

    if labels is None:
        with np.load(poses_path) as npz:
            labels = [str(x) for x in npz["labels"]]

    tracks: dict[str, ObservedTrack] = {}
    skipped: dict[str, str] = {}
    with _null_step(timer, "load_observed_tracks", uuid=uuid, n=max(len(labels), 1)):
        for label in labels:
            try:
                tracks[label] = build_observed_track(
                    uuid, camera_serial, label, profile=profile, timer=timer
                )
            except PhysicsError as exc:
                skipped[label] = str(exc)
                logger.warning("S9: %s/%s object %r unusable: %s", uuid, camera_serial, label, exc)
    return tracks, skipped


def object_crop(
    uuid: str,
    camera_serial: str,
    label: str,
    *,
    profile: DatagenProfile | None = None,
) -> Path:
    """Path to an RGB crop of ``label``, for the VLM material read.

    The VLM needs a picture of the object, and S5 already wrote one per object
    per episode while building its masks. Regenerating a crop here would mean
    re-decoding the mp4 and re-reading the mask h5 to produce an image that is
    already on disk, so this only locates it.

    Deliberately raises instead of returning ``None`` when nothing is found: the
    caller's fallback (a material-agnostic prior) is a real, recorded downgrade
    in the report, and it should be reached through the same ``PhysicsError``
    path as every other VLM failure rather than through a quietly-empty return
    that looks like success.

    Raises:
        PhysicsError: If no crop exists for this object.
    """
    profile = _load_profile(profile)
    master_dir = profile.paths.master_dir(uuid, camera_serial)
    for pattern in (f"crops/{label}.png", f"crops/{label}_*.png", f"{label}_crop.png"):
        hits = sorted(master_dir.glob(pattern))
        if hits:
            return hits[0]
    raise PhysicsError(
        f"{uuid}/{camera_serial}: no crop image for object {label!r} under {master_dir}/crops -- "
        "S5 writes one per object; a missing crop means the VLM cannot see this object"
    )


def _kinematic_body_spec(
    master_dir: Path, label: str, n_frames_video: int, scratch_dir: Path,
) -> tuple[BodySpec, list[str]]:
    """One OTHER tracked object as a ``kind="kinematic"`` :class:`BodySpec`.

    Reuses exactly the artifacts/derivations :func:`build_sim_spec` already uses for the
    body of interest -- :func:`_load_master_artifacts`, :func:`_canonical_geometry_points`,
    the convex-hull export, :func:`_pure_scale` -- so there is exactly one place in this
    module that knows how to turn an S6 pose track + geometry file into a MuJoCo-ready
    body, not two subtly-diverging copies.

    **Scale, from a single frame, not a window.** The body of interest's ``init_pose``
    measures scale from the LAST frame of its first-PNP *velocity* window (see
    :func:`_first_pnp_window`) because that same window also finite-differences an initial
    velocity. A kinematic body needs neither a velocity nor a single "start" frame -- its
    whole ``pose_track`` is used -- so it only needs ONE trustworthy (PNP-sourced) frame,
    anywhere in the episode, to measure its own baked-in scale (see the module docstring's
    measured mesh-vs-observed-surface fact) and its hull. The first PNP-sourced frame is
    used for that, deliberately not averaged across several: the baked-in scale is a fixed
    per-object constant (a CAD-alignment factor, or exactly 1.0), not a noisy per-frame
    quantity, so a second frame would not reduce any variance -- it would only add a
    dependency on that frame also being well-tracked.

    **Untrustworthy frames.** Frames that fail S6's own gate (:func:`_valid_mask`: not
    PNP-sourced, or below ``visible_frac_gap_threshold``) are NOT held out of the returned
    ``pose_track`` -- MuJoCo needs a pose at every output frame for a mocap body, there is
    no "skip this frame" affordance. Substituting a fabricated pose (extrapolated, or
    silently reusing whatever ``poses.npz`` already interpolated/gap-filled there) would let
    an unmeasured guess drive contact geometry as if it were data. The choice made here
    instead: hold the body at its *nearest measured* pose (forward-filled from the last
    valid frame; back-filled from the first valid frame for any run of invalid frames at
    the very start) -- honestly stale rather than fabricated, and the number of frames this
    happened to is always returned so a caller can judge whether that staleness could
    plausibly matter for this particular episode.

    Returns:
        ``(body, limitations)`` -- ``limitations`` is empty unless some frames needed
        holding at a stale pose (see above), in which case it names exactly how many.

    Raises:
        PhysicsError: If this object's own S6 artifacts cannot support a kinematic body at
            all -- missing geometry, a video-frame-axis mismatch, zero PNP-sourced frames
            to measure a scale from, or (degenerate) zero frames passing S6's own gate even
            though a PNP frame existed. The caller (:func:`build_sim_spec`) downgrades this
            into a per-object skip + a recorded diagnostic, the same discipline
            :func:`load_observed_tracks` already applies to a whole :class:`ObservedTrack`.
    """
    art = _load_master_artifacts(master_dir, label)
    T_world_obj = art["T_world_obj"]
    pose_source = art["pose_source"]
    visible_frac = art["visible_frac"]
    meta = art["meta"]
    if T_world_obj.shape[0] != n_frames_video:
        raise PhysicsError(
            f"{label}: has {T_world_obj.shape[0]} pose frames, but the primary object's "
            f"video has {n_frames_video} -- video-frame-axis artifacts have gone out of sync"
        )

    valid = _valid_mask(pose_source, visible_frac)
    pnp_idx = np.where(pose_source == PoseSource.PNP)[0]
    if pnp_idx.size == 0:
        raise PhysicsError(f"{label}: no PNP-sourced frame at all -- cannot measure a scale/hull")
    scale = _pure_scale(T_world_obj[int(pnp_idx[0]), :3, :3])

    geometry_path = _geometry_path(meta, label)
    points_canonical = _canonical_geometry_points(geometry_path, label)
    points_scaled = points_canonical * scale
    hull = trimesh.Trimesh(vertices=points_scaled, process=False).convex_hull
    hull_stl = scratch_dir / f"{label}_hull.stl"
    hull.export(str(hull_stl))

    pose_track = T_world_obj.copy()
    pose_track[:, :3, :3] = T_world_obj[:, :3, :3] / scale
    # Translation is untouched: the module docstring's measured fact is that the baked-in
    # alignment scale lives ONLY in the rotation block's column norms, never in position.

    limitations: list[str] = []
    n_untrusted = int((~valid).sum())
    if n_untrusted:
        if not valid.any():
            raise PhysicsError(
                f"{label}: 0 of {pose_track.shape[0]} frames pass S6's own PNP+visibility "
                "gate even though a PNP-sourced frame existed for scale -- refusing to build "
                "a kinematic body whose entire track would be fabricated"
            )
        first_good_idx = int(np.argmax(valid))
        pose_track[:first_good_idx] = pose_track[first_good_idx]
        last_good = pose_track[first_good_idx].copy()
        for t in range(first_good_idx + 1, pose_track.shape[0]):
            if valid[t]:
                last_good = pose_track[t].copy()
            else:
                pose_track[t] = last_good
        limitations.append(
            f"{label}: {n_untrusted}/{pose_track.shape[0]} frames had an untrustworthy pose "
            "(not PNP-sourced, or below visible_frac_gap_threshold) and were held at the "
            "nearest MEASURED pose (forward-filled, back-filled only before the first valid "
            "frame) rather than treated as new measurements -- this kinematic body's motion "
            "is stale, not observed, on those frames"
        )

    body = BodySpec(
        label=label, mesh_path=hull_stl, init_pose=pose_track[0], kind="kinematic",
        pose_track=pose_track,
    )
    return body, limitations


def build_sim_spec(
    uuid: str,
    camera_serial: str,
    label: str,
    *,
    profile: DatagenProfile | None = None,
    scratch_dir: Path,
    substeps: int = DEFAULT_SUBSTEPS,
    n_vel_frames: int = _DEFAULT_N_VEL_FRAMES,
    timer: StepTimer | None = None,
) -> tuple[SimSpec, dict[str, Any]]:
    """Build a multi-body :class:`~fpgm.physics.types.SimSpec` for one episode-object.

    ``spec.bodies[0]`` is ``label``, built exactly as before (a ``"free"`` or
    ``"prismatic"`` body -- the one whose parameters this stage identifies). Every OTHER
    label ``poses.npz`` records for this episode is then appended as a ``kind="kinematic"``
    passenger, driven along its own measured pose track -- see the module docstring's
    "Scenes are multi-body" section for why (in one sentence: so the object actually
    supporting/contacting ``label`` is present in the scene at all, without this module
    ever having to know or guess which tracked object that is).

    Args:
        uuid, camera_serial, label: Which episode/camera/object to build a spec for.
        profile: Defaults to ``configs/datagen_droid.yaml``.
        scratch_dir: Where every body's convex-hull STL is written -- ``label``'s and every
            kinematic passenger's alike (``SimSpec``/``BodySpec`` carry a path, not mesh
            bytes -- see ``BodySpec.mesh_path``'s docstring). Caller-owned; not cleaned up
            here, since :class:`~fpgm.physics.simulate.MujocoSimulator` needs the files to
            still exist when the worker subprocess reads them.
        substeps: See :data:`DEFAULT_SUBSTEPS`.
        n_vel_frames: See :data:`_DEFAULT_N_VEL_FRAMES`.
        timer: See module-level Timing note; ``None`` is a no-op.

    Returns:
        ``(spec, diagnostics)`` -- ``diagnostics`` records every ASSUMED/DERIVED choice
        made along the way (heightfield hole fraction, prismatic joint range, measured
        scale, kinematic-body count, limitations) so a caller can put it straight into a
        ``StageCache`` payload without re-deriving why a particular spec looks the way it
        does. A kinematic passenger being unreadable (missing geometry, no PNP-sourced
        frame to measure scale from) never raises -- it is dropped and recorded in
        ``diagnostics["limitations"]``, the same discipline :func:`load_observed_tracks`
        already applies; only ``label`` itself (``bodies[0]``) being unreadable raises.

    Raises:
        PhysicsError: Missing artifact for ``label`` itself, unknown ``label``, or a
            video-frame-axis/trajectory mismatch (never silently degrades to a
            smaller/misaligned track).
    """
    profile = _load_profile(profile)
    scratch_dir = Path(scratch_dir)
    scratch_dir.mkdir(parents=True, exist_ok=True)
    master_dir = profile.paths.master_dir(uuid, camera_serial)
    limitations: list[str] = []

    with _null_step(timer, "load_artifacts", uuid=uuid, body_label=label):
        art = _load_master_artifacts(master_dir, label)
        T_world_obj = art["T_world_obj"]
        pose_source = art["pose_source"]
        visible_frac = art["visible_frac"]
        meta = art["meta"]
        n_frames = T_world_obj.shape[0]
        valid = _valid_mask(pose_source, visible_frac)

    with _null_step(timer, "frame_index", uuid=uuid, n=n_frames):
        episode_dir = profile.paths.episode_dir(uuid)
        trajectory_path = episode_dir / "trajectory.h5"
        if not trajectory_path.exists():
            raise PhysicsError(f"{label}: trajectory.h5 not found: {trajectory_path}")
        mp4_path = episode_dir / "recordings" / "MP4" / f"{camera_serial}.mp4"
        if not mp4_path.exists():
            raise PhysicsError(f"{label}: mp4 not found: {mp4_path}")
        mp4_fps, n_video_frames, video_wh = read_mp4_properties(mp4_path)
        if n_video_frames != n_frames:
            raise PhysicsError(
                f"{label}: poses.npz has {n_frames} frames but the mp4 reports "
                f"{n_video_frames} -- video-frame-axis artifacts have gone out of sync"
            )
        dt = 1.0 / float(mp4_fps)

        with h5py.File(trajectory_path, "r") as f:
            joint_positions = np.asarray(f[TRAJECTORY_JOINT_POSITIONS_KEY])
            gripper_signal = np.asarray(f[TRAJECTORY_GRIPPER_POSITION_KEY])

        flows_path = profile.paths.flows_h5(uuid)
        with FlowsReader(flows_path, episode_uuid=uuid) as reader:
            frame_index = EpisodeFrameIndex.build(
                uuid, reader, trajectory_path,
                mp4_properties=(mp4_fps, n_video_frames, video_wh), camera_serial=camera_serial,
            )

    with _null_step(timer, "convex_hull", uuid=uuid, body_label=label):
        anchor_idx = int(_first_pnp_window(pose_source, n_vel_frames)[-1])
        anchor_rot = T_world_obj[anchor_idx, :3, :3]
        scale = _pure_scale(anchor_rot)
        r_pure = anchor_rot / scale

        geometry_path = _geometry_path(meta, label)
        points_canonical = _canonical_geometry_points(geometry_path, label)
        points_scaled = points_canonical * scale
        hull = trimesh.Trimesh(vertices=points_scaled, process=False).convex_hull
        hull_stl = scratch_dir / f"{label}_hull.stl"
        hull.export(str(hull_stl))
        logger.info(
            "%s: convex hull %d verts/%d faces (scale=%.6f, from %d canonical points at %s)",
            label, hull.vertices.shape[0], hull.faces.shape[0], scale,
            points_canonical.shape[0], geometry_path.name,
        )

    with _null_step(timer, "init_state", uuid=uuid, body_label=label):
        vel_window = _first_pnp_window(pose_source, n_vel_frames)
        lin_vel_world, ang_vel_world = _finite_difference_velocity(
            T_world_obj, vel_window, scale, dt
        )

        init_pose = np.eye(4, dtype=np.float64)
        init_pose[:3, :3] = r_pure
        init_pose[:3, 3] = T_world_obj[anchor_idx, :3, 3]

        kind, axis_world, origin_world, joint_range = _prismatic_kind_and_range(
            art["prismatic"], T_world_obj, valid, anchor_idx
        )

    body = BodySpec(
        label=label, mesh_path=hull_stl, init_pose=init_pose,
        init_lin_vel=lin_vel_world, init_ang_vel=ang_vel_world,
        kind=kind, joint_axis_world=axis_world, joint_origin_world=origin_world,
        joint_range=joint_range,
    )

    with _null_step(timer, "heightfield", uuid=uuid):
        heightfield, heightfield_diag = _build_heightfield_spec(
            profile, uuid, camera_serial, master_dir, scratch_dir,
            video_width=video_wh[0], video_height=video_wh[1], timer=timer,
        )
        if heightfield_diag["hole_frac"] > 0:
            limitations.append(
                f"heightfield: {heightfield_diag['hole_frac'] * 100:.1f}% of "
                f"{heightfield_diag['n_cells']} rasterised cells had zero directly-observed "
                f"support and were set {_HFIELD_HOLE_DROP_M:.2f} m below the observed "
                "minimum (see fpgm.physics.scene._fill_unknown_cells) -- a body cannot rest "
                "on those cells; it will fall through to an honest divergence instead. Of "
                f"that, {heightfield_diag['hole_frac_no_observation'] * 100:.1f}pp was never "
                f"observed at all (independent of any tracked object) and "
                f"{heightfield_diag['hole_frac_no_clean_observation'] * 100:.1f}pp WAS "
                "observed but a tracked object covers it in every single frame (see "
                "fpgm.physics.scene._tracked_object_carve_mask)."
            )
        for skipped_label, reason in heightfield_diag["carve"]["roles_skipped"].items():
            limitations.append(
                f"heightfield carve: {skipped_label!r} was NOT excluded from the static "
                f"surface ({reason}) -- if this object sits mostly still, its own body may "
                "still be baked into the heightfield at its own footprint (see "
                "fpgm.physics.scene._tracked_object_carve_mask's docstring)."
            )

    with _null_step(timer, "finger_poses", uuid=uuid, n=n_frames):
        robot = RobotModel(str(profile.paths.urdf), load_meshes=True)
        finger_size = _finger_geometry(robot)
        finger_poses = np.empty((n_frames, 2, 4, 4), dtype=np.float64)
        for vf in range(n_frames):
            row = frame_index.trajectory_row(vf)
            g = float(gripper_signal[row])
            poses = robot.link_poses(joint_positions[row], g)
            finger_poses[vf, 0] = poses["left_inner_finger"]
            finger_poses[vf, 1] = poses["right_inner_finger"]
        gripper = GripperSpec(finger_poses=finger_poses, finger_size=finger_size)

    with np.load(master_dir / "poses.npz") as npz:
        all_labels = [str(x) for x in npz["labels"]]
    other_labels = [lbl for lbl in all_labels if lbl != label]
    kinematic_bodies: list[BodySpec] = []
    with _null_step(timer, "kinematic_bodies", uuid=uuid, n=max(len(other_labels), 1)):
        for other_label in other_labels:
            try:
                kbody, klimitations = _kinematic_body_spec(
                    master_dir, other_label, n_frames, scratch_dir,
                )
            except PhysicsError as exc:
                limitations.append(
                    f"{other_label}: dropped as a kinematic body in {label}'s scene ({exc})"
                )
                logger.warning(
                    "%s/%s: dropping kinematic body %r from %r's scene: %s",
                    uuid, camera_serial, other_label, label, exc,
                )
                continue
            kinematic_bodies.append(kbody)
            limitations.extend(klimitations)

    spec = SimSpec(
        uuid=uuid, camera_serial=camera_serial, dt=dt, n_frames=n_frames,
        bodies=[body, *kinematic_bodies], gripper=gripper, heightfield=heightfield,
        substeps=substeps,
    )

    with np.load(heightfield.grid_path) as hz:
        anchor_surface_z = _surface_height_under_xy(
            np.asarray(hz["height_m"]), np.asarray(hz["valid"]), heightfield.origin_xy,
            heightfield.cell_size_m, init_pose[:3, 3][:2],
        )
    anchor_height_above_heightfield_m = None
    if anchor_surface_z is not None:
        anchor_height_above_heightfield_m = float(init_pose[2, 3] - anchor_surface_z)
        if anchor_height_above_heightfield_m > 0.05:
            limitations.append(
                f"anchor frame ({anchor_idx}) is {anchor_height_above_heightfield_m:.3f} m "
                "above the observed heightfield surface directly under it -- the object is "
                "not resting near the static scene surface at simulation start (e.g. it has "
                "not been placed there yet in the episode); a free body will free-fall this "
                "whole distance before any contact is possible, which can exhaust n_frames or "
                "hit the numerical safety floor (ok=False) before ever reaching a surface. "
                "This is not a bug: it is an honest consequence of always anchoring at the "
                "FIRST PNP frames (see module docstring) regardless of which episode phase "
                "the surface below is relevant to."
            )
    else:
        limitations.append(
            f"anchor frame ({anchor_idx})'s (x, y) falls on an UNOBSERVED heightfield cell -- "
            "no directly-measured surface height is available directly under the object at "
            "simulation start (see fpgm.physics.scene._surface_height_under_xy)."
        )

    diagnostics: dict[str, Any] = {
        "kind": kind,
        "scale": scale,
        "bounding_hull_n_vertices": int(hull.vertices.shape[0]),
        "n_kinematic_bodies": len(kinematic_bodies),
        "anchor_frame_idx": anchor_idx,
        "vel_window_frame_idx": vel_window.tolist(),
        "init_lin_speed_mps": float(np.linalg.norm(lin_vel_world)),
        "init_ang_speed_radps": float(np.linalg.norm(ang_vel_world)),
        "joint_range_m": list(joint_range) if kind == "prismatic" else None,
        "heightfield": heightfield_diag,
        "anchor_height_above_heightfield_m": anchor_height_above_heightfield_m,
        "mp4_fps": float(mp4_fps),
        "substeps": substeps,
        "sim_dt_s": dt / substeps,
        "limitations": limitations,
    }
    return spec, diagnostics