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:
@@ -6,9 +6,9 @@ Diese Datei beschreibt
|
|||||||
3. die angestrebte **ideale Nullstellung** und
|
3. die angestrebte **ideale Nullstellung** und
|
||||||
4. die Schritte, um den Driver auf diese Konvention zu bringen (**Weg 2: Modell auf −Y**).
|
4. die Schritte, um den Driver auf diese Konvention zu bringen (**Weg 2: Modell auf −Y**).
|
||||||
|
|
||||||
> **Status:** **Phase 1 (y-Flip) ✅** und **Phase 2 (B-Konvention) ✅** umgesetzt.
|
> **Status:** **Phase 1 (y-Flip) ist umgesetzt und am Roboter verifiziert.** α/β werden
|
||||||
> α=0 → Arm zeigt nach −y; **b=0 = gerade Hand** (homing-konsistent).
|
> jetzt von **−y** aus gemessen (α=0 → Arm zeigt nach −y), passend zu robot.json und
|
||||||
> Offen: C-Nullpunkt, Greifer-Kopplung-Validierung.
|
> appRobotHoming. Offen ist **Phase 2** (Handgelenk-/Finger-Nullstellung, B/C).
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
@@ -87,8 +87,8 @@ Diese drei Bedingungen erfüllt der Driver **nach Phase 1 bereits** (verifiziert
|
|||||||
| Y (α) | **0°** | Oberarm waagerecht entlang −y | ✅ Phase 1 |
|
| Y (α) | **0°** | Oberarm waagerecht entlang −y | ✅ Phase 1 |
|
||||||
| Z (β) | **0°** | Unterarm waagerecht entlang −y (gestreckt) | ✅ Phase 1 |
|
| Z (β) | **0°** | Unterarm waagerecht entlang −y (gestreckt) | ✅ Phase 1 |
|
||||||
| A (a) | **0°** | Hand-Knick-Achse ∥ x | ✅ (Code erfüllt es) |
|
| A (a) | **0°** | Hand-Knick-Achse ∥ x | ✅ (Code erfüllt es) |
|
||||||
| B (b) | **0°** | Hand gerade = b=0, Knick = b>0 | ✅ Phase 2 |
|
| B (b) | 0° (Ziel) | Hand gerade — **derzeit ist gerade = 180°** | ⏳ Phase 2 |
|
||||||
| C (c) | 0° (Ziel) | kein Hand-Roll — **derzeit neutral ≠ 0** | ⏳ Phase 2 Rest |
|
| C (c) | 0° (Ziel) | kein Hand-Roll — **derzeit neutral ≠ 0** | ⏳ Phase 2 |
|
||||||
| E (e) | **0** | Greifer geschlossen / Referenz | — |
|
| E (e) | **0** | Greifer geschlossen / Referenz | — |
|
||||||
|
|
||||||
→ Resultierende Fingerspitze (α=β=a=0, gerade Hand): **(xMotor, −(l1+l2+l3), 0) ≈ (x, −590, 0)**.
|
→ Resultierende Fingerspitze (α=β=a=0, gerade Hand): **(xMotor, −(l1+l2+l3), 0) ≈ (x, −590, 0)**.
|
||||||
@@ -104,7 +104,7 @@ Diese drei Bedingungen erfüllt der Driver **nach Phase 1 bereits** (verifiziert
|
|||||||
| β=0 Unterarm | **−y** ✅ | −y |
|
| β=0 Unterarm | **−y** ✅ | −y |
|
||||||
| a=0 Hand-Knick-Achse | **∥ x** ✅ | ∥ x |
|
| a=0 Hand-Knick-Achse | **∥ x** ✅ | ∥ x |
|
||||||
| G92 der Grundstellung → y | **≈ −590** ✅ | ≈ −590 |
|
| G92 der Grundstellung → y | **≈ −590** ✅ | ≈ −590 |
|
||||||
| gerade Hand | **b = 0°** ✅ | b = 0° |
|
| gerade Hand | **b = 180°** | **b = 0°** |
|
||||||
| neutraler Roll | **c: ψ = 90° − C** (posenabh.) | **c = 0°** |
|
| neutraler Roll | **c: ψ = 90° − C** (posenabh.) | **c = 0°** |
|
||||||
|
|
||||||
Verifiziert per FK: `FK(α=0, β=0, gerade Hand) → (0, −590, 0)`; bei `a=0` bleibt
|
Verifiziert per FK: `FK(α=0, β=0, gerade Hand) → (0, −590, 0)`; bei `a=0` bleibt
|
||||||
@@ -126,25 +126,19 @@ Umgesetzt:
|
|||||||
Stellung (`|y| = l1+l2+l3`) ist eine Handgelenk-Singularität, in der die IK `a`/`c` nicht
|
Stellung (`|y| = l1+l2+l3`) ist eine Handgelenk-Singularität, in der die IK `a`/`c` nicht
|
||||||
bestimmen kann (Müll wie `a=135°, c=45°` → Finger schräg). G28 setzt dort die Motorwerte
|
bestimmen kann (Müll wie `a=135°, c=45°` → Finger schräg). G28 setzt dort die Motorwerte
|
||||||
**direkt** (`alpha=beta=a=c=0`, `b=π` = gerade Hand) und füllt die Pose per FK.
|
**direkt** (`alpha=beta=a=c=0`, `b=π` = gerade Hand) und füllt die Pose per FK.
|
||||||
**Interim:** G28 lässt den Greifer (`e`/`eMotor`) **unangetastet** — sonst ergäbe
|
|
||||||
`eMotor = e−b−c` bei `b=π` den Wert −180° → Finger-Anschlag-Slam (Phase 2 behebt das mit `b=0`).
|
|
||||||
- **Tests:** `test/Robot.Kinematics.NegativeY.test.js` (Grundstellung −590, Homing-Pose
|
- **Tests:** `test/Robot.Kinematics.NegativeY.test.js` (Grundstellung −590, Homing-Pose
|
||||||
in −y, Round-Trip in −y, a=0 → Knick-Achse ∥ x).
|
in −y, Round-Trip in −y, a=0 → Knick-Achse ∥ x).
|
||||||
- **Migration:** `Robot.02_UpperArm` und der G28-Test auf −y umgestellt.
|
- **Migration:** `Robot.02_UpperArm` und der G28-Test auf −y umgestellt.
|
||||||
- **Doku:** `doc/Info_G92.md` Y/Z (und C/A nach der Spiegelung) aktualisiert.
|
- **Doku:** `doc/Info_G92.md` Y/Z (und C/A nach der Spiegelung) aktualisiert.
|
||||||
|
|
||||||
### Phase 2 — B-Konvention ✅ ERLEDIGT
|
### Phase 2 — Handgelenk-/Finger-Nullstellung (B, C, Greifer) — OFFEN
|
||||||
|
|
||||||
**gerade Hand = b = 0** (war b = π = 180°). Homing sendet B≈0 für gerade → Driver jetzt konsistent.
|
> **Voraussetzung (User):** Erst die Finger visualisieren/prüfen — dort werden noch Fehler
|
||||||
|
> vermutet. **a-Achse ist bereits korrekt** (a=0 → Knick-Achse ∥ x). Phase 2 betrifft
|
||||||
|
> **B (Knick)**, **C (Roll)** und die **Greifer-Kopplung**.
|
||||||
|
|
||||||
Umgesetzt:
|
**Ziel-Konvention:** gerade Hand → **B = 0°** (statt 180°); neutraler Roll → **C = 0°**;
|
||||||
- **IK** (`_ikPlusY`): `this.b = Math.PI − Math.acos(cosB)` (statt `acos(cosB)`).
|
Greifer-Kopplung konsistent und gegen die echte Mechanik kalibriert.
|
||||||
- **FK** (`_fkPlusY`): Rotation um `Math.PI − this.b` (statt `this.b`).
|
|
||||||
- **G28**: `robot.b = 0` (statt `Math.PI`).
|
|
||||||
- `gripperMotorFromOpening = e − b − c` bleibt; mit b=0 für gerade ist b-Beitrag Null → kein Slam mehr.
|
|
||||||
- Tests: 452/452 grün.
|
|
||||||
|
|
||||||
**Noch offen (Phase 2 Rest):** C-Nullpunkt, Greifer-Kopplung-Validierung, toter x-Port-Pfad.
|
|
||||||
|
|
||||||
#### Vorab-Erkenntnis aus dem Code (wichtig!)
|
#### Vorab-Erkenntnis aus dem Code (wichtig!)
|
||||||
|
|
||||||
@@ -182,7 +176,13 @@ Fundstellen:
|
|||||||
- Aufgabe: Kopplung gegen die echte Sehnenmechanik validieren, toten x-Port-Pfad +
|
- Aufgabe: Kopplung gegen die echte Sehnenmechanik validieren, toten x-Port-Pfad +
|
||||||
`factorOpenTurn` aufräumen, **Vorzeichen** je nach Motor-Verkabelung prüfen.
|
`factorOpenTurn` aufräumen, **Vorzeichen** je nach Motor-Verkabelung prüfen.
|
||||||
|
|
||||||
3. **B-Konvention (gerade = 0°) ✅ ERLEDIGT** — siehe oben (Phase 2).
|
3. **B-Konvention (gerade = 0°).** Durchgängig:
|
||||||
|
- FK/IK in `Arm3SegmentLinearX` (b-Definition / acos-Zweig),
|
||||||
|
- `gripperMotorFromOpening` nachziehen,
|
||||||
|
- G92-Eingabe (`b = B/D`) + M1 + G28,
|
||||||
|
- `portInverse.js` (Umkehrung),
|
||||||
|
- **Sender-Formeln so kompensieren, dass die FluidNC-Ports unverändert bleiben** —
|
||||||
|
ODER bewusst die Hardware-Nullpunkte neu kalibrieren (Entscheidung dokumentieren).
|
||||||
|
|
||||||
4. **C-Nullpunkt (neutral = 0°).** Der `c↔ψ`-Bezug ist **posenabhängig**
|
4. **C-Nullpunkt (neutral = 0°).** Der `c↔ψ`-Bezug ist **posenabhängig**
|
||||||
(`ψ = acos(cos β · sin a) − c`). Ein konstantes `c=0=neutral` ist **nicht global** möglich,
|
(`ψ = acos(cos β · sin a) − c`). Ein konstantes `c=0=neutral` ist **nicht global** möglich,
|
||||||
@@ -193,9 +193,8 @@ Fundstellen:
|
|||||||
(`|Hand.to[1]| + |FingerA.to[1]|` = 35 + 60 = **95**) statt aus dem Ellbogen-Versatz (90).
|
(`|Hand.to[1]| + |FingerA.to[1]|` = 35 + 60 = **95**) statt aus dem Ellbogen-Versatz (90).
|
||||||
Zusätzlich sind `kinematics.l1/l2/l3` in robot.json **explizit überschreibbar** (Vorrang
|
Zusätzlich sind `kinematics.l1/l2/l3` in robot.json **explizit überschreibbar** (Vorrang
|
||||||
vor der Ableitung) — zum Kalibrieren auf die gemessene Reichweite.
|
vor der Ableitung) — zum Kalibrieren auf die gemessene Reichweite.
|
||||||
Reichweite damit 595 — passt zur aktuellen Hardware (60 mm Finger). Die früher beobachteten
|
⚠️ Geometrie liefert Reichweite 595, beobachtet wurden **~550** → l3 (oder l1/l2) sollte
|
||||||
~550 stammten von **kürzeren 50 mm-Greifern**. Bei Greifer-Wechsel `kinematics.l3` per
|
per `kinematics.l3` explizit kalibriert werden (deutet auf l3 ≈ 50, falls l1=l2=250 stimmen).
|
||||||
Override anpassen (oder Finger-Geometrie in robot.json pflegen).
|
|
||||||
|
|
||||||
6. **Tests + Doku** nachziehen: Round-Trip mit neuer Konvention, Greifer-Kopplung,
|
6. **Tests + Doku** nachziehen: Round-Trip mit neuer Konvention, Greifer-Kopplung,
|
||||||
G92-Referenztabellen in `Info_G92.md`, sowie diese Datei.
|
G92-Referenztabellen in `Info_G92.md`, sowie diese Datei.
|
||||||
|
|||||||
@@ -67,11 +67,12 @@ class RobotController {
|
|||||||
robot.alpha = 0;
|
robot.alpha = 0;
|
||||||
robot.beta = 0;
|
robot.beta = 0;
|
||||||
robot.a = 0;
|
robot.a = 0;
|
||||||
robot.b = 0; // gerade Hand (Phase-2-Konvention: b=0 = gerade)
|
robot.b = Math.PI; // gerade Hand (Phase-1-Konvention; Phase 2: 0)
|
||||||
robot.c = 0;
|
robot.c = 0;
|
||||||
// Greifer (e/eMotor) bewusst UNANGETASTET lassen: G28 fährt nur den Arm.
|
// Greifer (e/eMotor) bewusst UNANGETASTET lassen: G28 fährt nur den Arm.
|
||||||
// Mit b=0 (Phase 2) wäre eMotor = e−0−c = e−c, kein Slam mehr. Trotzdem
|
// Würde man e=0 setzen, ergäbe die Kopplung eMotor = e − b − c bei b=π den
|
||||||
// unangetastet, damit der Greifer unabhängig vom Arm gesteuert werden kann.
|
// Wert −π → −180° am Finger-Motor → Anschlag-Slam (siehe
|
||||||
|
// doc/Info_Koordinaten.md, Phase 2). Phase 2 (b=0 = gerade) löst das sauber.
|
||||||
robot.calculatePositionFromMotorAngles(); // FK -> x=0, y=-(l1+l2+l3), z=0
|
robot.calculatePositionFromMotorAngles(); // FK -> x=0, y=-(l1+l2+l3), z=0
|
||||||
} else {
|
} else {
|
||||||
// Allgemeiner (nicht-singulärer) Home-Punkt: regulär über die IK.
|
// Allgemeiner (nicht-singulärer) Home-Punkt: regulär über die IK.
|
||||||
|
|||||||
@@ -110,10 +110,9 @@ class Arm3SegmentLinearX extends RobotBase {
|
|||||||
if(Math.sin(this.theta) < 0) {this.a = -this.a}
|
if(Math.sin(this.theta) < 0) {this.a = -this.a}
|
||||||
|
|
||||||
|
|
||||||
// Handgelenk-Knick-Winkel: b=0 = gerade (Phase 2). cosB ist cos des internen
|
// Handgelenk-Knick-Winkel ist zwischen Arm und Punkt-O
|
||||||
// Winkels zwischen Unterarm und Hand; acos liefert [0,π], also b = π−acos(cosB) ∈ [0,π].
|
|
||||||
var cosB = Math.cos(this.beta)*Math.sin(this.theta)*Math.sin(this.phi) + Math.sin(this.beta)*Math.cos(this.theta);
|
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.
|
// 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);
|
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, this.b);
|
||||||
const vHand = this.rotateAroundAxis(vecUnterarm, n, Math.PI - this.b);
|
|
||||||
|
|
||||||
this.x = this.pX - this.l3 * vHand.x;
|
this.x = this.pX - this.l3 * vHand.x;
|
||||||
this.y = this.pY - this.l3 * vHand.y;
|
this.y = this.pY - this.l3 * vHand.y;
|
||||||
|
|||||||
@@ -102,7 +102,7 @@ describe('GCode.receiveGCode', () => {
|
|||||||
expect(robot.alpha).toBe(0)
|
expect(robot.alpha).toBe(0)
|
||||||
expect(robot.beta).toBe(0)
|
expect(robot.beta).toBe(0)
|
||||||
expect(robot.a).toBe(0)
|
expect(robot.a).toBe(0)
|
||||||
expect(robot.b).toBe(0) // gerade Hand (Phase-2-Konvention: b=0)
|
expect(robot.b).toBe(Math.PI) // gerade Hand (Phase-1-Konvention)
|
||||||
expect(robot.c).toBe(0)
|
expect(robot.c).toBe(0)
|
||||||
// Greifer unverändert -> kein Finger-Slam am Anschlag
|
// Greifer unverändert -> kein Finger-Slam am Anschlag
|
||||||
expect(robot.e).toBe(7)
|
expect(robot.e).toBe(7)
|
||||||
|
|||||||
@@ -20,25 +20,23 @@ describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => {
|
|||||||
}
|
}
|
||||||
|
|
||||||
test('voll ausgestreckt (alpha=beta=0, Hand gerade) -> y ~ -590', () => {
|
test('voll ausgestreckt (alpha=beta=0, Hand gerade) -> y ~ -590', () => {
|
||||||
// B=0 = gerade Hand (Phase-2-Konvention)
|
// B=180 = aktuelle "gerade Hand"-Konvention (Phase 2 macht daraus 0)
|
||||||
const r = fkFromMotors(0, 0, 0, 0, 0);
|
const r = fkFromMotors(0, 0, 0, 180, 0);
|
||||||
expect(r.y).toBeLessThan(0);
|
expect(r.y).toBeLessThan(0);
|
||||||
expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 0); // ~ -590
|
expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 0); // ~ -590
|
||||||
expect(r.z).toBeCloseTo(0, 6);
|
expect(r.z).toBeCloseTo(0, 6);
|
||||||
});
|
});
|
||||||
|
|
||||||
test('gemeldete Homing-Pose landet in -y (war faelschlich +405)', () => {
|
test('gemeldete Homing-Pose landet in -y (war faelschlich +405)', () => {
|
||||||
// G92 X160.53 Y4.53 Z13.93 A124.04 B0 C0 (Homing: B=0 = gerade Hand, Phase-2-Konvention).
|
// G92 X160.53 Y4.53 Z13.93 A124.04 (B=C=0)
|
||||||
// Phase 1: y=-405 (B=0 wurde als "gefaltet" interpretiert).
|
|
||||||
// Phase 2: y=-579 (B=0 = gerade → Hand streckt sich in Armrichtung).
|
|
||||||
const r = fkFromMotors(4.53, 13.93, 124.04, 0, 0, 160.53);
|
const r = fkFromMotors(4.53, 13.93, 124.04, 0, 0, 160.53);
|
||||||
expect(r.y).toBeLessThan(0);
|
expect(r.y).toBeLessThan(0);
|
||||||
expect(r.y).toBeCloseTo(-579, 0);
|
expect(r.y).toBeCloseTo(-405, 0);
|
||||||
expect(r.z).toBeCloseTo(102, 0);
|
expect(r.z).toBeCloseTo(58, 0);
|
||||||
});
|
});
|
||||||
|
|
||||||
test('IK der -y-Grundstellung liefert alpha~0 / beta~0 (nicht ~180)', () => {
|
test('IK der -y-Grundstellung liefert alpha~0 / beta~0 (nicht ~180)', () => {
|
||||||
const ref = fkFromMotors(0, 0, 0, 0, 0); // -y-ausgestreckte Pose als Referenz (b=0 = gerade)
|
const ref = fkFromMotors(0, 0, 0, 180, 0); // -y-ausgestreckte Pose als Referenz
|
||||||
const r = new Robot(L1, L2, L3);
|
const r = new Robot(L1, L2, L3);
|
||||||
r.x = ref.x; r.y = ref.y; r.z = ref.z;
|
r.x = ref.x; r.y = ref.y; r.z = ref.z;
|
||||||
r.phi = ref.phi; r.theta = ref.theta; r.psi = ref.psi; r.e = 0;
|
r.phi = ref.phi; r.theta = ref.theta; r.psi = ref.psi; r.e = 0;
|
||||||
@@ -67,8 +65,8 @@ describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => {
|
|||||||
expect(B.psi).toBeCloseTo(A.psi, EPS);
|
expect(B.psi).toBeCloseTo(A.psi, EPS);
|
||||||
});
|
});
|
||||||
|
|
||||||
test('Nullpose (alpha=beta=a=0, Hand gerade b=0) -> Fingerspitze (xMotor, -590, 0)', () => {
|
test('Nullpose (alpha=beta=a=0, Hand gerade b=180) -> Fingerspitze (xMotor, -590, 0)', () => {
|
||||||
const r = fkFromMotors(0, 0, 0, 0, 0, 7);
|
const r = fkFromMotors(0, 0, 0, 180, 0, 7);
|
||||||
expect(r.x).toBeCloseTo(7, 6); // x = xMotor
|
expect(r.x).toBeCloseTo(7, 6); // x = xMotor
|
||||||
expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 6);
|
expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 6);
|
||||||
expect(r.z).toBeCloseTo(0, 6);
|
expect(r.z).toBeCloseTo(0, 6);
|
||||||
@@ -76,14 +74,13 @@ describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => {
|
|||||||
|
|
||||||
test('a=0 -> Hand-Knick-Achse laeuft parallel zur x-Achse', () => {
|
test('a=0 -> Hand-Knick-Achse laeuft parallel zur x-Achse', () => {
|
||||||
// Bei a=0 knickt die Hand (b) in der y-z-Ebene -> Fingerspitze.x bleibt = xMotor.
|
// Bei a=0 knickt die Hand (b) in der y-z-Ebene -> Fingerspitze.x bleibt = xMotor.
|
||||||
// b=0=gerade, b=90=90°-Knick, b=135=135°-Knick (Phase-2-Konvention).
|
|
||||||
const xM = 5;
|
const xM = 5;
|
||||||
for (const bDeg of [0, 45, 90, 135]) {
|
for (const bDeg of [90, 135, 180, 225]) {
|
||||||
const r = fkFromMotors(0, 0, 0, bDeg, 0, xM);
|
const r = fkFromMotors(0, 0, 0, bDeg, 0, xM);
|
||||||
expect(r.x).toBeCloseTo(xM, 6);
|
expect(r.x).toBeCloseTo(xM, 6);
|
||||||
}
|
}
|
||||||
// Gegenprobe: a=90 dreht die Knick-Achse aus der y-z-Ebene -> x aendert sich deutlich.
|
// Gegenprobe: a=90 dreht die Knick-Achse aus der y-z-Ebene -> x aendert sich deutlich.
|
||||||
const r90 = fkFromMotors(0, 0, 90, 45, 0, xM);
|
const r90 = fkFromMotors(0, 0, 90, 135, 0, xM);
|
||||||
expect(Math.abs(r90.x - xM)).toBeGreaterThan(1);
|
expect(Math.abs(r90.x - xM)).toBeGreaterThan(1);
|
||||||
});
|
});
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user