Files

chk c1f29bc1ee parallel foto

2026-06-26 07:18:30 +02:00

6.6 KiB

Raw Permalink Blame History

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

Technische Doku zur Konvertierung des Homing-Ergebnisses (FK-State aus 4b_revolute_angle.py / 5_pose_estimation.py) in die G92-Konvention von appRobotDriver.

Umgesetzt in server/server.js → Funktion fkStateToDriverG92(), aufgerufen im POST /api/homing/send-state-Handler.

Vollständige Driver-Konvention: 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:

"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:

"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():

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:

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).

6.6 KiB Raw Permalink Blame History Unescape Escape