ToDo 9a — Umkehr-Rechnung GRBL-Port → Motorwerte (Herleitung)

Status: durchgerechnet und verifiziert. Baustein für ToDo_9 / Paket 4 (Sync-Command). Verifikation: test/Robot.PortInverse.test.js (15 Tests grün).

Zweck

Der Sync-Command (ToDo_9, Paket 4) liest die echten Achs-Positionen (MPos) der drei GRBL/FluidNC-Controller und muss daraus die sieben Motorwerte des Roboters rekonstruieren: xMotor, alpha, beta, a, b, c, eMotor. Diese werden dann auf den Roboter geschrieben und per Vorwärtskinematik (calculatePositionFromMotorAngles()) in die Pose x/y/z/phi/theta/psi überführt.

Dieses Dokument leitet die Umkehrung her und hält das (verifizierte) Ergebnis fest.

Kernergebnis (vorweg)

Für die produktive Verkabelung ist die Abbildung Motorwerte → gesendete GRBL-Achswerte linear und eindeutig umkehrbar. Es gibt auf Port-Ebene keine Mehrdeutigkeit.

Die B3-Mehrdeutigkeit (Ellbogen oben/unten) steckt ausschließlich in der kartesischen Inverskinematik calculateAngles3D() (Pose → Gelenkwinkel). Der Sync nutzt diese Richtung nicht — er geht MPos → Motorwerte → Vorwärtskinematik → Pose, und beide Schritte sind eindeutige Funktionen. Damit ist der Sync-Pfad als Ganzes eindeutig.

Ausgangslage

Produktiv-Verkabelung (`startRobot.js`)

new TelnetSenderClass(baseIP,  2300, 'x', 'y', 'z')      // Base
new TelnetSenderClass(elbowIP, 5000, 'a', null, null)    // Elbow
new TelnetSenderClass(handIP,  5000, 'c', 'e', 'b')      // Hand

Bedeutung der Motor-Felder (`RobotMotorPosition`)

Feld	Bedeutung
`x`	`xMotor` (Schulterposition auf X-Schiene, mm)
`y`	`alpha` (Schulterwinkel, rad)
`z`	`beta` (Unterarm-Neigung, rad)
`a`	`a` (Ellbogen-Dreher, rad)
`b`	`b` (Handgelenk-Knick, rad)
`c`	`c` (Hand-Dreher, rad)
`e`	`eMotor` (Greifer)

Mit D = 180/π (Grad-Faktor).

Was jeder Controller real sendet (`execCommand`, Produktiv-Verkabelung)

Aus dem Sende-Pfad (robot/TelnetSenderGRBL.js) ergeben sich genau sieben GRBL-Achswerte:

Controller	GRBL-Achse	gesendeter Wert	in Motorwerten
Base	`x`	`xMotor`	`xMotor`
Base	`y`	`alpha·D`	`α·D`
Base	`z`	`(beta − alpha)·D`	`(β − α)·D`
Elbow	`x`	`a·D`	`a·D`
Hand	`x`	`(c − b)·D`	`(c − b)·D`
Hand	`y`	`eMotor·D`	`e·D`
Hand	`z`	`b·D`	`b·D`

Drei Ports koppeln je zwei Motorwerte (base.z, hand.x), aber jeder gekoppelte Wert wird mit einem unabhängig gelesenen Wert kombiniert (alpha bzw. b). Das System ist damit unteres Dreieckssystem → trivial auflösbar.

Herleitung der Umkehrung

Gegeben die GRBL-Readings base.{x,y,z}, elbow.{x}, hand.{x,y,z} (Grad bzw. mm):

xMotor = base.x                          // direkt
alpha  = base.y / D
beta   = (base.z + base.y) / D           // base.z = (β−α)·D  ⇒  β = base.z/D + α
a      = elbow.x / D
b      = hand.z / D
c      = (hand.x + hand.z) / D           // hand.x = (c−b)·D  ⇒  c = hand.x/D + b
eMotor = hand.y / D

Als Funktion (siehe Test, kann später 1:1 in Paket 4 übernommen werden):

function motorStateFromPorts(r) {           // r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} }
  const D = 180 / Math.PI;
  return {
    xMotor: r.base.x,
    alpha:  r.base.y / D,
    beta:  (r.base.z + r.base.y) / D,
    a:      r.elbow.x / D,
    b:      r.hand.z / D,
    c:     (r.hand.x + r.hand.z) / D,
    eMotor: r.hand.y / D,
  };
}

Verifikation

test/Robot.PortInverse.test.js — 15 Tests, fünf repräsentative Motorzustände (Nullstellung, gemischt, negative/große Winkel, gekoppelt c≈b):

A) Exaktheit gegen portValue (volle Präzision): Rückgewinnung auf 1e-9 genau.
B) Gegen den echten Sende-Pfad execCommand (inkl. 2-Dezimal-Rundung der G-Code-Werte): Rückgewinnung innerhalb der Rundung (Winkel < 1e-3 rad, xMotor < 0.02 mm).
C) Voll-Kette Sync: Ports → motorStateFromPorts → calculatePositionFromMotorAngles liefert dieselbe Pose x/y/z/phi/theta/psi wie die Original-Motorwerte (auf 1e-6).

Konsequenzen für ToDo_9 / Paket 4

Keine Zweig-Wahl auf Port-Ebene nötig. Die frühere Annahme (B3-Disambiguierung im Baustein) trifft auf die Port-Rückrechnung nicht zu — sie ist linear und eindeutig.
Rundung beachten. MPos kommt mit endlicher Präzision; gekoppelte Werte (beta, c) summieren zwei gerundete Ports → Toleranz im Sub-Promille-Bereich, unkritisch.
Achszahl je Controller. FluidNC meldet MPos für alle in seiner Config definierten Achsen. Genutzt werden nur: Base x,y,z · Elbow x · Hand x,y,z. Beim Parsen die übrigen (falls vorhanden) ignorieren.
Verkabelungs-Abhängigkeit. motorStateFromPorts() gilt für die aktuelle Verkabelung. Ändert sich startRobot.js (andere Port-Zuordnung), muss die Umkehrung mitgezogen werden — der Round-Trip-Test (portValue(motorStateFromPorts(p)) ≈ p) schützt davor.

5.1 KiB Raw Blame History Unescape Escape