Revert B=180-B (Phase 2): gerade Hand ist b=pi, nicht 0
Die Phase-2-Umstellung (Commits549d10b,2197a89) war falsch: sie machte gerade Hand = b=0. Hardware-Daten zeigen aber gerade = B~180 (z.B. G92 ... B179.20). Folge: G28 fuhr nach b=0 -> ~179 Grad Einklappen -> Hand schlug in den Arm (Crash). Zurueck auf den verifizierten Stand933a017: - IK: this.b = Math.acos(cosB) - FK: rotateAroundAxis(vecUnterarm, n, this.b) - G28: robot.b = Math.PI Verifiziert mit echten Daten: G92 B179.20 -> b=179.20; G28 -> b=180 (Weg 0.8 Grad, kein Slam); IK-Round-Trip exakt; 452/452 Tests gruen. Logs nicht enthalten. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
This commit is contained in:
@@ -110,10 +110,9 @@ class Arm3SegmentLinearX extends RobotBase {
|
||||
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,π].
|
||||
// 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.PI - Math.acos(cosB);
|
||||
this.b = Math.acos(cosB);
|
||||
|
||||
|
||||
// Winkel zwischen n und z muss rumgedreht werden.
|
||||
@@ -154,8 +153,7 @@ class Arm3SegmentLinearX extends RobotBase {
|
||||
|
||||
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);
|
||||
const vHand = this.rotateAroundAxis(vecUnterarm, n, this.b);
|
||||
|
||||
this.x = this.pX - this.l3 * vHand.x;
|
||||
this.y = this.pY - this.l3 * vHand.y;
|
||||
|
||||
Reference in New Issue
Block a user