G92 senden besser

2026-06-25 17:34:41 +02:00
parent 7818604c02
commit da2a5d5ae6
6 changed files with 54 additions and 43 deletions
--- a/doc/Homing.md
+++ b/doc/Homing.md
@@ -85,8 +85,9 @@ X-Position aus Marker-Positionen schätzen
        │    → state_Arm2.json
        ▼
 4b_revolute_angle.py  --link Hand   --from-state state_Arm2.json
-        │    → state_Hand.json  ← accumulated_state enthält x,y,z,a,b
-        │      (c/Palm + e/Greifer werden nicht bestimmt → für G92 als 0 ergänzt)
+        │    → state_Hand.json  ← accumulated_state: x,y,z,a,b (4b-Primärkette)
+        │      Fallback 5_pose_estimation.py liefert alle 7 (…,c,e). Nur bekannte
+        │      Achsen gehen ins G92; wirklich fehlende werden weggelassen.
        ▼
 G92 über Driver-WebSocket (DRIVER_WS_URL) — setzt Motorposition ohne Bewegung
 ```
--- a/docker-compose.yaml
+++ b/docker-compose.yaml
@@ -12,9 +12,14 @@ services:
      - WEBCAM_URL=http://host.docker.internal:8444
      - BODYTRACKER_URL=http://host.docker.internal:8446
      # Driver-WebSocket (Plain-Text-G-Code, self-signed). Homing sendet G92
-      # hierhin (= Motorposition setzen ohne Bewegung). Host-Port lt.
-      # appRobotDriver/doc/API.md: 2096.
-      - DRIVER_WS_URL=wss://host.docker.internal:2096
+      # hierhin (= Motorposition setzen ohne Bewegung).
+      # WICHTIG: Der Input-WS lauscht container-intern auf 2095 (startRobot.js:
+      # PORT||2095) und ist NICHT per host-port veröffentlicht. Die Driver-Ports
+      # 2081 (Node --inspect) und 2098 (Info/Status) sind NICHT dieser WS.
+      # Variante A (robust): beide Container im selben Netz (approbots), per Name:
+      - DRIVER_WS_URL=wss://appRobot_Driver:2095
+      # Variante B (über Host): im Driver `- "2095:2095"` veröffentlichen, dann
+      #   DRIVER_WS_URL=wss://host.docker.internal:2095
    extra_hosts:
      # Macht host.docker.internal auf Linux verfügbar (Standard auf macOS/Windows)
      - "host.docker.internal:host-gateway"
--- a/public/client.js
+++ b/public/client.js
@@ -346,20 +346,16 @@ function setHomingProgress(step, total, text) {
  if (txt) txt.textContent = text || `Schritt ${step} / ${total}`;
 }

-// Schreibt das G92-Kommando ins Eingabefeld.
-//  - progressiv (full=false): nur die bereits bestimmten Achsen, je Gelenk-Update
-//  - final     (full=true):   alle 7 Achsen; fehlende c (Palm) / e (Greifer)
-//                              werden als 0 ergänzt — identisch zu dem, was
-//                              "An Roboter senden" via server/buildG92.cjs sendet.
-function writePartialGCode(state, { full = false } = {}) {
+// Schreibt das G92-Kommando ins Eingabefeld — nur die tatsächlich bestimmten
+// Achsen, identisch zu dem, was "An Roboter senden" via server/buildG92.cjs
+// sendet (fehlende/unbeobachtbare Achsen werden weggelassen, nicht 0-gefüllt).
+function writePartialGCode(state) {
  const axisMap = { x: 'X', y: 'Y', z: 'Z', a: 'A', b: 'B', c: 'C', e: 'E' };
  const parts = [];
  for (const [key, axis] of Object.entries(axisMap)) {
    const num = Number(state[key]);
    if (state[key] != null && Number.isFinite(num)) {
      parts.push(`${axis}${num.toFixed(2)}`);
-    } else if (full) {
-      parts.push(`${axis}0.00`);
    }
  }
  if (!parts.length) return;
@@ -559,9 +555,10 @@ async function runHoming() {
            if (evt.state) {
              _homingState = evt.state;
              showHomingResult(evt.state);
-              // Vollständiges G92 (inkl. C0/E0) ins Feld — exakt das, was
-              // "An Roboter senden" schickt.
-              writePartialGCode(evt.state, { full: true });
+              // Finales G92 ins Feld — auch wenn der Lauf über den Fallback
+              // (5_pose_estimation → analysis 'robot_state' statt 'state_*')
+              // lief und progressiv kein G92 geschrieben wurde.
+              writePartialGCode(evt.state);
              if (btnSend) {
                btnSend.disabled     = false;
                btnSend.style.opacity = '';
--- a/server/buildG92.cjs
+++ b/server/buildG92.cjs
@@ -7,9 +7,15 @@
 * exakt die Homing-Semantik. Die Achsbuchstaben bilden 1:1 auf die Motorachsen
 * ab: X→xMotor, Y→alpha, Z→beta, A→a, B→b, C→c, E→e.
 *
- * Die Homing-Kette (4b: Arm1→y, Ellbow→z, Arm2→a, Hand→b) bestimmt c (Palm) und
- * e (Greifer) nicht. Entscheidung: fehlende Achsen als 0 mitsenden
- * (`fillMissingWithZero`), damit G92 alle 7 Achsen trägt.
+ * Bekannte Achsen werden immer mit ihrem realen Wert gesendet. Welche Achsen
+ * bekannt sind, hängt vom Pfad ab:
+ *   - 5_pose_estimation.py (Fallback) liefert alle 7 (x,y,z,a,b,c,e),
+ *   - die 4b-Primärkette (Arm1→y … Hand→b) liefert nur x,y,z,a,b.
+ * Eine Achse, die wirklich fehlt oder als unbeobachtbar `null` markiert ist,
+ * wird per Default WEGGELASSEN — der Driver lässt nicht genannte Achsen
+ * unverändert (M92 setzt nur Achsen mit endlichem Zahlenwert), statt eine
+ * unbekannte Position fälschlich als 0 zu behaupten. `fillMissingWithZero`
+ * erzwingt bei Bedarf das alte 0-Auffüllen.
 *
 * CommonJS, damit Jest (CJS) und der ESM-Server dieselbe Funktion nutzen
 * (gleiches Muster wie spinNormalize.cjs / homingXEstimate.cjs).
@@ -24,9 +30,9 @@ const AXES = [
 /**
 * @param {Record<string, number|null>} state  flacher Joint-State (accumulated_state)
 * @param {{decimals?: number, fillMissingWithZero?: boolean}} [opts]
- * @returns {string} z.B. "G92 X192.73 Y35.99 Z-30.88 A-1.70 B12.34 C0.00 E0.00"
+ * @returns {string} z.B. "G92 X164.57 Y-2.09 Z60.58 A86.75 B-46.97 C-64.91 E22.59"
 */
-function buildG92(state = {}, { decimals = 2, fillMissingWithZero = true } = {}) {
+function buildG92(state = {}, { decimals = 2, fillMissingWithZero = false } = {}) {
  const parts = [];
  for (const [key, axis] of AXES) {
    const num = Number(state?.[key]);
--- a/server/server.js
+++ b/server/server.js
@@ -918,8 +918,9 @@ app.post('/api/homing/run', async (req, res) => {
 * Baut aus { state: { x, y, z, a, b[, c, e] } } ein G92 und sendet es als
 * Plain-Text-G-Code über den Driver-WebSocket (DRIVER_WS_URL). G92 setzt am
 * Driver die Motorposition ohne Bewegung (intern M92) = Homing.
- * Fehlende Achsen (c/Palm, e/Greifer werden vom Homing nicht bestimmt) werden
- * als 0 mitgesendet (siehe server/buildG92.cjs).
+ * Bekannte Achsen werden real gesendet; wirklich fehlende/unbeobachtbare
+ * Achsen (z.B. c/Palm, e/Greifer in der 4b-Kette) werden weggelassen — der
+ * Driver lässt sie unverändert (siehe server/buildG92.cjs).
 */
 app.post('/api/homing/send-state', async (req, res) => {
  try {
--- a/test/buildG92.test.js
+++ b/test/buildG92.test.js
@@ -2,18 +2,24 @@
 * buildG92.test.js
 * Unit-Tests für server/buildG92.cjs
 *
- * Sichert ab, dass aus dem Homing-State der korrekte G92-String entsteht und —
- * gemäß Entscheidung — fehlende Achsen c (Palm) / e (Greifer) als 0 mitgesendet
- * werden. Achsbuchstaben + Reihenfolge müssen zur Driver-Erwartung passen
+ * Sichert ab, dass aus dem Homing-State der korrekte G92-String entsteht:
+ * bekannte Achsen werden real gesendet, wirklich fehlende/null-Achsen per
+ * Default WEGGELASSEN (Driver lässt sie unverändert). Achsbuchstaben +
+ * Reihenfolge müssen zur Driver-Erwartung passen
 * (X→xMotor, Y→alpha, Z→beta, A→a, B→b, C→c, E→e).
 */

 const { buildG92 } = require('../server/buildG92.cjs');

 describe('buildG92', () => {
-  test('typischer Homing-State (x,y,z,a,b) → c/e als 0 ergänzt, alle 7 Achsen', () => {
+  test('Fallback-State (alle 7 DOF) → alle Achsen mit realem Wert', () => {
+    const state = { x: 164.57045, y: -2.08983, z: 60.58375, a: 86.75125, b: -46.96569, c: -64.90875, e: 22.58589 };
+    expect(buildG92(state)).toBe('G92 X164.57 Y-2.09 Z60.58 A86.75 B-46.97 C-64.91 E22.59');
+  });
+
+  test('4b-Primärkette (nur x,y,z,a,b) → c/e werden weggelassen', () => {
    const state = { x: 192.72935, y: 35.99125, z: -30.87771, a: -1.69522, b: 12.34 };
-    expect(buildG92(state)).toBe('G92 X192.73 Y35.99 Z-30.88 A-1.70 B12.34 C0.00 E0.00');
+    expect(buildG92(state)).toBe('G92 X192.73 Y35.99 Z-30.88 A-1.70 B12.34');
  });

  test('Reihenfolge ist immer x,y,z,a,b,c,e (unabhängig von Key-Reihenfolge)', () => {
@@ -21,28 +27,23 @@ describe('buildG92', () => {
    expect(buildG92(state)).toBe('G92 X3.00 Y6.00 Z5.00 A2.00 B1.00 C7.00 E4.00');
  });

-  test('null- und undefined-Achsen werden als 0 gesendet', () => {
-    const state = { x: 10, y: null, z: undefined, a: 0, b: -0.0 };
-    expect(buildG92(state)).toBe('G92 X10.00 Y0.00 Z0.00 A0.00 B0.00 C0.00 E0.00');
+  test('null/undefined/NaN-Achsen werden weggelassen (keine falsche 0)', () => {
+    const state = { x: 10, y: null, z: undefined, a: 0, b: NaN, c: 'abc' };
+    expect(buildG92(state)).toBe('G92 X10.00 A0.00');
  });

-  test('fillMissingWithZero=false lässt fehlende Achsen weg', () => {
+  test('fillMissingWithZero=true füllt fehlende Achsen wieder mit 0', () => {
    const state = { x: 10, y: 20 };
-    expect(buildG92(state, { fillMissingWithZero: false })).toBe('G92 X10.00 Y20.00');
+    expect(buildG92(state, { fillMissingWithZero: true }))
+      .toBe('G92 X10.00 Y20.00 Z0.00 A0.00 B0.00 C0.00 E0.00');
  });

  test('decimals steuert die Nachkommastellen', () => {
-    expect(buildG92({ x: 1.23456 }, { decimals: 3 }))
-      .toBe('G92 X1.235 Y0.000 Z0.000 A0.000 B0.000 C0.000 E0.000');
+    expect(buildG92({ x: 1.23456 }, { decimals: 3 })).toBe('G92 X1.235');
  });

-  test('leerer State → alle Achsen 0', () => {
-    expect(buildG92({})).toBe('G92 X0.00 Y0.00 Z0.00 A0.00 B0.00 C0.00 E0.00');
-    expect(buildG92()).toBe('G92 X0.00 Y0.00 Z0.00 A0.00 B0.00 C0.00 E0.00');
-  });
-
-  test('nicht-numerische Werte (NaN/Strings) werden als 0 behandelt', () => {
-    expect(buildG92({ x: 'abc', y: NaN, z: 5 }))
-      .toBe('G92 X0.00 Y0.00 Z5.00 A0.00 B0.00 C0.00 E0.00');
+  test('leerer State → "G92 " ohne Achsen', () => {
+    expect(buildG92({})).toBe('G92 ');
+    expect(buildG92()).toBe('G92 ');
  });
 });