ChK/appRobotDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
2197a8954f2574a292522f90d046b13f166d80f4
appRobotDriver/doc/Info_Koordinaten.md
chk 2197a8954f G28 und Phase2 Arbeiten
2026-06-26 15:28:34 +02:00

14 KiB
Raw Blame History

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 dokumentiert und nach Phase 1 aktualisiert.
Reference in New Issue View Git Blame Copy Permalink