134 lines
5.2 KiB
JavaScript
134 lines
5.2 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
|
|
*/
|
|
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
|