parallel foto
This commit is contained in:
chk
191
doc/Homing_8_appRobotDriver.md
Normal file
191
doc/Homing_8_appRobotDriver.md
Normal file
@@ -0,0 +1,191 @@
|
||||
# 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).
|
||||
Reference in New Issue
Block a user