diff --git a/README.md b/README.md index 4262a89..8281c25 100644 --- a/README.md +++ b/README.md @@ -114,6 +114,99 @@ All three fixes are real-only. The sim/real detector switches them on, and the s - **The brain has not been tested on the robot.** End-to-end runs with the real llama.cpp server take place on a GPU simulator box. The unit tests cover only a server that is down or late. +## Real-robot postmortem + +In the finals the policy scored 0 points in both real runs (`sort_fixed`, seed 0, 15 Nov 2026). In simulation the same +code scored 478 of 527 objects over 128 episodes (64 seeds x 2 tasks), with 2 objects placed in the wrong bin. This +section records what happened on the robot, the cause, and what we changed afterwards. The brain never ran on the +real robot. + +### What happened + +The bins at the venue were wrapped in red-orange paper. In the simulator they are plain orange-yellow cardboard. In both +runs the colour gate kept 8 of 9 candidates, the policy picked "red", and the right arm went to a grasp point on the +right bin. The hand closed on the lower edge of the bin's front wall and dragged the bin out of position. The held +check passed, the release check failed twice, and the policy stopped at about 39 s of a 120 s run. No object was moved. + +![Head camera at t = 11 s. Green marks the pixels the colour gate still accepted as red: three of the five objects, and a block on the right bin's front wall.](docs/postmortem/gate_overlay.jpg) + +![Run 1, t = 11 to 35 s. The right hand reaches the right bin's front wall and the bin slides out of position.](docs/postmortem/bin_dragged_sequence.jpg) + +### Cause + +1. **Gate row cutoff.** The image gate (`gate_below_bins`) only counted bin runs starting above row 216 + (`first_run_top_frac` 0.45 of 480). The right bin's front face starts at rows 215 to 222, so the gate put its + bottom edge near row 89 instead of about 277 and let the wall through. +2. **Defences skipped while the gate is on.** With the gate active, the 3D footprint cut, the `in_bin` reject and the + colour gate's workspace and bin rules were bypassed. The gate was meant as an extra layer and acted as a switch + for the others. +3. **Blob splitting.** `split_blobs` cut the wall strip into cube-sized pieces before the `too_big` check ran. Colour + could not separate them: robust red covers hue 0 to 16, the paper sits at 4 to 8 and the orange objects at 11 to 12. +4. **Hard-coded mapping.** `policy_server.py` always injected the venue colour-to-bin mapping for `sort_fixed`, so the + instruction-first path was never reached. The instruction said red goes left, the mapping said right. +5. **`bin.vote` disagreed** in both runs (confidence 0.3), so live bin positions were not trusted. The real bins were + 8 to 15 cm from the nominal footprint. The logs do not show why the vote disagreed. +6. **Held detector and early stop.** Finger sum was 0.266 at close and 0.046 at lift against a 0.045 threshold, so a + bin wall counted as held. After two failed release checks the policy stopped picking with 80 s left. + +The hackathon hotfix (`bin_size_guard`) did not help: the surviving strip was below its size thresholds. + +### Offline replay + +`tools/venue_replay.py` runs perception on 11 labelled frames from the two real runs (RGB only, no depth, no GPU) and +counts accepted colour candidates that lie on a bin. + +| Config | Candidates on a bin (11 frames) | Objects found | +|---|---|---| +| As run on the robot | 20 | 55/55 | +| As run + hotfix `bin_size_guard` | 20 | 55/55 | +| After the fix | 2 | 55/55 | + +The two remaining candidates are the robot's own fingers, which the arm mask removes on the robot. On the frame the +robot used to choose its target, the old code ranks the right bin's wall first as "red"; the fix ranks the orange +cube first. + +### Simulation check: inconclusive + +We built a venue-twin scene (`aaltonomous/sim/VENUE_TWIN.md`: paper-coloured bin walls, brown bin interiors, venue +table and object colours sampled from the run video) and ran an A/B in Isaac Sim on `sort_fixed`, seeds 1000 to 1003: +the submitted code and the fix, each on the venue twin and on the nominal scene. + +| Arm | Scene | Objects correct / total | Clips | Rate | +|---|---|---|---|---| +| submitted | nominal | 0 / 15 | 0 | ~30 Hz | +| submitted | venue twin | 0 / 15 | 0 | ~30 Hz | +| fix | nominal | 0 / 15 | 0 | ~30 Hz | +| fix | venue twin | 0 / 15 | 0 | ~30 Hz | + +All four arms scored zero. The images were built with the real profile forced (`PROFILE_MODE=real`), and that profile +does not sort in simulation at all, so this run neither reproduces the failure nor validates the fix. No +`[grasp_diag]` lift values were captured. The only validation of the fix is the offline replay above. + +### The fix and what it does not cover + +The fix turns on `bin_image_guard` (rejects dark-warm components of bin size in the image), raises +`first_run_top_frac` to 0.55, keeps the `in_bin` defence active while the gate is on, and makes the `sort_fixed` +mapping instruction-first. `bin.vote` now logs its voters, and `bin.vote.tape_body_reject` ignores paper-wrapped bin +bodies (7.9k to 19k px in the replay frames) when looking for tape. Unit tests cover each piece. + +Not covered: +- Not validated on real depth or on the robot. +- `grasp.wrong_object_recovery` (a stricter post-lift finger check and a bounded re-look after a failed release) is + implemented but **off**: it needs a lift finger sum of at least 0.09, and only the venue bin wall (0.046 and 0.088) + has ever been measured, never a genuine grasp. +- The reason `bin.vote` disagreed is unknown, and the organisers' colour-to-bin mapping is unconfirmed. + +### Lessons + +- Colour thresholds tuned on sim materials did not transfer: bin paper, orange objects and the red range overlapped. +- A guard that disables the footprint, `in_bin` and `too_big` checks turns three independent layers into one. +- Eleven frames and an RGB-only replay reproduced the failure offline. We had no venue-coloured data in the loop + before the finals. +- The abstain rules in the resolver never fired because perception handed over a confident wrong candidate, and the + held detector then confirmed it. +- Two failed release checks ended a 120 s run at 39 s. A bounded retry would have given a second attempt. + ## Build the submitted image To run it without building, pull the submitted image. It is a byte-identical, digest-pinned copy published by diff --git a/aaltonomous/baselines/classical/bin_vote.py b/aaltonomous/baselines/classical/bin_vote.py index 884fbc3..126db30 100644 --- a/aaltonomous/baselines/classical/bin_vote.py +++ b/aaltonomous/baselines/classical/bin_vote.py @@ -98,13 +98,15 @@ def _box_edge_dist(Q, ctr, half): return np.abs(sd) -def rim_votes(perception, depth, centers, v, transform_snapshot=None): +def rim_votes(perception, depth, centers, v, transform_snapshot=None, diag=None): """{bin: xy|None}: per-bin fit of the outer+inner rim rectangles to elevated depth points, +-search_m window.""" sc, cfg = perception.scene, perception.cfg c = cfg.get("self_calibration") or {} b = sc.cfg["bins"] out = {n: None for n in centers} + diag = {} if diag is None else diag if depth is None or np.ndim(depth) != 2: + diag["why"] = "no depth" return out clean_depth = np.nan_to_num(np.asarray(depth, float), nan=0.0) if transform_snapshot is None: @@ -112,6 +114,7 @@ def rim_votes(perception, depth, centers, v, transform_snapshot=None): else: P, coef, why = perception._counter_plane(clean_depth, c, transform_snapshot=transform_snapshot) if why: + diag["why"] = f"plane: {why}" return out height = P[:, 2] - (np.c_[P[:, :2], np.ones(len(P))] @ coef) keep = (height >= float(c.get("min_wall_height_m", 0.008))) & (height <= float(b["wall_height"]) + 0.015) @@ -122,6 +125,7 @@ def rim_votes(perception, depth, centers, v, transform_snapshot=None): trunc = 0.015 for n, Q in _nearest_bin_points(P, centers, outer, search).items(): if len(Q) < int(v["min_rim_points"]): + diag[n] = f"{len(Q)} pts < min {int(v['min_rim_points'])}" continue Q = Q[np.linspace(0, len(Q) - 1, min(len(Q), 1500)).astype(int)] signed = bool(v.get("signed_rim_fit", False)) @@ -142,7 +146,10 @@ def search_grid(ctr, span, step): off = search_grid(off, 0.002, 0.0005) fit = Q - off if signed else Q + off d = np.minimum(_box_edge_dist(fit, centers[n], outer), _box_edge_dist(fit, centers[n], inner)) - if float(np.mean(d <= 0.006)) < 0.35: + inl = float(np.mean(d <= 0.006)) + diag[n] = f"{len(Q)} pts, inlier={inl:.2f}, off=({1000 * off[0]:+.0f},{1000 * off[1]:+.0f})mm" + if inl < 0.35: + diag[n] += " REJECTED (inlier < 0.35)" continue out[n] = np.asarray(centers[n], float) + off return out @@ -178,7 +185,7 @@ def _bin_blob_vote(blobs, elong): return _blob_vote(blobs[0], elong) -def tape_votes(perception, rgb, depth, centers, v, transform_snapshot=None): +def tape_votes(perception, rgb, depth, centers, v, transform_snapshot=None, diag=None): """{bin: xy|None}: per orange-tape blob near the bin's rim (8-connected in the image), lifted via depth or via the rim-height plane where depth is missing; the blob votes its centroid, or its inner end when elongated (see _blob_vote). Ambiguous or missing -> None. # ponytail: no per-edge tape layout model; upgrade once the real @@ -186,7 +193,9 @@ def tape_votes(perception, rgb, depth, centers, v, transform_snapshot=None): import cv2 sc = perception.scene out = {n: None for n in centers} + diag = {} if diag is None else diag if rgb is None or np.ndim(rgb) != 3: + diag["why"] = "no rgb" return out hsv = cv2.cvtColor(np.ascontiguousarray(rgb), cv2.COLOR_RGB2HSV) lo, hi = v["tape_hsv"] @@ -194,9 +203,19 @@ def tape_votes(perception, rgb, depth, centers, v, transform_snapshot=None): mask[int(mask.shape[0] * TAPE_V_MAX_FRAC):] = 0 vv, uu = np.nonzero(mask) if len(vv) < int(v["min_tape_px"]): + diag["why"] = f"{len(vv)} tape-coloured px < min" return out labels = cv2.connectedComponents(mask, connectivity=8)[1] lab = labels[vv, uu] + sizes = np.bincount(lab) + diag["blob_px"] = sorted((int(x) for x in sizes[1:] if x >= int(v["min_tape_px"])), reverse=True)[:8] + max_px = int(v.get("tape_max_px", 0) or 0) if v.get("tape_body_reject", False) else 0 + if max_px > 0: # venue: the red-orange paper-wrapped bin body (8-19k px) matches tape_hsv; real tape is a thin strip + big = sizes[lab] > max_px + diag["rejected_body_px"] = sorted((int(x) for x in np.unique(sizes[lab[big]])), reverse=True) + vv, uu, lab = vv[~big], uu[~big], lab[~big] + if len(vv) < int(v["min_tape_px"]): + return out rim_z = sc.counter_z + float(sc.cfg["bins"]["wall_height"]) d = np.zeros(len(vv)) if depth is not None and np.ndim(depth) == 2: @@ -241,6 +260,7 @@ def tape_votes(perception, rgb, depth, centers, v, transform_snapshot=None): per_bin[names[k]].append(B) for n in names: out[n] = _bin_blob_vote(per_bin[n], float(v["tape_elong_aspect"])) + diag["blobs_per_bin"] = {n: [len(b) for b in per_bin[n]] for n in names} return out @@ -249,26 +269,50 @@ def _med(frames, n): return np.median(xs, axis=0) if xs else None +def _fmt(x): + return "none" if x is None else f"({x[0]:.3f},{x[1]:.3f})" + + +def _log_voters(log, names, priors, rim, tape, diags): + """[bin.vote.voters]: every voter's estimate (pelvis xy, m) + distance to the prior + why a voter was missing, so a + disagreement (conf 0.3) is explained by the next real run's log. Never raises.""" + try: + for n in names: + parts = [] + for k, est in (("rim", rim[n]), ("tape", tape[n])): + dist = "" if est is None else f" d_prior={1000 * float(np.linalg.norm(np.asarray(est) - priors[n])):.0f}mm" + parts.append(f"{k}={_fmt(est)}{dist}") + log(f"[bin.vote.voters] {n}: prior={_fmt(priors[n])} " + " ".join(parts)) + rd, td = diags[0] + log(f"[bin.vote.voters] frame0 rim: {dict(rd)}") + log(f"[bin.vote.voters] frame0 tape: {dict(td)}") + except Exception as exc: + log(f"[bin.vote.voters] log failed ({type(exc).__name__})") + + def vote_bins(perception, planner, frames, cfg, log=print): """frames: [{"head_rgb", "head_depth"}]. -> (shift, drop_extra, conf). Never raises (prior on any error).""" v = cfg["bin"]["vote"] names = list(planner.scene.bin_names) priors = {n: np.asarray(planner.bin_xy(n), float) for n in names} try: - rims, tapes = [], [] + rims, tapes, diags = [], [], [] for f in frames: + rd, td = {}, {} + diags.append((rd, td)) if v.get("frame_transform_snapshot", False): T = f["transform_snapshot"] - rims.append(rim_votes(perception, f.get("head_depth"), priors, v, T)) - tapes.append(tape_votes(perception, f.get("head_rgb"), f.get("head_depth"), priors, v, T)) + rims.append(rim_votes(perception, f.get("head_depth"), priors, v, T, diag=rd)) + tapes.append(tape_votes(perception, f.get("head_rgb"), f.get("head_depth"), priors, v, T, diag=td)) else: frame = f.get("transform") or perception.frame_transform() with perception.use_frame_transform(frame): - rims.append(rim_votes(perception, f.get("head_depth"), priors, v)) - tapes.append(tape_votes(perception, f.get("head_rgb"), f.get("head_depth"), priors, v)) + rims.append(rim_votes(perception, f.get("head_depth"), priors, v, diag=rd)) + tapes.append(tape_votes(perception, f.get("head_rgb"), f.get("head_depth"), priors, v, diag=td)) rim = {n: _med(rims, n) for n in names} tape = {n: _med(tapes, n) for n in names} shift, extra, info = resolve(priors, rim, tape, v) + _log_voters(log, names, priors, rim, tape, diags) except Exception as exc: # optional feature: never terminate the control loop log(f"[bin.vote] failed ({type(exc).__name__}: {exc}); prior positions") return {}, {n: float(v["low_conf_extra_m"]) for n in names}, {n: CONF_PRIOR for n in names} diff --git a/aaltonomous/baselines/classical/config.py b/aaltonomous/baselines/classical/config.py index 250b545..c566c6c 100644 --- a/aaltonomous/baselines/classical/config.py +++ b/aaltonomous/baselines/classical/config.py @@ -147,6 +147,9 @@ "perception.prism_speckle_floor": ("prism floor", False, "color_gate speckle floor: orange/red blobs with a triangular outline (minEnclosingTriangle fill) use prism_speckle_floor_params.area_frac instead of color_gate.area_frac"), "perception.real_border_partial": ("real perc", False, "real only: color_gate speckle floor uses real_border_partial_params.area_frac for blobs cut by the left/right/bottom image border"), "perception.bin_bottom_first_run": ("real perc", False, "real only: gate_below_bins observed bin-bottom row = first vertical warm run per column (a shadowed orange object on the mat is not bin mass)"), + "perception.bin_size_guard": ("real perc", False, "real only (hotfix): a colour blob that is bin-sized (area / 3D footprint) or bin-tall is rejected before split_blobs (bin_size_guard_params)"), + "perception.bin_image_guard": ("real perc", False, "real only (venue replay): a colour blob that lies on a bin-sized dark-warm image component (bin wall in shade, V < 150) is rejected before split_blobs; needs no depth (bin_image_guard_params). Also keeps the nominal in_bin reject active while the image gate_below_bins is on"), + "perception.gate_below_bins_params.first_run_top_frac": ("real perc", 0.45, "gate_below_bins + bin_bottom_first_run: a warm run counts as bin wall only if it starts in the top this-fraction of the frame (venue: 0.45 cut the shaded bin wall; finals_r25_gen 0.55)"), "perception.fk_colour_rescue": ("real perc", False, "real only: colour pixels inside the FK arm mask stay candidates; detections overlapping the mask must sit at object height (fk_colour_rescue_params.z_above_counter_m)"), "perception.real_merge_split": ("real perc", False, "real only: split_blob tries merged same-colour pairs from real_merge_split_params.area_factor x expected area (default 0.9 instead of 1.5)"), "perception.gate_below_bins": ("gate fix", False, "when live bins are untrusted, gate candidates below observed bin mass in image space"), @@ -266,6 +269,8 @@ "bin.vote.max_shift_m": ("bin vote", 0.10, "a bin vote result further than this from the prior is discarded (prior kept); real venue bins measure +8..+15 cm vs nominal"), "bin.vote.enabled": ("bin vote", False, "per-bin position from 3 votes (depth rim, tape, prior) over the first frames"), "bin.vote.signed_rim_fit": ("C3", False, "fit physical rim displacement with its signed +x/+y direction"), + "bin.vote.tape_body_reject": ("venue", False, "bin.vote: a tape-coloured blob larger than bin.vote.tape_max_px is a paper-wrapped bin body (venue: 8-19k px), not tape: it casts no tape vote (else the body, lifted to rim height, disagrees with prior and rim -> conf 0.3)"), + "grasp.wrong_object_recovery": ("venue", False, "real: (a) the post-lift finger read must clear grasp.wrong_object_recovery_params.lift_margin_mult x threshold (and optionally lift_min_ratio x the close read) else open + retreat + blacklist the spot (bin wall mistaken for a cube); (b) a failed release_confirm opens fully, goes home and keeps picking (max_recoveries per episode) instead of stopping"), "bin.vote.async_extract": ("H5", False, "extract bin votes in the low-priority look worker; act only polls/holds"), "bin.vote.frame_transform_snapshot": ("H4", False, "one immutable camera transform per frame shared by rim and tape"), "hand.closed_tight": ("pinch fix", STOCK, "the closed_tight posture (LEFT DDS order, right mirrored)"), @@ -461,7 +466,7 @@ def check_flag_values(cfg): if not isinstance(mc, int) or isinstance(mc, bool) or mc < 0: raise ValueError(f"instruction.max_chars = {mc!r}: want an int >= 0 (0 = off)") for key in ("safety.nan_guard", "policy.generation_workers", "grasp.held_calibration.enabled", - "grasp.drop_slot_grid"): + "grasp.drop_slot_grid", "bin.vote.tape_body_reject", "grasp.wrong_object_recovery"): if not isinstance(_get(cfg, key), bool): raise ValueError(f"{key}: want true or false") cb = _get(cfg, "planner.candidate_budget") @@ -489,7 +494,7 @@ def check_flag_values(cfg): p = cfg.get("perception") or {} for key in ("sim_center_principal_point", "split_by_solidity", "tape_value_cut", "gate_below_bins", "bins_from_image", "reject_log", "still_vote", "prism_speckle_floor", "real_border_partial", "real_merge_split", - "bin_bottom_first_run", "fk_colour_rescue"): + "bin_bottom_first_run", "fk_colour_rescue", "bin_size_guard", "bin_image_guard"): if not isinstance(p.get(key), bool): raise ValueError(f"perception.{key}: want true or false") aim = p.get("aim_z_frac", 0.25) diff --git a/aaltonomous/baselines/classical/config.yaml b/aaltonomous/baselines/classical/config.yaml index 322209e..1cc3237 100644 --- a/aaltonomous/baselines/classical/config.yaml +++ b/aaltonomous/baselines/classical/config.yaml @@ -296,7 +296,8 @@ perception: tape_value_cut_params: {min_s: 140, min_v: 140, min_core_frac: 0.20, min_aspect: 3.0, max_thickness_px: 18} gate_below_bins: false # FLAG: untrusted live bins use the observed bin/tape mass instead of nominal projected geometry gate_below_bins_params: {h_max: 17, s_min: 77, v_min: 50, object_s_min: 140, object_v_min: 150, - open_px: 5, min_component_px: 2000, min_mass_px: 18000, margin_px: 3, trusted_bin_conf: 0.5} + open_px: 5, min_component_px: 2000, min_mass_px: 18000, margin_px: 3, trusted_bin_conf: 0.5, + first_run_top_frac: 0.45} bins_from_image: false # FLAG: find the taped bin rims in the image (rim_detect); image bin masks replace the nominal # projected hull / in_bin when the runtime bin poses are untrusted, tape is never a target bins_from_image_params: {hint: false, with_depth: true, hint_max_m: 0.25, hint_max_resid_px: 6.0, crosscheck_px: 12.0, @@ -311,6 +312,10 @@ perception: fk_colour_rescue_params: {min_overlap_frac: 0.02, min_area_px: 1200, z_above_counter_m: [0.034, 0.056]} real_merge_split: false # FLAG (real only): split merged same-colour neighbours (cube+cylinder) from a lower area factor real_merge_split_params: {area_factor: 0.9, min_part_frac: 0.3} + bin_size_guard: false # FLAG (real only, hotfix): reject a colour blob that is bin-sized (area / footprint) or bin-tall, before split_blobs + bin_size_guard_params: {max_area_factor: 4.0, max_top_m: 0.075, max_extent_m: 0.13, min_point_height_m: 0.01} + bin_image_guard: false # FLAG (real only, venue replay): reject a colour blob lying on a bin-sized dark-warm image component (bin wall in shade), before split_blobs; no depth needed + bin_image_guard_params: {h_max: 17, s_min: 77, v_min: 50, object_v_min: 150, open_px: 5, min_frac: 0.5, min_side_objects: 1.8} look_summary: false # FLAG (fsm, passive). per-look colour-blob / rejection summary for the policy's [look0] 0-detection line reject_log: false # FLAG: per-frame log plus cumulative per-colour rejected-stage tally still_vote: false # FLAG: one detect per still frame; publish tracks seen in min_hits of the last n_frames @@ -409,6 +414,8 @@ bin: min_rim_points: 180 min_tape_px: 60 tape_hsv: [[3, 150, 70], [8, 255, 180]] + tape_body_reject: false # FLAG (venue). tape-coloured blobs above tape_max_px are a paper-wrapped bin body, not tape: no tape vote + tape_max_px: 3000 # tape_body_reject: venue bin faces measure 8-19k px in the head image; a tape strip is far smaller grasp: hold_s: 3.0 # skill "hold": seconds the hand stays closed with the arm still, right after the lift @@ -433,6 +440,12 @@ grasp: held_detector: stall # FLAG: stall = legacy max |cmd-q|; finger_sum = real-Dex3 bias-corrected finger pairs release_wait_s: null # FLAG (lag R44): null = today (open_hold_s); s = hold >= this after the open command before the retreat starts release_confirm: false # FLAG (lag R49): after open + wait the finger state must read empty (held detector); not empty -> re-open once, then home and stop picking + wrong_object_recovery: false # FLAG (venue). lift-time held margin + bounded recovery after a failed release_confirm (params below) + wrong_object_recovery_params: + lift_margin_mult: 2.0 # lift finger_sum must reach threshold x this (venue bin wall: 0.046 / 0.088 vs 0.045 passed the plain threshold) + lift_min_ratio: 0.0 # >0: the lift read must also keep this fraction of the close read (a slipping grip falls off); 0 = off + max_recoveries: 2 # failed release_confirm -> open, home, re-look; at most this many per episode, then stop as before + bad_radius_m: 0.06 # pelvis xy radius around a rejected grasp point that is never planned again this episode release_confirm_s: 1.5 # release_confirm: how long to poll for an empty hand after each open (Dex3 lag 0.3-0.4 s) held_threshold_rad: 0.045 # finger_sum: held when max(index j0+j1, middle j0+j1), above the open reference, reaches this held_calibration: # M3 FLAG: an empty close at home supplies e0; invalid/unfinished calibration disables abort diff --git a/aaltonomous/baselines/classical/overlays/finals_r25_gen.yaml b/aaltonomous/baselines/classical/overlays/finals_r25_gen.yaml index 5794c1a..a65db26 100644 --- a/aaltonomous/baselines/classical/overlays/finals_r25_gen.yaml +++ b/aaltonomous/baselines/classical/overlays/finals_r25_gen.yaml @@ -117,6 +117,11 @@ flags: perception.real_merge_split: true # realperc-r23 (real-only; sim_overrides false) perception.bin_bottom_first_run: true # realperc-final (real-only; sim_overrides false): shadowed orange object is not bin mass perception.fk_colour_rescue: true # realperc-final (real-only; sim_overrides false): FK silhouette off by ~80 px on real frames; height-judge instead + perception.bin_size_guard: true # hotfix (real only; sim_overrides false): bin-sized/bin-tall colour blob is never a candidate (venue: robot picked an orange bin) + bin.vote.tape_body_reject: true # venue fix: paper-wrapped bin bodies (8-19k px) matched tape_hsv and voted a bogus rim position (conf 0.3 in both runs) + grasp.wrong_object_recovery: false # UNVALIDATED, kept OFF: needs post-lift finger_sum >= 2.0 x 0.045 = 0.09 but no genuine-grasp lift value was ever measured (only the venue bin wall: 0.046 / 0.088); enabling could reject real picks. Flag + tests remain. + perception.bin_image_guard: true # venue fix (ON in sim too, perception-only): dark-warm bin-sized image component is never a candidate; also keeps the nominal in_bin reject active under the image gate_below_bins + perception.gate_below_bins_params.first_run_top_frac: 0.55 # venue fix: 0.45 cut off the shaded bin wall at the venue (robot grabbed the bin); sim bins unaffected perception.real_border_partial: false # realperc-r23 (real-only) perception.split_by_solidity: false # realperc-r23 (real-only) perception.prism_speckle_floor: true # realperc-r23 (real-only) @@ -125,7 +130,7 @@ flags: grasp.approach_slow_mps: 0.05 # real demo: last 5 cm of the descend <= 0.05 m/s grasp.novel_near_rim_retry: true # novel R22 (SHA2 2513d3f) perception.novel.fp_filter: true # oliver/novel-fp-v2 94aa3a8 (v2.2, arm-still gate); brain variant ON - profile_auto.sim_overrides: {bin.fallback_centre_x: null, bin.fallback_drop_z: null, bin.reach_x_max: null, bin.reach_drop_z: null, calib.plane.robust_agree_m: 0.03, perception.height_range: [0.4, 2.0], calib.plane.robust_height: false, perception.plan_frame: fitted, policy.contact_start_unblock: false, perception.look_summary: false, policy.late_sweep: false, planner.candidate_budget: {max_ik_calls: 800, max_wall_s: 10.0}, policy.zero_det_relook: false, policy.startup_overlap: false, policy.descend_contact_guard: false, bin.geometry_runtime: false, bin.vote.async_extract: false, bin.vote.enabled: false, bin.vote.frame_transform_snapshot: false, bin.vote.max_shift_m: 0.1, bin.vote.signed_rim_fit: false, calib.plane.apply_height: false, grasp.drop_slot_grid: false, grasp.held_calibration.enabled: false, grasp.held_detector: stall, grasp.left_hand_fix.shift_z: 0.01, grasp.release_confirm: false, grasp.release_wait_s: null, hand.mirror_index_middle: false, instruction.max_chars: 0, instruction.multilingual: false, instruction.venue_veto: false, instruction.strict: false, instruction.venue_colour_aliases: false, motion.arm_vel_cap: null, motion.output_caps: null, motion.sag_comp: false, perception.color_gate.enabled: false, perception.color_gate.fk_dilate_px: 0, perception.color_gate.fk_mask: false, perception.color_gate.live_bins: false, perception.color_gate.near_margin_m: 0.0, perception.color_gate.sat_floor: false, perception.gate_below_bins: false, perception.online_pitch_cal: false, perception.table_pitch_calibration.max_abs_pitch_deg: 15.0, planner.lift_from_contact: false, policy.fixed_instruction_first: false, policy.fixed_mapping: null, perception.color_gate.fk_silhouette: false, perception.prism_speckle_floor: false, perception.real_border_partial: false, perception.real_merge_split: false, perception.bin_bottom_first_run: false, perception.fk_colour_rescue: false, perception.split_by_solidity: false, policy.frozen_rgb_guard: false, policy.generation_workers: false, planner.carry_corridor: false, policy.safety_shell: false, safety.nan_guard: false, scene.counter_z_prior: null, grasp.point_in_palm: [0.0862, -0.0864, 0.0143], hand.closed_tight: [0.0, 0.0, 1.2, -1.0, -0.6, -1.0, -0.6], grasp.arm_xy_offset: {left: [0.0, -0.01], right: [0.0, 0.0]}, grasp.close_posture_by_shape: true, grasp.drop_height_extra_m: 0.0, grasp.approach_slow_mps: 0.0, insert.reach_gated: true, insert.pg_bias_xy: [0.007, 0.003]} # sim only: ALL 28 rc1-vs-r22 keys at rc1 values + gate_below_bins/fixed_instruction_first/safety_shell off -> sim = rc1 + lang (X8 A43); real robot keeps r23 + profile_auto.sim_overrides: {perception.bin_size_guard: false, bin.fallback_centre_x: null, bin.fallback_drop_z: null, bin.reach_x_max: null, bin.reach_drop_z: null, calib.plane.robust_agree_m: 0.03, perception.height_range: [0.4, 2.0], calib.plane.robust_height: false, perception.plan_frame: fitted, policy.contact_start_unblock: false, perception.look_summary: false, policy.late_sweep: false, planner.candidate_budget: {max_ik_calls: 800, max_wall_s: 10.0}, policy.zero_det_relook: false, policy.startup_overlap: false, policy.descend_contact_guard: false, bin.geometry_runtime: false, bin.vote.async_extract: false, bin.vote.enabled: false, bin.vote.frame_transform_snapshot: false, bin.vote.max_shift_m: 0.1, bin.vote.signed_rim_fit: false, calib.plane.apply_height: false, grasp.drop_slot_grid: false, grasp.held_calibration.enabled: false, grasp.held_detector: stall, grasp.left_hand_fix.shift_z: 0.01, grasp.release_confirm: false, grasp.release_wait_s: null, hand.mirror_index_middle: false, instruction.max_chars: 0, instruction.multilingual: false, instruction.venue_veto: false, instruction.strict: false, instruction.venue_colour_aliases: false, motion.arm_vel_cap: null, motion.output_caps: null, motion.sag_comp: false, perception.color_gate.enabled: false, perception.color_gate.fk_dilate_px: 0, perception.color_gate.fk_mask: false, perception.color_gate.live_bins: false, perception.color_gate.near_margin_m: 0.0, perception.color_gate.sat_floor: false, perception.gate_below_bins: false, perception.online_pitch_cal: false, perception.table_pitch_calibration.max_abs_pitch_deg: 15.0, planner.lift_from_contact: false, policy.fixed_instruction_first: false, policy.fixed_mapping: null, perception.color_gate.fk_silhouette: false, perception.prism_speckle_floor: false, perception.real_border_partial: false, perception.real_merge_split: false, perception.bin_bottom_first_run: false, perception.fk_colour_rescue: false, perception.split_by_solidity: false, policy.frozen_rgb_guard: false, policy.generation_workers: false, planner.carry_corridor: false, policy.safety_shell: false, safety.nan_guard: false, scene.counter_z_prior: null, grasp.point_in_palm: [0.0862, -0.0864, 0.0143], hand.closed_tight: [0.0, 0.0, 1.2, -1.0, -0.6, -1.0, -0.6], grasp.arm_xy_offset: {left: [0.0, -0.01], right: [0.0, 0.0]}, grasp.close_posture_by_shape: true, grasp.drop_height_extra_m: 0.0, grasp.approach_slow_mps: 0.0, insert.reach_gated: true, insert.pg_bias_xy: [0.007, 0.003]} # sim only: ALL 28 rc1-vs-r22 keys at rc1 values + gate_below_bins/fixed_instruction_first/safety_shell off -> sim = rc1 + lang (X8 A43); real robot keeps r23 brain.backend: llamacpp brain.call_timeout_s: 4.0 brain.closed_loop: true diff --git a/aaltonomous/baselines/classical/perception.py b/aaltonomous/baselines/classical/perception.py index e6ba153..5f69625 100644 --- a/aaltonomous/baselines/classical/perception.py +++ b/aaltonomous/baselines/classical/perception.py @@ -406,6 +406,12 @@ def __init__(self, cfg, scene, calibration, log=None): self.fk_rescue_cfg = dict(self.cfg.get("fk_colour_rescue_params") or {}) self.real_merge_split = bool(self.cfg.get("real_merge_split", False)) self.real_merge_cfg = dict(self.cfg.get("real_merge_split_params") or {}) + # hotfix (real only): a bin-sized / bin-tall colour blob is a bin, never a pick candidate (checked BEFORE split_blobs, + # which otherwise slices one 21 cm orange bin wall into up to 3 cube-sized parts) + self.bin_size_guard = bool(self.cfg.get("bin_size_guard", False)) + self.bin_size_cfg = dict(self.cfg.get("bin_size_guard_params") or {}) + self.bin_image_guard = bool(self.cfg.get("bin_image_guard", False)) + self.bin_image_cfg = dict(self.cfg.get("bin_image_guard_params") or {}) self.partial_ok_near_hand = bool(self.cfg.get("partial_ok_near_hand", False)) self.partial_near_hand_cfg = dict(self.cfg.get("partial_ok_near_hand_params") or {}) # perception.neutral_obstacles (FLAG, default OFF): non-target dark / grey blobs are never detections; they @@ -1750,7 +1756,8 @@ def _detect_frame(self, rgb, depth, debug=False, include_bins=False, arm_clear=T # either is on, share one RGB->HSV conversion. need_hsv = (self.place_gate or self.colour_kmeans or self.hue_guard or self.hue_guard_yellow_fix or self.sat_floor or self.sat_floor_adaptive or self.depth_blob_recover or self.tape_value_cut or self.split_by_solidity or self.gate_below_bins - or self.glare_reject or self.reflection_reject or self.local_bg or self.neutral_on or self.bins_from_image) + or self.glare_reject or self.reflection_reject or self.local_bg or self.neutral_on or self.bins_from_image + or self.bin_image_guard) self._neutral_new = [] if (self.neutral_on and not include_bins) else None hsv = early_hsv if early_hsv is not None else ( cv2.cvtColor(np.ascontiguousarray(rgb), cv2.COLOR_RGB2HSV) if need_hsv else None) @@ -1783,6 +1790,7 @@ def _detect_frame(self, rgb, depth, debug=False, include_bins=False, arm_clear=T items = ((color, mask) for color, mask in masks.items() if color not in ("red", "yellow")) else: items = masks.items() + bin_img = self._bin_image_components(hsv) if (self.bin_image_guard and hsv is not None) else None _ls = {"raw_px": {}, "mask_px": {}, "blobs": {}, "min_area": 0, "blob_hsv": []} if self.look_summary else None _ls_hsv = None if _ls is not None: @@ -1828,6 +1836,16 @@ def _detect_frame(self, rgb, depth, debug=False, include_bins=False, arm_clear=T continue if all_masks is not None and self._local_bg_reject(blob, hsv, all_masks): continue + if self.bin_size_guard: + why_bin = self._bin_sized_blob(blob, area, cent[i], depth) + if why_bin is not None: + rejected.append((self._blob_placeholder(color, area, cent[i]), why_bin)) + continue + if bin_img is not None: + why_bin = self._bin_image_blob(blob, area, cent[i], depth, bin_img) + if why_bin is not None: + rejected.append((self._blob_placeholder(color, area, cent[i]), why_bin)) + continue parts = [(blob, area, cent[i])] if self.split_by_solidity: lo, hi = self.split_solidity_cfg.get("area_px", [520, 15500]) @@ -2109,6 +2127,11 @@ def _observed_bin_bottom(self, hsv): rows[supported] = keep.shape[0] - 1 - np.argmax(keep[::-1, supported], axis=0) fallback = float(np.nanmedian(rows)) rows[~np.isfinite(rows)] = fallback + w = int(p.get("row_median_px", 0)) # 0 = off. Rolling column median: one bin's missing front-face columns follow its neighbours + if w > 1 and self.bin_bottom_first_run: + pad = w // 2 + ext = np.r_[np.full(pad, rows[0]), rows, np.full(pad, rows[-1])] + rows = np.array([np.median(ext[i:i + 2 * pad + 1]) for i in range(len(rows))]) return rows @staticmethod @@ -2437,6 +2460,85 @@ def _detect_blob(self, color, blob, area, centroid, depth, include_bins, dets, r else: dets.append(det) + def _bin_sized_blob(self, blob, area, centroid, depth): + """perception.bin_size_guard: rejection reason when one colour blob is bin-sized, else None. Objects are ~4.5 cm + (two touching ~9 cm); a bin is ~21 x 21 cm with a 6-12 cm rim. Checks (any one rejects): + area > max_area_factor x the expected single-object pixel area (works without depth), + top (92nd percentile z) > max_top_m above the counter, + extent of the points above the counter (5..95 percentile along the principal axes) > max_extent_m. + Never raises: on any error the blob is kept (fail open = the pre-hotfix behaviour).""" + try: + p = self.bin_size_cfg + exp = float(self.expected_area_px(blob, centroid, depth) or 0.0) + if exp > 0 and area > float(p.get("max_area_factor", 4.0)) * exp: + return "bin_size:area" + if depth is None: + return None + c = self.cfg + dlo, dhi = c["depth_range_mm"] + ys, xs = np.nonzero(blob) + d = depth[ys, xs].astype(float) + ok = (d >= dlo) & (d <= dhi) + if int(ok.sum()) < int(c["min_valid_depth_px"]): + return None + P = self._pixel_to_pelvis(xs[ok], ys[ok], d[ok]) + P = P[np.all(np.isfinite(P), axis=1)] + if len(P) < int(c["min_valid_depth_px"]): + return None + zc = float(self.scene.counter_z) + if float(np.percentile(P[:, 2], c["top_percentile"])) - zc > float(p.get("max_top_m", 0.075)): + return "bin_size:top" + Q = P[P[:, 2] > zc + float(p.get("min_point_height_m", 0.01)), :2] + if len(Q) < 10: + return None + Q = Q - Q.mean(axis=0) + _, vecs = np.linalg.eigh(np.cov(Q.T)) + proj = Q @ vecs + ext = np.percentile(proj, 95, axis=0) - np.percentile(proj, 5, axis=0) + if float(ext.max()) > float(p.get("max_extent_m", 0.13)): + return "bin_size:extent" + except Exception as exc: # never raise into act() + self.log(f"[bin_size_guard] failed: {type(exc).__name__}: {exc}") + return None + + def _bin_image_components(self, hsv): + """perception.bin_image_guard: (labels, stats) of the image-space bin mass, or None. Bin mass = dark warm pixels + (cardboard / paper wall in shade: H <= h_max or >= 170, S >= s_min, v_min <= V < object_v_min), opened. Foam + objects are lit and bright (V ~ 230-250), the venue bin walls sit at V ~ 80-125. Needs no depth.""" + p = self.bin_image_cfg + h, s, v = hsv[..., 0], hsv[..., 1], hsv[..., 2] + m = (((h <= int(p.get("h_max", 17))) | (h >= 170)) & (s >= int(p.get("s_min", 77))) & + (v >= int(p.get("v_min", 50))) & (v < int(p.get("object_v_min", 150)))).astype(np.uint8) + k = int(p.get("open_px", 5)) + if k > 0: + m = cv2.morphologyEx(m, cv2.MORPH_OPEN, np.ones((k, k), np.uint8)) + n, lab, stats, _ = cv2.connectedComponentsWithStats(m, connectivity=8) + return (lab, stats) if n > 1 else None + + def _bin_image_blob(self, blob, area, centroid, depth, bin_img): + """perception.bin_image_guard: 'bin_image:wall' when >= min_frac of the blob lies on one dark-warm image component + that is bin-sized (both bbox sides > min_side_objects x the expected single-object side, i.e. a bin face, not an + object in shadow or two touching ones), else None. Never raises (fail open).""" + try: + lab, stats = bin_img + p = self.bin_image_cfg + ids = lab[blob > 0] + ids = ids[ids > 0] + if len(ids) < float(p.get("min_frac", 0.5)) * area: + return None + cid = int(np.bincount(ids).argmax()) + if int(np.count_nonzero(ids == cid)) < float(p.get("min_frac", 0.5)) * area: + return None + exp = float(self.expected_area_px(blob, centroid, depth) or 0.0) + coef = float((self.cfg.get("split") or {}).get("area_coef", 1.2)) + side = (exp / max(coef, 1e-9)) ** 0.5 + w, hh = int(stats[cid, cv2.CC_STAT_WIDTH]), int(stats[cid, cv2.CC_STAT_HEIGHT]) + if side > 0 and min(w, hh) > float(p.get("min_side_objects", 1.8)) * side: + return "bin_image:wall" + except Exception as exc: # never raise into act() + self.log(f"[bin_image_guard] failed: {type(exc).__name__}: {exc}") + return None + def _fk_rescue_reject(self, det, blob, area): """perception.fk_colour_rescue: a blob that overlaps the FK arm silhouette is the robot's own hand / forearm unless its top sits at object height above the counter (hands float 6-9 cm higher).""" @@ -3017,7 +3119,7 @@ def _reject(self, det, bins_first=False): return "off_mat" bm = c["bin_exclusion_margin"] half = np.asarray(sc.cfg["bins"]["outer_size"], float) / 2 + bm - if not self._image_bin_gate_active: + if not self._image_bin_gate_active or self.bin_image_guard: # bin_image_guard: defence in depth, nominal in_bin stays on under the image gate for name, ctr in self._bin_centres_mat().items(): if np.all(np.abs(pm[:2] - ctr) <= half): return f"in_bin:{name}" @@ -3217,7 +3319,7 @@ def _color_gate_depth(self, top): def _color_gate_bin(self, det, g): """Rule 2, 3D in the mat frame (nominal footprint, no margin). Inside a bin -> in_bin. Within edge_m outside one -> kept only as a depth top face edge_h above the counter (a bin rim), else bin_edge. Else keep.""" - if self._image_bin_gate_active: + if self._image_bin_gate_active: # deliberately NOT re-enabled by bin_image_guard: its hedged footprint (max_shift_m 0.18 towards the robot) rejected real cubes at pelvis x ~0.30 in the venue replay return None sc = self.scene pm = sc.pelvis_to_mat(det.top)[:2] diff --git a/aaltonomous/baselines/classical/policy.py b/aaltonomous/baselines/classical/policy.py index ad2771e..3a4e139 100644 --- a/aaltonomous/baselines/classical/policy.py +++ b/aaltonomous/baselines/classical/policy.py @@ -729,6 +729,8 @@ def reset(self, info=None): self._exec = None # D2 T-8 s: the plan being executed {"plan", "n", "cut", "t8"} self._rel = None # grasp.release_confirm: the plan's release watcher self._rel_fail = False # release_confirm: the hand never read empty -> stop picking + self._bad_grasp = [] # grasp.wrong_object_recovery: pelvis xy of grasps rejected as the wrong object (never replanned) + self._wor_recoveries = 0 # grasp.wrong_object_recovery: failed-release recoveries used this episode self._ig = None # insert.reach_gated: the plan's pre-release gate {"plan", "n", "stops", "i", "t0"} self._cap_logged = set() self._dec_warned = set() # F7: decision helpers that already fell back to stock this episode @@ -2680,6 +2682,8 @@ def _ordered_cands(self, dets, obs, verify, decision=True): continue if any(np.linalg.norm(xy - f) < self.cfg["policy"]["same_object_radius"] for f in self.failed): continue + if self._bad_grasp_near(xy): + continue if pol.get("place_gate", False): gs = self._tracked_here(self.gate_skipped, d.color, xy) if gs is not None and gs[2] >= self.GATE_MAX_SKIPS: @@ -4876,6 +4880,51 @@ def _held_abort_due(self, g, hq, hc, margin=None): return not det.held(g["plan"].side, hc, hq, margin)[0] return not det.held(g["plan"].side, hc, hq)[0] + def _wor_params(self): + """grasp.wrong_object_recovery params when the flag is on, else None.""" + g = self.cfg["grasp"] + if not g.get("wrong_object_recovery", False): + return None + return g.get("wrong_object_recovery_params") or {} + + def _bad_grasp_near(self, xy): + """True when xy lies within bad_radius_m of a grasp rejected by the lift-time held check (flag off: never).""" + if not self._bad_grasp: + return False + p = self._wor_params() or {} + rad = float(p.get("bad_radius_m", 0.06)) + return any(np.linalg.norm(np.asarray(xy, float)[:2] - f) < rad for f in self._bad_grasp) + + def _lift_margin_fail(self, g, hq, hc): + """grasp.wrong_object_recovery (a): the post-lift finger_sum must clear lift_margin_mult x the held threshold (and, + if lift_min_ratio > 0, keep that fraction of the close read). Plain finger_sum mode only; unsure (no bias + reference, bad data, soft detector) is never a failure. A failure blacklists the grasp point for the episode.""" + p = self._wor_params() + det = self.held_detector + if p is None or det.mode != "finger_sum" or det.soft is not None or not det.ready: + return False + if hq.shape != (14,) or hc.shape != (14,) or not (np.all(np.isfinite(hq)) and np.all(np.isfinite(hc))): + return False + side = g["plan"].side + lift = float(det.held(side, hc, hq)[1]["finger_sum"]) + need = det.threshold_rad * float(p.get("lift_margin_mult", 2.0)) + why = None + if lift < need: + why = f"lift finger_sum {lift:.3f} < {need:.3f} (threshold {det.threshold_rad:.3f} x {float(p.get('lift_margin_mult', 2.0)):g})" + ratio = float(p.get("lift_min_ratio", 0.0)) + close = g["samples"].get("close") + if why is None and ratio > 0.0 and close is not None: + c_fs = float(det.held(side, np.asarray(close[2], float), np.asarray(close[1], float))[1]["finger_sum"]) + if c_fs > 0.0 and lift < ratio * c_fs: + why = f"lift finger_sum {lift:.3f} < {ratio:g} x close {c_fs:.3f}" + if why is None: + return False + xy = np.asarray(g["plan"].waypoints["grasp"][:2], float).copy() + self._bad_grasp.append(xy) + self.log(f"[wrong_object] t={g['t']:.1f} {g['plan'].color} {side}: {why}: not held (bin wall / wrong object?); " + f"open, retreat, blacklist ({xy[0]:+.2f},{xy[1]:+.2f}) r={float(p.get('bad_radius_m', 0.06)):g} m") + return True + def _abort_empty_grasp(self, g, sample="close"): """Cancel carry and request a proved open -> vertical lift -> home recovery in a worker.""" plan = g["plan"] @@ -4978,12 +5027,20 @@ def _release_confirm(self, obs, t): self._enqueue(np.repeat(arm[None], len(h), 0), h) self.log(f"[release_confirm] t={t:.1f} {r['plan'].color} {side}: hand still not empty -> re-open once") return self._pop() - self._rel, self._rel_fail = None, True + wp = self._wor_params() + recover = wp is not None and self._wor_recoveries < int(wp.get("max_recoveries", 2)) + self._rel, self._rel_fail = None, not recover home = arm.copy() home[ArmKinematics.sl(side)] = self.home[ArmKinematics.sl(side)] a, h = self.planner.go_to(arm, self.hand_cmd, home, self.post["open"]) self._enqueue(a, h) - self.log(f"[release_confirm] t={t:.1f} {r['plan'].color} {side}: still not empty after the re-open -> home, stop") + if recover: # grasp.wrong_object_recovery (b): same open + home path, then the normal settle -> look continues + self._wor_recoveries += 1 + self._bad_grasp.append(np.asarray(r["plan"].waypoints["grasp"][:2], float).copy()) + self.log(f"[release_confirm] t={t:.1f} {r['plan'].color} {side}: still not empty after the re-open -> open, home, " + f"re-look and continue (recovery {self._wor_recoveries}/{int(wp.get('max_recoveries', 2))})") + else: + self.log(f"[release_confirm] t={t:.1f} {r['plan'].color} {side}: still not empty after the re-open -> home, stop") return self._pop() def _insert_gate(self, obs, t): @@ -5199,6 +5256,11 @@ def _grasp_diag(self, obs): self._abort_empty_grasp(g) # queue replaced; _act pops it on this same tick self._log_aborted_diag(g) return + if key == "lift" and not g.get("skill_only") \ + and self._lift_margin_fail(g, np.asarray(q, float), np.asarray(cmd, float)): + self._abort_empty_grasp(g, "lift-margin") + self._log_aborted_diag(g) + return if key == "lift" and (read_through or g.get("lift_check")) and not g.get("skill_only") \ and self._held_abort_due(g, np.asarray(q, float), np.asarray(cmd, float), self._soft_margin("lift")): diff --git a/aaltonomous/baselines/classical/policy_server.py b/aaltonomous/baselines/classical/policy_server.py index 4e53674..92f6ae3 100644 --- a/aaltonomous/baselines/classical/policy_server.py +++ b/aaltonomous/baselines/classical/policy_server.py @@ -105,10 +105,13 @@ def _reset_now(self, instruction, task=None, seed=None): if task not in (None, "sort_fixed"): log.warning("task %r: the classical baseline only does rung 1 (colour = bin from scene.yaml)", task) info = {"instruction": instruction, "task": task, "seed": seed} - if task == "sort_fixed": + fixed_first = bool((self.pol.cfg.get("policy") or {}).get("fixed_instruction_first", False)) + if task == "sort_fixed" and not fixed_first: # The server reset is the episode contract: only the fixed task receives fixed mapping. The optional # finals venue order is explicit scene metadata in policy.fixed_mapping; language tasks never inherit it. info["mapping"] = dict(self.pol.fixed_mapping) + # policy.fixed_instruction_first: no injected mapping; ClassicalPolicy._episode_mapping applies the instruction's + # full mapping when it parses, else falls back to policy.fixed_mapping itself (sort_fixed only). self.pol.reset(info) # task id: language/fixed/insert routing; seed: skills log.info("reset task=%s seed=%s instruction=%r", task, seed, instruction) diff --git a/aaltonomous/baselines/classical/test_bin_gate_defence.py b/aaltonomous/baselines/classical/test_bin_gate_defence.py new file mode 100644 index 0000000..ff2bbcb --- /dev/null +++ b/aaltonomous/baselines/classical/test_bin_gate_defence.py @@ -0,0 +1,63 @@ +"""perception.bin_image_guard defence in depth: while the image gate_below_bins is active the nominal in_bin reject +(_reject) stays on; without the flag the image gate replaces it. The colour gate's hedged bin rule (_color_gate_bin) +is deliberately NOT re-enabled: its footprint (nominal + 18 cm towards the robot) deleted real cubes in the venue replay. Run: + python -m unittest aaltonomous.baselines.classical.test_bin_gate_defence -v +""" +import unittest + +import numpy as np + +from aaltonomous.interface.calibration import Calibration +from aaltonomous.scene.scene import Scene + +from .config import load_config +from .perception import Detection, Perception + +SC, CAL = Scene.load(), Calibration.load() +BIN = np.asarray(SC.cfg["bins"]["centers"]["center"], float) + + +def _perc(guard): + cfg = load_config(overlay="finals_r25_gen", overrides={"perception.bin_image_guard": guard}) + p = Perception(cfg, SC, CAL, log=lambda *_: None) + p._image_bin_gate_active = True + return p + + +def _det(p, xy): + top = np.array(SC.mat_to_pelvis([float(xy[0]), float(xy[1]), 0.0]), float) + top[2] = p.scene.counter_z + 0.9 * p.height # an object-height top face whatever the overlay's height_range + return Detection("red", top, 5000, 500, "depth") + + +class TestBinGateDefence(unittest.TestCase): + def test_overlay_enables_guard_and_wider_first_run_band(self): + p = _perc(True) + self.assertTrue(p.bin_image_guard) + self.assertEqual(p.gate_below_cfg["first_run_top_frac"], 0.55) + self.assertEqual(Perception(load_config(overlay="none"), SC, CAL, log=lambda *_: None) + .gate_below_cfg.get("first_run_top_frac", 0.45), 0.45) + + def test_guard_on_keeps_in_bin_reject_under_image_gate(self): + p = _perc(True) + self.assertEqual(p._reject(_det(p, BIN)), "in_bin:center") + + def test_guard_off_image_gate_skips_in_bin_reject(self): + p = _perc(False) + self.assertIsNone(p._reject(_det(p, BIN))) + + def test_colour_gate_bin_rule_stays_off_under_image_gate_even_with_guard(self): + for guard in (True, False): + p = _perc(guard) + self.assertIsNone(p._color_gate_bin(_det(p, BIN), p.color_gate_cfg), guard) + + def test_object_clear_of_bins_still_passes_with_guard(self): + p = _perc(True) + far = (0.25, 0.10) # mat front-left, far from every bin + d = _det(p, far) + self.assertIsNone(p._reject(d)) + self.assertIsNone(p._color_gate_bin(d, p.color_gate_cfg)) + + +if __name__ == "__main__": + unittest.main() diff --git a/aaltonomous/baselines/classical/test_bin_size_guard.py b/aaltonomous/baselines/classical/test_bin_size_guard.py new file mode 100644 index 0000000..92f54ec --- /dev/null +++ b/aaltonomous/baselines/classical/test_bin_size_guard.py @@ -0,0 +1,98 @@ +"""perception.bin_size_guard (real-only hotfix): a bin-sized / bin-tall colour blob is never a pick candidate. Run: + python -m unittest aaltonomous.baselines.classical.test_bin_size_guard -v +""" +import unittest + +import numpy as np + +from aaltonomous.interface.calibration import Calibration +from aaltonomous.scene.scene import Scene + +from .config import load_config +from .perception import Perception + +SC, CAL = Scene.load(), Calibration.load() +H, W = 480, 640 +MM_PER_PX = 1.0 # synthetic camera: 1 px = 1 mm on the counter + + +def _perc(overlay="finals_r25_gen", overrides=None): + p = Perception(load_config(overlay=overlay, overrides=overrides), SC, CAL, log=lambda *_: None) + return p + + +def _rig(p, heights, expected_px=3000.0): + """Stub the camera: pixel (u, v) -> pelvis (0.3 + u mm, v mm, counter_z + heights[v, u]).""" + zc = float(p.scene.counter_z) + + def pix(u, v, d, frame=None, transform_snapshot=None): + u, v = np.asarray(u), np.asarray(v) + return np.c_[0.3 + u * MM_PER_PX / 1000.0, v * MM_PER_PX / 1000.0, zc + heights[v, u]] + p._pixel_to_pelvis = pix + p.expected_area_px = lambda blob, centroid, depth: expected_px + return np.full((H, W), 600, np.uint16) + + +def _blob(x0, y0, w, h): + m = np.zeros((H, W), np.uint8) + m[y0:y0 + h, x0:x0 + w] = 1 + return m + + +class TestBinSizeGuard(unittest.TestCase): + def test_flag_real_on_sim_off_stock_off(self): + self.assertTrue(_perc().bin_size_guard) + real = load_config(overlay="finals_r25_gen") + sim = load_config(overlay="finals_r25_gen", overrides=dict(real["profile_auto"]["sim_overrides"])) + self.assertFalse(Perception(sim, SC, CAL, log=lambda *_: None).bin_size_guard) + self.assertFalse(_perc(overlay="none").bin_size_guard) + + def test_cube_accepted(self): + p = _perc() + hts = np.zeros((H, W)) + cube = _blob(300, 200, 45, 45) # 4.5 cm footprint, 4.5 cm tall + hts[cube > 0] = 0.045 + depth = _rig(p, hts) + self.assertIsNone(p._bin_sized_blob(cube, int(cube.sum()), (322, 222), depth)) + + def test_two_touching_cubes_accepted(self): + p = _perc() + hts = np.zeros((H, W)) + pair = _blob(300, 200, 90, 45) + hts[pair > 0] = 0.045 + depth = _rig(p, hts) + self.assertIsNone(p._bin_sized_blob(pair, int(pair.sum()), (345, 222), depth)) + + def test_cube_without_depth_accepted(self): + p = _perc() + _rig(p, np.zeros((H, W))) + cube = _blob(300, 200, 45, 45) + self.assertIsNone(p._bin_sized_blob(cube, int(cube.sum()), (322, 222), None)) + + def test_bin_sized_orange_blob_rejected_by_area(self): + p = _perc() + hts = np.zeros((H, W)) + wall = _blob(200, 100, 210, 90) # 21 cm bin wall + rim, 6 cm rim + hts[wall > 0] = 0.06 + depth = _rig(p, hts) + self.assertEqual(p._bin_sized_blob(wall, int(wall.sum()), (305, 145), depth), "bin_size:area") + + def test_bin_rejected_by_extent_when_area_looks_small(self): + p = _perc() + hts = np.zeros((H, W)) + wall = _blob(200, 100, 210, 30) # thin 21 cm wall sliver at a 6 cm rim + hts[wall > 0] = 0.06 + depth = _rig(p, hts, expected_px=1e9) + self.assertEqual(p._bin_sized_blob(wall, int(wall.sum()), (305, 115), depth), "bin_size:extent") + + def test_bin_rejected_by_top_height(self): + p = _perc() + hts = np.zeros((H, W)) + rim = _blob(300, 200, 60, 40) # small rim fragment, 9 cm above the counter + hts[rim > 0] = 0.09 + depth = _rig(p, hts, expected_px=1e9) + self.assertEqual(p._bin_sized_blob(rim, int(rim.sum()), (330, 220), depth), "bin_size:top") + + +if __name__ == "__main__": + unittest.main() diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t03.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t03.json new file mode 100644 index 0000000..82eaa2f --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t03.json @@ -0,0 +1,89 @@ +{ + "frame": "r1_t03.png", + "run": "larp-interns_20261115_091835", + "t_s": 3, + "arm_visible": true, + "note": "arm/hand at home visible at left and right edge; yellow cube partly occluded by right hand", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 245, + 320, + 322, + 412 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 372, + 298, + 455, + 395 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 452, + 293, + 532, + 388 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 270, + 420, + 335, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 375, + 398, + 445, + 478 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 60, + 230, + 260 + ] + }, + { + "name": "middle", + "box": [ + 218, + 62, + 455, + 280 + ] + }, + { + "name": "right", + "box": [ + 458, + 65, + 640, + 280 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t03.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t03.png new file mode 100644 index 0000000..7c7e5a0 Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t03.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t12.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t12.json new file mode 100644 index 0000000..e543ca4 --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t12.json @@ -0,0 +1,89 @@ +{ + "frame": "r1_t12.png", + "run": "larp-interns_20261115_091835", + "t_s": 12, + "arm_visible": false, + "note": "clean look, the frame the policy chose 'red right' from", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 245, + 320, + 322, + 412 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 372, + 298, + 455, + 395 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 452, + 293, + 532, + 388 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 270, + 420, + 335, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 375, + 398, + 445, + 478 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 60, + 235, + 258 + ] + }, + { + "name": "middle", + "box": [ + 218, + 62, + 455, + 280 + ] + }, + { + "name": "right", + "box": [ + 458, + 68, + 640, + 285 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t12.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t12.png new file mode 100644 index 0000000..bf252e9 Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t12.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t15.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t15.json new file mode 100644 index 0000000..8828c71 --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t15.json @@ -0,0 +1,89 @@ +{ + "frame": "r1_t15.png", + "run": "larp-interns_20261115_091835", + "t_s": 15, + "arm_visible": false, + "note": "clean; right arm entering at top right edge", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 245, + 320, + 322, + 412 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 372, + 298, + 455, + 395 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 452, + 293, + 532, + 388 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 270, + 420, + 335, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 375, + 398, + 445, + 478 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 58, + 235, + 258 + ] + }, + { + "name": "middle", + "box": [ + 218, + 62, + 455, + 280 + ] + }, + { + "name": "right", + "box": [ + 458, + 65, + 640, + 280 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t15.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t15.png new file mode 100644 index 0000000..0ac5ad0 Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t15.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t21.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t21.json new file mode 100644 index 0000000..d1e4b5f --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t21.json @@ -0,0 +1,89 @@ +{ + "frame": "r1_t21.png", + "run": "larp-interns_20261115_091835", + "t_s": 21, + "arm_visible": true, + "note": "right hand over the right bin wall (dark, grasping the wall)", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 245, + 320, + 322, + 412 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 372, + 298, + 455, + 395 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 452, + 293, + 532, + 388 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 270, + 420, + 335, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 375, + 398, + 445, + 478 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 58, + 235, + 258 + ] + }, + { + "name": "middle", + "box": [ + 218, + 62, + 455, + 280 + ] + }, + { + "name": "right", + "box": [ + 458, + 68, + 640, + 285 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t21.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t21.png new file mode 100644 index 0000000..004c21f Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t21.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t27.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t27.json new file mode 100644 index 0000000..7bf8c3e --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t27.json @@ -0,0 +1,89 @@ +{ + "frame": "r1_t27.png", + "run": "larp-interns_20261115_091835", + "t_s": 27, + "arm_visible": true, + "note": "right bin dragged and rotated; hand at top right", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 245, + 320, + 322, + 412 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 372, + 298, + 455, + 395 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 452, + 293, + 532, + 388 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 270, + 420, + 335, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 375, + 398, + 445, + 478 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 52, + 235, + 255 + ] + }, + { + "name": "middle", + "box": [ + 215, + 55, + 450, + 280 + ] + }, + { + "name": "right", + "box": [ + 468, + 80, + 640, + 300 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t27.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t27.png new file mode 100644 index 0000000..db4c4b8 Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t27.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t36.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t36.json new file mode 100644 index 0000000..ff776b6 --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t36.json @@ -0,0 +1,89 @@ +{ + "frame": "r1_t36.png", + "run": "larp-interns_20261115_091835", + "t_s": 36, + "arm_visible": false, + "note": "right bin displaced, arm gone", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 245, + 320, + 322, + 412 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 372, + 298, + 455, + 395 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 452, + 293, + 532, + 388 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 270, + 420, + 335, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 375, + 398, + 445, + 478 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 52, + 235, + 255 + ] + }, + { + "name": "middle", + "box": [ + 215, + 55, + 450, + 280 + ] + }, + { + "name": "right", + "box": [ + 468, + 80, + 640, + 300 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t36.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t36.png new file mode 100644 index 0000000..83ec22b Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r1_t36.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t03.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t03.json new file mode 100644 index 0000000..adb1204 --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t03.json @@ -0,0 +1,89 @@ +{ + "frame": "r2_t03.png", + "run": "larp-interns_20261115_091958", + "t_s": 3, + "arm_visible": true, + "note": "hands at home visible; orange cylinder partly out of view at the bottom", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 237, + 322, + 318, + 415 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 333, + 272, + 412, + 358 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 463, + 308, + 552, + 398 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 262, + 420, + 328, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 377, + 435, + 445, + 480 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 62, + 230, + 265 + ] + }, + { + "name": "middle", + "box": [ + 215, + 68, + 445, + 282 + ] + }, + { + "name": "right", + "box": [ + 455, + 72, + 640, + 285 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t03.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t03.png new file mode 100644 index 0000000..5928b35 Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t03.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t12.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t12.json new file mode 100644 index 0000000..f2c7b33 --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t12.json @@ -0,0 +1,89 @@ +{ + "frame": "r2_t12.png", + "run": "larp-interns_20261115_091958", + "t_s": 12, + "arm_visible": false, + "note": "clean look", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 237, + 322, + 318, + 415 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 333, + 272, + 412, + 358 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 463, + 308, + 552, + 398 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 262, + 420, + 328, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 377, + 435, + 445, + 480 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 62, + 230, + 265 + ] + }, + { + "name": "middle", + "box": [ + 215, + 68, + 445, + 282 + ] + }, + { + "name": "right", + "box": [ + 455, + 72, + 640, + 285 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t12.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t12.png new file mode 100644 index 0000000..47db5ea Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t12.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t21.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t21.json new file mode 100644 index 0000000..db5e98d --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t21.json @@ -0,0 +1,89 @@ +{ + "frame": "r2_t21.png", + "run": "larp-interns_20261115_091958", + "t_s": 21, + "arm_visible": true, + "note": "right hand reaching the right bin wall", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 237, + 322, + 318, + 415 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 333, + 272, + 412, + 358 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 463, + 308, + 552, + 398 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 262, + 420, + 328, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 377, + 435, + 445, + 480 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 60, + 230, + 262 + ] + }, + { + "name": "middle", + "box": [ + 215, + 65, + 445, + 282 + ] + }, + { + "name": "right", + "box": [ + 458, + 70, + 640, + 285 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t21.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t21.png new file mode 100644 index 0000000..534d61a Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t21.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t24.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t24.json new file mode 100644 index 0000000..af65e9a --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t24.json @@ -0,0 +1,89 @@ +{ + "frame": "r2_t24.png", + "run": "larp-interns_20261115_091958", + "t_s": 24, + "arm_visible": false, + "note": "right bin lifted off the counter and tilted", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 237, + 322, + 318, + 415 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 333, + 272, + 412, + 358 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 463, + 308, + 552, + 398 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 262, + 420, + 328, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 377, + 435, + 445, + 480 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 60, + 230, + 262 + ] + }, + { + "name": "middle", + "box": [ + 212, + 62, + 445, + 282 + ] + }, + { + "name": "right", + "box": [ + 440, + 35, + 640, + 265 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t24.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t24.png new file mode 100644 index 0000000..d37efd9 Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t24.png differ diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t33.json b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t33.json new file mode 100644 index 0000000..c2981c2 --- /dev/null +++ b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t33.json @@ -0,0 +1,98 @@ +{ + "frame": "r2_t33.png", + "run": "larp-interns_20261115_091958", + "t_s": 33, + "arm_visible": false, + "note": "right bin thrown up/over the middle bin; orange paper debris on the counter at right", + "objects": [ + { + "colour": "blue", + "shape": "cube", + "box": [ + 230, + 320, + 312, + 412 + ] + }, + { + "colour": "orange", + "shape": "cube", + "box": [ + 328, + 268, + 407, + 355 + ] + }, + { + "colour": "yellow", + "shape": "cube", + "box": [ + 460, + 300, + 550, + 398 + ] + }, + { + "colour": "yellow", + "shape": "triangular_prism", + "box": [ + 258, + 418, + 325, + 480 + ] + }, + { + "colour": "orange", + "shape": "cylinder", + "box": [ + 372, + 433, + 442, + 480 + ] + } + ], + "bins": [ + { + "name": "left", + "box": [ + 0, + 58, + 220, + 258 + ] + }, + { + "name": "middle", + "box": [ + 205, + 55, + 440, + 278 + ] + }, + { + "name": "right", + "box": [ + 400, + 0, + 640, + 205 + ] + }, + { + "name": "debris", + "box": [ + 535, + 218, + 640, + 264 + ] + } + ], + "boxes": "[x0,y0,x1,y1] pixels in the 640x480 frame, hand-labelled (approximate, +-8 px); bin boxes cover the whole box incl. rim and the displaced/lifted bin" +} \ No newline at end of file diff --git a/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t33.png b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t33.png new file mode 100644 index 0000000..b3f37ef Binary files /dev/null and b/aaltonomous/baselines/classical/test_data/venue_20261115/r2_t33.png differ diff --git a/aaltonomous/baselines/classical/test_fixed_mapping.py b/aaltonomous/baselines/classical/test_fixed_mapping.py index 49a203a..6f662ac 100644 --- a/aaltonomous/baselines/classical/test_fixed_mapping.py +++ b/aaltonomous/baselines/classical/test_fixed_mapping.py @@ -108,6 +108,34 @@ def test_sort_fixed_submission_stock_sentence_uses_sim_mapping_without_conflict( self.assertEqual(mapping("submission", STOCK, task="sort_fixed", logs=logs), FIXED_MAPPING) self.assertFalse([l for l in logs if "conflict" in l], logs) + def test_finals_overlay_uses_venue_instruction_mapping_not_the_venue_map(self): + # venue sentence: red left, yellow center, blue right (differs from the overlay's fixed_mapping on purpose) + m = mapping("finals_r25_gen", STOCK, task="sort_fixed") + self.assertEqual(m, {"red": "left", "yellow": "center", "blue": "right"}) + + def test_finals_overlay_falls_back_to_fixed_mapping_when_instruction_is_unclear(self): + for ins in ("", None, "sort the blocks", "Put the red objects in the left bin."): + self.assertEqual(mapping("finals_r25_gen", ins, task="sort_fixed"), REAL, ins) + + def test_server_does_not_inject_the_venue_map_when_instruction_first(self): + from .policy_server import ClassicalServerPolicy + + class Stub: + _job = None + cfg = {"policy": {"fixed_instruction_first": True}} + fixed_mapping = REAL + + def reset(self, info): + self.info = info + + server = ClassicalServerPolicy.__new__(ClassicalServerPolicy) + server._auto_buf, server._pending_reset, server.probe_cfg = None, None, None + server.pol = Stub() + server.reset(STOCK, task="sort_fixed", seed=7) + self.assertNotIn("mapping", server.pol.info) + self.assertEqual(server.pol.info["task"], "sort_fixed") + self.assertEqual(server.pol.info["instruction"], STOCK) + def test_bad_values_raise(self): bad = ({"red": "left", "yellow": "center"}, {"red": "left", "yellow": "left", "blue": "right"}, {"red": "left", "yellow": "center", "blue": "up"}, {"red": "left", "yellow": "center", "green": "right"}, diff --git a/aaltonomous/baselines/classical/test_flags.py b/aaltonomous/baselines/classical/test_flags.py index 042bc6b..5fd3894 100644 --- a/aaltonomous/baselines/classical/test_flags.py +++ b/aaltonomous/baselines/classical/test_flags.py @@ -137,7 +137,7 @@ def test_shipped_submission_overlay_is_stock_plus_the_ab_proven_flags(self): "brain.known_task_hard_off", "planner.candidate_budget", "perception.table_pitch_calibration.max_abs_pitch_deg", "policy.zero_det_relook", "policy.startup_overlap", - "policy.descend_contact_guard", "perception.look_summary", "policy.contact_start_unblock", "calib.plane.robust_height", "perception.plan_frame", "calib.plane.robust_agree_m", "perception.height_range", "bin.fallback_centre_x", "bin.fallback_drop_z", "bin.reach_x_max", "bin.reach_drop_z", "planner.budget_escalate"}) + "policy.descend_contact_guard", "perception.look_summary", "policy.contact_start_unblock", "calib.plane.robust_height", "perception.plan_frame", "calib.plane.robust_agree_m", "perception.height_range", "bin.fallback_centre_x", "bin.fallback_drop_z", "bin.reach_x_max", "bin.reach_drop_z", "planner.budget_escalate", "perception.bin_size_guard", "perception.bin_image_guard", "perception.gate_below_bins_params.first_run_top_frac", "bin.vote.tape_body_reject", "grasp.wrong_object_recovery"}) def test_ab_safety_is_cand_r17_plus_the_safety_shell(self): def on(name): diff --git a/aaltonomous/baselines/classical/test_venue_replay.py b/aaltonomous/baselines/classical/test_venue_replay.py new file mode 100644 index 0000000..4223953 --- /dev/null +++ b/aaltonomous/baselines/classical/test_venue_replay.py @@ -0,0 +1,97 @@ +"""Venue replay: the real Perception.detect() colour path on real finals frames (no depth, no GPU, no simulator). +Frames + hand labels: test_data/venue_20261115/. Harness: tools/venue_replay.py. Run: + python -m unittest aaltonomous.baselines.classical.test_venue_replay -v + +What is (and is not) asserted, honestly: + * 1063951 behaviour (bin_size_guard off) reproduces the venue failure: on the clean look of run 1 (the frame the policy + chose "execute red right arm -> right bin" from) an accepted red candidate lies on the right bin. + * the hotfix bin_size_guard alone does NOT change anything on these frames: its area rule needs a blob > 4x an object, + the bin's wall blob is ~1x (its 3D rules need depth, which the recordings do not have -> not exercised here). + * with perception.bin_image_guard (+ the wider gate_below_bins first-run band) no accepted red/yellow/blue candidate + lies on a labelled bin in the arm-free frames and all 5 objects of every frame are still found as the right colour class. + * NOT asserted: shape labels (without depth the plane-fallback shape is unreliable), positions, depth stages. +Frames where a robot hand is in view (arm_visible) only get the object-recall assertion: the policy's FK arm mask, which +needs joint states, removes hand blobs; here they show up as 'bin' hits on the left bin and are reported, not asserted. +""" +import importlib.util +import os +import unittest + +ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "..")) +_spec = importlib.util.spec_from_file_location("venue_replay", os.path.join(ROOT, "tools", "venue_replay.py")) +vr = importlib.util.module_from_spec(_spec) +_spec.loader.exec_module(vr) + +FIX = "guard+bin_image+first_run_0.55" +_CACHE = {} + + +def _res(config): + if config not in _CACHE: + _CACHE[config] = vr.evaluate(configs=[config], verbose=False)[config] + return _CACHE[config] + + +def _clean(res): + return {f: r for f, r in res.items() if not r["lab"]["arm_visible"]} + + +class TestVenueReplay(unittest.TestCase): + def test_fixtures_present_and_small(self): + frames = vr.load_frames() + self.assertGreaterEqual(len(frames), 8) + self.assertLess(sum(os.path.getsize(p) for _, p in frames), 3 * 1024 * 1024) + for lab, _ in frames: + self.assertEqual(len(lab["objects"]), 5) + self.assertGreaterEqual(len(lab["bins"]), 3) + + def test_legacy_reproduces_bin_as_red(self): + r = _res("legacy")["r1_t12"] + self.assertTrue(any(c["colour"] == "red" and c["kind"] == "bin" for c in r["on_bin"])) + self.assertEqual(r["first_red"]["kind"], "bin") # ranked first: the bin wall is nearest to the red bin + self.assertEqual(r["first_red"]["name"], "right") + + def test_size_guard_alone_is_noop_without_depth(self): + legacy, guard = _clean(_res("legacy")), _clean(_res("guard")) + for f in legacy: + self.assertEqual(len(legacy[f]["on_bin"]), len(guard[f]["on_bin"]), f) + self.assertGreater(sum(len(r["on_bin"]) for r in guard.values()), 0) # failure still there with the hotfix + + def test_fix_no_accepted_candidate_on_a_bin(self): + for f, r in _clean(_res(FIX)).items(): + self.assertEqual(r["on_bin"], [], f"{f}: accepted on bin {[(c['colour'], c['area'], c['uv']) for c in r['on_bin']]}") + if r["first_red"] is not None: + self.assertEqual(r["first_red"]["kind"], "object", f) + + def test_bin_image_guard_alone_residual(self): + """bin_image_guard alone removes the bin-wall blobs; one 1015-px lit edge strip of the LEFT bin survives (r2_t12).""" + res = _clean(_res("guard+bin_image")) + left = {f: len(r["on_bin"]) for f, r in res.items() if r["on_bin"]} + self.assertLessEqual(sum(left.values()), 1, left) + for f, r in res.items(): + for c in r["on_bin"]: + self.assertEqual(c["name"], "left") + + def test_real_objects_still_found(self): + for config in ("legacy", "guard", FIX): + for f, r in _res(config).items(): + self.assertEqual(r["missed"], [], f"{config} {f}: missed {r['missed']}") + self.assertEqual(len(r["found"]), 5) + + def test_flag_default_off_and_no_depth_needed(self): + from .config import load_config + self.assertFalse(load_config(overlay="none")["perception"]["bin_image_guard"]) + + def test_shipping_overlay_has_the_fix_on_and_it_holds(self): + from .config import load_config + p = load_config(overlay="finals_r25_gen")["perception"] + self.assertTrue(p["bin_image_guard"]) + self.assertEqual(p["gate_below_bins_params"]["first_run_top_frac"], 0.55) + for f, r in _clean(_res("shipping")).items(): + self.assertEqual(r["on_bin"], [], f) + for f, r in _res("shipping").items(): + self.assertEqual(r["missed"], [], f) + + +if __name__ == "__main__": + unittest.main() diff --git a/aaltonomous/baselines/classical/test_wrong_object_recovery.py b/aaltonomous/baselines/classical/test_wrong_object_recovery.py new file mode 100644 index 0000000..aac6314 --- /dev/null +++ b/aaltonomous/baselines/classical/test_wrong_object_recovery.py @@ -0,0 +1,234 @@ +"""Venue 2026-11-15 causes 3+4: bin.vote tape_body_reject + voter logging, grasp.wrong_object_recovery. Stubs only (no URDF). Run: + python -m unittest aaltonomous.baselines.classical.test_wrong_object_recovery -v +""" +import copy +import unittest + +import numpy as np + +from aaltonomous.scene.scene import Scene + +from .bin_vote import tape_votes, vote_bins +from .config import load_config +from .held import HeldDetector +from .policy import ClassicalPolicy +from .test_held_detector import trace + +SC = Scene.load() +TAPE_RGB = [150, 52, 32] # OpenCV HSV ~[5, 201, 150], inside bin.vote.tape_hsv (and the venue's paper-wrapped bin colour) + + +class _Perc: + """Pixel (u, v) -> pelvis point on the centre bin's rim plane; z linear in depth so the depth-less rim-height solve works.""" + scene = SC + cfg = {"depth_range_mm": [100, 3000]} + + def _pixel_to_pelvis(self, u, v, d, transform_snapshot=None): + u, d = np.asarray(u, float), np.asarray(d, float) + rim = SC.counter_z + float(SC.cfg["bins"]["wall_height"]) + return np.c_[np.full(len(u), 0.505), (u - 12) * 0.001, rim + (d - 1500.0) * 1e-3] + + +def _vote(rgb, **over): + cfg = load_config(overlay="finals_r25_gen", overrides=over) + v = dict(cfg["bin"]["vote"], min_tape_px=20) + return tape_votes(_Perc(), rgb, None, {"center": np.array([0.505, 0.0])}, v)["center"] + + +def _rgb(side): + rgb = np.zeros((120, 120, 3), np.uint8) + rgb[10:10 + side, 10:10 + side] = TAPE_RGB + return rgb + + +class TestTapeBodyReject(unittest.TestCase): + def test_flag_registered_default_off_enabled_in_finals_r25_gen(self): + self.assertFalse(load_config(overlay="none")["bin"]["vote"]["tape_body_reject"]) + self.assertTrue(load_config(overlay="finals_r25_gen")["bin"]["vote"]["tape_body_reject"]) + with self.assertRaises(ValueError): + load_config(overrides={"bin.vote.tape_body_reject": "yes"}) + + def test_bin_body_sized_blob_votes_only_with_the_flag_off(self): + body = _rgb(80) # 6400 px > tape_max_px 3000 + self.assertIsNotNone(_vote(body, **{"bin.vote.tape_body_reject": False})) + self.assertIsNone(_vote(body, **{"bin.vote.tape_body_reject": True})) + + def test_real_tape_sized_blob_still_votes_with_the_flag_on(self): + self.assertIsNotNone(_vote(_rgb(30), **{"bin.vote.tape_body_reject": True})) + + def test_body_blob_does_not_hide_a_small_tape_blob(self): + rgb = _rgb(60) + rgb[20:35, 100:115] = TAPE_RGB + diag = {} + cfg = load_config(overlay="finals_r25_gen") + v = dict(cfg["bin"]["vote"], min_tape_px=20) + tape_votes(_Perc(), rgb, None, {"center": np.array([0.505, 0.0])}, v, diag=diag) + self.assertEqual(diag["rejected_body_px"], [3600]) + self.assertEqual(diag["blob_px"], [3600, 225]) + + +class _P: # minimal planner/perception for vote_bins + scene = SC + + def __init__(self): + self.bin_names = SC.bin_names + + def bin_xy(self, n): + return np.asarray(SC.mat_to_pelvis(np.r_[SC.cfg["bins"]["centers"][n], 0.0])[:2], float) + + def frame_transform(self): + return None + + def use_frame_transform(self, f): + import contextlib + return contextlib.nullcontext() + + +class _Perc2(_Perc): + def frame_transform(self): + return None + + def use_frame_transform(self, f): + import contextlib + return contextlib.nullcontext() + + +class TestVoterLogging(unittest.TestCase): + def test_vote_bins_logs_every_voter_and_why_one_is_missing(self): + planner = _P() + cfg = load_config(overlay="finals_r25_gen", overrides={"bin.vote.frame_transform_snapshot": False}) + logs = [] + vote_bins(_Perc2(), planner, [{"head_rgb": _rgb(30), "head_depth": None}], cfg, log=logs.append) + txt = "\n".join(logs) + for n in planner.bin_names: + self.assertIn(f"[bin.vote.voters] {n}: prior=(", txt) + self.assertIn("rim=none", txt) + self.assertIn("frame0 rim: {'why': 'no depth'}", txt) + self.assertIn("blob_px", txt) + self.assertTrue(any(m.startswith("[bin.vote] ") for m in logs)) # the old summary line is unchanged + + +def _pol(**params): + pol = ClassicalPolicy.__new__(ClassicalPolicy) + cfg = load_config(overlay="finals_r25_gen", overrides={"grasp.wrong_object_recovery": True}) # overlay has it OFF (unvalidated) + cfg["grasp"]["wrong_object_recovery_params"].update(params) + pol.cfg = cfg + pol.logs = [] + pol.log = pol.logs.append + pol._bad_grasp, pol._wor_recoveries, pol._rel_fail = [], 0, False + det = HeldDetector("finger_sum", 0.045) + det._reference_count = 1 # bias reference present (e0 = 0) + pol.held_detector = det + return pol + + +def _g(side="right", close=None): + plan = type("Plan", (), {"side": side, "color": "red", "waypoints": {"grasp": np.array([0.42, -0.21, 0.04])}})() + return {"plan": plan, "t": 11.9, "samples": {"close": close} if close else {}} + + +class TestLiftMargin(unittest.TestCase): + def test_flag_default_off_and_off_in_gen_overlay_unvalidated(self): + self.assertFalse(load_config(overlay="none")["grasp"]["wrong_object_recovery"]) + self.assertFalse(load_config(overlay="finals_r25_gen")["grasp"]["wrong_object_recovery"]) # genuine lift finger_sum never measured + with self.assertRaises(ValueError): + load_config(overrides={"grasp.wrong_object_recovery": 1}) + + def test_venue_bin_wall_reads_are_rejected_and_blacklisted(self): + for lift in (0.046, 0.088): # run 1 / run 2 lift reads, threshold 0.045 + pol = _pol() + cmd, q = trace("right", 0.0, lift) + self.assertTrue(pol.held_detector.held("right", cmd, q)[0]) # the old detector accepted both + self.assertTrue(pol._lift_margin_fail(_g(), q, cmd)) + self.assertTrue(pol._bad_grasp_near((0.43, -0.20))) + self.assertFalse(pol._bad_grasp_near((0.30, -0.05))) + self.assertTrue(any("[wrong_object]" in m for m in pol.logs)) + + def test_clear_grip_passes_and_is_not_blacklisted(self): + pol = _pol() + cmd, q = trace("right", 0.0, 0.15) + self.assertFalse(pol._lift_margin_fail(_g(), q, cmd)) + self.assertEqual(pol._bad_grasp, []) + + def test_ratio_rule_is_optional_and_uses_the_close_read(self): + close = trace("right", 0.0, 0.30)[::-1] # samples hold (tick, hq, hc) + cmd, q = trace("right", 0.0, 0.12) + g = _g(close=(300, close[0], close[1])) + self.assertFalse(_pol()._lift_margin_fail(g, q, cmd)) # off by default + self.assertTrue(_pol(lift_min_ratio=0.5)._lift_margin_fail(g, q, cmd)) # 0.12 < 0.5 x 0.30 + + def test_flag_off_unsure_and_soft_never_fail(self): + cmd, q = trace("right", 0.0, 0.046) + pol = _pol() + pol.cfg["grasp"]["wrong_object_recovery"] = False + self.assertFalse(pol._lift_margin_fail(_g(), q, cmd)) + self.assertFalse(pol._bad_grasp_near((0.42, -0.21))) + pol = _pol() + pol.held_detector = HeldDetector("finger_sum", 0.045) # no bias reference + self.assertFalse(pol._lift_margin_fail(_g(), q, cmd)) + pol = _pol() + self.assertFalse(pol._lift_margin_fail(_g(), q[:7], cmd)) # missing data + self.assertFalse(pol._lift_margin_fail(_g(), np.full(14, np.nan), cmd)) + pol.held_detector.soft = {"rest": np.zeros(14)} + self.assertFalse(pol._lift_margin_fail(_g(), q, cmd)) + + +class _Planner: + def go_to(self, arm, hand, home, post): + return np.zeros((4, 14)), np.zeros((4, 14)) + + +class TestReleaseRecovery(unittest.TestCase): + def _stuck(self, **params): + pol = _pol(**params) + pol.planner = _Planner() + pol.home = np.zeros(14) + pol.post = {"open": np.zeros(14)} + pol.arm_cmd, pol.hand_cmd = np.ones(14), np.zeros(14) + pol._held_hand_lo, pol._held_hand_hi = np.full(14, -2.0), np.full(14, 2.0) + pol._pop = lambda: "popped" + plan = _g()["plan"] + pol._rel = {"plan": plan, "phase": "check", "t0": 0.0, "reopened": True, "rest": (None, None), "n": 10, "open_end": 1} + return pol + + def _hand_stuck(self): + cmd, q = trace("right", 0.0, 0.2) + return {"hand_q": 2 * cmd - q}, cmd # fingers still closed past the "open" command: not empty + + def _step(self, pol, t=5.0): + obs, cmd = self._hand_stuck() + pol.hand_cmd = cmd + return pol._release_confirm(obs, t) + + def test_failed_release_continues_up_to_max_recoveries_then_stops(self): + pol = self._stuck() + for i in (1, 2): + self.assertEqual(self._step(pol), "popped") + self.assertFalse(pol._rel_fail, i) + self.assertEqual(pol._wor_recoveries, i) + self.assertIsNone(pol._rel) + pol._rel = self._stuck()._rel + self.assertEqual(self._step(pol), "popped") + self.assertTrue(pol._rel_fail) # third failure: stop as before + self.assertEqual(len(pol._bad_grasp), 2) # the two recovered grasps are blacklisted + + def test_flag_off_stops_like_before(self): + pol = self._stuck() + pol.cfg["grasp"]["wrong_object_recovery"] = False + self.assertEqual(self._step(pol), "popped") + self.assertTrue(pol._rel_fail) + self.assertEqual(pol._wor_recoveries, 0) + self.assertEqual(pol._bad_grasp, []) + + def test_recovery_uses_the_existing_open_home_path_only(self): + pol = self._stuck() + calls = [] + pol.planner.go_to = lambda *a: calls.append(a) or (np.zeros((4, 14)), np.zeros((4, 14))) + self._step(pol) + self.assertEqual(len(calls), 1) + np.testing.assert_array_equal(calls[0][3], pol.post["open"]) # fully open hand + self.assertTrue(any("recovery 1/2" in m for m in pol.logs)) + + +if __name__ == "__main__": + unittest.main() diff --git a/aaltonomous/cloud/aalto.sh b/aaltonomous/cloud/aalto.sh index 5ca021c..d65d864 100755 --- a/aaltonomous/cloud/aalto.sh +++ b/aaltonomous/cloud/aalto.sh @@ -58,7 +58,7 @@ snap) python aaltonomous/cloud/snap.py "$@" ;; test) python -m unittest aaltonomous/interface/test_safety.py aaltonomous/interface/test_env.py \ - aaltonomous/scene/test_scene.py aaltonomous/deploy/test_deploy.py aaltonomous/sim/aalto_task/test_layout.py ;; + aaltonomous/scene/test_scene.py aaltonomous/deploy/test_deploy.py aaltonomous/sim/aalto_task/test_layout.py aaltonomous/sim/aalto_task/test_venue.py ;; py) exec python "$@" ;; *) diff --git a/aaltonomous/sim/VENUE_TWIN.md b/aaltonomous/sim/VENUE_TWIN.md new file mode 100644 index 0000000..2f5a64d --- /dev/null +++ b/aaltonomous/sim/VENUE_TWIN.md @@ -0,0 +1,81 @@ +# Venue twin: `venue_20261115` + +Opt-in sim variant that looks like the finals table of 2026-11-15. On the real robot the policy grabbed a +red-orange paper-wrapped bin instead of a cube; the default sim bins are plain orange boxes +(`scene.yaml` `bins.rgb` [230,110,30]) and never showed this. **Nothing changes unless `AALTO_DR=venue_20261115`.** + +## What it changes + +Selected like every other DR variant: `AALTO_DR=venue_20261115` when the sim starts (config +`aaltonomous/sim/dr/venue_20261115.yaml`, loaded by `aalto_task/dr.py`). `evaluate.py` needs no flag: it reads the +sim's DR config name (variant label `venue_20261115`) and calls `randomize` with `seed` per trial. + +| Surface | Sim colour | +|---|---| +| Bin outer walls + rim | red-orange paper (`bin_outer_rgb`) | +| Bin inside (floor + inner faces) | cardboard brown (`bin_inner_rgb`) | +| Table (counter box and the printed mat, whose texture is replaced by a flat colour) | `table_rgb` | +| Object classes red / yellow / blue | venue orange / yellow / blue (`object_rgb`; the "red" class IS the orange parts) | +| Jitter per episode | +-4 % per channel and 0.9-1.1 brightness per surface (paper 0.65-1.1), key light 0.9-1.5x, plus the usual pose / camera noise / mass / friction | + +Hook: `hooks._apply_venue` sets `diffuseColor` of the existing `UsdPreviewSurface` shaders. The bin walls are single +3 mm boxes (one material), so "orange outside, brown inside" is done with a 0.6 mm visual-only liner cube (no collision) +on the inner face of each wall, created once under the wall prim (`dr.bin_liner_specs`). No USD asset or geometry +is edited; bin physics and sizes are unchanged. Sampled sRGB values are converted to linear before being used. + +## Sampled colours (sRGB 0-255) + +Source: `knowledge/humanoid/real_runs/larp-interns_20261115_091835/episode_000.mp4`, 640x480, frame at t=5 s +(`ffmpeg -ss 5 -i episode_000.mp4 -frames:v 1 f_5.png`; robot arm not in view). Median over the patch (x,y,w,h in px): + +| Surface | Patch | Measured | Used | +|---|---|---|---| +| Bin paper, camera-facing front faces (3 bins) | L (40,215,140,28) / C (250,238,160,22) / R (490,228,130,22) | (99,40,25) / (103,42,32) / (106,47,28) | hue of these, brightness of the lit side face below | +| Bin paper, lit side face (left bin) | (200,90,25,100) | (172,80,61) | **(172,68,47)** | +| Bin inner back wall (centre, shaded) / floor | (290,110,100,40) / (50,125,120,40) | (90,81,71) / (130,141,127) | **(126,113,99)** (shaded wall x ~1.4) | +| Table | (130,305,100,35), (20,420,200,50), (0,60,15,100), (330,290,150,30) | (143,135,131), (137,141,136), (137,139,133), (146,135,130) | **(141,136,132)** | +| Blue cube | top (268,332,32,12) / front (255,360,50,28) | (84,134,226) / (82,132,225) | **(84,134,226)** | +| Orange cube (class red) | top (392,318,30,12) / left (375,340,12,30) | (244,147,81) / (133,57,25) | **(244,147,81)** | +| Orange cylinder | top (380,405,55,25) | (250,168,114), overexposed | not used (same class) | +| Yellow cube | top (470,312,40,14) / left (455,340,18,20) | (213,255,182) overexposed / (151,118,53) | **(232,205,95)** (hue of the sides, brightened) | +| Yellow prism | top (280,440,40,20) | (209,253,180) overexposed | same class colour | + +Caveats: the camera has a neutral/greenish white balance (table reads grey, not brown) and the paper is 0.6x darker on +camera-facing faces than on the lit side face; the "Used" paper and yellow values are therefore derived (measured +hue, lit brightness), not raw medians. The raw numbers above are all you need to re-derive them; edit the yaml to change. +Re-check with `aalto snap` that a sim head frame looks like the real one before trusting a result. + +## Run it (GPU box, container `aalto`, see `aaltonomous/cloud/QUICKSTART.md`) + +```bash +cd /opt/aalto && git fetch && git checkout timi/venue-twin # or copy the branch into the container +aalto test # offline unit tests incl. test_venue.py (should be OK) +aalto stop; AALTO_DR=venue_20261115 aalto sim # restart the sim with the variant (log /tmp/aalto_sim.log must say + # "domain randomization ON: .../venue_20261115.yaml") +aalto snap --out /tmp/venue_snap # look at head.png: orange paper bins outside, brown inside, flat table +# the policy image under test (serves ws on 8000), 20 trials with the same seeds as the nominal baseline: +aalto eval --container --team venue-twin --task sort_fixed --trials 20 --seed 1000 \ + --out runs/venue --variant venue_20261115 +# policy already served elsewhere: aalto eval --policy ws://localhost:8000 --team venue-twin --task sort_fixed --trials 20 --seed 1000 --out runs/venue +# identical call without the variant for the A/B baseline: +aalto stop; aalto sim; aalto eval --container --team nominal --task sort_fixed --trials 20 --seed 1000 --out runs/nominal +# same thing without the wrapper: python -m aaltonomous.sim.evaluate --container --team venue-twin \ +# --task sort_fixed --trials 20 --seed 1000 --out runs/venue +``` + +Results: `runs/venue/_sort_fixed_venue_20261115_/eval.json` (per trial outcomes, score, `dr_params`) and +the per-episode logs (`episode_NNN.npz` / `.jsonl`) next to it (`deploy.runner.Recorder`); the policy's own logs hold +its grasp targets and release_confirm messages. + +## What counts as "reproduced the failure" + +Compared with the nominal run on the same seeds, in the venue run at least one of: +- the commanded grasp / hand target (arm + hand close in the episode log) lies on a bin wall or rim rather than on a cube, + and the hand closes on it (video: hand on a box wall); +- the hand closes on the bin and the object count in the bin does not change; the policy's release step then reports + "hand still not empty" (release_confirm) or never releases; +- scored outcomes show cubes left on the counter (`counter`) while the policy spent the episode on a bin, with + score clearly below the nominal run. +It is NOT reproduced if the venue run behaves like the nominal one (same grasp targets on cubes). In that case try +`bin_outer_brightness: [1.0, 1.0]` + `bin_outer_rgb: [200, 90, 50]` (closer to the orange cube) and report back. +After a fix, rerun the same command: score back to nominal, no grasp on a bin wall. diff --git a/aaltonomous/sim/aalto_task/dr.py b/aaltonomous/sim/aalto_task/dr.py index d37fbe7..6cbf90a 100644 --- a/aaltonomous/sim/aalto_task/dr.py +++ b/aaltonomous/sim/aalto_task/dr.py @@ -18,6 +18,9 @@ pose: {xy_jitter: m (per object, uniform in a disc), yaw_jitter_deg} camera: {rgb_noise_std: 0-255 units, rgb_gain: [lo, hi], depth_noise_mm, depth_noise_rel (x depth), depth_hole_frac, pose_jitter_m, pose_jitter_deg} + venue: optional (venue twin, dr/venue_20261115.yaml; absent = nothing changes): ABSOLUTE sRGB 0-255 colours + {srgb, table_rgb, bin_outer_rgb, bin_inner_rgb, object_rgb: {: rgb}, rgb_jitter, brightness, + bin_outer_brightness} that replace the scene.yaml colours; hooks.py applies them. """ from __future__ import annotations @@ -104,9 +107,51 @@ def sample(cfg, seed, object_names=(), bin_names=()): "rot_offset_rad": [float(rc.uniform(-aj, aj)) if aj else 0.0 for _ in range(3)], "stream_seed": int(rc.integers(2 ** 31)), } + if cfg.get("venue"): + out["venue"] = _sample_venue(cfg["venue"], np.random.default_rng(ss.spawn(1)[0]), object_names, bin_names) return out +def _sample_venue(V, rng, object_names, bin_names): + """Per-surface RGB multipliers (rgb_jitter x brightness); drawn from a 6th child stream so the other sections' + draws for a seed are unchanged.""" + def mult(bright): + return [float(j * b) for j, b in zip(_rgb_jit(rng, V.get("rgb_jitter")), [_u(rng, bright)] * 3)] + return { + "table": mult(V.get("brightness")), + "bins": {b: {"outer": mult(V.get("bin_outer_brightness", V.get("brightness"))), + "inner": mult(V.get("brightness"))} for b in bin_names}, + "objects": {n: mult(V.get("brightness")) for n in object_names}, + } + + +def venue_color(rgb255, mult=(1.0, 1.0, 1.0), srgb=True): + """sampled sRGB 0-255 x multipliers -> diffuseColor 0..1 (linear if srgb), clipped.""" + c = np.clip(np.asarray(rgb255, float) / 255.0 * np.asarray(mult, float), 0.0, 1.0) + if srgb: + c = np.where(c <= 0.04045, c / 12.92, ((c + 0.055) / 1.055) ** 2.4) + return [float(v) for v in c] + + +LINER_EPS = 0.0006 # m, thickness of the visual-only inner liner + + +def bin_liner_specs(bins_cfg): + """Visual-only inner-face liners for the 4 walls of a bin (the walls are single 3 mm boxes, so one material + cannot be 'orange outside, cardboard inside'). Axis-aligned, in the wall's own frame (= pelvis axes, the wall + prims are unrotated): {part: {"center": (x, y, z), "size": (x, y, z)}}, relative to the wall prim centre. + Mat x -> pelvis y, mat y -> pelvis x (Scene.mat_to_pelvis).""" + ox, oy = bins_cfg["outer_size"] + t, h, e = bins_cfg["wall_thickness"], bins_cfg["wall_height"], LINER_EPS + s = t / 2 + e / 2 + return { + "wall_xlo": {"center": (0.0, +s, 0.0), "size": (oy, e, h)}, + "wall_xhi": {"center": (0.0, -s, 0.0), "size": (oy, e, h)}, + "wall_ylo": {"center": (+s, 0.0, 0.0), "size": (e, ox - 2 * t, h)}, + "wall_yhi": {"center": (-s, 0.0, 0.0), "size": (e, ox - 2 * t, h)}, + } + + def jitter_layout(layout, params): """Apply pose jitter (mat coords) to a layout copy. Deterministic for the params' stream seed.""" P = (params or {}).get("pose") or {} diff --git a/aaltonomous/sim/aalto_task/hooks.py b/aaltonomous/sim/aalto_task/hooks.py index 0475371..da6b607 100644 --- a/aaltonomous/sim/aalto_task/hooks.py +++ b/aaltonomous/sim/aalto_task/hooks.py @@ -301,7 +301,10 @@ def _apply_dr(env, rt, P): ok["lighting"] = f"{type(e).__name__}: {e}" try: T, O = P["textures"], P["objects"] - for k, lst in base["colors"].items(): + if P.get("venue"): # venue twin (dr/venue_20261115.yaml): absolute colours replace the nominal ones + _apply_venue(stage, rt, P["venue"], rt.dr_cfg["venue"], base) + T = None + for k, lst in ({} if T is None else base["colors"]).items(): for sh, rgb in lst: if k == "Counter": new = [c * j for c, j in zip(rgb, T["counter_rgb"])] @@ -312,7 +315,7 @@ def _apply_dr(env, rt, P): new = dr.jitter_color(rgb, o["dh"], o["s"], o["v"]) sh.GetInput("diffuseColor").Set(Gf.Vec3f(*[float(min(max(c, 0.0), 1.0)) for c in new])) tex = stage.GetPrimAtPath("/World/envs/env_0/Mat/material/texture") - if tex.IsValid(): + if T is not None and tex.IsValid(): from pxr import UsdShade sh = UsdShade.Shader(tex) s, bias = T["mat_scale"], T["mat_bias"] @@ -352,6 +355,66 @@ def _apply_dr(env, rt, P): return ok +def _set_color(sh, rgb): + from pxr import Gf + sh.GetInput("diffuseColor").Set(Gf.Vec3f(*[float(c) for c in rgb])) + + +def _apply_venue(stage, rt, V, Vcfg, base): + """Venue twin colours (override hook, only reached when the DR config has a `venue` section): table, bin outer + paper (wall boxes), bin inner cardboard (floor + visual-only liners inside the walls), object classes.""" + from pxr import Gf, Sdf, UsdGeom, UsdShade + from .layout import parse_name + srgb = bool(Vcfg.get("srgb", True)) + col = lambda rgb, m: dr.venue_color(rgb, m, srgb) + # table: counter box + the printed mat (its texture is replaced by the flat colour) + for sh, _ in base["colors"].get("Counter", []): + _set_color(sh, col(Vcfg["table_rgb"], V["table"])) + mat = _shaders(stage, "/World/envs/env_0/Mat/material", "diffuseColor") + for sh in mat: + inp = sh.GetInput("diffuseColor") + try: + inp.ClearSources() + except Exception: + inp.DisconnectSource() + _set_color(sh, col(Vcfg["table_rgb"], V["table"])) + # bins + specs = dr.bin_liner_specs(rt.sc.cfg["bins"]) + for bn in rt.sc.bin_names: + outer, inner = col(Vcfg["bin_outer_rgb"], V["bins"][bn]["outer"]), col(Vcfg["bin_inner_rgb"], V["bins"][bn]["inner"]) + for part in ("floor", "wall_xlo", "wall_xhi", "wall_ylo", "wall_yhi"): + for sh, _ in base["colors"].get(f"bin:{bn}:{part}", []): + _set_color(sh, inner if part == "floor" else outer) + if part == "floor": + continue + root = f"/World/envs/env_0/Bin_{bn}_{part}" + liner = root + "/inner_liner" + if not stage.GetPrimAtPath(liner).IsValid(): + sp = specs[part] + cube = UsdGeom.Cube.Define(stage, liner) + cube.GetSizeAttr().Set(1.0) + xf = UsdGeom.Xformable(cube) + xf.AddTranslateOp().Set(Gf.Vec3d(*sp["center"])) + xf.AddScaleOp().Set(Gf.Vec3f(*sp["size"])) + mt = UsdShade.Material.Define(stage, liner + "/material") + sh = UsdShade.Shader.Define(stage, liner + "/material/surface") + sh.CreateIdAttr("UsdPreviewSurface") + sh.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.9) + sh.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.0) + sh.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(*inner)) + mt.CreateSurfaceOutput().ConnectToSource(sh.ConnectableAPI(), "surface") + UsdShade.MaterialBindingAPI.Apply(cube.GetPrim()).Bind(mt) + else: + for sh in _shaders(stage, liner, "diffuseColor"): + _set_color(sh, inner) + # objects: venue colour per class (red = the venue's orange parts) + for n in rt.names: + c = parse_name(n)[0] + if c in Vcfg["object_rgb"]: + for sh, _ in base["colors"].get(f"obj:{n}", []): + _set_color(sh, col(Vcfg["object_rgb"][c], V["objects"][n])) + + def runtime(env): global _RT if _RT is None: diff --git a/aaltonomous/sim/aalto_task/test_venue.py b/aaltonomous/sim/aalto_task/test_venue.py new file mode 100644 index 0000000..7853c13 --- /dev/null +++ b/aaltonomous/sim/aalto_task/test_venue.py @@ -0,0 +1,89 @@ +"""Offline (CPU, no Isaac) tests of the venue twin DR variant `venue_20261115` and that the default stays untouched.""" +import json +import os +import unittest +from unittest import mock + +import numpy as np + +from aaltonomous.scene.scene import Scene +from aaltonomous.sim.aalto_task import dr +from aaltonomous.sim.aalto_task import layout as L + +OBJS = L.object_names(Scene.load()) + + +class VenueTwin(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.sc = Scene.load() + cls.cfg = dr.load("venue_20261115") + + def test_parses(self): + v = self.cfg["venue"] + self.assertEqual(self.cfg["name"], "venue_20261115") + for k in ("table_rgb", "bin_outer_rgb", "bin_inner_rgb"): + self.assertEqual(len(v[k]), 3) + self.assertTrue(all(0 <= c <= 255 for c in v[k])) + self.assertEqual(set(v["object_rgb"]), set(self.sc.cfg["objects"]["colors"])) + # the point of the twin: outer paper is red-orange (R >> G > B), inner is a low-saturation brown + r, g, b = v["bin_outer_rgb"] + self.assertTrue(r > 2 * g > 2 * b * 0.9 and r > 150) + ri, gi, bi = v["bin_inner_rgb"] + self.assertLess(max(v["bin_inner_rgb"]) - min(v["bin_inner_rgb"]), 60) + + def test_sample_deterministic_and_complete(self): + a = dr.sample(self.cfg, 7, OBJS, self.sc.bin_names) + b = dr.sample(self.cfg, 7, OBJS, self.sc.bin_names) + self.assertEqual(json.dumps(a, sort_keys=True), json.dumps(b, sort_keys=True)) + V = a["venue"] + self.assertEqual(set(V["bins"]), set(self.sc.bin_names)) + self.assertEqual(set(V["objects"]), set(OBJS)) + self.assertNotEqual(a["venue"], dr.sample(self.cfg, 8, OBJS, self.sc.bin_names)["venue"]) + for m in [V["table"]] + [x for bb in V["bins"].values() for x in bb.values()] + list(V["objects"].values()): + self.assertTrue(all(0.5 < c < 1.3 for c in m)) + + def test_venue_stream_does_not_shift_other_sections(self): + a = dr.sample(self.cfg, 3, OBJS, self.sc.bin_names) + nov = {k: v for k, v in self.cfg.items() if k != "venue"} + b = dr.sample(nov, 3, OBJS, self.sc.bin_names) + self.assertNotIn("venue", b) + a.pop("venue") + self.assertEqual(json.dumps(a, sort_keys=True), json.dumps(b, sort_keys=True)) + + def test_default_unchanged(self): + with mock.patch.dict(os.environ, {}, clear=False): + os.environ.pop("AALTO_DR", None) + self.assertIsNone(dr.load()) # DR (and the venue twin) off unless asked for + tr = dr.load("train") + self.assertNotIn("venue", tr) + self.assertNotIn("venue", dr.sample(tr, 1, OBJS, self.sc.bin_names)) + self.assertEqual(list(self.sc.cfg["bins"]["rgb"]), [230, 110, 30]) # nominal bin colour not edited + self.assertEqual(self.sc.cfg["objects"]["colors"]["red"], [200, 30, 30]) + + def test_venue_color_linear(self): + self.assertEqual(dr.venue_color([255, 255, 255]), [1.0, 1.0, 1.0]) + self.assertAlmostEqual(dr.venue_color([128, 128, 128])[0], 0.2158, places=3) + self.assertAlmostEqual(dr.venue_color([128, 0, 0], srgb=False)[0], 128 / 255) + self.assertEqual(dr.venue_color([250, 0, 0], [2, 1, 1], srgb=False)[0], 1.0) + + def test_liners_sit_on_inner_faces(self): + bc = self.sc.cfg["bins"] + ox, oy = bc["outer_size"] + t, h = bc["wall_thickness"], bc["wall_height"] + specs = dr.bin_liner_specs(bc) + cx, cy = bc["centers"]["center"] + centre = np.asarray(self.sc.mat_to_pelvis([cx, cy, 0.0])) + for part, mat_off in (("wall_xlo", (-ox / 2 + t / 2, 0)), ("wall_xhi", (ox / 2 - t / 2, 0)), + ("wall_ylo", (0, -oy / 2 + t / 2)), ("wall_yhi", (0, oy / 2 - t / 2))): + wall = np.asarray(self.sc.mat_to_pelvis([cx + mat_off[0], cy + mat_off[1], 0.0])) + liner = wall + np.asarray(specs[part]["center"]) + # liner centre is closer to the bin centre than the wall centre, by half wall + half liner + self.assertAlmostEqual(np.linalg.norm(wall - centre) - np.linalg.norm(liner - centre), + t / 2 + dr.LINER_EPS / 2, places=9) + self.assertEqual(specs[part]["size"][2], h) + self.assertEqual(min(specs[part]["size"][:2]), dr.LINER_EPS) + + +if __name__ == "__main__": + unittest.main() diff --git a/aaltonomous/sim/dr/venue_20261115.yaml b/aaltonomous/sim/dr/venue_20261115.yaml new file mode 100644 index 0000000..99d8427 --- /dev/null +++ b/aaltonomous/sim/dr/venue_20261115.yaml @@ -0,0 +1,37 @@ +# "Venue twin" of the Junction 2026 finals table (real-robot run larp-interns_20261115_091835, head camera). +# Opt-in: AALTO_DR=venue_20261115 aalto sim (docs: aaltonomous/sim/VENUE_TWIN.md). Default sim is untouched. +# The `venue` section sets ABSOLUTE colours (sRGB 0-255, sampled from the video, see VENUE_TWIN.md) instead of +# jittering the scene.yaml colours; everything else is the usual DR schema (aalto_task/dr.py). +name: venue_20261115 +venue: + srgb: true # values are sRGB as seen by the camera -> converted to linear for the shaders + table_rgb: [141, 136, 132] # counter + printed mat (mat texture replaced by this flat colour) + bin_outer_rgb: [172, 68, 47] # red-orange paper on the OUTSIDE (and rim) of all three boxes + bin_inner_rgb: [126, 113, 99] # plain cardboard on the inside faces + floor (thin visual-only liner) + object_rgb: # per scene.yaml class; "red" = the venue's ORANGE parts + red: [244, 147, 81] + yellow: [232, 205, 95] + blue: [84, 134, 226] + rgb_jitter: 0.04 # +-4 % per channel, per surface + brightness: [0.9, 1.1] # one factor per surface + bin_outer_brightness: [0.65, 1.1] # paper looked 0.6x darker on camera-facing faces than on lit side faces +lighting: # bright venue light, shadows from the arm (hard key light) + key_intensity_scale: [0.9, 1.5] + key_pos_jitter: [0.4, 0.4, 0.3] + key_rgb_jitter: 0.04 + dome_intensity_scale: [0.8, 1.2] + dome_rgb_jitter: 0.03 +objects: + mass_scale: [0.7, 1.5] + friction_scale: [0.75, 1.1] +pose: + xy_jitter: 0.005 + yaw_jitter_deg: 5 +camera: + rgb_noise_std: 3.0 + rgb_gain: [0.9, 1.1] + depth_noise_mm: 1.0 + depth_noise_rel: 0.002 + depth_hole_frac: 0.01 + pose_jitter_m: 0.01 + pose_jitter_deg: 1.0 diff --git a/docs/postmortem/bin_dragged_sequence.jpg b/docs/postmortem/bin_dragged_sequence.jpg new file mode 100644 index 0000000..5dd24dd Binary files /dev/null and b/docs/postmortem/bin_dragged_sequence.jpg differ diff --git a/docs/postmortem/gate_overlay.jpg b/docs/postmortem/gate_overlay.jpg new file mode 100644 index 0000000..6520cf5 Binary files /dev/null and b/docs/postmortem/gate_overlay.jpg differ diff --git a/tools/check_release_flags.py b/tools/check_release_flags.py index bb64387..ac3772f 100644 --- a/tools/check_release_flags.py +++ b/tools/check_release_flags.py @@ -90,7 +90,9 @@ GEN_FINAL_FLAGS = frozenset({"grasp.arm_xy_offset", "motion.output_hold_on_late", "instruction.generality", "instruction.venue_veto", "policy.shape_rule_guard", "motion.arm_tau_ff", "motion.sag_comp", "perception.bin_bottom_first_run", "perception.fk_colour_rescue", "calib.plane.robust_height", "perception.plan_frame", - "calib.plane.robust_agree_m", "perception.height_range", "bin.fallback_centre_x", "bin.fallback_drop_z", "bin.reach_x_max", "bin.reach_drop_z", "planner.budget_escalate"}) + "calib.plane.robust_agree_m", "perception.height_range", "bin.fallback_centre_x", "bin.fallback_drop_z", "bin.reach_x_max", "bin.reach_drop_z", "planner.budget_escalate", + "perception.bin_size_guard", "perception.bin_image_guard", "bin.vote.tape_body_reject", "grasp.wrong_object_recovery", + "perception.gate_below_bins_params.first_run_top_frac"}) R25_FLAGS = frozenset({"brain.known_task_hard_off", "planner.candidate_budget", "profile_auto.second_cue"}) # fsm layer (real-only dead-time / failure-mode fixes; false in profile_auto.sim_overrides), finals_r25_gen only FSM_FLAGS = frozenset({"policy.zero_det_relook", "policy.startup_overlap", "policy.contact_start_unblock", "perception.look_summary"}) diff --git a/tools/ci_unit_modules.txt b/tools/ci_unit_modules.txt index b55f59c..8b9545e 100644 --- a/tools/ci_unit_modules.txt +++ b/tools/ci_unit_modules.txt @@ -2,6 +2,8 @@ # third_party/ (URDFs, sim) is not in git, so the other modules need the sim image. aaltonomous.baselines.classical.test_arm_vel_cap aaltonomous.baselines.classical.test_bare_table +aaltonomous.baselines.classical.test_bin_gate_defence +aaltonomous.baselines.classical.test_bin_size_guard aaltonomous.baselines.classical.test_bin_vote_perception aaltonomous.baselines.classical.test_brain_planner aaltonomous.baselines.classical.test_color_gate @@ -37,6 +39,7 @@ aaltonomous.baselines.classical.test_reviewfix_a aaltonomous.baselines.classical.test_sat_floor aaltonomous.baselines.classical.test_shape_synonyms aaltonomous.baselines.classical.test_skill_memory +aaltonomous.baselines.classical.test_venue_replay aaltonomous.baselines.classical.test_venue_veto aaltonomous.data.test_record aaltonomous.data.test_to_groot @@ -44,3 +47,4 @@ aaltonomous.data.test_to_lerobot aaltonomous.sim.aalto_task.test_layout tools.brain.test_policy_server_sigterm tools.test_guard_eval_args +aaltonomous.baselines.classical.test_wrong_object_recovery diff --git a/tools/venue_replay.py b/tools/venue_replay.py new file mode 100644 index 0000000..1529fa7 --- /dev/null +++ b/tools/venue_replay.py @@ -0,0 +1,269 @@ +#!/usr/bin/env python +"""Offline replay of the classical policy's colour perception on real venue frames (no GPU, no simulator, no depth). + +Runs the REAL `Perception.detect()` code path (HSV masks -> blob extraction -> bin_size_guard -> split_blobs -> +`_detect_blob` -> reject -> color_gate) on RGB frames cut from the finals recordings, and compares every candidate +blob with hand-labelled ground truth (object + bin boxes, `test_data/venue_20261115/*.json`). + +DEPTH: the recordings contain no depth. We pass depth=None. Stages that need depth are therefore NOT exercised: + - bin_size_guard 3D checks (top height > 7.5 cm, footprint > 13 cm): skipped; only its AREA rule (> 4x expected object + area, expected area from the camera ray at the object-top plane) is evaluated. + - depth top-face localisation / height estimate / depth_recover / 3D color_gate rules: skipped, detections are located + by the plane fallback (pixel ray x counter plane), so `xy` is only a plane projection. +Everything else (colour thresholds, open_px, min_area, split, color_gate no-Z rules, sat floor) is the shipped code. + +Usage (from the repo root): + uv run -q --with numpy --with opencv-python-headless --with pyyaml --with scipy python tools/venue_replay.py + ... tools/venue_replay.py --frames r1_t12 r2_t12 --configs legacy guard guard+bin_image --save-overlays /tmp/ov +`legacy` = 1063951 behaviour (finals_r25_gen with perception.bin_size_guard off), `guard` = hotfix 77ba4e0, +`guard+bin_image` = hotfix plus the default-off perception.bin_image_guard rule. +""" +from __future__ import annotations + +import argparse +import glob +import json +import os +import sys + +import numpy as np + +ROOT = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) +if ROOT not in sys.path: + sys.path.insert(0, ROOT) + +DATA = os.path.join(ROOT, "aaltonomous", "baselines", "classical", "test_data", "venue_20261115") +# policy colour class of each physical object colour (orange cube / cylinder are the policy's "red" class) +POLICY_CLASS = {"blue": "blue", "orange": "red", "yellow": "yellow"} +# finals_r25_gen now SHIPS bin_image_guard + first_run_top_frac 0.55; the named configs below pin the pre-fix values +# first, so each still means what its name says. "shipping" = the overlay exactly as baked. +_PRE_FIX = {"perception.bin_image_guard": False, "perception.gate_below_bins_params.first_run_top_frac": 0.45} +_RAW_CONFIGS = { + "shipping": None, + "legacy": {"perception.bin_size_guard": False}, + "guard": {"perception.bin_size_guard": True}, + "guard+bin_image": {"perception.bin_size_guard": True, "perception.bin_image_guard": True}, + # data point only: widen the gate_below_bins first-run band (venue right-bin front wall starts at row ~220 > 0.45*480) + "guard+bin_image+first_run_0.55": {"perception.bin_size_guard": True, "perception.bin_image_guard": True, + "perception.gate_below_bins_params.first_run_top_frac": 0.55}, + "guard+first_run_0.55+rowmed61": {"perception.bin_size_guard": True, "perception.gate_below_bins_params.first_run_top_frac": 0.55, + "perception.gate_below_bins_params.row_median_px": 61}, + "guard+rowmed61_only": {"perception.bin_size_guard": True, "perception.gate_below_bins_params.row_median_px": 61}, + "guard+first_run_0.55": {"perception.bin_size_guard": True, "perception.gate_below_bins_params.first_run_top_frac": 0.55}, +} +CONFIGS = {k: (None if v is None else {**_PRE_FIX, **v}) for k, v in _RAW_CONFIGS.items()} +CONFIGS["shipping"] = {} + + +def load_frames(names=None): + out = [] + for jp in sorted(glob.glob(os.path.join(DATA, "*.json"))): + with open(jp) as fh: + lab = json.load(fh) + if names and lab["frame"][:-4] not in names: + continue + out.append((lab, os.path.join(DATA, lab["frame"]))) + return out + + +def make_perception(overrides, overlay="finals_r25_gen", log=lambda *_: None): + from aaltonomous.baselines.classical.config import load_config + from aaltonomous.baselines.classical.perception import Perception + from aaltonomous.interface.calibration import Calibration + from aaltonomous.scene.scene import Scene + cfg = load_config(overlay=overlay, overrides=dict(overrides)) + return Perception(cfg, Scene.load(), Calibration.load(), log=log) + + +def _box_mask(shape, boxes): + m = np.zeros(shape, bool) + for x0, y0, x1, y1 in boxes: + m[max(0, y0):y1, max(0, x0):x1] = True + return m + + +def run_frame(perc, rgb): + """-> list of candidate dicts: accepted detections and rejected ones, each with colour, area, uv, mask (if known), + status ('accepted' | 'rejected:').""" + import cv2 + masks = {} # id(det) -> blob mask + guard_masks = [] # (colour-less) masks of blobs rejected by the size guard (rejected before _detect_blob) + orig_blob, orig_guard = perc._detect_blob, perc._bin_sized_blob + + def det_blob(color, blob, area, centroid, depth, include_bins, dets, rejected, *a, **k): + n_d, n_r = len(dets), len(rejected) + orig_blob(color, blob, area, centroid, depth, include_bins, dets, rejected, *a, **k) + for d in dets[n_d:]: + masks[id(d)] = blob + for d, _ in rejected[n_r:]: + masks[id(d)] = blob + + def guard(blob, area, centroid, depth): + why = orig_guard(blob, area, centroid, depth) + if why: + guard_masks.append((tuple(centroid), blob)) + return why + + perc._detect_blob, perc._bin_sized_blob = det_blob, guard + try: + dets, rejected = perc.detect(rgb, None, debug=True) + finally: + perc._detect_blob, perc._bin_sized_blob = orig_blob, orig_guard + cands = [] + for d in dets: + cands.append(dict(colour=d.color, area=int(d.n_px), uv=tuple(d.uv) if d.uv is not None else None, + xy=tuple(float(v) for v in d.top[:2]), status="accepted", mask=masks.get(id(d)), det=d)) + for d, why in rejected: + m = masks.get(id(d)) + if m is None and d.uv is not None: # guard rejection: match the mask by centroid + for c, bm in guard_masks: + if abs(c[0] - d.uv[0]) < 1.0 and abs(c[1] - d.uv[1]) < 1.0: + m = bm + break + cands.append(dict(colour=d.color, area=int(d.n_px), uv=tuple(d.uv) if d.uv is not None else None, + xy=tuple(float(v) for v in d.top[:2]), status="rejected:" + str(why), mask=m, det=d)) + return cands + + +def classify(cand, lab, shape): + """Where does this blob sit w.r.t. the labels? -> (kind, name, bin_frac, obj_frac, matched_object_index).""" + objs = [o["box"] for o in lab["objects"]] + bins = [b["box"] for b in lab["bins"]] + om = [_box_mask(shape, [b]) for b in objs] + obj_union = _box_mask(shape, objs) + bin_only = _box_mask(shape, bins) & ~obj_union + m = cand["mask"] + if m is None: + if cand["uv"] is None: + return "unknown", "", 0.0, 0.0, None + m = np.zeros(shape, bool) + u, v = int(round(cand["uv"][0])), int(round(cand["uv"][1])) + m[max(0, v - 2):v + 3, max(0, u - 2):u + 3] = True + m = m > 0 + n = max(1, int(m.sum())) + bf = float((m & bin_only).sum()) / n + of = float((m & obj_union).sum()) / n + ov = [float((m & x).sum()) for x in om] + j = int(np.argmax(ov)) if ov and max(ov) > 0 else None + if bf >= 0.5 and of < 0.5: + bn = max(lab["bins"], key=lambda b: float((m & _box_mask(shape, [b["box"]])).sum()))["name"] + return "bin", bn, bf, of, None + if of >= 0.5: + return "object", f'{lab["objects"][j]["colour"]} {lab["objects"][j]["shape"]}', bf, of, j + return ("mixed" if of > 0.1 and bf > 0.1 else "other"), "", bf, of, j + + +def rank_red(perc, cands): + """Order the planner would try the accepted red candidates: policy.order=nearest_bin -> distance (plane xy) to the + mapped bin (policy.fixed_mapping red->right). Returns the sorted candidate list.""" + from aaltonomous.baselines.classical.planner import lookup_detection_bin + import aaltonomous.baselines.classical.planner as pl + pol = perc.cfg.get("policy") or {} + mapping = pol.get("fixed_mapping") or {} + out = [] + for c in cands: + if c["status"] != "accepted" or c["colour"] != "red": + continue + b = lookup_detection_bin(perc.cfg, mapping, c["det"]) + if b is None or not np.all(np.isfinite(c["xy"])): + key = float("inf") + else: + bxy = perc.scene.mat_to_pelvis(np.r_[perc.scene.cfg["bins"]["centers"][b], 0.0])[:2] + key = float(np.linalg.norm(np.asarray(c["xy"]) - bxy)) + out.append((key, c)) + out.sort(key=lambda t: t[0]) + return [c for _, c in out] + + +def evaluate(names=None, configs=None, overlay="finals_r25_gen", save_overlays=None, verbose=True): + """-> {config: {frame: result}} with result = dict(cands, accepted_on_bin, found, missed, first_red).""" + import cv2 + res = {} + for cname in (configs or list(CONFIGS)): + perc = make_perception(CONFIGS[cname], overlay=overlay) + res[cname] = {} + for lab, path in load_frames(names): + rgb = cv2.cvtColor(cv2.imread(path), cv2.COLOR_BGR2RGB) + cands = run_frame(perc, rgb) + for c in cands: + c["kind"], c["name"], c["bin_frac"], c["obj_frac"], c["obj_idx"] = classify(c, lab, rgb.shape[:2]) + acc = [c for c in cands if c["status"] == "accepted"] + on_bin = [c for c in acc if c["kind"] in ("bin", "mixed") and c["colour"] in ("red", "yellow", "blue")] + found, missed = [], [] + for j, o in enumerate(lab["objects"]): + hit = [c for c in acc if c["obj_idx"] == j and c["kind"] == "object" + and c["colour"] == POLICY_CLASS[o["colour"]]] + (found if hit else missed).append(f'{o["colour"]} {o["shape"]}') + ranked = rank_red(perc, cands) + res[cname][lab["frame"][:-4]] = dict(cands=cands, on_bin=on_bin, found=found, missed=missed, + first_red=ranked[0] if ranked else None, ranked=ranked, lab=lab) + if save_overlays: + _save_overlay(save_overlays, cname, lab, rgb, cands) + if verbose: + report(res) + return res + + +def _save_overlay(outdir, cname, lab, rgb, cands): + import cv2 + os.makedirs(outdir, exist_ok=True) + im = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR).copy() + for b in lab["bins"]: + x0, y0, x1, y1 = b["box"] + cv2.rectangle(im, (x0, y0), (x1, y1), (255, 0, 255), 1) + for o in lab["objects"]: + x0, y0, x1, y1 = o["box"] + cv2.rectangle(im, (x0, y0), (x1, y1), (255, 255, 255), 1) + for c in cands: + if c["uv"] is None: + continue + ok = c["status"] == "accepted" + col = (0, 200, 0) if ok and c["kind"] == "object" else (0, 0, 255) if ok else (128, 128, 128) + u, v = int(c["uv"][0]), int(c["uv"][1]) + cv2.circle(im, (u, v), 5, col, -1) + cv2.putText(im, f'{c["colour"][:3]}{c["area"]}', (u + 6, v), 0, 0.4, col, 1) + cv2.imwrite(os.path.join(outdir, f'{lab["frame"][:-4]}__{cname.replace("+", "_")}.png'), im) + + +def report(res): + for cname, frames in res.items(): + print(f"\n=== config: {cname} ({CONFIGS[cname]})") + print("frame arm blobs(r/y/b acc) on-bin acc | objects found / 5 | first red -> what it is") + tot_bin = tot_found = tot_n = 0 + for f, r in frames.items(): + acc = [c for c in r["cands"] if c["status"] == "accepted"] + cnt = {k: sum(1 for c in acc if c["colour"] == k) for k in ("red", "yellow", "blue")} + fr = r["first_red"] + br = [i + 1 for i, c in enumerate(r["ranked"]) if c["kind"] in ("bin", "mixed")] + br_s = f" [bin red ranks: {br} of {len(r['ranked'])}]" if br else "" + fr_s = "-" if fr is None else f'{fr["kind"]}{(":" + fr["name"]) if fr["name"] else ""} area={fr["area"]} uv=({fr["uv"][0]:.0f},{fr["uv"][1]:.0f})' + tot_bin += len(r["on_bin"]) + tot_found += len(r["found"]) + tot_n += len(r["lab"]["objects"]) + print(f'{f:7s} {"Y" if r["lab"]["arm_visible"] else "n"} {cnt["red"]}/{cnt["yellow"]}/{cnt["blue"]}' + f'{"":13s}{len(r["on_bin"]):>2} | {len(r["found"])}/5 miss={r["missed"] or "-"}' + f' | {fr_s}{br_s}') + for c in r["on_bin"]: + print(f' ON-BIN accepted {c["colour"]} area={c["area"]} uv=({c["uv"][0]:.0f},{c["uv"][1]:.0f}) ' + f'{c["name"]} bin_frac={c["bin_frac"]:.2f}') + why = {} + for c in r["cands"]: + if c["status"] != "accepted" and c["kind"] in ("bin", "mixed"): + why[c["status"]] = why.get(c["status"], 0) + 1 + if why: + print(f' bin-region blobs rejected: {why}') + print(f"TOTAL {cname}: accepted candidates on a bin = {tot_bin}; objects found = {tot_found}/{tot_n}") + + +def main(argv=None): + ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("--frames", nargs="*", help="frame names (default: all), e.g. r1_t12") + ap.add_argument("--configs", nargs="*", default=list(CONFIGS), choices=list(CONFIGS)) + ap.add_argument("--overlay", default="finals_r25_gen") + ap.add_argument("--save-overlays", help="directory for annotated PNGs") + a = ap.parse_args(argv) + evaluate(a.frames, a.configs, a.overlay, a.save_overlays) + + +if __name__ == "__main__": + main()