# Koordinatensystem, Roboter-Aufstellung & Nullstellung

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

> **Hintergrund / Problem:** Das Kinematik-Modell (`Arm3SegmentLinearX`) misst die
> Armwinkel aktuell von **+Y** (α=0 → Arm zeigt nach +y). Der reale Roboter steht und
> arbeitet aber in **−Y** (robot.json: `Arm1.skeleton.to = [0,-250,0]`,
> `coordinateSystem.y = "backward"`). Dadurch landet eine fast-waagerechte
> Grundstellung im Driver bei **y ≈ +590** statt **−590** — Modell und Hardware sind an
> der y-Achse gespiegelt.

---

## 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):

  z
  ▲
  |   ■══════════════════════●   Arm waagerecht ausgestreckt → Fingerspitze
  |   Schulter (Ursprung, z=0)
  └───────────────────────────────►  −y  (Arbeitsrichtung)

Draufsicht (Blick von oben, −z):

  −y ▲                ● Fingerspitze (Grundstellung y ≈ −590)
     │                │
     │                │  Arm (Ober- + Unterarm, gestreckt)
     │                │
  ───┼────────────────■─────────────►  x (Schiene)
     │                Schulter
  +y │
```

---

## 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:** Arm **waagerecht voll ausgestreckt entlang −y**, Hand **gerade** in
Verlängerung des Unterarms, Greifer geschlossen.

**Ziel:** in dieser Stellung sind **alle Gelenkwinkel = 0°**.

| Achse  | Ideal-Wert | Bedeutung                                |
|--------|-----------|-------------------------------------------|
| X      | (frei)    | Schienenposition `xMotor` in mm           |
| Y (α)  | **0°**    | Oberarm waagerecht entlang −y             |
| Z (β)  | **0°**    | Unterarm waagerecht entlang −y (gestreckt) |
| A (a)  | **0°**    | kein Unterarm-Dreh                        |
| B (b)  | **0°**    | Hand gerade                               |
| C (c)  | **0°**    | kein Hand-Roll                            |
| E (e)  | **0**     | Greifer geschlossen / Referenz            |

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

---

## 4. Ist-Zustand vs. Ideal

| Aspekt                     | aktuell (Modell +Y)       | ideal (−Y)        |
|----------------------------|---------------------------|-------------------|
| α=0 zeigt nach             | +y  (FK: y=+410)          | −y  (y=−410)      |
| Grundstellung Y (α)        | ≈175° (=180−4,5)          | **0°**            |
| gerade Hand B (b)          | 180°                      | **0°**            |
| neutraler Roll C (c)       | 90° (posenabhängig)       | **0°**            |
| G92 der Grundstellung → y  | ≈ +590                    | **≈ −590**        |

Verifiziert per FK: `α=β=0 → y=+410`, `α=β=180 → y=−410`. Die +Y/−Y-Spiegelung
entspricht `α→180−α, β→180−β`.

---

## 5. Schritte, um das zu erreichen (Weg 2)

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

### Phase 1 — y-Flip (behebt den gemeldeten Bug)

1. **Tests schreiben** (zunächst rot):
   - G92 der Grundstellung (α=β=a=b=c=0) → erwartet **y ≈ −590** (statt +590).
   - Round-Trip: `IK(x=0, y=−550, z, φ, θ)` → `FK` → identische Pose zurück.
2. **`Arm3SegmentLinearX` umstellen** (α/β von −y aus messen):
   - **FK** (`calculatePositionFromMotorAngles`): y-Komponenten von `vecBizeps` und
     `vecUnterarm` negieren; Handgelenk-Vektoren (`n`, `vHand`) und `φ = atan2(vHand.y,…)`
     konsistent nachziehen.
   - **IK** (`calculateAngles3D`): `pY`, `gamma = atan2(pZ, pY)`, die `n`-Konstruktion
     und `φ` entsprechend spiegeln.
3. **Bestehende +Y-Tests auf −Y migrieren** (erwartete y-Werte spiegeln).
4. **`doc/Info_G92.md`** aktualisieren: Y/Z-Konvention auf „0° = waagerecht entlang −y".
5. Suite grün; Round-Trip + Grundstellung grün.

### Phase 2 — Handgelenk auf Null (optional, für echte All-Zero-Grundstellung)

6. **B-Konvention drehen:** gerade Hand = **0°** statt 180° (`b → 180°−b` an der
   Schnittstelle; FK/IK + Greifer-Kopplung `eMotor = e − b − c` mit anpassen).
7. **C-Nullpunkt:** neutral = **0°** statt 90°.
   ⚠️ Der C↔ψ-Offset ist **posenabhängig** (`acos(cos β · sin a)`); ein global sauberes
   `c=0=neutral` braucht ggf. eine tiefere Umparametrierung des Handgelenks — **vor**
   der Umsetzung bewerten.
8. Tests + `Info_G92.md` nachziehen.

### Verifikation (Definition of Done)

- G92 der Grundstellung (alle Winkel 0) → Driver meldet **y ≈ −590, z ≈ 0**.
- appRobotHoming kann die **physisch gemessenen Winkel direkt** senden (ohne Spiegelung).
- Volle Test-Suite grün, inkl. Round-Trip und Grundstellungs-Tests.

---

## Anhang: Stand der bisherigen Arbeit

- Der atan2-Fix in der IK (`gamma = Math.atan2(pZ, pY)`, [Arm3SegmentLinearX.js:59](../robot/kinematics/Arm3SegmentLinearX.js#L59))
  ist umgesetzt und macht die IK mathematisch für −Y-Eingaben korrekt — eine
  Voraussetzung für Phase 1. Er betrifft **nicht** den G92/FK-Pfad.
- Die Winkel-Konventionen (B/C/E) sind in [doc/Info_G92.md](Info_G92.md) dokumentiert;
  die Y/Z-Konvention dort wird in Phase 1, Schritt 4 angepasst.