From d906b094b180dc213407bdc778ef033f52a0a18c Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Wed, 10 Jun 2026 23:18:29 +0200 Subject: [PATCH] Umbau 12: Robot-Kinematics als extends RobotBase --- doc/ToDo_12_InverseKinematikConfig_ROADMAP.md | 28 +- doc/ToDo_9_HardwareFeedback.md | 189 +++++++++ docker-compose.yaml | 4 + logs/gcode_commands.log | 10 + logs/pings.log | 4 + robot/KinematicsFactory.js | 76 ++++ robot/Robot.js | 378 +----------------- robot/RobotBase.js | 319 +++++++++++++++ robot/kinematics/Arm3SegmentLinearX.js | 133 ++++++ startRobot.js | 12 +- test/KinematicsFactory.test.js | 69 ++++ 11 files changed, 837 insertions(+), 385 deletions(-) create mode 100644 robot/KinematicsFactory.js create mode 100644 robot/RobotBase.js create mode 100644 robot/kinematics/Arm3SegmentLinearX.js create mode 100644 test/KinematicsFactory.test.js diff --git a/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md b/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md index c72f393..f6024d9 100644 --- a/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md +++ b/doc/ToDo_12_InverseKinematikConfig_ROADMAP.md @@ -85,10 +85,10 @@ calculateAngles3D() // Workspace → Motorwinkel (schreibt auf t calculatePositionFromMotorAngles() // Motorwinkel → Workspace (schreibt auf this.*) ``` -- [ ] `robot/RobotBase.js` anlegen — generische Infrastruktur aus `Robot.js` -- [ ] Beide Kinematik-Methoden in `RobotBase` als Stub mit `throw new Error('not implemented')` -- [ ] JSDoc: Interface-Vertrag dokumentieren -- [ ] `rotateAroundAxis()` wandert in `RobotBase` als geschützte Hilfsmethode +- [x] `robot/RobotBase.js` anlegen — generische Infrastruktur aus `Robot.js` +- [x] Beide Kinematik-Methoden in `RobotBase` als Stub mit `throw new Error('not implemented')` +- [x] JSDoc: Interface-Vertrag dokumentieren +- [x] `rotateAroundAxis()` wandert in `RobotBase` als geschützte Hilfsmethode --- @@ -101,16 +101,16 @@ robot/ └── Arm3SegmentLinearX.js ← bisheriger Robot.js-Kinematik-Teil ``` -- [ ] `robot/kinematics/Arm3SegmentLinearX.js` anlegen +- [x] `robot/kinematics/Arm3SegmentLinearX.js` anlegen - `class Arm3SegmentLinearX extends RobotBase` - Konstruktor: `constructor(l1, l2, l3)` → `super()` + Längen - `calculateAngles3D()` — unverändert übernommen - `calculatePositionFromMotorAngles()` — unverändert übernommen -- [ ] `robot/Robot.js` wird zum Kompatibilitäts-Alias für die Übergangsperiode: +- [x] `robot/Robot.js` wird zum Kompatibilitäts-Alias für die Übergangsperiode: ```js module.exports = require('./kinematics/Arm3SegmentLinearX'); ``` -- [ ] Alle bestehenden Tests müssen grün bleiben — kein Verhalten ändert sich +- [x] Alle bestehenden Tests müssen grün bleiben — kein Verhalten ändert sich (`Robot.Kinematics.RoundTrip.test.js` ist das primäre Sicherheitsnetz) --- @@ -124,15 +124,17 @@ environment: ROBOT_KINEMATICS_PARAMS: '{"l1": 250, "l2": 264, "l3": 100}' ``` -- [ ] `ROBOT_KINEMATICS` — Bezeichner der Kinematik-Klasse (Default: `arm3segmentlinearx`) -- [ ] `ROBOT_KINEMATICS_PARAMS` — JSON mit Konstruktor-Parametern -- [ ] `KinematicsFactory.js` oder direkt in `startRobot.js`: +- [x] `ROBOT_KINEMATICS` — Bezeichner der Kinematik-Klasse (Default: `arm3segmentlinearx`) +- [x] `ROBOT_KINEMATICS_PARAMS` — JSON mit Konstruktor-Parametern +- [x] `KinematicsFactory.js` (`createRobotFromEnv`), eingebunden in `startRobot.js`: ```js - const kin = loadKinematics(process.env.ROBOT_KINEMATICS, params); - const robot = new kin(params.l1, params.l2, params.l3); + const robot = createRobotFromEnv(processEnv, { l1: 250, l2: 264, l3: 100 }); ``` -- [ ] Unbekannte Kinematik → klare Fehlermeldung beim Start, kein silent fail +- [x] Unbekannte Kinematik → klare Fehlermeldung beim Start, kein silent fail - [ ] Neue Variablen ins zentrale Config-Modul aufnehmen (koordinieren mit `ToDo_3_Config`) + → **offen:** `ToDo_3_Config` ist noch nicht umgesetzt. Die Factory liest `process.env` + vorerst direkt (gleicher Stil wie `ROBOT_DEFAULT_FEEDRATE`); das zentrale Config-Modul + kann die beiden Variablen später übernehmen. In `docker-compose.yaml` bereits dokumentiert. --- diff --git a/doc/ToDo_9_HardwareFeedback.md b/doc/ToDo_9_HardwareFeedback.md index badd9ad..a6f142c 100644 --- a/doc/ToDo_9_HardwareFeedback.md +++ b/doc/ToDo_9_HardwareFeedback.md @@ -43,6 +43,195 @@ WebSocket → GCode → calculateAngles3D → sendCommand → tSocket.write() - [ ] Status (`Idle`, `Run`, `Alarm`, `Hold`) für den `InfoServer` bereitstellen - `/api/status` um GRBL-Zustand erweitern +--- + +## Baustein für Paket 4 + 5: Rückabbildung Port → Motorwerte + +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. + +- [ ] `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 +- [ ] **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()` + +> Hinweis: Gelesen wird auf dem **aktiven** Sender `TelnetSenderGRBL` (im `data`-Handler, +> siehe Paket 1) — nicht auf `FluidNCClient.js`. + +--- + +## Paket 4: Hardware-Position auslesen und übernehmen (Sync-Command) + +**Ziel:** Ein Befehl liest die echten Motor-Koordinaten aller drei Controller aus, +übernimmt sie als neuen Soll-Zustand und passt die berechnete Roboter-Pose entsprechend an. +Nötig nach Homing, manuellem Jog, Endschalter-Auslösung oder Reconnect — die Software weiß +sonst nicht, wo der Roboter physisch wirklich steht. + +- [ ] Neuer Eingabe-Befehl, z. B. `M114 R` (Read-Hardware) oder WS-Message `syncFromHardware` + - klar abgegrenzt vom bestehenden `M114`, das nur die **Software**-Position zurückgibt + (`GCode.getM114(robot)` in `server/InputWS.js`) +- [ ] Ablauf des Sync: + 1. an alle drei Sender `?` senden, je `MPos` aus der Antwort parsen (Paket 3) + 2. `motorStateFromPorts(...)` → sieben Motorwerte rekonstruieren (Baustein oben) + 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 + 5. `motorPosition`/`motorPositionOld` zurücksetzen, damit der nächste Move sauber von + der echten Position aus rechnet (sonst falscher Speed-Delta im Korrekt-Modus) +- [ ] dem anfragenden Client die übernommene Pose zurückmelden (`reply(ws, …)`) +- [ ] **kein** automatisches Nachfahren — Sync ändert nur den Soll-Zustand, sendet keinen Move + +> Warum nicht einfach „letzten gesendeten Wert" merken? Weil die Hardware nach Homing/Jog/ +> Stall von dem abweicht, was zuletzt gesendet wurde — genau diese Differenz soll Sync auflösen. + +--- + +## Paket 5: Bewegungs-Fortschritt ermitteln (Move-Progress) + +**Ziel:** Herausfinden, wie weit FluidNC einen laufenden Move bereits abgearbeitet hat — +also wie weit sich jeder Controller schon zur Ziel-Position bewegt hat (0…100 %). + +- [ ] Beim Absenden eines Moves Start und Ziel je Controller festhalten + - `mStart` = Port-Werte vor dem Move, `mTarget` = gesendete Port-Werte + - liegt bereits vor: `robot.motorPositionOld` (Start) und `robot.motorPosition` (Ziel), + über `portValue()` in Port-Werte umgerechnet +- [ ] Während der Bewegung periodisch `?` pollen (Paket 3) und je Controller berechnen: + ``` + fortschritt_i = |MPos_jetzt − Start_i| / |Ziel_i − Start_i| (auf 0…1 geklemmt) + ``` +- [ ] Aggregat-Fortschritt + Fertig-Erkennung: + - **Fertig**, wenn alle drei Controller `state == Idle` UND `MPos ≈ Ziel` + - der Gesamt-Fortschritt eines Schritts = **Minimum** über die Controller + (der langsamste bestimmt, wann der Schritt fertig ist) + - im **Korrekt-Modus** (ToDo_6a) sollten alle Controller etwa gleich schnell fertig sein — + eine große Spreizung der Einzel-Fortschritte ist dort ein Warnsignal (Feedrate-Fehler) +- [ ] Fortschritt + Status nach außen geben + - über `InfoServer` (`/api/status`) und/oder als WS-Push an die Clients + - ermöglicht eine Fortschrittsanzeige beim Abspielen von Dateien (ToDo_6b) +- [ ] Zusammenspiel mit der Command-Queue (Paket 2): erst den nächsten Move senden, wenn + der vorige `Idle` erreicht hat → verhindert, dass Fortschritt mehrerer Moves verschwimmt + +### Offener Kernpunkt: Was, wenn währenddessen der nächste Befehl kommt? + +Heute ist der Treiber **fire-and-forget**: ein neuer Befehl wird sofort an alle drei GRBLs +geschickt und landet in deren Planner-Puffer. Das hat zwei Folgen, die Paket 5 für sich +genommen nicht löst: + +1. **Fortschritt wird mehrdeutig.** `motorPositionOld`/`motorPosition` halten nur die + *letzten zwei* Stellungen. Sind mehrere Moves gleichzeitig im GRBL-Puffer, weiß der + Treiber nicht mehr, *welcher* gerade fährt — der `?`-Fortschritt bezieht sich dann auf + das falsche Start/Ziel-Paar. +2. **Stille Befehlsverwerfung.** Kommen Befehle dauerhaft schneller als sie ausgeführt + werden, läuft GRBLs 128-Byte-RX-Puffer über → Befehle gehen verloren (vgl. Paket 2). + +Die saubere Lösung ist eine **zeitgesteuerte Sende-Queue** — bewusst als größerer, +**abschaltbarer** Umbau in Paket 6. + +--- + +## Paket 6: Zeitgesteuerte Sende-Queue (`ROBOT_USE_QUEUE`) + +**Ziel:** Befehle nicht mehr blind sofort raushauen, sondern in einer Queue puffern und +**zeitlich getaktet** absenden — jeden Befehl gerade rechtzeitig, bevor der vorige fertig +ist. So bleibt die Bewegung flüssig (GRBL-Planner läuft nie leer), nichts geht verloren +(kein Puffer-Überlauf), und das Netz wird nicht mit Dauer-Polling oder Befehls-Salven +belastet. + +### Schalter (Pflicht — Absicherung wie bei ToDo_6a) + +| Env | Default | Wirkung | +|---|---|---| +| `ROBOT_USE_QUEUE` | `false` | **Aus = exakt heutiges Fire-and-forget.** Jeder Befehl geht sofort an alle Sender, kein Pacing, keine Queue. Byte-identisch zu vorher. | +| `ROBOT_USE_QUEUE` | `true` | Neue zeitgesteuerte Queue-Logik aktiv. | + +```yaml +# docker-compose.yaml → appRobotDriver +environment: + - ROBOT_USE_QUEUE=false # oder: true +``` + +> Wie `ROBOT_SPEED_MODE` greift der Umbau **nur** bei `true`. Solange `false`, ist der +> Sende-Pfad unverändert — das bestehende Sicherheitsnetz (Sender-Tests) bleibt gültig. + +### Idee: zwei Uhren — eine geschätzte (gratis), eine gemessene (kostet Netz) + +Der Trick, das Netz **nicht** zu überlasten: primär nach einer **geschätzten** Uhr takten, +die `?`-Messung nur sparsam zur Korrektur einsetzen. + +- **Geschätzte Uhr (Haupttakt, kein Netzverkehr):** Jeder Queue-Eintrag trägt seine + **voraussichtliche Ausführzeit** — die liegt mit `moveTime` aus ToDo_6a bereits vor. + Der Treiber führt je Controller einen lokalen Zeitstempel `controllerFreiAb`: + ``` + beim Senden: controllerFreiAb += moveTime_dieses_Befehls + nächster Send, wenn: jetzt >= controllerFreiAb − vorlauf + ``` + `vorlauf` = kleine Sicherheitsmarge, damit immer ~1 Move im GRBL-Planner wartet und die + Bewegung nicht stockt. Das ist ein reiner Timer → **null Zusatz-Traffic**. + +- **Gemessene Uhr (Korrektur, sparsam):** Die Schätzung driftet (Beschleunigung/Abbremsen + stecken nicht in `dist/feedrate`, kurze Moves dauern real länger). Deshalb ab und zu — + **nicht** bei jedem Move — per `?` (Paket 3) den echten Stand holen und `controllerFreiAb` + nachjustieren. Auslöser: Queue läuft fast leer, ein langer Move (einmal mittendrin prüfen), + oder ein fester Maximaltakt. **`?` strikt raten-begrenzen** (z. B. ≤ 5 Hz, GRBL-üblich) → + Netzlast bleibt gedeckelt. + +### Eintrag in der Queue + +- [ ] Queue-Eintrag hält: geparster Befehl / Motorziel, `moveTime` (geschätzt), die je + Sender resultierenden G-Code-Strings, Status (`pending → sent → done`) +- [ ] `done` wird gesetzt durch geschätzte Uhr **oder** (falls gepollt) durch `?`=`Idle` am Ziel + +### Pacing-Schleife + +- [ ] je Controller `controllerFreiAb` führen, Sendezeitpunkt aus `moveTime − vorlauf` ableiten +- [ ] Tiefe im GRBL-Planner begrenzen (z. B. ≤ 1–2 vorausgesendete Moves) — flüssig, aber + noch steuerbar +- [ ] Drift-Korrektur per ratenbegrenztem `?`; bei großer Abweichung Schätzung neu setzen +- [ ] **Optionaler Sicherheitsboden:** zusätzlich Character-Counting (Summe ungequittierter + Zeilenlängen < 128 B) als Netz gegen Puffer-Überlauf, falls die Schätzung mal stark danebenliegt + +### Verhalten bei „neuer Befehl kommt mitten in der Bewegung" + +Zwei Betriebsarten — je nach Quelle der Befehle: + +- [ ] **Anhängen (Datei abspielen / ToDo_6b):** Reihenfolge erhalten, nach Schätzung takten, + In-Flight-Tiefe begrenzen. Kein Befehl wird verworfen. +- [ ] **Neuester gewinnt (interaktives Streaming, z. B. 3D-Input / Bodytracker):** Trifft ein + neues Ziel ein, während noch ungesendete (`pending`) Einträge in der Queue liegen, den + veralteten Schwanz **ersetzen** statt anzuhängen (Coalescing) → niedrige Latenz, kein + Auflaufen veralteter Zwischenziele. (Kann als Unter-Schalter / spätere Ausbaustufe kommen.) +- [ ] **Backpressure:** maximale Queue-Länge festlegen. Bei Dauer-Überlauf entweder + Ältestes-verwerfen (Streaming) oder Druck an den WS-Client zurückgeben (Datei) — nie + unbegrenzt wachsen lassen. + +### Fortschritt mit Pipelining (behebt die Mehrdeutigkeit aus Paket 5) + +- [ ] Den „aktuell fahrenden" Eintrag in der Queue markieren (Kopf vorrücken, wenn geschätzte + Uhr abgelaufen **oder** `?`=`Idle`). Erst dieser Kopf liefert das richtige Start/Ziel-Paar + für die `?`-Fortschrittsrechnung aus Paket 5. + +### Kanten / Sonderfälle + +- [ ] **Alarm/Error** mitten in der Queue (Paket 1) → Queue leeren, Senden stoppen, Fehler melden +- [ ] **Sync-Command (Paket 4)** bei nicht-leerer Queue → erst Queue leeren; die gepufferten + Ziele sind nach einem Re-Homing veraltet +- [ ] **Drei Controller driften auseinander:** Im Korrekt-Modus (ToDo_6a) laufen sie auf + dieselbe `moveTime` → ihre `controllerFreiAb` sollten zusammenbleiben; große Spreizung ist + ein Warnsignal (Feedrate-/Schätzfehler) +- [ ] **Schätzgüte:** `moveTime = dist/feedrate` ignoriert Beschleunigung; kurze Moves dauern + real länger. Vorlauf-Marge muss das abfangen; später ggf. Trapez-Profil-Schätzung verfeinern + +--- + ## Hinweis zur Implementierung `robot/fluidnc/FluidNCClient.js` ist eine bidirektionale WebSocket-Anbindung an FluidNC (Port 81) mit Reconnect-Logik und `EventEmitter`-Interface — diese Klasse ist eine gute Grundlage für alle drei Pakete und sollte bei der Umsetzung von `ToDo_2` (Sender-Interface) mit evaluiert werden. diff --git a/docker-compose.yaml b/docker-compose.yaml index 7465782..6725701 100755 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -26,6 +26,10 @@ services: - GRBL_BASE_IP=192.168.0.183 - GRBL_ELLBOW_IP=192.168.0.202 - GRBL_HAND_IP=192.168.0.250 + # Austauschbare Kinematik (siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md). + # Defaults entsprechen exakt dem bisherigen Verhalten. + - ROBOT_KINEMATICS=arm3segmentlinearx + - ROBOT_KINEMATICS_PARAMS={"l1": 250, "l2": 264, "l3": 100} ports: - "2096:2095" - "2098:2098" diff --git a/logs/gcode_commands.log b/logs/gcode_commands.log index 7ddcc65..e785a32 100644 --- a/logs/gcode_commands.log +++ b/logs/gcode_commands.log @@ -10339,3 +10339,13 @@ 2026-06-09T10:41:44.720Z ::ffff:127.0.0.1: M114 2026-06-09T10:41:44.937Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 2026-06-09T10:41:45.162Z ::ffff:127.0.0.1: G1 X1 +2026-06-10T21:02:32.604Z ::ffff:127.0.0.1: M114 +2026-06-10T21:02:32.641Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-10T21:02:32.913Z ::ffff:127.0.0.1: M114 +2026-06-10T21:02:33.168Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-10T21:02:33.440Z ::ffff:127.0.0.1: G1 X1 +2026-06-10T21:03:41.815Z ::ffff:127.0.0.1: M114 +2026-06-10T21:03:41.839Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-10T21:03:42.477Z ::ffff:127.0.0.1: M114 +2026-06-10T21:03:42.692Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-10T21:03:42.921Z ::ffff:127.0.0.1: G1 X1 diff --git a/logs/pings.log b/logs/pings.log index 57901bc..4f6957d 100644 --- a/logs/pings.log +++ b/logs/pings.log @@ -14610,3 +14610,7 @@ 2026-06-09T10:39:57.863Z ::ffff:127.0.0.1 : Ping 2026-06-09T10:41:44.479Z ::ffff:127.0.0.1 : Ping 2026-06-09T10:41:44.499Z ::ffff:127.0.0.1 : Ping +2026-06-10T21:02:32.574Z ::ffff:127.0.0.1 : Ping +2026-06-10T21:02:32.642Z ::ffff:127.0.0.1 : Ping +2026-06-10T21:03:41.789Z ::ffff:127.0.0.1 : Ping +2026-06-10T21:03:42.257Z ::ffff:127.0.0.1 : Ping diff --git a/robot/KinematicsFactory.js b/robot/KinematicsFactory.js new file mode 100644 index 0000000..734bad1 --- /dev/null +++ b/robot/KinematicsFactory.js @@ -0,0 +1,76 @@ +// robot/KinematicsFactory.js — Phase 2 der Roadmap 12. +// +// Wählt anhand der Umgebungsvariable ROBOT_KINEMATICS eine Kinematik-Klasse aus +// `robot/kinematics/` und instantiiert sie mit den Parametern aus +// ROBOT_KINEMATICS_PARAMS (JSON). Eine unbekannte Kinematik führt zu einem +// klaren Fehler beim Start — kein silent fail. +// +// Liest process.env vorerst direkt (gleicher Stil wie ROBOT_DEFAULT_FEEDRATE in +// RobotBase). Ein späteres zentrales Config-Modul (ToDo_3_Config) kann diese +// Variablen übernehmen. + +/** Default-Bezeichner der Kinematik, wenn ROBOT_KINEMATICS nicht gesetzt ist. */ +const DEFAULT_KINEMATICS = 'arm3segmentlinearx'; + +// Registry: Bezeichner (lowercase) → Modulpfad relativ zu dieser Datei. +// Neue Kinematiken hier eintragen (siehe Phase 3). +const KINEMATICS_REGISTRY = { + arm3segmentlinearx: './kinematics/Arm3SegmentLinearX', +}; + +/** + * Lädt die Kinematik-Klasse zu einem Bezeichner. + * @param {string} [identifier] z. B. 'arm3segmentlinearx' (case-insensitive) + * @returns {Function} die Konstruktor-Klasse + * @throws {Error} bei unbekanntem Bezeichner + */ +function loadKinematicsClass(identifier = DEFAULT_KINEMATICS) { + const key = String(identifier).toLowerCase(); + const modulePath = KINEMATICS_REGISTRY[key]; + if (!modulePath) { + const known = Object.keys(KINEMATICS_REGISTRY).join(', '); + throw new Error( + `Unbekannte Kinematik "${identifier}" (ROBOT_KINEMATICS). Verfügbar: ${known}.` + ); + } + return require(modulePath); +} + +/** + * Parst ROBOT_KINEMATICS_PARAMS (JSON) zu einem Parameter-Objekt. + * @param {string} [raw] JSON-String, z. B. '{"l1":250,"l2":264,"l3":100}' + * @returns {Object} geparste Parameter (leeres Objekt, wenn nicht gesetzt) + * @throws {Error} bei ungültigem JSON + */ +function parseParams(raw) { + if (!raw) return {}; + try { + return JSON.parse(raw); + } catch (err) { + throw new Error(`ROBOT_KINEMATICS_PARAMS ist kein gültiges JSON: ${err.message}`); + } +} + +/** + * Erzeugt eine Roboter-Instanz anhand der Umgebungsvariablen. + * + * @param {Object} [env] Umgebungs-Objekt (Default: process.env) + * @param {Object} [defaultParams] Fallback-Parameter für die Default-Kinematik + * (z. B. die Armlängen der produktiven Standard-Hardware). Werte aus + * ROBOT_KINEMATICS_PARAMS überschreiben diese. + * @returns {import('./RobotBase')} die instantiierte Kinematik + */ +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); +} + +module.exports = { + createRobotFromEnv, + loadKinematicsClass, + parseParams, + DEFAULT_KINEMATICS, + KINEMATICS_REGISTRY, +}; diff --git a/robot/Robot.js b/robot/Robot.js index 53f2d0b..a2e7ddf 100755 --- a/robot/Robot.js +++ b/robot/Robot.js @@ -1,369 +1,9 @@ -const MotorPosition = require('./RobotMotorPosition.js') - -class Robot{ - - - constructor(l1, l2, l3) { - // Umgebungsvariablen-Logik - const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ? - Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000; - // Speed-Regelung-Schalter: 'legacy' (Default — exakt wie bisher) oder 'correct'. - // Siehe doc/ToDo_6a_Speed.md. - this.speedMode = (process.env.ROBOT_SPEED_MODE || 'legacy').toLowerCase(); - // ROBOT_USE_SPEED_CALC bleibt der interne Schalter für calculateSpeeds(); - // der Korrekt-Modus aktiviert die Berechnung automatisch. - this.useSpeedCalc = this.speedMode === 'correct' || - process.env.ROBOT_USE_SPEED_CALC === 'true' || - process.env.ROBOT_USE_SPEED_CALC === '1'; - /** @type {number} Bewegungszeit des letzten Schritts in Minuten (für koordinierte Feedrate) */ - this.lastMoveTime = 0; - - /** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */ - this.speedX = 200; - /** @type {number} Bewegungsgeschwindigkeit Y-Achse in mm/min */ - this.speedY = 200; - /** @type {number} Bewegungsgeschwindigkeit Z-Achse in mm/min */ - this.speedZ = 200; - - /** @type {number} Zeitstempel des zuletzt gesendeten Kommandos */ - this.lastCommandSend = 0; - - if(this.lastCommandSend == 0){ this.lastCommandSend = Date.now() }; - /** @type {boolean} Animation aktiviert */ - this.doAnimate = false; - - /** @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; - - // Plan-Koordinaten - XYZ FingerSpitze - /** @type {number} X-Position der Fingerspitze in mm */ - this.x = 0; - /** @type {number} Y-Position der Fingerspitze in mm */ - this.y = 0; - /** @type {number} Z-Position der Fingerspitze in mm */ - this.z = 0; - - // Plan-Koordinaten - HandRichtung (Euler-Winkel) - /** @type {number} Phi - Euler-Winkel (Längengrad): Rotation um Z-Achse in rad */ - this.phi = 0.0; - /** @type {number} Theta - Euler-Winkel (Breitengrad): Neigungswinkel der Handachse in rad */ - this.theta = -Math.PI/2; - /** @type {number} Psi - Euler-Winkel: Zusätzliche Drehung des Handgelenks in rad */ - this.psi = 0.0; - - /** @type {number} Finger-Abstands-Einstellung (Öffnungsweite) */ - this.e = 0.0; - - /** @type {number} Feedrate für Bewegungen in mm/min */ - this.feedrate = DEFAULT_FEEDRATE; - - /** @type {Object} Motor-Geschwindigkeiten in Einheiten pro Minute */ - this.motorSpeeds = {x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0}; - - // Zwischen-Ergebnisse: Handgelenk-Punkt (Koordinaten des Handgelenks, nur für Tests public) - /** @type {number} Handgelenk-Position X in mm (berechneter Zwischenwert) */ - this.pX = 0.0; - /** @type {number} Handgelenk-Position Y in mm (berechneter Zwischenwert) */ - this.pY = 0.0; - /** @type {number} Handgelenk-Position Z in mm (berechneter Zwischenwert) */ - this.pZ = 0.0; - - // Motor-Koordinaten - Schulter, Ellebogen, Hand-Dreher - /** @type {number} X-Motor-Position (Schulterposition auf X-Schiene) in mm */ - this.xMotor = 0; - /** @type {number} Alpha - Y-Motor-Winkel (Schulterposition) in rad */ - this.alpha = 0; - /** @type {number} Beta - Z-Motor-Winkel (Unterarm-Neigung unter Y-Achse) in rad */ - this.beta = 0; - - this.xMotorChanged = false; - this.yMotorChanged = false; - this.zMotorChanged = false; - - // Motor-Winkel für's Handgelenk - /** @type {number} a-Motor-Winkel: Rotation am Ellbogen in rad */ - this.a = 0; - /** @type {number} b-Motor-Winkel: Handgelenk-Knicker-Winkel in rad */ - this.b = 0; - /** @type {number} c-Motor-Winkel: Hand-Dreher-Rotation in rad */ - this.c = 0; - - this.aMotorChanged = false; - this.bMotorChanged = false; - this.cMotorChanged = false; - this.eMotorChanged = false; - - /** @type {number} e-Motor-Wert: Finger-Abstands-Motor-Position */ - this.eMotor = 0; - - /** @type {number} Zeitstempel des letzten verarbeiteten Kommandos */ - this.oldCommandTime = Date.now(); - /** @type {Function[]} Array von Visualisierungs-Funktionen */ - this.showFunctions = []; - - /** @type {Object[]} Gespeicherte Roboterpositionen/Punkte */ - this.savedPoints = []; - /** @type {number} Index des aktuell angesteuerten Punktes */ - this.atPointNr = 0; - /** @type {number} Zeitstempel des aktuellen Punktes in ms */ - this.t = 0; - - /** @type {boolean} Relative oder absolute Bewegung (true = relativ) */ - this.moveRelative = true; - - /** @type {Object[]} Array von Kommando-Empfängern */ - this.cmdReceivers = []; - } - - createMotorPosition(){ - this.motorPosition = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor); - - // Setze Changed-Flags basierend auf Änderungen seit der letzten Position - this.motorPosition.xMotorChanged = this.motorPositionOld ? this.xMotor !== this.motorPositionOld.x : true; - this.motorPosition.yMotorChanged = this.motorPositionOld ? this.alpha !== this.motorPositionOld.y : true; - this.motorPosition.zMotorChanged = this.motorPositionOld ? this.beta !== this.motorPositionOld.z : true; - this.motorPosition.aMotorChanged = this.motorPositionOld ? this.a !== this.motorPositionOld.a : true; - this.motorPosition.bMotorChanged = this.motorPositionOld ? this.b !== this.motorPositionOld.b : true; - this.motorPosition.cMotorChanged = this.motorPositionOld ? this.c !== this.motorPositionOld.c : true; - this.motorPosition.eMotorChanged = this.motorPositionOld ? this.eMotor !== this.motorPositionOld.e : true; - - // Setze Handgelenk-Koordinaten (für Speed-Berechnung) - this.motorPosition.pX = this.pX; - this.motorPosition.pY = this.pY; - this.motorPosition.pZ = this.pZ; - - // Setze Geschwindigkeiten - this.motorPosition.speeds = {...this.motorSpeeds}; - this.motorPosition.feedrate = this.feedrate || 200; - - // Speed-Regelung: Modus und (vorläufige) Bewegungszeit für die Sender - this.motorPosition.speedMode = this.speedMode; - this.motorPosition.moveTime = this.lastMoveTime || 0; - } - - // Berechnet aus XYZ die Motor-Winkel für den GCode - calculateAngles3D(verbose){ - 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} - - // Handgelenk-Punkt ausrechnen: - this.pX = this.x + this.l3*Math.sin(this.theta)*Math.cos(this.phi); - this.pY = this.y + this.l3*Math.sin(this.theta)*Math.sin(this.phi); - this.pZ = this.z + this.l3*Math.cos(this.theta); - - var pX = this.pX; - var pY = this.pY; - var pZ = this.pZ; - - this.xMotor = pX; - // Ziel-Punkt ausrechnen ==> 2D Rechnung Arm - var r = Math.sqrt(pY * pY + pZ * pZ); - - if (r > (this.l1 + this.l2)) { return; } - if (r == 0) { return; } - - var gamma = Math.asin(pZ / r); - var delta = Math.acos((this.l1 * this.l1 + this.l2 * this.l2 - r * r) / (2 * this.l1 * this.l2)); - this.alpha = Math.acos((this.l1 * this.l1 + r * r - this.l2 * this.l2) / (2 * r * this.l1)) + gamma; - this.beta = -Math.PI + (this.alpha + delta); - // Ende <== 2D Rechnung Arm - - // Richtung der Hand ausgerechnet - // Arm = (0, cos(beta), sin(beta)) Punkt = (sin(theta)cos(phi), sin(theta)sin(phi), cos(theta)) - // - // Unterarm muss gedreht werden. Aus der Y-Z-Ebene raus. Hin in die Ebene n x r - // wobei n = Unterarm x (P-O) ist - var nX = Math.cos(this.beta)*Math.cos(this.theta) - Math.sin(this.theta)*Math.sin(this.phi)*Math.sin(this.beta); - var nY = Math.sin(this.beta)*Math.sin(this.theta)*Math.cos(this.phi); - var nZ = -1.0*Math.sin(this.theta)*Math.cos(this.phi)*Math.cos(this.beta); - var nBetrag = Math.sqrt(nX*nX + nY*nY + nZ*nZ); - - if(verbose) console.log("Richtung: > ", nX/nBetrag, nY/nBetrag, nZ/nBetrag); - - var cosA = (nX)/nBetrag; - this.a = Math.acos(cosA) - if(Math.cos(this.phi) > 0){this.a = -this.a} - if(Math.sin(this.theta) < 0) {this.a = -this.a} - - - // 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.acos(cosB); - - - // Winkel zwischen n und z muss rumgedreht werden. - var cosC = - nZ / nBetrag; - this.c = Math.acos(cosC); - this.c += this.psi; - - // a um 180° drehen - this.a += Math.PI; - while(this.a > Math.PI){this.a -= 2*Math.PI} - while(this.a < -Math.PI){this.a += 2*Math.PI} - - this.eMotor = this.e - this.b - this.c; - } - - // Berechnet die Motor-Geschwindigkeiten basierend auf Feedrate und Positionsänderung - calculateSpeeds(oldPos, newPos) { - if (!this.useSpeedCalc) return; // Neue Logik nur aktivieren, wenn Flag gesetzt - if (!oldPos || !newPos || this.feedrate <= 0) return; - - this.lastMoveTime = 0; // wird unten gesetzt, sobald eine Bewegung erkannt wird - - // 1. Berechne xyz-Distanz (primär) - const dx = newPos.x - oldPos.x; - const dy = newPos.y - oldPos.y; - const dz = newPos.z - oldPos.z; - const xyz_dist = Math.sqrt(dx*dx + dy*dy + dz*dz); - - if (xyz_dist > 0.001) { - const time = xyz_dist / this.feedrate; - this.lastMoveTime = time; - this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; - this.motorSpeeds.y = (this.alpha - oldPos.y) / time; - this.motorSpeeds.z = (this.beta - oldPos.z) / time; - this.motorSpeeds.a = (this.a - oldPos.a) / time; - this.motorSpeeds.b = (this.b - oldPos.b) / time; - this.motorSpeeds.c = (this.c - oldPos.c) / time; - this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; - return; - } - - // 2. Berechne Handgelenk-Punkt-Distanz (falls xyz = 0) - const dpx = newPos.pX - oldPos.pX; - const dpy = newPos.pY - oldPos.pY; - const dpz = newPos.pZ - oldPos.pZ; - const handgelenk_dist = Math.sqrt(dpx*dpx + dpy*dpy + dpz*dpz); - - if (handgelenk_dist > 0.001) { - const time = handgelenk_dist / this.feedrate; - this.lastMoveTime = time; - this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; - this.motorSpeeds.y = (this.alpha - oldPos.y) / time; - this.motorSpeeds.z = (this.beta - oldPos.z) / time; - this.motorSpeeds.a = (this.a - oldPos.a) / time; - this.motorSpeeds.b = (this.b - oldPos.b) / time; - this.motorSpeeds.c = (this.c - oldPos.c) / time; - this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; - return; - } - - // 3. Berechne Finger-Distanz (falls Handgelenk = 0) - const de = Math.abs(this.eMotor - oldPos.e); - if (de > 0.001) { - const time = de / this.feedrate; - this.lastMoveTime = time; - this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; - this.motorSpeeds.y = (this.alpha - oldPos.y) / time; - this.motorSpeeds.z = (this.beta - oldPos.z) / time; - this.motorSpeeds.a = (this.a - oldPos.a) / time; - this.motorSpeeds.b = (this.b - oldPos.b) / time; - this.motorSpeeds.c = (this.c - oldPos.c) / time; - this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; - return; - } - - // 4. Keine Bewegung erkannt → motorSpeeds bleiben AUF DEFAULT - - - // ToDo: Aus motorSpeed mit einzelnenen Werten muss noch die feedrate berechnet werden. - // hier bin ich unsicher, ob das nicht in den Sender rein sollte, da es eventuell - // abhngig vom FluidNC und dessen speed interpretation ist. - } - - rotateAroundAxis(v, n, angle) { - const cos = Math.cos(angle); - const sin = Math.sin(angle); - - const dot = v.x*n.x + v.y*n.y + v.z*n.z; - - const cross = { - x: n.y*v.z - n.z*v.y, - y: n.z*v.x - n.x*v.z, - z: n.x*v.y - n.y*v.x - }; - - return { - x: v.x*cos + cross.x*sin + n.x*dot*(1 - cos), - y: v.y*cos + cross.y*sin + n.y*dot*(1 - cos), - z: v.z*cos + cross.z*sin + n.z*dot*(1 - cos) - }; - } - - calculatePositionFromMotorAngles(verbose = false) { - - const vecBizeps = {x: this.xMotor, y: this.l1 * Math.cos(this.alpha), z: this.l1 * Math.sin(this.alpha)} - const vecUnterarm = {x: 0, y: Math.cos(this.beta), z: Math.sin(this.beta)} - - // der Handgelenk-Punkt - this.pX = vecBizeps.x + this.l2 * vecUnterarm.x; - this.pY = vecBizeps.y + this.l2 * vecUnterarm.y; - this.pZ = vecBizeps.z + this.l2 * vecUnterarm.z; - - // n: Die Handgelenk-Unterarm-Knick-Achse. X-Achse wird um den Unterarm gedreht. - const n = { x: -Math.cos(this.a), y: vecUnterarm.z * Math.sin(this.a), z: -vecUnterarm.y * Math.sin(this.a) }; - - if(verbose) console.log("n inverse:", n.x, n.y, n.z); - - const vHand = this.rotateAroundAxis(vecUnterarm, n, this.b); - - this.x = this.pX - this.l3 * vHand.x; - this.y = this.pY - this.l3 * vHand.y; - this.z = this.pZ - this.l3 * vHand.z; - - this.theta = Math.atan2(Math.sqrt(vHand.x*vHand.x + vHand.y*vHand.y),vHand.z); - this.phi = Math.atan2(vHand.y, vHand.x); - this.psi = this.c - Math.acos(-n.z); - - 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} - } - - sendCommand(cmd="G1"){ - const isFirstCall = !this.motorPosition; - - if (isFirstCall) { - this.motorPositionOld = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor); - } else { - this.motorPositionOld = this.motorPosition; - } - - this.createMotorPosition() - - // Für den ersten Aufruf setze alle Changed-Flags auf true - if (isFirstCall) { - this.motorPosition.xMotorChanged = true; - this.motorPosition.yMotorChanged = true; - this.motorPosition.zMotorChanged = true; - this.motorPosition.aMotorChanged = true; - this.motorPosition.bMotorChanged = true; - this.motorPosition.cMotorChanged = true; - this.motorPosition.eMotorChanged = true; - } - - // Berechne Geschwindigkeiten - this.calculateSpeeds(this.motorPositionOld, this.motorPosition); - this.motorPosition.speeds = {...this.motorSpeeds}; - // moveTime nach der Berechnung aktualisieren (createMotorPosition lief davor) - this.motorPosition.moveTime = this.lastMoveTime || 0; - - console.log("Robot.sendCommand: ", cmd.toString(), " Motor-Pos: x=", this.motorPosition.x.toFixed(3), "yMotor=",this.motorPosition.y.toFixed(3), "zMotor=",this.motorPosition.z.toFixed(3), "aM=", this.motorPosition.a.toFixed(3), "bM=", this.motorPosition.b.toFixed(3), "cM=", this.motorPosition.c.toFixed(3), " e=", this.motorPosition.e.toFixed(3)); - - this.cmdReceivers.forEach(receiver => { - receiver.execCommand(cmd,this.motorPositionOld, this.motorPosition); - }); - } -} - - - -module.exports = Robot // Export class \ No newline at end of file +// robot/Robot.js — Kompatibilitäts-Alias (siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md). +// +// Externer Code (und Tests) kann weiterhin `require('./robot/Robot')` schreiben. +// Während der Übergangsperiode zeigt der Alias auf die konkrete Default-Kinematik +// `Arm3SegmentLinearX`, damit `new Robot(l1, l2, l3)` unverändert funktioniert. +// +// Die generische Basisklasse / der Interface-Vertrag liegt in `RobotBase.js`; +// die produktive Auswahl der Kinematik übernimmt `KinematicsFactory.js`. +module.exports = require('./kinematics/Arm3SegmentLinearX'); diff --git a/robot/RobotBase.js b/robot/RobotBase.js new file mode 100644 index 0000000..a595efa --- /dev/null +++ b/robot/RobotBase.js @@ -0,0 +1,319 @@ +const MotorPosition = require('./RobotMotorPosition.js') + +/** + * RobotBase — abstrakte Basisklasse und zugleich der Interface-Vertrag des + * Frameworks. Enthält die roboter-unabhängige Infrastruktur (Zustand, + * `sendCommand`, `createMotorPosition`, `calculateSpeeds`, `rotateAroundAxis`). + * + * Der arm-spezifische Teil sind ausschließlich die beiden Kinematik-Methoden + * {@link RobotBase#calculateAngles3D} und + * {@link RobotBase#calculatePositionFromMotorAngles}. Sie sind hier abstrakt + * (werfen) und MÜSSEN von jeder konkreten Kinematik-Klasse überschrieben werden. + * + * Verträge (siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md): + * - Workspace-Koordinaten: `x, y, z, phi, theta, psi` + `e` (Greifer) — 6-DOF. + * - Motor-Zustand: 7 feste Slots `xMotor, alpha, beta, a, b, c, eMotor`. + * + * Konkrete Implementierungen leben in `robot/kinematics/`. + * + * @abstract + */ +class RobotBase{ + + + constructor() { + // Umgebungsvariablen-Logik + const DEFAULT_FEEDRATE = process.env.ROBOT_DEFAULT_FEEDRATE ? + Number(process.env.ROBOT_DEFAULT_FEEDRATE) : 1000; + // Speed-Regelung-Schalter: 'legacy' (Default — exakt wie bisher) oder 'correct'. + // Siehe doc/ToDo_6a_Speed.md. + this.speedMode = (process.env.ROBOT_SPEED_MODE || 'legacy').toLowerCase(); + // ROBOT_USE_SPEED_CALC bleibt der interne Schalter für calculateSpeeds(); + // der Korrekt-Modus aktiviert die Berechnung automatisch. + this.useSpeedCalc = this.speedMode === 'correct' || + process.env.ROBOT_USE_SPEED_CALC === 'true' || + process.env.ROBOT_USE_SPEED_CALC === '1'; + /** @type {number} Bewegungszeit des letzten Schritts in Minuten (für koordinierte Feedrate) */ + this.lastMoveTime = 0; + + /** @type {number} Bewegungsgeschwindigkeit X-Achse in mm/min */ + this.speedX = 200; + /** @type {number} Bewegungsgeschwindigkeit Y-Achse in mm/min */ + this.speedY = 200; + /** @type {number} Bewegungsgeschwindigkeit Z-Achse in mm/min */ + this.speedZ = 200; + + /** @type {number} Zeitstempel des zuletzt gesendeten Kommandos */ + this.lastCommandSend = 0; + + if(this.lastCommandSend == 0){ this.lastCommandSend = Date.now() }; + /** @type {boolean} Animation aktiviert */ + this.doAnimate = false; + + // Plan-Koordinaten - XYZ FingerSpitze + /** @type {number} X-Position der Fingerspitze in mm */ + this.x = 0; + /** @type {number} Y-Position der Fingerspitze in mm */ + this.y = 0; + /** @type {number} Z-Position der Fingerspitze in mm */ + this.z = 0; + + // Plan-Koordinaten - HandRichtung (Euler-Winkel) + /** @type {number} Phi - Euler-Winkel (Längengrad): Rotation um Z-Achse in rad */ + this.phi = 0.0; + /** @type {number} Theta - Euler-Winkel (Breitengrad): Neigungswinkel der Handachse in rad */ + this.theta = -Math.PI/2; + /** @type {number} Psi - Euler-Winkel: Zusätzliche Drehung des Handgelenks in rad */ + this.psi = 0.0; + + /** @type {number} Finger-Abstands-Einstellung (Öffnungsweite) */ + this.e = 0.0; + + /** @type {number} Feedrate für Bewegungen in mm/min */ + this.feedrate = DEFAULT_FEEDRATE; + + /** @type {Object} Motor-Geschwindigkeiten in Einheiten pro Minute */ + this.motorSpeeds = {x: 0, y: 0, z: 0, a: 0, b: 0, c: 0, e: 0}; + + // Zwischen-Ergebnisse: Handgelenk-Punkt (Koordinaten des Handgelenks, nur für Tests public) + /** @type {number} Handgelenk-Position X in mm (berechneter Zwischenwert) */ + this.pX = 0.0; + /** @type {number} Handgelenk-Position Y in mm (berechneter Zwischenwert) */ + this.pY = 0.0; + /** @type {number} Handgelenk-Position Z in mm (berechneter Zwischenwert) */ + this.pZ = 0.0; + + // Motor-Koordinaten - Schulter, Ellebogen, Hand-Dreher + /** @type {number} X-Motor-Position (Schulterposition auf X-Schiene) in mm */ + this.xMotor = 0; + /** @type {number} Alpha - Y-Motor-Winkel (Schulterposition) in rad */ + this.alpha = 0; + /** @type {number} Beta - Z-Motor-Winkel (Unterarm-Neigung unter Y-Achse) in rad */ + this.beta = 0; + + this.xMotorChanged = false; + this.yMotorChanged = false; + this.zMotorChanged = false; + + // Motor-Winkel für's Handgelenk + /** @type {number} a-Motor-Winkel: Rotation am Ellbogen in rad */ + this.a = 0; + /** @type {number} b-Motor-Winkel: Handgelenk-Knicker-Winkel in rad */ + this.b = 0; + /** @type {number} c-Motor-Winkel: Hand-Dreher-Rotation in rad */ + this.c = 0; + + this.aMotorChanged = false; + this.bMotorChanged = false; + this.cMotorChanged = false; + this.eMotorChanged = false; + + /** @type {number} e-Motor-Wert: Finger-Abstands-Motor-Position */ + this.eMotor = 0; + + /** @type {number} Zeitstempel des letzten verarbeiteten Kommandos */ + this.oldCommandTime = Date.now(); + /** @type {Function[]} Array von Visualisierungs-Funktionen */ + this.showFunctions = []; + + /** @type {Object[]} Gespeicherte Roboterpositionen/Punkte */ + this.savedPoints = []; + /** @type {number} Index des aktuell angesteuerten Punktes */ + this.atPointNr = 0; + /** @type {number} Zeitstempel des aktuellen Punktes in ms */ + this.t = 0; + + /** @type {boolean} Relative oder absolute Bewegung (true = relativ) */ + this.moveRelative = true; + + /** @type {Object[]} Array von Kommando-Empfängern */ + this.cmdReceivers = []; + } + + createMotorPosition(){ + this.motorPosition = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor); + + // Setze Changed-Flags basierend auf Änderungen seit der letzten Position + this.motorPosition.xMotorChanged = this.motorPositionOld ? this.xMotor !== this.motorPositionOld.x : true; + this.motorPosition.yMotorChanged = this.motorPositionOld ? this.alpha !== this.motorPositionOld.y : true; + this.motorPosition.zMotorChanged = this.motorPositionOld ? this.beta !== this.motorPositionOld.z : true; + this.motorPosition.aMotorChanged = this.motorPositionOld ? this.a !== this.motorPositionOld.a : true; + this.motorPosition.bMotorChanged = this.motorPositionOld ? this.b !== this.motorPositionOld.b : true; + this.motorPosition.cMotorChanged = this.motorPositionOld ? this.c !== this.motorPositionOld.c : true; + this.motorPosition.eMotorChanged = this.motorPositionOld ? this.eMotor !== this.motorPositionOld.e : true; + + // Setze Handgelenk-Koordinaten (für Speed-Berechnung) + this.motorPosition.pX = this.pX; + this.motorPosition.pY = this.pY; + this.motorPosition.pZ = this.pZ; + + // Setze Geschwindigkeiten + this.motorPosition.speeds = {...this.motorSpeeds}; + this.motorPosition.feedrate = this.feedrate || 200; + + // Speed-Regelung: Modus und (vorläufige) Bewegungszeit für die Sender + this.motorPosition.speedMode = this.speedMode; + this.motorPosition.moveTime = this.lastMoveTime || 0; + } + + /** + * Vorwärts-Kinematik: Workspace-Koordinaten → Motorwinkel. + * + * Liest `this.x/y/z/phi/theta/psi/e` und schreibt das Ergebnis auf die + * Motor-Slots `this.xMotor/alpha/beta/a/b/c/eMotor` (sowie die Zwischenwerte + * `this.pX/pY/pZ` des Handgelenk-Punkts). + * + * ABSTRAKT — muss von der konkreten Kinematik-Klasse überschrieben werden. + * + * @abstract + * @param {boolean} [verbose] Debug-Ausgaben aktivieren + */ + calculateAngles3D(verbose){ + throw new Error('calculateAngles3D() not implemented — RobotBase ist abstrakt; eine konkrete Kinematik-Klasse muss diese Methode überschreiben.'); + } + + // Berechnet die Motor-Geschwindigkeiten basierend auf Feedrate und Positionsänderung + calculateSpeeds(oldPos, newPos) { + if (!this.useSpeedCalc) return; // Neue Logik nur aktivieren, wenn Flag gesetzt + if (!oldPos || !newPos || this.feedrate <= 0) return; + + this.lastMoveTime = 0; // wird unten gesetzt, sobald eine Bewegung erkannt wird + + // 1. Berechne xyz-Distanz (primär) + const dx = newPos.x - oldPos.x; + const dy = newPos.y - oldPos.y; + const dz = newPos.z - oldPos.z; + const xyz_dist = Math.sqrt(dx*dx + dy*dy + dz*dz); + + if (xyz_dist > 0.001) { + const time = xyz_dist / this.feedrate; + this.lastMoveTime = time; + this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; + this.motorSpeeds.y = (this.alpha - oldPos.y) / time; + this.motorSpeeds.z = (this.beta - oldPos.z) / time; + this.motorSpeeds.a = (this.a - oldPos.a) / time; + this.motorSpeeds.b = (this.b - oldPos.b) / time; + this.motorSpeeds.c = (this.c - oldPos.c) / time; + this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; + return; + } + + // 2. Berechne Handgelenk-Punkt-Distanz (falls xyz = 0) + const dpx = newPos.pX - oldPos.pX; + const dpy = newPos.pY - oldPos.pY; + const dpz = newPos.pZ - oldPos.pZ; + const handgelenk_dist = Math.sqrt(dpx*dpx + dpy*dpy + dpz*dpz); + + if (handgelenk_dist > 0.001) { + const time = handgelenk_dist / this.feedrate; + this.lastMoveTime = time; + this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; + this.motorSpeeds.y = (this.alpha - oldPos.y) / time; + this.motorSpeeds.z = (this.beta - oldPos.z) / time; + this.motorSpeeds.a = (this.a - oldPos.a) / time; + this.motorSpeeds.b = (this.b - oldPos.b) / time; + this.motorSpeeds.c = (this.c - oldPos.c) / time; + this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; + return; + } + + // 3. Berechne Finger-Distanz (falls Handgelenk = 0) + const de = Math.abs(this.eMotor - oldPos.e); + if (de > 0.001) { + const time = de / this.feedrate; + this.lastMoveTime = time; + this.motorSpeeds.x = (this.xMotor - oldPos.x) / time; + this.motorSpeeds.y = (this.alpha - oldPos.y) / time; + this.motorSpeeds.z = (this.beta - oldPos.z) / time; + this.motorSpeeds.a = (this.a - oldPos.a) / time; + this.motorSpeeds.b = (this.b - oldPos.b) / time; + this.motorSpeeds.c = (this.c - oldPos.c) / time; + this.motorSpeeds.e = (this.eMotor - oldPos.e) / time; + return; + } + + // 4. Keine Bewegung erkannt → motorSpeeds bleiben AUF DEFAULT + + + // ToDo: Aus motorSpeed mit einzelnenen Werten muss noch die feedrate berechnet werden. + // hier bin ich unsicher, ob das nicht in den Sender rein sollte, da es eventuell + // abhngig vom FluidNC und dessen speed interpretation ist. + } + + /** + * Geschützte Hilfsmethode (Rodrigues-Rotation): dreht den Vektor `v` um die + * (normierte) Achse `n` um den Winkel `angle`. Für Kinematik-Implementierungen. + */ + rotateAroundAxis(v, n, angle) { + const cos = Math.cos(angle); + const sin = Math.sin(angle); + + const dot = v.x*n.x + v.y*n.y + v.z*n.z; + + const cross = { + x: n.y*v.z - n.z*v.y, + y: n.z*v.x - n.x*v.z, + z: n.x*v.y - n.y*v.x + }; + + return { + x: v.x*cos + cross.x*sin + n.x*dot*(1 - cos), + y: v.y*cos + cross.y*sin + n.y*dot*(1 - cos), + z: v.z*cos + cross.z*sin + n.z*dot*(1 - cos) + }; + } + + /** + * Rückwärts-Kinematik: Motorwinkel → Workspace-Koordinaten. + * + * Liest die Motor-Slots `this.xMotor/alpha/beta/a/b/c` und schreibt das + * Ergebnis auf `this.x/y/z/phi/theta/psi` (sowie `this.pX/pY/pZ`). + * + * ABSTRAKT — muss von der konkreten Kinematik-Klasse überschrieben werden. + * + * @abstract + * @param {boolean} [verbose] Debug-Ausgaben aktivieren + */ + calculatePositionFromMotorAngles(verbose = false) { + throw new Error('calculatePositionFromMotorAngles() not implemented — RobotBase ist abstrakt; eine konkrete Kinematik-Klasse muss diese Methode überschreiben.'); + } + + sendCommand(cmd="G1"){ + const isFirstCall = !this.motorPosition; + + if (isFirstCall) { + this.motorPositionOld = new MotorPosition(this.xMotor, this.alpha, this.beta, this.a, this.b, this.c, this.eMotor); + } else { + this.motorPositionOld = this.motorPosition; + } + + this.createMotorPosition() + + // Für den ersten Aufruf setze alle Changed-Flags auf true + if (isFirstCall) { + this.motorPosition.xMotorChanged = true; + this.motorPosition.yMotorChanged = true; + this.motorPosition.zMotorChanged = true; + this.motorPosition.aMotorChanged = true; + this.motorPosition.bMotorChanged = true; + this.motorPosition.cMotorChanged = true; + this.motorPosition.eMotorChanged = true; + } + + // Berechne Geschwindigkeiten + this.calculateSpeeds(this.motorPositionOld, this.motorPosition); + this.motorPosition.speeds = {...this.motorSpeeds}; + // moveTime nach der Berechnung aktualisieren (createMotorPosition lief davor) + this.motorPosition.moveTime = this.lastMoveTime || 0; + + console.log("Robot.sendCommand: ", cmd.toString(), " Motor-Pos: x=", this.motorPosition.x.toFixed(3), "yMotor=",this.motorPosition.y.toFixed(3), "zMotor=",this.motorPosition.z.toFixed(3), "aM=", this.motorPosition.a.toFixed(3), "bM=", this.motorPosition.b.toFixed(3), "cM=", this.motorPosition.c.toFixed(3), " e=", this.motorPosition.e.toFixed(3)); + + this.cmdReceivers.forEach(receiver => { + receiver.execCommand(cmd,this.motorPositionOld, this.motorPosition); + }); + } +} + + + +module.exports = RobotBase // Export abstrakte Basisklasse / Interface diff --git a/robot/kinematics/Arm3SegmentLinearX.js b/robot/kinematics/Arm3SegmentLinearX.js new file mode 100644 index 0000000..bbfa634 --- /dev/null +++ b/robot/kinematics/Arm3SegmentLinearX.js @@ -0,0 +1,133 @@ +const RobotBase = require('../RobotBase.js') + +/** + * Arm3SegmentLinearX — 6-DOF-Arm mit linearer X-Schiene und drei Segmenten + * (Oberarm `l1`, Unterarm `l2`, Hand/Endeffector `l3`) plus Greifer. + * + * Erste konkrete Kinematik-Implementierung des Frameworks. Implementiert den + * Interface-Vertrag von {@link RobotBase} — die gesamte generische Infrastruktur + * (Zustand, sendCommand, Speed-Berechnung, ...) wird geerbt. + * + * Physikalische Struktur: + * - X-Achse: lineare Schiene (Schulterposition `xMotor`) + * - Schulter/Ellbogen: 2D-Arm in der Y-Z-Ebene (`alpha`, `beta`) + * - Handgelenk: drei Winkel `a` (Ellbogen-Dreher), `b` (Knicker), `c` (Hand-Dreher) + * - Greifer: `e` + */ +class Arm3SegmentLinearX extends RobotBase { + + /** + * @param {number} l1 Länge des Oberarms in mm + * @param {number} l2 Länge des Unterarms in mm + * @param {number} l3 Länge der Hand (Endeffector) in mm + */ + constructor(l1, l2, l3) { + 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; + } + + // Berechnet aus XYZ die Motor-Winkel für den GCode + calculateAngles3D(verbose){ + 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} + + // Handgelenk-Punkt ausrechnen: + this.pX = this.x + this.l3*Math.sin(this.theta)*Math.cos(this.phi); + this.pY = this.y + this.l3*Math.sin(this.theta)*Math.sin(this.phi); + this.pZ = this.z + this.l3*Math.cos(this.theta); + + var pX = this.pX; + var pY = this.pY; + var pZ = this.pZ; + + this.xMotor = pX; + // Ziel-Punkt ausrechnen ==> 2D Rechnung Arm + var r = Math.sqrt(pY * pY + pZ * pZ); + + if (r > (this.l1 + this.l2)) { return; } + if (r == 0) { return; } + + var gamma = Math.asin(pZ / r); + var delta = Math.acos((this.l1 * this.l1 + this.l2 * this.l2 - r * r) / (2 * this.l1 * this.l2)); + this.alpha = Math.acos((this.l1 * this.l1 + r * r - this.l2 * this.l2) / (2 * r * this.l1)) + gamma; + this.beta = -Math.PI + (this.alpha + delta); + // Ende <== 2D Rechnung Arm + + // Richtung der Hand ausgerechnet + // Arm = (0, cos(beta), sin(beta)) Punkt = (sin(theta)cos(phi), sin(theta)sin(phi), cos(theta)) + // + // Unterarm muss gedreht werden. Aus der Y-Z-Ebene raus. Hin in die Ebene n x r + // wobei n = Unterarm x (P-O) ist + var nX = Math.cos(this.beta)*Math.cos(this.theta) - Math.sin(this.theta)*Math.sin(this.phi)*Math.sin(this.beta); + var nY = Math.sin(this.beta)*Math.sin(this.theta)*Math.cos(this.phi); + var nZ = -1.0*Math.sin(this.theta)*Math.cos(this.phi)*Math.cos(this.beta); + var nBetrag = Math.sqrt(nX*nX + nY*nY + nZ*nZ); + + if(verbose) console.log("Richtung: > ", nX/nBetrag, nY/nBetrag, nZ/nBetrag); + + var cosA = (nX)/nBetrag; + this.a = Math.acos(cosA) + if(Math.cos(this.phi) > 0){this.a = -this.a} + if(Math.sin(this.theta) < 0) {this.a = -this.a} + + + // 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.acos(cosB); + + + // Winkel zwischen n und z muss rumgedreht werden. + var cosC = - nZ / nBetrag; + this.c = Math.acos(cosC); + this.c += this.psi; + + // a um 180° drehen + this.a += Math.PI; + while(this.a > Math.PI){this.a -= 2*Math.PI} + while(this.a < -Math.PI){this.a += 2*Math.PI} + + this.eMotor = this.e - this.b - this.c; + } + + calculatePositionFromMotorAngles(verbose = false) { + + const vecBizeps = {x: this.xMotor, y: this.l1 * Math.cos(this.alpha), z: this.l1 * Math.sin(this.alpha)} + const vecUnterarm = {x: 0, y: Math.cos(this.beta), z: Math.sin(this.beta)} + + // der Handgelenk-Punkt + this.pX = vecBizeps.x + this.l2 * vecUnterarm.x; + this.pY = vecBizeps.y + this.l2 * vecUnterarm.y; + this.pZ = vecBizeps.z + this.l2 * vecUnterarm.z; + + // n: Die Handgelenk-Unterarm-Knick-Achse. X-Achse wird um den Unterarm gedreht. + const n = { x: -Math.cos(this.a), y: vecUnterarm.z * Math.sin(this.a), z: -vecUnterarm.y * Math.sin(this.a) }; + + if(verbose) console.log("n inverse:", n.x, n.y, n.z); + + const vHand = this.rotateAroundAxis(vecUnterarm, n, this.b); + + this.x = this.pX - this.l3 * vHand.x; + this.y = this.pY - this.l3 * vHand.y; + this.z = this.pZ - this.l3 * vHand.z; + + this.theta = Math.atan2(Math.sqrt(vHand.x*vHand.x + vHand.y*vHand.y),vHand.z); + this.phi = Math.atan2(vHand.y, vHand.x); + this.psi = this.c - Math.acos(-n.z); + + 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} + } +} + + +module.exports = Arm3SegmentLinearX diff --git a/startRobot.js b/startRobot.js index c3303fa..f8a2e9b 100755 --- a/startRobot.js +++ b/startRobot.js @@ -1,6 +1,6 @@ const fs = require('fs'); const https = require('https'); -const Robot = require('./robot/Robot'); +const { createRobotFromEnv } = require('./robot/KinematicsFactory'); const GCode = require('./robot/GCode'); const TelnetSender = require('./robot/TelnetSenderGRBL'); @@ -43,7 +43,7 @@ function createApp(options = {}) { fsModule = fs, httpsModule = https, processEnv = process.env, - RobotClass = Robot, + RobotClass = null, GCodeModule = GCode, TelnetSenderClass = TelnetSender, initInputWSFn = initInputWS, @@ -69,7 +69,13 @@ function createApp(options = {}) { const httpsServer = httpsModule.createServer(httpsOptions); - const robot = new RobotClass(250, 264, 100); + // Kinematik wählen: explizit injizierte RobotClass (Tests) hat Vorrang, + // sonst per Umgebungsvariable (ROBOT_KINEMATICS / ROBOT_KINEMATICS_PARAMS). + // Die Armlängen der Standard-Hardware dienen als Default, wenn keine Params + // gesetzt sind — siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md. + const robot = RobotClass + ? new RobotClass(250, 264, 100) + : createRobotFromEnv(processEnv, { l1: 250, l2: 264, l3: 100 }); const sharedState = { connectedClients: [], diff --git a/test/KinematicsFactory.test.js b/test/KinematicsFactory.test.js new file mode 100644 index 0000000..4d07717 --- /dev/null +++ b/test/KinematicsFactory.test.js @@ -0,0 +1,69 @@ +const { + createRobotFromEnv, + loadKinematicsClass, + parseParams, + DEFAULT_KINEMATICS, +} = require('../robot/KinematicsFactory'); + +const Arm3SegmentLinearX = require('../robot/kinematics/Arm3SegmentLinearX'); +const RobotBase = require('../robot/RobotBase'); + +describe('KinematicsFactory (Roadmap 12, Phase 2)', () => { + + test('Default-Kinematik ohne Env, mit Fallback-Params', () => { + const robot = createRobotFromEnv({}, { l1: 250, l2: 264, l3: 100 }); + expect(robot).toBeInstanceOf(Arm3SegmentLinearX); + expect(robot).toBeInstanceOf(RobotBase); + expect([robot.l1, robot.l2, robot.l3]).toEqual([250, 264, 100]); + }); + + test('ROBOT_KINEMATICS_PARAMS überschreibt die Fallback-Params', () => { + const robot = createRobotFromEnv( + { + ROBOT_KINEMATICS: 'arm3segmentlinearx', + ROBOT_KINEMATICS_PARAMS: '{"l1":300,"l2":200,"l3":10}', + }, + { l1: 250, l2: 264, l3: 100 } + ); + expect([robot.l1, robot.l2, robot.l3]).toEqual([300, 200, 10]); + }); + + test('Bezeichner ist case-insensitive', () => { + expect(loadKinematicsClass('Arm3SegmentLinearX')).toBe(Arm3SegmentLinearX); + expect(loadKinematicsClass(DEFAULT_KINEMATICS)).toBe(Arm3SegmentLinearX); + }); + + test('Unbekannte Kinematik → klare Fehlermeldung (kein silent fail)', () => { + expect(() => createRobotFromEnv({ ROBOT_KINEMATICS: 'doesNotExist' })) + .toThrow(/Unbekannte Kinematik "doesNotExist"/); + }); + + test('Ungültiges JSON in ROBOT_KINEMATICS_PARAMS → klare Fehlermeldung', () => { + expect(() => parseParams('{bad json')) + .toThrow(/ROBOT_KINEMATICS_PARAMS ist kein gültiges JSON/); + }); + + test('parseParams: leer/undefined → leeres Objekt', () => { + expect(parseParams()).toEqual({}); + expect(parseParams('')).toEqual({}); + }); +}); + +describe('RobotBase ist abstrakt (Roadmap 12, Phase 0)', () => { + + test('calculateAngles3D() wirft, wenn nicht überschrieben', () => { + expect(() => new RobotBase().calculateAngles3D()) + .toThrow(/not implemented/); + }); + + test('calculatePositionFromMotorAngles() wirft, wenn nicht überschrieben', () => { + expect(() => new RobotBase().calculatePositionFromMotorAngles()) + .toThrow(/not implemented/); + }); + + test('Arm3SegmentLinearX überschreibt beide Kinematik-Methoden', () => { + const robot = new Arm3SegmentLinearX(250, 264, 100); + expect(() => robot.calculateAngles3D()).not.toThrow(); + expect(() => robot.calculatePositionFromMotorAngles()).not.toThrow(); + }); +});