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
16.6 kB
"""The nozzle's path through time: what the drone flies, not only where it was planned to go.
``drone_areas.simulate_deposit`` lays paint down from the *geometry* of the passes: every stroke becomes one Gaussian
band, whatever its length, speed or standoff. That is enough to compare plans, and it cannot see the defects a real pass
has, because those are all in the timing:
* a controller that slows down inside a pass leaves a heavy band where it lingered;
* a gun that opens late leaves the first few centimetres thin (``application.trigger_offsets``);
* a nozzle that drifts closer to the wall widens the fan and thins the film;
* a flight log records all three, and none of them survives being reduced to a polyline.
This module turns either a planned sortie or a flight log into a ``Timeline``: samples of time, wall position, standoff,
speed and valve flow. ``drone_areas.deposit_from_timeline`` integrates it, so the film (and the twin's belief) carries
the timing and not only the drawing.
Assumptions, stated: the valve opens and closes linearly over the tip's latency; the drone holds its planned speed
inside a pass unless ``decel_mm`` says the controller slows there; and a planned pass is flown along the planner's
straight line. A log has none of those assumptions: its samples are what the aircraft reported.
Wall frame: x mm along the facade from its A end, z mm up from the ground, standoff the nozzle's distance from the
wall. A planned pass is sampled at 10 mm along its length; a log keeps its own samples.
"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
import numpy as np
import application
DS_MM = 10.0 # arc-length sampling of a planned pass
@dataclass
class Timeline:
"""Samples of a sortie, all arrays the same length and in time order."""
t: np.ndarray # s since the sortie started
x_mm: np.ndarray # along the facade from A
z_mm: np.ndarray # up from the ground
standoff_mm: np.ndarray # nozzle distance from the wall
speed_mm_s: np.ndarray # along-track speed
flow: np.ndarray # 0-1 valve flow (pump PWM normalised); 0 = not spraying
pass_id: np.ndarray # int, -1 in transit or line-up
phase: np.ndarray # "lineup" | "spray" | "transit"
sortie: int = 1
source: str = "planned"
spray: dict = field(default_factory=dict)
mm_per_px: float = 1.0
shape: tuple = (0, 0)
ramps: bool = True # planned: the valve ramps between samples; log: its state holds until the next sample
reaction_n: float = 0.0 # the spray's push back on the nozzle, N (application.spray_reaction)
sag_mm: float = 0.0 # the steady standoff the reaction costs at the modelled hold stiffness
standoff_bias_mm: float = 0.0 # what the controller was asked to bias its setpoint by to cancel that sag
def __len__(self):
return int(len(self.t))
def flow_mid(self):
"""Flow over each interval: the ramp's midpoint for a plan, the held state for a log (which samples a switch)."""
if len(self) < 2:
return np.zeros(0)
if self.ramps:
return 0.5 * (self.flow[:-1] + self.flow[1:])
return self.flow[:-1].astype(float)
def intervals(self):
"""dt plus the mid-interval speed and flow: a gap *between* passes is not charged to the sample after it."""
if len(self) < 2:
return np.zeros(0), np.zeros(0), np.zeros(0)
return np.diff(self.t), 0.5 * (self.speed_mm_s[:-1] + self.speed_mm_s[1:]), self.flow_mid()
def stats(self):
"""What the sortie did: time, spray time, paint pumped, and the path flown (all of it, and while spraying)."""
if not len(self):
return {"duration_s": 0.0, "spray_s": 0.0, "pump_on_s": 0.0, "paint_l": 0.0, "path_m": 0.0,
"spray_path_m": 0.0, "reaction_n": 0.0, "standoff_sag_mm": 0.0, "standoff_bias_mm": 0.0,
"passes": 0}
dt, _v, f = self.intervals()
seg = np.hypot(np.diff(self.x_mm), np.diff(self.z_mm)) / 1000.0
flow_lps = float(self.spray.get("flow_l_min", 0.0)) / 60.0
return {"duration_s": round(float(self.t[-1] - self.t[0]), 2),
"spray_s": round(float(np.sum(f * dt)), 2),
"pump_on_s": round(float(dt[f > 0].sum()), 2),
"paint_l": round(float(np.sum(f * dt) * flow_lps), 3),
"path_m": round(float(seg.sum()), 2),
"spray_path_m": round(float(seg[f > 0].sum()), 2),
"reaction_n": round(float(self.reaction_n), 2), "standoff_sag_mm": round(float(self.sag_mm), 1),
"standoff_bias_mm": round(float(self.standoff_bias_mm), 1),
"passes": int(len({int(p) for p in self.pass_id if p >= 0}))}
def strokes(self):
"""The spraying runs as polylines in the twin's pixel frame (col, row), for the parts that need strokes."""
out, cur = [], []
h = self.shape[0] if self.shape else 0
for i in range(len(self)):
if self.flow[i] > 0:
cur.append([self.x_mm[i] / self.mm_per_px, h - self.z_mm[i] / self.mm_per_px])
elif cur:
if len(cur) >= 2:
out.append(cur)
cur = []
if len(cur) >= 2:
out.append(cur)
return out
def _polyline_mm(stroke, mm_per_px, height_px):
pts = np.asarray(stroke, float)
if pts.ndim != 2 or pts.shape[0] < 2:
raise ValueError("a pass needs at least two points")
return np.column_stack([pts[:, 0] * mm_per_px, (height_px - pts[:, 1]) * mm_per_px])
def _at(pts, cum, s):
"""Position (N, 2) at arc length ``s``, extrapolating along the end segments outside [0, L]."""
s = np.asarray(s, float)
L = float(cum[-1])
out = np.empty((len(s), 2), float)
inside = (s >= 0) & (s <= L)
if inside.any():
out[inside, 0] = np.interp(s[inside], cum, pts[:, 0])
out[inside, 1] = np.interp(s[inside], cum, pts[:, 1])
if (s < 0).any():
d = pts[1] - pts[0]
n = float(np.hypot(*d))
d = d / n if n > 1e-9 else np.array([1.0, 0.0])
out[s < 0] = pts[0] + s[s < 0][:, None] * d # behind the start, along the first segment
if (s > L).any():
d = pts[-1] - pts[-2]
n = float(np.hypot(*d))
d = d / n if n > 1e-9 else np.array([1.0, 0.0])
out[s > L] = pts[-1] + (s[s > L] - L)[:, None] * d # past the end, along the last segment
return out
def from_strokes(strokes_px, mm_per_px, shape, spray, standoff_mm=None, *, speed_ms=None, line_up_s=4.0,
transit_speed_ms=2.0, sortie=1, decel_mm=0.0, end_speed_mm_s=50.0, compensated=True,
lateral_sigma_mm=0.0, seed=0, ds_mm=DS_MM, pressure_psi=2000.0, hold_stiffness_n_per_mm=0.5,
kick_mm=None, kick_tau_s=0.35, compensate_sag=True):
"""A planned sortie as a timeline: each pass at the planned speed and standoff, the gun led by the tip's latency.
``compensated``: the controller issues open ``open_lead`` before the pass start and close ``close_lead`` before its
end (what ``application.trigger_offsets`` exists to tell it), so the fan is at full flow from the first millimetre
of the pass. False models a controller that ignores the lead: the ramps land inside the pass instead.
``decel_mm``: >0 models a controller that slows to ``end_speed_mm_s`` over the last ``decel_mm`` of the pass while
still spraying. That is the pass-end heavy band the audit exists to catch; a controller that closes the gun before
slowing does not produce it (the default).
``hold_stiffness_n_per_mm`` / ``kick_mm``: the spray pushes back (``application.spray_reaction``), so the nozzle sits
further from the wall than the setpoint by force / stiffness, and the trigger adds a decaying kick on top. The
reaction is proportional to flow, so it follows the valve ramps. A stiffer hold (bigger number) sags less; ``kick_mm``
defaults to half the sag, and ``kick_tau_s`` is how long the machine takes to settle.
``compensate_sag``: the plan tells the controller to bias its standoff setpoint in by the sag (the same number
``drone_plan.build`` puts in ``standoff_setpoint_mm`` and the waypoints), so the nozzle holds the design standoff.
``False`` models a controller that ignores the bias: the steady standoff is then the design one *plus* the sag.
"""
h = int(shape[0])
v = float(speed_ms) * 1000.0 if speed_ms is not None else float(spray.get("traverse_speed_mm_s", 400.0))
if v <= 0:
raise ValueError("the pass speed must be positive")
standoff = float(standoff_mm if standoff_mm is not None else spray.get("standoff_mm", 300.0))
leads = application.trigger_offsets(v)
open_lead, close_lead = float(leads["open_lead_mm"]), float(leads["close_lead_mm"])
reaction = application.spray_reaction(float(spray.get("flow_l_min", 0.0)) / 60.0,
float(spray.get("pressure_psi") or pressure_psi))
sag = reaction["reaction_n"] / max(float(hold_stiffness_n_per_mm), 1e-3)
kick = (0.5 * sag) if kick_mm is None else float(kick_mm)
bias = sag if compensate_sag else 0.0 # the setpoint the controller is asked to hold
base = standoff - bias
rng = np.random.default_rng(seed)
step = max(2.0, min(float(ds_mm), 2.0 * float(mm_per_px)))
T, X, Z, S, V, F, P, PH = [], [], [], [], [], [], [], []
t_cursor, prev_end, pid = 0.0, None, 0
for stroke in strokes_px:
pts = _polyline_mm(stroke, mm_per_px, h)
seg = np.hypot(*np.diff(pts, axis=0).T)
cum = np.concatenate([[0.0], np.cumsum(seg)])
L = float(cum[-1])
if L < 1.0:
continue
pid += 1
s0 = -open_lead if compensated else 0.0
s1 = L + close_lead if compensated else L
n = max(8, int(math.ceil((s1 - s0) / step)) + 1)
s = np.linspace(s0, s1, n)
xy = _at(pts, cum, s)
x, z = xy[:, 0], xy[:, 1]
if lateral_sigma_mm > 0: # what-if: this pass flown off its line
err = rng.normal(0.0, float(lateral_sigma_mm))
if abs(x[-1] - x[0]) >= abs(z[-1] - z[0]):
z = z + err
else:
x = x + err
speed = np.full(n, v, float)
if decel_mm > 0: # the controller slows while the gun is still open
tail = s > L - float(decel_mm)
k = np.clip((L - s) / float(decel_mm), 0.0, 1.0)
speed = np.where(tail, float(end_speed_mm_s) + (v - float(end_speed_mm_s)) * k, v)
speed = np.maximum(speed, 1.0)
if compensated:
flow = np.clip((s + open_lead) / max(open_lead, 1e-6), 0, 1) * np.clip((L + close_lead - s) / max(close_lead, 1e-6), 0, 1)
else:
flow = np.clip(s / max(open_lead, 1e-6), 0, 1) * np.clip((L - s) / max(close_lead, 1e-6), 0, 1)
flow = np.clip(flow, 0.0, 1.0)
if prev_end is not None:
t_cursor += float(np.hypot(*(xy[0] - prev_end))) / (float(transit_speed_ms) * 1000.0)
T.append(t_cursor); X.append(xy[0, 0]); Z.append(xy[0, 1]); S.append(base)
V.append(0.0); F.append(0.0); P.append(-1); PH.append("lineup")
t_cursor += float(line_up_s)
ds = np.diff(s)
dt = ds / (0.5 * (speed[:-1] + speed[1:])) if n > 1 else np.zeros(0)
tp = t_cursor + np.concatenate([[0.0], np.cumsum(dt)])
off = sag * flow # the jet pushes the nozzle off its standoff...
if kick > 0: # ...and the trigger adds a decaying kick on top
off = off + kick * flow * np.exp(-np.maximum(tp - tp[0], 0.0) / max(float(kick_tau_s), 1e-3))
T.append(tp); X.append(x); Z.append(z); S.append(np.full(n, base) + off)
V.append(speed); F.append(flow); P.append(np.full(n, pid))
PH.append(np.where(flow > 0, "spray", "transit"))
t_cursor = float(tp[-1])
prev_end = xy[-1]
if not T: # a plan with nothing to fly is a valid (empty) sortie, not an error
empty = np.zeros(0, np.float64)
return Timeline(t=empty, x_mm=empty.astype(np.float32), z_mm=empty.astype(np.float32),
standoff_mm=empty.astype(np.float32), speed_mm_s=empty.astype(np.float32),
flow=empty.astype(np.float32), pass_id=np.zeros(0, np.int32), phase=np.zeros(0, "<U8"),
sortie=int(sortie), source="planned", spray=dict(spray), mm_per_px=float(mm_per_px),
shape=(h, int(shape[1])), reaction_n=reaction["reaction_n"], sag_mm=sag, standoff_bias_mm=bias)
cat = lambda k, dt_: np.concatenate([np.atleast_1d(x) for x in k]).astype(dt_)
return Timeline(t=cat(T, np.float64), x_mm=cat(X, np.float32), z_mm=cat(Z, np.float32),
standoff_mm=cat(S, np.float32), speed_mm_s=cat(V, np.float32), flow=cat(F, np.float32),
pass_id=cat(P, np.int32), phase=cat(PH, "<U8"), sortie=int(sortie), source="planned",
spray=dict(spray), mm_per_px=float(mm_per_px), shape=(h, int(shape[1])),
reaction_n=reaction["reaction_n"], sag_mm=sag, standoff_bias_mm=bias)
def from_log(log, a_en, b_en, mm_per_px, height_px, ground_alt_m=0.0, target_standoff_m=0.3, boom_m=0.5,
spray=None, *, sortie=1, width_px=None, speed_window=5, max_gap_s=1.5, pressure_psi=2000.0):
"""A flight log as a timeline: the samples the aircraft reported, with the pump state as the valve flow.
Speed is the distance between samples over their time step, smoothed over ``speed_window`` samples; a pump-on run
ends when the pump goes off or the samples skip more than ``max_gap_s``. That is the point of this constructor: a log
already contains the wobble, the standoff drift and the timing, so nothing here has to be assumed about them.
The spray's push on the nozzle is still a design quantity (flow and pressure, from ``spray``), so it is computed
here; the standoff *cost* is not modelled but measured - the median standoff while spraying against the design one.
"""
import drone_log as dlog
w = dlog.to_wall_frame(log, a_en, b_en)
t = np.asarray(w["t"], float)
if len(t) < 2:
raise ValueError("the log has fewer than 2 samples")
x = np.asarray(w["x"], float) * 1000.0
z = (np.asarray(w["z"], float) - float(ground_alt_m)) * 1000.0
sd = np.maximum(np.asarray(w["y"], float) * 1000.0 - float(boom_m) * 1000.0, 10.0)
pump = np.asarray(w["pump"], bool)
flow_lps = float((spray or {}).get("flow_l_min", 0.0)) / 60.0
reaction_n = float(application.spray_reaction(flow_lps, float((spray or {}).get("pressure_psi") or pressure_psi))["reaction_n"]) if flow_lps > 0 else 0.0
sag_mm = float(np.median(sd[pump]) - float(target_standoff_m) * 1000.0) if pump.any() and flow_lps > 0 else 0.0
dt = np.diff(t, prepend=t[0])
dist = np.hypot(np.diff(x, prepend=x[0]), np.diff(z, prepend=z[0]))
v = dist / np.maximum(dt, 1e-6)
v[0] = v[1] if len(v) > 1 else 0.0
if speed_window > 1 and len(v) >= speed_window:
n = int(speed_window)
half = n // 2
v = np.convolve(np.pad(v, (half, n - 1 - half), mode="edge"), np.ones(n) / n, mode="valid")
pid = np.full(len(t), -1, int)
current, prev, last_on = 0, False, None
for i in range(len(t)):
on = bool(pump[i])
if on and (not prev or (last_on is not None and t[i] - last_on > float(max_gap_s))):
current += 1
if on:
pid[i] = current
last_on = t[i]
prev = on
return Timeline(t=t, x_mm=x.astype(np.float32), z_mm=z.astype(np.float32), standoff_mm=sd.astype(np.float32),
speed_mm_s=np.maximum(v, 0.0).astype(np.float32), flow=pump.astype(np.float32),
pass_id=pid.astype(np.int32), phase=np.where(pump, "spray", "transit").astype("<U8"),
sortie=int(sortie), source="log", spray=dict(spray or {}), mm_per_px=float(mm_per_px),
shape=(int(height_px), int(width_px) if width_px else 0), ramps=False,
reaction_n=reaction_n, sag_mm=sag_mm)