diff --git a/README.md b/README.md index 9efbe28..2c093a2 100644 --- a/README.md +++ b/README.md @@ -131,7 +131,8 @@ Die Achszuordnung kann in `robot/TelnetSenderGRBL.js` durch Anpassung der Konstr - `server/InputWS.js` - `server/InfoServer.js` - `robot/RobotBase.js` — abstrakte Basisklasse / Interface-Vertrag (generische Infrastruktur) -- `robot/kinematics/Arm3SegmentLinearX.js` — konkrete Kinematik (Modell + Inverse/Vorwärts) +- `robot/kinematics/Arm3SegmentLinearX.js` — konkrete Kinematik (Modell + Inverse/Vorwärts), Default-Arm +- `robot/kinematics/Arm3SegmentRotaryBase.js` — Kinematik für den Joy-IT „Grab-It" (Robot02), 5 Achsen + Greifer mit Drehbasis (`ROBOT_KINEMATICS=grabit`) - `robot/KinematicsFactory.js` — wählt die Kinematik per Umgebungsvariable - `robot/GCodeParser.js` — wandelt rohe Nachrichten in strukturierte Befehlsobjekte - `robot/RobotController.js` — wendet geparste Befehle auf das Modell an (Steuerlogik) diff --git a/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md b/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md index 888642c..830357a 100644 --- a/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md +++ b/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md @@ -146,10 +146,29 @@ environment: Erst wenn ein konkreter zweiter Roboter definiert ist. -- [ ] Physikalische Spezifikation dokumentieren (DOF, Achsen, Gelenkreihenfolge) -- [ ] `robot/kinematics/.js` anlegen — nur die zwei Kinematik-Methoden -- [ ] RoundTrip-Tests für die neue Implementierung schreiben -- [ ] Prüfen ob die 7 Motor-Slots ausreichen; falls nicht → `RobotMotorPosition` anpassen +**Umgesetzt: Joy-IT „Grab-It" (Robot02)** als `Arm3SegmentRotaryBase`. + +- [x] Physikalische Spezifikation dokumentieren (DOF, Achsen, Gelenkreihenfolge) + → im JSDoc von `robot/kinematics/Arm3SegmentRotaryBase.js`. 5 Achsen + Greifer: + Basis-Yaw, Schulter, Ellbogen, Handgelenk-Pitch, Handgelenk-Roll, Greifer. +- [x] `robot/kinematics/Arm3SegmentRotaryBase.js` anlegen — nur die zwei Kinematik-Methoden +- [x] RoundTrip-Tests für die neue Implementierung schreiben + (`test/Robot.GrabIt.RoundTrip.test.js`) +- [x] Prüfen ob die 7 Motor-Slots ausreichen → **ja**: 6 Slots belegt + (`xMotor, alpha, beta, a, b, eMotor`), `c` bleibt frei. `RobotMotorPosition` + unverändert. +- [x] In `KinematicsFactory` registriert (Bezeichner `arm3segmentrotarybase`, + Aliase `grabit` / `robot02`). Factory reicht jetzt das vollständige + `params`-Objekt als 4. Konstruktor-Argument durch (für `baseHeight`). + +**Offene Punkte / Annahmen (kein Blocker, aber vor Echtbetrieb zu klären):** +- ⚠️ **5-DOF-Constraint:** `phi` (Hand-Azimut) ist an die Position gekoppelt + (= Basis-Drehung) und nicht frei. In der Inversen aus `atan2(y,x)` abgeleitet. +- ⚠️ **Segmentlängen sind Schätzwerte** (l1=105, l2=98, l3=100, baseHeight=110 mm), + abgeleitet aus Reichweite (300 mm) / Höhe (420 mm). Vor Echtbetrieb am Arm + messen und per `ROBOT_KINEMATICS_PARAMS` setzen. +- ⚠️ **Gelenkmodell** (Pitch/Roll am Handgelenk) folgt der Standardkonfiguration + dieser Arm-Klasse; gegen das physische Gerät / die Kalibrieranleitung prüfen. --- diff --git a/doc/ToDo_9_HardwareFeedback.md b/doc/ToDo_9_HardwareFeedback.md index c5388a4..ae9cd32 100644 --- a/doc/ToDo_9_HardwareFeedback.md +++ b/doc/ToDo_9_HardwareFeedback.md @@ -55,11 +55,11 @@ Quellen: [Serial Protocol](http://wiki.fluidnc.com/en/support/serial_protocol), ## Designentscheidungen (festgeschrieben) -**B3 — Umkehr-Kinematik ist disambiguierbar.** Global nicht eindeutig, aber im Arbeitsraum -dieses Roboters per **dokumentierter physikalischer Zusatzbedingung** auflösbar -(z. B. „Ellbogen höher als Hand" bzw. „Ellbogen hinter der x-Achse"). `motorStateFromPorts()` -bekommt eine feste Zweig-Wahl-Regel; die exakte Vorzeichen-Konvention wird beim Herleiten -der Umkehrung gepinnt. +**B3 — Umkehr-Kinematik.** *Aktualisiert nach der Analyse (ToDo_9a):* Die **Port→Motor**-Rückrechnung, +die der Sync braucht, ist linear und **eindeutig** — keine Zweig-Wahl nötig. Die Ellbogen-oben/unten- +Mehrdeutigkeit betrifft nur die **kartesische** Inverskinematik `calculateAngles3D()` (Pose → +Gelenkwinkel), die der Sync nicht verwendet. Falls dort je eine Disambiguierung gebraucht wird, +gilt die physikalische Zusatzbedingung („Ellbogen höher als Hand" bzw. „hinter der x-Achse"). **B5 — Lockstep als abschaltbare Absicherung.** Durch die koordinierte Feedrate (ToDo_6a `correct`) treffen ohnehin alle Achsen *gleichzeitig* am nächsten Ziel ein — Lockstep ist @@ -125,26 +125,42 @@ erstmals einen Promise zurückgeben/awaiten können. --- -## Baustein für Paket 4 + 5: Rückabbildung Port → Motorwerte +## Baustein für Paket 4 + 5: Rückabbildung Port → Motorwerte — ✅ DURCHGERECHNET + +> **Erledigt als Analyse.** Vollständige Herleitung: **`doc/ToDo_9a_PortRueckrechnung.md`**. +> Verifikation: **`test/Robot.PortInverse.test.js`** (15 Tests grün). Beide folgenden Pakete brauchen denselben Baustein: aus den von GRBL gemeldeten `MPos`-Werten der drei Controller die **sieben Motorwerte des Roboters** rekonstruieren (`xMotor, alpha, beta, a, b, c, eMotor`). -Das ist die **Umkehrung von `portValue()`** (`robot/TelnetSenderGRBL.js`). `portValue()` -bildet *eine Roboter-Achse → einen GRBL-Port-Wert* ab, dabei koppeln einige Ports mehrere -Achsen (z. B. der z-Port der Hand mischt `c, b, z, y`). Die Rückrichtung muss diese -Kopplung **explizit auflösen** — sie ergibt sich nicht automatisch. +**Ergebnis:** Für die produktive Verkabelung (`startRobot.js`) ist die Abbildung +Motorwerte → gesendete GRBL-Achswerte **linear und eindeutig umkehrbar** — auf Port-Ebene +gibt es **keine** Mehrdeutigkeit. `factorTurnLift`/`handOpenInMM` kommen in der produktiven +Verkabelung gar nicht vor (nur in nicht-genutzten `portValue`-Zweigen). -- [ ] `motorStateFromPorts(portReadings)` definieren — algebraische Umkehrung von `portValue()` - - Eingang: pro Sender die gelesenen Port-Werte (`{x, y, z}` Base, `{a}` Elbow, `{c, e, b}` Hand) - - Ausgang: `{xMotor, alpha, beta, a, b, c, eMotor}` - - Grad→Rad zurückrechnen, `factorTurnLift`/`handOpenInMM` herausrechnen, gekoppelte Ports auflösen - - **Zweig-Wahl (B3):** wo die Lösung mehrdeutig ist, die dokumentierte physikalische - Zusatzbedingung anwenden (z. B. „Ellbogen höher als Hand"). Konvention im Code festhalten. -- [ ] **Round-Trip-Invariante** als Test: `portValue(motorStateFromPorts(p)) ≈ p` - - dasselbe Muster wie `test/Robot.Kinematics.RoundTrip.test.js` - - schützt die Umkehrfunktion gegen Drift gegenüber `portValue()` +```js +// D = 180/π ; r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} } +xMotor = r.base.x +alpha = r.base.y / D +beta = (r.base.z + r.base.y) / D +a = r.elbow.x / D +b = r.hand.z / D +c = (r.hand.x + r.hand.z) / D +eMotor = r.hand.y / D +``` + +> **B3 ist hier kein Thema.** Die Ellbogen-oben/unten-Mehrdeutigkeit steckt allein in der +> kartesischen Inverskinematik `calculateAngles3D()` (Pose → Gelenkwinkel). Der Sync nutzt +> diese Richtung nicht — er geht `MPos → Motorwerte → Vorwärtskinematik → Pose`, beide +> Schritte eindeutig. Der gesamte Sync-Pfad ist damit eindeutig. + +Offen für die spätere **Umsetzung** (Paket 4, nicht mehr Analyse): + +- [ ] `motorStateFromPorts()` aus der Analyse in den Produktiv-Code heben (Ort: Sender oder + Kinematik-Helfer) und im Sync verdrahten +- [ ] **Round-Trip-Invariante** als Dauer-Test mitführen: `portValue(motorStateFromPorts(p)) ≈ p` + — schützt gegen Drift, falls sich die Verkabelung in `startRobot.js` ändert > Hinweis: Gelesen wird auf dem **aktiven** Sender `TelnetSenderGRBL` (im `data`-Handler, > siehe Paket 1) — nicht auf `FluidNCClient.js`. @@ -167,7 +183,7 @@ sonst nicht, wo der Roboter physisch wirklich steht. im bisher synchronen Dispatch-Pfad. - [ ] Ablauf des Sync: 1. an alle drei Sender einmalig `?` senden, je `MPos` aus der Antwort parsen (Paket 3) - 2. `motorStateFromPorts(...)` → sieben Motorwerte rekonstruieren (Baustein oben, inkl. B3-Zweigwahl) + 2. `motorStateFromPorts(...)` → sieben Motorwerte rekonstruieren (Baustein oben — linear/eindeutig, ToDo_9a) 3. diese auf den Roboter schreiben: `robot.xMotor/alpha/beta/a/b/c/eMotor = …` 4. **Vorwärtskinematik** anstoßen: `robot.calculatePositionFromMotorAngles()` → füllt `robot.x/y/z` und `phi/theta/psi` aus den Hardwarewerten diff --git a/doc/ToDo_9a_PortRueckrechnung.md b/doc/ToDo_9a_PortRueckrechnung.md new file mode 100644 index 0000000..eea2533 --- /dev/null +++ b/doc/ToDo_9a_PortRueckrechnung.md @@ -0,0 +1,130 @@ +# ToDo 9a — Umkehr-Rechnung GRBL-Port → Motorwerte (Herleitung) + +> **Status: durchgerechnet und verifiziert.** +> Baustein für ToDo_9 / Paket 4 (Sync-Command). Verifikation: +> `test/Robot.PortInverse.test.js` (15 Tests grün). + +## Zweck + +Der Sync-Command (ToDo_9, Paket 4) liest die echten Achs-Positionen (`MPos`) der drei +GRBL/FluidNC-Controller und muss daraus die **sieben Motorwerte** des Roboters +rekonstruieren: `xMotor, alpha, beta, a, b, c, eMotor`. Diese werden dann auf den Roboter +geschrieben und per **Vorwärtskinematik** (`calculatePositionFromMotorAngles()`) in die +Pose `x/y/z/phi/theta/psi` überführt. + +Dieses Dokument leitet die Umkehrung her und hält das (verifizierte) Ergebnis fest. + +## Kernergebnis (vorweg) + +**Für die produktive Verkabelung ist die Abbildung Motorwerte → gesendete GRBL-Achswerte +linear und eindeutig umkehrbar.** Es gibt auf Port-Ebene **keine** Mehrdeutigkeit. + +> Die B3-Mehrdeutigkeit (Ellbogen oben/unten) steckt ausschließlich in der **kartesischen** +> Inverskinematik `calculateAngles3D()` (Pose → Gelenkwinkel). Der Sync nutzt diese Richtung +> **nicht** — er geht `MPos → Motorwerte → Vorwärtskinematik → Pose`, und beide Schritte sind +> eindeutige Funktionen. **Damit ist der Sync-Pfad als Ganzes eindeutig.** + +--- + +## Ausgangslage + +### Produktiv-Verkabelung (`startRobot.js`) + +```js +new TelnetSenderClass(baseIP, 2300, 'x', 'y', 'z') // Base +new TelnetSenderClass(elbowIP, 5000, 'a', null, null) // Elbow +new TelnetSenderClass(handIP, 5000, 'c', 'e', 'b') // Hand +``` + +### Bedeutung der Motor-Felder (`RobotMotorPosition`) + +| Feld | Bedeutung | +|---|---| +| `x` | `xMotor` (Schulterposition auf X-Schiene, **mm**) | +| `y` | `alpha` (Schulterwinkel, **rad**) | +| `z` | `beta` (Unterarm-Neigung, **rad**) | +| `a` | `a` (Ellbogen-Dreher, **rad**) | +| `b` | `b` (Handgelenk-Knick, **rad**) | +| `c` | `c` (Hand-Dreher, **rad**) | +| `e` | `eMotor` (Greifer) | + +Mit `D = 180/π` (Grad-Faktor). + +### Was jeder Controller real sendet (`execCommand`, Produktiv-Verkabelung) + +Aus dem Sende-Pfad (`robot/TelnetSenderGRBL.js`) ergeben sich genau **sieben** GRBL-Achswerte: + +| Controller | GRBL-Achse | gesendeter Wert | in Motorwerten | +|---|---|---|---| +| **Base** | `x` | `xMotor` | `xMotor` | +| **Base** | `y` | `alpha·D` | `α·D` | +| **Base** | `z` | `(beta − alpha)·D` | `(β − α)·D` | +| **Elbow** | `x` | `a·D` | `a·D` | +| **Hand** | `x` | `(c − b)·D` | `(c − b)·D` | +| **Hand** | `y` | `eMotor·D` | `e·D` | +| **Hand** | `z` | `b·D` | `b·D` | + +Drei Ports koppeln je zwei Motorwerte (`base.z`, `hand.x`), aber **jeder gekoppelte Wert +wird mit einem unabhängig gelesenen Wert kombiniert** (`alpha` bzw. `b`). Das System ist +damit *unteres Dreieckssystem* → trivial auflösbar. + +--- + +## Herleitung der Umkehrung + +Gegeben die GRBL-Readings `base.{x,y,z}`, `elbow.{x}`, `hand.{x,y,z}` (Grad bzw. mm): + +``` +xMotor = base.x // direkt +alpha = base.y / D +beta = (base.z + base.y) / D // base.z = (β−α)·D ⇒ β = base.z/D + α +a = elbow.x / D +b = hand.z / D +c = (hand.x + hand.z) / D // hand.x = (c−b)·D ⇒ c = hand.x/D + b +eMotor = hand.y / D +``` + +Als Funktion (siehe Test, kann später 1:1 in Paket 4 übernommen werden): + +```js +function motorStateFromPorts(r) { // r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} } + const D = 180 / Math.PI; + return { + xMotor: r.base.x, + alpha: r.base.y / D, + beta: (r.base.z + r.base.y) / D, + a: r.elbow.x / D, + b: r.hand.z / D, + c: (r.hand.x + r.hand.z) / D, + eMotor: r.hand.y / D, + }; +} +``` + +--- + +## Verifikation + +`test/Robot.PortInverse.test.js` — 15 Tests, fünf repräsentative Motorzustände +(Nullstellung, gemischt, negative/große Winkel, gekoppelt `c≈b`): + +- **A) Exaktheit gegen `portValue`** (volle Präzision): Rückgewinnung auf 1e-9 genau. +- **B) Gegen den echten Sende-Pfad `execCommand`** (inkl. 2-Dezimal-Rundung der + G-Code-Werte): Rückgewinnung innerhalb der Rundung (Winkel < 1e-3 rad, `xMotor` < 0.02 mm). +- **C) Voll-Kette Sync**: `Ports → motorStateFromPorts → calculatePositionFromMotorAngles` + liefert dieselbe Pose `x/y/z/phi/theta/psi` wie die Original-Motorwerte (auf 1e-6). + +--- + +## Konsequenzen für ToDo_9 / Paket 4 + +1. **Keine Zweig-Wahl auf Port-Ebene nötig.** Die frühere Annahme (B3-Disambiguierung im + Baustein) trifft auf die Port-Rückrechnung **nicht** zu — sie ist linear und eindeutig. +2. **Rundung beachten.** `MPos` kommt mit endlicher Präzision; gekoppelte Werte (`beta`, `c`) + summieren zwei gerundete Ports → Toleranz im Sub-Promille-Bereich, unkritisch. +3. **Achszahl je Controller.** FluidNC meldet `MPos` für **alle** in seiner Config definierten + Achsen. Genutzt werden nur: Base `x,y,z` · Elbow `x` · Hand `x,y,z`. Beim Parsen die + übrigen (falls vorhanden) ignorieren. +4. **Verkabelungs-Abhängigkeit.** `motorStateFromPorts()` gilt für die aktuelle Verkabelung. + Ändert sich `startRobot.js` (andere Port-Zuordnung), muss die Umkehrung mitgezogen werden — + der Round-Trip-Test (`portValue(motorStateFromPorts(p)) ≈ p`) schützt davor. diff --git a/robot/KinematicsFactory.js b/robot/KinematicsFactory.js index 734bad1..e9bbb55 100644 --- a/robot/KinematicsFactory.js +++ b/robot/KinematicsFactory.js @@ -13,9 +13,13 @@ const DEFAULT_KINEMATICS = 'arm3segmentlinearx'; // Registry: Bezeichner (lowercase) → Modulpfad relativ zu dieser Datei. -// Neue Kinematiken hier eintragen (siehe Phase 3). +// Neue Kinematiken hier eintragen (siehe Phase 3). Mehrere Bezeichner dürfen auf +// dasselbe Modul zeigen (Alias, z. B. Produktname). const KINEMATICS_REGISTRY = { arm3segmentlinearx: './kinematics/Arm3SegmentLinearX', + arm3segmentrotarybase: './kinematics/Arm3SegmentRotaryBase', + grabit: './kinematics/Arm3SegmentRotaryBase', // Alias: Joy-IT „Grab-It" (Robot02) + robot02: './kinematics/Arm3SegmentRotaryBase', // Alias: Joy-IT-Modellbezeichnung }; /** @@ -64,7 +68,11 @@ function createRobotFromEnv(env = process.env, defaultParams = {}) { const identifier = env.ROBOT_KINEMATICS || DEFAULT_KINEMATICS; const params = { ...defaultParams, ...parseParams(env.ROBOT_KINEMATICS_PARAMS) }; const KinematicsClass = loadKinematicsClass(identifier); - return new KinematicsClass(params.l1, params.l2, params.l3); + // l1/l2/l3 positional (Kompatibilität mit Arm3SegmentLinearX); das vollständige + // params-Objekt als 4. Argument für Kinematiken mit weiteren Parametern + // (z. B. baseHeight bei Arm3SegmentRotaryBase). Klassen, die es nicht brauchen, + // ignorieren das Argument. + return new KinematicsClass(params.l1, params.l2, params.l3, params); } module.exports = { diff --git a/robot/kinematics/Arm3SegmentRotaryBase.js b/robot/kinematics/Arm3SegmentRotaryBase.js new file mode 100644 index 0000000..181e83b --- /dev/null +++ b/robot/kinematics/Arm3SegmentRotaryBase.js @@ -0,0 +1,187 @@ +const RobotBase = require('../RobotBase.js') + +/** + * Arm3SegmentRotaryBase — Kinematik für den **Joy-IT „Grab-It" (Robot02)**. + * + * Strukturell wie {@link Arm3SegmentLinearX} (drei Arm-Segmente: Oberarm `l1`, + * Unterarm `l2`, Hand `l3`), aber statt der linearen X-Schiene sitzt der Arm auf + * einer **drehbaren Basis** (Yaw). Dazu kommt eine Handgelenk-Rotation. + * + * ─────────────────────────────────────────────────────────────────────────── + * Physische Spezifikation (recherchiert aus dem Joy-IT-Datenblatt / Handbuch): + * - 6 PWM-Servos (20 kg, Metallgetriebe), 360° mech. / 180° Arbeitsbereich + * - 5 Bewegungsachsen + Greifer + * - Reichweite ca. 300 mm ab Mitte Drehteller, Bauhöhe bis 420 mm + * + * Gelenkkette (Basis → Greifer): + * q1 Basis-Drehung (Yaw um die vertikale Z-Achse) → this.xMotor + * q2 Schulter (Pitch) → this.alpha + * q3 Ellbogen (Pitch, relativ zum Oberarm) → this.beta + * q4 Handgelenk (Pitch, relativ zum Unterarm) → this.a + * q5 Handgelenk-Roll (Drehung um die Hand-/Anflugachse)→ this.b + * – (ungenutzt) → this.c (= 0) + * q6 Greifer (Öffnungsweite) → this.eMotor + * + * Damit reichen die 7 Motor-Slots des Frameworks aus; ein Slot (`c`) bleibt frei. + * + * ─────────────────────────────────────────────────────────────────────────── + * 5-DOF-Einschränkung (wichtig!): + * Ein 5-Achs-Arm kann **nicht** jede beliebige 6-DOF-Pose (x,y,z,phi,theta,psi) + * erreichen. Da die Basis-Drehung den Azimut bestimmt, liegt der gesamte Arm in + * **einer** vertikalen Ebene durch die Basisachse. Folge: + * - `phi` (Hand-Azimut) ist **nicht frei** — er ist an die Position gekoppelt + * und wird in der Inversen aus `atan2(y, x)` abgeleitet. Ein als Eingabe + * gesetztes `this.phi` wird in {@link calculateAngles3D} **ignoriert** und in + * {@link calculatePositionFromMotorAngles} als Ergebnis überschrieben. + * - Frei wählbar sind: Position `x,y,z`, Hand-Neigung `theta` (Polarwinkel von + * +Z, gleiche Konvention wie Arm3SegmentLinearX) und Hand-Rotation `psi`. + * + * ─────────────────────────────────────────────────────────────────────────── + * Geometrie-Modell / Annahmen: + * - Schultergelenk sitzt auf der Basis-Drehachse, Höhe `baseHeight` über der + * Grundplatte (radialer Versatz = 0 — vereinfachende Annahme). + * - Winkel `alpha`, `beta`, `a` werden intern als Elevation gegen die + * Horizontale geführt; `alpha` ist absolut, `beta`/`a` relativ zum + * vorhergehenden Segment. + * - Es wird genau **ein** Ellbogen-Zweig gewählt (`beta` ∈ [0, π]). + * - Gelenk-Endanschläge werden **nicht** geprüft (wie bei Arm3SegmentLinearX). + * + * ⚠️ Die Default-Längen sind **Schätzwerte**, abgeleitet aus den + * veröffentlichten Maßen (Σ Segmente ≈ Reichweite 300 mm, `baseHeight` ≈ + * Höhe 420 mm − Reichweite). Für den Echtbetrieb am physischen Arm sollten sie + * **kalibriert** und per `ROBOT_KINEMATICS_PARAMS` gesetzt werden. + */ +class Arm3SegmentRotaryBase extends RobotBase { + + /** + * @param {number} [l1=105] Länge des Oberarms (Schulter→Ellbogen) in mm + * @param {number} [l2=98] Länge des Unterarms (Ellbogen→Handgelenk) in mm + * @param {number} [l3=100] Länge der Hand (Handgelenk→Greiferspitze) in mm + * @param {Object} [params] zusätzliche Parameter + * @param {number} [params.baseHeight=110] Höhe der Schulterachse über der Basis in mm + */ + constructor(l1 = 105, l2 = 98, l3 = 100, params = {}) { + super(); + + /** @type {number} Länge des Oberarms in mm */ + this.l1 = l1; + /** @type {number} Länge des Unterarms in mm */ + this.l2 = l2; + /** @type {number} Länge der Hand (Endeffector) in mm */ + this.l3 = l3; + /** @type {number} Höhe der Schulter-Drehachse über der Grundplatte in mm */ + this.baseHeight = (params && params.baseHeight != null) ? params.baseHeight : 110; + + // Neutrale Default-Pose: Hand horizontal nach vorn (theta = π/2 = Polarwinkel). + this.theta = Math.PI / 2; + } + + /** + * Inverse Kinematik: Workspace (x, y, z, theta, psi, e) → Gelenkwinkel. + * `phi` wird ignoriert und aus der Position abgeleitet (5-DOF, siehe Klassendoc). + */ + calculateAngles3D(verbose) { + // Winkel normalisieren (Konsistenz mit Arm3SegmentLinearX) + while (this.theta > Math.PI) { this.theta -= 2 * Math.PI } + while (this.theta < -Math.PI) { this.theta += 2 * Math.PI } + + // 1) Basis-Drehung aus dem Azimut der Zielposition (phi ist gekoppelt) + const q1 = Math.atan2(this.y, this.x); + this.xMotor = q1; + this.phi = q1; + + // 2) Hand-Vektor abziehen → Handgelenk-Punkt + // theta = Polarwinkel von +Z. Hand-Radial = l3·sin(theta), Hand-Z = l3·cos(theta). + const st = Math.sqrt(this.x * this.x + this.y * this.y); // radialer Abstand der Spitze + const zt = this.z; + + const sw = st - this.l3 * Math.sin(this.theta); // radial: Handgelenk + const zw = zt - this.l3 * Math.cos(this.theta); // Höhe: Handgelenk + + // Handgelenk-Punkt in 3D (für Speed-Berechnung) + this.pX = sw * Math.cos(q1); + this.pY = sw * Math.sin(q1); + this.pZ = zw; + + // 3) 2-Gelenk-IK in der Armebene: von Schulter (0, baseHeight) zum Handgelenk (sw, zw) + const dx = sw; + const dz = zw - this.baseHeight; + const R2 = dx * dx + dz * dz; + const R = Math.sqrt(R2); + + if (R > (this.l1 + this.l2)) { return; } // unerreichbar — Slots unverändert lassen + if (R === 0) { return; } + + // Ellbogen-Relativwinkel (ein Zweig: qe ∈ [0, π]) + let cosE = (R2 - this.l1 * this.l1 - this.l2 * this.l2) / (2 * this.l1 * this.l2); + cosE = Math.max(-1, Math.min(1, cosE)); + const qe = Math.atan2(Math.sqrt(1 - cosE * cosE), cosE); + + // Absoluter Oberarmwinkel (Elevation gegen Horizontale) + const A2 = Math.atan2(dz, dx) - Math.atan2(this.l2 * Math.sin(qe), this.l1 + this.l2 * Math.cos(qe)); + const A3 = A2 + qe; // absoluter Unterarmwinkel + const A4 = Math.PI / 2 - this.theta; // absolute Hand-Elevation aus Polarwinkel + + // 4) Motor-Slots schreiben + this.alpha = A2; // Schulter (absolut) + this.beta = qe; // Ellbogen (relativ) = A3 − A2 + this.a = A4 - A3; // Handgelenk-Pitch (relativ) + this.b = this.psi; // Handgelenk-Roll + this.c = 0; // ungenutzt + this.eMotor = this.e; // Greifer + + // a auf [-π, π] normalisieren + while (this.a > Math.PI) { this.a -= 2 * Math.PI } + while (this.a < -Math.PI) { this.a += 2 * Math.PI } + + if (verbose) { + console.log("IK GrabIt: q1=", q1.toFixed(3), "alpha=", this.alpha.toFixed(3), + "beta=", this.beta.toFixed(3), "a=", this.a.toFixed(3), "b=", this.b.toFixed(3)); + } + } + + /** + * Vorwärts-Kinematik: Gelenkwinkel → Workspace (x, y, z, phi, theta, psi, e). + * Exakte Umkehrung von {@link calculateAngles3D}. + */ + calculatePositionFromMotorAngles(verbose = false) { + const q1 = this.xMotor; // Basis-Yaw + const A2 = this.alpha; // Oberarm absolut + const A3 = this.alpha + this.beta; // Unterarm absolut + const A4 = this.alpha + this.beta + this.a; // Hand absolut (Elevation) + + // Handgelenk-Punkt in der Armebene + const sw = this.l1 * Math.cos(A2) + this.l2 * Math.cos(A3); + const zw = this.baseHeight + this.l1 * Math.sin(A2) + this.l2 * Math.sin(A3); + + // Greiferspitze in der Armebene (Hand-Elevation A4) + const st = sw + this.l3 * Math.cos(A4); + const zt = zw + this.l3 * Math.sin(A4); + + // In den 3D-Raum drehen + this.pX = sw * Math.cos(q1); + this.pY = sw * Math.sin(q1); + this.pZ = zw; + + this.x = st * Math.cos(q1); + this.y = st * Math.sin(q1); + this.z = zt; + + this.phi = q1; // Azimut = Basis-Drehung + this.theta = Math.PI / 2 - A4; // Polarwinkel von +Z + this.psi = this.b; // Hand-Roll + + while (this.phi > Math.PI) { this.phi -= 2 * Math.PI } + while (this.phi < -Math.PI) { this.phi += 2 * Math.PI } + while (this.theta > Math.PI) { this.theta -= 2 * Math.PI } + while (this.theta < -Math.PI) { this.theta += 2 * Math.PI } + + if (verbose) { + console.log("FK GrabIt: x=", this.x.toFixed(2), "y=", this.y.toFixed(2), + "z=", this.z.toFixed(2), "phi=", this.phi.toFixed(3), "theta=", this.theta.toFixed(3)); + } + } +} + + +module.exports = Arm3SegmentRotaryBase diff --git a/test/Robot.GrabIt.RoundTrip.test.js b/test/Robot.GrabIt.RoundTrip.test.js new file mode 100644 index 0000000..5fed1f1 --- /dev/null +++ b/test/Robot.GrabIt.RoundTrip.test.js @@ -0,0 +1,140 @@ +// Kinematik-RoundTrip-Tests für den Joy-IT „Grab-It" (Robot02), +// implementiert als Arm3SegmentRotaryBase (Roadmap 12, Phase 3). +const Arm3SegmentRotaryBase = require('../robot/kinematics/Arm3SegmentRotaryBase'); +const { createRobotFromEnv } = require('../robot/KinematicsFactory'); +const RobotBase = require('../robot/RobotBase'); + +// Default-Geometrie (Schätzwerte): l1=105, l2=98, l3=100, baseHeight=110 +const mk = () => new Arm3SegmentRotaryBase(); + +describe('Arm3SegmentRotaryBase — Workspace → Motor → Workspace', () => { + + // Erreichbare Workspace-Punkte mit konsistentem (impliziten) Azimut. + const cases = [ + { x: 200, y: 0, z: 110, theta: Math.PI / 2, psi: 0.0, e: 10 }, + { x: 120, y: 120, z: 200, theta: Math.PI / 3, psi: -0.5, e: 20 }, + { x: -90, y: 90, z: 150, theta: Math.PI / 2, psi: 0.4, e: 5 }, + { x: 80, y: -60, z: 90, theta: 2 * Math.PI / 3, psi: 0.2, e: 0 }, + ]; + + test.each(cases)('roundtrip x=$x y=$y z=$z', (c) => { + // A: Inverse (Workspace → Motorwinkel) + const A = mk(); + A.x = c.x; A.y = c.y; A.z = c.z; + A.theta = c.theta; A.psi = c.psi; A.e = c.e; + A.calculateAngles3D(); + + // B: Vorwärts (Motorwinkel → Workspace) + const B = mk(); + B.xMotor = A.xMotor; + B.alpha = A.alpha; + B.beta = A.beta; + B.a = A.a; + B.b = A.b; + B.eMotor = A.eMotor; + B.calculatePositionFromMotorAngles(); + + // Die steuerbaren DOF müssen exakt zurückkommen. + expect(B.x).toBeCloseTo(c.x, 4); + expect(B.y).toBeCloseTo(c.y, 4); + expect(B.z).toBeCloseTo(c.z, 4); + expect(B.theta).toBeCloseTo(c.theta, 5); + expect(B.psi).toBeCloseTo(c.psi, 5); + + // phi ist gekoppelt: gleich dem Azimut der Position (5-DOF-Constraint). + expect(B.phi).toBeCloseTo(Math.atan2(c.y, c.x), 5); + }); +}); + +describe('Arm3SegmentRotaryBase — Motor → Workspace → Motor', () => { + + // Motorwinkel mit Ellbogen im gewählten Zweig (beta ∈ [0, π]). + const motors = [ + { xMotor: 0.5, alpha: 0.8, beta: 0.6, a: -0.3, b: 0.2, eMotor: 15 }, + { xMotor: -0.7, alpha: 0.4, beta: 1.0, a: 0.5, b: -0.4, eMotor: 30 }, + { xMotor: 1.2, alpha: 1.1, beta: 0.3, a: -0.6, b: 0.1, eMotor: 0 }, + ]; + + test.each(motors)('roundtrip alpha=$alpha beta=$beta', (m) => { + // A: Vorwärts (Motor → Workspace) + const A = mk(); + Object.assign(A, m); + A.calculatePositionFromMotorAngles(); + + // B: Inverse (Workspace → Motor) + const B = mk(); + B.x = A.x; B.y = A.y; B.z = A.z; + B.theta = A.theta; B.psi = A.psi; B.e = A.eMotor; + B.calculateAngles3D(); + + expect(B.xMotor).toBeCloseTo(m.xMotor, 5); + expect(B.alpha).toBeCloseTo(m.alpha, 5); + expect(B.beta).toBeCloseTo(m.beta, 5); + expect(B.a).toBeCloseTo(m.a, 5); + expect(B.b).toBeCloseTo(m.b, 5); + expect(B.eMotor).toBeCloseTo(m.eMotor, 5); + }); +}); + +describe('Arm3SegmentRotaryBase — 5-DOF-Eigenschaften', () => { + + test('phi-Eingabe wird in der Inversen ignoriert', () => { + const base = { x: 150, y: 60, z: 130, theta: Math.PI / 2, psi: 0.1, e: 0 }; + + const withPhi = mk(); + Object.assign(withPhi, base, { phi: 2.5 }); // bewusst widersprüchliches phi + withPhi.calculateAngles3D(); + + const noPhi = mk(); + Object.assign(noPhi, base, { phi: -1.0 }); // anderes phi + noPhi.calculateAngles3D(); + + // Trotz unterschiedlicher phi-Eingabe identische Motorwinkel. + for (const k of ['xMotor', 'alpha', 'beta', 'a', 'b', 'eMotor']) { + expect(noPhi[k]).toBeCloseTo(withPhi[k], 9); + } + // phi wird aus der Position abgeleitet. + expect(withPhi.xMotor).toBeCloseTo(Math.atan2(base.y, base.x), 9); + }); + + test('c-Slot bleibt ungenutzt (= 0)', () => { + const r = mk(); + r.x = 180; r.y = 0; r.z = 120; r.theta = Math.PI / 2; r.psi = 0; r.e = 0; + r.calculateAngles3D(); + expect(r.c).toBe(0); + }); + + test('unerreichbarer Punkt lässt die Motor-Slots unverändert', () => { + const r = mk(); + r.alpha = 0; r.beta = 0; r.a = 0; // bekannter Startzustand + r.x = 5000; r.y = 0; r.z = 0; r.theta = Math.PI / 2; r.psi = 0; r.e = 0; + r.calculateAngles3D(); + // R > l1+l2 → frühzeitiges return, alpha/beta/a unverändert + expect(r.alpha).toBe(0); + expect(r.beta).toBe(0); + expect(r.a).toBe(0); + }); +}); + +describe('KinematicsFactory lädt den Grab-It', () => { + + test('Alias "grabit" liefert eine Arm3SegmentRotaryBase-Instanz', () => { + const robot = createRobotFromEnv({ ROBOT_KINEMATICS: 'grabit' }); + expect(robot).toBeInstanceOf(Arm3SegmentRotaryBase); + expect(robot).toBeInstanceOf(RobotBase); + }); + + test('ROBOT_KINEMATICS_PARAMS setzt Längen und baseHeight', () => { + const robot = createRobotFromEnv({ + ROBOT_KINEMATICS: 'robot02', + ROBOT_KINEMATICS_PARAMS: '{"l1":110,"l2":100,"l3":95,"baseHeight":130}', + }); + expect([robot.l1, robot.l2, robot.l3]).toEqual([110, 100, 95]); + expect(robot.baseHeight).toBe(130); + }); + + test('Default-baseHeight wenn nicht gesetzt', () => { + const robot = createRobotFromEnv({ ROBOT_KINEMATICS: 'grabit' }); + expect(robot.baseHeight).toBe(110); + }); +}); diff --git a/test/Robot.PortInverse.test.js b/test/Robot.PortInverse.test.js new file mode 100644 index 0000000..fc9e7a8 --- /dev/null +++ b/test/Robot.PortInverse.test.js @@ -0,0 +1,150 @@ +/** + * Verifikation der Port→Motor-Rückrechnung für ToDo_9 / Paket 4 (Sync-Command). + * + * Herleitung: doc/ToDo_9a_PortRueckrechnung.md + * + * Kernaussage: Für die PRODUKTIV-Verkabelung (startRobot.js) ist die Abbildung + * der sieben Motorwerte auf die sieben gesendeten GRBL-Achswerte LINEAR und + * EINDEUTIG umkehrbar. `motorStateFromPorts()` ist diese Umkehrung; die Tests + * beweisen sie gegen den echten Sende-Pfad (`execCommand`) und gegen `portValue`. + * + * Damit ist klar: die B3-Mehrdeutigkeit (Ellbogen oben/unten) betrifft nur die + * kartesische Inverskinematik (`calculateAngles3D`), die der Sync gar nicht nutzt. + */ +const TelnetSender = require('../robot/TelnetSenderGRBL'); +const MotorPosition = require('../robot/RobotMotorPosition'); +const Robot = require('../robot/kinematics/Arm3SegmentLinearX'); + +const D = 180 / Math.PI; + +/* ---------- Produktiv-Verkabelung (1:1 wie startRobot.js) ---------- */ +function buildSenders() { + const mk = (...args) => new TelnetSender('test.test', ...args); // test.test → Fake-tSocket + return { + base: mk(2300, 'x', 'y', 'z'), // GRBL x←xMotor, y←alpha, z←(beta-alpha) + elbow: mk(5000, 'a', null, null), // GRBL x←a + hand: mk(5000, 'c', 'e', 'b'), // GRBL x←(c-b), y←eMotor, z←b + }; +} + +/* ---------- Die Umkehrung: GRBL-Readings → 7 Motorwerte ---------- */ +// r = { base:{x,y,z}, elbow:{x}, hand:{x,y,z} } (GRBL-Achswerte, Grad bzw. mm) +function motorStateFromPorts(r) { + const xMotor = r.base.x; // x-Port = xMotor (mm, direkt) + const alpha = r.base.y / D; // y-Port = alpha·D + const beta = (r.base.z + r.base.y) / D; // z-Port = (beta-alpha)·D ⇒ beta = z/D + alpha + const a = r.elbow.x / D; // Elbow x-Port = a·D + const b = r.hand.z / D; // Hand z-Port = b·D + const c = (r.hand.x + r.hand.z) / D; // Hand x-Port = (c-b)·D ⇒ c = x/D + b + const eMotor = r.hand.y / D; // Hand y-Port = eMotor·D + return { xMotor, alpha, beta, a, b, c, eMotor }; +} + +/* ---------- Hilfen ---------- */ +// {x,y,z,a,b,c,e}-pos aus Motorwerten (Feld-Mapping der RobotMotorPosition) +function posFromMotors(m) { + return { x: m.xMotor, y: m.alpha, z: m.beta, a: m.a, b: m.b, c: m.c, e: m.eMotor }; +} + +// Port-Readings über die reine Funktion portValue (volle Präzision) +function portsViaPortValue(S, pos) { + return { + base: { x: S.base.portValue('x', 'x', pos), y: S.base.portValue('y', 'y', pos), z: S.base.portValue('z', 'z', pos) }, + elbow: { x: S.elbow.portValue('x', 'a', pos) }, + hand: { x: S.hand.portValue('x', 'c', pos), y: S.hand.portValue('y', 'e', pos), z: S.hand.portValue('z', 'b', pos) }, + }; +} + +// Achswerte aus einer real gesendeten G-Code-Zeile (z. B. "G1 x12.34 y90.00 z-90.00") +function parseAxes(line) { + const out = {}; + const re = /([xyz])(-?\d+(?:\.\d+)?)/g; + let m; + while ((m = re.exec(line)) !== null) out[m[1]] = parseFloat(m[2]); + return out; +} + +// Port-Readings über den ECHTEN Sende-Pfad execCommand (inkl. 2-Dezimal-Rundung) +function portsViaExecCommand(S, m) { + const mNew = new MotorPosition(m.xMotor, m.alpha, m.beta, m.a, m.b, m.c, m.eMotor); + for (const f of ['xMotorChanged','yMotorChanged','zMotorChanged','aMotorChanged','bMotorChanged','cMotorChanged','eMotorChanged']) mNew[f] = true; + const mOld = new MotorPosition(0, 0, 0, 0, 0, 0, 0); + + S.base.execCommand('G1', mOld, mNew); + S.elbow.execCommand('G1', mOld, mNew); + S.hand.execCommand('G1', mOld, mNew); + + return { + base: parseAxes(S.base.tSocket.written), + elbow: parseAxes(S.elbow.tSocket.written), + hand: parseAxes(S.hand.tSocket.written), + }; +} + +/* ---------- Repräsentative Motorzustände (plausible Bereiche) ---------- */ +const CASES = [ + { name: 'Nullstellung', xMotor: 0, alpha: 0, beta: 0, a: 0, b: 0, c: 0, eMotor: 0 }, + { name: 'gemischt klein', xMotor: 100, alpha: 0.30, beta: 0.50, a: 0.20, b: 0.40, c: 0.90, eMotor: 1.0 }, + { name: 'negative Winkel', xMotor: -55, alpha:-0.70, beta:-0.25, a:-1.10, b:-0.60, c:-0.30, eMotor:-0.5 }, + { name: 'große Winkel', xMotor: 250, alpha: 1.20, beta: 2.00, a: 2.90, b: 1.50, c: 3.00, eMotor: 2.5 }, + { name: 'gekoppelt c≈b', xMotor: 42, alpha: 0.10, beta: 0.10, a: 0.05, b: 1.20, c: 1.25, eMotor: 0.0 }, +]; + +beforeAll(() => { jest.spyOn(console, 'log').mockImplementation(() => {}); }); +afterAll(() => { jest.restoreAllMocks(); }); + +describe('Port→Motor-Rückrechnung (ToDo_9, Baustein für Paket 4)', () => { + + describe('A) exakte Umkehrung gegen portValue (volle Präzision)', () => { + for (const c of CASES) { + test(c.name, () => { + const S = buildSenders(); + const ports = portsViaPortValue(S, posFromMotors(c)); + const rec = motorStateFromPorts(ports); + for (const k of ['xMotor','alpha','beta','a','b','c','eMotor']) { + expect(rec[k]).toBeCloseTo(c[k], 9); + } + }); + } + }); + + describe('B) Umkehrung gegen den echten Sende-Pfad execCommand (mit Rundung)', () => { + for (const c of CASES) { + test(c.name, () => { + const S = buildSenders(); + const ports = portsViaExecCommand(S, c); + const rec = motorStateFromPorts(ports); + // xMotor in mm (Rundung 0.01) → 0.02 Toleranz; Winkel in rad (Rundung 0.01° je Port) → 1e-3 + expect(rec.xMotor).toBeCloseTo(c.xMotor, 2); + for (const k of ['alpha','beta','a','b','c','eMotor']) { + expect(Math.abs(rec[k] - c[k])).toBeLessThan(1e-3); + } + }); + } + }); + + describe('C) Voll-Kette Sync: Ports → Motoren → Vorwärtskinematik → Pose', () => { + // Beweist, dass die rekonstruierten Motorwerte über die Vorwärtskinematik + // exakt dieselbe Pose liefern wie die Originalwerte (= was Sync dem Client meldet). + for (const c of CASES) { + test(c.name, () => { + const S = buildSenders(); + + // Referenz-Pose direkt aus den Original-Motorwerten + const ref = new Robot(250, 264, 100); + Object.assign(ref, { xMotor: c.xMotor, alpha: c.alpha, beta: c.beta, a: c.a, b: c.b, c: c.c, eMotor: c.eMotor }); + ref.calculatePositionFromMotorAngles(); + + // Sync-Pfad: Ports → Inverse → Motoren setzen → Vorwärtskinematik + const rec = motorStateFromPorts(portsViaPortValue(S, posFromMotors(c))); + const sync = new Robot(250, 264, 100); + Object.assign(sync, { xMotor: rec.xMotor, alpha: rec.alpha, beta: rec.beta, a: rec.a, b: rec.b, c: rec.c, eMotor: rec.eMotor }); + sync.calculatePositionFromMotorAngles(); + + for (const k of ['x','y','z','phi','theta','psi']) { + expect(sync[k]).toBeCloseTo(ref[k], 6); + } + }); + } + }); +});