Neues Kinematics

2026-06-11 07:57:51 +02:00
parent efe04b731f
commit 05355facf1
8 changed files with 678 additions and 27 deletions
--- a/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md
+++ b/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md
@@ -146,10 +146,29 @@ environment:

 Erst wenn ein konkreter zweiter Roboter definiert ist.

- [ ] Physikalische Spezifikation dokumentieren (DOF, Achsen, Gelenkreihenfolge)
- [ ] `robot/kinematics/<Name>.js` anlegen — nur die zwei Kinematik-Methoden
- [ ] RoundTrip-Tests für die neue Implementierung schreiben
- [ ] Prüfen ob die 7 Motor-Slots ausreichen; falls nicht → `RobotMotorPosition` anpassen
+**Umgesetzt: Joy-IT „Grab-It" (Robot02)** als `Arm3SegmentRotaryBase`.
+
+- [x] Physikalische Spezifikation dokumentieren (DOF, Achsen, Gelenkreihenfolge)
+  → im JSDoc von `robot/kinematics/Arm3SegmentRotaryBase.js`. 5 Achsen + Greifer:
+  Basis-Yaw, Schulter, Ellbogen, Handgelenk-Pitch, Handgelenk-Roll, Greifer.
+- [x] `robot/kinematics/Arm3SegmentRotaryBase.js` anlegen — nur die zwei Kinematik-Methoden
+- [x] RoundTrip-Tests für die neue Implementierung schreiben
+  (`test/Robot.GrabIt.RoundTrip.test.js`)
+- [x] Prüfen ob die 7 Motor-Slots ausreichen → **ja**: 6 Slots belegt
+  (`xMotor, alpha, beta, a, b, eMotor`), `c` bleibt frei. `RobotMotorPosition`
+  unverändert.
+- [x] In `KinematicsFactory` registriert (Bezeichner `arm3segmentrotarybase`,
+  Aliase `grabit` / `robot02`). Factory reicht jetzt das vollständige
+  `params`-Objekt als 4. Konstruktor-Argument durch (für `baseHeight`).
+
+**Offene Punkte / Annahmen (kein Blocker, aber vor Echtbetrieb zu klären):**
+- ⚠️ **5-DOF-Constraint:** `phi` (Hand-Azimut) ist an die Position gekoppelt
+  (= Basis-Drehung) und nicht frei. In der Inversen aus `atan2(y,x)` abgeleitet.
+- ⚠️ **Segmentlängen sind Schätzwerte** (l1=105, l2=98, l3=100, baseHeight=110 mm),
+  abgeleitet aus Reichweite (300 mm) / Höhe (420 mm). Vor Echtbetrieb am Arm
+  messen und per `ROBOT_KINEMATICS_PARAMS` setzen.
+- ⚠️ **Gelenkmodell** (Pitch/Roll am Handgelenk) folgt der Standardkonfiguration
+  dieser Arm-Klasse; gegen das physische Gerät / die Kalibrieranleitung prüfen.

 ---

--- a/doc/ToDo_9_HardwareFeedback.md
+++ b/doc/ToDo_9_HardwareFeedback.md
@@ -55,11 +55,11 @@ Quellen: [Serial Protocol](http://wiki.fluidnc.com/en/support/serial_protocol),

 ## Designentscheidungen (festgeschrieben)

-**B3 — Umkehr-Kinematik ist disambiguierbar.** Global nicht eindeutig, aber im Arbeitsraum
-dieses Roboters per **dokumentierter physikalischer Zusatzbedingung** auflösbar
-(z. B. „Ellbogen höher als Hand" bzw. „Ellbogen hinter der x-Achse"). `motorStateFromPorts()`
-bekommt eine feste Zweig-Wahl-Regel; die exakte Vorzeichen-Konvention wird beim Herleiten
-der Umkehrung gepinnt.
+**B3 — Umkehr-Kinematik.** *Aktualisiert nach der Analyse (ToDo_9a):* Die **Port→Motor**-Rückrechnung,
+die der Sync braucht, ist linear und **eindeutig** — keine Zweig-Wahl nötig. Die Ellbogen-oben/unten-
+Mehrdeutigkeit betrifft nur die **kartesische** Inverskinematik `calculateAngles3D()` (Pose →
+Gelenkwinkel), die der Sync nicht verwendet. Falls dort je eine Disambiguierung gebraucht wird,
+gilt die physikalische Zusatzbedingung („Ellbogen höher als Hand" bzw. „hinter der x-Achse").

 **B5 — Lockstep als abschaltbare Absicherung.** Durch die koordinierte Feedrate (ToDo_6a
 `correct`) treffen ohnehin alle Achsen *gleichzeitig* am nächsten Ziel ein — Lockstep ist
@@ -125,26 +125,42 @@ erstmals einen Promise zurückgeben/awaiten können.

 ---

-## Baustein für Paket 4 + 5: Rückabbildung Port → Motorwerte
+## Baustein für Paket 4 + 5: Rückabbildung Port → Motorwerte — ✅ DURCHGERECHNET
+
+> **Erledigt als Analyse.** Vollständige Herleitung: **`doc/ToDo_9a_PortRueckrechnung.md`**.
+> Verifikation: **`test/Robot.PortInverse.test.js`** (15 Tests grün).

 Beide folgenden Pakete brauchen denselben Baustein: aus den von GRBL gemeldeten
 `MPos`-Werten der drei Controller die **sieben Motorwerte des Roboters** rekonstruieren
 (`xMotor, alpha, beta, a, b, c, eMotor`).

-Das ist die **Umkehrung von `portValue()`** (`robot/TelnetSenderGRBL.js`). `portValue()`
-bildet *eine Roboter-Achse → einen GRBL-Port-Wert* ab, dabei koppeln einige Ports mehrere
-Achsen (z. B. der z-Port der Hand mischt `c, b, z, y`). Die Rückrichtung muss diese
-Kopplung **explizit auflösen** — sie ergibt sich nicht automatisch.
+**Ergebnis:** Für die produktive Verkabelung (`startRobot.js`) ist die Abbildung
+Motorwerte → gesendete GRBL-Achswerte **linear und eindeutig umkehrbar** — auf Port-Ebene
+gibt es **keine** Mehrdeutigkeit. `factorTurnLift`/`handOpenInMM` kommen in der produktiven
+Verkabelung gar nicht vor (nur in nicht-genutzten `portValue`-Zweigen).

- [ ] `motorStateFromPorts(portReadings)` definieren — algebraische Umkehrung von `portValue()`
-  - Eingang: pro Sender die gelesenen Port-Werte (`{x, y, z}` Base, `{a}` Elbow, `{c, e, b}` Hand)
-  - Ausgang: `{xMotor, alpha, beta, a, b, c, eMotor}`
-  - Grad→Rad zurückrechnen, `factorTurnLift`/`handOpenInMM` herausrechnen, gekoppelte Ports auflösen
-  - **Zweig-Wahl (B3):** wo die Lösung mehrdeutig ist, die dokumentierte physikalische
-    Zusatzbedingung anwenden (z. B. „Ellbogen höher als Hand"). Konvention im Code festhalten.
- [ ] **Round-Trip-Invariante** als Test: `portValue(motorStateFromPorts(p)) ≈ p`
-  - dasselbe Muster wie `test/Robot.Kinematics.RoundTrip.test.js`
-  - schützt die Umkehrfunktion gegen Drift gegenüber `portValue()`
+```js
+// D = 180/π ; r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} }
+xMotor = r.base.x
+alpha  = r.base.y / D
+beta   = (r.base.z + r.base.y) / D
+a      = r.elbow.x / D
+b      = r.hand.z / D
+c      = (r.hand.x + r.hand.z) / D
+eMotor = r.hand.y / D
+```
+
+> **B3 ist hier kein Thema.** Die Ellbogen-oben/unten-Mehrdeutigkeit steckt allein in der
+> kartesischen Inverskinematik `calculateAngles3D()` (Pose → Gelenkwinkel). Der Sync nutzt
+> diese Richtung nicht — er geht `MPos → Motorwerte → Vorwärtskinematik → Pose`, beide
+> Schritte eindeutig. Der gesamte Sync-Pfad ist damit eindeutig.
+
+Offen für die spätere **Umsetzung** (Paket 4, nicht mehr Analyse):
+
+- [ ] `motorStateFromPorts()` aus der Analyse in den Produktiv-Code heben (Ort: Sender oder
+  Kinematik-Helfer) und im Sync verdrahten
+- [ ] **Round-Trip-Invariante** als Dauer-Test mitführen: `portValue(motorStateFromPorts(p)) ≈ p`
+  — schützt gegen Drift, falls sich die Verkabelung in `startRobot.js` ändert

 > Hinweis: Gelesen wird auf dem **aktiven** Sender `TelnetSenderGRBL` (im `data`-Handler,
 > siehe Paket 1) — nicht auf `FluidNCClient.js`.
@@ -167,7 +183,7 @@ sonst nicht, wo der Roboter physisch wirklich steht.
  im bisher synchronen Dispatch-Pfad.
 - [ ] Ablauf des Sync:
  1. an alle drei Sender einmalig `?` senden, je `MPos` aus der Antwort parsen (Paket 3)
-  2. `motorStateFromPorts(...)` → sieben Motorwerte rekonstruieren (Baustein oben, inkl. B3-Zweigwahl)
+  2. `motorStateFromPorts(...)` → sieben Motorwerte rekonstruieren (Baustein oben — linear/eindeutig, ToDo_9a)
  3. diese auf den Roboter schreiben: `robot.xMotor/alpha/beta/a/b/c/eMotor = …`
  4. **Vorwärtskinematik** anstoßen: `robot.calculatePositionFromMotorAngles()`
     → füllt `robot.x/y/z` und `phi/theta/psi` aus den Hardwarewerten
--- a/doc/ToDo_9a_PortRueckrechnung.md
+++ b/doc/ToDo_9a_PortRueckrechnung.md
@@ -0,0 +1,130 @@
+# ToDo 9a — Umkehr-Rechnung GRBL-Port → Motorwerte (Herleitung)
+
+> **Status: durchgerechnet und verifiziert.**
+> Baustein für ToDo_9 / Paket 4 (Sync-Command). Verifikation:
+> `test/Robot.PortInverse.test.js` (15 Tests grün).
+
+## Zweck
+
+Der Sync-Command (ToDo_9, Paket 4) liest die echten Achs-Positionen (`MPos`) der drei
+GRBL/FluidNC-Controller und muss daraus die **sieben Motorwerte** des Roboters
+rekonstruieren: `xMotor, alpha, beta, a, b, c, eMotor`. Diese werden dann auf den Roboter
+geschrieben und per **Vorwärtskinematik** (`calculatePositionFromMotorAngles()`) in die
+Pose `x/y/z/phi/theta/psi` überführt.
+
+Dieses Dokument leitet die Umkehrung her und hält das (verifizierte) Ergebnis fest.
+
+## Kernergebnis (vorweg)
+
+**Für die produktive Verkabelung ist die Abbildung Motorwerte → gesendete GRBL-Achswerte
+linear und eindeutig umkehrbar.** Es gibt auf Port-Ebene **keine** Mehrdeutigkeit.
+
+> Die B3-Mehrdeutigkeit (Ellbogen oben/unten) steckt ausschließlich in der **kartesischen**
+> Inverskinematik `calculateAngles3D()` (Pose → Gelenkwinkel). Der Sync nutzt diese Richtung
+> **nicht** — er geht `MPos → Motorwerte → Vorwärtskinematik → Pose`, und beide Schritte sind
+> eindeutige Funktionen. **Damit ist der Sync-Pfad als Ganzes eindeutig.**
+
+---
+
+## Ausgangslage
+
+### Produktiv-Verkabelung (`startRobot.js`)
+
+```js
+new TelnetSenderClass(baseIP,  2300, 'x', 'y', 'z')      // Base
+new TelnetSenderClass(elbowIP, 5000, 'a', null, null)    // Elbow
+new TelnetSenderClass(handIP,  5000, 'c', 'e', 'b')      // Hand
+```
+
+### Bedeutung der Motor-Felder (`RobotMotorPosition`)
+
+| Feld | Bedeutung |
+|---|---|
+| `x` | `xMotor` (Schulterposition auf X-Schiene, **mm**) |
+| `y` | `alpha` (Schulterwinkel, **rad**) |
+| `z` | `beta`  (Unterarm-Neigung, **rad**) |
+| `a` | `a` (Ellbogen-Dreher, **rad**) |
+| `b` | `b` (Handgelenk-Knick, **rad**) |
+| `c` | `c` (Hand-Dreher, **rad**) |
+| `e` | `eMotor` (Greifer) |
+
+Mit `D = 180/π` (Grad-Faktor).
+
+### Was jeder Controller real sendet (`execCommand`, Produktiv-Verkabelung)
+
+Aus dem Sende-Pfad (`robot/TelnetSenderGRBL.js`) ergeben sich genau **sieben** GRBL-Achswerte:
+
+| Controller | GRBL-Achse | gesendeter Wert | in Motorwerten |
+|---|---|---|---|
+| **Base**  | `x` | `xMotor`              | `xMotor` |
+| **Base**  | `y` | `alpha·D`             | `α·D` |
+| **Base**  | `z` | `(beta − alpha)·D`    | `(β − α)·D` |
+| **Elbow** | `x` | `a·D`                 | `a·D` |
+| **Hand**  | `x` | `(c − b)·D`           | `(c − b)·D` |
+| **Hand**  | `y` | `eMotor·D`            | `e·D` |
+| **Hand**  | `z` | `b·D`                 | `b·D` |
+
+Drei Ports koppeln je zwei Motorwerte (`base.z`, `hand.x`), aber **jeder gekoppelte Wert
+wird mit einem unabhängig gelesenen Wert kombiniert** (`alpha` bzw. `b`). Das System ist
+damit *unteres Dreieckssystem* → trivial auflösbar.
+
+---
+
+## Herleitung der Umkehrung
+
+Gegeben die GRBL-Readings `base.{x,y,z}`, `elbow.{x}`, `hand.{x,y,z}` (Grad bzw. mm):
+
+```
+xMotor = base.x                          // direkt
+alpha  = base.y / D
+beta   = (base.z + base.y) / D           // base.z = (β−α)·D  ⇒  β = base.z/D + α
+a      = elbow.x / D
+b      = hand.z / D
+c      = (hand.x + hand.z) / D           // hand.x = (c−b)·D  ⇒  c = hand.x/D + b
+eMotor = hand.y / D
+```
+
+Als Funktion (siehe Test, kann später 1:1 in Paket 4 übernommen werden):
+
+```js
+function motorStateFromPorts(r) {           // r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} }
+  const D = 180 / Math.PI;
+  return {
+    xMotor: r.base.x,
+    alpha:  r.base.y / D,
+    beta:  (r.base.z + r.base.y) / D,
+    a:      r.elbow.x / D,
+    b:      r.hand.z / D,
+    c:     (r.hand.x + r.hand.z) / D,
+    eMotor: r.hand.y / D,
+  };
+}
+```
+
+---
+
+## Verifikation
+
+`test/Robot.PortInverse.test.js` — 15 Tests, fünf repräsentative Motorzustände
+(Nullstellung, gemischt, negative/große Winkel, gekoppelt `c≈b`):
+
+- **A) Exaktheit gegen `portValue`** (volle Präzision): Rückgewinnung auf 1e-9 genau.
+- **B) Gegen den echten Sende-Pfad `execCommand`** (inkl. 2-Dezimal-Rundung der
+  G-Code-Werte): Rückgewinnung innerhalb der Rundung (Winkel < 1e-3 rad, `xMotor` < 0.02 mm).
+- **C) Voll-Kette Sync**: `Ports → motorStateFromPorts → calculatePositionFromMotorAngles`
+  liefert dieselbe Pose `x/y/z/phi/theta/psi` wie die Original-Motorwerte (auf 1e-6).
+
+---
+
+## Konsequenzen für ToDo_9 / Paket 4
+
+1. **Keine Zweig-Wahl auf Port-Ebene nötig.** Die frühere Annahme (B3-Disambiguierung im
+   Baustein) trifft auf die Port-Rückrechnung **nicht** zu — sie ist linear und eindeutig.
+2. **Rundung beachten.** `MPos` kommt mit endlicher Präzision; gekoppelte Werte (`beta`, `c`)
+   summieren zwei gerundete Ports → Toleranz im Sub-Promille-Bereich, unkritisch.
+3. **Achszahl je Controller.** FluidNC meldet `MPos` für **alle** in seiner Config definierten
+   Achsen. Genutzt werden nur: Base `x,y,z` · Elbow `x` · Hand `x,y,z`. Beim Parsen die
+   übrigen (falls vorhanden) ignorieren.
+4. **Verkabelungs-Abhängigkeit.** `motorStateFromPorts()` gilt für die aktuelle Verkabelung.
+   Ändert sich `startRobot.js` (andere Port-Zuordnung), muss die Umkehrung mitgezogen werden —
+   der Round-Trip-Test (`portValue(motorStateFromPorts(p)) ≈ p`) schützt davor.