105 lines
3.5 KiB
JavaScript
Executable File
105 lines
3.5 KiB
JavaScript
Executable File
const Robot = require('../robot/kinematics/Arm3SegmentLinearX');
|
|
|
|
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 = 0.02; // 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;
|
|
}
|
|
});
|
|
|
|
});
|
|
|
|
});
|
|
|
|
}); |