calculatePositionFromMotorAngles ist jetzt OK

2026-03-29 10:40:52 +02:00
parent 566d3894c4
commit a361d96802
4 changed files with 392 additions and 11 deletions
--- a/test/Robot.Kinematics.RoundTripList.test.js
+++ b/test/Robot.Kinematics.RoundTripList.test.js
@@ -0,0 +1,105 @@
+const Robot = require('../robot/Robot.js');
+
+describe("Robot Kinematics Roundtrip (parametrisiert)", () => {
+
+    const geometries = [
+        { L1: 300, L2: 200, L3: 10 },
+        { L1: 240, L2: 260, L3: 10 },
+        { L1: 200, L2: 300, L3: 10 },
+        { L1: 200, L2: 270, L3: 30 },
+        { L1: 300, L2: 300, L3: 40 },
+        { L1: 220, L2: 200, L3: 40 },
+    ];
+
+    const poses = [
+        // stabil mittig
+        { x: 100, y: 100, z: 0, phi: 0.3, theta: 1.0, psi: 0 },
+        { x: 100, y: 200, z: 0, phi: 2, theta: 0.2, psi: 0 },
+        { x: 100, y: 100, z: 30, phi: 0, theta: 0.2, psi: 0 },
+        { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: 0 },
+        { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: .3 },
+        { x: 100, y: 350, z: 40, phi: 0.1, theta: 1.2, psi: .9 },
+        { x: -100, y: 200, z: -40, phi: 0.1, theta: 1.2, psi: .9 },
+        { x: -100, y: 200, z: -240, phi: 0.1, theta: 1.2, psi: .9 },
+        { x: -100, y: 200, z: -240, phi: -0.5, theta: 1.2, psi: -0.9 },
+        { x: -100, y: 200, z: -240, phi: -0.5, theta: Math.PI - 0.3, psi: .9 },
+    ];
+
+    geometries.forEach(({ L1, L2, L3 }, gIndex) => {
+
+        poses.forEach((pose, pIndex) => {
+
+            test(`Roundtrip G${gIndex} P${pIndex}`, () => {
+
+                const A = new Robot(L1, L2, L3);
+
+                // Input setzen
+                A.x = pose.x;
+                A.y = pose.y;
+                A.z = pose.z;
+
+                A.phi   = pose.phi;
+                A.theta = pose.theta;
+                A.psi   = pose.psi;
+                A.e     = 0;
+
+                A.calculateAngles3D();
+
+                const motor = {
+                    xMotor: A.xMotor,
+                    alpha:  A.alpha,
+                    beta:   A.beta,
+                    a:      A.a,
+                    b:      A.b,
+                    c:      A.c
+                };
+
+                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;
+
+                B.calculatePositionFromMotorAngles();
+
+                const EPS = 2; // 10^-2 mm → realistisch bei trig
+
+                try {
+                    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);
+                    expect(B.psi).toBeCloseTo(A.psi, EPS);
+                } catch (err) {
+                    console.log("❌ Fehler bei:");
+                    console.log("Geometrie:", { L1, L2, L3 });
+                    console.log("Pose:", pose);
+
+                    console.log("Soll (A):", {
+                        x: A.x, y: A.y, z: A.z,
+                        pY: A.pY, pZ: A.pZ
+                    });
+
+                    console.log("Ist (B):", {
+                        x: B.x, y: B.y, z: B.z,
+                        pY: B.pY, pZ: B.pZ
+                    });
+
+                    console.log("Motor:", motor);
+
+                    throw err;
+                }
+            });
+
+        });
+
+    });
+
+});