ChK/appRobotDriver
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Umbau 12: Robot-Kinematics als extends RobotBase

Browse Source
This commit is contained in:
chk
2026-06-10 23:18:29 +02:00
parent 9274b2c52a
commit d906b094b1
11 changed files with 837 additions and 385 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
doc/ToDo_12_InverseKinematikConfig_ROADMAP.md
View File

@@ -85,10 +85,10 @@ calculateAngles3D() // Workspace → Motorwinkel (schreibt auf t
calculatePositionFromMotorAngles() // Motorwinkel → Workspace (schreibt auf this.*) calculatePositionFromMotorAngles() // Motorwinkel → Workspace (schreibt auf this.*)
``` ```
- [ ] `robot/RobotBase.js` anlegen — generische Infrastruktur aus `Robot.js` - [x] `robot/RobotBase.js` anlegen — generische Infrastruktur aus `Robot.js`
- [ ] Beide Kinematik-Methoden in `RobotBase` als Stub mit `throw new Error('not implemented')` - [x] Beide Kinematik-Methoden in `RobotBase` als Stub mit `throw new Error('not implemented')`
- [ ] JSDoc: Interface-Vertrag dokumentieren - [x] JSDoc: Interface-Vertrag dokumentieren
- [ ] `rotateAroundAxis()` wandert in `RobotBase` als geschützte Hilfsmethode - [x] `rotateAroundAxis()` wandert in `RobotBase` als geschützte Hilfsmethode
--- ---
@@ -101,16 +101,16 @@ robot/
└── Arm3SegmentLinearX.js ← bisheriger Robot.js-Kinematik-Teil └── Arm3SegmentLinearX.js ← bisheriger Robot.js-Kinematik-Teil
``` ```
- [ ] `robot/kinematics/Arm3SegmentLinearX.js` anlegen - [x] `robot/kinematics/Arm3SegmentLinearX.js` anlegen
- `class Arm3SegmentLinearX extends RobotBase` - `class Arm3SegmentLinearX extends RobotBase`
- Konstruktor: `constructor(l1, l2, l3)` → `super()` + Längen - Konstruktor: `constructor(l1, l2, l3)` → `super()` + Längen
- `calculateAngles3D()` — unverändert übernommen - `calculateAngles3D()` — unverändert übernommen
- `calculatePositionFromMotorAngles()` — 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 ```js
module.exports = require('./kinematics/Arm3SegmentLinearX'); 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) (`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_PARAMS: '{"l1": 250, "l2": 264, "l3": 100}'
``` ```
- [ ] `ROBOT_KINEMATICS` — Bezeichner der Kinematik-Klasse (Default: `arm3segmentlinearx`) - [x] `ROBOT_KINEMATICS` — Bezeichner der Kinematik-Klasse (Default: `arm3segmentlinearx`)
- [ ] `ROBOT_KINEMATICS_PARAMS` — JSON mit Konstruktor-Parametern - [x] `ROBOT_KINEMATICS_PARAMS` — JSON mit Konstruktor-Parametern
- [ ] `KinematicsFactory.js` oder direkt in `startRobot.js`: - [x] `KinematicsFactory.js` (`createRobotFromEnv`), eingebunden in `startRobot.js`:
```js ```js
const kin = loadKinematics(process.env.ROBOT_KINEMATICS, params); const robot = createRobotFromEnv(processEnv, { l1: 250, l2: 264, l3: 100 });
const robot = new kin(params.l1, params.l2, params.l3);
``` ```
- [ ] 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`) - [ ] 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.
--- ---

189
doc/ToDo_9_HardwareFeedback.md
View File

@@ -43,6 +43,195 @@ WebSocket → GCode → calculateAngles3D → sendCommand → tSocket.write()
- [ ] Status (`Idle`, `Run`, `Alarm`, `Hold`) für den `InfoServer` bereitstellen - [ ] Status (`Idle`, `Run`, `Alarm`, `Hold`) für den `InfoServer` bereitstellen
- `/api/status` um GRBL-Zustand erweitern - `/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 ## 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. `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.

4
docker-compose.yaml
View File

@@ -26,6 +26,10 @@ services:
- GRBL_BASE_IP=192.168.0.183 - GRBL_BASE_IP=192.168.0.183
- GRBL_ELLBOW_IP=192.168.0.202 - GRBL_ELLBOW_IP=192.168.0.202
- GRBL_HAND_IP=192.168.0.250 - 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: ports:
- "2096:2095" - "2096:2095"
- "2098:2098" - "2098:2098"

10
logs/gcode_commands.log
View File

@@ -10339,3 +10339,13 @@
2026-06-09T10:41:44.720Z ::ffff:127.0.0.1: M114 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: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-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

4
logs/pings.log
View File

@@ -14610,3 +14610,7 @@
2026-06-09T10:39:57.863Z ::ffff:127.0.0.1 : Ping 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.479Z ::ffff:127.0.0.1 : Ping
2026-06-09T10:41:44.499Z ::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

76
robot/KinematicsFactory.js Normal file
View File

@@ -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,
};

376
robot/Robot.js
View File

@@ -1,369 +1,9 @@
const MotorPosition = require('./RobotMotorPosition.js') // robot/Robot.js — Kompatibilitäts-Alias (siehe doc/ToDo_12_InverseKinematikConfig_ROADMAP.md).
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 // Externer Code (und Tests) kann weiterhin `require('./robot/Robot')` schreiben.
// wobei n = Unterarm x (P-O) ist // Während der Übergangsperiode zeigt der Alias auf die konkrete Default-Kinematik
var nX = Math.cos(this.beta)*Math.cos(this.theta) - Math.sin(this.theta)*Math.sin(this.phi)*Math.sin(this.beta); // `Arm3SegmentLinearX`, damit `new Robot(l1, l2, l3)` unverändert funktioniert.
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); // Die generische Basisklasse / der Interface-Vertrag liegt in `RobotBase.js`;
var nBetrag = Math.sqrt(nX*nX + nY*nY + nZ*nZ); // die produktive Auswahl der Kinematik übernimmt `KinematicsFactory.js`.
module.exports = require('./kinematics/Arm3SegmentLinearX');
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

319
robot/RobotBase.js Normal file
View File

@@ -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

133
robot/kinematics/Arm3SegmentLinearX.js Normal file
View File

@@ -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

12
startRobot.js
View File

@@ -1,6 +1,6 @@
const fs = require('fs'); const fs = require('fs');
const https = require('https'); const https = require('https');
const Robot = require('./robot/Robot'); const { createRobotFromEnv } = require('./robot/KinematicsFactory');
const GCode = require('./robot/GCode'); const GCode = require('./robot/GCode');
const TelnetSender = require('./robot/TelnetSenderGRBL'); const TelnetSender = require('./robot/TelnetSenderGRBL');
@@ -43,7 +43,7 @@ function createApp(options = {}) {
fsModule = fs, fsModule = fs,
httpsModule = https, httpsModule = https,
processEnv = process.env, processEnv = process.env,
RobotClass = Robot, RobotClass = null,
GCodeModule = GCode, GCodeModule = GCode,
TelnetSenderClass = TelnetSender, TelnetSenderClass = TelnetSender,
initInputWSFn = initInputWS, initInputWSFn = initInputWS,
@@ -69,7 +69,13 @@ function createApp(options = {}) {
const httpsServer = httpsModule.createServer(httpsOptions); 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 = { const sharedState = {
connectedClients: [], connectedClients: [],

69
test/KinematicsFactory.test.js Normal file
View File

@@ -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();
});
});