diff --git a/doc/Info_Koordinaten.md b/doc/Info_Koordinaten.md index 37db367..887d89e 100644 --- a/doc/Info_Koordinaten.md +++ b/doc/Info_Koordinaten.md @@ -122,34 +122,95 @@ Umgesetzt: - **Spiegelung an der x-z-Ebene** in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`): die interne Mathematik (`_ikPlusY`/`_fkPlusY`) rechnet weiter in +y, die öffentlichen Methoden spiegeln die Workspace-Pose (y, pY, φ, ψ; θ bleibt) → α=0 zeigt nach −y. -- **G28** (Home) auf −y umgestellt: `y = -(l1+l2+l3)`, `phi = +π/2`. +- **G28** (Home) auf −y umgestellt **und Singularität behandelt**: die voll ausgestreckte + 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 + **direkt** (`alpha=beta=a=c=0`, `b=π` = gerade Hand) und füllt die Pose per FK. - **Tests:** `test/Robot.Kinematics.NegativeY.test.js` (Grundstellung −590, Homing-Pose in −y, Round-Trip in −y, a=0 → Knick-Achse ∥ x). - **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. -### Phase 2 — Handgelenk-/Finger-Nullstellung (B, C) — OFFEN +### Phase 2 — Handgelenk-/Finger-Nullstellung (B, C, Greifer) — OFFEN -> **Voraussetzung (User):** Erst Visualisierung/Überprüfung der Finger, dort werden noch -> Fehler vermutet. **a-Achse ist bereits korrekt** (a=0 → Knick-Achse ∥ x) — Phase 2 -> betrifft nur **B (Knick)**, **C (Roll)** und die **Greifer-Kopplung**. +> **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**. -1. **Finger/Hand visualisieren** und gegen die echte Mechanik prüfen (User). -2. **B-Konvention:** gerade Hand soll **0°** sein (derzeit 180°; physischer Knick = 180° − B). - Mapping festlegen (z.B. `b → 180° − b` an der Schnittstelle) und FK/IK anpassen. -3. **C-Nullpunkt:** neutral soll **0°** sein (derzeit `ψ = 90° − C`). - ⚠️ Der C↔ψ-Bezug ist **posenabhängig** (`acos(cos β · sin a)`); ein global sauberes - `c=0 = neutral` braucht ggf. eine tiefere Umparametrierung — **vor** der Umsetzung bewerten. -4. **Greifer-Kopplung** `eMotor = e − b − c` prüfen: mischt mm (e) mit Radiant (b, c) - — gegen die echte Sehnen-Mechanik validieren. -5. Tests + `Info_G92.md` nachziehen. +**Ziel-Konvention:** gerade Hand → **B = 0°** (statt 180°); neutraler Roll → **C = 0°**; +Greifer-Kopplung konsistent und gegen die echte Mechanik kalibriert. + +#### Vorab-Erkenntnis aus dem Code (wichtig!) + +Die b/c/e-Konvention ist an **mehreren** Stellen kodiert, die **gemeinsam** geändert werden +müssen. **Invariante:** solange die Hardware-Nullpunkte nicht neu kalibriert werden, müssen +die an FluidNC gesendeten Port-Werte **gleich bleiben** — eine reine Modell-Umbenennung darf +die Hardware-Bewegung nicht verändern. + +Fundstellen: + +| Datei / Stelle | aktuelle Kodierung | +|----------------|--------------------| +| `Arm3SegmentLinearX._fkPlusY` | `vHand = rotate(vecUnterarm, n, b)`; `psi = c − acos(−n.z)` → **b=π = gerade** | +| `Arm3SegmentLinearX._ikPlusY` | `b = acos(cosB)` (∈[0,π]); `c = acos(cosC) + psi` → **c hat posenabh. Offset** | +| `Arm3SegmentLinearX.gripperMotorFromOpening` | `eMotor = e − b − c` (b,c in **rad**) — **Greifer-Kopplung #1** | +| `RobotController` G92/M92 | `b = B/D`, `c = C/D`, `eMotor = gripperMotorFromOpening(e)` | +| `RobotController` M1 | `b += B`, `c += C` (relativer Motor-Jog) | +| `RobotController` G28 | `b = π`, `c = 0` (Phase 1) → nach B-Umstellung auf `b = 0` ändern | +| `portInverse.js` | `b = hand.z/D`, `c = hand.x/D + b` (Port→Motor, Hardware-Sync) | +| `TelnetSenderGRBL.execCommand` / `portValue` | Hand-Ports: `z = b·D`, `x = (c−b)·D`; **e-Port mit `factorTurnLift=1.2`** — **Greifer-Kopplung #2** | + +#### Aufgaben + +1. **Finger visualisieren** (User) → Soll-Bild, gegen das kalibriert wird. + +2. **Greifer-Kopplung vereinheitlichen.** Es existieren **zwei widersprüchliche** Kopplungen: + - Kinematik: `eMotor = e − b − c` (b,c in rad) + - Sender: `e-Port = e + 1.2·b·D − c·D` (b,c in Grad, Faktor **1.2** nur auf b) + + Eine Quelle der Wahrheit festlegen und gegen die echte Sehnenmechanik messen. + (`factorOpenTurn = 1.92` im Sender ist deklariert, aber **ungenutzt** → klären/entfernen.) + +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** + (`ψ = acos(cos β · sin a) − c`). Ein konstantes `c=0=neutral` ist **nicht global** möglich, + ohne die Hand-Parametrierung zu ändern. Bewerten: c als reinen Gelenkwinkel führen + (Offset herausrechnen) oder die ψ-Definition anpassen. + +5. **l3-Ableitung korrigieren** (`RobotConfig.js`): `l3` kommt aus + `Ellbow.skeleton.to[0] = 90` (Ellbogen-Versatz), **nicht** aus der echten Hand-/Finger- + Länge — das erklärt die beobachteten −550 statt −590. Aus der echten Finger-Geometrie ableiten. + +6. **Tests + Doku** nachziehen: Round-Trip mit neuer Konvention, Greifer-Kopplung, + G92-Referenztabellen in `Info_G92.md`, sowie diese Datei. + +#### Ansatz-Entscheidung (vor Umsetzung) + +- **Klein/lokal:** nur die **G92-Eingabe** umrechnen (appRobotHoming sendet B=0/C=0 für + gerade/neutral, Driver mappt intern auf die alte Konvention). Wenig Risiko, aber das + interne Modell bleibt „unsauber". +- **Groß/sauber:** interne Konvention durchgängig umstellen (alle Fundstellen oben) mit + Hardware-Port-Invariante. Sauberes All-Zero-Home, aber koordinierter Eingriff. + +#### Verifikation + +Jede B/C/Greifer-Änderung gegen **Visualisierung UND einen Hardware-Test** (eine Achse +isoliert) prüfen — das Modell allein genügt hier nicht, weil es um die Hardware-Abbildung geht. ### Verifikation (Definition of Done) - **Phase 1 (erfüllt):** G92 der Grundstellung → Driver `y ≈ −590, z ≈ 0`; appRobotHoming - sendet die gemessenen α/β/a **direkt** (ohne Spiegelung); volle Suite grün. -- **Phase 2 (Ziel):** Grundstellung mit **allen** Gelenkwinkeln 0 (inkl. B=C=0); Finger - visuell korrekt. + sendet die gemessenen α/β/a **direkt** (ohne Spiegelung); **G28 fährt sauber gestreckt + nach −y** (a=0, kein Singularitäts-Müll); volle Suite grün. +- **Phase 2 (Ziel):** Grundstellung mit **allen** Gelenkwinkeln 0 (inkl. B=C=0); Greifer- + Kopplung vereinheitlicht; Finger visuell korrekt. --- @@ -157,6 +218,8 @@ Umgesetzt: - **y-Flip (Phase 1):** Spiegelung in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`, genutzt von `calculateAngles3D` und `calculatePositionFromMotorAngles`). Am Roboter bestätigt. +- **G28-Singularität (Phase 1):** voll ausgestreckt setzt `RobotController` die Motorwerte + direkt (statt der singulären IK) → Finger sauber entlang −y. - **atan2-Fix** in der IK (`gamma = Math.atan2(pZ, pY)`): macht die interne IK für −y mathematisch korrekt — Voraussetzung des y-Flips. - **Winkel-Konventionen** (Y/Z/A/B/C/E) sind in [doc/Info_G92.md](Info_G92.md) dokumentiert diff --git a/logs/gcode_commands.log b/logs/gcode_commands.log index cc26654..32ea1e3 100644 --- a/logs/gcode_commands.log +++ b/logs/gcode_commands.log @@ -11137,3 +11137,75 @@ 2026-06-26T08:24:40.035Z ::ffff:127.0.0.1: M114 2026-06-26T08:24:40.264Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 2026-06-26T08:24:40.497Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T09:26:48.688Z ::ffff:127.0.0.1: FList +2026-06-26T09:26:48.711Z ::ffff:127.0.0.1: M114 +2026-06-26T09:26:48.729Z ::ffff:127.0.0.1: FPlus +2026-06-26T09:26:48.732Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:26:48.744Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T09:26:48.758Z ::ffff:127.0.0.1: FShow +2026-06-26T09:26:48.922Z ::ffff:127.0.0.1: M114 +2026-06-26T09:26:49.142Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:26:49.372Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T09:27:11.069Z ::ffff:127.0.0.1: FList +2026-06-26T09:27:11.100Z ::ffff:127.0.0.1: M114 +2026-06-26T09:27:11.120Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:27:11.122Z ::ffff:127.0.0.1: FPlus +2026-06-26T09:27:11.140Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T09:27:11.157Z ::ffff:127.0.0.1: FShow +2026-06-26T09:27:11.295Z ::ffff:127.0.0.1: M114 +2026-06-26T09:27:11.517Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:27:11.744Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T09:27:19.593Z ::ffff:127.0.0.1: FList +2026-06-26T09:27:19.607Z ::ffff:127.0.0.1: M114 +2026-06-26T09:27:19.624Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:27:19.627Z ::ffff:127.0.0.1: FPlus +2026-06-26T09:27:19.639Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T09:27:19.650Z ::ffff:127.0.0.1: FShow +2026-06-26T09:27:19.814Z ::ffff:127.0.0.1: M114 +2026-06-26T09:27:20.029Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:27:20.258Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T09:27:36.334Z ::ffff:127.0.0.1: FList +2026-06-26T09:27:36.366Z ::ffff:127.0.0.1: M114 +2026-06-26T09:27:36.367Z ::ffff:127.0.0.1: FPlus +2026-06-26T09:27:36.382Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:27:36.387Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T09:27:36.401Z ::ffff:127.0.0.1: FShow +2026-06-26T09:27:36.558Z ::ffff:127.0.0.1: M114 +2026-06-26T09:27:36.772Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:27:37.001Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T09:28:07.329Z ::ffff:127.0.0.1: M114 +2026-06-26T09:28:07.361Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:28:07.445Z ::ffff:127.0.0.1: M114 +2026-06-26T09:28:07.499Z ::ffff:127.0.0.1: FList +2026-06-26T09:28:07.549Z ::ffff:127.0.0.1: FPlus +2026-06-26T09:28:07.574Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T09:28:07.593Z ::ffff:127.0.0.1: FShow +2026-06-26T09:28:07.692Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:28:07.941Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T09:28:10.419Z ::ffff:127.0.0.1: M114 +2026-06-26T09:28:10.422Z ::ffff:127.0.0.1: FList +2026-06-26T09:28:10.444Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:28:10.464Z ::ffff:127.0.0.1: FPlus +2026-06-26T09:28:10.477Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T09:28:10.485Z ::ffff:127.0.0.1: FShow +2026-06-26T09:28:10.667Z ::ffff:127.0.0.1: M114 +2026-06-26T09:28:10.883Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:28:11.112Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T09:35:29.613Z ::ffff:127.0.0.1: M114 +2026-06-26T09:35:29.633Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:35:29.819Z ::ffff:127.0.0.1: M114 +2026-06-26T09:35:29.908Z ::ffff:127.0.0.1: FList +2026-06-26T09:35:29.947Z ::ffff:127.0.0.1: FPlus +2026-06-26T09:35:29.964Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T09:35:29.991Z ::ffff:127.0.0.1: FShow +2026-06-26T09:35:30.059Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:35:30.309Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T09:35:33.075Z ::ffff:127.0.0.1: FList +2026-06-26T09:35:33.109Z ::ffff:127.0.0.1: FPlus +2026-06-26T09:35:33.125Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T09:35:33.138Z ::ffff:127.0.0.1: FShow +2026-06-26T09:35:33.254Z ::ffff:127.0.0.1: M114 +2026-06-26T09:35:33.271Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:35:33.429Z ::ffff:127.0.0.1: M114 +2026-06-26T09:35:33.652Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T09:35:33.884Z ::ffff:127.0.0.1: G1 X1 diff --git a/logs/pings.log b/logs/pings.log index 807e012..6e73b6b 100644 --- a/logs/pings.log +++ b/logs/pings.log @@ -14836,3 +14836,19 @@ 2026-06-26T06:25:53.087Z ::ffff:127.0.0.1 : Ping 2026-06-26T08:24:39.787Z ::ffff:127.0.0.1 : Ping 2026-06-26T08:24:39.805Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:26:48.670Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:26:48.675Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:27:11.052Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:27:11.059Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:27:19.569Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:27:19.582Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:27:36.325Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:27:36.338Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:28:07.197Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:28:07.297Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:28:10.358Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:28:10.433Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:35:29.584Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:35:29.593Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:35:33.181Z ::ffff:127.0.0.1 : Ping +2026-06-26T09:35:33.224Z ::ffff:127.0.0.1 : Ping diff --git a/robot/RobotController.js b/robot/RobotController.js index 77e7faa..5f58d84 100644 --- a/robot/RobotController.js +++ b/robot/RobotController.js @@ -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; } diff --git a/test/GCode.receiveGCode.test.js b/test/GCode.receiveGCode.test.js index 6f19d68..0a1d096 100644 --- a/test/GCode.receiveGCode.test.js +++ b/test/GCode.receiveGCode.test.js @@ -90,17 +90,21 @@ describe('GCode.receiveGCode', () => { expect(robot.sendCommand).toHaveBeenCalled() }) - test('G28 setzt Home-Position und löst Bewegung aus', () => { + test('G28 setzt Home-Motorwerte direkt (Singularität) und löst Bewegung aus', () => { const robot = createDummyRobot() GCode.receiveGCode(robot, 'G28') - expect(robot.x).toBe(0) - expect(robot.z).toBe(0) - expect(robot.y).toBe(-(robot.l1 + robot.l2 + robot.l3)) // -y Grundstellung - expect(robot.phi).toBeCloseTo(Math.PI / 2) - expect(robot.theta).toBeCloseTo(Math.PI / 2) - expect(robot.calculateAngles3D).toHaveBeenCalledTimes(1) + // Voll ausgestreckt = Handgelenk-Singularität -> Motorwerte DIREKT, dann FK (nicht IK). + expect(robot.xMotor).toBe(0) + expect(robot.alpha).toBe(0) + expect(robot.beta).toBe(0) + expect(robot.a).toBe(0) + expect(robot.b).toBe(Math.PI) // gerade Hand (Phase-1-Konvention) + expect(robot.c).toBe(0) + expect(robot.e).toBe(0) + expect(robot.calculateAngles3D).not.toHaveBeenCalled() + expect(robot.calculatePositionFromMotorAngles).toHaveBeenCalledTimes(1) expect(robot.sendCommand).toHaveBeenCalledTimes(1) }) diff --git a/test/Robot.Kinematics.NegativeY.test.js b/test/Robot.Kinematics.NegativeY.test.js index b24731c..877d6f1 100644 --- a/test/Robot.Kinematics.NegativeY.test.js +++ b/test/Robot.Kinematics.NegativeY.test.js @@ -1,6 +1,7 @@ // Phase 1: Der reale Roboter arbeitet in -Y (robot.json: Arm1 -> [0,-250,0]). // alpha=0 muss nach -y zeigen, nicht nach +y. Siehe doc/Info_Koordinaten.md. const Robot = require('../robot/kinematics/Arm3SegmentLinearX'); +const GCode = require('../robot/GCode'); const D = 180 / Math.PI; describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => { @@ -82,4 +83,25 @@ describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => { const r90 = fkFromMotors(0, 0, 90, 135, 0, xM); expect(Math.abs(r90.x - xM)).toBeGreaterThan(1); }); + + test('G28 Home: saubere Grundstellung (a=0, c=0, Finger entlang -y) — keine Singularitaets-Garbage', () => { + const robot = new Robot(L1, L2, L3); + GCode.receiveGCode(robot, 'G28'); + + // Motorwerte sauber gesetzt (nicht der IK-Singularitaets-Muell a=135/c=45) + expect(robot.a).toBeCloseTo(0, 9); + expect(robot.c).toBeCloseTo(0, 9); + expect(robot.alpha).toBeCloseTo(0, 9); + expect(robot.beta).toBeCloseTo(0, 9); + + // Workspace: voll ausgestreckt entlang -y + expect(robot.x).toBeCloseTo(0, 6); + expect(robot.y).toBeCloseTo(-(L1 + L2 + L3), 6); + expect(robot.z).toBeCloseTo(0, 6); + + // Finger (Handgelenk -> Fingerspitze) zeigt nach -y + const hx = robot.x - robot.pX, hy = robot.y - robot.pY, hz = robot.z - robot.pZ; + const n = Math.hypot(hx, hy, hz); + expect(hy / n).toBeCloseTo(-1, 6); + }); });