# Koordinatensystem, Roboter-Aufstellung & Nullstellung

Diese Datei beschreibt
1. das Koordinatensystem,
2. wie der Roboter darin steht,
3. die angestrebte **ideale Nullstellung** und
4. die Schritte, um den Driver auf diese Konvention zu bringen (**Weg 2: Modell auf −Y**).

> **Status:** **Phase 1 (y-Flip) ist umgesetzt und am Roboter verifiziert.** α/β werden
> jetzt von **−y** aus gemessen (α=0 → Arm zeigt nach −y), passend zu robot.json und
> appRobotHoming. Offen ist **Phase 2** (Handgelenk-/Finger-Nullstellung, B/C).

---

## 1. Koordinatensystem

Aus robot.json (`coordinateSystem`): **rechtshändig**, Längen **mm**, Winkel **Grad**.

| Achse | Richtung   | Bedeutung im Aufbau                          |
|-------|------------|----------------------------------------------|
| x     | rechts     | entlang der Linearschiene                    |
| y     | „backward" | Arm-Arbeitsrichtung: Arm streckt nach **−y** |
| z     | oben       | Höhe                                         |

**Seitenansicht** (Blick entlang +x, also die −y/z-Ebene):

```
  z
  ▲
  |   ■═══════════════●===========●---o   Arm1 ═══ , Arm2 === und Finger --- waagerecht
  |   Schulter (Ursprung, z=0)             ausgestreckt → Fingerspitze o
  └───────────────────────────────►  −y  (Arbeitsrichtung)
```

In der −y/z-Seitenansicht liegen beide Handgelenk-Achsen **end-on** (als Punkt ●), weil
sie entlang x verlaufen:
- linkes ● = **a-Achse** (Unterarm-Dreher, zwischen Ellbogen und Arm2)
- rechtes ● = **b-Achse** (Hand-Knick-Achse)

**Draufsicht** (Blick von oben auf −z; x nach oben, −y nach rechts):

```
  x
  ▲          │                  │
  │     ■════╪═══ Arm1 ═════════╪══ Arm2 ══----o     o = Fingerspitze (y ≈ −590)
  │   Schulter                                  
  └────────────────────────────────────────────►  −y

  │ = a-Achse (Ellbogen↔Arm2) bzw. b-Achse (Hand-Knick).
  Bei a=0 laufen BEIDE parallel zur x-Achse (hier als senkrechte Striche).
```

---

## 2. Wie der Roboter darin steht

Gelenk-Kette (robot.json `links`):
**Board → Base (Schiene x) → Arm1 (Oberarm) → Ellbow → Arm2 (Unterarm) → Hand → Palm → Finger.**

- **Linearschiene** entlang x; die Base fährt darauf (`xMotor`, mm).
- **Schultergelenk** (Base→Arm1, `variable y`): Drehung in der y-z-Ebene → **α**.
  Bei Gelenkwinkel 0 zeigt Arm1 entlang **−y** (`skeleton.to = [0,-250,0]`).
- **Ellbogen** (`variable z`) → **β**; **Unterarm-Dreher** (`variable a`) → **a**;
  **Handgelenk-Knick** (`variable b`) → **b**; **Hand-Roll** (`variable c`) → **c**;
  **Greifer** (`variable e`) → **e**.
- **z = 0 im Driver-Modell = Schulterachse.** (In der Welt liegt sie ~45 mm über dem
  Brett — robot.json Joint1-Origin z=45 —, der Driver rechnet schulter-relativ.)
- Armlängen (aus `links` abgeleitet): **l1 = 250** (Oberarm), **l2 = 250** (Unterarm),
  **l3 ≈ 90** (Hand). Σ ≈ **590 mm**.

---

## 3. Ideale Nullstellung (Grundstellung)

**Definition (deine Vorgabe):**

1. **Arm1 entlang −y** (Schulter-/y-Gelenk α) → **α = 0**.
2. **Ellbogen so angewinkelt, dass auch Arm2 entlang −y** (z-Gelenk β) → **β = 0**
   (β ist absolut, also auch direkt entlang −y, nicht relativ zum Oberarm).
3. **a-Achse so gedreht, dass die Hand-Knick-Achse genau in x-Richtung läuft** → **a = 0**.

Diese drei Bedingungen erfüllt der Driver **nach Phase 1 bereits** (verifiziert, s.u.):

| Achse  | Nullwert | Bedeutung                                   | Status                  |
|--------|----------|---------------------------------------------|-------------------------|
| X      | (frei)   | Schienenposition `xMotor` in mm             | —                       |
| Y (α)  | **0°**   | Oberarm waagerecht entlang −y               | ✅ Phase 1              |
| Z (β)  | **0°**   | Unterarm waagerecht entlang −y (gestreckt)  | ✅ Phase 1              |
| A (a)  | **0°**   | Hand-Knick-Achse ∥ x                        | ✅ (Code erfüllt es)   |
| B (b)  | 0° (Ziel) | Hand gerade — **derzeit ist gerade = 180°** | ⏳ Phase 2             |
| C (c)  | 0° (Ziel) | kein Hand-Roll — **derzeit neutral ≠ 0**    | ⏳ Phase 2             |
| E (e)  | **0**    | Greifer geschlossen / Referenz              | —                       |

→ Resultierende Fingerspitze (α=β=a=0, gerade Hand): **(xMotor, −(l1+l2+l3), 0) ≈ (x, −590, 0)**.
(Beobachtet ~−550; Differenz steckt in der l3-Ableitung / Resthandstellung.)

---

## 4. Ist-Zustand (nach Phase 1) vs. Ideal

| Aspekt                       | nach Phase 1 (jetzt)        | Ideal (nach Phase 2) |
|------------------------------|-----------------------------|----------------------|
| α=0 zeigt nach               | **−y** ✅                   | −y                   |
| β=0 Unterarm                 | **−y** ✅                   | −y                   |
| a=0 Hand-Knick-Achse         | **∥ x** ✅                  | ∥ x                  |
| G92 der Grundstellung → y    | **≈ −590** ✅               | ≈ −590               |
| gerade Hand                  | **b = 180°**                | **b = 0°**           |
| neutraler Roll               | **c: ψ = 90° − C** (posenabh.) | **c = 0°**        |

Verifiziert per FK: `FK(α=0, β=0, gerade Hand) → (0, −590, 0)`; bei `a=0` bleibt
`Fingerspitze.x = xMotor` konstant, während b variiert (Knick in der y-z-Ebene).

---

## 5. Schritte (Weg 2)

Reihenfolge nach Workflow: **erst Tests (rot), dann Code, dann grün.**

### Phase 1 — y-Flip ✅ ERLEDIGT (am Roboter verifiziert)

Umgesetzt:
- **Spiegelung an der x-z-Ebene** in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`): die
  interne Mathematik (`_ikPlusY`/`_fkPlusY`) rechnet weiter in +y, die öffentlichen
  Methoden spiegeln die Workspace-Pose (y, pY, φ, ψ; θ bleibt) → α=0 zeigt nach −y.
- **G28** (Home) auf −y umgestellt **und Singularität behandelt**: die voll ausgestreckte
  Stellung (`|y| = l1+l2+l3`) ist eine Handgelenk-Singularität, in der die IK `a`/`c` nicht
  bestimmen kann (Müll wie `a=135°, c=45°` → Finger schräg). G28 setzt dort die Motorwerte
  **direkt** (`alpha=beta=a=c=0`, `b=π` = gerade Hand) und füllt die Pose per FK.
  **Interim:** G28 lässt den Greifer (`e`/`eMotor`) **unangetastet** — sonst ergäbe
  `eMotor = e−b−c` bei `b=π` den Wert −180° → Finger-Anschlag-Slam (Phase 2 behebt das mit `b=0`).
- **Tests:** `test/Robot.Kinematics.NegativeY.test.js` (Grundstellung −590, Homing-Pose
  in −y, Round-Trip in −y, a=0 → Knick-Achse ∥ x).
- **Migration:** `Robot.02_UpperArm` und der G28-Test auf −y umgestellt.
- **Doku:** `doc/Info_G92.md` Y/Z (und C/A nach der Spiegelung) aktualisiert.

### Phase 2 — Handgelenk-/Finger-Nullstellung (B, C, Greifer) — OFFEN

> **Voraussetzung (User):** Erst die Finger visualisieren/prüfen — dort werden noch Fehler
> vermutet. **a-Achse ist bereits korrekt** (a=0 → Knick-Achse ∥ x). Phase 2 betrifft
> **B (Knick)**, **C (Roll)** und die **Greifer-Kopplung**.

**Ziel-Konvention:** gerade Hand → **B = 0°** (statt 180°); neutraler Roll → **C = 0°**;
Greifer-Kopplung konsistent und gegen die echte Mechanik kalibriert.

#### Vorab-Erkenntnis aus dem Code (wichtig!)

Die b/c/e-Konvention ist an **mehreren** Stellen kodiert, die **gemeinsam** geändert werden
müssen. **Invariante:** solange die Hardware-Nullpunkte nicht neu kalibriert werden, müssen
die an FluidNC gesendeten Port-Werte **gleich bleiben** — eine reine Modell-Umbenennung darf
die Hardware-Bewegung nicht verändern.

Fundstellen:

| Datei / Stelle | aktuelle Kodierung |
|----------------|--------------------|
| `Arm3SegmentLinearX._fkPlusY` | `vHand = rotate(vecUnterarm, n, b)`; `psi = c − acos(−n.z)` → **b=π = gerade** |
| `Arm3SegmentLinearX._ikPlusY` | `b = acos(cosB)` (∈[0,π]); `c = acos(cosC) + psi` → **c hat posenabh. Offset** |
| `Arm3SegmentLinearX.gripperMotorFromOpening` | `eMotor = e − b − c` (b,c in **rad**) — **Greifer-Kopplung #1** |
| `RobotController` G92/M92 | `b = B/D`, `c = C/D`, `eMotor = gripperMotorFromOpening(e)` |
| `RobotController` M1 | `b += B`, `c += C` (relativer Motor-Jog) |
| `RobotController` G28 | `b = π`, `c = 0` (Phase 1) → nach B-Umstellung auf `b = 0` ändern |
| `portInverse.js` | `b = hand.z/D`, `c = hand.x/D + b` (Port→Motor, Hardware-Sync) |
| `TelnetSenderGRBL.execCommand` / `portValue` | Hand-Ports: `z = b·D`, `x = (c−b)·D`; **e-Port mit `factorTurnLift=1.2`** — **Greifer-Kopplung #2** |

#### Aufgaben

1. **Finger visualisieren** (User) → Soll-Bild, gegen das kalibriert wird.

2. **Greifer-Kopplung — aktuell aktiv (identifiziert):** Bei der realen Verkabelung
   (`hand.axes = ['c','e','b']`) liegt der Greifer auf dem **y-Port**. Gesendet wird daher
   `mNew.e · D = eMotor · D = (e − b − c) · D` — die Kopplung steckt in
   `gripperMotorFromOpening` (→ `eMotor`), der Sender hängt nur noch `·180/π` dran.
   - Die **x-Port-Variante** (`e + 1.2·b·D − c·D`, mit `factorTurnLift = 1.2`) greift nur
     bei anderer Verkabelung → **derzeit toter Pfad**. (`factorOpenTurn = 1.92` ungenutzt.)
   - **Folge / Slam:** bei `b = π` (Phase-1-„gerade Hand") wird `eMotor = e−b−c = −π → −180°`
     an den Finger-Motor gesendet → er fährt an den Anschlag und verdreht über die Sehne die
     ganze Hand. Phase 2 (`b = 0` = gerade) behebt das automatisch (`eMotor = 0`).
   - Aufgabe: Kopplung gegen die echte Sehnenmechanik validieren, toten x-Port-Pfad +
     `factorOpenTurn` aufräumen, **Vorzeichen** je nach Motor-Verkabelung prüfen.

3. **B-Konvention (gerade = 0°).**

   **Verifizierter Mismatch Homing ↔ Driver:** Das appRobotHoming meldet für die **gerade**
   Hand **B ≈ 0** (Messung: `B=-6.92`), der Driver rechnet aber mit **gerade = b = 180°**.
   Der Driver interpretiert das empfangene `b≈0` daher als Knick `180−0 = 180°` (fast voll
   zurückgeklappt) → im Modell zeigt der Finger nach **+y (rückwärts)** statt −y. D.h. **nach
   dem Homing ist der interne Hand-Zustand des Drivers falsch** (gefaltet), was Folge-Moves
   verfälscht. Das ist das beobachtete „Driver interpretiert als B=180".
   → Konsequenz: Driver auf **B=0=gerade** umstellen (passt dann ohne Umrechnung zum Homing),
   **oder** appRobotHoming sendet `B+180`. (C ist konsistent: Homing `C=90`=flach = Driver `c=90`=flach,
   also `C=0`=aufrecht — kein Versatz.)

   Umstellung durchgängig:
   - FK/IK in `Arm3SegmentLinearX` (b-Definition / acos-Zweig),
   - `gripperMotorFromOpening` nachziehen (behebt auch den G28-Greifer-Slam),
   - G92-Eingabe (`b = B/D`) + M1 + G28,
   - `portInverse.js` (Umkehrung),
   - **Sender-Formeln so kompensieren, dass die FluidNC-Ports unverändert bleiben** —
     ODER bewusst die Hardware-Nullpunkte neu kalibrieren (Entscheidung dokumentieren).

4. **C-Nullpunkt (neutral = 0°).** Der `c↔ψ`-Bezug ist **posenabhängig**
   (`ψ = acos(cos β · sin a) − c`). Ein konstantes `c=0=neutral` ist **nicht global** möglich,
   ohne die Hand-Parametrierung zu ändern. Bewerten: c als reinen Gelenkwinkel führen
   (Offset herausrechnen) oder die ψ-Definition anpassen.

5. **l3-Ableitung korrigiert** ✅ (`RobotConfig.js`): `l3` kommt jetzt aus **Hand + Finger**
   (`|Hand.to[1]| + |FingerA.to[1]|` = 35 + 60 = **95**) statt aus dem Ellbogen-Versatz (90).
   Zusätzlich sind `kinematics.l1/l2/l3` in robot.json **explizit überschreibbar** (Vorrang
   vor der Ableitung) — zum Kalibrieren auf die gemessene Reichweite.
   Reichweite damit 595 — passt zur aktuellen Hardware (60 mm Finger). Die früher beobachteten
   ~550 stammten von **kürzeren 50 mm-Greifern**. Bei Greifer-Wechsel `kinematics.l3` per
   Override anpassen (oder Finger-Geometrie in robot.json pflegen).

6. **Tests + Doku** nachziehen: Round-Trip mit neuer Konvention, Greifer-Kopplung,
   G92-Referenztabellen in `Info_G92.md`, sowie diese Datei.

#### Ansatz-Entscheidung (vor Umsetzung)

- **Klein/lokal:** nur die **G92-Eingabe** umrechnen (appRobotHoming sendet B=0/C=0 für
  gerade/neutral, Driver mappt intern auf die alte Konvention). Wenig Risiko, aber das
  interne Modell bleibt „unsauber".
- **Groß/sauber:** interne Konvention durchgängig umstellen (alle Fundstellen oben) mit
  Hardware-Port-Invariante. Sauberes All-Zero-Home, aber koordinierter Eingriff.

#### Verifikation

Jede B/C/Greifer-Änderung gegen **Visualisierung UND einen Hardware-Test** (eine Achse
isoliert) prüfen — das Modell allein genügt hier nicht, weil es um die Hardware-Abbildung geht.

### Verifikation (Definition of Done)

- **Phase 1 (erfüllt):** G92 der Grundstellung → Driver `y ≈ −590, z ≈ 0`; appRobotHoming
  sendet die gemessenen α/β/a **direkt** (ohne Spiegelung); **G28 fährt sauber gestreckt
  nach −y** (a=0, kein Singularitäts-Müll); volle Suite grün.
- **Phase 2 (Ziel):** Grundstellung mit **allen** Gelenkwinkeln 0 (inkl. B=C=0); Greifer-
  Kopplung vereinheitlicht; Finger visuell korrekt.

---

## Anhang: Stand der Kinematik

- **y-Flip (Phase 1):** Spiegelung in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`, genutzt von
  `calculateAngles3D` und `calculatePositionFromMotorAngles`). Am Roboter bestätigt.
- **G28-Singularität (Phase 1):** voll ausgestreckt setzt `RobotController` die Motorwerte
  direkt (statt der singulären IK) → Finger sauber entlang −y.
- **atan2-Fix** in der IK (`gamma = Math.atan2(pZ, pY)`): macht die interne IK für −y
  mathematisch korrekt — Voraussetzung des y-Flips.
- **Winkel-Konventionen** (Y/Z/A/B/C/E) sind in [doc/Info_G92.md](Info_G92.md) dokumentiert
  und nach Phase 1 aktualisiert.