From eae6b6098a88b77d7b554f2d160799fcef15a369 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Tue, 16 Jun 2026 17:36:46 +0200 Subject: [PATCH] Axis automatisch --- doc/Homing_5_Pose.md | 148 ++++------- doc/Kalibrierung.md | 32 +-- public/index.html | 18 -- scripts/5_pose_estimation.py | 243 +++++++----------- .../5_pose_estimation.cpython-311.pyc | Bin 45870 -> 42147 bytes scripts/robot_1781069752019.json | 3 +- 6 files changed, 158 insertions(+), 286 deletions(-) diff --git a/doc/Homing_5_Pose.md b/doc/Homing_5_Pose.md index 80c3665..fa41b21 100644 --- a/doc/Homing_5_Pose.md +++ b/doc/Homing_5_Pose.md @@ -10,10 +10,12 @@ > verifiziert** — `--from-state` (Startwert aus 4b, mit Multi-Start-Schutz für alles, > was der Startwert nicht abdeckt), `null` statt `0` für unbeobachtbare Gelenke > (z. B. `Hand`/`Palm`/Finger, aktuell ohne Marker in `robot_1781069752019.json`), -> `scipy` in `docker-compose.yaml` ergänzt, sowie neu der Kalibrier-Switch -> `--calibrate-origin` (siehe eigener Abschnitt unten). **Noch offen:** Anbindung in -> `homingOrchestrator.js`/`server.js`/Frontend (siehe Integrationsschritte — -> Umfang/Fehlerfall/Robotersteuerung-Politik dafür sind noch nicht festgelegt). +> `scipy` in `docker-compose.yaml` ergänzt, sowie der **eine Schalter** +> `pose_estimation.fit_origin_link` (siehe eigener Abschnitt unten), der einen +> Gelenk-Drehpunkt automatisch mitbestimmt und in `robot.json` übernimmt. **Noch +> offen:** Anbindung in `homingOrchestrator.js`/`server.js`/Frontend (siehe +> Integrationsschritte — Umfang/Fehlerfall/Robotersteuerung-Politik dafür sind +> noch nicht festgelegt). --- @@ -296,82 +298,44 @@ bzw. `state_Arm1.json` für die unvollständige erste Fixture): --- -## Kalibrier-Switch: Gelenk-Origin (`--calibrate-origin`) +## Kalibrier-Switch: Gelenk-Origin (`pose_estimation.fit_origin_link`) **Motivation:** `doc/Kalibrierung.md` Schritt [4] bestimmt `links.Arm1.jointToParent.origin[1,2]` (Y/Z des Schultergelenk-Drehpunkts) geometrisch -aus einer **dedizierten 3-Pose-Aufnahme** (Verfahren B: Kreis-Umkreismittelpunkt -durch 3 Positionen je Marker, nur Marker-**Mittelpunkte**, keine Normalen — Details -dort). Diese Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen". `5_pose_estimation.py` -hat mit `residual_vector()` (Position **und** Normale, robuste Verlustfunktion, -generischer Least-Squares-Löser) bereits die Bausteine, um densel­ben Drehpunkt -**aus den ohnehin vorhandenen Homing-Aufnahmen** zu verfeinern, statt eine eigene -Aufnahme-Session zu brauchen. +aus einer dedizierten 3-Pose-Aufnahme (Marker-Mittelpunkte, keine Normalen). Diese +Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen" — `5_pose_estimation.py` hat mit +Position+Normale und einem robusten Least-Squares-Löser bereits die Bausteine, um +denselben Drehpunkt aus den ohnehin vorhandenen Homing-Aufnahmen zu verfeinern. -### Ansatz +**Ein Schalter, eine Stelle:** `robot.json` → `pose_estimation.fit_origin_link` +(aktuell `"Arm1"`). Wenn gesetzt, gibt `estimate_global_ba()` für diesen Link +`jointToParent.origin[1,2]` (Y,Z) als **2 zusätzliche Variablen derselben +Optimierung** frei (kein separater Lauf, keine Restore-Logik, keine eigene +Funktion) — die Gelenkvariable und der Drehpunkt werden **gemeinsam** bestimmt. +Bei Erfolg übernimmt `main()` das Ergebnis automatisch: `patch_robot_json_origin()` +schreibt nur die eine `"origin": [...]`-Zeile des Links in `robot.json` zurück +(Text-Patch, nicht `json.dump()` — der Rest der handgepflegten, kompakten Datei +bleibt unverändert). `null`/Feld weglassen = aus, keine Verhaltensänderung. -Statt nur die Gelenkvariable `q` zu fitten, werden für **einen** angegebenen Link -zusätzlich 2 Komponenten seines `jointToParent.origin` freigegeben: +### Befund an echten Daten (drei reale Captures, sequenziell, 2026-06-16) -``` -Normalfall: min_q Σ_marker ρ(‖r(q)‖) (3 Freiheitsgrade weniger) ---calibrate-origin: min_{q_link, origin_y, origin_z} Σ_marker∈link ρ(‖r(q_link, origin_y, origin_z)‖) -``` +| Lauf | Fixture | Origin Y / Z danach | Δ zum Vorlauf | +|---|---|---|---| +| 1 | `20260616_133151` | 108,28 / 34,81 | +7,18 / −20,39 (ggü. ursprünglich 101,1 / 55,2) | +| 2 | `20260616_135403` | 108,84 / 34,89 | +0,57 / +0,08 | +| 3 | `20260616_120456` (unvollst. Seed) | 107,42 / 35,33 | −1,43 / +0,45 | -Implementiert in `estimate_origin_calibration()` (neu, -`scripts/5_pose_estimation.py`): mutiert `fk.links[]["jointToParent"]["origin"][1,2]` -**transient** während des Solves (jeder `fk.compute()`-Aufruf liest `origin` frisch -aus dem Dict, siehe `robot_fk.py:compute()` — kein Caching, daher funktioniert die -direkte Mutation ohne Änderung an `robot_fk.py`) und stellt den Originalwert danach -**immer** wieder her — das Skript bleibt ein reines Report-Tool, **`robot.json` wird -nie geschrieben**. Multi-Start `{0,60,…,300}°` für die eigene Gelenkvariable, wenn -revolut (wie bei den anderen Verfahren). Alle anderen Gelenke bleiben fix (aus -`--from-state`, sonst `0`) — Vorbedingung wie beim bestehenden Verfahren: die -übrige Kette (insbesondere `x`) muss schon vertrauenswürdig sein. +Drei unabhängige Aufnahmen (unterschiedliche Marker, unterschiedliche Posen) landen +im selben Bereich, und die Schritte werden **kleiner statt größer** — spricht für +Konvergenz, nicht für Rauschen/Drift. (Für die Doku danach wieder auf den +Ursprungswert `101.1, 55.2` zurückgesetzt — die Tabelle zeigt nur den Testlauf.) +**Konsequenz des „bei jedem Lauf automatisch":** der Wert wandert mit jeder neuen +Aufnahme leicht weiter, statt einmalig fixiert zu werden — gewünscht laut Nutzer, +aber gut zu wissen. Schalter auf `null` setzen, um das einzufrieren. -### Aufruf - -```bash -python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ - -robot scripts/robot_1781069752019.json \ - --from-state data/homing//state_Arm2.json \ - --calibrate-origin Arm1 -# -> schreibt Arm1_origin_calibration.json (Report), robot.json unverändert -``` - -Funktioniert generisch für jeden Link mit eigenen Markern (an `Ellbow` mit -`--calibrate-origin Ellbow` getestet) — keine Arm1-spezifische Hardcodierung. - -### Befund an echten Daten (2026-06-16, vorläufig) - -Auf zwei **unabhängigen** Fixtures mit **unterschiedlichen** sichtbaren -Arm1-Markern ergibt sich eine konsistente Korrektur: - -| Fixture | gesehene Marker | Δ Origin Y | Δ Origin Z | Residuum RMS | -|---|---|---|---|---| -| `20260616_133151` | 198, 229 | **+6,46 mm** | **−19,97 mm** | 1,19 mm | -| `20260616_135403` | 197, 243 | **+7,33 mm** | **−18,49 mm** | 1,19 mm | - -Beide Läufe sehen **andere** Markerpaare und kommen trotzdem auf nahezu -denselben Versatz (~+7 mm / ~−19 mm) — das ist kein Zufallsrauschen eines -einzelnen Markers, sondern ein konsistenter Hinweis, dass der aktuell in -`robot_1781069752019.json` hinterlegte Wert (`[110, 101.1, 55.2]`) tatsächlich -um ungefähr diesen Betrag daneben liegt. **Noch nicht** unabhängig gegen das -geometrische Verfahren B (3-Pose-Aufnahme) gegengeprüft — siehe Offene Punkte. - -### Einschränkungen / Unterschiede zum bestehenden Verfahren - -| | Verfahren B (`yAxisCompute.js`, bestehend) | `--calibrate-origin` (neu) | -|---|---|---| -| Aufnahmen nötig | 3 Posen, ≥15° Drehung dazwischen | **1** Pose (mehr optional, noch nicht implementiert) | -| Signal | Marker-Mittelpunkt über 3 Zeitpunkte | Position **+ Normale**, robuste Verlustfunktion | -| Fehlerabschätzung | Residuum εᵢ je Marker (Kreis-Abweichung) | `residual_rms` über alle Link-Marker | -| Achsrichtung | wird mitbestimmt (Kreuzprodukt/Ebenen-Normale) | wird **nicht** gefittet — nur `origin`, `axis` bleibt aus robot.json | -| Identifizierbarkeit | durch Drehung explizit entkoppelt von Winkel | aus 1 Pose: Winkel/Origin-Korrelation theoretisch möglich, durch mehrere Marker + Normalen an verschiedenen Hebelarmen empirisch entkoppelt (s. Befund oben) — **nicht formal bewiesen** | -| Schreibt robot.json | ja, über „Joint-Origin Y/Z übernehmen" | nein — nur Report, gleiche Übernahme-Aktion nutzbar | - -Die Achs**richtung** (`jointToParent.axis`) fitten beide Verfahren nicht — das -bleibt vorerst bei Verfahren B, falls sie ebenfalls ungenau ist. +Ergänzt, ersetzt nicht: `doc/Kalibrierung.md` Schritt [4] (3-Pose-Kreis-Fit, nur +Marker-Mittelpunkte) bleibt die unabhängige Gegenmessung. Die Achs**richtung** +(`jointToParent.axis`) fitten beide Verfahren nicht. --- @@ -425,10 +389,11 @@ Gegen die echten Testdaten in `test/homing/*/` ausprobiert — siehe `confidence:"none"` statt erfundener `0`. `main()`s Output-Writer mappt `observable:false → value:null` (intern bleibt `0.0` für die FK-Rechnung der anderen Gelenke — nur der Output-Vertrag ändert sich). -- [x] **Kalibrier-Switch `--calibrate-origin `** umgesetzt - (`estimate_origin_calibration()`) — generisch für jeden Link mit eigenen - Markern, getestet an `Arm1` und `Ellbow`. Schreibt nie `robot.json`, nur - einen `*_origin_calibration.json`-Report. Details: eigener Abschnitt oben. +- [x] **Kalibrier-Switch `pose_estimation.fit_origin_link`** umgesetzt — ein + Konfig-Feld, direkt in `estimate_global_ba()` integriert (keine separate + Funktion/Report/CLI-Flag mehr), übernimmt das Ergebnis automatisch in + `robot.json` (`patch_robot_json_origin()`, Text-Patch). Generisch für jeden + Link, aktuell für `Arm1` aktiviert. Details: eigener Abschnitt oben. **Noch offen:** @@ -439,25 +404,24 @@ Gegen die echten Testdaten in `test/homing/*/` ausprobiert — siehe noch nicht festgelegt** (offene Rückfrage vom 2026-06-16, noch unbeantwortet: Minimal-Fix vs. Voll-Integration; Abbruch vs. Fallback bei Fehler; Senden vs. nur Anzeigen). Diese drei Entscheidungen zuerst klären, dann verdrahten. -- [ ] **Arm1-Origin-Befund anwenden oder verwerfen:** Δ(Y,Z) ≈ (+7, −19) mm ist - auf zwei unabhängigen Fixtures konsistent (s. Abschnitt „Kalibrier-Switch"). - Vor dem Übernehmen: (a) mit mehr Fixtures/Posen erhärten, (b) wenn möglich - gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken, (c) erst dann via - Kalibrierung-Tab „Joint-Origin Y/Z übernehmen" übernehmen. -- [ ] **`--calibrate-origin` an die Kalibrierung-UI anbinden** (`doc/Kalibrierung.md` - Schritt [4]) — aktuell nur CLI/Report; Tab „Arm1 – Y" könnte beide Verfahren - (Geometrisch/Verfahren B und `--calibrate-origin`) nebeneinander anzeigen. -- [ ] **Mehrpose-Erweiterung für `--calibrate-origin`** (mehrere - `aruco_marker_poses.json` + gemeinsames `origin`, je Pose ein eigener - Gelenkwinkel) — würde die Winkel/Origin-Korrelationsschwäche aus einer - Einzelpose weiter reduzieren, analog zur bestehenden 3-Pose-Aufnahme. +- [ ] **Arm1-Origin-Wert beobachten:** wandert bei jedem Lauf mit `fit_origin_link` + leicht weiter (s. Befund-Tabelle oben, wird kleiner/konvergiert bisher). Falls + das nicht erwünscht ist: Schalter nach dem Einschwingen auf `null` setzen, oder + gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken. +- [ ] **`fit_origin_link` in der Kalibrierung-UI sichtbar machen** (`doc/Kalibrierung.md` + Schritt [4]) — aktuell nur in `robot.json` umschaltbar; Tab „Arm1 – Y" könnte + den aktuellen Wert/letzte Änderung neben Verfahren B anzeigen. +- [ ] **Mehrpose-Erweiterung** (mehrere `aruco_marker_poses.json` mit + gemeinsamem `origin`, je Pose ein eigener Gelenkwinkel, ein gemeinsamer Solve) + — würde die Winkel/Origin-Korrelation aus einer Einzelpose weiter reduzieren, + analog zur bestehenden 3-Pose-Aufnahme. - [ ] `huber_delta_mm`/`normal_weight` ggf. gegen reale Marker-Genauigkeit - nachjustieren — reales Residuum (4,3–4,5 mm RMS) liegt deutlich über der + nachjustieren — reales Residuum (2,4–4,5 mm RMS) liegt deutlich über der Simulation; Defaults sind unverändert aus appRobotRendering übernommen. - [ ] Python-Tests (`pytest`) für `load_seed_state()`, den Block-Skip in - `estimate_sequential_fk()` und `estimate_origin_calibration()` — aktuell nur - manuell gegen die drei Fixtures verifiziert (s. oben); appRobotHoming hat - bisher keine Python-Testinfrastruktur (nur Jest/JS), das wäre die erste. + `estimate_sequential_fk()` und die Origin-Erweiterung in `estimate_global_ba()` + — aktuell nur manuell gegen die drei Fixtures verifiziert (s. oben); + appRobotHoming hat bisher keine Python-Testinfrastruktur (nur Jest/JS). - [ ] Eintrag in `Homing.md`-Tabelle (Doku-Übersicht) ergänzen, sobald `homingOrchestrator.js` verdrahtet ist. diff --git a/doc/Kalibrierung.md b/doc/Kalibrierung.md index 77ecb5d..7a8819a 100644 --- a/doc/Kalibrierung.md +++ b/doc/Kalibrierung.md @@ -102,28 +102,22 @@ befestigt. Diese werden in `links.Base.markers` eingetragen. - `links.Arm1.jointToParent.origin[1]` (Y) und `[2]` (Z) in `robot.json` - Optional: `links.Base.markers` ergänzt -### Alternative/Ergänzung — `--calibrate-origin` (`5_pose_estimation.py`) +### Alternative/Ergänzung — `pose_estimation.fit_origin_link` (`5_pose_estimation.py`) -🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Mathematik und -Vergleichstabelle: [`doc/Homing_5_Pose.md`](Homing_5_Pose.md) (Abschnitt +🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Befund und +Vergleich zu Verfahren B: [`doc/Homing_5_Pose.md`](Homing_5_Pose.md) (Abschnitt „Kalibrier-Switch: Gelenk-Origin"). -Bestimmt `origin[1,2]` desselben Gelenks **aus einer einzelnen vorhandenen -Homing-Aufnahme** (Position + gemessene Normale aller Arm1-Marker, robuster -Least-Squares-Fit) statt aus der dedizierten 3-Pose-Aufnahme oben — keine -eigene Mess-Session nötig, dafür (noch) ohne die explizite Drehung, die hier -Achse und Winkel sauber entkoppelt. Auf zwei realen Captures ergab sich eine -konsistente Korrektur von ca. **+7 mm (Y) / −19 mm (Z)** gegenüber dem -aktuellen `robot.json`-Wert — bisher **nicht** gegen eine frische Messung mit -Verfahren B gegengeprüft, daher noch nicht über „Joint-Origin Y/Z übernehmen" -angewendet. Aufruf: - -```bash -python scripts/5_pose_estimation.py /aruco_marker_poses.json \ - -robot scripts/robot_1781069752019.json \ - --from-state /state_Arm2.json \ - --calibrate-origin Arm1 -``` +Ein Schalter in `robot.json` (`pose_estimation.fit_origin_link: "Arm1"`, aktuell +gesetzt): bestimmt `origin[1,2]` (Y,Z) **zusammen mit** der Gelenkvariable in +derselben Pose-Schätzung — aus einer einzelnen vorhandenen Homing-Aufnahme +(Position + gemessene Normale aller Arm1-Marker), keine eigene Mess-Session +nötig. Bei Erfolg **automatisch übernommen**: jeder Lauf schreibt den neuen +Wert direkt in `robot.json` zurück (wie der „Joint-Origin Y/Z übernehmen"-Button, +nur automatisch statt per Klick). Auf drei realen Captures ergab sich eine +konsistente, sich einschwingende Korrektur von ca. **+6 bis +7 mm (Y) / −19 bis +−20 mm (Z)** gegenüber dem ursprünglichen Wert — bisher **nicht** gegen eine +frische Messung mit Verfahren B gegengeprüft. --- diff --git a/public/index.html b/public/index.html index b0bc400..d135857 100755 --- a/public/index.html +++ b/public/index.html @@ -65,24 +65,6 @@ - -
-

Result – Raw JSON

-
- - -
-
- - -
-

Result – Tree View

-
- -
-
-
-

Board-Viewer

diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py index 0fff55d..4d181aa 100644 --- a/scripts/5_pose_estimation.py +++ b/scripts/5_pose_estimation.py @@ -33,13 +33,15 @@ Homing integration (appRobotHoming, see doc/Homing_5_Pose.md): variables default to 0 and are estimated/flagged normally. Without --from-state, behaviour is unchanged (internal cold start, as before). - --calibrate-origin special mode: instead of estimating the full - pose, fit 's own joint value TOGETHER WITH - its jointToParent.origin Y/Z from that link's own - markers (complements the geometric multi-pose - method in doc/Kalibrierung.md Schritt [4]). - Writes a *_origin_calibration.json report; never - modifies robot.json. + robot.json -> pose_estimation.fit_origin_link = "Arm1" one switch: the + named link's jointToParent.origin Y/Z is fit + TOGETHER WITH the normal pose in the same + global_ba solve (complements the geometric + multi-pose method in doc/Kalibrierung.md + Schritt [4]) and the result is adopted + automatically -- written back into robot.json + (surgical text patch, see patch_robot_json_origin()). + Off by default (key absent/null). Unobservable joints (confidence "none") are written as value=null in the output JSON — never a fabricated 0 (see movements..observable). @@ -52,6 +54,7 @@ import argparse import json import math import os +import re import sys import time from collections import defaultdict @@ -85,6 +88,12 @@ DEFAULT_CFG: Dict[str, Any] = { "min_cameras_per_marker": 2, "finger_block_joints": ["b", "c", "e"], "per_link_method": {}, + # One switch: if set to a link name (e.g. "Arm1"), that link's + # jointToParent.origin Y/Z is fit together with the normal pose (same + # global_ba solve) and the result is written back into robot.json + # automatically. None/absent = off, no behaviour change. See + # doc/Homing_5_Pose.md "Kalibrier-Switch". + "fit_origin_link": None, } @@ -277,20 +286,42 @@ def estimate_global_ba(fk: RobotFK, obs: Dict[int, Dict[str, Any]], var_names: L marker_ids = list(obs.keys()) base = {k: 0.0 for k in STATE_KEYS} base.update(x0) - vec0 = np.array([base.get(v, 0.0) for v in var_names], dtype=float) + + # One switch (pose_estimation.fit_origin_link): also free that link's + # jointToParent.origin Y/Z as 2 extra parameters of THIS SAME solve, no + # separate pass. fk.links[...] is mutated in place -- compute() re-reads + # it fresh every call (robot_fk.py), so this takes effect immediately and + # is left adopted on success (main() writes it back into robot.json). + origin_link = cfg.get("fit_origin_link") + origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin") if origin_link else None + origin = origin if isinstance(origin, list) and len(origin) == 3 else None + origin_before = list(origin) if origin else None + n_state = len(var_names) + + vec0 = np.array([base.get(v, 0.0) for v in var_names] + + (origin_before[1:3] if origin else []), dtype=float) def fun(vec): - st = _state_from_vec(var_names, vec, base) + st = _state_from_vec(var_names, vec[:n_state], base) + if origin: + origin[1], origin[2] = float(vec[n_state]), float(vec[n_state + 1]) return residual_vector(st, fk, obs, marker_ids, cfg) loss = cfg.get("robust_loss", "huber") f_scale = float(cfg.get("huber_delta_mm", 8.0)) try: sol = least_squares(fun, vec0, loss=loss, f_scale=f_scale, - max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(var_names))) - return _state_from_vec(var_names, sol.x, base) + max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(vec0))) + state = _state_from_vec(var_names, sol.x[:n_state], base) + if origin: + origin[1], origin[2] = float(sol.x[n_state]), float(sol.x[n_state + 1]) + print(f"[INFO] fit_origin_link={origin_link}: Y,Z {origin_before[1:3]} " + f"-> [{origin[1]:.3f}, {origin[2]:.3f}]") + return state except Exception as exc: print(f"[WARN] global_ba failed: {exc}") + if origin: + origin[1], origin[2] = origin_before[1], origin_before[2] # restore on failure return dict(base) @@ -510,127 +541,6 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict return info -# ================================================================== -# Mode: joint-origin calibration (--calibrate-origin) -# ================================================================== - -def estimate_origin_calibration(fk: RobotFK, obs: Dict[int, Dict[str, Any]], - link_name: str, cfg: Dict[str, Any], - seed: Optional[Dict[str, float]] = None, - free_axes: Tuple[int, ...] = (1, 2)) -> Dict[str, Any]: - """ - Fit `link_name`'s OWN joint variable together with its - `jointToParent.origin` components (default: indices 1,2 = Y,Z) from that - link's own markers, in a single robust least-squares solve. All other - joint variables are held fixed at `seed` (or 0) — this assumes the rest of - the chain (in particular a slider `x` seed, if relevant) is already - trustworthy, same precondition as the existing geometric method. - - Complements doc/Kalibrierung.md Schritt [4] ("Arm1 Y-Rotationsachse"), - which fits the axis from a dedicated 3-pose capture using marker *centres* - only (circle fit). This fits from a single capture's marker corner poses - (position + measured normal, same residual as estimate_pose), reusing - whatever Homing run data is already on hand instead of a separate capture - session — useful for ANY revolute/linear joint's origin, not just Arm1/y. - - Never writes robot.json. `fk.links[link_name]["jointToParent"]["origin"]` - is mutated transiently during the solve (RobotFK.compute() re-reads it - fresh on every call — see robot_fk.py) and always restored before - returning, success or not. - - Returns a report dict; result["status"] is one of: - "ok" | "scipy_missing" | "insufficient_markers" | "unknown_link" | "failed" - """ - if link_name not in fk.links: - return {"link": link_name, "status": "unknown_link"} - - chain = analyze_chain(fk) - link_var = next((v for v, links in chain["var_links"].items() if link_name in links), None) - if link_var is None: - return {"link": link_name, "status": "unknown_link", - "detail": "link has no movable jointToParent"} - - own_markers = [m for m in chain["link_markers"].get(link_name, []) if m in obs] - joint = fk.links[link_name].get("jointToParent", {}) or {} - origin = joint.get("origin", [0.0, 0.0, 0.0]) - if not isinstance(origin, list): - origin = list(origin) - joint["origin"] = origin - origin_before = list(origin) - var_type = chain["var_type"].get(link_var, "linear") - - result: Dict[str, Any] = { - "link": link_name, "joint_variable": link_var, - "joint_unit": "mm" if var_type == "linear" else "deg", - "origin_before_mm": origin_before, "free_axes": list(free_axes), - "n_markers": len(own_markers), "status": "skipped", - } - if not HAVE_SCIPY: - result["status"] = "scipy_missing" - return result - if len(own_markers) < 2: - result["status"] = "insufficient_markers" - return result - - base = {k: 0.0 for k in STATE_KEYS} - if seed: - base.update({k: v for k, v in seed.items() if k in STATE_KEYS}) - - def fun(vec): - st = dict(base) - st[link_var] = vec[0] - for i, ax in enumerate(free_axes): - origin[ax] = vec[1 + i] - return residual_vector(st, fk, obs, own_markers, cfg) - - starts = [base.get(link_var, 0.0)] if var_type != "revolute" else _multistart_values("revolute") - best, best_cost = None, float("inf") - try: - for a0 in starts: - vec0 = np.array([a0] + [origin_before[ax] for ax in free_axes], dtype=float) - try: - sol = least_squares(fun, vec0, loss=cfg.get("robust_loss", "huber"), - f_scale=float(cfg.get("huber_delta_mm", 8.0)), - max_nfev=int(cfg.get("max_iterations", 200)) * 3) - if sol.cost < best_cost: - best_cost, best = sol.cost, sol.x - except Exception: - continue - finally: - for ax in free_axes: - origin[ax] = origin_before[ax] # always restore — report-only tool - - if best is None: - result["status"] = "failed" - return result - - fitted_joint = float(best[0]) - if var_type == "revolute": - fitted_joint = (fitted_joint + 180.0) % 360.0 - 180.0 - fitted_origin = list(origin_before) - for i, ax in enumerate(free_axes): - fitted_origin[ax] = float(best[1 + i]) - - final_state = dict(base) - final_state[link_var] = fitted_joint - for i, ax in enumerate(free_axes): - origin[ax] = fitted_origin[ax] - final_res = residual_vector(final_state, fk, obs, own_markers, cfg) - for ax in free_axes: - origin[ax] = origin_before[ax] # restore again after the check above - - result.update({ - "status": "ok", - "joint_value": fitted_joint, - "origin_after_mm": fitted_origin, - "origin_delta_mm": [round(b - a, 4) for a, b in zip(origin_before, fitted_origin)], - "residual_rms": float(np.sqrt(np.mean(final_res ** 2))) if final_res.size else 0.0, - "note": "robot.json NOT modified — apply via Kalibrierung-Tab " - "\"Joint-Origin Y/Z übernehmen\" (editRobot.js) if this looks good.", - }) - return result - - def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any], seed: Optional[Dict[str, float]] = None) -> Dict[str, Any]: """ @@ -669,6 +579,36 @@ def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, An # CLI # ================================================================== +def patch_robot_json_origin(robot_path: str, link_name: str, yz: Tuple[float, float]) -> bool: + """ + Surgically rewrite links..jointToParent.origin[1],[2] (Y,Z) in + the robot.json TEXT in place. robot.json has a hand-curated, compact + format (markers etc. one per line) -- a full json.load()+json.dump() + round-trip would reformat the whole file, so this only touches the one + "origin": [...] array belonging to (X left untouched). + Returns True if a match was found and patched. + """ + with open(robot_path, "r", encoding="utf-8") as f: + text = f.read() + link_m = re.search(r'"%s"\s*:\s*\{' % re.escape(link_name), text) + if not link_m: + return False + origin_m = re.search(r'"origin"\s*:\s*\[([^\]]*)\]', text[link_m.end():]) + if not origin_m: + return False + parts = [p.strip() for p in origin_m.group(1).split(",")] + if len(parts) != 3: + return False + parts[1] = f"{float(yz[0]):.4f}".rstrip("0").rstrip(".") + parts[2] = f"{float(yz[1]):.4f}".rstrip("0").rstrip(".") + new_array = f"[{parts[0]}, {parts[1]}, {parts[2]}]" + start = link_m.end() + origin_m.start() + end = link_m.end() + origin_m.end() + with open(robot_path, "w", encoding="utf-8") as f: + f.write(text[:start] + '"origin": ' + new_array + text[end:]) + return True + + def main() -> None: ap = argparse.ArgumentParser(description="Estimate robot joint angles from marker poses") ap.add_argument("markers", help="aruco_marker_poses.json (corner_pose) or aruco_positions_*.json (center)") @@ -679,10 +619,6 @@ def main() -> None: help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as " "written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid " "instead of the internal cold start. See doc/Homing_5_Pose.md.") - ap.add_argument("--calibrate-origin", default=None, metavar="LINK", - help="Instead of estimating the full pose, fit LINK's own joint value " - "together with its jointToParent.origin Y/Z from LINK's own markers. " - "Writes a *_origin_calibration.json report; never modifies robot.json.") args = ap.parse_args() robot_data = json.load(open(args.robot, "r", encoding="utf-8")) @@ -696,27 +632,9 @@ def main() -> None: seed = load_seed_state(args.from_state) if args.from_state else None print(f"[INFO] method={cfg['method']} | observed markers={len(obs)} | use_normals={cfg.get('use_normals')}" + (f" | seed={seed}" if seed else "")) + if cfg.get("fit_origin_link"): + print(f"[INFO] fit_origin_link={cfg['fit_origin_link']} -> robot.json wird bei Erfolg aktualisiert") - # ── Mode: joint-origin calibration ────────────────────────────────────── - if args.calibrate_origin: - calib = estimate_origin_calibration(fk, obs, args.calibrate_origin, cfg, seed=seed) - print(f"\nOrigin calibration for link={calib['link']} status={calib['status']}") - if calib["status"] == "ok": - unit = calib["joint_unit"] - print(f" joint {calib['joint_variable']}: {calib['joint_value']:.2f} {unit}") - print(f" origin before: {calib['origin_before_mm']}") - print(f" origin after: {calib['origin_after_mm']} (delta {calib['origin_delta_mm']} mm)") - print(f" residual RMS over {calib['n_markers']} markers: {calib['residual_rms']:.3f}") - print(f" {calib['note']}") - else: - print(f" (no fit — {calib.get('detail', calib['status'])}, n_markers={calib.get('n_markers', 0)})") - out_path = args.out or os.path.join( - os.path.dirname(args.markers), f"{args.calibrate_origin}_origin_calibration.json") - json.dump(calib, open(out_path, "w", encoding="utf-8"), indent=2) - print(f"[INFO] wrote {out_path}") - return - - # ── Mode: full pose estimation (default) ──────────────────────────────── result = estimate_pose(fk, obs, cfg, seed=seed) st = result["state"] @@ -754,6 +672,19 @@ def main() -> None: json.dump(out, open(out_path, "w", encoding="utf-8"), indent=2) print(f"[INFO] wrote {out_path}") + # "Uebernehmen": fit_origin_link's Y/Z is already adopted on `fk` (see + # estimate_global_ba) -- write it back into robot.json itself. + origin_link = cfg.get("fit_origin_link") + if origin_link: + origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin") + if isinstance(origin, list) and len(origin) == 3: + if patch_robot_json_origin(args.robot, origin_link, (origin[1], origin[2])): + print(f"[INFO] robot.json aktualisiert: {origin_link}.jointToParent.origin " + f"= [{origin[0]}, {origin[1]:.3f}, {origin[2]:.3f}]") + else: + print(f"[WARN] {origin_link}.jointToParent.origin in {args.robot} nicht gefunden — " + f"nicht aktualisiert (Wert nur in {out_path} sichtbar)") + if __name__ == "__main__": main() diff --git a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc index 4831c796ce8db7f2d9a1426f5ea0d65206065ce6..63ef7df50d6f18eb81a8914b057e66fc1e12bc62 100644 GIT binary patch delta 12578 zcmbVy4Okr4kzn`y&%iJY|9=vVK!O3pkA(g(NWv262O0e!OA!)6!*m0K<_EuNghU1p z_Bs(LN+jpWwrp7&c5TZf#*SjLiL$XbO4gewoBj3}eX|kQyWH8E&%2l0Z5Ah+i=&PA zuIlv+GlS&I-8Dn?>*`na-m6!yUcIV*>z_2Q{8&@|y47mo;QCL4^@C^6aNN)EA$Fyz z1^(Bm&zA4qul1O_%px!9JQiQZ1ALcZCy5RdRMyF-evb@bY*xm zyE553ttYE1TQUvi#8^+|(hqX73waGE<-xyI%D=&L+#B$(UV{bnqbs>|s^8kc7#KZ; z-lDD|?~1M!JZHwHO`etB;;v$NYwi+!)>2L?DdUJ9o-JJh-OjHLX~oQ!Q@U1(SyHK( z-2{Ba{bJrTx~?+ufOt?8p3!%$77vN50JBEy6j#H&Ts$mRz`a6zT&#h+Lp&m`hkK=X zRNMgfD)BSoX1G^F4~~glViWLKD;^h{0aqiQ5Fdqmt+?YEuB%S$-Y>SoV?C>SQfz~& z>%<K9j7%oVv-6{Xt)8TH}H{NFcfVk`*%2pFIi?cY1SF(4c*Gh zKDXTCC+>c?ugBx|ofevfRV~C@ze*4UzfTeZBW~F>&?v|Q5)GRKi_j9% zk%S7D-#g@yypm52usZ#c-z&+)?LxK|@32R9uSFj8JC>D*TGQzoAZ}R}jyH5W1gB4A zR746uO#wRO6#YZ8Bx2RI&SBZ_b;@p+)8jcOtX(UNAf@CJdY!J*&?(t35Pz>`BFG z>73x~4Zyf+eZwA)W415lOFW-Kf0O!(U9l~bJH2ZCAK9faRf=JFNOa1QqV1PthlZ5V z7t@;cYvGMJ`}b+3dc#gAl2)qA5)74CTu&>r)|A#_F(1J?0E!M+I2EmbSXOwiqV>8( zvYQ67%2!n5Tf4{a6np&8H*yAq=))Z`ftOi~>jARei*^8hZfMRh6+`fS9^tmKbfmrONqIb)ZH{j3` z*3pBA(h}!LKno>MHBwE#QIM_gho>*npA`6^6OR=Bk;%{q2+~B~D{bc+X?j_Ao`eX- z(cl~!l6)drkC;08xw6tcY|fEI1`u-&fTENALw+(sZN3-5#A~M;dtNi+3NL+ zlBdV(B&Q`3AQW)_lm3gN4#-zl3VhYjxilMv@Q@+3YPWWUE9kR4WneNhK|1xwE%sKP3G)*leE6i~DyJw8Q0 z9FTN9K604;YVE2*Hf9^Vbc@4IPtO_2CHo0^6)69Gwzj4Tw19eQXIoAJ0wxUwt7u2v zL#%HO19=S)#4eI{ab)YqoZ|u)}hKMnzMB- zJrrV>6$+pS{I90~d!9SbpVyq%oliS&JD+~uPCu&qB5$Li`VxKGWZIO0+S9UWb-hi; zbCWvyesgZVHrd*b>4SRm=p;92qMsD4v;_5T?gp5}H#GQ2y(=@RVMX>DQJ+lZ<3;X* zUCxa4iQ^{ivVp$enU_wo{h&X$UNdP|Nha;YCL0sqKgf8@AZOD8S3&4Px||!Y zI+z+OPo(wnqVXBiq-H0VY`1~EC1?;$qTzEIT;A&CTpY(mDQsdoP!tu(CGl6HDN%?)BYn2NC^QQ5J!8KC@Qo#X7)z1S3`Va> zO)zCJBlaY6lj(_Km`u9?dh-U_5O$5Fg1Ckf$B;-pnI0D)n zn@_({of~q&Jc}mW@8O<-cX_Ebu*jJ~{WVU^dT@dOn~jw57R8)vIx!cyEW_npHz#Wh z&jnLExl&GE!-d$e5P&H~jMc=JXp`RT9w|CMY@=0oA5_zUJCQ-xGCPBtQeo{ASoYYThlU2VHOl=2Y zqd_|a5moTaq)@~J!-`Ccx@4K(57Vh)i z)V!%El%-g9w;XBh>D;mBz%fNXM8I=Xor^JGtLVVh3J|oW#DReAo#(J5BEVI|c2V*e zmO2pNp2J)RaE_d3RY89NaEiJ0tVG$}1Vax7yk~tvA$7$RHtLWp8sdck6w3xBNN-SF5H-*jgE}R^GGL+_lzBhi+@a)*TV+j(gT!cdfhbn8MbB z5$nM@`1K8rw=LwBOs#pdESy^#$*rBUcKq3nTZV76f3y8|{oRJ0a}7Huj!Yc6Q#Rf< zUsgGnRoL*{&Z{}k?z^(@^1hq8@y8a@?3W#rzKg!`T^~~a=EBf?O~YJX(^cz~bDE!W zMe-`=@+xnw`bf)XZ(V5JyQt+;_wgTb2>*@2@tunXoxXXI`~L%}T&nHTrVE=cH&2y^ zQz|1=*}RIk(oZ(mg}TCrjz2Q@+7HuCbeHEg!f*?qX;$|QC8LKO z`M{D9xd+_IVJSfV4qor0&o?^YTyVSbr^X`OC>R6sXGosB@A!Ar^1bvQA1wv@lXf7V z7VoGpjyA?@L=TeSz5|OawpSsINWZe9I-PAP!^rOzhe!3*=6S4)VH(Tz?e1CYdg1=p)0M9IdNyZm&Ag8vU3aXp_^Lq*f7vKn8o~W zjcT6Q7GNT;)BoCh(hL$L1qo5}ycw}}1481z8cj?Nu3Vwv{YFjU`t}wx)w1^)8 zPD+QOb^H2A3z+J>5Sk!$2l@*vMF9LW{Bzv+neC3u1QRO(PSL`tn*0z-sx1aH8MVDQ zU*xk0IuI~pyAMmu0RI?Diq7SSZ46B=`D+AU1K{Y+iw}qV6wz#mC?xfygY%;3dp&;F zX>tL}rry|z3w;D;bQJ^Boq%HYNN~)R&kae6##u++pkLaRpZ>R4Z3e-c2zu$acddp~ z?ZU1i{jUJgIr|^GzVrw5YkL3s_0yoS`3<)2L=6qK#i}={_Nljyt-h1 zhaCPL!S81;bUvjwyolvp)PBrct17XggWYQY_CdEVx`-5hgegGsT6~|rd90TI2fA?V z5m*HIT?1_8KqLfCe4uimHpGB?UeW?)M4<(o6cYjzox`S>A?iS6zuyxeJffKnUc;Vs zAlQ%K002cR`en5NJp@jC>lD3<_yYl^!_ehnFVj!OC`$cAk^&Api4Pd90Qo(D4XhD` zcPcukuRow@Mgj`&CZER!G-tfb^K9{ z4hKOGcynemPG)I&W{CcP{ygcd_>l^m{f=>6=s#71gDL(qoy<#1E4nS=%?x>+LPySz^%$i!R z_1Nowr8iuSs=55|A(qII_UU$pO{Eo?Cz}}Z-w?OV!q^iG3{!o!9N5^|>R?{mf9JTv z9Qh@Z|0{w;1b>SlZaA2^S(0AB4sXGAOIRjpF*BO!thX2ar-^!(ZX3q!CtxTYWt z%myipX)|d=HYirulIl)3@PtaITUVehRi$iJ7dmRAq63TWCqJh@I=ymjFCYSD084#i z^9fF?8vpBcd))FlQVR`fG_x5VBhM6X_RNLH^}I~~=F++lv%i_x63(qduiD~VL?D@x zx)etu-hh(g8}`QHV6adwVzoL1l?Z|{DuO12pv`4|tg4^r&XYR;6kWhQDyhr#2+k`Y zo**(95y|OO2b`KPm23i8H<`7=QxC!ZDLNSC8P)Fwp}1wm+~9C~V7+L1Ax$o`VUFFfZ=N}!cpd%a#k{6b1IGn*$&4{17SM=>&QD`x4C;dVDLus2vao-8 zQ3qib`ax%&-4HaWl%nYwE@*)G8>Cw3(Ho`&Sf!Y+SsS#7@;>moi^_q?j4dvRBLJ$}o7|JcTC36cJMROC-CkE_-8qxfR z_#AzZPdf4u?Z7cLKh`A<;x`%C#sipy7!uUUylxybB}$G8NApmEbm|G6Va%NPN(a(g zk}$rn9C2D50$w3HJai&r+>=^=`37QiFU}84L9D*!h1yg8Y^R_eye3vXX z$O8R8>7AGZF^OE~*I+?d1R*2{ozdh7BvXilxfF>dg90^8OY355&7dES=ZF0VoCI;czt*E@pauo1swAokaz)hf||EkO1}t)TR(Fc6GcNFr%S3yNX!TWPDtwrp#ezt ztaS|&I0cK<0_KgJE}3xxYX{ziP!V+o1xa?*u>94KM6idF1Cu{ap$`%<0w!;2Fa=WK zsA6St*gI6=V2p@=7}|jB(~vOYANGha%qVYpAS4MR1AdPL;R}xhIUzp`0P;_5(R~Wbgvl&jq>eoH0JgPpQVC-nRQ$ z&Bff#*zrx#gBR}}0{83`5Gedm(pl^~fE%r1J$o>5MxS+Mg^t1|7Z9M@I5(Http2Xn zPaM2-^up1}V;7GtAyq#c_rLpmHTgE&nOi-ot!d~}_`3UijT&O&kCXTCY2M+(^r4UugKwgXW3K1e-5`cN^jW#Ls!(Fl%HYXA_?^(&Ho{ zOz9j#x()z=6len%M)XoQ>fBF}AL#;;Hk99G0OQ5E1*pE^rRrt_qgK5ew ze{~$)^P6D+q$r|HDYarxDbmW>a*t|7%{wHvtK+nm%r&&)~&wp4gPs( z@H?J&Jh!~Jypb*Y!nVgEw#UYI&!=Q!Ln^^M(~2q6>uGM%g-y4# zVWBA^G=YkQQ=22H&Eu^LM$5#8r$(O{y?pTL(0E8~F(db?=4$=R`e&=JRL|MU;CIhf zb=Ouky(4T3)kSP|_iUT)+K`>CDPn6HZ=EQ4YWKqbiR#mc=F>M_ZoOJ^W%peACiqS1 zr<|{v)be!e&61fY9x&s3=2J5+?Y$7%JGt-TzN?$Rvh}5{;lip&VO2P_YP@ycl76Z3 zLgi%j#p)EY6h&Y4`OL7{1OgZ7)c&BpKYn~gtm)b1|R-YL`Vt}x!I;&*E*jP#K&7ir2YbktTb z8~C!3r!(|$N|u;ClcVm0@a;wpExFRn=hB`loqQghy|O`@FBV{mi5j0fnpqgsin&P} zbrF5yxnh0=efGKhO?k;XAY@Fyr6GE8Wzc}x6G-9}KWv{w+EyW%mgFa`72f@^MoCF%}%a+3Va2lSWdtHV##WAEMW<wV-DsgM_ zuvPr7jpDXsytw^keK0M^T+fT27h7aUT$Y2`v9kDR5+1ZSH;4YBGwmcuAwf|gEyoS> zz%EHP$+8o7ylfmx5AuU}m|@pYi;Rb<=ry<`)Tsi>7iH%@{9Pf-n%}&de?;7wJfbnX zjAz3byP{Yrm@eayI(h}|VrzUw#;{_+Oj~q~w!Dy=yDMmaSUwV0QvR})G?o#}Anylt zgDdF4YsKb_n8w4?N{hHV@o|;7Cs|R(GUeu255V`$l(%A*UPrzlucoy^K}g&i{R(PS zZb_)e4`M`vUBQfCrr5qDIp84{#Gm?>r2)yhq!qI-m;tK(7|?8s(*UL;$`d0Vc%hCn zWCCl3+y7v`SLEPimbferutpBTyf1YcnmB|47CWymJ6VAV0s7cKC>8kZhv^5`x3jNj zMt9S0Lq6U9$Ily1p`US*{=*+Hti+;XgwL#;XTU2xi7yg7M*7awv5*%Aq2_CU0Hx6? z%*@5Vl@dnWLfEwqw8zo%bto|fT568QE90&ZkIGQ@bwXr84tnzs@Kb}kUqkbWaX zp}qhW6#ksTj}ki)WMPZAibxK}Rj9EAa4fMJVmR_A03cKcF}Mn>--fu=^v<8I+%T#U zgi%eSzycJby3IA~`tI|>Xeu~Tu`@w4Sp!6)Mm#w9`kL?a4r+R3#kLKIw17v=zS{u~ z@Qolx{t{W2Ud~TmJ1Ru0BnUfExQ)VSe)1736g^|Xw|fcB@2K(k z(UwCU-49fc)rOPSa04_b_}s1mS?HG_tOtD*{&DK;00I`Yy@%Wk5DJ3t^~iS--HRBFzn8diifaHUT6wjzpLp?^%{%Lh3BIiB!GI9? z36gya09~=VAg3DXATP9hY``T02+W9I4m}MfDdpc_JpyoXPW{wP3dBt7DqbqUj;YU1P=~XVUNI=k1wS z@?I$Va?#b()9b?anuxvTo_*b2`?{OkZa*5f?~2%W-LtpfwYT5d9kxFnu|GaxSV+mf z+;?3Jr<6ugN*6hUc|ZTAcDn7WDKjZIpSWAGd9Gsff^E%|{OahJCduSgrfrmu+8v|$0!I~ZXREm06;cfA~!NlkJEiKq& z2kS@-BJ=&|@dAX(%b^VgtBQ9~X9$6OR4IXA;K$CSim3;po$&c=k3*+w*(y|t)d;Wz zQE&`)!N+~5ylNyAcMejIU>yQn1u6lqM_?1L$=GzM*a5aA*fNT9H>mo|uxN1!R#usP z3CU8`U@gNk&1W^4`eS427lY4ZsR}{7?=K*=N&W8O2EKY7KSv^nR=-||H^llE4aY8H z{~A_jxJuqqukxVu=Lr7q%(L(z51I{s6vz`7gfhYOm_BZ$k*k|3b? zupdU9Nb@^n!o<}~O$B>9w zY8Kq5`1mS}viL1@W)deoGD4OT`=df(Ip zU$G8*q^*Pk8T^X4K%)=PqOOkDEpkhsmo9#N%dBv71-~Yh%PU$eu@`F=IRN8e z7VJEhqu?V}s-P#o@ltj*58p*3gFc?7zWsTAD}DFv;<`dUeUVEBO?!DB4Z*{1UG(4H WcG$O>9yM`4Fc}}s(Eh;A1N`5-T_k$| delta 16032 zcmbVz3shUzmFT_t3rUFoAAI>E%-?p5Kj1Ie7#nP3$ADuC!4)8^4?dFop;EAs#-<(A(y+p+v!xEE+Vq2ZTiRfnEqyTEmNA$?^Q6|y!7Q_KB0Gk1 zE=OR!*@dixG3Ua+)|~e=%P`NwzxYh#3#G;!;qix4H&O&jYk{qBu+UaCSi~}FY+7Y4 zwv`N)K(2alon2SXn7Imu$RVCGxK7y5R{An;qzsnwS>`f6dpls|d-=RevcYoxFn@&S zF3ATg_@jIoq*U^Kd?ox=@yGZY_^sxT^9}G@!#~JxfL{ZDg5LzcwR}I{3cu^2{Q-WE z-wrtH_>+7aq}B5e@w?%-f!}k98EoWF@f{C9v`LMfJ6{7_(M=) zgLxz0b4G%rCR(7x2BAcfFB^7$pkCZ2X;JR0uOBg4EyKj*GS@qZWz1sdwjZ?E&$M%# zbJ9FwF}W2|rU83bKki;jl_6G*zf8 zMjWaY)A47%dKa!-Uge=ZJaIL0sE7yZM}%Ag&AtZyB{fE#eqGfv&$eM<-$&Vo4gKkPA+~*UoKbdK|=2pp3mYG z1`M_cp{%O%1`OsSXa*3G0SZ$@>TtUvtSut7S$MKfD9ElZUXQuyR)>ilc5KlP%S3LSzph*etLGpDs8A!DporNkdbFJriNiIi--maU5AM z%oS{8uL(acc)(W=X$B?PkJY*m8~_kWu{d#ZP4*Epsm4_7ctj2~v^fn@Li>IglcdCS zK8_bt^t7u^vza#n0+*SLLh2>>cVb|gc`t|2rd{3g=&{*~IqAG`rSJsw|Cfb-tx{}( ze6meAQ_;0nMC3xOOyKc&k}I&N41N)Qvt9s#wUc8D)9tB~?JOm%-Mb zk7Y2fjMX6Xg6wH%=y|$oVX9Z|*Z5_A#;^6We#xkUmtRtO6#>~?iojmZo^x#e&09fE$Kz zc$3_$ds+@1gn#kzs$J`1VTK7vJnD(GSUjM48f5W#lt=pXG3AU4*OKT1yUKcRifc6= zc9K+2%9og%65*T8MGa6rY3CY|3@|+7P5ER>U-b7pd!D%<9bnG02EEX*`I7}kz)Xe! zIFW+524KQ40O1?0>x4H8)Rj4a^N$6PGR*`v)#5vI*1yZ|T#b@?p|*9f>P0AIU@6xE zA5qj);GTi>sGv;4BJ*79W;RdwWowfH7a6isDBhAU{HR;*Q#{7Fpt0B!PUO;lP8OrKT%k6V-=40+0}=Tof!$49H$8wWB0FQg=p5nw#pMmw ze$xf=NsOYXkt>*{?zk{wp2US8mH|2#x&ly%{W@cTV%X`33>kM`((ag>uO$yZ9U71Y;ziG;Bo+y6%}2q z(joj?S6x~?q>G!~c??YghOGvyOuzd#PcdarijdBA9F?|=baJOLOv~g+mxsHUs&onVcE1!kDN$W zV&K_D&G2pstdTu@y_BVE*Z+fef|gqLd%}N=t*%petk^q#Xun8kdJ3RhC#W@e6cD$) zUtx&OJQYHa%;;*Ugb*$>@I+S{;V0we?3=;|81h>TzJnln*+?Xs@WDiZIBV}>K?L7ftUB|3 zIeY$Nh+fhu%L}{rOS4-rqhzwS82l=H? zL`v#Skl<7USQS)JK6O#Wc-UgKxGs_!Xy{k$;uuk~o9m%891m$@xVXSF3YH9`-DERI z)T6-6M$-ke^L_m>H<%Yr!m+K6YiMXVB^=>%e4VfhMKv4gW%=dY%XydcFBe=cd<;&j z0cHZQiO)Hf$O96v4@quF`k2IXYk)~UsCyNzRsDcrKy9Ilk4M4F;+n>sG>TSeT&EL) zxg}e9d9pThB`=USt)ML`c;yXgALC~Mlj@QRFsTzT$w2qYk_9AVjNmw3Qgo#-vDX1f zQc2dEQ($3|8=7P-j92MV&dJbnR;?LhcUT;MM^BV|4pdsL z!{=TC#a3u)DOBfrWTZKeiz=%(Tb_4`@yLW%FINaZ%}o<_x>IXe#-Hn#f{rWq=Yf`+ z?=Kux@%fDBP^L?xFmT9dEyYSoNK zvzIybnm5%|7@bb|nMjR=J*i`?zX15}P%-H6^0hiVo|{~GvBC`VLP>HLy?W5S@jM=V zJSiT1Kq_)v={kOmBA?jVSSe{KUq-d^JkZKl6U+GuO!V=UQy_9LR3$5p-ZW3zMEYu* zy69A;CZ(oLWW-8%wO-xTzXv6~Dwe`9prlJ#W`Mc+YE0KwGdTWJI&XTQz>`kd7WX6+ z3r{-o%+*wTcKL@3ttk^j=h03p`RYX8NgTdM%hw2Jy?H)w2IU6apcta@Wbg*0IbS=a z0BD#>0b5B%k`%0Ef=riz_JD%G8X1A{a1Q})ZqUVXA94{n@0-y z2EOrzB6+|pT_p@n?lj=R6g*RQf+c)tbqg8-b9nEGo>g-=v6zOfX3pgROY*pxaOW+q zan9m$(yBwkFVtDSAr2Kahn-r&Yk2dh3GCl!isvmOV5)AeYvJ0sfx1BhhZZ}qm783& zHntfxzDIS8I;g~%I5?1wL7QOXb~{}hC>c&yz4NTw1ZG4IEC=Vz4csoPm2+T+QaB%s zA!fW6dkaQ&V&=xpRvyg!7tB0of)S$mcZjQT5U$C<{c`#`^uq!Lop6){wCEuK^e%85 z(7s`uT4K2x=+Y!8(UuXn)kLs&Rtpb=7`iaTA*!RO`9)(x{IEQ1SbN}+rc-aP%L{Ad=0!~q_LZN zLo8bk%Si8|OZyFhQn#&Az6xaE2%g8wTKg6a@(6l%) zCXP45l!1Yq=QdFH1vg@vbh&|+Zk!TMoF%Sy1ZEnDRZG!1e0J+au4cqSMqthWt)YQC zhNPt}&>3Hy(I`5Y^eCzk2eAV!(N!XHR1E}0%B<&XF!9hh&xw}mDEXjaYD-4a!Gu|C zKIpI!F&OG#8fZ_WAyS6dQEV zi4%sL<93?Co5aB(klWQW;Dc7rIjnA%xe++p3_L=)5jmTBs_LNsE^Y!@8Rw<(VsuV> zXy?w0UZA)?r-2(9J=1`);XJu2m8VXYQKZKlhk>>2t};jvv6r3l4bct)#cgi%Zt$Fo znCwmq6oZN3-C+3>Jz!LHxSFHrI_m6hK%s?B*BGGx^~kVJkZXzx4x!F*qz(4)B9H-u z4aW`aE;^o!(8I}#1`bJQ@>$QDE`mo2Cc}ZVHEagLm?s%62n8mU)cvz#dEY2i7YINIjBs%-^a#?1Ri>p!{A<<(a5i zShQQo5)9PnCn9jHVw^Gv!BQ4g`F;YisRqj6$lv$z5N-z}jmhe47pA&Xd=;=CI$NO5 zz27pwWx!mX)erCEt+UOu=Gz0oqbGtL_|6;=f1{8V60Im`JL#D!MdE-+IAM~&qk4|6 z{Yjh$S|yT(G(wjn3f}AjB95@aA162Y7BQWI``A|?Z>sU*45wlHs_D;wgDQcV_h3L7 zed0Ttey8+~^G?m3nwe&VcWT4|obzGxr~*Y4BDJ8aFJW37P|eehX>(Xk3dVzo(s{-L zo;)5@b2{g#bhuzL759=ziWqm>Ev`sfbQ(k!H`;7uJ&=K{L(tfQ6wx6^t}-O05}`_7 z*8TWWcy}^KcyoXL_Kz~eLQg-Ea>DI`EX@ue75RG{*{Zd+jZVdnG3_*fWb@4M$HCgL z3n!o)iXuaGitpq6tZ6QN!Su@31;>J8=Ey9IzY7j%7%UOKr=awssx{seS@Idg%X;AM zLi`8?z7!QbfT3(fpMWyC4jlS2sennmQm8HQNbpD*#}P>JNCAhBxtf)z&;n{jKpoJ) z84}JraaUL$m`8d<3km@|z$$3xh^WnWI1b?AFM4jYqPgaqkVkYlzY=hSJcBWme$lW& zS>O^z1!`N}faNr}(nL=+PAz1DWOLM_6F2HW>Qv3g5(zLb@4=}sLKTpOWnKDAMPTsi zpkK0-TQJ*v{hVLoZw+XdvrCT7UJPX$!r6uuhE-Qk5aOiw$gNF)i(8Y2`G{z&GA)c{wyZO-6yzjJ=*y7uaY&Oy z861$a25N%977mKxPlYWN4dr{3bF_*?P>zta&Vb2K^r>> z8i~yrk-@3LY$&AnF6iwENHJe<5gv#kUS{x(V97a%9a)m*>K`BQWOzgG$xWxT(C5w7Pwe~ zHzs^=A4o06pcQcyyM&l;&K}f;4c%u6kl|<%&K#@evK2g&(nSk$%74f}HMqfCiZ@PO)n|gKs z%s^PX?k#QkU2XYX@50fLwl%D64QktO?Vj#f(q%k6aAjcT!rVIi3+bA}y5{N5Wli?X z=uJMPDGzJP(dS#bobH39Yem%fa%Wnw2U?f+s|1l^ws(m{tF>h-jY7suUl58__u|WxnX7Q zZ0TKPVNhAPRM?zY;^*|n#mxh7)gq*+4r{8X_bw?lKu!66c1fjuSaU@qo(SgnXC}Tf z@r3m$E5t)tone5gePPwUplaWWOfGMS&H(UH_!ogxX8neRiMzEsgS9*T{KMl{#(~h8 zq14K7YGqJY1@DhL`-1}`p-w*B$xnAK_tzfubU(S6g zH@Kzi&W2EPZ@8klH&lN(Tz@#&cPvzYY(*}~eGsk=Ap8@BGtw1>@VUd=d@JeL-y9~h z2siNc6!>>9ZqQik@ma{17w4lQNmEajFj71k`#z3~+(Ok`WAbs*e20Q+I6a4=jr zxU}b>@3r<|-r!C7&EXqkUmbpGY~FIK`B{6Ysx4gA7Ak2Em$ZlSc7*eG1mQg~xFTWG zA7oc_s!R|%0Piv?x$;j4APOLZ(Pm5^ayG-N`u(=X?i}V${cgp9jglX1U?K8@jVax% z{0Hq42*1g&5Pp+Qkzyn#qq{~bn2zKu`kg!EtW&sip~XiX^>7#w&t?X>=(Nwp1avWx zg8(8LIK#%yp5Pb#EasvGQQQVUv0C`g82TN8I|%4bMz_cBV6+$U#rMO*7~OjWA?}_A zZQ|aBSO?V+(~h{F|gjd@&f`o!KQGDPIs*8@|Nw>s*yWh%b;qkQ4sapAQvY4diM|JqoYjDd87InLZCYft07u3`=e$ny5W8xSp4g?~!^GFO(;%9A2fXJXRkLi51{&s!Zxg z6{J)rrrb=_n@XWB7;lq`@HJ82(pMneAb79jZ>WXP`a~pr9i-I9Q}~82$@oT(3I-Q7 z4GUlXd~SXdlxmKb!l8=_80OZuK<@YUOt&yLBLM zcdUzXlrU@AvDjpnt1cD>e-g4VedhAeYnk_7WaiWBq)ngCA4=o^Xl)OW5Dq~J7ev)A zbQMRR1dF{g(HR2XmA%Nu$+r*r;ukuE$3I_O-04x@k2Gjva%~olUn$>~;z{B6#R=)v zBoS?YvR(~e3Uq;PJ?GUztpmcYtEH9NwN6*B4m>;E=itTxe-Py1kXPl=A>p%o_}(=m zOcisQ;p>9r9H?6cb&q=WNxj+b(E_FFJQ~24!$C8B$<_E{F>d9Li>QlR*ER|W9`vN* z^m_EdTc0g2KLJDOj}riLwm?b}?+L|Mb9HHS=%0=cJ^Ow`r$f#E(15(@o^+BUc3g%X z7hb)ZnVv)^PrBIU%pQIqd01llz7O#zKX^m#N%Ls81CLz&41dzKCB`klkHxVt|4>qT ziEy?qSNP(?y3;VqgtZME)r^}3bV*}RrVW1zJjxkKLa^(<;Pa0q1O>gPlSc~f>ihG2 zk^}JnpXUum;f;BJ>64pbbaLaBh*nLemjE2qs}X;d_}(;`H-JX__?}Zf_gy`YD5h~4cuS)4Mg8ZVp`Xv73j_TYXyk~k^FC{s%DbNx9lx_7jPSGT#q3pKP~bXH$&auXBkUAe zwagIQB1jGbJZ6D=fSLkm!W^c;=UEIXMbLrK65-oI$q2QdP03n;TvJ*OIvSn!wkc&D zxJ>P%ZBqp)C%bw&dr#eu0;doi-u^boBXW9!HzGqTG=Z9^X)sP7$)JeFGN5-54 zk3$KDQ~{%SjD)*>lwHrlm?A3p{>V597Zph#j+2^VO;||c7$((NLh3S&5jSR1%3u$i zcv2J2Dl>CMYX#hNgpX>jK$$r-OV|~{*2lhTkSj0ueyTT^RkKiWTd^XM6qwi*DI?E% zmyybcK`BGHf^hM_KUT>4R#c!wg_T9`GHG)Ca@zX&-3!u%W8qDp`2`;w45pn7rJW3? zo%G9>Rk~p6iFxg7Dsw`)=97tp~*z!oCw?IHVm6YX^hcL0WCsoHUT;i~$U3$HUt3pmv;=9-hyh z|3uiZEtoM3?`@y^>Jhr}4bkai@j9SLejXyK|E_w0k$y}^tscthIiu(mo#-?wov zXXiFOv*jyW=KB}ixBCCi7b@QuF5d^q_#Pj8>-g!r$4>{16G5vjble_3ZjWYVoDFG7 zSWALhvXtq|z1|A?U6zr(Np76~vgeDQWw6fdd${*X@9f?i2SZvzSZkQ)!wq|a>raXA zyHb`j;>CP`e%0G5?N#+m<;|?w@p+N z*1#K58Nw<M=B6t z2rgtIKf;Z36bexn!{dqgeHChkqLGnS*nlNv2_IoD z=&uCj+`-IlOl$xUQGgQ@u5ZKVj-zuse77-4(*;+5h^vDT_^z)EQ}jlo$qv?d@#=}u zDDE?KcNv9z(H`*#hMq+5H3ZZE@k0##4#A%h(EHf$Vu+eH(56gH1auh(k=skJ%mV&J zEK`qQJ%IOB+rf3=wwl|?m!Jx~jI&M3FkmiY*%es>D_db!!D(Uqi7kuIJ(0zhW^2|- zRu}-&VB^%V7>X{bo}^;f=_mBYd(Jjr0IAUye*_4`?7FAIjzwmmCKxbdE`g7@C< p2dz?};Hh