67 lines
2.5 KiB
JavaScript
67 lines
2.5 KiB
JavaScript
// Phase 1: Der reale Roboter arbeitet in -Y (robot.json: Arm1 -> [0,-250,0]).
|
|
// alpha=0 muss nach -y zeigen, nicht nach +y. Siehe doc/Info_Koordinaten.md.
|
|
const Robot = require('../robot/kinematics/Arm3SegmentLinearX');
|
|
const D = 180 / Math.PI;
|
|
|
|
describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => {
|
|
beforeAll(() => jest.spyOn(console, 'log').mockImplementation(() => {}));
|
|
afterAll(() => jest.restoreAllMocks());
|
|
|
|
const L1 = 250, L2 = 250, L3 = 90;
|
|
|
|
function fkFromMotors(alphaDeg, betaDeg, aDeg, bDeg, cDeg, xMotor = 0) {
|
|
const r = new Robot(L1, L2, L3);
|
|
r.xMotor = xMotor;
|
|
r.alpha = alphaDeg / D; r.beta = betaDeg / D;
|
|
r.a = aDeg / D; r.b = bDeg / D; r.c = cDeg / D;
|
|
r.calculatePositionFromMotorAngles();
|
|
return r;
|
|
}
|
|
|
|
test('voll ausgestreckt (alpha=beta=0, Hand gerade) -> y ~ -590', () => {
|
|
// B=180 = aktuelle "gerade Hand"-Konvention (Phase 2 macht daraus 0)
|
|
const r = fkFromMotors(0, 0, 0, 180, 0);
|
|
expect(r.y).toBeLessThan(0);
|
|
expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 0); // ~ -590
|
|
expect(r.z).toBeCloseTo(0, 6);
|
|
});
|
|
|
|
test('gemeldete Homing-Pose landet in -y (war faelschlich +405)', () => {
|
|
// G92 X160.53 Y4.53 Z13.93 A124.04 (B=C=0)
|
|
const r = fkFromMotors(4.53, 13.93, 124.04, 0, 0, 160.53);
|
|
expect(r.y).toBeLessThan(0);
|
|
expect(r.y).toBeCloseTo(-405, 0);
|
|
expect(r.z).toBeCloseTo(58, 0);
|
|
});
|
|
|
|
test('IK der -y-Grundstellung liefert alpha~0 / beta~0 (nicht ~180)', () => {
|
|
const ref = fkFromMotors(0, 0, 0, 180, 0); // -y-ausgestreckte Pose als Referenz
|
|
const r = new Robot(L1, L2, L3);
|
|
r.x = ref.x; r.y = ref.y; r.z = ref.z;
|
|
r.phi = ref.phi; r.theta = ref.theta; r.psi = ref.psi; r.e = 0;
|
|
r.calculateAngles3D();
|
|
expect(r.alpha).toBeCloseTo(0, 3);
|
|
expect(r.beta).toBeCloseTo(0, 3);
|
|
});
|
|
|
|
test('Round-Trip im -y-Arbeitsraum bleibt konsistent', () => {
|
|
const A = new Robot(L1, L2, L3);
|
|
A.x = 10; A.y = -430; A.z = 30;
|
|
A.phi = Math.PI / 7; A.theta = Math.PI / 2; A.psi = Math.PI / 6; A.e = 0;
|
|
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();
|
|
|
|
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);
|
|
});
|
|
});
|