y war falsch
This commit is contained in:
@@ -52,10 +52,12 @@ class RobotController {
|
||||
}
|
||||
|
||||
if (cmd === 'G28') {
|
||||
// Home = Grundstellung: Arm voll ausgestreckt entlang -y (siehe
|
||||
// doc/Info_Koordinaten.md). y und phi in der -y-Konvention.
|
||||
robot.x = 0;
|
||||
robot.y = robot.l1 + robot.l2 + robot.l3;
|
||||
robot.y = -(robot.l1 + robot.l2 + robot.l3);
|
||||
robot.z = 0;
|
||||
robot.phi = -Math.PI / 2;
|
||||
robot.phi = Math.PI / 2;
|
||||
robot.theta = Math.PI / 2;
|
||||
robot.psi = 0;
|
||||
robot.e = 0;
|
||||
|
||||
@@ -33,8 +33,38 @@ class Arm3SegmentLinearX extends RobotBase {
|
||||
this.l3 = l3;
|
||||
}
|
||||
|
||||
// Berechnet aus XYZ die Motor-Winkel für den GCode
|
||||
calculateAngles3D(verbose){
|
||||
// ─────────────────────────────────────────────────────────────────────
|
||||
// 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}
|
||||
@@ -108,7 +138,7 @@ class Arm3SegmentLinearX extends RobotBase {
|
||||
return e - this.b - this.c;
|
||||
}
|
||||
|
||||
calculatePositionFromMotorAngles(verbose = false) {
|
||||
_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)}
|
||||
|
||||
Reference in New Issue
Block a user