ChK/appRobotDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Neues Kinematics

Browse Source
This commit is contained in:
chk
2026-06-11 07:57:51 +02:00
parent efe04b731f
commit 05355facf1
8 changed files with 678 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

140
test/Robot.GrabIt.RoundTrip.test.js Normal file
View File

@@ -0,0 +1,140 @@
// Kinematik-RoundTrip-Tests für den Joy-IT „Grab-It" (Robot02),
// implementiert als Arm3SegmentRotaryBase (Roadmap 12, Phase 3).
const Arm3SegmentRotaryBase = require('../robot/kinematics/Arm3SegmentRotaryBase');
const { createRobotFromEnv } = require('../robot/KinematicsFactory');
const RobotBase = require('../robot/RobotBase');
// Default-Geometrie (Schätzwerte): l1=105, l2=98, l3=100, baseHeight=110
const mk = () => new Arm3SegmentRotaryBase();
describe('Arm3SegmentRotaryBase — Workspace → Motor → Workspace', () => {
// Erreichbare Workspace-Punkte mit konsistentem (impliziten) Azimut.
const cases = [
{ x: 200, y: 0, z: 110, theta: Math.PI / 2, psi: 0.0, e: 10 },
{ x: 120, y: 120, z: 200, theta: Math.PI / 3, psi: -0.5, e: 20 },
{ x: -90, y: 90, z: 150, theta: Math.PI / 2, psi: 0.4, e: 5 },
{ x: 80, y: -60, z: 90, theta: 2 * Math.PI / 3, psi: 0.2, e: 0 },
];
test.each(cases)('roundtrip x=$x y=$y z=$z', (c) => {
// A: Inverse (Workspace → Motorwinkel)
const A = mk();
A.x = c.x; A.y = c.y; A.z = c.z;
A.theta = c.theta; A.psi = c.psi; A.e = c.e;
A.calculateAngles3D();
// B: Vorwärts (Motorwinkel → Workspace)
const B = mk();
B.xMotor = A.xMotor;
B.alpha = A.alpha;
B.beta = A.beta;
B.a = A.a;
B.b = A.b;
B.eMotor = A.eMotor;
B.calculatePositionFromMotorAngles();
// Die steuerbaren DOF müssen exakt zurückkommen.
expect(B.x).toBeCloseTo(c.x, 4);
expect(B.y).toBeCloseTo(c.y, 4);
expect(B.z).toBeCloseTo(c.z, 4);
expect(B.theta).toBeCloseTo(c.theta, 5);
expect(B.psi).toBeCloseTo(c.psi, 5);
// phi ist gekoppelt: gleich dem Azimut der Position (5-DOF-Constraint).
expect(B.phi).toBeCloseTo(Math.atan2(c.y, c.x), 5);
});
});
describe('Arm3SegmentRotaryBase — Motor → Workspace → Motor', () => {
// Motorwinkel mit Ellbogen im gewählten Zweig (beta ∈ [0, π]).
const motors = [
{ xMotor: 0.5, alpha: 0.8, beta: 0.6, a: -0.3, b: 0.2, eMotor: 15 },
{ xMotor: -0.7, alpha: 0.4, beta: 1.0, a: 0.5, b: -0.4, eMotor: 30 },
{ xMotor: 1.2, alpha: 1.1, beta: 0.3, a: -0.6, b: 0.1, eMotor: 0 },
];
test.each(motors)('roundtrip alpha=$alpha beta=$beta', (m) => {
// A: Vorwärts (Motor → Workspace)
const A = mk();
Object.assign(A, m);
A.calculatePositionFromMotorAngles();
// B: Inverse (Workspace → Motor)
const B = mk();
B.x = A.x; B.y = A.y; B.z = A.z;
B.theta = A.theta; B.psi = A.psi; B.e = A.eMotor;
B.calculateAngles3D();
expect(B.xMotor).toBeCloseTo(m.xMotor, 5);
expect(B.alpha).toBeCloseTo(m.alpha, 5);
expect(B.beta).toBeCloseTo(m.beta, 5);
expect(B.a).toBeCloseTo(m.a, 5);
expect(B.b).toBeCloseTo(m.b, 5);
expect(B.eMotor).toBeCloseTo(m.eMotor, 5);
});
});
describe('Arm3SegmentRotaryBase — 5-DOF-Eigenschaften', () => {
test('phi-Eingabe wird in der Inversen ignoriert', () => {
const base = { x: 150, y: 60, z: 130, theta: Math.PI / 2, psi: 0.1, e: 0 };
const withPhi = mk();
Object.assign(withPhi, base, { phi: 2.5 }); // bewusst widersprüchliches phi
withPhi.calculateAngles3D();
const noPhi = mk();
Object.assign(noPhi, base, { phi: -1.0 }); // anderes phi
noPhi.calculateAngles3D();
// Trotz unterschiedlicher phi-Eingabe identische Motorwinkel.
for (const k of ['xMotor', 'alpha', 'beta', 'a', 'b', 'eMotor']) {
expect(noPhi[k]).toBeCloseTo(withPhi[k], 9);
}
// phi wird aus der Position abgeleitet.
expect(withPhi.xMotor).toBeCloseTo(Math.atan2(base.y, base.x), 9);
});
test('c-Slot bleibt ungenutzt (= 0)', () => {
const r = mk();
r.x = 180; r.y = 0; r.z = 120; r.theta = Math.PI / 2; r.psi = 0; r.e = 0;
r.calculateAngles3D();
expect(r.c).toBe(0);
});
test('unerreichbarer Punkt lässt die Motor-Slots unverändert', () => {
const r = mk();
r.alpha = 0; r.beta = 0; r.a = 0; // bekannter Startzustand
r.x = 5000; r.y = 0; r.z = 0; r.theta = Math.PI / 2; r.psi = 0; r.e = 0;
r.calculateAngles3D();
// R > l1+l2 → frühzeitiges return, alpha/beta/a unverändert
expect(r.alpha).toBe(0);
expect(r.beta).toBe(0);
expect(r.a).toBe(0);
});
});
describe('KinematicsFactory lädt den Grab-It', () => {
test('Alias "grabit" liefert eine Arm3SegmentRotaryBase-Instanz', () => {
const robot = createRobotFromEnv({ ROBOT_KINEMATICS: 'grabit' });
expect(robot).toBeInstanceOf(Arm3SegmentRotaryBase);
expect(robot).toBeInstanceOf(RobotBase);
});
test('ROBOT_KINEMATICS_PARAMS setzt Längen und baseHeight', () => {
const robot = createRobotFromEnv({
ROBOT_KINEMATICS: 'robot02',
ROBOT_KINEMATICS_PARAMS: '{"l1":110,"l2":100,"l3":95,"baseHeight":130}',
});
expect([robot.l1, robot.l2, robot.l3]).toEqual([110, 100, 95]);
expect(robot.baseHeight).toBe(130);
});
test('Default-baseHeight wenn nicht gesetzt', () => {
const robot = createRobotFromEnv({ ROBOT_KINEMATICS: 'grabit' });
expect(robot.baseHeight).toBe(110);
});
});

150
test/Robot.PortInverse.test.js Normal file
View File

@@ -0,0 +1,150 @@
/**
* Verifikation der Port→Motor-Rückrechnung für ToDo_9 / Paket 4 (Sync-Command).
*
* Herleitung: doc/ToDo_9a_PortRueckrechnung.md
*
* Kernaussage: Für die PRODUKTIV-Verkabelung (startRobot.js) ist die Abbildung
* der sieben Motorwerte auf die sieben gesendeten GRBL-Achswerte LINEAR und
* EINDEUTIG umkehrbar. `motorStateFromPorts()` ist diese Umkehrung; die Tests
* beweisen sie gegen den echten Sende-Pfad (`execCommand`) und gegen `portValue`.
*
* Damit ist klar: die B3-Mehrdeutigkeit (Ellbogen oben/unten) betrifft nur die
* kartesische Inverskinematik (`calculateAngles3D`), die der Sync gar nicht nutzt.
*/
const TelnetSender = require('../robot/TelnetSenderGRBL');
const MotorPosition = require('../robot/RobotMotorPosition');
const Robot = require('../robot/kinematics/Arm3SegmentLinearX');
const D = 180 / Math.PI;
/* ---------- Produktiv-Verkabelung (1:1 wie startRobot.js) ---------- */
function buildSenders() {
const mk = (...args) => new TelnetSender('test.test', ...args); // test.test → Fake-tSocket
return {
base: mk(2300, 'x', 'y', 'z'), // GRBL x←xMotor, y←alpha, z←(beta-alpha)
elbow: mk(5000, 'a', null, null), // GRBL x←a
hand: mk(5000, 'c', 'e', 'b'), // GRBL x←(c-b), y←eMotor, z←b
};
}
/* ---------- Die Umkehrung: GRBL-Readings → 7 Motorwerte ---------- */
// r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} } (GRBL-Achswerte, Grad bzw. mm)
function motorStateFromPorts(r) {
const xMotor = r.base.x; // x-Port = xMotor (mm, direkt)
const alpha = r.base.y / D; // y-Port = alpha·D
const beta = (r.base.z + r.base.y) / D; // z-Port = (beta-alpha)·D ⇒ beta = z/D + alpha
const a = r.elbow.x / D; // Elbow x-Port = a·D
const b = r.hand.z / D; // Hand z-Port = b·D
const c = (r.hand.x + r.hand.z) / D; // Hand x-Port = (c-b)·D ⇒ c = x/D + b
const eMotor = r.hand.y / D; // Hand y-Port = eMotor·D
return { xMotor, alpha, beta, a, b, c, eMotor };
}
/* ---------- Hilfen ---------- */
// {x,y,z,a,b,c,e}-pos aus Motorwerten (Feld-Mapping der RobotMotorPosition)
function posFromMotors(m) {
return { x: m.xMotor, y: m.alpha, z: m.beta, a: m.a, b: m.b, c: m.c, e: m.eMotor };
}
// Port-Readings über die reine Funktion portValue (volle Präzision)
function portsViaPortValue(S, pos) {
return {
base: { x: S.base.portValue('x', 'x', pos), y: S.base.portValue('y', 'y', pos), z: S.base.portValue('z', 'z', pos) },
elbow: { x: S.elbow.portValue('x', 'a', pos) },
hand: { x: S.hand.portValue('x', 'c', pos), y: S.hand.portValue('y', 'e', pos), z: S.hand.portValue('z', 'b', pos) },
};
}
// Achswerte aus einer real gesendeten G-Code-Zeile (z. B. "G1 x12.34 y90.00 z-90.00")
function parseAxes(line) {
const out = {};
const re = /([xyz])(-?\d+(?:\.\d+)?)/g;
let m;
while ((m = re.exec(line)) !== null) out[m[1]] = parseFloat(m[2]);
return out;
}
// Port-Readings über den ECHTEN Sende-Pfad execCommand (inkl. 2-Dezimal-Rundung)
function portsViaExecCommand(S, m) {
const mNew = new MotorPosition(m.xMotor, m.alpha, m.beta, m.a, m.b, m.c, m.eMotor);
for (const f of ['xMotorChanged','yMotorChanged','zMotorChanged','aMotorChanged','bMotorChanged','cMotorChanged','eMotorChanged']) mNew[f] = true;
const mOld = new MotorPosition(0, 0, 0, 0, 0, 0, 0);
S.base.execCommand('G1', mOld, mNew);
S.elbow.execCommand('G1', mOld, mNew);
S.hand.execCommand('G1', mOld, mNew);
return {
base: parseAxes(S.base.tSocket.written),
elbow: parseAxes(S.elbow.tSocket.written),
hand: parseAxes(S.hand.tSocket.written),
};
}
/* ---------- Repräsentative Motorzustände (plausible Bereiche) ---------- */
const CASES = [
{ name: 'Nullstellung', xMotor: 0, alpha: 0, beta: 0, a: 0, b: 0, c: 0, eMotor: 0 },
{ name: 'gemischt klein', xMotor: 100, alpha: 0.30, beta: 0.50, a: 0.20, b: 0.40, c: 0.90, eMotor: 1.0 },
{ name: 'negative Winkel', xMotor: -55, alpha:-0.70, beta:-0.25, a:-1.10, b:-0.60, c:-0.30, eMotor:-0.5 },
{ name: 'große Winkel', xMotor: 250, alpha: 1.20, beta: 2.00, a: 2.90, b: 1.50, c: 3.00, eMotor: 2.5 },
{ name: 'gekoppelt c≈b', xMotor: 42, alpha: 0.10, beta: 0.10, a: 0.05, b: 1.20, c: 1.25, eMotor: 0.0 },
];
beforeAll(() => { jest.spyOn(console, 'log').mockImplementation(() => {}); });
afterAll(() => { jest.restoreAllMocks(); });
describe('Port→Motor-Rückrechnung (ToDo_9, Baustein für Paket 4)', () => {
describe('A) exakte Umkehrung gegen portValue (volle Präzision)', () => {
for (const c of CASES) {
test(c.name, () => {
const S = buildSenders();
const ports = portsViaPortValue(S, posFromMotors(c));
const rec = motorStateFromPorts(ports);
for (const k of ['xMotor','alpha','beta','a','b','c','eMotor']) {
expect(rec[k]).toBeCloseTo(c[k], 9);
}
});
}
});
describe('B) Umkehrung gegen den echten Sende-Pfad execCommand (mit Rundung)', () => {
for (const c of CASES) {
test(c.name, () => {
const S = buildSenders();
const ports = portsViaExecCommand(S, c);
const rec = motorStateFromPorts(ports);
// xMotor in mm (Rundung 0.01) → 0.02 Toleranz; Winkel in rad (Rundung 0.01° je Port) → 1e-3
expect(rec.xMotor).toBeCloseTo(c.xMotor, 2);
for (const k of ['alpha','beta','a','b','c','eMotor']) {
expect(Math.abs(rec[k] - c[k])).toBeLessThan(1e-3);
}
});
}
});
describe('C) Voll-Kette Sync: Ports → Motoren → Vorwärtskinematik → Pose', () => {
// Beweist, dass die rekonstruierten Motorwerte über die Vorwärtskinematik
// exakt dieselbe Pose liefern wie die Originalwerte (= was Sync dem Client meldet).
for (const c of CASES) {
test(c.name, () => {
const S = buildSenders();
// Referenz-Pose direkt aus den Original-Motorwerten
const ref = new Robot(250, 264, 100);
Object.assign(ref, { xMotor: c.xMotor, alpha: c.alpha, beta: c.beta, a: c.a, b: c.b, c: c.c, eMotor: c.eMotor });
ref.calculatePositionFromMotorAngles();
// Sync-Pfad: Ports → Inverse → Motoren setzen → Vorwärtskinematik
const rec = motorStateFromPorts(portsViaPortValue(S, posFromMotors(c)));
const sync = new Robot(250, 264, 100);
Object.assign(sync, { xMotor: rec.xMotor, alpha: rec.alpha, beta: rec.beta, a: rec.a, b: rec.b, c: rec.c, eMotor: rec.eMotor });
sync.calculatePositionFromMotorAngles();
for (const k of ['x','y','z','phi','theta','psi']) {
expect(sync[k]).toBeCloseTo(ref[k], 6);
}
});
}
});
});