Neues Kinematics

test/Robot.PortInverse.test.js

@@ -0,0 +1,150 @@
+/**
+ * Verifikation der Port→Motor-Rückrechnung für ToDo_9 / Paket 4 (Sync-Command).
+ *
+ * Herleitung: doc/ToDo_9a_PortRueckrechnung.md
+ *
+ * Kernaussage: Für die PRODUKTIV-Verkabelung (startRobot.js) ist die Abbildung
+ * der sieben Motorwerte auf die sieben gesendeten GRBL-Achswerte LINEAR und
+ * EINDEUTIG umkehrbar. `motorStateFromPorts()` ist diese Umkehrung; die Tests
+ * beweisen sie gegen den echten Sende-Pfad (`execCommand`) und gegen `portValue`.
+ *
+ * Damit ist klar: die B3-Mehrdeutigkeit (Ellbogen oben/unten) betrifft nur die
+ * kartesische Inverskinematik (`calculateAngles3D`), die der Sync gar nicht nutzt.
+ */
+const TelnetSender = require('../robot/TelnetSenderGRBL');
+const MotorPosition = require('../robot/RobotMotorPosition');
+const Robot = require('../robot/kinematics/Arm3SegmentLinearX');
+
+const D = 180 / Math.PI;
+
+/* ---------- Produktiv-Verkabelung (1:1 wie startRobot.js) ---------- */
+function buildSenders() {
+  const mk = (...args) => new TelnetSender('test.test', ...args); // test.test → Fake-tSocket
+  return {
+    base:  mk(2300, 'x', 'y', 'z'),     // GRBL x←xMotor, y←alpha, z←(beta-alpha)
+    elbow: mk(5000, 'a', null, null),   // GRBL x←a
+    hand:  mk(5000, 'c', 'e', 'b'),     // GRBL x←(c-b), y←eMotor, z←b
+  };
+}
+
+/* ---------- Die Umkehrung: GRBL-Readings → 7 Motorwerte ---------- */
+// r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} }  (GRBL-Achswerte, Grad bzw. mm)
+function motorStateFromPorts(r) {
+  const xMotor = r.base.x;                        // x-Port = xMotor (mm, direkt)
+  const alpha  = r.base.y / D;                    // y-Port = alpha·D
+  const beta   = (r.base.z + r.base.y) / D;       // z-Port = (beta-alpha)·D  ⇒ beta = z/D + alpha
+  const a      = r.elbow.x / D;                   // Elbow x-Port = a·D
+  const b      = r.hand.z / D;                    // Hand z-Port = b·D
+  const c      = (r.hand.x + r.hand.z) / D;       // Hand x-Port = (c-b)·D    ⇒ c = x/D + b
+  const eMotor = r.hand.y / D;                    // Hand y-Port = eMotor·D
+  return { xMotor, alpha, beta, a, b, c, eMotor };
+}
+
+/* ---------- Hilfen ---------- */
+// {x,y,z,a,b,c,e}-pos aus Motorwerten (Feld-Mapping der RobotMotorPosition)
+function posFromMotors(m) {
+  return { x: m.xMotor, y: m.alpha, z: m.beta, a: m.a, b: m.b, c: m.c, e: m.eMotor };
+}
+
+// Port-Readings über die reine Funktion portValue (volle Präzision)
+function portsViaPortValue(S, pos) {
+  return {
+    base:  { x: S.base.portValue('x', 'x', pos), y: S.base.portValue('y', 'y', pos), z: S.base.portValue('z', 'z', pos) },
+    elbow: { x: S.elbow.portValue('x', 'a', pos) },
+    hand:  { x: S.hand.portValue('x', 'c', pos), y: S.hand.portValue('y', 'e', pos), z: S.hand.portValue('z', 'b', pos) },
+  };
+}
+
+// Achswerte aus einer real gesendeten G-Code-Zeile (z. B. "G1 x12.34 y90.00 z-90.00")
+function parseAxes(line) {
+  const out = {};
+  const re = /([xyz])(-?\d+(?:\.\d+)?)/g;
+  let m;
+  while ((m = re.exec(line)) !== null) out[m[1]] = parseFloat(m[2]);
+  return out;
+}
+
+// Port-Readings über den ECHTEN Sende-Pfad execCommand (inkl. 2-Dezimal-Rundung)
+function portsViaExecCommand(S, m) {
+  const mNew = new MotorPosition(m.xMotor, m.alpha, m.beta, m.a, m.b, m.c, m.eMotor);
+  for (const f of ['xMotorChanged','yMotorChanged','zMotorChanged','aMotorChanged','bMotorChanged','cMotorChanged','eMotorChanged']) mNew[f] = true;
+  const mOld = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
+
+  S.base.execCommand('G1', mOld, mNew);
+  S.elbow.execCommand('G1', mOld, mNew);
+  S.hand.execCommand('G1', mOld, mNew);
+
+  return {
+    base:  parseAxes(S.base.tSocket.written),
+    elbow: parseAxes(S.elbow.tSocket.written),
+    hand:  parseAxes(S.hand.tSocket.written),
+  };
+}
+
+/* ---------- Repräsentative Motorzustände (plausible Bereiche) ---------- */
+const CASES = [
+  { name: 'Nullstellung',     xMotor: 0,   alpha: 0,    beta: 0,    a: 0,    b: 0,    c: 0,    eMotor: 0 },
+  { name: 'gemischt klein',   xMotor: 100, alpha: 0.30, beta: 0.50, a: 0.20, b: 0.40, c: 0.90, eMotor: 1.0 },
+  { name: 'negative Winkel',  xMotor: -55, alpha:-0.70, beta:-0.25, a:-1.10, b:-0.60, c:-0.30, eMotor:-0.5 },
+  { name: 'große Winkel',     xMotor: 250, alpha: 1.20, beta: 2.00, a: 2.90, b: 1.50, c: 3.00, eMotor: 2.5 },
+  { name: 'gekoppelt c≈b',    xMotor:  42, alpha: 0.10, beta: 0.10, a: 0.05, b: 1.20, c: 1.25, eMotor: 0.0 },
+];
+
+beforeAll(() => { jest.spyOn(console, 'log').mockImplementation(() => {}); });
+afterAll(() => { jest.restoreAllMocks(); });
+
+describe('Port→Motor-Rückrechnung (ToDo_9, Baustein für Paket 4)', () => {
+
+  describe('A) exakte Umkehrung gegen portValue (volle Präzision)', () => {
+    for (const c of CASES) {
+      test(c.name, () => {
+        const S = buildSenders();
+        const ports = portsViaPortValue(S, posFromMotors(c));
+        const rec = motorStateFromPorts(ports);
+        for (const k of ['xMotor','alpha','beta','a','b','c','eMotor']) {
+          expect(rec[k]).toBeCloseTo(c[k], 9);
+        }
+      });
+    }
+  });
+
+  describe('B) Umkehrung gegen den echten Sende-Pfad execCommand (mit Rundung)', () => {
+    for (const c of CASES) {
+      test(c.name, () => {
+        const S = buildSenders();
+        const ports = portsViaExecCommand(S, c);
+        const rec = motorStateFromPorts(ports);
+        // xMotor in mm (Rundung 0.01) → 0.02 Toleranz; Winkel in rad (Rundung 0.01° je Port) → 1e-3
+        expect(rec.xMotor).toBeCloseTo(c.xMotor, 2);
+        for (const k of ['alpha','beta','a','b','c','eMotor']) {
+          expect(Math.abs(rec[k] - c[k])).toBeLessThan(1e-3);
+        }
+      });
+    }
+  });
+
+  describe('C) Voll-Kette Sync: Ports → Motoren → Vorwärtskinematik → Pose', () => {
+    // Beweist, dass die rekonstruierten Motorwerte über die Vorwärtskinematik
+    // exakt dieselbe Pose liefern wie die Originalwerte (= was Sync dem Client meldet).
+    for (const c of CASES) {
+      test(c.name, () => {
+        const S = buildSenders();
+
+        // Referenz-Pose direkt aus den Original-Motorwerten
+        const ref = new Robot(250, 264, 100);
+        Object.assign(ref, { xMotor: c.xMotor, alpha: c.alpha, beta: c.beta, a: c.a, b: c.b, c: c.c, eMotor: c.eMotor });
+        ref.calculatePositionFromMotorAngles();
+
+        // Sync-Pfad: Ports → Inverse → Motoren setzen → Vorwärtskinematik
+        const rec = motorStateFromPorts(portsViaPortValue(S, posFromMotors(c)));
+        const sync = new Robot(250, 264, 100);
+        Object.assign(sync, { xMotor: rec.xMotor, alpha: rec.alpha, beta: rec.beta, a: rec.a, b: rec.b, c: rec.c, eMotor: rec.eMotor });
+        sync.calculatePositionFromMotorAngles();
+
+        for (const k of ['x','y','z','phi','theta','psi']) {
+          expect(sync[k]).toBeCloseTo(ref[k], 6);
+        }
+      });
+    }
+  });
+});