# Homing 8 – Übergabe an appRobotDriver (G92-Konvention)

> Technische Doku zur Konvertierung des Homing-Ergebnisses
> (FK-State aus [`4b_revolute_angle.py`](../scripts/4b_revolute_angle.py) /
> [`5_pose_estimation.py`](../scripts/5_pose_estimation.py))
> in die G92-Konvention von **appRobotDriver**.
>
> Umgesetzt in [`server/server.js`](../server/server.js) → Funktion `fkStateToDriverG92()`,
> aufgerufen im `POST /api/homing/send-state`-Handler.
>
> Vollständige Driver-Konvention: [`appRobotDriver/doc/Info_G92.md`](../../appRobotDriver/doc/Info_G92.md).

---

## Das Problem: zwei verschiedene Winkel-Konventionen

Die Homing-Pipeline (4b / 5_pose_estimation) liefert Gelenkwinkel im
**FK-Koordinatensystem von `robot.json`** (forward kinematics, Blender-Hierarchie).
Der appRobotDriver erwartet in `G92` eine **eigene Konvention**, die sich an
physischen Konfigurationen und Motor-Nullpunkten orientiert — nicht an der FK-Nulllage.

Für X (Schiene), Y (Oberarm), A (Unterarm-Dreher) stimmen die Konventionen überein.
Für **B, C und Z** gibt es definierte Verschiebungen, die vor dem Senden umgerechnet
werden müssen.

---

## Befehlsformat G92

```
G92 X<mm> Y<°> Z<°> A<°> B<°> C<°> E<mm>
```

Der Befehl setzt am Driver die Motorposition **ohne Bewegung** (intern M92 = Homing).
Fehlende Achsen werden weggelassen; der Driver lässt sie unverändert.

---

## Umrechnungstabelle FK → Driver

| Achse | FK-State (Homing)                          | Driver G92 erwartet                        | Umrechnung               |
|-------|--------------------------------------------|--------------------------------------------|--------------------------|
| X     | xMotor in mm                               | xMotor in mm                               | — (identisch)            |
| Y     | Oberarm-Winkel α, absolut (0=waagerecht)   | α absolut (0=waagerecht, 90=hoch)          | — (identisch)            |
| Z     | Ellbogen-Knick **relativ** zu Arm1         | Unterarm-Winkel β **absolut** (wie Y)      | `Z = Y + z_relativ`      |
| A     | Unterarm-Dreher (Arm2-Rotation um -y)      | Unterarm-Dreher (Roll um Unterarm-Achse)   | — (identisch)            |
| B     | FK `b=0` = gerade Hand (kein Knick)        | `B=180°` = gerade Hand, `B=0°` = zurück   | `B = 180 − b`            |
| C     | FK `c=0` = neutraler Palm-Roll             | `C=90°` = neutral (ψ=0°)                  | `C = c + 90`             |
| E     | Finger-Öffnung in mm (rein geometrisch)    | Finger-Öffnung in mm (Sehnen-Kopplung im Driver) | — (identisch)        |

---

## Detailerklärung je Achse

### Z — Ellbogen: relativ vs. absolut

`4b_revolute_angle.py` misst den Ellbogen-Winkel **relativ zu Arm1** (FK-Variable z =
Rotation des Ellbow-Gelenks gegenüber der Arm1-Nulllage). Der Driver interpretiert Z
dagegen als **absoluten** Unterarm-Winkel zur Horizontalen — genauso wie Y den
Oberarm beschreibt.

```
Z_Driver = Y_FK + z_relativ_FK
```

Intern rechnet der Driver für die FluidNC-Weiterleitung wieder zurück:
`FluidNC-z = (β − α) × D`, d.h. relativer Motor-Winkel.

> **Typische Falle:** z_relativ direkt als Z senden. Bei kleinen Y-Winkeln (Y ≈ 0) ist
> der Fehler kaum merklich; bei Y = 86° beträgt er 86°.

---

### B — Hand-Knick: 180°-Dreher

Im `robot.json`:
```json
"Hand": {
  "jointToParent": { "axis": [1, 0, 0], "rotation": [0, 0, 0], "variable": "b" }
}
```

Die FK-Nulllage (`b = 0`) bedeutet: Hand verlängert Arm2 geradeaus — kein Knick.
Der Driver definiert dagegen:

| B (G92) | physischer Knick Unterarm↔Hand |
|---------|-------------------------------|
| 0°      | 180° (Hand voll zurückgeklappt) |
| 90°     | 90° (Hand ⊥ Unterarm)          |
| 180°    | 0° (Hand gerade)               |

Ohne Umrechnung wird `b ≈ 0` (gerade Hand) als `B = 0°` gesendet → Driver klappt
die Hand voll zurück → „rückwärts haltende Hand-Stellung".

```
B_Driver = 180 − b_FK
```

---

### C — Palm-Roll: 90°-Offset

Im `robot.json`:
```json
"Palm": {
  "jointToParent": { "axis": [0, -1, 0], "rotation": [0, 0, 0], "variable": "c" }
}
```

Die FK-Nulllage (`c = 0`) entspricht keiner Rotation des Palm-Gelenks = neutraler Roll.
Der Driver definiert `C = 90°` als neutral (ψ = 0°):

| C (G92) | Hand-Roll ψ |
|---------|-------------|
| 0°      | −90°        |
| 90°     | 0° (neutral) |
| 180°    | +90°        |

```
C_Driver = c_FK + 90
```

> **Vorzeichen prüfen:** Falls nach dem Fix die Palm-Rotation spiegelverkehrt erscheint,
> lautet die Formel `C = 90 − c_FK` (Vorzeichen des physischen Motors umgekehrt).
> Hängt von der Einbaurichtung des Palm-Servos ab.

---

### E — Greifer: Sehnen-Kopplung

E wird als reine Finger-Öffnung in mm übergeben (FK und Driver identisch — keine
Winkelumrechnung). Der Driver berechnet den tatsächlichen Motor-Wert intern:

```
eMotor = E − b − c     (b, c in Radiant!)
```

Die Sehnen-Kopplung kompensiert, dass Handgelenk-Knick (B) und Palm-Roll (C)
an der Greifer-Sehne ziehen. Sind B und C korrekt gesetzt, stimmt die Kompensation
automatisch — E selbst braucht nicht angepasst zu werden.

---

## Implementierung

**`server/server.js`** — Funktion `fkStateToDriverG92()`:

```js
function fkStateToDriverG92(s) {
  const d = { ...s };
  if (d.b != null) d.b = 180 - d.b;          // Hand-Knick: 180°-Dreher
  if (d.c != null) d.c = d.c + 90;            // Palm-Roll:  +90° Offset
  if (d.z != null && d.y != null) d.z = d.y + d.z;  // Ellbogen: relativ → absolut
  return d;
}
```

Aufruf im `POST /api/homing/send-state`-Handler:

```js
const gcode = buildG92(fkStateToDriverG92(state));
```

Null-Werte (unbeobachtbare Achsen) bleiben null → werden von `buildG92` weggelassen →
Driver lässt die entsprechenden Achsen unverändert.

---

## Kontext: Woher kommen die FK-Werte?

```
4b_revolute_angle.py × N   →  accumulated_state {x, y, z, a, b}
                               (Hand/b schlägt fehl, wenn keine Marker am Hand-Link)
        ↓ (falls Hand/b fehlt)
5_pose_estimation.py        →  movements {x,y,z,a,b,c,e} als FK-Winkel (Bundle-Adjustment)
        ↓
homingOrchestrator.js       →  finalState (flaches {x,y,z,a,b,c,e})
        ↓
POST /api/homing/send-state
        ↓
fkStateToDriverG92()        →  Konvertierung FK → Driver
        ↓
buildG92()                  →  "G92 X… Y… Z… A… B… C… E…"
        ↓
sendGcode()  →  appRobotDriver (WebSocket)
```

Im aktuellen `robot_1781069752019.json` hat der Hand-Link **keine eigenen Marker**;
der b-Wert kommt daher immer aus `5_pose_estimation.py` (Bundle-Adjustment über
die FingerA/FingerB-Marker, die durch die kinematische Kette Hand→Palm→Finger
indirekt b einschränken).