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

G28 Singularität und Planungen

Browse Source
This commit is contained in:
chk
2026-06-26 11:39:20 +02:00
parent bd1752f567
commit 7639266170
6 changed files with 232 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
robot/RobotController.js
View File

@@ -52,16 +52,37 @@ 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.z = 0;
robot.phi = Math.PI / 2;
robot.theta = Math.PI / 2;
robot.psi = 0;
robot.e = 0;
robot.calculateAngles3D();
// Home = Grundstellung: Arm + Hand gestreckt entlang -y (siehe
// doc/Info_Koordinaten.md). Ziel-Fingerspitze: (0, -(l1+l2+l3), 0).
const reach = robot.l1 + robot.l2 + robot.l3;
const homeY = -reach;
if (Math.abs(Math.abs(homeY) - reach) < 1e-6) {
// Sonderfall voll ausgestreckt: |y| = l1+l2+l3 ist eine Handgelenk-
// Singularität — die IK kann den Unterarm-Dreher a (und damit c) dort
// nicht bestimmen und liefert Müll (z.B. a=135°, c=45°), wodurch der
// Finger schräg/nach unten zeigt. Daher die Motorwerte DIREKT in die
// Grundstellung setzen und die Workspace-Pose per FK füllen.
robot.xMotor = 0;
robot.alpha = 0;
robot.beta = 0;
robot.a = 0;
robot.b = Math.PI; // gerade Hand (Phase-1-Konvention; Phase 2: 0)
robot.c = 0;
robot.e = 0;
robot.eMotor = robot.gripperMotorFromOpening(robot.e);
robot.calculatePositionFromMotorAngles(); // FK -> x=0, y=-(l1+l2+l3), z=0
} else {
// Allgemeiner (nicht-singulärer) Home-Punkt: regulär über die IK.
robot.x = 0;
robot.y = homeY;
robot.z = 0;
robot.phi = Math.PI / 2;
robot.theta = Math.PI / 2;
robot.psi = 0;
robot.e = 0;
robot.calculateAngles3D();
}
robot.sendCommand();
return;
}