188 lines
9.1 KiB
JavaScript
188 lines
9.1 KiB
JavaScript
const RobotBase = require('../RobotBase.js')
|
||
|
||
/**
|
||
* Arm3SegmentRotaryBase — Kinematik für den **Joy-IT „Grab-It" (Robot02)**.
|
||
*
|
||
* Strukturell wie {@link Arm3SegmentLinearX} (drei Arm-Segmente: Oberarm `l1`,
|
||
* Unterarm `l2`, Hand `l3`), aber statt der linearen X-Schiene sitzt der Arm auf
|
||
* einer **drehbaren Basis** (Yaw). Dazu kommt eine Handgelenk-Rotation.
|
||
*
|
||
* ───────────────────────────────────────────────────────────────────────────
|
||
* Physische Spezifikation (recherchiert aus dem Joy-IT-Datenblatt / Handbuch):
|
||
* - 6 PWM-Servos (20 kg, Metallgetriebe), 360° mech. / 180° Arbeitsbereich
|
||
* - 5 Bewegungsachsen + Greifer
|
||
* - Reichweite ca. 300 mm ab Mitte Drehteller, Bauhöhe bis 420 mm
|
||
*
|
||
* Gelenkkette (Basis → Greifer):
|
||
* q1 Basis-Drehung (Yaw um die vertikale Z-Achse) → this.xMotor
|
||
* q2 Schulter (Pitch) → this.alpha
|
||
* q3 Ellbogen (Pitch, relativ zum Oberarm) → this.beta
|
||
* q4 Handgelenk (Pitch, relativ zum Unterarm) → this.a
|
||
* q5 Handgelenk-Roll (Drehung um die Hand-/Anflugachse)→ this.b
|
||
* – (ungenutzt) → this.c (= 0)
|
||
* q6 Greifer (Öffnungsweite) → this.eMotor
|
||
*
|
||
* Damit reichen die 7 Motor-Slots des Frameworks aus; ein Slot (`c`) bleibt frei.
|
||
*
|
||
* ───────────────────────────────────────────────────────────────────────────
|
||
* 5-DOF-Einschränkung (wichtig!):
|
||
* Ein 5-Achs-Arm kann **nicht** jede beliebige 6-DOF-Pose (x,y,z,phi,theta,psi)
|
||
* erreichen. Da die Basis-Drehung den Azimut bestimmt, liegt der gesamte Arm in
|
||
* **einer** vertikalen Ebene durch die Basisachse. Folge:
|
||
* - `phi` (Hand-Azimut) ist **nicht frei** — er ist an die Position gekoppelt
|
||
* und wird in der Inversen aus `atan2(y, x)` abgeleitet. Ein als Eingabe
|
||
* gesetztes `this.phi` wird in {@link calculateAngles3D} **ignoriert** und in
|
||
* {@link calculatePositionFromMotorAngles} als Ergebnis überschrieben.
|
||
* - Frei wählbar sind: Position `x,y,z`, Hand-Neigung `theta` (Polarwinkel von
|
||
* +Z, gleiche Konvention wie Arm3SegmentLinearX) und Hand-Rotation `psi`.
|
||
*
|
||
* ───────────────────────────────────────────────────────────────────────────
|
||
* Geometrie-Modell / Annahmen:
|
||
* - Schultergelenk sitzt auf der Basis-Drehachse, Höhe `baseHeight` über der
|
||
* Grundplatte (radialer Versatz = 0 — vereinfachende Annahme).
|
||
* - Winkel `alpha`, `beta`, `a` werden intern als Elevation gegen die
|
||
* Horizontale geführt; `alpha` ist absolut, `beta`/`a` relativ zum
|
||
* vorhergehenden Segment.
|
||
* - Es wird genau **ein** Ellbogen-Zweig gewählt (`beta` ∈ [0, π]).
|
||
* - Gelenk-Endanschläge werden **nicht** geprüft (wie bei Arm3SegmentLinearX).
|
||
*
|
||
* ⚠️ Die Default-Längen sind **Schätzwerte**, abgeleitet aus den
|
||
* veröffentlichten Maßen (Σ Segmente ≈ Reichweite 300 mm, `baseHeight` ≈
|
||
* Höhe 420 mm − Reichweite). Für den Echtbetrieb am physischen Arm sollten sie
|
||
* **kalibriert** und per `ROBOT_KINEMATICS_PARAMS` gesetzt werden.
|
||
*/
|
||
class Arm3SegmentRotaryBase extends RobotBase {
|
||
|
||
/**
|
||
* @param {number} [l1=105] Länge des Oberarms (Schulter→Ellbogen) in mm
|
||
* @param {number} [l2=98] Länge des Unterarms (Ellbogen→Handgelenk) in mm
|
||
* @param {number} [l3=100] Länge der Hand (Handgelenk→Greiferspitze) in mm
|
||
* @param {Object} [params] zusätzliche Parameter
|
||
* @param {number} [params.baseHeight=110] Höhe der Schulterachse über der Basis in mm
|
||
*/
|
||
constructor(l1 = 105, l2 = 98, l3 = 100, params = {}) {
|
||
super(params);
|
||
|
||
/** @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;
|
||
/** @type {number} Höhe der Schulter-Drehachse über der Grundplatte in mm */
|
||
this.baseHeight = (params && params.baseHeight != null) ? params.baseHeight : 110;
|
||
|
||
// Neutrale Default-Pose: Hand horizontal nach vorn (theta = π/2 = Polarwinkel).
|
||
this.theta = Math.PI / 2;
|
||
}
|
||
|
||
/**
|
||
* Inverse Kinematik: Workspace (x, y, z, theta, psi, e) → Gelenkwinkel.
|
||
* `phi` wird ignoriert und aus der Position abgeleitet (5-DOF, siehe Klassendoc).
|
||
*/
|
||
calculateAngles3D(verbose) {
|
||
// Winkel normalisieren (Konsistenz mit Arm3SegmentLinearX)
|
||
while (this.theta > Math.PI) { this.theta -= 2 * Math.PI }
|
||
while (this.theta < -Math.PI) { this.theta += 2 * Math.PI }
|
||
|
||
// 1) Basis-Drehung aus dem Azimut der Zielposition (phi ist gekoppelt)
|
||
const q1 = Math.atan2(this.y, this.x);
|
||
this.xMotor = q1;
|
||
this.phi = q1;
|
||
|
||
// 2) Hand-Vektor abziehen → Handgelenk-Punkt
|
||
// theta = Polarwinkel von +Z. Hand-Radial = l3·sin(theta), Hand-Z = l3·cos(theta).
|
||
const st = Math.sqrt(this.x * this.x + this.y * this.y); // radialer Abstand der Spitze
|
||
const zt = this.z;
|
||
|
||
const sw = st - this.l3 * Math.sin(this.theta); // radial: Handgelenk
|
||
const zw = zt - this.l3 * Math.cos(this.theta); // Höhe: Handgelenk
|
||
|
||
// Handgelenk-Punkt in 3D (für Speed-Berechnung)
|
||
this.pX = sw * Math.cos(q1);
|
||
this.pY = sw * Math.sin(q1);
|
||
this.pZ = zw;
|
||
|
||
// 3) 2-Gelenk-IK in der Armebene: von Schulter (0, baseHeight) zum Handgelenk (sw, zw)
|
||
const dx = sw;
|
||
const dz = zw - this.baseHeight;
|
||
const R2 = dx * dx + dz * dz;
|
||
const R = Math.sqrt(R2);
|
||
|
||
if (R > (this.l1 + this.l2)) { return; } // unerreichbar — Slots unverändert lassen
|
||
if (R === 0) { return; }
|
||
|
||
// Ellbogen-Relativwinkel (ein Zweig: qe ∈ [0, π])
|
||
let cosE = (R2 - this.l1 * this.l1 - this.l2 * this.l2) / (2 * this.l1 * this.l2);
|
||
cosE = Math.max(-1, Math.min(1, cosE));
|
||
const qe = Math.atan2(Math.sqrt(1 - cosE * cosE), cosE);
|
||
|
||
// Absoluter Oberarmwinkel (Elevation gegen Horizontale)
|
||
const A2 = Math.atan2(dz, dx) - Math.atan2(this.l2 * Math.sin(qe), this.l1 + this.l2 * Math.cos(qe));
|
||
const A3 = A2 + qe; // absoluter Unterarmwinkel
|
||
const A4 = Math.PI / 2 - this.theta; // absolute Hand-Elevation aus Polarwinkel
|
||
|
||
// 4) Motor-Slots schreiben
|
||
this.alpha = A2; // Schulter (absolut)
|
||
this.beta = qe; // Ellbogen (relativ) = A3 − A2
|
||
this.a = A4 - A3; // Handgelenk-Pitch (relativ)
|
||
this.b = this.psi; // Handgelenk-Roll
|
||
this.c = 0; // ungenutzt
|
||
this.eMotor = this.e; // Greifer
|
||
|
||
// a auf [-π, π] normalisieren
|
||
while (this.a > Math.PI) { this.a -= 2 * Math.PI }
|
||
while (this.a < -Math.PI) { this.a += 2 * Math.PI }
|
||
|
||
if (verbose) {
|
||
console.log("IK GrabIt: q1=", q1.toFixed(3), "alpha=", this.alpha.toFixed(3),
|
||
"beta=", this.beta.toFixed(3), "a=", this.a.toFixed(3), "b=", this.b.toFixed(3));
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Vorwärts-Kinematik: Gelenkwinkel → Workspace (x, y, z, phi, theta, psi, e).
|
||
* Exakte Umkehrung von {@link calculateAngles3D}.
|
||
*/
|
||
calculatePositionFromMotorAngles(verbose = false) {
|
||
const q1 = this.xMotor; // Basis-Yaw
|
||
const A2 = this.alpha; // Oberarm absolut
|
||
const A3 = this.alpha + this.beta; // Unterarm absolut
|
||
const A4 = this.alpha + this.beta + this.a; // Hand absolut (Elevation)
|
||
|
||
// Handgelenk-Punkt in der Armebene
|
||
const sw = this.l1 * Math.cos(A2) + this.l2 * Math.cos(A3);
|
||
const zw = this.baseHeight + this.l1 * Math.sin(A2) + this.l2 * Math.sin(A3);
|
||
|
||
// Greiferspitze in der Armebene (Hand-Elevation A4)
|
||
const st = sw + this.l3 * Math.cos(A4);
|
||
const zt = zw + this.l3 * Math.sin(A4);
|
||
|
||
// In den 3D-Raum drehen
|
||
this.pX = sw * Math.cos(q1);
|
||
this.pY = sw * Math.sin(q1);
|
||
this.pZ = zw;
|
||
|
||
this.x = st * Math.cos(q1);
|
||
this.y = st * Math.sin(q1);
|
||
this.z = zt;
|
||
|
||
this.phi = q1; // Azimut = Basis-Drehung
|
||
this.theta = Math.PI / 2 - A4; // Polarwinkel von +Z
|
||
this.psi = this.b; // Hand-Roll
|
||
|
||
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 }
|
||
|
||
if (verbose) {
|
||
console.log("FK GrabIt: x=", this.x.toFixed(2), "y=", this.y.toFixed(2),
|
||
"z=", this.z.toFixed(2), "phi=", this.phi.toFixed(3), "theta=", this.theta.toFixed(3));
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
module.exports = Arm3SegmentRotaryBase
|