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