calculatePositionFromMotorAngles ist jetzt OK
This commit is contained in:
@@ -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,17 +128,56 @@ class Robot{
|
||||
while(this.a < -Math.PI){this.a += 2*Math.PI}
|
||||
|
||||
this.eMotor = this.e - this.b - this.c;
|
||||
|
||||
}
|
||||
|
||||
calculatePositionFromMotorAngles(){
|
||||
rotateAroundAxis(v, n, angle) {
|
||||
const cos = Math.cos(angle);
|
||||
const sin = Math.sin(angle);
|
||||
|
||||
// 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 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(verbose = false) {
|
||||
|
||||
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(){
|
||||
|
||||
@@ -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();
|
||||
|
||||
233
test/Robot.Kinematics.RoundTrip.test.js
Executable file
233
test/Robot.Kinematics.RoundTrip.test.js
Executable file
@@ -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);
|
||||
});
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
});
|
||||
105
test/Robot.Kinematics.RoundTripList.test.js
Executable file
105
test/Robot.Kinematics.RoundTripList.test.js
Executable file
@@ -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;
|
||||
}
|
||||
});
|
||||
|
||||
});
|
||||
|
||||
});
|
||||
|
||||
});
|
||||
Reference in New Issue
Block a user