From 933a017e2eacdf32c7f1805cb50c64ca2bf4a889 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Fri, 26 Jun 2026 12:28:40 +0200 Subject: [PATCH] Interrim G28 ohne e --- doc/Info_Koordinaten.md | 26 +++++++++++++++++--------- logs/gcode_commands.log | 27 +++++++++++++++++++++++++++ logs/pings.log | 6 ++++++ robot/RobotConfig.js | 18 +++++++++++++----- robot/RobotController.js | 6 ++++-- test/GCode.receiveGCode.test.js | 10 +++++++--- test/RobotConfig.test.js | 20 +++++++++++++++----- 7 files changed, 89 insertions(+), 24 deletions(-) diff --git a/doc/Info_Koordinaten.md b/doc/Info_Koordinaten.md index 887d89e..3ba2561 100644 --- a/doc/Info_Koordinaten.md +++ b/doc/Info_Koordinaten.md @@ -164,12 +164,17 @@ Fundstellen: 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.) +2. **Greifer-Kopplung — aktuell aktiv (identifiziert):** Bei der realen Verkabelung + (`hand.axes = ['c','e','b']`) liegt der Greifer auf dem **y-Port**. Gesendet wird daher + `mNew.e · D = eMotor · D = (e − b − c) · D` — die Kopplung steckt in + `gripperMotorFromOpening` (→ `eMotor`), der Sender hängt nur noch `·180/π` dran. + - Die **x-Port-Variante** (`e + 1.2·b·D − c·D`, mit `factorTurnLift = 1.2`) greift nur + bei anderer Verkabelung → **derzeit toter Pfad**. (`factorOpenTurn = 1.92` ungenutzt.) + - **Folge / Slam:** bei `b = π` (Phase-1-„gerade Hand") wird `eMotor = e−b−c = −π → −180°` + an den Finger-Motor gesendet → er fährt an den Anschlag und verdreht über die Sehne die + ganze Hand. Phase 2 (`b = 0` = gerade) behebt das automatisch (`eMotor = 0`). + - Aufgabe: Kopplung gegen die echte Sehnenmechanik validieren, toten x-Port-Pfad + + `factorOpenTurn` aufräumen, **Vorzeichen** je nach Motor-Verkabelung prüfen. 3. **B-Konvention (gerade = 0°).** Durchgängig: - FK/IK in `Arm3SegmentLinearX` (b-Definition / acos-Zweig), @@ -184,9 +189,12 @@ Fundstellen: 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. +5. **l3-Ableitung korrigiert** ✅ (`RobotConfig.js`): `l3` kommt jetzt aus **Hand + Finger** + (`|Hand.to[1]| + |FingerA.to[1]|` = 35 + 60 = **95**) statt aus dem Ellbogen-Versatz (90). + Zusätzlich sind `kinematics.l1/l2/l3` in robot.json **explizit überschreibbar** (Vorrang + vor der Ableitung) — zum Kalibrieren auf die gemessene Reichweite. + ⚠️ Geometrie liefert Reichweite 595, beobachtet wurden **~550** → l3 (oder l1/l2) sollte + per `kinematics.l3` explizit kalibriert werden (deutet auf l3 ≈ 50, falls l1=l2=250 stimmen). 6. **Tests + Doku** nachziehen: Round-Trip mit neuer Konvention, Greifer-Kopplung, G92-Referenztabellen in `Info_G92.md`, sowie diese Datei. diff --git a/logs/gcode_commands.log b/logs/gcode_commands.log index 32ea1e3..cb79968 100644 --- a/logs/gcode_commands.log +++ b/logs/gcode_commands.log @@ -11209,3 +11209,30 @@ 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 +2026-06-26T10:18:30.756Z ::ffff:127.0.0.1: FList +2026-06-26T10:18:30.784Z ::ffff:127.0.0.1: FPlus +2026-06-26T10:18:30.808Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T10:18:30.826Z ::ffff:127.0.0.1: FShow +2026-06-26T10:18:31.251Z ::ffff:127.0.0.1: M114 +2026-06-26T10:18:31.274Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T10:18:31.425Z ::ffff:127.0.0.1: M114 +2026-06-26T10:18:31.639Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T10:18:31.880Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T10:26:37.183Z ::ffff:127.0.0.1: FList +2026-06-26T10:26:37.230Z ::ffff:127.0.0.1: FPlus +2026-06-26T10:26:37.250Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T10:26:37.265Z ::ffff:127.0.0.1: FShow +2026-06-26T10:26:37.361Z ::ffff:127.0.0.1: M114 +2026-06-26T10:26:37.382Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T10:26:37.461Z ::ffff:127.0.0.1: M114 +2026-06-26T10:26:37.683Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T10:26:37.905Z ::ffff:127.0.0.1: G1 X1 +2026-06-26T10:26:51.872Z ::ffff:127.0.0.1: FList +2026-06-26T10:26:51.904Z ::ffff:127.0.0.1: M114 +2026-06-26T10:26:51.913Z ::ffff:127.0.0.1: FPlus +2026-06-26T10:26:51.923Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T10:26:51.928Z ::ffff:127.0.0.1: FLoad nichtda +2026-06-26T10:26:51.947Z ::ffff:127.0.0.1: FShow +2026-06-26T10:26:52.112Z ::ffff:127.0.0.1: M114 +2026-06-26T10:26:52.341Z ::ffff:127.0.0.1: G1 X1 Y2 Z3 +2026-06-26T10:26:52.567Z ::ffff:127.0.0.1: G1 X1 diff --git a/logs/pings.log b/logs/pings.log index 6e73b6b..4145d29 100644 --- a/logs/pings.log +++ b/logs/pings.log @@ -14852,3 +14852,9 @@ 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 +2026-06-26T10:18:31.191Z ::ffff:127.0.0.1 : Ping +2026-06-26T10:18:31.221Z ::ffff:127.0.0.1 : Ping +2026-06-26T10:26:37.190Z ::ffff:127.0.0.1 : Ping +2026-06-26T10:26:37.324Z ::ffff:127.0.0.1 : Ping +2026-06-26T10:26:51.865Z ::ffff:127.0.0.1 : Ping +2026-06-26T10:26:51.878Z ::ffff:127.0.0.1 : Ping diff --git a/robot/RobotConfig.js b/robot/RobotConfig.js index 1ba4473..715594a 100644 --- a/robot/RobotConfig.js +++ b/robot/RobotConfig.js @@ -27,10 +27,16 @@ function deriveKinematicParams(links) { const result = {}; const l1raw = links.Arm1?.skeleton?.to?.[1]; const l2raw = links.Arm2?.skeleton?.to?.[1]; - const l3raw = links.Ellbow?.skeleton?.to?.[0]; if (l1raw != null) result.l1 = Math.abs(l1raw); if (l2raw != null) result.l2 = Math.abs(l2raw); - if (l3raw != null) result.l3 = l3raw; + // l3 = Endeffektor-Länge (Handgelenk → Fingerspitze) = Hand-Segment + Finger. + // FRÜHER fälschlich aus Ellbow.skeleton.to[0] abgeleitet — das ist der seitliche + // Ellbogen-Versatz, nicht die Hand/Finger-Länge (siehe doc/Info_Koordinaten.md, Phase 2). + const handLen = links.Hand?.skeleton?.to?.[1]; // Hand-Segment (z. B. -35) + const fingerLen = links.FingerA?.skeleton?.to?.[1]; // Finger (z. B. -60) + if (handLen != null || fingerLen != null) { + result.l3 = Math.abs(handLen ?? 0) + Math.abs(fingerLen ?? 0); + } return result; } @@ -58,11 +64,13 @@ function load(fsModule, processEnv, consoleObj) { // Kinematik-Typ und Armlängen (aus links abgeleitet) const linkParams = deriveKinematicParams(json?.links); + // Explizite kinematics.l1/l2/l3 in robot.json haben Vorrang vor der links-Ableitung + // (zum Kalibrieren der realen Längen, z. B. l3 an die gemessene Reichweite anpassen). const kinematics = { type: json?.kinematics?.type ?? DEFAULTS.kinematics.type, - l1: linkParams.l1 ?? DEFAULTS.kinematics.l1, - l2: linkParams.l2 ?? DEFAULTS.kinematics.l2, - l3: linkParams.l3 ?? DEFAULTS.kinematics.l3 + l1: json?.kinematics?.l1 ?? linkParams.l1 ?? DEFAULTS.kinematics.l1, + l2: json?.kinematics?.l2 ?? linkParams.l2 ?? DEFAULTS.kinematics.l2, + l3: json?.kinematics?.l3 ?? linkParams.l3 ?? DEFAULTS.kinematics.l3 }; // Bewegungs-Defaults — Env hat Vorrang, dann robot.json, dann Hard-Default diff --git a/robot/RobotController.js b/robot/RobotController.js index 5f58d84..bf77437 100644 --- a/robot/RobotController.js +++ b/robot/RobotController.js @@ -69,8 +69,10 @@ class RobotController { 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); + // Greifer (e/eMotor) bewusst UNANGETASTET lassen: G28 fährt nur den Arm. + // Würde man e=0 setzen, ergäbe die Kopplung eMotor = e − b − c bei b=π den + // Wert −π → −180° am Finger-Motor → Anschlag-Slam (siehe + // doc/Info_Koordinaten.md, Phase 2). Phase 2 (b=0 = gerade) löst das sauber. robot.calculatePositionFromMotorAngles(); // FK -> x=0, y=-(l1+l2+l3), z=0 } else { // Allgemeiner (nicht-singulärer) Home-Punkt: regulär über die IK. diff --git a/test/GCode.receiveGCode.test.js b/test/GCode.receiveGCode.test.js index 0a1d096..6cb1dcd 100644 --- a/test/GCode.receiveGCode.test.js +++ b/test/GCode.receiveGCode.test.js @@ -90,19 +90,23 @@ describe('GCode.receiveGCode', () => { expect(robot.sendCommand).toHaveBeenCalled() }) - test('G28 setzt Home-Motorwerte direkt (Singularität) und löst Bewegung aus', () => { + test('G28 setzt Home-Motorwerte direkt (Singularität), lässt aber den Greifer unangetastet', () => { const robot = createDummyRobot() + robot.e = 7 // vorher gesetzte Greifer-Öffnung ... + robot.eMotor = 3 // ... darf von G28 NICHT bewegt werden (sonst e−b−c-Slam bei b=π) GCode.receiveGCode(robot, 'G28') - // Voll ausgestreckt = Handgelenk-Singularität -> Motorwerte DIREKT, dann FK (nicht IK). + // Voll ausgestreckt = Handgelenk-Singularität -> Arm-Motorwerte DIREKT, dann FK (nicht IK). expect(robot.xMotor).toBe(0) expect(robot.alpha).toBe(0) expect(robot.beta).toBe(0) expect(robot.a).toBe(0) expect(robot.b).toBe(Math.PI) // gerade Hand (Phase-1-Konvention) expect(robot.c).toBe(0) - expect(robot.e).toBe(0) + // Greifer unverändert -> kein Finger-Slam am Anschlag + expect(robot.e).toBe(7) + expect(robot.eMotor).toBe(3) expect(robot.calculateAngles3D).not.toHaveBeenCalled() expect(robot.calculatePositionFromMotorAngles).toHaveBeenCalledTimes(1) expect(robot.sendCommand).toHaveBeenCalledTimes(1) diff --git a/test/RobotConfig.test.js b/test/RobotConfig.test.js index 92613fa..e228773 100644 --- a/test/RobotConfig.test.js +++ b/test/RobotConfig.test.js @@ -24,9 +24,11 @@ const FULL_ROBOT_JSON = { hand: { ip: 'fluidNcHand.local', port: 5000, protocol: 'telnet', axes: ['c', 'e', 'b'] } }, links: { - Arm1: { skeleton: { from: [0,0,0], to: [0,-250,0] } }, - Arm2: { skeleton: { from: [0,0,0], to: [0,-250,0] } }, - Ellbow: { skeleton: { from: [0,0,0], to: [90,0,0] } } + Arm1: { skeleton: { from: [0,0,0], to: [0,-250,0] } }, + Arm2: { skeleton: { from: [0,0,0], to: [0,-250,0] } }, + Ellbow: { skeleton: { from: [0,0,0], to: [90,0,0] } }, + Hand: { skeleton: { from: [0,0,0], to: [0,-35,0] } }, + FingerA: { skeleton: { from: [0,0,0], to: [0,-60,0] } } } }; @@ -45,8 +47,16 @@ describe('RobotConfig.load — Vollständige robot.json', () => { expect(cfg.kinematics.l2).toBe(250); }); - test('l3 aus links.Ellbow.skeleton.to[0]', () => { - expect(cfg.kinematics.l3).toBe(90); + test('l3 aus Hand + Finger (Handgelenk → Fingerspitze), NICHT Ellbow-Versatz', () => { + expect(cfg.kinematics.l3).toBe(95); // |Hand.to[1]|=35 + |FingerA.to[1]|=60 + }); + + test('kinematics.l1/l2/l3 explizit in robot.json überschreiben die links-Ableitung', () => { + const json = { ...FULL_ROBOT_JSON, kinematics: { type: 'arm3segmentlinearx', l1: 260, l2: 270, l3: 50 } }; + const c = load(makeFs(JSON.stringify(json)), {}, log); + expect(c.kinematics.l1).toBe(260); + expect(c.kinematics.l2).toBe(270); + expect(c.kinematics.l3).toBe(50); }); test('motion.defaultFeedrate aus robot.json', () => {