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

Umbau 12: Robot-Kinematics als extends RobotBase

Browse Source
This commit is contained in:
chk
2026-06-10 23:18:29 +02:00
parent 9274b2c52a
commit d906b094b1
11 changed files with 837 additions and 385 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
robot/KinematicsFactory.js Normal file
View File

@@ -0,0 +1,76 @@
// robot/KinematicsFactory.js — Phase 2 der Roadmap 12.
//
// Wählt anhand der Umgebungsvariable ROBOT_KINEMATICS eine Kinematik-Klasse aus
// `robot/kinematics/` und instantiiert sie mit den Parametern aus
// ROBOT_KINEMATICS_PARAMS (JSON). Eine unbekannte Kinematik führt zu einem
// klaren Fehler beim Start — kein silent fail.
//
// Liest process.env vorerst direkt (gleicher Stil wie ROBOT_DEFAULT_FEEDRATE in
// RobotBase). Ein späteres zentrales Config-Modul (ToDo_3_Config) kann diese
// Variablen übernehmen.
/** Default-Bezeichner der Kinematik, wenn ROBOT_KINEMATICS nicht gesetzt ist. */
const DEFAULT_KINEMATICS = 'arm3segmentlinearx';
// Registry: Bezeichner (lowercase) → Modulpfad relativ zu dieser Datei.
// Neue Kinematiken hier eintragen (siehe Phase 3).
const KINEMATICS_REGISTRY = {
arm3segmentlinearx: './kinematics/Arm3SegmentLinearX',
};
/**
* Lädt die Kinematik-Klasse zu einem Bezeichner.
* @param {string} [identifier] z. B. 'arm3segmentlinearx' (case-insensitive)
* @returns {Function} die Konstruktor-Klasse
* @throws {Error} bei unbekanntem Bezeichner
*/
function loadKinematicsClass(identifier = DEFAULT_KINEMATICS) {
const key = String(identifier).toLowerCase();
const modulePath = KINEMATICS_REGISTRY[key];
if (!modulePath) {
const known = Object.keys(KINEMATICS_REGISTRY).join(', ');
throw new Error(
`Unbekannte Kinematik "${identifier}" (ROBOT_KINEMATICS). Verfügbar: ${known}.`
);
}
return require(modulePath);
}
/**
* Parst ROBOT_KINEMATICS_PARAMS (JSON) zu einem Parameter-Objekt.
* @param {string} [raw] JSON-String, z. B. '{"l1":250,"l2":264,"l3":100}'
* @returns {Object} geparste Parameter (leeres Objekt, wenn nicht gesetzt)
* @throws {Error} bei ungültigem JSON
*/
function parseParams(raw) {
if (!raw) return {};
try {
return JSON.parse(raw);
} catch (err) {
throw new Error(`ROBOT_KINEMATICS_PARAMS ist kein gültiges JSON: ${err.message}`);
}
}
/**
* Erzeugt eine Roboter-Instanz anhand der Umgebungsvariablen.
*
* @param {Object} [env] Umgebungs-Objekt (Default: process.env)
* @param {Object} [defaultParams] Fallback-Parameter für die Default-Kinematik
* (z. B. die Armlängen der produktiven Standard-Hardware). Werte aus
* ROBOT_KINEMATICS_PARAMS überschreiben diese.
* @returns {import('./RobotBase')} die instantiierte Kinematik
*/
function createRobotFromEnv(env = process.env, defaultParams = {}) {
const identifier = env.ROBOT_KINEMATICS || DEFAULT_KINEMATICS;
const params = { ...defaultParams, ...parseParams(env.ROBOT_KINEMATICS_PARAMS) };
const KinematicsClass = loadKinematicsClass(identifier);
return new KinematicsClass(params.l1, params.l2, params.l3);
}
module.exports = {
createRobotFromEnv,
loadKinematicsClass,
parseParams,
DEFAULT_KINEMATICS,
KINEMATICS_REGISTRY,
};

378
robot/Robot.js
View File

@@ -1,369 +1,9 @@
const MotorPosition = require('./RobotMotorPosition.js')
class Robot{
constructor(l1, l2, l3) {
// Umgebungsvariablen-Logik
const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ?
Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000;
// Speed-Regelung-Schalter: 'legacy' (Default — exakt wie bisher) oder 'correct'.
// Siehe doc/ToDo_6a_Speed.md.
this.speedMode = (process.env.ROBOT_SPEED_MODE || 'legacy').toLowerCase();
// ROBOT_USE_SPEED_CALC bleibt der interne Schalter für calculateSpeeds();
// der Korrekt-Modus aktiviert die Berechnung automatisch.
this.useSpeedCalc = this.speedMode === 'correct' ||
process.env.ROBOT_USE_SPEED_CALC === 'true' ||
process.env.ROBOT_USE_SPEED_CALC === '1';
/** @type {number} Bewegungszeit des letzten Schritts in Minuten (für koordinierte Feedrate) */
this.lastMoveTime = 0;
/** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */
this.speedX = 200;
/** @type {number} Bewegungsgeschwindigkeit Y-Achse in mm/min */
this.speedY = 200;
/** @type {number} Bewegungsgeschwindigkeit Z-Achse in mm/min */
this.speedZ = 200;
/** @type {number} Zeitstempel des zuletzt gesendeten Kommandos */
this.lastCommandSend = 0;
if(this.lastCommandSend == 0){ this.lastCommandSend = Date.now() };
/** @type {boolean} Animation aktiviert */
this.doAnimate = false;
/** @type {number} Länge des Oberarms in mm */
this.l1 = l1;
/** @type {number} Länge des Unterarms in mm */
this.l2 = l2;
/** @type {number} Länge der Hand (Endeffector) in mm */
this.l3 = l3;
// Plan-Koordinaten - XYZ FingerSpitze
/** @type {number} X-Position der Fingerspitze in mm */
this.x = 0;
/** @type {number} Y-Position der Fingerspitze in mm */
this.y = 0;
/** @type {number} Z-Position der Fingerspitze in mm */
this.z = 0;
// Plan-Koordinaten - HandRichtung (Euler-Winkel)
/** @type {number} Phi - Euler-Winkel (Längengrad): Rotation um Z-Achse in rad */
this.phi = 0.0;
/** @type {number} Theta - Euler-Winkel (Breitengrad): Neigungswinkel der Handachse in rad */
this.theta = -Math.PI/2;
/** @type {number} Psi - Euler-Winkel: Zusätzliche Drehung des Handgelenks in rad */
this.psi = 0.0;
/** @type {number} Finger-Abstands-Einstellung (Öffnungsweite) */
this.e = 0.0;
/** @type {number} Feedrate für Bewegungen in mm/min */
this.feedrate = DEFAULT_FEEDRATE;
/** @type {Object} Motor-Geschwindigkeiten in Einheiten pro Minute */
this.motorSpeeds = {x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0};
// Zwischen-Ergebnisse: Handgelenk-Punkt (Koordinaten des Handgelenks, nur für Tests public)
/** @type {number} Handgelenk-Position X in mm (berechneter Zwischenwert) */
this.pX = 0.0;
/** @type {number} Handgelenk-Position Y in mm (berechneter Zwischenwert) */
this.pY = 0.0;
/** @type {number} Handgelenk-Position Z in mm (berechneter Zwischenwert) */
this.pZ = 0.0;
// Motor-Koordinaten - Schulter, Ellebogen, Hand-Dreher
/** @type {number} X-Motor-Position (Schulterposition auf X-Schiene) in mm */
this.xMotor = 0;
/** @type {number} Alpha - Y-Motor-Winkel (Schulterposition) in rad */
this.alpha = 0;
/** @type {number} Beta - Z-Motor-Winkel (Unterarm-Neigung unter Y-Achse) in rad */
this.beta = 0;
this.xMotorChanged = false;
this.yMotorChanged = false;
this.zMotorChanged = false;
// Motor-Winkel für's Handgelenk
/** @type {number} a-Motor-Winkel: Rotation am Ellbogen in rad */
this.a = 0;
/** @type {number} b-Motor-Winkel: Handgelenk-Knicker-Winkel in rad */
this.b = 0;
/** @type {number} c-Motor-Winkel: Hand-Dreher-Rotation in rad */
this.c = 0;
this.aMotorChanged = false;
this.bMotorChanged = false;
this.cMotorChanged = false;
this.eMotorChanged = false;
/** @type {number} e-Motor-Wert: Finger-Abstands-Motor-Position */
this.eMotor = 0;
/** @type {number} Zeitstempel des letzten verarbeiteten Kommandos */
this.oldCommandTime = Date.now();
/** @type {Function[]} Array von Visualisierungs-Funktionen */
this.showFunctions = [];
/** @type {Object[]} Gespeicherte Roboterpositionen/Punkte */
this.savedPoints = [];
/** @type {number} Index des aktuell angesteuerten Punktes */
this.atPointNr = 0;
/** @type {number} Zeitstempel des aktuellen Punktes in ms */
this.t = 0;
/** @type {boolean} Relative oder absolute Bewegung (true = relativ) */
this.moveRelative = true;
/** @type {Object[]} Array von Kommando-Empfängern */
this.cmdReceivers = [];
}
createMotorPosition(){
this.motorPosition = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);
// Setze Changed-Flags basierend auf Änderungen seit der letzten Position
this.motorPosition.xMotorChanged = this.motorPositionOld ? this.xMotor !== this.motorPositionOld.x : true;
this.motorPosition.yMotorChanged = this.motorPositionOld ? this.alpha !== this.motorPositionOld.y : true;
this.motorPosition.zMotorChanged = this.motorPositionOld ? this.beta !== this.motorPositionOld.z : true;
this.motorPosition.aMotorChanged = this.motorPositionOld ? this.a !== this.motorPositionOld.a : true;
this.motorPosition.bMotorChanged = this.motorPositionOld ? this.b !== this.motorPositionOld.b : true;
this.motorPosition.cMotorChanged = this.motorPositionOld ? this.c !== this.motorPositionOld.c : true;
this.motorPosition.eMotorChanged = this.motorPositionOld ? this.eMotor !== this.motorPositionOld.e : true;
// Setze Handgelenk-Koordinaten (für Speed-Berechnung)
this.motorPosition.pX = this.pX;
this.motorPosition.pY = this.pY;
this.motorPosition.pZ = this.pZ;
// Setze Geschwindigkeiten
this.motorPosition.speeds = {...this.motorSpeeds};
this.motorPosition.feedrate = this.feedrate || 200;
// Speed-Regelung: Modus und (vorläufige) Bewegungszeit für die Sender
this.motorPosition.speedMode = this.speedMode;
this.motorPosition.moveTime = this.lastMoveTime || 0;
}
// Berechnet aus XYZ die Motor-Winkel für den GCode
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}
while(this.theta < -Math.PI){this.theta += 2*Math.PI}
// Handgelenk-Punkt ausrechnen:
this.pX = this.x + this.l3*Math.sin(this.theta)*Math.cos(this.phi);
this.pY = this.y + this.l3*Math.sin(this.theta)*Math.sin(this.phi);
this.pZ = this.z + this.l3*Math.cos(this.theta);
var pX = this.pX;
var pY = this.pY;
var pZ = this.pZ;
this.xMotor = pX;
// Ziel-Punkt ausrechnen ==> 2D Rechnung Arm
var r = Math.sqrt(pY * pY + pZ * pZ);
if (r > (this.l1 + this.l2)) { return; }
if (r == 0) { return; }
var gamma = Math.asin(pZ / r);
var delta = Math.acos((this.l1 * this.l1 + this.l2 * this.l2 - r * r) / (2 * this.l1 * this.l2));
this.alpha = Math.acos((this.l1 * this.l1 + r * r - this.l2 * this.l2) / (2 * r * this.l1)) + gamma;
this.beta = -Math.PI + (this.alpha + delta);
// Ende <== 2D Rechnung Arm
// Richtung der Hand ausgerechnet
// Arm = (0, cos(beta), sin(beta)) Punkt = (sin(theta)cos(phi), sin(theta)sin(phi), cos(theta))
//
// Unterarm muss gedreht werden. Aus der Y-Z-Ebene raus. Hin in die Ebene n x r
// wobei n = Unterarm x (P-O) ist
var nX = Math.cos(this.beta)*Math.cos(this.theta) - Math.sin(this.theta)*Math.sin(this.phi)*Math.sin(this.beta);
var nY = Math.sin(this.beta)*Math.sin(this.theta)*Math.cos(this.phi);
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}
if(Math.sin(this.theta) < 0) {this.a = -this.a}
// Handgelenk-Knick-Winkel ist zwischen Arm und Punkt-O
var cosB = Math.cos(this.beta)*Math.sin(this.theta)*Math.sin(this.phi) + Math.sin(this.beta)*Math.cos(this.theta);
this.b = Math.acos(cosB);
// Winkel zwischen n und z muss rumgedreht werden.
var cosC = - nZ / nBetrag;
this.c = Math.acos(cosC);
this.c += this.psi;
// a um 180° drehen
this.a += Math.PI;
while(this.a > Math.PI){this.a -= 2*Math.PI}
while(this.a < -Math.PI){this.a += 2*Math.PI}
this.eMotor = this.e - this.b - this.c;
}
// Berechnet die Motor-Geschwindigkeiten basierend auf Feedrate und Positionsänderung
calculateSpeeds(oldPos, newPos) {
if (!this.useSpeedCalc) return; // Neue Logik nur aktivieren, wenn Flag gesetzt
if (!oldPos || !newPos || this.feedrate <= 0) return;
this.lastMoveTime = 0; // wird unten gesetzt, sobald eine Bewegung erkannt wird
// 1. Berechne xyz-Distanz (primär)
const dx = newPos.x - oldPos.x;
const dy = newPos.y - oldPos.y;
const dz = newPos.z - oldPos.z;
const xyz_dist = Math.sqrt(dx*dx + dy*dy + dz*dz);
if (xyz_dist > 0.001) {
const time = xyz_dist / this.feedrate;
this.lastMoveTime = time;
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
this.motorSpeeds.a = (this.a - oldPos.a) / time;
this.motorSpeeds.b = (this.b - oldPos.b) / time;
this.motorSpeeds.c = (this.c - oldPos.c) / time;
this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
return;
}
// 2. Berechne Handgelenk-Punkt-Distanz (falls xyz = 0)
const dpx = newPos.pX - oldPos.pX;
const dpy = newPos.pY - oldPos.pY;
const dpz = newPos.pZ - oldPos.pZ;
const handgelenk_dist = Math.sqrt(dpx*dpx + dpy*dpy + dpz*dpz);
if (handgelenk_dist > 0.001) {
const time = handgelenk_dist / this.feedrate;
this.lastMoveTime = time;
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
this.motorSpeeds.a = (this.a - oldPos.a) / time;
this.motorSpeeds.b = (this.b - oldPos.b) / time;
this.motorSpeeds.c = (this.c - oldPos.c) / time;
this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
return;
}
// 3. Berechne Finger-Distanz (falls Handgelenk = 0)
const de = Math.abs(this.eMotor - oldPos.e);
if (de > 0.001) {
const time = de / this.feedrate;
this.lastMoveTime = time;
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
this.motorSpeeds.a = (this.a - oldPos.a) / time;
this.motorSpeeds.b = (this.b - oldPos.b) / time;
this.motorSpeeds.c = (this.c - oldPos.c) / time;
this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
return;
}
// 4. Keine Bewegung erkannt → motorSpeeds bleiben AUF DEFAULT
// ToDo: Aus motorSpeed mit einzelnenen Werten muss noch die feedrate berechnet werden.
// hier bin ich unsicher, ob das nicht in den Sender rein sollte, da es eventuell
// abhngig vom FluidNC und dessen speed interpretation ist.
}
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(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(cmd="G1"){
const isFirstCall = !this.motorPosition;
if (isFirstCall) {
this.motorPositionOld = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);
} else {
this.motorPositionOld = this.motorPosition;
}
this.createMotorPosition()
// Für den ersten Aufruf setze alle Changed-Flags auf true
if (isFirstCall) {
this.motorPosition.xMotorChanged = true;
this.motorPosition.yMotorChanged = true;
this.motorPosition.zMotorChanged = true;
this.motorPosition.aMotorChanged = true;
this.motorPosition.bMotorChanged = true;
this.motorPosition.cMotorChanged = true;
this.motorPosition.eMotorChanged = true;
}
// Berechne Geschwindigkeiten
this.calculateSpeeds(this.motorPositionOld, this.motorPosition);
this.motorPosition.speeds = {...this.motorSpeeds};
// moveTime nach der Berechnung aktualisieren (createMotorPosition lief davor)
this.motorPosition.moveTime = this.lastMoveTime || 0;
console.log("Robot.sendCommand: ", cmd.toString(), " Motor-Pos: x=", this.motorPosition.x.toFixed(3), "yMotor=",this.motorPosition.y.toFixed(3), "zMotor=",this.motorPosition.z.toFixed(3), "aM=", this.motorPosition.a.toFixed(3), "bM=", this.motorPosition.b.toFixed(3), "cM=", this.motorPosition.c.toFixed(3), " e=", this.motorPosition.e.toFixed(3));
this.cmdReceivers.forEach(receiver => {
receiver.execCommand(cmd,this.motorPositionOld, this.motorPosition);
});
}
}
module.exports = Robot // Export class
// robot/Robot.js — Kompatibilitäts-Alias (siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md).
//
// Externer Code (und Tests) kann weiterhin `require('./robot/Robot')` schreiben.
// Während der Übergangsperiode zeigt der Alias auf die konkrete Default-Kinematik
// `Arm3SegmentLinearX`, damit `new Robot(l1, l2, l3)` unverändert funktioniert.
//
// Die generische Basisklasse / der Interface-Vertrag liegt in `RobotBase.js`;
// die produktive Auswahl der Kinematik übernimmt `KinematicsFactory.js`.
module.exports = require('./kinematics/Arm3SegmentLinearX');

319
robot/RobotBase.js Normal file
View File

@@ -0,0 +1,319 @@
const MotorPosition = require('./RobotMotorPosition.js')
/**
* RobotBase — abstrakte Basisklasse und zugleich der Interface-Vertrag des
* Frameworks. Enthält die roboter-unabhängige Infrastruktur (Zustand,
* `sendCommand`, `createMotorPosition`, `calculateSpeeds`, `rotateAroundAxis`).
*
* Der arm-spezifische Teil sind ausschließlich die beiden Kinematik-Methoden
* {@link RobotBase#calculateAngles3D} und
* {@link RobotBase#calculatePositionFromMotorAngles}. Sie sind hier abstrakt
* (werfen) und MÜSSEN von jeder konkreten Kinematik-Klasse überschrieben werden.
*
* Verträge (siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md):
* - Workspace-Koordinaten: `x, y, z, phi, theta, psi` + `e` (Greifer) — 6-DOF.
* - Motor-Zustand: 7 feste Slots `xMotor, alpha, beta, a, b, c, eMotor`.
*
* Konkrete Implementierungen leben in `robot/kinematics/`.
*
* @abstract
*/
class RobotBase{
constructor() {
// Umgebungsvariablen-Logik
const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ?
Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000;
// Speed-Regelung-Schalter: 'legacy' (Default — exakt wie bisher) oder 'correct'.
// Siehe doc/ToDo_6a_Speed.md.
this.speedMode = (process.env.ROBOT_SPEED_MODE || 'legacy').toLowerCase();
// ROBOT_USE_SPEED_CALC bleibt der interne Schalter für calculateSpeeds();
// der Korrekt-Modus aktiviert die Berechnung automatisch.
this.useSpeedCalc = this.speedMode === 'correct' ||
process.env.ROBOT_USE_SPEED_CALC === 'true' ||
process.env.ROBOT_USE_SPEED_CALC === '1';
/** @type {number} Bewegungszeit des letzten Schritts in Minuten (für koordinierte Feedrate) */
this.lastMoveTime = 0;
/** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */
this.speedX = 200;
/** @type {number} Bewegungsgeschwindigkeit Y-Achse in mm/min */
this.speedY = 200;
/** @type {number} Bewegungsgeschwindigkeit Z-Achse in mm/min */
this.speedZ = 200;
/** @type {number} Zeitstempel des zuletzt gesendeten Kommandos */
this.lastCommandSend = 0;
if(this.lastCommandSend == 0){ this.lastCommandSend = Date.now() };
/** @type {boolean} Animation aktiviert */
this.doAnimate = false;
// Plan-Koordinaten - XYZ FingerSpitze
/** @type {number} X-Position der Fingerspitze in mm */
this.x = 0;
/** @type {number} Y-Position der Fingerspitze in mm */
this.y = 0;
/** @type {number} Z-Position der Fingerspitze in mm */
this.z = 0;
// Plan-Koordinaten - HandRichtung (Euler-Winkel)
/** @type {number} Phi - Euler-Winkel (Längengrad): Rotation um Z-Achse in rad */
this.phi = 0.0;
/** @type {number} Theta - Euler-Winkel (Breitengrad): Neigungswinkel der Handachse in rad */
this.theta = -Math.PI/2;
/** @type {number} Psi - Euler-Winkel: Zusätzliche Drehung des Handgelenks in rad */
this.psi = 0.0;
/** @type {number} Finger-Abstands-Einstellung (Öffnungsweite) */
this.e = 0.0;
/** @type {number} Feedrate für Bewegungen in mm/min */
this.feedrate = DEFAULT_FEEDRATE;
/** @type {Object} Motor-Geschwindigkeiten in Einheiten pro Minute */
this.motorSpeeds = {x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0};
// Zwischen-Ergebnisse: Handgelenk-Punkt (Koordinaten des Handgelenks, nur für Tests public)
/** @type {number} Handgelenk-Position X in mm (berechneter Zwischenwert) */
this.pX = 0.0;
/** @type {number} Handgelenk-Position Y in mm (berechneter Zwischenwert) */
this.pY = 0.0;
/** @type {number} Handgelenk-Position Z in mm (berechneter Zwischenwert) */
this.pZ = 0.0;
// Motor-Koordinaten - Schulter, Ellebogen, Hand-Dreher
/** @type {number} X-Motor-Position (Schulterposition auf X-Schiene) in mm */
this.xMotor = 0;
/** @type {number} Alpha - Y-Motor-Winkel (Schulterposition) in rad */
this.alpha = 0;
/** @type {number} Beta - Z-Motor-Winkel (Unterarm-Neigung unter Y-Achse) in rad */
this.beta = 0;
this.xMotorChanged = false;
this.yMotorChanged = false;
this.zMotorChanged = false;
// Motor-Winkel für's Handgelenk
/** @type {number} a-Motor-Winkel: Rotation am Ellbogen in rad */
this.a = 0;
/** @type {number} b-Motor-Winkel: Handgelenk-Knicker-Winkel in rad */
this.b = 0;
/** @type {number} c-Motor-Winkel: Hand-Dreher-Rotation in rad */
this.c = 0;
this.aMotorChanged = false;
this.bMotorChanged = false;
this.cMotorChanged = false;
this.eMotorChanged = false;
/** @type {number} e-Motor-Wert: Finger-Abstands-Motor-Position */
this.eMotor = 0;
/** @type {number} Zeitstempel des letzten verarbeiteten Kommandos */
this.oldCommandTime = Date.now();
/** @type {Function[]} Array von Visualisierungs-Funktionen */
this.showFunctions = [];
/** @type {Object[]} Gespeicherte Roboterpositionen/Punkte */
this.savedPoints = [];
/** @type {number} Index des aktuell angesteuerten Punktes */
this.atPointNr = 0;
/** @type {number} Zeitstempel des aktuellen Punktes in ms */
this.t = 0;
/** @type {boolean} Relative oder absolute Bewegung (true = relativ) */
this.moveRelative = true;
/** @type {Object[]} Array von Kommando-Empfängern */
this.cmdReceivers = [];
}
createMotorPosition(){
this.motorPosition = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);
// Setze Changed-Flags basierend auf Änderungen seit der letzten Position
this.motorPosition.xMotorChanged = this.motorPositionOld ? this.xMotor !== this.motorPositionOld.x : true;
this.motorPosition.yMotorChanged = this.motorPositionOld ? this.alpha !== this.motorPositionOld.y : true;
this.motorPosition.zMotorChanged = this.motorPositionOld ? this.beta !== this.motorPositionOld.z : true;
this.motorPosition.aMotorChanged = this.motorPositionOld ? this.a !== this.motorPositionOld.a : true;
this.motorPosition.bMotorChanged = this.motorPositionOld ? this.b !== this.motorPositionOld.b : true;
this.motorPosition.cMotorChanged = this.motorPositionOld ? this.c !== this.motorPositionOld.c : true;
this.motorPosition.eMotorChanged = this.motorPositionOld ? this.eMotor !== this.motorPositionOld.e : true;
// Setze Handgelenk-Koordinaten (für Speed-Berechnung)
this.motorPosition.pX = this.pX;
this.motorPosition.pY = this.pY;
this.motorPosition.pZ = this.pZ;
// Setze Geschwindigkeiten
this.motorPosition.speeds = {...this.motorSpeeds};
this.motorPosition.feedrate = this.feedrate || 200;
// Speed-Regelung: Modus und (vorläufige) Bewegungszeit für die Sender
this.motorPosition.speedMode = this.speedMode;
this.motorPosition.moveTime = this.lastMoveTime || 0;
}
/**
* Vorwärts-Kinematik: Workspace-Koordinaten → Motorwinkel.
*
* Liest `this.x/y/z/phi/theta/psi/e` und schreibt das Ergebnis auf die
* Motor-Slots `this.xMotor/alpha/beta/a/b/c/eMotor` (sowie die Zwischenwerte
* `this.pX/pY/pZ` des Handgelenk-Punkts).
*
* ABSTRAKT — muss von der konkreten Kinematik-Klasse überschrieben werden.
*
* @abstract
* @param {boolean} [verbose] Debug-Ausgaben aktivieren
*/
calculateAngles3D(verbose){
throw new Error('calculateAngles3D() not implemented — RobotBase ist abstrakt; eine konkrete Kinematik-Klasse muss diese Methode überschreiben.');
}
// Berechnet die Motor-Geschwindigkeiten basierend auf Feedrate und Positionsänderung
calculateSpeeds(oldPos, newPos) {
if (!this.useSpeedCalc) return; // Neue Logik nur aktivieren, wenn Flag gesetzt
if (!oldPos || !newPos || this.feedrate <= 0) return;
this.lastMoveTime = 0; // wird unten gesetzt, sobald eine Bewegung erkannt wird
// 1. Berechne xyz-Distanz (primär)
const dx = newPos.x - oldPos.x;
const dy = newPos.y - oldPos.y;
const dz = newPos.z - oldPos.z;
const xyz_dist = Math.sqrt(dx*dx + dy*dy + dz*dz);
if (xyz_dist > 0.001) {
const time = xyz_dist / this.feedrate;
this.lastMoveTime = time;
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
this.motorSpeeds.a = (this.a - oldPos.a) / time;
this.motorSpeeds.b = (this.b - oldPos.b) / time;
this.motorSpeeds.c = (this.c - oldPos.c) / time;
this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
return;
}
// 2. Berechne Handgelenk-Punkt-Distanz (falls xyz = 0)
const dpx = newPos.pX - oldPos.pX;
const dpy = newPos.pY - oldPos.pY;
const dpz = newPos.pZ - oldPos.pZ;
const handgelenk_dist = Math.sqrt(dpx*dpx + dpy*dpy + dpz*dpz);
if (handgelenk_dist > 0.001) {
const time = handgelenk_dist / this.feedrate;
this.lastMoveTime = time;
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
this.motorSpeeds.a = (this.a - oldPos.a) / time;
this.motorSpeeds.b = (this.b - oldPos.b) / time;
this.motorSpeeds.c = (this.c - oldPos.c) / time;
this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
return;
}
// 3. Berechne Finger-Distanz (falls Handgelenk = 0)
const de = Math.abs(this.eMotor - oldPos.e);
if (de > 0.001) {
const time = de / this.feedrate;
this.lastMoveTime = time;
this.motorSpeeds.x = (this.xMotor - oldPos.x) / time;
this.motorSpeeds.y = (this.alpha - oldPos.y) / time;
this.motorSpeeds.z = (this.beta - oldPos.z) / time;
this.motorSpeeds.a = (this.a - oldPos.a) / time;
this.motorSpeeds.b = (this.b - oldPos.b) / time;
this.motorSpeeds.c = (this.c - oldPos.c) / time;
this.motorSpeeds.e = (this.eMotor - oldPos.e) / time;
return;
}
// 4. Keine Bewegung erkannt → motorSpeeds bleiben AUF DEFAULT
// ToDo: Aus motorSpeed mit einzelnenen Werten muss noch die feedrate berechnet werden.
// hier bin ich unsicher, ob das nicht in den Sender rein sollte, da es eventuell
// abhngig vom FluidNC und dessen speed interpretation ist.
}
/**
* Geschützte Hilfsmethode (Rodrigues-Rotation): dreht den Vektor `v` um die
* (normierte) Achse `n` um den Winkel `angle`. Für Kinematik-Implementierungen.
*/
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)
};
}
/**
* Rückwärts-Kinematik: Motorwinkel → Workspace-Koordinaten.
*
* Liest die Motor-Slots `this.xMotor/alpha/beta/a/b/c` und schreibt das
* Ergebnis auf `this.x/y/z/phi/theta/psi` (sowie `this.pX/pY/pZ`).
*
* ABSTRAKT — muss von der konkreten Kinematik-Klasse überschrieben werden.
*
* @abstract
* @param {boolean} [verbose] Debug-Ausgaben aktivieren
*/
calculatePositionFromMotorAngles(verbose = false) {
throw new Error('calculatePositionFromMotorAngles() not implemented — RobotBase ist abstrakt; eine konkrete Kinematik-Klasse muss diese Methode überschreiben.');
}
sendCommand(cmd="G1"){
const isFirstCall = !this.motorPosition;
if (isFirstCall) {
this.motorPositionOld = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor);
} else {
this.motorPositionOld = this.motorPosition;
}
this.createMotorPosition()
// Für den ersten Aufruf setze alle Changed-Flags auf true
if (isFirstCall) {
this.motorPosition.xMotorChanged = true;
this.motorPosition.yMotorChanged = true;
this.motorPosition.zMotorChanged = true;
this.motorPosition.aMotorChanged = true;
this.motorPosition.bMotorChanged = true;
this.motorPosition.cMotorChanged = true;
this.motorPosition.eMotorChanged = true;
}
// Berechne Geschwindigkeiten
this.calculateSpeeds(this.motorPositionOld, this.motorPosition);
this.motorPosition.speeds = {...this.motorSpeeds};
// moveTime nach der Berechnung aktualisieren (createMotorPosition lief davor)
this.motorPosition.moveTime = this.lastMoveTime || 0;
console.log("Robot.sendCommand: ", cmd.toString(), " Motor-Pos: x=", this.motorPosition.x.toFixed(3), "yMotor=",this.motorPosition.y.toFixed(3), "zMotor=",this.motorPosition.z.toFixed(3), "aM=", this.motorPosition.a.toFixed(3), "bM=", this.motorPosition.b.toFixed(3), "cM=", this.motorPosition.c.toFixed(3), " e=", this.motorPosition.e.toFixed(3));
this.cmdReceivers.forEach(receiver => {
receiver.execCommand(cmd,this.motorPositionOld, this.motorPosition);
});
}
}
module.exports = RobotBase // Export abstrakte Basisklasse / Interface

133
robot/kinematics/Arm3SegmentLinearX.js Normal file
View File

@@ -0,0 +1,133 @@
const RobotBase = require('../RobotBase.js')
/**
* Arm3SegmentLinearX — 6-DOF-Arm mit linearer X-Schiene und drei Segmenten
* (Oberarm `l1`, Unterarm `l2`, Hand/Endeffector `l3`) plus Greifer.
*
* Erste konkrete Kinematik-Implementierung des Frameworks. Implementiert den
* Interface-Vertrag von {@link RobotBase} — die gesamte generische Infrastruktur
* (Zustand, sendCommand, Speed-Berechnung, ...) wird geerbt.
*
* Physikalische Struktur:
* - X-Achse: lineare Schiene (Schulterposition `xMotor`)
* - Schulter/Ellbogen: 2D-Arm in der Y-Z-Ebene (`alpha`, `beta`)
* - Handgelenk: drei Winkel `a` (Ellbogen-Dreher), `b` (Knicker), `c` (Hand-Dreher)
* - Greifer: `e`
*/
class Arm3SegmentLinearX extends RobotBase {
/**
* @param {number} l1 Länge des Oberarms in mm
* @param {number} l2 Länge des Unterarms in mm
* @param {number} l3 Länge der Hand (Endeffector) in mm
*/
constructor(l1, l2, l3) {
super();
/** @type {number} Länge des Oberarms in mm */
this.l1 = l1;
/** @type {number} Länge des Unterarms in mm */
this.l2 = l2;
/** @type {number} Länge der Hand (Endeffector) in mm */
this.l3 = l3;
}
// Berechnet aus XYZ die Motor-Winkel für den GCode
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}
while(this.theta < -Math.PI){this.theta += 2*Math.PI}
// Handgelenk-Punkt ausrechnen:
this.pX = this.x + this.l3*Math.sin(this.theta)*Math.cos(this.phi);
this.pY = this.y + this.l3*Math.sin(this.theta)*Math.sin(this.phi);
this.pZ = this.z + this.l3*Math.cos(this.theta);
var pX = this.pX;
var pY = this.pY;
var pZ = this.pZ;
this.xMotor = pX;
// Ziel-Punkt ausrechnen ==> 2D Rechnung Arm
var r = Math.sqrt(pY * pY + pZ * pZ);
if (r > (this.l1 + this.l2)) { return; }
if (r == 0) { return; }
var gamma = Math.asin(pZ / r);
var delta = Math.acos((this.l1 * this.l1 + this.l2 * this.l2 - r * r) / (2 * this.l1 * this.l2));
this.alpha = Math.acos((this.l1 * this.l1 + r * r - this.l2 * this.l2) / (2 * r * this.l1)) + gamma;
this.beta = -Math.PI + (this.alpha + delta);
// Ende <== 2D Rechnung Arm
// Richtung der Hand ausgerechnet
// Arm = (0, cos(beta), sin(beta)) Punkt = (sin(theta)cos(phi), sin(theta)sin(phi), cos(theta))
//
// Unterarm muss gedreht werden. Aus der Y-Z-Ebene raus. Hin in die Ebene n x r
// wobei n = Unterarm x (P-O) ist
var nX = Math.cos(this.beta)*Math.cos(this.theta) - Math.sin(this.theta)*Math.sin(this.phi)*Math.sin(this.beta);
var nY = Math.sin(this.beta)*Math.sin(this.theta)*Math.cos(this.phi);
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}
if(Math.sin(this.theta) < 0) {this.a = -this.a}
// Handgelenk-Knick-Winkel ist zwischen Arm und Punkt-O
var cosB = Math.cos(this.beta)*Math.sin(this.theta)*Math.sin(this.phi) + Math.sin(this.beta)*Math.cos(this.theta);
this.b = Math.acos(cosB);
// Winkel zwischen n und z muss rumgedreht werden.
var cosC = - nZ / nBetrag;
this.c = Math.acos(cosC);
this.c += this.psi;
// a um 180° drehen
this.a += Math.PI;
while(this.a > Math.PI){this.a -= 2*Math.PI}
while(this.a < -Math.PI){this.a += 2*Math.PI}
this.eMotor = this.e - this.b - this.c;
}
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}
}
}
module.exports = Arm3SegmentLinearX