From bd1752f56734024d988e8bef78e0e8b57546ca0b Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Fri, 26 Jun 2026 10:27:42 +0200 Subject: [PATCH] Phase1 Koordinaten --- doc/Info_G92.md | 17 +-- doc/Info_Koordinaten.md | 170 +++++++++++++----------- logs/gcode_commands.log | 9 ++ logs/pings.log | 2 + test/Robot.Kinematics.NegativeY.test.js | 19 +++ 5 files changed, 131 insertions(+), 86 deletions(-) diff --git a/doc/Info_G92.md b/doc/Info_G92.md index 175efd9..7f81146 100644 --- a/doc/Info_G92.md +++ b/doc/Info_G92.md @@ -84,16 +84,17 @@ A=180 → von der Schulter weg (y=−340); Knick konstant 90°. ### C = Hand-Dreher (Roll) -C dreht die Hand um ihre eigene Achse. Verifizierte Referenz (α=0, β=90, A=0, B=90): +C dreht die Hand um ihre eigene Achse. Verifizierte Referenz (α=0, β=90, A=0, B=90), +nach Phase 1: | C (G92) | Hand-Roll ψ | |---------|-------------| -| 0° | −90° | +| 0° | +90° | | 90° | 0° (neutral) | -| 180° | +90° | +| 180° | −90° | -→ In dieser Stellung gilt `ψ = C − 90°`. **Achtung:** der genaue Zusammenhang hängt von -der Armstellung ab — exakt `ψ = C − acos(cos β · sin A)` (Winkel in rad). C selbst ist der +→ In dieser Stellung gilt `ψ = 90° − C`. **Achtung:** der genaue Zusammenhang hängt von +der Armstellung ab — exakt `ψ = acos(cos β · sin A) − c` (Winkel in rad). C selbst ist der physische Hand-Roll-Gelenkwinkel; nur der Bezug zum Welt-ψ verschiebt sich mit der Pose. ### E = Greifer-Öffnung (mm) + Sehnen-Kopplung @@ -115,9 +116,9 @@ nur das Handgelenk, kompensiert eMotor, damit die Öffnung konstant bleibt. | Achse | Driver erwartet | Typische Falle | |-------|------------------------------------------------|-----------------------------------------| | X | xMotor in mm | — | -| Y | Oberarm absolut (0=waagerecht, 90=hoch) | — | -| Z | Unterarm **absolut** (nicht relativ zum Oberarm) | relativ statt absolut gesendet | -| A | Unterarm-Dreher (Richtung des Knicks) | Nullpunkt/Vorzeichen | +| Y | Oberarm absolut (0=waagerecht **−y**, 90=hoch) | — | +| Z | Unterarm **absolut** (0=−y; nicht relativ zum Oberarm) | relativ statt absolut gesendet | +| A | Unterarm-Dreher; **A=0 → Knick-Achse ∥ x** | Nullpunkt/Vorzeichen | | B | **180° = gerade Hand**; Knick = 180° − B | gemessenen Knick direkt gesendet | | C | Hand-Roll, **90° = neutral** | Nullpunkt/Vorzeichen | | E | Öffnung in mm | Motorwert statt Öffnung gesendet | diff --git a/doc/Info_Koordinaten.md b/doc/Info_Koordinaten.md index 2ad61c7..37db367 100644 --- a/doc/Info_Koordinaten.md +++ b/doc/Info_Koordinaten.md @@ -4,15 +4,11 @@ Diese Datei beschreibt 1. das Koordinatensystem, 2. wie der Roboter darin steht, 3. die angestrebte **ideale Nullstellung** und -4. die nötigen Schritte, um den Driver auf diese Konvention zu bringen (**Weg 2: - Modell auf −Y drehen**). +4. die Schritte, um den Driver auf diese Konvention zu bringen (**Weg 2: Modell auf −Y**). -> **Hintergrund / Problem:** Das Kinematik-Modell (`Arm3SegmentLinearX`) misst die -> Armwinkel aktuell von **+Y** (α=0 → Arm zeigt nach +y). Der reale Roboter steht und -> arbeitet aber in **−Y** (robot.json: `Arm1.skeleton.to = [0,-250,0]`, -> `coordinateSystem.y = "backward"`). Dadurch landet eine fast-waagerechte -> Grundstellung im Driver bei **y ≈ +590** statt **−590** — Modell und Hardware sind an -> der y-Achse gespiegelt. +> **Status:** **Phase 1 (y-Flip) ist umgesetzt und am Roboter verifiziert.** α/β werden +> jetzt von **−y** aus gemessen (α=0 → Arm zeigt nach −y), passend zu robot.json und +> appRobotHoming. Offen ist **Phase 2** (Handgelenk-/Finger-Nullstellung, B/C). --- @@ -20,30 +16,38 @@ Diese Datei beschreibt Aus robot.json (`coordinateSystem`): **rechtshändig**, Längen **mm**, Winkel **Grad**. -| Achse | Richtung | Bedeutung im Aufbau | -|-------|------------|-------------------------------------------| -| x | rechts | entlang der Linearschiene | +| Achse | Richtung | Bedeutung im Aufbau | +|-------|------------|----------------------------------------------| +| x | rechts | entlang der Linearschiene | | y | „backward" | Arm-Arbeitsrichtung: Arm streckt nach **−y** | -| z | oben | Höhe | +| z | oben | Höhe | + +**Seitenansicht** (Blick entlang +x, also die −y/z-Ebene): ``` -Seitenansicht (Blick entlang +x): - z ▲ - | ■══════════════════════● Arm waagerecht ausgestreckt → Fingerspitze - | Schulter (Ursprung, z=0) + | ■═══════════════●===========●---o Arm1 ═══ , Arm2 === und Finger --- waagerecht + | Schulter (Ursprung, z=0) ausgestreckt → Fingerspitze o └───────────────────────────────► −y (Arbeitsrichtung) +``` -Draufsicht (Blick von oben, −z): +In der −y/z-Seitenansicht liegen beide Handgelenk-Achsen **end-on** (als Punkt ●), weil +sie entlang x verlaufen: +- linkes ● = **a-Achse** (Unterarm-Dreher, zwischen Ellbogen und Arm2) +- rechtes ● = **b-Achse** (Hand-Knick-Achse) - −y ▲ ● Fingerspitze (Grundstellung y ≈ −590) - │ │ - │ │ Arm (Ober- + Unterarm, gestreckt) - │ │ - ───┼────────────────■─────────────► x (Schiene) - │ Schulter - +y │ +**Draufsicht** (Blick von oben auf −z; x nach oben, −y nach rechts): + +``` + x + ▲ │ │ + │ ■════╪═══ Arm1 ═════════╪══ Arm2 ══----o o = Fingerspitze (y ≈ −590) + │ Schulter + └────────────────────────────────────────────► −y + + │ = a-Achse (Ellbogen↔Arm2) bzw. b-Achse (Hand-Knick). + Bei a=0 laufen BEIDE parallel zur x-Achse (hier als senkrechte Striche). ``` --- @@ -68,82 +72,92 @@ Gelenk-Kette (robot.json `links`): ## 3. Ideale Nullstellung (Grundstellung) -**Definition:** Arm **waagerecht voll ausgestreckt entlang −y**, Hand **gerade** in -Verlängerung des Unterarms, Greifer geschlossen. +**Definition (deine Vorgabe):** -**Ziel:** in dieser Stellung sind **alle Gelenkwinkel = 0°**. +1. **Arm1 entlang −y** (Schulter-/y-Gelenk α) → **α = 0**. +2. **Ellbogen so angewinkelt, dass auch Arm2 entlang −y** (z-Gelenk β) → **β = 0** + (β ist absolut, also auch direkt entlang −y, nicht relativ zum Oberarm). +3. **a-Achse so gedreht, dass die Hand-Knick-Achse genau in x-Richtung läuft** → **a = 0**. -| Achse | Ideal-Wert | Bedeutung | -|--------|-----------|-------------------------------------------| -| X | (frei) | Schienenposition `xMotor` in mm | -| Y (α) | **0°** | Oberarm waagerecht entlang −y | -| Z (β) | **0°** | Unterarm waagerecht entlang −y (gestreckt) | -| A (a) | **0°** | kein Unterarm-Dreh | -| B (b) | **0°** | Hand gerade | -| C (c) | **0°** | kein Hand-Roll | -| E (e) | **0** | Greifer geschlossen / Referenz | +Diese drei Bedingungen erfüllt der Driver **nach Phase 1 bereits** (verifiziert, s.u.): -→ Resultierende Fingerspitze: **(xMotor, −(l1+l2+l3), 0) ≈ (x, −590, 0)**. +| Achse | Nullwert | Bedeutung | Status | +|--------|----------|---------------------------------------------|-------------------------| +| X | (frei) | Schienenposition `xMotor` in mm | — | +| Y (α) | **0°** | Oberarm waagerecht entlang −y | ✅ Phase 1 | +| Z (β) | **0°** | Unterarm waagerecht entlang −y (gestreckt) | ✅ Phase 1 | +| A (a) | **0°** | Hand-Knick-Achse ∥ x | ✅ (Code erfüllt es) | +| B (b) | 0° (Ziel) | Hand gerade — **derzeit ist gerade = 180°** | ⏳ Phase 2 | +| C (c) | 0° (Ziel) | kein Hand-Roll — **derzeit neutral ≠ 0** | ⏳ Phase 2 | +| E (e) | **0** | Greifer geschlossen / Referenz | — | + +→ Resultierende Fingerspitze (α=β=a=0, gerade Hand): **(xMotor, −(l1+l2+l3), 0) ≈ (x, −590, 0)**. (Beobachtet ~−550; Differenz steckt in der l3-Ableitung / Resthandstellung.) --- -## 4. Ist-Zustand vs. Ideal +## 4. Ist-Zustand (nach Phase 1) vs. Ideal -| Aspekt | aktuell (Modell +Y) | ideal (−Y) | -|----------------------------|---------------------------|-------------------| -| α=0 zeigt nach | +y (FK: y=+410) | −y (y=−410) | -| Grundstellung Y (α) | ≈175° (=180−4,5) | **0°** | -| gerade Hand B (b) | 180° | **0°** | -| neutraler Roll C (c) | 90° (posenabhängig) | **0°** | -| G92 der Grundstellung → y | ≈ +590 | **≈ −590** | +| Aspekt | nach Phase 1 (jetzt) | Ideal (nach Phase 2) | +|------------------------------|-----------------------------|----------------------| +| α=0 zeigt nach | **−y** ✅ | −y | +| β=0 Unterarm | **−y** ✅ | −y | +| a=0 Hand-Knick-Achse | **∥ x** ✅ | ∥ x | +| G92 der Grundstellung → y | **≈ −590** ✅ | ≈ −590 | +| gerade Hand | **b = 180°** | **b = 0°** | +| neutraler Roll | **c: ψ = 90° − C** (posenabh.) | **c = 0°** | -Verifiziert per FK: `α=β=0 → y=+410`, `α=β=180 → y=−410`. Die +Y/−Y-Spiegelung -entspricht `α→180−α, β→180−β`. +Verifiziert per FK: `FK(α=0, β=0, gerade Hand) → (0, −590, 0)`; bei `a=0` bleibt +`Fingerspitze.x = xMotor` konstant, während b variiert (Knick in der y-z-Ebene). --- -## 5. Schritte, um das zu erreichen (Weg 2) +## 5. Schritte (Weg 2) Reihenfolge nach Workflow: **erst Tests (rot), dann Code, dann grün.** -### Phase 1 — y-Flip (behebt den gemeldeten Bug) +### Phase 1 — y-Flip ✅ ERLEDIGT (am Roboter verifiziert) -1. **Tests schreiben** (zunächst rot): - - G92 der Grundstellung (α=β=a=b=c=0) → erwartet **y ≈ −590** (statt +590). - - Round-Trip: `IK(x=0, y=−550, z, φ, θ)` → `FK` → identische Pose zurück. -2. **`Arm3SegmentLinearX` umstellen** (α/β von −y aus messen): - - **FK** (`calculatePositionFromMotorAngles`): y-Komponenten von `vecBizeps` und - `vecUnterarm` negieren; Handgelenk-Vektoren (`n`, `vHand`) und `φ = atan2(vHand.y,…)` - konsistent nachziehen. - - **IK** (`calculateAngles3D`): `pY`, `gamma = atan2(pZ, pY)`, die `n`-Konstruktion - und `φ` entsprechend spiegeln. -3. **Bestehende +Y-Tests auf −Y migrieren** (erwartete y-Werte spiegeln). -4. **`doc/Info_G92.md`** aktualisieren: Y/Z-Konvention auf „0° = waagerecht entlang −y". -5. Suite grün; Round-Trip + Grundstellung grün. +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`. +- **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 auf Null (optional, für echte All-Zero-Grundstellung) +### Phase 2 — Handgelenk-/Finger-Nullstellung (B, C) — OFFEN -6. **B-Konvention drehen:** gerade Hand = **0°** statt 180° (`b → 180°−b` an der - Schnittstelle; FK/IK + Greifer-Kopplung `eMotor = e − b − c` mit anpassen). -7. **C-Nullpunkt:** neutral = **0°** statt 90°. - ⚠️ Der C↔ψ-Offset ist **posenabhängig** (`acos(cos β · sin a)`); ein global sauberes - `c=0=neutral` braucht ggf. eine tiefere Umparametrierung des Handgelenks — **vor** - der Umsetzung bewerten. -8. Tests + `Info_G92.md` nachziehen. +> **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**. + +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. ### Verifikation (Definition of Done) -- G92 der Grundstellung (alle Winkel 0) → Driver meldet **y ≈ −590, z ≈ 0**. -- appRobotHoming kann die **physisch gemessenen Winkel direkt** senden (ohne Spiegelung). -- Volle Test-Suite grün, inkl. Round-Trip und Grundstellungs-Tests. +- **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. --- -## Anhang: Stand der bisherigen Arbeit +## Anhang: Stand der Kinematik -- Der atan2-Fix in der IK (`gamma = Math.atan2(pZ, pY)`, [Arm3SegmentLinearX.js:59](../robot/kinematics/Arm3SegmentLinearX.js#L59)) - ist umgesetzt und macht die IK mathematisch für −Y-Eingaben korrekt — eine - Voraussetzung für Phase 1. Er betrifft **nicht** den G92/FK-Pfad. -- Die Winkel-Konventionen (B/C/E) sind in [doc/Info_G92.md](Info_G92.md) dokumentiert; - die Y/Z-Konvention dort wird in Phase 1, Schritt 4 angepasst. +- **y-Flip (Phase 1):** Spiegelung in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`, genutzt von + `calculateAngles3D` und `calculatePositionFromMotorAngles`). Am Roboter bestätigt. +- **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 + und nach Phase 1 aktualisiert. diff --git a/logs/gcode_commands.log b/logs/gcode_commands.log index f7b5643..cc26654 100644 --- a/logs/gcode_commands.log +++ b/logs/gcode_commands.log @@ -11128,3 +11128,12 @@ 2026-06-26T06:25:53.335Z ::ffff:127.0.0.1: M114 2026-06-26T06:25:53.565Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 2026-06-26T06:25:53.811Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T08:24:39.807Z ::ffff:127.0.0.1: M114 +2026-06-26T08:24:39.825Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T08:24:39.902Z ::ffff:127.0.0.1: FList +2026-06-26T08:24:39.925Z ::ffff:127.0.0.1: FPlus +2026-06-26T08:24:39.944Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T08:24:39.958Z ::ffff:127.0.0.1: FShow +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 diff --git a/logs/pings.log b/logs/pings.log index 6d712cb..807e012 100644 --- a/logs/pings.log +++ b/logs/pings.log @@ -14834,3 +14834,5 @@ 2026-06-26T06:23:06.656Z ::ffff:127.0.0.1 : Ping 2026-06-26T06:25:53.069Z ::ffff:127.0.0.1 : Ping 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 diff --git a/test/Robot.Kinematics.NegativeY.test.js b/test/Robot.Kinematics.NegativeY.test.js index 54950f1..b24731c 100644 --- a/test/Robot.Kinematics.NegativeY.test.js +++ b/test/Robot.Kinematics.NegativeY.test.js @@ -63,4 +63,23 @@ describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => { expect(B.theta).toBeCloseTo(A.theta, EPS); expect(B.psi).toBeCloseTo(A.psi, EPS); }); + + test('Nullpose (alpha=beta=a=0, Hand gerade b=180) -> Fingerspitze (xMotor, -590, 0)', () => { + const r = fkFromMotors(0, 0, 0, 180, 0, 7); + expect(r.x).toBeCloseTo(7, 6); // x = xMotor + expect(r.y).toBeCloseTo(-(L1 + L2 + L3), 6); + expect(r.z).toBeCloseTo(0, 6); + }); + + 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. + const xM = 5; + for (const bDeg of [90, 135, 180, 225]) { + const r = fkFromMotors(0, 0, 0, bDeg, 0, xM); + expect(r.x).toBeCloseTo(xM, 6); + } + // Gegenprobe: a=90 dreht die Knick-Achse aus der y-z-Ebene -> x aendert sich deutlich. + const r90 = fkFromMotors(0, 0, 90, 135, 0, xM); + expect(Math.abs(r90.x - xM)).toBeGreaterThan(1); + }); });