diff --git a/public/boardViewer.html b/public/boardViewer.html index 0f62c1c..a4b5a50 100644 --- a/public/boardViewer.html +++ b/public/boardViewer.html @@ -1160,7 +1160,10 @@ window.addEventListener('message', async (e) => { await loadData(e.data.runDir); } if (e.data?.type === 'homing-state' && IS_HOMING) { - _homingAngles = e.data.state; + // Gefundene Winkel über Default-Position mergen, damit noch nicht erkannte + // Gelenke nicht auf 0 zusammenfallen, sondern sinnvoll stehen bleiben. + const base = _currentRobot?.defaultPosition ?? {}; + _homingAngles = { ...base, ...e.data.state }; if (_currentRobot) buildSkeletonFK(_currentRobot, _homingAngles); } }); diff --git a/server/homingOrchestrator.js b/server/homingOrchestrator.js index bb7d192..b528e9e 100644 --- a/server/homingOrchestrator.js +++ b/server/homingOrchestrator.js @@ -10,23 +10,80 @@ import path from 'path'; import fs from 'fs'; import fsPromises from 'fs/promises'; +/** + * Modell-Welt-x eines Markers bei slider=0, durch Aufsummieren der origin.x + * entlang der Kette Link→…→Base. Das Ergebnis ist winkel-unabhängig (und damit + * exakt) genau dann, wenn alle revolute-Gelenke der Kette um die x-Achse drehen + * (Rotation um x erhält die x-Koordinate). Andernfalls xSafe=false. + * + * @param {object} links robot.json links + * @param {string} linkName + * @param {number[]} localPos Marker-Position im lokalen Link-Frame [x,y,z] + * @returns {{ worldX: number, xSafe: boolean }} + */ +function modelWorldXAtSliderZero(links, linkName, localPos) { + let xOffset = 0; + let xSafe = true; + let cur = linkName; + const seen = new Set(); + + while (cur && links[cur]?.jointToParent && !seen.has(cur)) { + seen.add(cur); + const jtp = links[cur].jointToParent; + xOffset += jtp.origin?.[0] ?? 0; + + const axis = jtp.axis ?? [0, 0, 0]; + const isXAxis = Math.abs(axis[0]) === 1 && axis[1] === 0 && axis[2] === 0; + if (jtp.type === 'revolute' && !isXAxis) xSafe = false; + + cur = links[cur].parent; + } + return { worldX: xOffset + (localPos?.[0] ?? 0), xSafe }; +} + /** * Schätzt die Slider-X-Position aus den triangulierten Marker-Positionen - * (aruco_marker_poses.json). Nutzt den Durchschnitt der x_mm aller - * Nicht-Board-Marker. Fallback: 0.0 wenn keine Arm-Marker sichtbar. + * (aruco_marker_poses.json). + * + * Für jeden beobachteten Arm-Marker wird der implizierte Slider-Wert berechnet: + * slider_i = beobachtetes_world_x − Modell_world_x(slider=0) + * und über alle x-zuverlässigen Marker gemittelt. So wird der Gelenk-Offset + * (z.B. Arm1.origin.x = 110 mm) korrekt herausgerechnet. + * + * Fallback (kein robot.json oder keine zuverlässigen Marker): alter Mittelwert + * der rohen world-x – nur als Notlösung. * * @param {string} arucoJsonPath + * @param {string} [robotJsonPath] * @returns {number} x_mm */ -export function estimateXFromMarkers(arucoJsonPath) { +export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) { try { - const data = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8')); - const armMarkers = (data.markers ?? []).filter( - m => m.link && m.link !== 'Board', - ); + const data = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8')); + const links = robotJsonPath + ? (JSON.parse(fs.readFileSync(robotJsonPath, 'utf8')).links ?? {}) + : {}; + + const samples = []; + for (const obs of (data.markers ?? [])) { + if (!obs.link || obs.link === 'Board') continue; + const modelMarker = links[obs.link]?.markers?.find(m => m.id === obs.marker_id); + if (!modelMarker?.position) continue; + const { worldX, xSafe } = modelWorldXAtSliderZero(links, obs.link, modelMarker.position); + if (!xSafe) continue; + const obsX = obs.position_mm?.[0]; + if (obsX == null) continue; + samples.push(obsX - worldX); + } + + if (samples.length > 0) { + return samples.reduce((a, b) => a + b, 0) / samples.length; + } + + // ── Fallback: alter, geometrisch ungenauer Mittelwert ── + const armMarkers = (data.markers ?? []).filter(m => m.link && m.link !== 'Board'); if (armMarkers.length === 0) return 0.0; - const sumX = armMarkers.reduce((s, m) => s + (m.position_mm?.[0] ?? 0), 0); - return sumX / armMarkers.length; + return armMarkers.reduce((s, m) => s + (m.position_mm?.[0] ?? 0), 0) / armMarkers.length; } catch { return 0.0; } @@ -78,7 +135,7 @@ export async function runHoming({ // ── Schritt 2: X-Position bestimmen ───────────────────────────────────── send({ type: 'step', step: 2, total: 6, text: 'X-Position bestimmen …' }); - const xMm = estimateXFromMarkers(arucoJson); + const xMm = estimateXFromMarkers(arucoJson, robotJsonPath); send({ type: 'log', text: `▶ Geschätzte X-Position: ${xMm.toFixed(1)} mm` }); send({ type: 'analysis', key: 'x_mm', value: xMm });