From a361d968020f1b50d8bcfd3a29d4a7fb09664fd9 Mon Sep 17 00:00:00 2001 From: ChK Date: Sun, 29 Mar 2026 10:40:52 +0200 Subject: [PATCH] calculatePositionFromMotorAngles ist jetzt OK --- robot/Robot.js | 57 ++++- robot/TelnetSenderGRBL.js | 8 +- test/Robot.Kinematics.RoundTrip.test.js | 233 ++++++++++++++++++++ test/Robot.Kinematics.RoundTripList.test.js | 105 +++++++++ 4 files changed, 392 insertions(+), 11 deletions(-) create mode 100755 test/Robot.Kinematics.RoundTrip.test.js create mode 100755 test/Robot.Kinematics.RoundTripList.test.js diff --git a/robot/Robot.js b/robot/Robot.js index 78d7ac0..84f5e04 100755 --- a/robot/Robot.js +++ b/robot/Robot.js @@ -66,7 +66,7 @@ class Robot{ } // Berechnet aus XYZ die Motor-Winkel für den GCode - calculateAngles3D(){ + calculateAngles3D(verbose){ while(this.phi > Math.PI){this.phi -= 2*Math.PI} while(this.phi < -Math.PI){this.phi += 2*Math.PI} while(this.theta > Math.PI){this.theta -= 2*Math.PI} @@ -104,6 +104,8 @@ class Robot{ var nZ = -1.0*Math.sin(this.theta)*Math.cos(this.phi)*Math.cos(this.beta); var nBetrag = Math.sqrt(nX*nX + nY*nY + nZ*nZ); + if(verbose) console.log("Richtung: > ", nX/nBetrag, nY/nBetrag, nZ/nBetrag); + var cosA = (nX)/nBetrag; this.a = Math.acos(cosA) if(Math.cos(this.phi) > 0){this.a = -this.a} @@ -126,19 +128,58 @@ class Robot{ while(this.a < -Math.PI){this.a += 2*Math.PI} this.eMotor = this.e - this.b - this.c; + } + + rotateAroundAxis(v, n, angle) { + const cos = Math.cos(angle); + const sin = Math.sin(angle); + const dot = v.x*n.x + v.y*n.y + v.z*n.z; + + const cross = { + x: n.y*v.z - n.z*v.y, + y: n.z*v.x - n.x*v.z, + z: n.x*v.y - n.y*v.x + }; + + return { + x: v.x*cos + cross.x*sin + n.x*dot*(1 - cos), + y: v.y*cos + cross.y*sin + n.y*dot*(1 - cos), + z: v.z*cos + cross.z*sin + n.z*dot*(1 - cos) + }; } - calculatePositionFromMotorAngles(){ + calculatePositionFromMotorAngles(verbose = false) { - // Hier kommt generierter Code! Das ist höchst Fraglich !! - this.x = this.xMotor - this.l3*Math.sin(this.theta)*Math.cos(this.phi); - this.y = this.l1*Math.cos(this.alpha) + this.l2*Math.cos(this.beta) - this.l3*Math.sin(this.theta)*Math.sin(this.phi); - this.z = this.l1*Math.sin(this.alpha) + this.l2*Math.sin(this.beta) - this.l3*Math.cos(this.theta); - // Das war der generierte Code + const vecBizeps = {x: this.xMotor, y: this.l1 * Math.cos(this.alpha), z: this.l1 * Math.sin(this.alpha)} + const vecUnterarm = {x: 0, y: Math.cos(this.beta), z: Math.sin(this.beta)} + // der Handgelenk-Punkt + this.pX = vecBizeps.x + this.l2 * vecUnterarm.x; + this.pY = vecBizeps.y + this.l2 * vecUnterarm.y; + this.pZ = vecBizeps.z + this.l2 * vecUnterarm.z; + + // n: Die Handgelenk-Unterarm-Knick-Achse. X-Achse wird um den Unterarm gedreht. + const n = { x: -Math.cos(this.a), y: vecUnterarm.z * Math.sin(this.a), z: -vecUnterarm.y * Math.sin(this.a) }; + + if(verbose) console.log("n inverse:", n.x, n.y, n.z); + + const vHand = this.rotateAroundAxis(vecUnterarm, n, this.b); + + this.x = this.pX - this.l3 * vHand.x; + this.y = this.pY - this.l3 * vHand.y; + this.z = this.pZ - this.l3 * vHand.z; + + this.theta = Math.atan2(Math.sqrt(vHand.x*vHand.x + vHand.y*vHand.y),vHand.z); + this.phi = Math.atan2(vHand.y, vHand.x); + this.psi = this.c - Math.acos(-n.z); + + while(this.phi > Math.PI){this.phi -= 2*Math.PI} + while(this.phi < -Math.PI){this.phi += 2*Math.PI} + while(this.theta > Math.PI){this.theta -= 2*Math.PI} + while(this.theta < -Math.PI){this.theta += 2*Math.PI} } - + sendCommand(){ if(this.motorPosition == null){this.createMotorPosition() } this.motorPositionOld = this.motorPosition; diff --git a/robot/TelnetSenderGRBL.js b/robot/TelnetSenderGRBL.js index 5500467..453e4aa 100755 --- a/robot/TelnetSenderGRBL.js +++ b/robot/TelnetSenderGRBL.js @@ -52,9 +52,11 @@ module.exports = class TelnetSenderGRBL{ } - - moveTo(mOld, mNew){ + this.translateAxisNames(mOld, mNew, "G1") + } + + translateAxisNames(mOld, mNew, strCommand = "G1"){ // The Hand-Turn is not 1:1 to the Hand-Lift ° var factorTurnLift = 1.2; @@ -65,7 +67,7 @@ module.exports = class TelnetSenderGRBL{ // Hand-Open in mm var handOpenInMM = 1.0 - var data = "G1" + var data = strCommand; if(this.xAxisGrbl == "x"){ data += " x" + (mNew.x).toFixed(2).toString(); diff --git a/test/Robot.Kinematics.RoundTrip.test.js b/test/Robot.Kinematics.RoundTrip.test.js new file mode 100755 index 0000000..33850f4 --- /dev/null +++ b/test/Robot.Kinematics.RoundTrip.test.js @@ -0,0 +1,233 @@ +// __tests__/Robot.inverseKinematics.test.js +const Robot = require('../robot/Robot.js'); + +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; + + // 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); + }); + + + + + + +}); \ No newline at end of file diff --git a/test/Robot.Kinematics.RoundTripList.test.js b/test/Robot.Kinematics.RoundTripList.test.js new file mode 100755 index 0000000..e53533b --- /dev/null +++ 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; + } + }); + + }); + + }); + +}); \ No newline at end of file