From 29b5f2ae4bd194b54fd9309955f30e782beabcc3 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Fri, 26 Jun 2026 06:44:11 +0200 Subject: [PATCH] G92 sowie arctan2 --- doc/Info_G92.md | 88 ++++ logs/gcode_commands.log | 27 ++ logs/pings.log | 6 + robot/kinematics/Arm3SegmentLinearX.js | 2 +- test/Robot.Kinematics.RoundTrip.test.js | 558 ++++++++++++++---------- 5 files changed, 445 insertions(+), 236 deletions(-) diff --git a/doc/Info_G92.md b/doc/Info_G92.md index c21bca9..0da1306 100644 --- a/doc/Info_G92.md +++ b/doc/Info_G92.md @@ -32,6 +32,94 @@ G92 X158.14 Y4.19 Z57.74 A91.85 B-45.46 C-69.92 E21.20 --- +## Geometrische Bedeutung der Winkel (Driver-Konvention) + +> **Wichtig für appRobotHoming:** Der Driver interpretiert die G92-Winkel in einer +> **eigenen** Konvention. appRobotHoming muss die physisch gemessenen Gelenkwinkel +> **in diese Konvention umrechnen**, bevor sie als G92 gesendet werden. Die folgenden +> Tabellen sind aus der Kinematik (`Arm3SegmentLinearX`) **verifiziert** (jeweils eine +> Achse isoliert variiert). + +### Koordinatenrahmen + +- **z = 0** ist die Achse zwischen Base und Arm1 (Schulter) — kein Offset darunter. +- y = nach hinten (Hauptarbeitsrichtung), z = nach oben, x = Linearschiene. +- Alle Armwinkel liegen in der y-z-Ebene (bei fixer x-Schiene). + +### Y = α (Oberarm) und Z = β (Unterarm) — ABSOLUT + +Beide werden **absolut gegen die Horizontale (+y)** gemessen, **nicht** relativ zueinander: + +| Wert | Oberarm (Y) bzw. Unterarm (Z) | +|------|-------------------------------| +| 0° | waagerecht nach +y | +| 90° | senkrecht nach oben | + +⚠️ **Z ist der absolute Unterarmwinkel**, nicht der Ellbogen-Knick gegen den Oberarm. +Misst appRobotHoming den Ellbogen relativ zum Oberarm: `Z = Oberarmwinkel + Ellbogen_relativ`. +(Erst bei der Weiterleitung an FluidNC wird daraus `(β − α)` zurückgerechnet, siehe unten.) + +### B = Handgelenk-Knick + +Verifizierte Referenz (α=0, β=90, A=0, C=0): + +| B (G92) | physischer Knick Unterarm↔Hand | +|---------|--------------------------------| +| 0° | 180° (Hand voll zurückgeklappt) | +| 90° | 90° (Hand ⊥ Unterarm) | +| 180° | 0° (Hand **gerade**, in Verlängerung des Unterarms) | + +→ **Gerade Hand = B 180°.** Allgemein: `physischer Knick = 180° − B`, also `B = 180° − Knick`. +Der Driver (IK) erzeugt B nur im Bereich [0°, 180°]. + +### A = Unterarm-Dreher (Ellbogen-Roll) + +A dreht die **Richtung**, in die das Handgelenk knickt, um die Unterarm-Längsachse — +die Knick-**Größe** bleibt dabei gleich. Verifiziert (α=0, β=90, B=90, C=0): +A=0 → Fingerspitze auf der −y-Seite, A=90 → −x-Seite, A=180 → +y-Seite; Knick konstant 90°. + +### C = Hand-Dreher (Roll) + +C dreht die Hand um ihre eigene Achse. Verifizierte Referenz (α=0, β=90, A=0, B=90): + +| C (G92) | Hand-Roll ψ | +|---------|-------------| +| 0° | −90° | +| 90° | 0° (neutral) | +| 180° | +90° | + +→ In dieser Stellung gilt `ψ = C − 90°`. **Achtung:** der genaue Zusammenhang hängt von +der Armstellung ab — exakt `ψ = C − acos(cos β · sin A)` (Winkel in rad). C selbst ist der +physische Hand-Roll-Gelenkwinkel; nur der Bezug zum Welt-ψ verschiebt sich mit der Pose. + +### E = Greifer-Öffnung (mm) + Sehnen-Kopplung + +E ist die **Finger-Öffnung in mm** (ein Finger ab Null-Position) — keine Winkel-Umrechnung. +Der Driver leitet daraus den Motorwert ab: + +``` +eMotor = E − b − c (b, c in RADIANT!) + = E − B°/57.2958 − C°/57.2958 +``` + +Grund: die Greifer-Sehne läuft durchs Handgelenk; Knick (B) und Roll (C) ziehen an der Sehne. +appRobotHoming sendet die **reine Öffnung** als E; die Kopplung macht der Driver. Bewegt sich +nur das Handgelenk, kompensiert eMotor, damit die Öffnung konstant bleibt. + +### Zusammenfassung: was appRobotHoming senden muss + +| Achse | Driver erwartet | Typische Falle | +|-------|------------------------------------------------|-----------------------------------------| +| X | xMotor in mm | — | +| Y | Oberarm absolut (0=waagerecht, 90=hoch) | — | +| Z | Unterarm **absolut** (nicht relativ zum Oberarm) | relativ statt absolut gesendet | +| A | Unterarm-Dreher (Richtung des Knicks) | Nullpunkt/Vorzeichen | +| B | **180° = gerade Hand**; Knick = 180° − B | gemessenen Knick direkt gesendet | +| C | Hand-Roll, **90° = neutral** | Nullpunkt/Vorzeichen | +| E | Öffnung in mm | Motorwert statt Öffnung gesendet | + +--- + ## Interne Verarbeitung (`RobotController.js`) Winkel-Achsen werden von Grad nach Radiant umgerechnet (D = 180/π): diff --git a/logs/gcode_commands.log b/logs/gcode_commands.log index 5ef0fbf..c5f478a 100644 --- a/logs/gcode_commands.log +++ b/logs/gcode_commands.log @@ -11011,3 +11011,30 @@ 2026-06-25T16:42:31.758Z ::ffff:127.0.0.1: M114 2026-06-25T16:42:31.991Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 2026-06-25T16:42:32.218Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T03:31:10.436Z ::ffff:127.0.0.1: FList +2026-06-26T03:31:10.459Z ::ffff:127.0.0.1: FPlus +2026-06-26T03:31:10.466Z ::ffff:127.0.0.1: M114 +2026-06-26T03:31:10.484Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T03:31:10.501Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T03:31:10.502Z ::ffff:127.0.0.1: FShow +2026-06-26T03:31:10.665Z ::ffff:127.0.0.1: M114 +2026-06-26T03:31:10.885Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T03:31:11.101Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T04:00:22.876Z ::ffff:127.0.0.1: FList +2026-06-26T04:00:22.897Z ::ffff:127.0.0.1: FPlus +2026-06-26T04:00:22.931Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T04:00:22.969Z ::ffff:127.0.0.1: FShow +2026-06-26T04:00:23.464Z ::ffff:127.0.0.1: M114 +2026-06-26T04:00:23.498Z ::ffff:127.0.0.1: M114 +2026-06-26T04:00:23.511Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T04:00:23.743Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T04:00:24.008Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T04:36:17.245Z ::ffff:127.0.0.1: M114 +2026-06-26T04:36:17.277Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T04:36:17.299Z ::ffff:127.0.0.1: FList +2026-06-26T04:36:17.330Z ::ffff:127.0.0.1: FPlus +2026-06-26T04:36:17.366Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T04:36:17.400Z ::ffff:127.0.0.1: FShow +2026-06-26T04:36:17.472Z ::ffff:127.0.0.1: M114 +2026-06-26T04:36:17.716Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T04:36:17.971Z ::ffff:127.0.0.1: G1 X1 diff --git a/logs/pings.log b/logs/pings.log index 2c526b8..0b54aa7 100644 --- a/logs/pings.log +++ b/logs/pings.log @@ -14808,3 +14808,9 @@ 2026-06-25T16:42:29.198Z ::ffff:127.0.0.1 : Ping 2026-06-25T16:42:31.500Z ::ffff:127.0.0.1 : Ping 2026-06-25T16:42:31.578Z ::ffff:127.0.0.1 : Ping +2026-06-26T03:31:10.434Z ::ffff:127.0.0.1 : Ping +2026-06-26T03:31:10.438Z ::ffff:127.0.0.1 : Ping +2026-06-26T04:00:23.262Z ::ffff:127.0.0.1 : Ping +2026-06-26T04:00:23.430Z ::ffff:127.0.0.1 : Ping +2026-06-26T04:36:17.220Z ::ffff:127.0.0.1 : Ping +2026-06-26T04:36:17.238Z ::ffff:127.0.0.1 : Ping diff --git a/robot/kinematics/Arm3SegmentLinearX.js b/robot/kinematics/Arm3SegmentLinearX.js index 25b7ad6..3aee2bf 100644 --- a/robot/kinematics/Arm3SegmentLinearX.js +++ b/robot/kinematics/Arm3SegmentLinearX.js @@ -56,7 +56,7 @@ class Arm3SegmentLinearX extends RobotBase { if (r > (this.l1 + this.l2)) { return; } if (r == 0) { return; } - var gamma = Math.asin(pZ / r); + var gamma = Math.atan2(pZ, pY); var delta = Math.acos((this.l1 * this.l1 + this.l2 * this.l2 - r * r) / (2 * this.l1 * this.l2)); this.alpha = Math.acos((this.l1 * this.l1 + r * r - this.l2 * this.l2) / (2 * r * this.l1)) + gamma; this.beta = -Math.PI + (this.alpha + delta); diff --git a/test/Robot.Kinematics.RoundTrip.test.js b/test/Robot.Kinematics.RoundTrip.test.js index 1675ebb..8730088 100755 --- a/test/Robot.Kinematics.RoundTrip.test.js +++ b/test/Robot.Kinematics.RoundTrip.test.js @@ -1,236 +1,324 @@ -// __tests__/Robot.inverseKinematics.test.js -const Robot = require('../robot/kinematics/Arm3SegmentLinearX'); - -describe("Robot Kinematics Roundtrip", () => { - - test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 1", () => { - - // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) === - const L1 = 300; - const L2 = 200; - const L3 = 10; - - const A = new Robot(L1, L2, L3) - - // Beispiel-Eingabe - A.x = 0; - A.y = 310; - A.z = 0; - - A.phi = -Math.PI/2; - A.theta = Math.PI/2; - A.psi = 0; - A.e = 0; - - A.calculateAngles3D(); - - // Motorwerte aus Instanz A speichern - const motor = { - xMotor: A.xMotor, - alpha: A.alpha, - beta: A.beta, - a: A.a, - b: A.b, - c: A.c - }; - - // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) === - const B = new Robot(L1, L2, L3); - - B.xMotor = motor.xMotor; - B.alpha = motor.alpha; - B.beta = motor.beta; - B.a = motor.a; - B.b = motor.b; - B.c = motor.c; - - // Diese Funktion rekonstruiert nur x, y, z! - B.calculatePositionFromMotorAngles(); - - // === Vergleich mit Toleranz === - const EPS = 0.01; // 1/1000 mm Genauigkeit - - expect(B.pY).toBeCloseTo(A.pY, EPS); - expect(B.pZ).toBeCloseTo(A.pZ, EPS); - expect(B.x).toBeCloseTo(A.x, EPS); - expect(B.y).toBeCloseTo(A.y, EPS); - expect(B.z).toBeCloseTo(A.z, EPS); - }); - - test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 2", () => { - - // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) === - const L1 = 300; - const L2 = 300; - const L3 = 10; - - const A = new Robot(L1, L2, L3) - - // Beispiel-Eingabe - A.x = 0; - A.y = 410; - A.z = 0; - - A.phi = -Math.PI/2; - A.theta = Math.PI/2; - A.psi = 0; - A.e = 0; - - A.calculateAngles3D(); - - // Motorwerte aus Instanz A speichern - const motor = { - xMotor: A.xMotor, - alpha: A.alpha, - beta: A.beta, - a: A.a, - b: A.b, - c: A.c - }; - - // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) === - const B = new Robot(L1, L2, L3); - - B.xMotor = motor.xMotor; - B.alpha = motor.alpha; - B.beta = motor.beta; - B.a = motor.a; - B.b = motor.b; - B.c = motor.c; - - // Diese Funktion rekonstruiert nur x, y, z! - B.calculatePositionFromMotorAngles(); - - // === Vergleich mit Toleranz === - const EPS = 0.01; // 1/1000 mm Genauigkeit - - expect(B.pY).toBeCloseTo(A.pY, EPS); - expect(B.pZ).toBeCloseTo(A.pZ, EPS); - - expect(B.x).toBeCloseTo(A.x, EPS); - expect(B.y).toBeCloseTo(A.y, EPS); - expect(B.z).toBeCloseTo(A.z, EPS); - }); - - test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 3", () => { - - // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) === - const L1 = 300; - const L2 = 200; - const L3 = 10; - - const A = new Robot(L1, L2, L3) - - // Beispiel-Eingabe - A.x = 10; - A.y = 500; - A.z = 0; - - A.phi = 0; //-Math.PI/2; - A.theta = Math.PI/2; - A.psi = 0; - A.e = 0; - - A.calculateAngles3D(); - - - // Motorwerte aus Instanz A speichern - const motor = { - xMotor: A.xMotor, - alpha: A.alpha, - beta: A.beta, - a: A.a, - b: A.b, - c: A.c - }; - - // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) === - const B = new Robot(L1, L2, L3); - - B.xMotor = motor.xMotor; - B.alpha = motor.alpha; - B.beta = motor.beta; - B.a = motor.a; - B.b = motor.b; - B.c = motor.c; - - // Diese Funktion rekonstruiert nur x, y, z! - B.calculatePositionFromMotorAngles(); - - // === Vergleich mit Toleranz === - const EPS = 0.01; // 1/1000 mm Genauigkeit - - expect(B.pY).toBeCloseTo(A.pY, EPS); - expect(B.pZ).toBeCloseTo(A.pZ, EPS); - expect(B.x).toBeCloseTo(A.x, EPS); - expect(B.y).toBeCloseTo(A.y, EPS); - expect(B.z).toBeCloseTo(A.z, EPS); - }); - - - test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 4", () => { - - // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) === - const L1 = 300; - const L2 = 200; - const L3 = 10; - - const A = new Robot(L1, L2, L3) - - // Beispiel-Eingabe - A.x = 10; - A.y = 430; - A.z = 30; - - A.phi = Math.PI/7; //-Math.PI/2; - A.theta = Math.PI/2; - A.psi = 0; - A.e = 0; - - A.calculateAngles3D(); - - - // Motorwerte aus Instanz A speichern - const motor = { - xMotor: A.xMotor, - alpha: A.alpha, - beta: A.beta, - a: A.a, - b: A.b, - c: A.c - }; - - // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) === - const B = new Robot(L1, L2, L3); - - B.xMotor = motor.xMotor; - B.alpha = motor.alpha; - B.beta = motor.beta; - B.a = motor.a; - B.b = motor.b; - B.c = motor.c; - - - - // Diese Funktion rekonstruiert nur x, y, z! - B.calculatePositionFromMotorAngles(); - - // === Vergleich mit Toleranz === - const EPS = 0.01; // 1/1000 mm Genauigkeit - - expect(B.pY).toBeCloseTo(A.pY, EPS); - expect(B.pZ).toBeCloseTo(A.pZ, EPS); - expect(B.x).toBeCloseTo(A.x, EPS); - expect(B.y).toBeCloseTo(A.y, EPS); - expect(B.z).toBeCloseTo(A.z, EPS); - - expect(B.phi).toBeCloseTo(A.phi, EPS); - expect(B.theta).toBeCloseTo(A.theta, EPS); - }); - - - - - - +// __tests__/Robot.inverseKinematics.test.js +const Robot = require('../robot/kinematics/Arm3SegmentLinearX'); + +describe("Robot Kinematics Roundtrip", () => { + + test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 1", () => { + + // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) === + const L1 = 300; + const L2 = 200; + const L3 = 10; + + const A = new Robot(L1, L2, L3) + + // Beispiel-Eingabe + A.x = 0; + A.y = 310; + A.z = 0; + + A.phi = -Math.PI/2; + A.theta = Math.PI/2; + A.psi = 0; + A.e = 0; + + A.calculateAngles3D(); + + // Motorwerte aus Instanz A speichern + const motor = { + xMotor: A.xMotor, + alpha: A.alpha, + beta: A.beta, + a: A.a, + b: A.b, + c: A.c + }; + + // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) === + const B = new Robot(L1, L2, L3); + + B.xMotor = motor.xMotor; + B.alpha = motor.alpha; + B.beta = motor.beta; + B.a = motor.a; + B.b = motor.b; + B.c = motor.c; + + // Diese Funktion rekonstruiert nur x, y, z! + B.calculatePositionFromMotorAngles(); + + // === Vergleich mit Toleranz === + const EPS = 0.01; // 1/1000 mm Genauigkeit + + expect(B.pY).toBeCloseTo(A.pY, EPS); + expect(B.pZ).toBeCloseTo(A.pZ, EPS); + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + }); + + test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 2", () => { + + // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) === + const L1 = 300; + const L2 = 300; + const L3 = 10; + + const A = new Robot(L1, L2, L3) + + // Beispiel-Eingabe + A.x = 0; + A.y = 410; + A.z = 0; + + A.phi = -Math.PI/2; + A.theta = Math.PI/2; + A.psi = 0; + A.e = 0; + + A.calculateAngles3D(); + + // Motorwerte aus Instanz A speichern + const motor = { + xMotor: A.xMotor, + alpha: A.alpha, + beta: A.beta, + a: A.a, + b: A.b, + c: A.c + }; + + // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) === + const B = new Robot(L1, L2, L3); + + B.xMotor = motor.xMotor; + B.alpha = motor.alpha; + B.beta = motor.beta; + B.a = motor.a; + B.b = motor.b; + B.c = motor.c; + + // Diese Funktion rekonstruiert nur x, y, z! + B.calculatePositionFromMotorAngles(); + + // === Vergleich mit Toleranz === + const EPS = 0.01; // 1/1000 mm Genauigkeit + + expect(B.pY).toBeCloseTo(A.pY, EPS); + expect(B.pZ).toBeCloseTo(A.pZ, EPS); + + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + }); + + test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 3", () => { + + // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) === + const L1 = 300; + const L2 = 200; + const L3 = 10; + + const A = new Robot(L1, L2, L3) + + // Beispiel-Eingabe + A.x = 10; + A.y = 500; + A.z = 0; + + A.phi = 0; //-Math.PI/2; + A.theta = Math.PI/2; + A.psi = 0; + A.e = 0; + + A.calculateAngles3D(); + + + // Motorwerte aus Instanz A speichern + const motor = { + xMotor: A.xMotor, + alpha: A.alpha, + beta: A.beta, + a: A.a, + b: A.b, + c: A.c + }; + + // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) === + const B = new Robot(L1, L2, L3); + + B.xMotor = motor.xMotor; + B.alpha = motor.alpha; + B.beta = motor.beta; + B.a = motor.a; + B.b = motor.b; + B.c = motor.c; + + // Diese Funktion rekonstruiert nur x, y, z! + B.calculatePositionFromMotorAngles(); + + // === Vergleich mit Toleranz === + const EPS = 0.01; // 1/1000 mm Genauigkeit + + expect(B.pY).toBeCloseTo(A.pY, EPS); + expect(B.pZ).toBeCloseTo(A.pZ, EPS); + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + }); + + + test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 4", () => { + + // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) === + const L1 = 300; + const L2 = 200; + const L3 = 10; + + const A = new Robot(L1, L2, L3) + + // Beispiel-Eingabe + A.x = 10; + A.y = 430; + A.z = 30; + + A.phi = Math.PI/7; //-Math.PI/2; + A.theta = Math.PI/2; + A.psi = 0; + A.e = 0; + + A.calculateAngles3D(); + + + // Motorwerte aus Instanz A speichern + const motor = { + xMotor: A.xMotor, + alpha: A.alpha, + beta: A.beta, + a: A.a, + b: A.b, + c: A.c + }; + + // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) === + const B = new Robot(L1, L2, L3); + + B.xMotor = motor.xMotor; + B.alpha = motor.alpha; + B.beta = motor.beta; + B.a = motor.a; + B.b = motor.b; + B.c = motor.c; + + + + // Diese Funktion rekonstruiert nur x, y, z! + B.calculatePositionFromMotorAngles(); + + // === Vergleich mit Toleranz === + const EPS = 0.01; // 1/1000 mm Genauigkeit + + expect(B.pY).toBeCloseTo(A.pY, EPS); + expect(B.pZ).toBeCloseTo(A.pZ, EPS); + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + + expect(B.phi).toBeCloseTo(A.phi, EPS); + expect(B.theta).toBeCloseTo(A.theta, EPS); + }); + + + + + + + + + // --- phi/theta/psi Round-Trip (psi != 0) --- + // Prueft ob calculatePositionFromMotorAngles() auch die Orientierung + // (phi, theta, psi) korrekt zurueckrechnet, nicht nur x/y/z. + + function roundTrip(L1, L2, L3, x, y, z, phi, theta, psi) { + const A = new Robot(L1, L2, L3); + A.x = x; A.y = y; A.z = z; + A.phi = phi; A.theta = theta; A.psi = psi; + A.calculateAngles3D(); + + const B = new Robot(L1, L2, L3); + B.xMotor = A.xMotor; + B.alpha = A.alpha; + B.beta = A.beta; + B.a = A.a; + B.b = A.b; + B.c = A.c; + B.calculatePositionFromMotorAngles(); + return { A, B }; + } + + test("phi/theta/psi Round-Trip: psi = +45 Grad", () => { + const { A, B } = roundTrip(300, 200, 10, 10, 430, 30, Math.PI/7, Math.PI/2, Math.PI/4); + const EPS = 4; // 4 Dezimalstellen, ca. 0.1 mrad + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + expect(B.phi).toBeCloseTo(A.phi, EPS); + expect(B.theta).toBeCloseTo(A.theta, EPS); + expect(B.psi).toBeCloseTo(A.psi, EPS); + }); + + test("phi/theta/psi Round-Trip: psi = -30 Grad", () => { + const { A, B } = roundTrip(300, 200, 10, 10, 430, 30, Math.PI/7, Math.PI/2, -Math.PI/6); + const EPS = 4; + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + expect(B.phi).toBeCloseTo(A.phi, EPS); + expect(B.theta).toBeCloseTo(A.theta, EPS); + expect(B.psi).toBeCloseTo(A.psi, EPS); + }); + + test("phi/theta/psi Round-Trip: psi = +90 Grad, phi = -90 Grad", () => { + const { A, B } = roundTrip(300, 200, 10, 0, 450, 0, -Math.PI/2, Math.PI/2, Math.PI/2); + const EPS = 4; + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + expect(B.phi).toBeCloseTo(A.phi, EPS); + expect(B.theta).toBeCloseTo(A.theta, EPS); + expect(B.psi).toBeCloseTo(A.psi, EPS); + }); + + + // --- Null-Stellung und reale Positionen in -Y Richtung --- + // Der Roboter arbeitet mit y < 0 als Hauptrichtung. + // Null-Stellung: Arm voll ausgestreckt in -Y, Fingerspitze bei y=-(l1+l2+l3), z=0. + // Hand zeigt ebenfalls in -Y => Handvektor (Fingerspitze->Handgelenk) zeigt in +Y + // => phi = pi/2, theta = pi/2. + + test("Null-Stellung: Arm voll ausgestreckt in -Y, y=-(L1+L2+L3), z=0", () => { + const L1 = 300, L2 = 200, L3 = 10; + const { A, B } = roundTrip(L1, L2, L3, 0, -(L1+L2+L3), 0, Math.PI/2, Math.PI/2, 0); + const EPS = 4; + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + expect(B.phi).toBeCloseTo(A.phi, EPS); + expect(B.theta).toBeCloseTo(A.theta, EPS); + expect(B.psi).toBeCloseTo(A.psi, EPS); + }); + + test("Zwischenposition in -Y: Fingerspitze bei y=-400, z=100", () => { + const L1 = 300, L2 = 200, L3 = 10; + // Gleiche Handorientierung wie Null-Stellung: phi=pi/2, theta=pi/2 + const { A, B } = roundTrip(L1, L2, L3, 0, -400, 100, Math.PI/2, Math.PI/2, 0); + const EPS = 4; + expect(B.x).toBeCloseTo(A.x, EPS); + expect(B.y).toBeCloseTo(A.y, EPS); + expect(B.z).toBeCloseTo(A.z, EPS); + expect(B.phi).toBeCloseTo(A.phi, EPS); + expect(B.theta).toBeCloseTo(A.theta, EPS); + expect(B.psi).toBeCloseTo(A.psi, EPS); + }); + }); \ No newline at end of file