192 lines
6.6 KiB
Markdown
192 lines
6.6 KiB
Markdown
# 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).
|