G92 sowie arctan2

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

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

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

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

test/Robot.Kinematics.RoundTrip.test.js

@@ -5,7 +5,7 @@ describe("Robot Kinematics Roundtrip", () => {
 
     test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 1", () => {
 
-        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
+        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
         const L1 = 300;
         const L2 = 200;
         const L3 = 10;
@@ -34,7 +34,7 @@ describe("Robot Kinematics Roundtrip", () => {
             c:      A.c
         };
 
-        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
+        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
         const B = new Robot(L1, L2, L3);
 
         B.xMotor = motor.xMotor;
@@ -59,7 +59,7 @@ describe("Robot Kinematics Roundtrip", () => {
 
     test("calculateAngles3D() <-> calculatePositionFromMotorAngles()  handgelenk 2", () => {
 
-        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
+        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
         const L1 = 300;
         const L2 = 300;
         const L3 = 10;
@@ -88,7 +88,7 @@ describe("Robot Kinematics Roundtrip", () => {
             c:      A.c
         };
 
-        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
+        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
         const B = new Robot(L1, L2, L3);
 
         B.xMotor = motor.xMotor;
@@ -114,7 +114,7 @@ describe("Robot Kinematics Roundtrip", () => {
 
     test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 3", () => {
 
-        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
+        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
         const L1 = 300;
         const L2 = 200;
         const L3 = 10;
@@ -144,7 +144,7 @@ describe("Robot Kinematics Roundtrip", () => {
             c:      A.c
         };
 
-        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
+        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
         const B = new Robot(L1, L2, L3);
 
         B.xMotor = motor.xMotor;
@@ -170,7 +170,7 @@ describe("Robot Kinematics Roundtrip", () => {
 
     test("calculateAngles3D() <-> calculatePositionFromMotorAngles() handgelenk 4", () => {
 
-        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
+        // === Instanz A: Vorwärts-Kinematik (XYZ -> Motorwinkel) ===
         const L1 = 300;
         const L2 = 200;
         const L3 = 10;
@@ -200,7 +200,7 @@ describe("Robot Kinematics Roundtrip", () => {
             c:      A.c
         };
 
-        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
+        // === Instanz B: Rückwärts-Kinematik (Motorwinkel -> XYZ) ===
         const B = new Robot(L1, L2, L3);
 
         B.xMotor = motor.xMotor;
@@ -233,4 +233,92 @@ describe("Robot Kinematics Roundtrip", () => {
 
 
 
+
+
+    // --- 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);
+    });
+
 });