Files

chk 9a0ecfefab Umbau 12: Robot-Kinematics als extends RobotBase

2026-06-10 23:18:34 +02:00

18 KiB

Raw Blame History

ToDo 9 — Hardware-Feedback-Loop

Ziel

Der Roboter-Treiber soll nicht nur Befehle senden, sondern auch Antworten der Hardware lesen. Nur so können Fehler erkannt, Positionen verifiziert und Befehlsfolgen zuverlässig synchronisiert werden.

Aktuell ist der Datenfluss vollständig blind:

WebSocket → GCode → calculateAngles3D → sendCommand → tSocket.write()
                                                          ↓
                                               GRBL antwortet mit "ok" / "error"
                                               → wird nie gelesen (data => {})

Der blinde Kanal ist genau eine Stelle: socket.on('data', () => {}) in robot/TelnetSenderGRBL.js (Z. ~123). Alles in diesem ToDo hängt daran.

FluidNC-Protokoll: gesicherte Fakten (recherchiert)

Quellen: Serial Protocol, Automatic Reporting, Cross-Channel #750.

? funktioniert über Telnet. FluidNC verarbeitet Realtime-Kommandos (?, Feed-Hold, Cycle-Start, Reset) über eine Channel-Abstraktion auf allen Kanälen (USB, WiFi, Telnet, WebUI). Der bestehende telnet-stream-Socket kann ? senden und den <…>-Report zurücklesen.
Statuswahl über $10 (Bitmaske). Bit0 = MPos statt WPos, Bit1 = Bf (Puffer). → $10=3 liefert MPos + Bf (auf der eingesetzten FluidNC-Version verifizieren).
- MPos ist die richtige Quelle für die Rückrechnung: der Treiber sendet die portValue()-Werte als absolute G-Code-Koordinaten, MPos ist offset-fest.
- WCO (Work Coordinate Offset) erscheint periodisch; WPos = MPos − WCO. Für uns irrelevant, solange wir MPos lesen.
- Beispiel: <Idle|MPos:151.000,149.000,-1.000|Bf:15,128|FS:0,0|WCO:12,28,78>
Bf: = das kanonische Flow-Control-Signal. Erste Zahl = freie Planner-Blöcke, zweite = freie RX-Bytes. Damit liest man die echte Pufferfüllung, statt sie über moveTime zu schätzen (das ignoriert Beschleunigung).
Auto-Reporting statt Polling. $Report/Interval=N (ms) lässt FluidNC den Status während der Bewegung selbst pushen — pro Kanal einstellbar, im Stillstand nur bei Änderung. Ersetzt das ?-Polling und hält die Netzlast vorhersehbar gedeckelt.
Cross-Channel-Bleed-Through (Caveat). ok/Reports können auf anderen Kanälen auftauchen als dem auslösenden (Issue #750). Da pro FluidNC mehrere Kanäle aktiv sind (Treiber-Telnet und die appRobot_Access*-WebUI-Container), muss der Parser nach Nachrichtentyp demultiplexen (ok / error: / <…>) und fremde Zeilen tolerieren — kein striktes 1:1 Request→Response annehmen.

Designentscheidungen (festgeschrieben)

B3 — Umkehr-Kinematik ist disambiguierbar. Global nicht eindeutig, aber im Arbeitsraum dieses Roboters per dokumentierter physikalischer Zusatzbedingung auflösbar (z. B. „Ellbogen höher als Hand" bzw. „Ellbogen hinter der x-Achse"). motorStateFromPorts() bekommt eine feste Zweig-Wahl-Regel; die exakte Vorzeichen-Konvention wird beim Herleiten der Umkehrung gepinnt.

B5 — Lockstep als abschaltbare Absicherung. Durch die koordinierte Feedrate (ToDo_6a correct) treffen ohnehin alle Achsen gleichzeitig am nächsten Ziel ein — Lockstep ist die Absicherung, nicht der Hauptmechanismus. Umsetzung als Env-Schalter; Freerun zuerst (reines Zeit-/Bf-Pacing), echtes Lockstep erst nach Paket 3/5 (braucht den Feedback-Kanal).

B6 — Sync ist ein G-Code-Befehl. Läuft durch GCodeParser + RobotController (nicht als Sonderfall in InputWS wie heute M114). Folge: Sync ist der erste asynchrone Befehl — er muss auf die ?-Antworten aller drei Controller warten, bevor er den Roboterzustand setzt. Die Dispatch-Kette (RobotController.applyCommand) muss dafür erstmals einen Promise zurückgeben/awaiten können.

Schalter-Übersicht (Namen sind Vorschläge)

Env	Werte	Default	Wirkung
`ROBOT_SPEED_MODE`	`legacy` / `correct`	`legacy`	koordinierte Feedrate (ToDo_6a)
`ROBOT_USE_QUEUE`	`false` / `true`	`false`	zeitgesteuerte Sende-Queue (Paket 6)
`ROBOT_MOTION_SYNC`	`freerun` / `lockstep`	`freerun`	Schritt-für-Schritt-Synchronisation der 3 Controller

Konsistenz-Regel: ROBOT_MOTION_SYNC=lockstep ergibt nur mit ROBOT_SPEED_MODE=correct Sinn (sonst kommen die Controller zu unterschiedlichen Zeiten an und Lockstep müsste hart warten). Beim Start einmal prüfen und ggf. warnen.

Paket 1: GRBL-Antworten lesen

connection.on('data', data => {}) in TelnetSenderGRBL ersetzen durch echtes Lesen
- GRBL antwortet auf jeden G-Code-Befehl mit ok oder error: <Meldung>
- Antworten parsen und ins Log schreiben
Fehlerantworten nach außen meldbar machen
- an InfoServer oder über einen EventEmitter
- damit der WebSocket-Client Feedback bekommt, ob ein Befehl angenommen wurde

Paket 2: Command-Queue mit ok-Handshake

Verhältnis zu Paket 6: Dies ist die einfache, synchrone Variante (ein Befehl pro ok). Die ausgebaute, Bf-basierte und abschaltbare Queue steht in Paket 6 und löst dasselbe Problem flüssiger. Paket 2 kann als Zwischenschritt dienen oder direkt in Paket 6 aufgehen.

Sendepuffer einführen: Befehle erst abschicken, wenn das vorherige ok eingegangen ist
- GRBL hat intern ~128 Byte Puffer — bei schnellen Befehlsfolgen (Datei abspielen) droht sonst Puffer-Überlauf und stille Befehlsverwerfung
- Alternative: GRBL Line-Counting-Protokoll (sendet mehrere Befehle, zählt Zeichen im Puffer) → in Paket 6 über die zweite Bf-Zahl (freie RX-Bytes) abgedeckt
Timeout für ausbleibende ok-Antworten definieren
- nach X ms ohne Antwort: Fehler loggen, ggf. Verbindung zurücksetzen

Paket 3: Hardwareposition lesen (Auto-Report statt Polling)

Beim Verbindungsaufbau je Controller konfigurieren:
- $10=3 setzen → Report enthält MPos und Bf (Protokoll-Fakt 2)
- $Report/Interval=N setzen (z. B. N=100…200) → FluidNC pusht den Status während der Bewegung selbst (Protokoll-Fakt 4). Kein ?-Polling-Loop nötig; ? bleibt nur als Einzelabfrage on demand (z. B. für Sync, Paket 4).
data-Handler (Paket 1) parst die gepushten <…>-Reports: state, MPos, Bf
- robust gegen Cross-Channel-Fremdzeilen (Protokoll-Fakt 5) — nach Typ demultiplexen
Gemeldete Hardware-Position (MPos) mit Softwareposition vergleichen
- bei Abweichung: warnen oder synchronisieren (→ Paket 4)
- schützt gegen Drift durch Endschalter-Auslösung, Motor-Stall, Verbindungsunterbrechung
Status (Idle, Run, Alarm, Hold) + Bf für den InfoServer bereitstellen
- /api/status um GRBL-Zustand erweitern

Baustein für Paket 4 + 5: Rückabbildung Port → Motorwerte

Beide folgenden Pakete brauchen denselben Baustein: aus den von GRBL gemeldeten MPos-Werten der drei Controller die sieben Motorwerte des Roboters rekonstruieren (xMotor, alpha, beta, a, b, c, eMotor).

Das ist die Umkehrung von portValue() (robot/TelnetSenderGRBL.js). portValue() bildet eine Roboter-Achse → einen GRBL-Port-Wert ab, dabei koppeln einige Ports mehrere Achsen (z. B. der z-Port der Hand mischt c, b, z, y). Die Rückrichtung muss diese Kopplung explizit auflösen — sie ergibt sich nicht automatisch.

motorStateFromPorts(portReadings) definieren — algebraische Umkehrung von portValue()
- Eingang: pro Sender die gelesenen Port-Werte ({x, y, z} Base, {a} Elbow, {c, e, b} Hand)
- Ausgang: {xMotor, alpha, beta, a, b, c, eMotor}
- Grad→Rad zurückrechnen, factorTurnLift/handOpenInMM herausrechnen, gekoppelte Ports auflösen
- Zweig-Wahl (B3): wo die Lösung mehrdeutig ist, die dokumentierte physikalische Zusatzbedingung anwenden (z. B. „Ellbogen höher als Hand"). Konvention im Code festhalten.
Round-Trip-Invariante als Test: portValue(motorStateFromPorts(p)) ≈ p
- dasselbe Muster wie test/Robot.Kinematics.RoundTrip.test.js
- schützt die Umkehrfunktion gegen Drift gegenüber portValue()

Hinweis: Gelesen wird auf dem aktiven Sender TelnetSenderGRBL (im data-Handler, siehe Paket 1) — nicht auf FluidNCClient.js.

Paket 4: Hardware-Position auslesen und übernehmen (Sync-Command)

Ziel: Ein Befehl liest die echten Motor-Koordinaten aller drei Controller aus, übernimmt sie als neuen Soll-Zustand und passt die berechnete Roboter-Pose entsprechend an. Nötig nach Homing, manuellem Jog, Endschalter-Auslösung oder Reconnect — die Software weiß sonst nicht, wo der Roboter physisch wirklich steht.

G-Code-Befehl (B6), z. B. M114 R (Read-Hardware) — durch GCodeParser + RobotController geroutet, nicht als Sonderfall in InputWS wie heute M114
- klar abgegrenzt vom bestehenden M114, das nur die Software-Position zurückgibt (GCode.getM114(robot) in server/InputWS.js)
Async-Dispatch (B6-Folge): RobotController.applyCommand muss für diesen Befehl einen Promise zurückgeben und auf die ?-Antworten warten — der erste asynchrone Befehl im bisher synchronen Dispatch-Pfad.
Ablauf des Sync:
1. an alle drei Sender einmalig ? senden, je MPos aus der Antwort parsen (Paket 3)
2. motorStateFromPorts(...) → sieben Motorwerte rekonstruieren (Baustein oben, inkl. B3-Zweigwahl)
3. diese auf den Roboter schreiben: robot.xMotor/alpha/beta/a/b/c/eMotor = …
4. Vorwärtskinematik anstoßen: robot.calculatePositionFromMotorAngles() → füllt robot.x/y/z und phi/theta/psi aus den Hardwarewerten
5. motorPosition/motorPositionOld zurücksetzen, damit der nächste Move sauber von der echten Position aus rechnet (sonst falscher Speed-Delta im Korrekt-Modus)
dem anfragenden Client die übernommene Pose zurückmelden (reply(ws, …))
kein automatisches Nachfahren — Sync ändert nur den Soll-Zustand, sendet keinen Move

Warum nicht einfach „letzten gesendeten Wert" merken? Weil die Hardware nach Homing/Jog/ Stall von dem abweicht, was zuletzt gesendet wurde — genau diese Differenz soll Sync auflösen.

Paket 5: Bewegungs-Fortschritt ermitteln (Move-Progress)

Ziel: Herausfinden, wie weit FluidNC einen laufenden Move bereits abgearbeitet hat — also wie weit sich jeder Controller schon zur Ziel-Position bewegt hat (0…100 %).

Beim Absenden eines Moves Start und Ziel je Controller festhalten
- mStart = Port-Werte vor dem Move, mTarget = gesendete Port-Werte
- liegt bereits vor: robot.motorPositionOld (Start) und robot.motorPosition (Ziel), über portValue() in Port-Werte umgerechnet
Aus den auto-gepushten Reports (Paket 3, $Report/Interval) je Controller berechnen — kein eigenes Polling:
```
fortschritt_i = |MPos_jetzt − Start_i| / |Ziel_i − Start_i|     (auf 0…1 geklemmt)
```
Aggregat-Fortschritt + Fertig-Erkennung:
- Fertig, wenn alle drei Controller state == Idle UND MPos ≈ Ziel
- der Gesamt-Fortschritt eines Schritts = Minimum über die Controller (der langsamste bestimmt, wann der Schritt fertig ist)
- im Korrekt-Modus (ToDo_6a) sollten alle Controller etwa gleich schnell fertig sein — eine große Spreizung der Einzel-Fortschritte ist dort ein Warnsignal (Feedrate-Fehler)
Lockstep-Gate (B5, nur bei ROBOT_MOTION_SYNC=lockstep): den nächsten Schritt erst freigeben, wenn der langsamste Controller Idle/Ziel erreicht hat. In freerun entfällt das Gate — die Koordination kommt allein aus der Feedrate (ToDo_6a correct).
Fortschritt + Status nach außen geben
- über InfoServer (/api/status) und/oder als WS-Push an die Clients
- ermöglicht eine Fortschrittsanzeige beim Abspielen von Dateien (ToDo_6b)

Offener Kernpunkt: Was, wenn währenddessen der nächste Befehl kommt?

Heute ist der Treiber fire-and-forget: ein neuer Befehl wird sofort an alle drei GRBLs geschickt und landet in deren Planner-Puffer. Das hat zwei Folgen, die Paket 5 für sich genommen nicht löst:

Fortschritt wird mehrdeutig. motorPositionOld/motorPosition halten nur die letzten zwei Stellungen. Sind mehrere Moves gleichzeitig im GRBL-Puffer, weiß der Treiber nicht mehr, welcher gerade fährt — der ?-Fortschritt bezieht sich dann auf das falsche Start/Ziel-Paar.
Stille Befehlsverwerfung. Kommen Befehle dauerhaft schneller als sie ausgeführt werden, läuft GRBLs 128-Byte-RX-Puffer über → Befehle gehen verloren (vgl. Paket 2).

Die saubere Lösung ist eine zeitgesteuerte Sende-Queue — bewusst als größerer, abschaltbarer Umbau in Paket 6.

Paket 6: Zeitgesteuerte Sende-Queue (`ROBOT_USE_QUEUE`)

Ziel: Befehle nicht mehr blind sofort raushauen, sondern in einer Queue puffern und zeitlich getaktet absenden — jeden Befehl gerade rechtzeitig, bevor der vorige fertig ist. So bleibt die Bewegung flüssig (GRBL-Planner läuft nie leer), nichts geht verloren (kein Puffer-Überlauf), und das Netz wird nicht mit Dauer-Polling oder Befehls-Salven belastet.

Schalter

ROBOT_USE_QUEUE (siehe zentrale Schalter-Übersicht oben). false = exakt heutiges Fire-and-forget, byte-identisch zu vorher; true = Queue-Logik aktiv. Wie ROBOT_SPEED_MODE greift der Umbau nur bei true — das bestehende Sender-Test-Sicherheitsnetz bleibt gültig.

# docker-compose.yaml → appRobotDriver
environment:
  - ROBOT_USE_QUEUE=false   # oder: true

Takt: `Bf` ist die Wahrheit, `moveTime` nur der Vorhersage-Hint

Die Recherche ändert den ursprünglichen „reine Zeitschätzung"-Entwurf: FluidNC liefert die echte Pufferfüllung (Bf) ohnehin frei Haus über Auto-Reporting (Protokoll-Fakt 3+4). Damit muss nichts mehr aufwändig geschätzt werden.

Bf-basierter Haupttakt (echte Wahrheit, kein Extra-Traffic): Aus den auto-gepushten Reports (Paket 3) kennt der Treiber je Controller die freien Planner-Blöcke. Regel:
```
solange (freie Planner-Blöcke > schwelle)  →  nächsten Queue-Eintrag senden
```
Hält den Planner gefüllt (flüssige Bewegung) und kann nie überlaufen — das ist das Standard-GRBL-Streaming, nur mit gepushtem statt gepolltem Status.
moveTime als prädiktiver Zusatz (aus ToDo_6a): zwischen zwei Reports überbrückt die geschätzte Ausführzeit die Lücke (z. B. um zu entscheiden, ob jetzt schon der nächste Eintrag sinnvoll ist). Korrigiert wird die Schätzung laufend durch den nächsten Bf-Push; sie ist nie die alleinige Quelle.

Eintrag in der Queue

Queue-Eintrag hält: geparster Befehl / Motorziel, moveTime (Hint), die je Sender resultierenden G-Code-Strings, Status (pending → sent → done)
done wird aus dem Bf/state-Report abgeleitet (Idle am Ziel), nicht geraten

Pacing-Schleife

je Controller die freien Planner-Blöcke aus dem letzten Report führen; senden, solange über Schwelle — In-Flight-Tiefe damit implizit begrenzt (flüssig, aber steuerbar)
moveTime nur zur Überbrückung zwischen Reports nutzen
Sicherheitsboden: zusätzlich freie RX-Bytes aus Bf beachten (zweite Zahl) — nie mehr senden, als in den RX-Puffer passt (Character-Counting fällt damit faktisch ab)

Verhalten bei „neuer Befehl kommt mitten in der Bewegung"

Zwei Betriebsarten — je nach Quelle der Befehle:

Anhängen (Datei abspielen / ToDo_6b): Reihenfolge erhalten, nach Schätzung takten, In-Flight-Tiefe begrenzen. Kein Befehl wird verworfen.
Neuester gewinnt (interaktives Streaming, z. B. 3D-Input / Bodytracker): Trifft ein neues Ziel ein, während noch ungesendete (pending) Einträge in der Queue liegen, den veralteten Schwanz ersetzen statt anzuhängen (Coalescing) → niedrige Latenz, kein Auflaufen veralteter Zwischenziele. (Kann als Unter-Schalter / spätere Ausbaustufe kommen.)
Backpressure: maximale Queue-Länge festlegen. Bei Dauer-Überlauf entweder Ältestes-verwerfen (Streaming) oder Druck an den WS-Client zurückgeben (Datei) — nie unbegrenzt wachsen lassen.

Fortschritt mit Pipelining (behebt die Mehrdeutigkeit aus Paket 5)

Den „aktuell fahrenden" Eintrag in der Queue markieren (Kopf vorrücken, wenn geschätzte Uhr abgelaufen oder ?=Idle). Erst dieser Kopf liefert das richtige Start/Ziel-Paar für die ?-Fortschrittsrechnung aus Paket 5.

Kanten / Sonderfälle

Alarm/Error mitten in der Queue (Paket 1) → Queue leeren, Senden stoppen, Fehler melden
Sync-Command (Paket 4) bei nicht-leerer Queue → erst Queue leeren; die gepufferten Ziele sind nach einem Re-Homing veraltet
Lockstep vs. Freerun (B5): In freerun taktet jeder Controller eigenständig nach seinem Bf. In lockstep kommt zusätzlich das Gate aus Paket 5 dazu — der nächste Schritt wird erst gesendet, wenn der langsamste Controller Idle/Ziel meldet.
Drei Controller driften auseinander: Im Korrekt-Modus (ToDo_6a) sollten die Bf-Stände zusammenbleiben; große Spreizung ist ein Warnsignal (Feedrate-Fehler) — in lockstep zudem ein Auslöser, härter zu warten.
moveTime nur Hint: Da Bf die echte Wahrheit liefert, ist die Schätzungenauigkeit (dist/feedrate ignoriert Beschleunigung) unkritisch — sie überbrückt nur die Lücke zwischen zwei Reports. Keine Trapez-Profil-Verfeinerung nötig.

Hinweis zur Implementierung

Aktiver Pfad: Gelesen und konfiguriert ($10, $Report/Interval) wird auf dem produktiv genutzten robot/TelnetSenderGRBL.js (Telnet-Kanal) — der data-Handler dort ist die zentrale Stelle (Paket 1).
robot/fluidnc/FluidNCClient.js (bidirektionale WebSocket-Anbindung, Port 81, Reconnect + EventEmitter) ist eine alternative Anbindung. Nicht der aktive Pfad, kann aber bei ToDo_2 (Sender-Interface) als Referenz für das Event-Modell mit evaluiert werden.

Reihenfolge / Abhängigkeiten

Paket 1 (lesen) ist die Basis für alles Weitere.
Paket 3 (Auto-Report $10=3 + $Report/Interval, MPos/Bf parsen) liefert die Daten für Paket 4, 5, 6.
Baustein (Umkehr-Kinematik inkl. B3-Zweigwahl) → dann Paket 4 (Sync, async).
Paket 5 (Fortschritt) und Paket 6 (Queue) bauen auf Paket 3 auf; Freerun zuerst, Lockstep (B5) erst danach.

18 KiB Raw Blame History Unescape Escape