From f87610c9c1011b223c8e2fb60165aa46d3ab16bf Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Fri, 26 Jun 2026 22:01:02 +0200 Subject: [PATCH] Flip B --- doc/Homing_8_G92_B_Flip.md | 123 ++++++++++++++++++++++++++++++++ server/fkStateToDriverG92.cjs | 26 +++++++ server/server.js | 18 +---- test/fkStateToDriverG92.test.js | 91 +++++++++++++++++++++++ 4 files changed, 241 insertions(+), 17 deletions(-) create mode 100644 doc/Homing_8_G92_B_Flip.md create mode 100644 server/fkStateToDriverG92.cjs create mode 100644 test/fkStateToDriverG92.test.js diff --git a/doc/Homing_8_G92_B_Flip.md b/doc/Homing_8_G92_B_Flip.md new file mode 100644 index 0000000..268532b --- /dev/null +++ b/doc/Homing_8_G92_B_Flip.md @@ -0,0 +1,123 @@ +# Homing 8 – Korrektur: G92-**B**-Achse ist gespiegelt (`180 − b` → `180 + b`) + +> **Status:** Fehler identifiziert & bewiesen am 2026-06-26. Fix in diesem Projekt **noch +> nicht angewandt** (bewusste Entscheidung — siehe unten). +> **Geltungsbereich:** betrifft **ausschließlich den G92-Export** `fkStateToDriverG92()` +> in [`server/server.js`](../server/server.js). Das **interne Winkel-Modell** von +> appRobotHoming (FK, Bundle-Adjustment, `sceneViewer.html`-Visualisierung) ist +> **korrekt und bleibt unverändert**. + +--- + +## TL;DR – der eine nötige Umbau + +`server/server.js`, Funktion `fkStateToDriverG92()` (akt. Zeile 934): + +```diff + function fkStateToDriverG92(s) { + const d = { ...s }; +- if (d.b != null) d.b = 180 - d.b; // Hand-Knick: gespiegelt -> Driver baut Hand falsch ++ if (d.b != null) d.b = 180 + d.b; // Hand-Knick: korrekt + if (d.c != null) d.c = d.c + 90; // Palm-Roll: unverändert (korrekt) + if (d.z != null && d.y != null) d.z = d.y + d.z; // Ellbogen: unverändert (korrekt) + return d; + } +``` + +**Nur die B-Zeile.** C und Z bleiben. Das interne Modell bleibt. Sonst nichts. + +--- + +## Was appRobotDriver erwartet (B-Konvention) + +Der appRobotDriver speichert das empfangene `G92 B<°>` direkt als internen Motorwert +`robot.b` (in Radiant) und benutzt ihn **doppelt**: + +1. in der **Forward-Kinematik** ([`robot/kinematics/Arm3SegmentLinearX.js`](../../appRobotDriver/robot/kinematics/Arm3SegmentLinearX.js), + `_fkPlusY`): `vHand = rotate(vecUnterarm, n, b)`, +2. als **physischer Knick-Servo-Wert**: der Driver sendet `b·180/π` **direkt** an den + Hand-Knick-Servo (FluidNC-z-Port des Hand-Controllers). + +Verifizierte Driver-Konvention (die laufende Steuerung/IK ist korrekt): + +| `robot.b` (Driver) | Hand | +|--------------------|------| +| **180°** | **gerade** (Hand verlängert Arm2) | +| 180° − x | Knick um x in die eine Richtung | +| 180° + x | Knick um x in die andere Richtung | + +`robot.b` **ist** der Knick-Servo-Winkel. Weil der Driver `b` sowohl in die FK als +auch 1:1 an den Servo gibt, muss der per G92 gesetzte Wert **exakt** dem Servo-Winkel +entsprechen — sonst springt zusätzlich der nächste Move (G92 setzt die Servo-Null falsch). + +### Verhältnis zum Homing-FK-Winkel `b_FK` + +Im `robot.json` ist die Hand-Achse `[1,0,0]`, `b_FK = 0` = gerade. Der korrekte +Zusammenhang zwischen Homing-Modell und Driver ist: + +``` +B_Driver = 180 + b_FK (RICHTIG) +``` + +Aktuell sendet der Export `B = 180 − b_FK` — also das **Spiegelbild** um 180°. + +--- + +## Wo der Fehler war + +`fkStateToDriverG92()` rechnet `d.b = 180 − d.b`. Bei gerader Hand (`b_FK = 0`) +liefert das `B = 180` — **richtig**, deshalb fiel es bei G28/Nullstellung und +fast-gerader Hand nie auf. Sobald die Hand spürbar knickt, ist `B` um **2·b_FK** +daneben, und der Driver baut die Hand auf die **falsche Seite** (z. B. nach unten +statt nach oben). + +Der zugehörige Eintrag in [`Homing_8_appRobotDriver.md`](Homing_8_appRobotDriver.md) +(Tabelle „B = 180 − b", sowie das Code-Snippet) ist damit die **Fehlerquelle** und +sollte ebenfalls auf `180 + b` korrigiert werden. + +--- + +## Beweis (reale Daten, gerechnet mit der Driver-FK) + +**Pose „Hand schräg nach oben" (≈ Richtung (0,−1,1)):** + +| Größe | Wert | +|-------|------| +| Homing-FK `b_FK` | −70.14° | +| Export jetzt (`180 − b`) → gesendet | **B = 250.14** → Driver-FK: Fingerspitze **z = −36** (Hand **runter**) ❌ | +| Export korrekt (`180 + b`) → gesendet | **B = 109.86** → Driver-FK: Fingerspitze **z = +111** (Hand **hoch**) ✅ | + +(Handgelenk liegt in beiden Fällen bei z = +54.6 — der Arm stimmt; nur die Hand kippt +in die falsche Richtung.) + +**Unabhängige Gegenprobe (eine vom Driver angefahrene, physisch reale Pose):** + +- Welt `x0 y−525 z130 φ90 θ90 ψ90` → Driver-**IK** liefert **b = 170.78°** (das ist + der echte Knick-Servo-Wert dieser Stellung). +- Das Homing meldete für dieselbe Stellung `B = 188.16` (mit der falschen Formel). +- Richtig wäre `B = 180 + b_FK = 180 + (−8.16) = 171.84 ≈ 170.78`. ✅ +- `360 − 188.16 = 171.84` zeigt dieselbe Spiegelung. + +--- + +## Warum die andere Variante (Fix im Driver) verworfen wurde + +Man könnte den Driver-G92-Eingang `b = 360 − B` rechnen lassen — funktioniert ebenso +vollständig. Aber dann spräche der Driver-G92 eine **andere** B-Konvention als seine +eigene IK/Anzeige (Driver zeigt 110, Homing-UI zeigt 250 für dieselbe Stellung). Die +Ursache sitzt eindeutig hier im Export, daher gehört der Fix hierher (eine Zeile, +Driver bleibt unangetastet, beide Systeme zeigen danach denselben b-Wert). + +--- + +## Status der anderen Achsen + +| Achse | Export | Bewertung | +|-------|--------|-----------| +| X | identisch | ok | +| Y (α) | identisch | ok | +| Z (β) | `y + z` | ok (relativ→absolut) | +| A | identisch | ok | +| **B** | **`180 − b`** | **FALSCH → `180 + b`** | +| C | `c + 90` | ok (nur bei ≈neutralem Roll geprüft: Driver-c ≈ 0 ↔ Homing C ≈ 3°). Bei stark gedrehter Hand noch gegen die Viz gegenprüfen. | +| E | identisch | ok | diff --git a/server/fkStateToDriverG92.cjs b/server/fkStateToDriverG92.cjs new file mode 100644 index 0000000..ef52727 --- /dev/null +++ b/server/fkStateToDriverG92.cjs @@ -0,0 +1,26 @@ +/** + * fkStateToDriverG92.cjs + * Rechnet einen internen Homing-FK-State in die G92-Driver-Konvention um + * (appRobotDriver/doc/Info_G92.md). + * + * Unterschiede: + * b: FK b=0 = gerade Hand; Driver B=180° = gerade Hand → B = 180 + b + * c: FK c=0 = neutral Roll; Driver C=90° = neutral → C = c + 90 + * z: 4b misst Ellbogen RELATIV zu Arm1; Driver braucht absoluten Winkel → Z = y + z + * + * CommonJS, damit Jest (CJS) und der ESM-Server dieselbe Funktion nutzen. + */ + +/** + * @param {Record} s flacher FK-State {x,y,z,a,b[,c,e]} + * @returns {Record} Driver-G92-State + */ +function fkStateToDriverG92(s) { + const d = { ...s }; + if (d.b != null) d.b = 180 + d.b; + if (d.c != null) d.c = d.c + 90; + if (d.z != null && d.y != null) d.z = d.y + d.z; + return d; +} + +module.exports = { fkStateToDriverG92 }; diff --git a/server/server.js b/server/server.js index ae466c5..fc53ac9 100755 --- a/server/server.js +++ b/server/server.js @@ -14,6 +14,7 @@ import { runHoming, runHomingOffline } from './homingOrchestrator.js'; import { fetchRobot, robotCachePath } from './robotConfig.js'; import { sendGcode, isDriverConfigured } from './driverClient.js'; import { buildG92 } from './buildG92.cjs'; +import { fkStateToDriverG92 } from './fkStateToDriverG92.cjs'; const __filename = fileURLToPath(import.meta.url); const __dirname = path.dirname(__filename); @@ -920,23 +921,6 @@ app.post('/api/homing/run', async (req, res) => { if (!res.writableEnded) res.end(); }); -/** - * Konvertiert den FK-State (von 4b_revolute_angle.py / 5_pose_estimation.py) - * in die G92-Driver-Konvention (appRobotDriver/doc/Info_G92.md). - * - * Unterschiede: - * b: FK b=0 = gerade Hand; Driver B=180° = gerade Hand → B = 180 − b - * c: FK c=0 = neutral Roll; Driver C=90° = neutral → C = c + 90 - * z: 4b misst Ellbogen RELATIV zu Arm1; Driver braucht absoluten Winkel → Z = y + z - */ -function fkStateToDriverG92(s) { - const d = { ...s }; - if (d.b != null) d.b = 180 - d.b; - if (d.c != null) d.c = d.c + 90; - if (d.z != null && d.y != null) d.z = d.y + d.z; - return d; -} - /** * POST /api/homing/send-state * Baut aus { state: { x, y, z, a, b[, c, e] } } ein G92 und sendet es als diff --git a/test/fkStateToDriverG92.test.js b/test/fkStateToDriverG92.test.js new file mode 100644 index 0000000..e7dcac0 --- /dev/null +++ b/test/fkStateToDriverG92.test.js @@ -0,0 +1,91 @@ +/** + * fkStateToDriverG92.test.js + * Unit-Tests für server/fkStateToDriverG92.cjs + * + * Sichert ab, dass der FK-State korrekt in die Driver-G92-Konvention umgerechnet + * wird — insbesondere B = 180 + b (nicht 180 − b, was Hand in die falsche + * Richtung knickt, siehe doc/Homing_8_G92_B_Flip.md). + */ + +const { fkStateToDriverG92 } = require('../server/fkStateToDriverG92.cjs'); + +describe('fkStateToDriverG92', () => { + + describe('B-Achse (Hand-Knick)', () => { + test('gerade Hand (b=0) → B = 180', () => { + expect(fkStateToDriverG92({ b: 0 }).b).toBeCloseTo(180, 5); + }); + + test('b=−70.14° → B = 109.86° (Beweis aus doc: Hand nach oben)', () => { + // Beweisdaten aus Homing_8_G92_B_Flip.md: falsch wäre 250.14, richtig 109.86 + expect(fkStateToDriverG92({ b: -70.14 }).b).toBeCloseTo(109.86, 2); + }); + + test('b=−8.16° → B ≈ 171.84° (Gegenprobe mit realem Driver-IK-Wert 170.78°)', () => { + // Driver-IK lieferte 170.78° für dieselbe Pose; 180 + (−8.16) = 171.84 ≈ 170.78 ✓ + expect(fkStateToDriverG92({ b: -8.16 }).b).toBeCloseTo(171.84, 2); + }); + + test('b fehlt (null) → b wird nicht gesetzt', () => { + const result = fkStateToDriverG92({ b: null, x: 10 }); + expect(result.b).toBeNull(); + expect(result.x).toBe(10); + }); + + test('b fehlt (undefined) → b bleibt undefined', () => { + const result = fkStateToDriverG92({ x: 5 }); + expect(result.b).toBeUndefined(); + }); + }); + + describe('C-Achse (Palm-Roll)', () => { + test('c=0 (neutral) → C = 90', () => { + expect(fkStateToDriverG92({ c: 0 }).c).toBeCloseTo(90, 5); + }); + + test('c=−64.9° → C ≈ 25.1°', () => { + expect(fkStateToDriverG92({ c: -64.9 }).c).toBeCloseTo(25.1, 1); + }); + + test('c fehlt (null) → c wird nicht verändert', () => { + expect(fkStateToDriverG92({ c: null }).c).toBeNull(); + }); + }); + + describe('Z-Achse (Ellbogen, relativ → absolut)', () => { + test('y + z ergibt absoluten Beta-Winkel', () => { + expect(fkStateToDriverG92({ y: 35, z: -30 }).z).toBeCloseTo(5, 5); + }); + + test('z fehlt (null) → z wird nicht umgerechnet', () => { + const result = fkStateToDriverG92({ y: 35, z: null }); + expect(result.z).toBeNull(); + }); + + test('y fehlt → z wird nicht umgerechnet', () => { + const result = fkStateToDriverG92({ z: -30 }); + expect(result.z).toBe(-30); + }); + }); + + describe('Alle Achsen kombiniert (typischer 4b-State)', () => { + test('vollständiger State mit allen Konvertierungen', () => { + const input = { x: 192.73, y: 35.99, z: -30.88, a: -1.70, b: 12.34 }; + const result = fkStateToDriverG92(input); + expect(result.x).toBeCloseTo(192.73, 2); + expect(result.y).toBeCloseTo(35.99, 2); + expect(result.z).toBeCloseTo(35.99 + (-30.88), 2); // absolut + expect(result.a).toBeCloseTo(-1.70, 2); + expect(result.b).toBeCloseTo(180 + 12.34, 2); // 192.34 + }); + + test('Input-Objekt bleibt unverändert (kein Seiteneffekt)', () => { + const input = { b: -70, c: 0, y: 10, z: -5 }; + fkStateToDriverG92(input); + expect(input.b).toBe(-70); + expect(input.c).toBe(0); + expect(input.z).toBe(-5); + }); + }); + +});