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

G28 Singularität und Planungen

Browse Source
This commit is contained in:
chk
2026-06-26 11:39:20 +02:00
parent bd1752f567
commit 7639266170
6 changed files with 232 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
doc/Info_Koordinaten.md
View File

@@ -122,34 +122,95 @@ Umgesetzt:
- **Spiegelung an der x-z-Ebene** in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`): die - **Spiegelung an der x-z-Ebene** in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`): die
interne Mathematik (`_ikPlusY`/`_fkPlusY`) rechnet weiter in +y, die öffentlichen interne Mathematik (`_ikPlusY`/`_fkPlusY`) rechnet weiter in +y, die öffentlichen
Methoden spiegeln die Workspace-Pose (y, pY, φ, ψ; θ bleibt) → α=0 zeigt nach −y. Methoden spiegeln die Workspace-Pose (y, pY, φ, ψ; θ bleibt) → α=0 zeigt nach −y.
- **G28** (Home) auf −y umgestellt: `y = -(l1+l2+l3)`, `phi = +π/2`. - **G28** (Home) auf −y umgestellt **und Singularität behandelt**: die voll ausgestreckte
Stellung (`|y| = l1+l2+l3`) ist eine Handgelenk-Singularität, in der die IK `a`/`c` nicht
bestimmen kann (Müll wie `a=135°, c=45°` → Finger schräg). G28 setzt dort die Motorwerte
**direkt** (`alpha=beta=a=c=0`, `b=π` = gerade Hand) und füllt die Pose per FK.
- **Tests:** `test/Robot.Kinematics.NegativeY.test.js` (Grundstellung −590, Homing-Pose - **Tests:** `test/Robot.Kinematics.NegativeY.test.js` (Grundstellung −590, Homing-Pose
in −y, Round-Trip in −y, a=0 → Knick-Achse ∥ x). in −y, Round-Trip in −y, a=0 → Knick-Achse ∥ x).
- **Migration:** `Robot.02_UpperArm` und der G28-Test auf −y umgestellt. - **Migration:** `Robot.02_UpperArm` und der G28-Test auf −y umgestellt.
- **Doku:** `doc/Info_G92.md` Y/Z (und C/A nach der Spiegelung) aktualisiert. - **Doku:** `doc/Info_G92.md` Y/Z (und C/A nach der Spiegelung) aktualisiert.
### Phase 2 — Handgelenk-/Finger-Nullstellung (B, C) — OFFEN ### Phase 2 — Handgelenk-/Finger-Nullstellung (B, C, Greifer) — OFFEN
> **Voraussetzung (User):** Erst Visualisierung/Überprüfung der Finger, dort werden noch > **Voraussetzung (User):** Erst die Finger visualisieren/prüfen — dort werden noch Fehler
> Fehler vermutet. **a-Achse ist bereits korrekt** (a=0 → Knick-Achse ∥ x) — Phase 2 > vermutet. **a-Achse ist bereits korrekt** (a=0 → Knick-Achse ∥ x). Phase 2 betrifft
> betrifft nur **B (Knick)**, **C (Roll)** und die **Greifer-Kopplung**. > **B (Knick)**, **C (Roll)** und die **Greifer-Kopplung**.
1. **Finger/Hand visualisieren** und gegen die echte Mechanik prüfen (User). **Ziel-Konvention:** gerade Hand → **B = 0°** (statt 180°); neutraler Roll → **C = 0°**;
2. **B-Konvention:** gerade Hand soll **0°** sein (derzeit 180°; physischer Knick = 180° − B). Greifer-Kopplung konsistent und gegen die echte Mechanik kalibriert.
Mapping festlegen (z.B. `b → 180° − b` an der Schnittstelle) und FK/IK anpassen.
3. **C-Nullpunkt:** neutral soll **0°** sein (derzeit `ψ = 90° − C`). #### Vorab-Erkenntnis aus dem Code (wichtig!)
⚠️ Der C↔ψ-Bezug ist **posenabhängig** (`acos(cos β · sin a)`); ein global sauberes
`c=0 = neutral` braucht ggf. eine tiefere Umparametrierung — **vor** der Umsetzung bewerten. Die b/c/e-Konvention ist an **mehreren** Stellen kodiert, die **gemeinsam** geändert werden
4. **Greifer-Kopplung** `eMotor = e − b − c` prüfen: mischt mm (e) mit Radiant (b, c) müssen. **Invariante:** solange die Hardware-Nullpunkte nicht neu kalibriert werden, müssen
— gegen die echte Sehnen-Mechanik validieren. die an FluidNC gesendeten Port-Werte **gleich bleiben** — eine reine Modell-Umbenennung darf
5. Tests + `Info_G92.md` nachziehen. die Hardware-Bewegung nicht verändern.
Fundstellen:
| Datei / Stelle | aktuelle Kodierung |
|----------------|--------------------|
| `Arm3SegmentLinearX._fkPlusY` | `vHand = rotate(vecUnterarm, n, b)`; `psi = c − acos(−n.z)` → **b=π = gerade** |
| `Arm3SegmentLinearX._ikPlusY` | `b = acos(cosB)` (∈[0,π]); `c = acos(cosC) + psi` → **c hat posenabh. Offset** |
| `Arm3SegmentLinearX.gripperMotorFromOpening` | `eMotor = e − b − c` (b,c in **rad**) — **Greifer-Kopplung #1** |
| `RobotController` G92/M92 | `b = B/D`, `c = C/D`, `eMotor = gripperMotorFromOpening(e)` |
| `RobotController` M1 | `b += B`, `c += C` (relativer Motor-Jog) |
| `RobotController` G28 | `b = π`, `c = 0` (Phase 1) → nach B-Umstellung auf `b = 0` ändern |
| `portInverse.js` | `b = hand.z/D`, `c = hand.x/D + b` (Port→Motor, Hardware-Sync) |
| `TelnetSenderGRBL.execCommand` / `portValue` | Hand-Ports: `z = b·D`, `x = (c−b)·D`; **e-Port mit `factorTurnLift=1.2`** — **Greifer-Kopplung #2** |
#### Aufgaben
1. **Finger visualisieren** (User) → Soll-Bild, gegen das kalibriert wird.
2. **Greifer-Kopplung vereinheitlichen.** Es existieren **zwei widersprüchliche** Kopplungen:
- Kinematik: `eMotor = e − b − c` (b,c in rad)
- Sender: `e-Port = e + 1.2·b·D − c·D` (b,c in Grad, Faktor **1.2** nur auf b)
Eine Quelle der Wahrheit festlegen und gegen die echte Sehnenmechanik messen.
(`factorOpenTurn = 1.92` im Sender ist deklariert, aber **ungenutzt** → klären/entfernen.)
3. **B-Konvention (gerade = 0°).** Durchgängig:
- FK/IK in `Arm3SegmentLinearX` (b-Definition / acos-Zweig),
- `gripperMotorFromOpening` nachziehen,
- G92-Eingabe (`b = B/D`) + M1 + G28,
- `portInverse.js` (Umkehrung),
- **Sender-Formeln so kompensieren, dass die FluidNC-Ports unverändert bleiben** —
ODER bewusst die Hardware-Nullpunkte neu kalibrieren (Entscheidung dokumentieren).
4. **C-Nullpunkt (neutral = 0°).** Der `c↔ψ`-Bezug ist **posenabhängig**
(`ψ = acos(cos β · sin a) − c`). Ein konstantes `c=0=neutral` ist **nicht global** möglich,
ohne die Hand-Parametrierung zu ändern. Bewerten: c als reinen Gelenkwinkel führen
(Offset herausrechnen) oder die ψ-Definition anpassen.
5. **l3-Ableitung korrigieren** (`RobotConfig.js`): `l3` kommt aus
`Ellbow.skeleton.to[0] = 90` (Ellbogen-Versatz), **nicht** aus der echten Hand-/Finger-
Länge — das erklärt die beobachteten −550 statt −590. Aus der echten Finger-Geometrie ableiten.
6. **Tests + Doku** nachziehen: Round-Trip mit neuer Konvention, Greifer-Kopplung,
G92-Referenztabellen in `Info_G92.md`, sowie diese Datei.
#### Ansatz-Entscheidung (vor Umsetzung)
- **Klein/lokal:** nur die **G92-Eingabe** umrechnen (appRobotHoming sendet B=0/C=0 für
gerade/neutral, Driver mappt intern auf die alte Konvention). Wenig Risiko, aber das
interne Modell bleibt „unsauber".
- **Groß/sauber:** interne Konvention durchgängig umstellen (alle Fundstellen oben) mit
Hardware-Port-Invariante. Sauberes All-Zero-Home, aber koordinierter Eingriff.
#### Verifikation
Jede B/C/Greifer-Änderung gegen **Visualisierung UND einen Hardware-Test** (eine Achse
isoliert) prüfen — das Modell allein genügt hier nicht, weil es um die Hardware-Abbildung geht.
### Verifikation (Definition of Done) ### Verifikation (Definition of Done)
- **Phase 1 (erfüllt):** G92 der Grundstellung → Driver `y ≈ −590, z ≈ 0`; appRobotHoming - **Phase 1 (erfüllt):** G92 der Grundstellung → Driver `y ≈ −590, z ≈ 0`; appRobotHoming
sendet die gemessenen α/β/a **direkt** (ohne Spiegelung); volle Suite grün. sendet die gemessenen α/β/a **direkt** (ohne Spiegelung); **G28 fährt sauber gestreckt
- **Phase 2 (Ziel):** Grundstellung mit **allen** Gelenkwinkeln 0 (inkl. B=C=0); Finger nach −y** (a=0, kein Singularitäts-Müll); volle Suite grün.
visuell korrekt. - **Phase 2 (Ziel):** Grundstellung mit **allen** Gelenkwinkeln 0 (inkl. B=C=0); Greifer-
Kopplung vereinheitlicht; Finger visuell korrekt.
--- ---
@@ -157,6 +218,8 @@ Umgesetzt:
- **y-Flip (Phase 1):** Spiegelung in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`, genutzt von - **y-Flip (Phase 1):** Spiegelung in `Arm3SegmentLinearX` (`_mirrorWorkspaceY`, genutzt von
`calculateAngles3D` und `calculatePositionFromMotorAngles`). Am Roboter bestätigt. `calculateAngles3D` und `calculatePositionFromMotorAngles`). Am Roboter bestätigt.
- **G28-Singularität (Phase 1):** voll ausgestreckt setzt `RobotController` die Motorwerte
direkt (statt der singulären IK) → Finger sauber entlang −y.
- **atan2-Fix** in der IK (`gamma = Math.atan2(pZ, pY)`): macht die interne IK für −y - **atan2-Fix** in der IK (`gamma = Math.atan2(pZ, pY)`): macht die interne IK für −y
mathematisch korrekt — Voraussetzung des y-Flips. mathematisch korrekt — Voraussetzung des y-Flips.
- **Winkel-Konventionen** (Y/Z/A/B/C/E) sind in [doc/Info_G92.md](Info_G92.md) dokumentiert - **Winkel-Konventionen** (Y/Z/A/B/C/E) sind in [doc/Info_G92.md](Info_G92.md) dokumentiert

72
logs/gcode_commands.log
View File

@@ -11137,3 +11137,75 @@
2026-06-26T08:24:40.035Z ::ffff:127.0.0.1: M114 2026-06-26T08:24:40.035Z ::ffff:127.0.0.1: M114
2026-06-26T08:24:40.264Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 2026-06-26T08:24:40.264Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T08:24:40.497Z ::ffff:127.0.0.1: G1 X1 2026-06-26T08:24:40.497Z ::ffff:127.0.0.1: G1 X1
2026-06-26T09:26:48.688Z ::ffff:127.0.0.1: FList
2026-06-26T09:26:48.711Z ::ffff:127.0.0.1: M114
2026-06-26T09:26:48.729Z ::ffff:127.0.0.1: FPlus
2026-06-26T09:26:48.732Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:26:48.744Z ::ffff:127.0.0.1: FLoad nichtda
2026-06-26T09:26:48.758Z ::ffff:127.0.0.1: FShow
2026-06-26T09:26:48.922Z ::ffff:127.0.0.1: M114
2026-06-26T09:26:49.142Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:26:49.372Z ::ffff:127.0.0.1: G1 X1
2026-06-26T09:27:11.069Z ::ffff:127.0.0.1: FList
2026-06-26T09:27:11.100Z ::ffff:127.0.0.1: M114
2026-06-26T09:27:11.120Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:27:11.122Z ::ffff:127.0.0.1: FPlus
2026-06-26T09:27:11.140Z ::ffff:127.0.0.1: FLoad nichtda
2026-06-26T09:27:11.157Z ::ffff:127.0.0.1: FShow
2026-06-26T09:27:11.295Z ::ffff:127.0.0.1: M114
2026-06-26T09:27:11.517Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:27:11.744Z ::ffff:127.0.0.1: G1 X1
2026-06-26T09:27:19.593Z ::ffff:127.0.0.1: FList
2026-06-26T09:27:19.607Z ::ffff:127.0.0.1: M114
2026-06-26T09:27:19.624Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:27:19.627Z ::ffff:127.0.0.1: FPlus
2026-06-26T09:27:19.639Z ::ffff:127.0.0.1: FLoad nichtda
2026-06-26T09:27:19.650Z ::ffff:127.0.0.1: FShow
2026-06-26T09:27:19.814Z ::ffff:127.0.0.1: M114
2026-06-26T09:27:20.029Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:27:20.258Z ::ffff:127.0.0.1: G1 X1
2026-06-26T09:27:36.334Z ::ffff:127.0.0.1: FList
2026-06-26T09:27:36.366Z ::ffff:127.0.0.1: M114
2026-06-26T09:27:36.367Z ::ffff:127.0.0.1: FPlus
2026-06-26T09:27:36.382Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:27:36.387Z ::ffff:127.0.0.1: FLoad nichtda
2026-06-26T09:27:36.401Z ::ffff:127.0.0.1: FShow
2026-06-26T09:27:36.558Z ::ffff:127.0.0.1: M114
2026-06-26T09:27:36.772Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:27:37.001Z ::ffff:127.0.0.1: G1 X1
2026-06-26T09:28:07.329Z ::ffff:127.0.0.1: M114
2026-06-26T09:28:07.361Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:28:07.445Z ::ffff:127.0.0.1: M114
2026-06-26T09:28:07.499Z ::ffff:127.0.0.1: FList
2026-06-26T09:28:07.549Z ::ffff:127.0.0.1: FPlus
2026-06-26T09:28:07.574Z ::ffff:127.0.0.1: FLoad nichtda
2026-06-26T09:28:07.593Z ::ffff:127.0.0.1: FShow
2026-06-26T09:28:07.692Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:28:07.941Z ::ffff:127.0.0.1: G1 X1
2026-06-26T09:28:10.419Z ::ffff:127.0.0.1: M114
2026-06-26T09:28:10.422Z ::ffff:127.0.0.1: FList
2026-06-26T09:28:10.444Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:28:10.464Z ::ffff:127.0.0.1: FPlus
2026-06-26T09:28:10.477Z ::ffff:127.0.0.1: FLoad nichtda
2026-06-26T09:28:10.485Z ::ffff:127.0.0.1: FShow
2026-06-26T09:28:10.667Z ::ffff:127.0.0.1: M114
2026-06-26T09:28:10.883Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:28:11.112Z ::ffff:127.0.0.1: G1 X1
2026-06-26T09:35:29.613Z ::ffff:127.0.0.1: M114
2026-06-26T09:35:29.633Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:35:29.819Z ::ffff:127.0.0.1: M114
2026-06-26T09:35:29.908Z ::ffff:127.0.0.1: FList
2026-06-26T09:35:29.947Z ::ffff:127.0.0.1: FPlus
2026-06-26T09:35:29.964Z ::ffff:127.0.0.1: FLoad nichtda
2026-06-26T09:35:29.991Z ::ffff:127.0.0.1: FShow
2026-06-26T09:35:30.059Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:35:30.309Z ::ffff:127.0.0.1: G1 X1
2026-06-26T09:35:33.075Z ::ffff:127.0.0.1: FList
2026-06-26T09:35:33.109Z ::ffff:127.0.0.1: FPlus
2026-06-26T09:35:33.125Z ::ffff:127.0.0.1: FLoad nichtda
2026-06-26T09:35:33.138Z ::ffff:127.0.0.1: FShow
2026-06-26T09:35:33.254Z ::ffff:127.0.0.1: M114
2026-06-26T09:35:33.271Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:35:33.429Z ::ffff:127.0.0.1: M114
2026-06-26T09:35:33.652Z ::ffff:127.0.0.1: G1 X1 Y2 Z3
2026-06-26T09:35:33.884Z ::ffff:127.0.0.1: G1 X1

16
logs/pings.log
View File

@@ -14836,3 +14836,19 @@
2026-06-26T06:25:53.087Z ::ffff:127.0.0.1 : Ping 2026-06-26T06:25:53.087Z ::ffff:127.0.0.1 : Ping
2026-06-26T08:24:39.787Z ::ffff:127.0.0.1 : Ping 2026-06-26T08:24:39.787Z ::ffff:127.0.0.1 : Ping
2026-06-26T08:24:39.805Z ::ffff:127.0.0.1 : Ping 2026-06-26T08:24:39.805Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:26:48.670Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:26:48.675Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:27:11.052Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:27:11.059Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:27:19.569Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:27:19.582Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:27:36.325Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:27:36.338Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:28:07.197Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:28:07.297Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:28:10.358Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:28:10.433Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:35:29.584Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:35:29.593Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:35:33.181Z ::ffff:127.0.0.1 : Ping
2026-06-26T09:35:33.224Z ::ffff:127.0.0.1 : Ping

27
robot/RobotController.js
View File

@@ -52,16 +52,37 @@ class RobotController {
} }
if (cmd === 'G28') { if (cmd === 'G28') {
// Home = Grundstellung: Arm voll ausgestreckt entlang -y (siehe // Home = Grundstellung: Arm + Hand gestreckt entlang -y (siehe
// doc/Info_Koordinaten.md). y und phi in der -y-Konvention. // doc/Info_Koordinaten.md). Ziel-Fingerspitze: (0, -(l1+l2+l3), 0).
const reach = robot.l1 + robot.l2 + robot.l3;
const homeY = -reach;
if (Math.abs(Math.abs(homeY) - reach) < 1e-6) {
// Sonderfall voll ausgestreckt: |y| = l1+l2+l3 ist eine Handgelenk-
// Singularität — die IK kann den Unterarm-Dreher a (und damit c) dort
// nicht bestimmen und liefert Müll (z.B. a=135°, c=45°), wodurch der
// Finger schräg/nach unten zeigt. Daher die Motorwerte DIREKT in die
// Grundstellung setzen und die Workspace-Pose per FK füllen.
robot.xMotor = 0;
robot.alpha = 0;
robot.beta = 0;
robot.a = 0;
robot.b = Math.PI; // gerade Hand (Phase-1-Konvention; Phase 2: 0)
robot.c = 0;
robot.e = 0;
robot.eMotor = robot.gripperMotorFromOpening(robot.e);
robot.calculatePositionFromMotorAngles(); // FK -> x=0, y=-(l1+l2+l3), z=0
} else {
// Allgemeiner (nicht-singulärer) Home-Punkt: regulär über die IK.
robot.x = 0; robot.x = 0;
robot.y = -(robot.l1 + robot.l2 + robot.l3); robot.y = homeY;
robot.z = 0; robot.z = 0;
robot.phi = Math.PI / 2; robot.phi = Math.PI / 2;
robot.theta = Math.PI / 2; robot.theta = Math.PI / 2;
robot.psi = 0; robot.psi = 0;
robot.e = 0; robot.e = 0;
robot.calculateAngles3D(); robot.calculateAngles3D();
}
robot.sendCommand(); robot.sendCommand();
return; return;
} }

18
test/GCode.receiveGCode.test.js
View File

@@ -90,17 +90,21 @@ describe('GCode.receiveGCode', () => {
expect(robot.sendCommand).toHaveBeenCalled() expect(robot.sendCommand).toHaveBeenCalled()
}) })
test('G28 setzt Home-Position und löst Bewegung aus', () => { test('G28 setzt Home-Motorwerte direkt (Singularität) und löst Bewegung aus', () => {
const robot = createDummyRobot() const robot = createDummyRobot()
GCode.receiveGCode(robot, 'G28') GCode.receiveGCode(robot, 'G28')
expect(robot.x).toBe(0) // Voll ausgestreckt = Handgelenk-Singularität -> Motorwerte DIREKT, dann FK (nicht IK).
expect(robot.z).toBe(0) expect(robot.xMotor).toBe(0)
expect(robot.y).toBe(-(robot.l1 + robot.l2 + robot.l3)) // -y Grundstellung expect(robot.alpha).toBe(0)
expect(robot.phi).toBeCloseTo(Math.PI / 2) expect(robot.beta).toBe(0)
expect(robot.theta).toBeCloseTo(Math.PI / 2) expect(robot.a).toBe(0)
expect(robot.calculateAngles3D).toHaveBeenCalledTimes(1) expect(robot.b).toBe(Math.PI) // gerade Hand (Phase-1-Konvention)
expect(robot.c).toBe(0)
expect(robot.e).toBe(0)
expect(robot.calculateAngles3D).not.toHaveBeenCalled()
expect(robot.calculatePositionFromMotorAngles).toHaveBeenCalledTimes(1)
expect(robot.sendCommand).toHaveBeenCalledTimes(1) expect(robot.sendCommand).toHaveBeenCalledTimes(1)
}) })

22
test/Robot.Kinematics.NegativeY.test.js
View File

@@ -1,6 +1,7 @@
// Phase 1: Der reale Roboter arbeitet in -Y (robot.json: Arm1 -> [0,-250,0]). // Phase 1: Der reale Roboter arbeitet in -Y (robot.json: Arm1 -> [0,-250,0]).
// alpha=0 muss nach -y zeigen, nicht nach +y. Siehe doc/Info_Koordinaten.md. // alpha=0 muss nach -y zeigen, nicht nach +y. Siehe doc/Info_Koordinaten.md.
const Robot = require('../robot/kinematics/Arm3SegmentLinearX'); const Robot = require('../robot/kinematics/Arm3SegmentLinearX');
const GCode = require('../robot/GCode');
const D = 180 / Math.PI; const D = 180 / Math.PI;
describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => { describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => {
@@ -82,4 +83,25 @@ describe('Phase 1 — Arm arbeitet in -Y (alpha=0 zeigt nach -y)', () => {
const r90 = fkFromMotors(0, 0, 90, 135, 0, xM); const r90 = fkFromMotors(0, 0, 90, 135, 0, xM);
expect(Math.abs(r90.x - xM)).toBeGreaterThan(1); expect(Math.abs(r90.x - xM)).toBeGreaterThan(1);
}); });
test('G28 Home: saubere Grundstellung (a=0, c=0, Finger entlang -y) — keine Singularitaets-Garbage', () => {
const robot = new Robot(L1, L2, L3);
GCode.receiveGCode(robot, 'G28');
// Motorwerte sauber gesetzt (nicht der IK-Singularitaets-Muell a=135/c=45)
expect(robot.a).toBeCloseTo(0, 9);
expect(robot.c).toBeCloseTo(0, 9);
expect(robot.alpha).toBeCloseTo(0, 9);
expect(robot.beta).toBeCloseTo(0, 9);
// Workspace: voll ausgestreckt entlang -y
expect(robot.x).toBeCloseTo(0, 6);
expect(robot.y).toBeCloseTo(-(L1 + L2 + L3), 6);
expect(robot.z).toBeCloseTo(0, 6);
// Finger (Handgelenk -> Fingerspitze) zeigt nach -y
const hx = robot.x - robot.pX, hy = robot.y - robot.pY, hz = robot.z - robot.pZ;
const n = Math.hypot(hx, hy, hz);
expect(hy / n).toBeCloseTo(-1, 6);
});
}); });