painting-vision-robotics-kit / coverage_loop.py
constructelligence's picture
Update code: real-machine G-code adapter, commissioning gate, machine_job runner, pump watchdog firmware, twin completion gate, realism and masking modules (card and weights unchanged)
4eecd03 verified
Raw History Blame Contribute Delete
24.7 kB
#!/usr/bin/env python3
"""Closed loop with a fail-closed default: plan, apply, re-observe, correct -- and know when to stop.
ROADMAP property 3 ("Closed loop with failsafes") is the one this kit has never had.
The retired ``hardware.closed_loop`` simulated this round trip but decided what got painted with a coin
flip (``miss_rate=0.1``, invented) and had no way to fail -- coverage only rose and the loop always ended
"done". A loop that cannot fail is a demo, and this project does not ship demos.
What makes this one worth trusting:
* **The observation is the estimator's, carrying its measured error.** Each round the machine
re-scans and believes what the measurement tells it. Two error directions, because they fail
differently (``docs/LEVEL.md``: painted-coverage error 0.075 val / 0.157 wild, false-complete
0.0 only at the fitted operating point):
- an **under-claim** hides paint that is there, so a finished area is painted again --
wasteful, safe;
- an **over-claim** sees paint that is not there, so the loop stops on an *unfinished* wall.
That is the dangerous one, and it is why completion has to be earned twice.
Errors are spatially correlated (drawn on a coarse grid and upscaled): a real estimator is
wrong about *regions*, not scattered pixels, and scattered noise makes a loop look safer than
it is.
* **Every job is gated.** Each round's commands are replayed through ``SafetySupervisor``, armed
only by a ``safety_gate`` decision (``hardware.supervise``). A refusal stops the loop where it
stands: it is never ignored or re-tried elsewhere.
* **Completion is de-bounced, and every stop is named.** ``covered`` needs ``confirm_rounds``
consecutive observations at or above target, so one lucky frame cannot finish a wall. The other
outcomes are ``stalled`` (a round added less than ``min_gain``), ``budget_exhausted``,
``time_exhausted``, ``rounds_exhausted``, ``nothing_left_to_plan``, ``refused`` and
``safety_stop``. **Only ``covered`` means finished**, and a stall is a failure, not a success
waiting to happen.
* **A refusal and a fault are different events.** ``refused`` is the safety gate declining this room
-- a judgement the order may move on from. ``safety_stop`` is the supervisor *latching*, because a
command fell outside the envelope or the controller stopped proving it was alive -- a fault, and
the layer above is told to stop the whole order. Pass ``supervisor=`` so one supervisor, and so
one latch, covers the job rather than a single round.
* **The record audits the verdict.** The loop decides from what it can see; the record also
reports the *true* coverage, so ``false_complete`` states plainly when the estimator let it stop
early. That number -- not the round count -- is what the roadmap is missing.
Run it with the default (measured, 5%) under-claim and a 0.95 target and it ends ``stalled``, not
``covered``: the estimator's bias holds the believed coverage below the target forever. That is the
finding rather than a bug -- closed-loop completion is blocked on perception precision, not control
(``docs/HARDWARE.md``). Pass ``--under-claim 0`` to see the loop finish a wall it can actually see.
python3 coverage_loop.py --over-claim 0.05 --out loop.json
"""
from __future__ import annotations
import argparse
import json
from dataclasses import dataclass
import numpy as np
from scipy import ndimage
import hardware as hw
from robot_planner import build_plan
# Wet film is laid at the dry-film thickness divided by the paint's volume solids, so the volume
# that leaves the tool is area x DFT / VS. Same physics as application.py's settings, stated here
# as the one number the loop's budget check consumes.
DEFAULT_DFT_UM = 100.0
DEFAULT_VOLUME_SOLIDS = 0.38
@dataclass(frozen=True)
class Budget:
"""Stopping rules. Every one of them is a way to stop *without* claiming success."""
target: float = 0.995 # share of the paintable wall that counts as finished
confirm_rounds: int = 2 # consecutive observations needed before saying "covered"
min_gain: float = 0.002 # a round adding less than this is a stall, not progress
max_rounds: int = 6
paint_l: float = 20.0 # the mast platform's tank
minutes: float = 120.0
miss_rate: float = 0.10 # share of passes the tool fails to deliver (execution)
under_claim: float = 0.05 # share of painted area the estimator fails to see
over_claim: float = 0.0 # share of unfinished area the estimator wrongly calls painted
reads: int = 1 # independent re-scans per round, combined by majority vote
scan_s: float = 5.0 # seconds one re-scan costs (charged to the time budget)
systematic: bool = False # True: every read misreads the *same* regions (bias, not noise)
max_ambiguous: float = 0.05 # largest *coherent* disputed region the machine will act over.
# Calibrated above the measured noise floor: scattered re-scan
# disagreement clusters reach 2.6%-3.6% of the wall on the fixtures,
# so a region has to be clearly bigger than that to mean anything.
ambiguous_patch: float = 0.0 # a contiguous share of wall half the reads see differently
# (a shadow, a reflection, a wet patch): the case the gate is for
dft_um: float = DEFAULT_DFT_UM
volume_solids: float = DEFAULT_VOLUME_SOLIDS
def _correlated(rng, shape, rate, block=8):
"""A boolean field True on `rate` of the grid in patches, not scattered pixels."""
if rate <= 0:
return np.zeros(shape, bool)
coarse = rng.random((max(1, shape[0] // block + 1), max(1, shape[1] // block + 1))) < rate
rows = np.repeat(coarse, block, axis=0)[:shape[0]]
return np.repeat(rows, block, axis=1)[:, :shape[1]]
def _read(painted, target, budget, rng, cache):
"""One re-scan: the truth, misread once. `cache` keeps a systematic error fixed across reads."""
if budget.systematic and cache:
missed, invented = cache
else:
missed = painted & _correlated(rng, painted.shape, budget.under_claim)
invented = (target & ~painted) & _correlated(rng, painted.shape, budget.over_claim)
if budget.systematic:
cache.extend((missed, invented))
return (painted & ~missed) | invented
def _patch(shape, share):
"""A centred rectangle covering `share` of the frame: one region the reads can disagree about."""
if share <= 0:
return None
h = max(1, int(round(shape[0] * share ** 0.5)))
w = max(1, int(round(shape[1] * share ** 0.5)))
mask = np.zeros(shape, bool)
top, left = (shape[0] - h) // 2, (shape[1] - w) // 2
mask[top:top + h, left:left + w] = True
return mask
def estimate(painted, target, budget, rng, cache=None):
"""What the re-scan reports, plus how much of the wall the reads disagreed about.
Returns ``(observed, ambiguity)`` where ``ambiguity`` is a dict with two shares of the wall:
``disputed`` (every pixel the reads disagreed on) and ``largest_region`` (the biggest *connected*
patch of them). The distinction is the whole point. Scattered disagreement is what a noisy
estimator does all the time -- with a 5% per-read error and three reads, ~14% of painted pixels
are disputed by chance, so gating on the total would stop every run. A disagreement that is
*one coherent region* is different evidence: the machine cannot decide what a whole area of wall
is, and that is a reason to stop.
Redundancy is the classical answer to an unreliable sensor, with a hard limit this makes
measurable: a vote cancels error *independent between reads* and does nothing to error that is
*systematic* (`--systematic` reuses the same misread regions, so every read lies identically and
the vote reproduces the lie -- and unanimously, which no disagreement check can see).
"""
cache = [] if cache is None else cache
empty = {"disputed": 0.0, "largest_region": 0.0}
if budget.reads <= 1:
return _read(painted, target, budget, rng, cache), empty
votes = np.zeros(painted.shape, np.int16)
patch = _patch(painted.shape, budget.ambiguous_patch)
for index in range(budget.reads):
read = _read(painted, target, budget, rng, cache)
if patch is not None and index % 2 == 0: # half the reads see that region differently
read = read ^ (patch & target)
votes += read
disputed = (votes > 0) & (votes < budget.reads) & target
total = int(target.sum())
if not disputed.any():
return votes * 2 > budget.reads, empty
labels, count = ndimage.label(disputed, structure=np.ones((3, 3), int))
largest = int(np.bincount(labels.ravel())[1:].max()) if count else 0
return votes * 2 > budget.reads, {"disputed": float(disputed.sum() / max(1, total)),
"largest_region": float(largest / max(1, total))}
def litres(delivered, mm_per_unit, budget):
"""Litres of paint that left the tool: area x DFT / volume solids (m^3 -> L)."""
area_m2 = float(delivered.sum()) * (mm_per_unit ** 2) / 1e6
return area_m2 * budget.dft_um * 1e-6 / budget.volume_solids * 1000.0
def run(paintable, keepout, mm_per_unit, decision, budget=Budget(), operation="paint_roller",
platform="mast_arm", wall_id="wall", seed=0, supervisor=None, heartbeat_every=1,
stall_after=None):
"""Plan -> apply -> re-observe -> re-plan what is left, until finished or stopped.
``decision`` is a ``safety_gate.assess`` result; a job is released only if it approves. The
re-plan uses what the machine *believes* it has painted, never the truth, because that is all a
robot has. The truth is recorded alongside it so the verdict can be audited.
"""
rng = np.random.default_rng(seed)
if budget.reads < 1 or budget.reads % 2 == 0:
raise ValueError(
f"reads must be odd (got {budget.reads}): an even vote can tie, and breaking a tie either "
"under-claims (AND) or over-claims (OR). Measured on the 2.7 m fixture, two reads double the "
"believed-true gap (0.051 -> 0.099) because a strict majority of two is an AND. Use 1, 3 or 5.")
target = np.asarray(paintable, bool) & ~np.asarray(keepout, bool)
height_mm, width_mm = paintable.shape[0] * mm_per_unit, paintable.shape[1] * mm_per_unit
result = {"outcome": "rounds_exhausted", "reason": None, "rounds": [], "target": budget.target,
"reads": budget.reads, "systematic_bias": budget.systematic,
"max_ambiguous": budget.max_ambiguous,
"paintable_pixels": int(target.sum()), "paint_l": 0.0, "minutes": 0.0,
"coverage_observed": 0.0, "coverage_true": 0.0, "ambiguous_fraction": 0.0,
"false_complete": False, "latched": False, "latched_reason": None}
if not target.any():
result["outcome"] = "nothing_to_paint"
return result
tool, brush = hw.TOOLS[hw.OPERATIONS[operation]], hw.TOOLS["brush"]
painted = np.zeros_like(target) # what the machine has put on the wall
observed = np.zeros_like(target) # what the machine believes is on the wall
confirmed, previous = 0, 0.0
ambiguity = {"disputed": 0.0, "largest_region": 0.0}
scan_cache = [] # keeps a systematic misread fixed across reads and rounds
for number in range(1, budget.max_rounds + 1):
plan, _ = build_plan(paintable, keepout, stroke_width=max(2, int(tool.width_mm / mm_per_unit)),
overlap=tool.overlap, margin=0, orientation="horizontal", corners=None,
mm_per_unit=mm_per_unit, boundary_tolerance=2.0, already_painted=observed,
trim_width=max(2, int(brush.width_mm / mm_per_unit)), trim_overlap=brush.overlap)
strokes, _ = hw.strokes_from_plan(plan)
trim, _ = hw.strokes_from_plan(plan, trim=True)
if not strokes and not trim:
# Nothing left to plan is the planner's own statement that the coverage grid is full; it
# counts as a second signal only alongside a re-scan that already read as finished.
if confirmed and previous >= budget.target:
result["outcome"] = "covered"
result["reason"] = "no strokes left to plan and the re-scan already read as finished"
else:
result["outcome"] = "nothing_left_to_plan"
break
delivered = np.zeros_like(target)
added_min, undelivered, refused = 0.0, 0, None
# Whether this step buys paint at all. The second pass is a *cut-in* -- the same coating worked
# into the edges with a brush -- so the step's own operation decides, not the pass: a sand step's
# cut-in is more sanding at the edge, not paint.
applies_material = hw.TOOLS[hw.OPERATIONS[operation]].material
for stroke_set, stroke_op in ((strokes, operation), (trim, "cut_in")):
if not stroke_set:
continue
job = hw.compile_job(stroke_set, height_mm, stroke_op, platform, wall_id=wall_id)
verdict = hw.supervise(job, width_mm, height_mm, decision, supervisor=supervisor,
heartbeat_every=heartbeat_every, stall_after=stall_after)
if not verdict["released"]:
refused = verdict
break
# What the tool actually delivers: some passes are never delivered at all.
missed = set(np.flatnonzero(rng.random(len(job["commands"])) < budget.miss_rate))
undelivered += len(missed)
pass_delivery = hw.rasterize(job, target.shape, mm_per_unit, height_mm,
miss=lambda i, missed=missed: i in missed) & target
delivered |= pass_delivery
# Only a tool that puts coating on the wall spends paint. Sanding, scoring, steaming, scraping
# and smoothing cost time, not litres -- charging them stops a job for paint it never used and
# reports litres that were never bought.
if applies_material:
result["paint_l"] += litres(pass_delivery, mm_per_unit, budget)
added_min += job["totals"]["duration_min"]
if refused is not None:
# A judgement and a fault both stop this loop, and they are not the same event. The gate
# refusing a room is a decision the order may move on from; a command outside the envelope
# -- or a controller that stopped proving it was alive -- latches the supervisor, and a
# latched supervisor is not something a job continues past.
tripped = bool(refused.get("cause") == "trip" or refused.get("latched"))
result["outcome"] = "safety_stop" if tripped else "refused"
result["reason"] = refused.get("stopped")
break
painted |= delivered
result["minutes"] += added_min
# Re-observing costs real time: every read is another look at the wall.
result["minutes"] += budget.reads * budget.scan_s / 60.0
observed, ambiguity = estimate(painted, target, budget, rng, scan_cache)
coverage = float((observed & target).sum() / target.sum())
true_coverage = float((painted & target).sum() / target.sum())
gain, previous = coverage - previous, coverage
result["rounds"].append({"round": number, "delivered_pixels": int((delivered & target).sum()),
"coverage_observed": round(coverage, 4),
"coverage_true": round(true_coverage, 4),
"reads": budget.reads,
"ambiguous_region": round(ambiguity["largest_region"], 4),
"disputed_fraction": round(ambiguity["disputed"], 4),
"gap_observed_minus_true": round(coverage - true_coverage, 4),
"paint_l": round(result["paint_l"], 3),
"minutes": round(result["minutes"], 1),
"gain": round(gain, 5), "passes_undelivered": undelivered})
# Fail-closed on an ambiguous observation, checked *before* anything is certified: a whole
# region the machine's own reads cannot agree about is a reason to stop, not to guess.
if ambiguity["largest_region"] > budget.max_ambiguous:
result["outcome"] = "ambiguous"
result["reason"] = (f"the reads disagree about {ambiguity['largest_region']:.1%} of the wall in a "
f"single region, over the {budget.max_ambiguous:.1%} the machine will act on")
break
confirmed = confirmed + 1 if coverage >= budget.target else 0
if confirmed >= budget.confirm_rounds:
result["outcome"] = "covered"
break
if gain < budget.min_gain:
result["outcome"], result["reason"] = "stalled", f"round {number} added {gain:.4f} coverage"
break
if result["paint_l"] > budget.paint_l:
result["outcome"], result["reason"] = "budget_exhausted", f"{result['paint_l']:.1f} L used"
break
if result["minutes"] > budget.minutes:
result["outcome"], result["reason"] = "time_exhausted", f"{result['minutes']:.0f} min used"
break
result["coverage_observed"] = round(float((observed & target).sum() / target.sum()), 4)
result["coverage_true"] = round(float((painted & target).sum() / target.sum()), 4)
result["ambiguous_region_share"] = round(ambiguity["largest_region"], 4)
result["disputed_fraction"] = round(ambiguity["disputed"], 4)
result["paint_l"] = round(result["paint_l"], 3)
result["minutes"] = round(result["minutes"], 1)
# The loop can only decide from what it saw; this audits what that decision was worth.
result["false_complete"] = bool(result["outcome"] == "covered"
and result["coverage_true"] < budget.target)
# Reported even when the loop was not what tripped it: the layer above has to stop the whole order,
# so it needs to see a latch it may have inherited or caused.
result["latched"] = bool(supervisor.latched) if supervisor is not None else False
result["latched_reason"] = supervisor.latched_reason if supervisor is not None else None
return result
def wall(h=270, w=400, window=True):
"""A wall-plane fixture: paintable area with a window keep-out (2.7 m of wall at 10 mm/unit)."""
paintable = np.zeros((h, w), bool)
paintable[5:h - 5, 5:w - 5] = True
keepout = np.zeros_like(paintable)
if window:
keepout[60:180, 150:260] = True
return paintable, keepout
def main():
ap = argparse.ArgumentParser(description=__doc__.splitlines()[0],
formatter_class=argparse.RawDescriptionHelpFormatter)
ap.add_argument("--mm-per-unit", type=float, default=10.0)
ap.add_argument("--operation", default="paint_roller", choices=sorted(hw.OPERATIONS))
ap.add_argument("--platform", default="mast_arm", choices=sorted(hw.PLATFORMS))
ap.add_argument("--over-claim", type=float, default=0.0,
help="share of unfinished wall the re-scan wrongly reports as painted")
ap.add_argument("--under-claim", type=float, default=0.05)
ap.add_argument("--miss-rate", type=float, default=0.10)
ap.add_argument("--target", type=float, default=0.995)
ap.add_argument("--reads", type=int, default=1,
help="independent re-scans per round, combined by majority vote (odd: 1, 3, 5)")
ap.add_argument("--scan-s", type=float, default=5.0, help="seconds one re-scan costs")
ap.add_argument("--systematic", action="store_true",
help="every read misreads the SAME regions: bias that a vote cannot remove")
ap.add_argument("--max-ambiguous", type=float, default=0.05,
help="largest coherent disputed region the machine will act over (1.0 disables)")
ap.add_argument("--ambiguous-patch", type=float, default=0.0,
help="simulate a contiguous share of wall that half the reads see differently")
ap.add_argument("--max-rounds", type=int, default=6)
ap.add_argument("--confirm-rounds", type=int, default=2)
ap.add_argument("--reject", action="store_true", help="arm with a rejecting safety gate (fail-closed path)")
ap.add_argument("--sweep", action="store_true",
help="run over a range of re-scan error rates and report the false-complete rate")
ap.add_argument("--sweep-reads", action="store_true",
help="run over a range of re-scan counts and report the believed-true gap")
ap.add_argument("--seed", type=int, default=0)
ap.add_argument("--out", type=argparse.FileType("w"), default=None)
args = ap.parse_args()
paintable, keepout = wall()
decision = ({"decision": "reject", "reasons": ["operator has not approved this wall"]}
if args.reject else {"decision": "approve"})
budget = Budget(target=args.target, max_rounds=args.max_rounds, confirm_rounds=args.confirm_rounds,
miss_rate=args.miss_rate, under_claim=args.under_claim, over_claim=args.over_claim,
reads=args.reads, scan_s=args.scan_s, systematic=args.systematic,
max_ambiguous=args.max_ambiguous, ambiguous_patch=args.ambiguous_patch)
if args.sweep_reads:
print(f"{'reads':>6}{'runs':>6}{'covered':>9}{'false complete':>16}{'|believed-true|':>17}"
f"{'minutes':>9}{'paint L':>9}")
for reads in (1, 3, 5, 7):
runs = [run(paintable, keepout, args.mm_per_unit, decision,
Budget(**{**budget.__dict__, "reads": reads}), operation=args.operation,
platform=args.platform, seed=seed)
for seed in range(8)]
covered = sum(r["outcome"] == "covered" for r in runs)
false_done = sum(r["false_complete"] for r in runs)
gap = sum(abs(r["coverage_observed"] - r["coverage_true"]) for r in runs) / len(runs)
print(f"{reads:>6}{len(runs):>6}{covered:>9}{false_done:>16}{gap:>17.4f}"
f"{sum(r['minutes'] for r in runs) / len(runs):>9.1f}"
f"{sum(r['paint_l'] for r in runs) / len(runs):>9.3f}")
return
if args.sweep:
print(f"{'re-scan error':>14}{'runs':>6}{'covered':>9}{'false complete':>16}"
f"{'true coverage':>15}{'paint L':>9}")
for error in (0.0, 0.02, 0.05, 0.10, 0.20):
runs = [run(paintable, keepout, args.mm_per_unit, decision,
Budget(**{**budget.__dict__, "over_claim": error}), operation=args.operation,
platform=args.platform, seed=seed)
for seed in range(16)]
covered = sum(r["outcome"] == "covered" for r in runs)
false_done = sum(r["false_complete"] for r in runs)
print(f"{error:>14.2f}{len(runs):>6}{covered:>9}{false_done:>16}"
f"{sum(r['coverage_true'] for r in runs) / len(runs):>15.4f}"
f"{sum(r['paint_l'] for r in runs) / len(runs):>9.3f}")
return
result = run(paintable, keepout, args.mm_per_unit, decision, budget,
operation=args.operation, platform=args.platform, seed=args.seed)
print(f"{'round':>6}{'observed':>10}{'true':>8}{'gain':>9}{'paint L':>9}{'min':>7}{'undelivered':>13}")
for row in result["rounds"]:
print(f"{row['round']:>6}{row['coverage_observed']:>10.4f}{row['coverage_true']:>8.4f}"
f"{row['gain']:>9.4f}{row['paint_l']:>9.3f}{row['minutes']:>7.1f}{row['passes_undelivered']:>13}")
print(f"\noutcome {result['outcome']!r}" + (f" ({result['reason']})" if result["reason"] else ""))
print(f"coverage: believed {result['coverage_observed']:.4f}, actually {result['coverage_true']:.4f}; "
f"paint {result['paint_l']:.2f} L, {result['minutes']:.1f} min over {len(result['rounds'])} round(s)")
if result["false_complete"]:
print("FALSE COMPLETE: the loop stopped believing it was finished on a wall it had not finished")
if args.out:
json.dump(result, args.out, indent=2)
if __name__ == "__main__":
main()