250 lines
14 KiB
Markdown
250 lines
14 KiB
Markdown
# 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.
|