Phase1 Koordinaten

test/Robot.Kinematics.NegativeY.test.js

@@ -63,4 +63,23 @@ describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => {
     expect(B.theta).toBeCloseTo(A.theta, EPS);
     expect(B.psi).toBeCloseTo(A.psi, EPS);
   });
+
+  test('Nullpose (alpha=beta=a=0, Hand gerade b=180) -> Fingerspitze (xMotor, -590, 0)', () => {
+    const r = fkFromMotors(0, 0, 0, 180, 0, 7);
+    expect(r.x).toBeCloseTo(7, 6);            // x = xMotor
+    expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 6);
+    expect(r.z).toBeCloseTo(0, 6);
+  });
+
+  test('a=0 -> Hand-Knick-Achse laeuft parallel zur x-Achse', () => {
+    // Bei a=0 knickt die Hand (b) in der y-z-Ebene -> Fingerspitze.x bleibt = xMotor.
+    const xM = 5;
+    for (const bDeg of [90, 135, 180, 225]) {
+      const r = fkFromMotors(0, 0, 0, bDeg, 0, xM);
+      expect(r.x).toBeCloseTo(xM, 6);
+    }
+    // Gegenprobe: a=90 dreht die Knick-Achse aus der y-z-Ebene -> x aendert sich deutlich.
+    const r90 = fkFromMotors(0, 0, 90, 135, 0, xM);
+    expect(Math.abs(r90.x - xM)).toBeGreaterThan(1);
+  });
 });