131 lines
5.1 KiB
Markdown
131 lines
5.1 KiB
Markdown
# 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.
|