ChK/appRobotDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

G92 sowie arctan2

Browse Source
This commit is contained in:
chk
2026-06-26 06:44:11 +02:00
parent 497d0fbc7b
commit 29b5f2ae4b
5 changed files with 445 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
doc/Info_G92.md
View File

@@ -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/π):

27
logs/gcode_commands.log
View File

@@ -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

6
logs/pings.log
View File

@@ -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

2
robot/kinematics/Arm3SegmentLinearX.js
View File

@@ -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);

558
test/Robot.Kinematics.RoundTrip.test.js
View File

@@ -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);
});
});