177 lines
7.5 KiB
JavaScript
177 lines
7.5 KiB
JavaScript
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
|
||
* @param {Object} [config] Bewegungs-Config (defaultFeedrate, speedMode, …)
|
||
*/
|
||
constructor(l1, l2, l3, config = {}) {
|
||
super(config);
|
||
|
||
/** @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;
|
||
}
|
||
|
||
// ─────────────────────────────────────────────────────────────────────
|
||
// Y-Konvention: Der reale Roboter steht/arbeitet in -y (robot.json:
|
||
// Arm1 -> [0,-250,0], coordinateSystem.y = "backward"). Die interne
|
||
// Kinematik (_ikPlusY/_fkPlusY) rechnet historisch in +y. Beide
|
||
// öffentlichen Methoden spiegeln daher die Workspace-Pose an der x-z-Ebene
|
||
// (y, pY, phi, psi; theta bleibt), sodass alpha=0 nach -y zeigt.
|
||
// Siehe doc/Info_Koordinaten.md (Weg 2, Phase 1).
|
||
// ─────────────────────────────────────────────────────────────────────
|
||
|
||
/** Reflexion der Workspace-Pose an der x-z-Ebene (Involution: zweimal = Identität). */
|
||
_mirrorWorkspaceY() {
|
||
this.y = -this.y;
|
||
this.pY = -this.pY;
|
||
this.phi = -this.phi;
|
||
this.psi = -this.psi;
|
||
}
|
||
|
||
calculateAngles3D(verbose) {
|
||
// -y-Eingabe in den internen +y-Frame spiegeln, rechnen, dann die
|
||
// Workspace-Felder zurückspiegeln (Motorwerte bleiben unberührt).
|
||
this._mirrorWorkspaceY();
|
||
this._ikPlusY(verbose);
|
||
this._mirrorWorkspaceY();
|
||
}
|
||
|
||
calculatePositionFromMotorAngles(verbose = false) {
|
||
this._fkPlusY(verbose);
|
||
this._mirrorWorkspaceY(); // +y-Ergebnis -> -y Workspace
|
||
}
|
||
|
||
// Berechnet aus XYZ die Motor-Winkel für den GCode (interne +y-Mathematik)
|
||
_ikPlusY(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.atan2(pZ, pY);
|
||
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: b=0 = gerade (Phase 2). cosB ist cos des internen
|
||
// Winkels zwischen Unterarm und Hand; acos liefert [0,π], also b = π−acos(cosB) ∈ [0,π].
|
||
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.PI - 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.gripperMotorFromOpening(this.e);
|
||
}
|
||
|
||
/**
|
||
* Greifer-Kopplung dieses Arms: die Finger-Sehne läuft durchs Handgelenk, daher
|
||
* ziehen Knick (`b`) und Dreh (`c`) am Greifer mit. `eMotor` kompensiert das, damit
|
||
* die Finger-Öffnung `e` (mm, ab Null-Position eines Fingers) unabhängig von der
|
||
* Handstellung bleibt. Einzige Quelle für diese Kopplung (auch via G92/M92 genutzt).
|
||
*/
|
||
gripperMotorFromOpening(e) {
|
||
return e - this.b - this.c;
|
||
}
|
||
|
||
_fkPlusY(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);
|
||
|
||
// b=0 = gerade (Phase 2): interne Rotation um π−b (bei b=0 → π → Hand gestreckt).
|
||
const vHand = this.rotateAroundAxis(vecUnterarm, n, Math.PI - 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
|