From 7f17427e0a10d8953df158a2e5382d206f9211bb Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Sat, 13 Jun 2026 06:13:31 +0200 Subject: [PATCH] Y-Axis checks --- public/boardViewer.html | 132 +++++------------------- public/calibration.js | 141 +++++++++++++++++++++++++- public/calibration_arm.html | 37 +++++++ public/yAxisCompute.js | 192 +++++++++++++++++++++++++++++++++++ server/editRobot.js | 121 ++++++++++++++++++++++ server/server.js | 57 ++++++++++- test/yAxisComputeJs.test.js | 195 ++++++++++++++++++++++++++++++++++++ 7 files changed, 765 insertions(+), 110 deletions(-) create mode 100644 public/yAxisCompute.js create mode 100644 test/yAxisComputeJs.test.js diff --git a/public/boardViewer.html b/public/boardViewer.html index 70102bf..13e30f7 100644 --- a/public/boardViewer.html +++ b/public/boardViewer.html @@ -149,6 +149,8 @@ } } + + @@ -736,10 +738,6 @@ function buildCompareLines() { // ── Y-Achsen-Berechnung aus drei Positionen ─────────────────────────────────── -/** Marker, die sich weniger als diesen Wert bewegen, werden ignoriert. - * Entspricht dem min_movement_mm-Parameter im Python-Skript. */ -const Y_AXIS_MIN_MOVEMENT_MM = 10.0; - function computeAndShowYAxis() { clearGroup(gYAxis); @@ -751,134 +749,56 @@ function computeAndShowYAxis() { return; } - const mapA = new Map(_primaryFremdMarkers .map(m => [m.marker_id, m])); - const mapB = new Map(_compareFremdMarkers .map(m => [m.marker_id, m])); - const mapC = new Map(_positionCFremdMarkers.map(m => [m.marker_id, m])); + // ── Berechnung via yAxisCompute.js (kein DOM/Three.js) ─────────────────── + const result = YAxisCompute.computeYAxis( + _primaryFremdMarkers, + _compareFremdMarkers, + _positionCFremdMarkers, + ); - function dist2(P, Q) { return (P[0]-Q[0])**2 + (P[1]-Q[1])**2 + (P[2]-Q[2])**2; } - - const circumcenters = []; // [{id, C:[x,y,z]}] in mm - const normals = []; // [[nx,ny,nz]] – Achsenrichtung je Marker - const markerData = []; // [{markerId, posA, posB, posC, circumcenter, normal}] für Speicherung - const skipped = []; // [{id, reason, maxMoveMm}] - - for (const [id, ma] of mapA) { - const mb = mapB.get(id); - const mc = mapC.get(id); - if (!mb || !mc) continue; - - const P1 = ma.position_mm.map(Number); - const P2 = mb.position_mm.map(Number); - const P3 = mc.position_mm.map(Number); - - // ── Mindest-Bewegungs-Filter ───────────────────────────────────────────── - // Marker, die sich zwischen den drei Positionen kaum bewegen, liefern - // degenerate Umkreismittelpunkte und korrumpieren die Achsenschätzung. - // Dieselbe Logik wie im Python-Skript (min_movement_mm). - const maxMoveMm = Math.max( - Math.sqrt(dist2(P1, P2)), - Math.sqrt(dist2(P2, P3)), - Math.sqrt(dist2(P1, P3)), - ); - if (maxMoveMm < Y_AXIS_MIN_MOVEMENT_MM) { - vlog(`Y-Achse: Marker ${id} übersprungen – Bewegung zu gering` + - ` (${maxMoveMm.toFixed(1)} mm < ${Y_AXIS_MIN_MOVEMENT_MM} mm, kein rotierender Marker)`, 'warn'); - skipped.push({ id, reason: 'Bewegung zu gering', maxMoveMm: +maxMoveMm.toFixed(2) }); - continue; - } - - // Normalenvektor der Kreisebene = Achsenrichtung - const v1 = [P2[0]-P1[0], P2[1]-P1[1], P2[2]-P1[2]]; - const v2 = [P3[0]-P1[0], P3[1]-P1[1], P3[2]-P1[2]]; - const cross = [ - v1[1]*v2[2] - v1[2]*v2[1], - v1[2]*v2[0] - v1[0]*v2[2], - v1[0]*v2[1] - v1[1]*v2[0], - ]; - const crossLen = Math.sqrt(cross[0]**2 + cross[1]**2 + cross[2]**2); - if (crossLen < 1e-3) { - vlog(`Y-Achse: Marker ${id} degenerat (Punkte zu nahe / kollinear)`, 'warn'); - continue; - } - const n = cross.map(c => c / crossLen); - normals.push(n); - - // Umkreismittelpunkt (bary­zentrischer Ansatz) - const a2 = dist2(P2, P3), b2 = dist2(P1, P3), c2 = dist2(P1, P2); - const w1 = a2*(b2+c2-a2), w2 = b2*(a2+c2-b2), w3 = c2*(a2+b2-c2); - const wSum = w1 + w2 + w3; - if (Math.abs(wSum) < 1e-6) { - vlog(`Y-Achse: Marker ${id} – Umkreis undefiniert`, 'warn'); - continue; - } - const C = [ - (w1*P1[0] + w2*P2[0] + w3*P3[0]) / wSum, - (w1*P1[1] + w2*P2[1] + w3*P3[1]) / wSum, - (w1*P1[2] + w2*P2[2] + w3*P3[2]) / wSum, - ]; - circumcenters.push({ id, C }); - markerData.push({ markerId: id, posA: P1, posB: P2, posC: P3, circumcenter: C, normal: n }); - - // Kreismittelpunkt (rose) - gYAxis.add(makeSphere(r2vArr(C), 0.007, 0xfb7185)); - // Bogen-Linie B→C (cyan) - gYAxis.add(makeLine(r2vArr(P2), r2vArr(P3), 0x22d3ee, 0.6)); - } - - if (circumcenters.length === 0) { - const why = skipped.length - ? `Alle ${skipped.length} Marker gefiltert (Bewegung < ${Y_AXIS_MIN_MOVEMENT_MM} mm)` - : 'Keine gemeinsamen fremd-Marker in Pos A+B+C gefunden'; - vlog(`Y-Achse: ${why}`, 'warn'); - window.parent.postMessage({ type: 'yaxis-measurement', axisDir: null, skipped }, '*'); + if (!result.ok) { + vlog(`Y-Achse: ${result.reason}`, 'warn'); + window.parent.postMessage({ type: 'yaxis-measurement', axisDir: null, skipped: result.skipped }, '*'); return; } - // Achsenrichtung: Mittlere Normale (Vorzeichen angleichen) - const ref = normals[0]; - const aligned = normals.map(n => { - const dot = n[0]*ref[0] + n[1]*ref[1] + n[2]*ref[2]; - return dot >= 0 ? n : n.map(c => -c); - }); - const meanN = [0, 1, 2].map(i => aligned.reduce((s, n) => s + n[i], 0) / aligned.length); - const meanNLen = Math.sqrt(meanN[0]**2 + meanN[1]**2 + meanN[2]**2); - const axisDir = meanN.map(c => c / meanNLen); // Roboter-Koordinaten + const { axisDir, axisPoint, tiltXY, tiltYZ, skipped, markerData } = result; - // Referenzpunkt: Schwerpunkt der Umkreismittelpunkte - const axisPoint = [0, 1, 2].map(i => - circumcenters.reduce((s, c) => s + c.C[i], 0) / circumcenters.length - ); + // ── Visualisierung (Three.js) ───────────────────────────────────────────── + for (const { posB: P2, posC: P3, circumcenter: C } of markerData) { + gYAxis.add(makeSphere(r2vArr(C), 0.007, 0xfb7185)); // Umkreismittelpunkt (rose) + gYAxis.add(makeLine(r2vArr(P2), r2vArr(P3), 0x22d3ee, 0.6)); // Bogen B→C (cyan) + } - // Achse als Linie ±500 mm visualisieren (magenta) const L = 500; const p1mm = axisPoint.map((v, i) => v - L * axisDir[i]); const p2mm = axisPoint.map((v, i) => v + L * axisDir[i]); gYAxis.add(makeLine(r2vArr(p1mm), r2vArr(p2mm), 0xe879f9, 0.9)); gYAxis.add(makeSphere(r2vArr(axisPoint), 0.011, 0xe879f9)); - // Abweichung von der idealen Y-Achse [0,1,0] in Roboter-Koordinaten - const [ax, ay, az] = axisDir; - const tiltXY = Math.atan2(ax, ay) * 180 / Math.PI; // Kippung in XY-Ebene - const tiltYZ = Math.atan2(az, ay) * 180 / Math.PI; // Kippung in YZ-Ebene + // ── Logging ─────────────────────────────────────────────────────────────── const fmt = v => (v >= 0 ? '+' : '') + v.toFixed(3) + '°'; const good = Math.abs(tiltXY) < 0.5 && Math.abs(tiltYZ) < 0.5; - - const usedIds = circumcenters.map(c => c.id); + const usedIds = markerData.map(m => m.markerId); const skippedIds = skipped.map(s => s.id); + vlog(`Y-Achse: ${usedIds.length} Marker genutzt (${usedIds.join(', ')})` + (skippedIds.length ? ` · ${skippedIds.length} gefiltert (${skippedIds.join(', ')})` : '')); vlog(` dir=[${axisDir.map(v => v.toFixed(4)).join(', ')}]`); vlog(` Referenzpunkt: [${axisPoint.map(v => v.toFixed(1)).join(', ')}] mm`); vlog(` Abw. von Y-Achse: XY ${fmt(tiltXY)} YZ ${fmt(tiltYZ)}`, good ? 'ok' : 'warn'); + if (skippedIds.length) { + skipped.forEach(s => vlog(` ↳ Marker ${s.id} übersprungen: ${s.reason} (${s.maxMoveMm} mm)`, 'warn')); + } window.parent.postMessage({ - type: 'yaxis-measurement', + type: 'yaxis-measurement', axisDir, axisPoint, tiltXY, tiltYZ, - numMarkers: circumcenters.length, - numMarkersCommon: circumcenters.length + skipped.length, + numMarkers: result.numMarkers, + numMarkersCommon: result.numMarkersCommon, skipped, // Für rotation_detection.json: Run-Referenzen und Marker-Rohdaten runA: document.getElementById('sel-run-primary')?.value ?? null, diff --git a/public/calibration.js b/public/calibration.js index c7fadf7..b226c83 100644 --- a/public/calibration.js +++ b/public/calibration.js @@ -556,15 +556,145 @@ function initArm(tab) { }); } - // Achsen-Messung vom Viewer empfangen + // ── Kalibrierungs-Aktionen (werden nach Rotation-Messung aktiv) ────────── + // Tab-Name → Link-Name in robot.json + const TAB_TO_LINK = { arm1: 'Arm1', arm2: 'Arm2', elbow: 'Ellbow', hand: 'Hand' }; + const robotLink = TAB_TO_LINK[tab] ?? tab; + + const calibActionsEl = document.getElementById(`${tab}-calib-actions`); + const assignFixedBtn = document.getElementById(`btn-${tab}-assign-fixed`); + const assignFixedInfo = document.getElementById(`${tab}-assign-fixed-info`); + const setOriginBtn = document.getElementById(`btn-${tab}-set-origin`); + const setOriginInfo = document.getElementById(`${tab}-set-origin-info`); + + let _lastYAxisMsg = null; // letztes gültiges yaxis-measurement + + function enableCalibActions(msg) { + if (!calibActionsEl) return; + calibActionsEl.style.display = 'block'; + + // ── Button 1: Fixe Marker → Base ──────────────────────────────────────── + if (assignFixedBtn) { + const skipped = msg.skipped ?? []; + if (skipped.length > 0) { + const ids = skipped.map(s => s.id).join(', '); + assignFixedBtn.disabled = false; + assignFixedBtn.style.opacity = '1'; + assignFixedBtn.style.cursor = 'pointer'; + assignFixedBtn.title = + `Marker ${ids} in robot.json dem Link 'Base' zuordnen`; + if (assignFixedInfo) { + assignFixedInfo.textContent = + `Kaum-bewegende Marker: ${ids} ` + + `(Bewegung < ${msg.skipped.map(s => s.maxMoveMm + ' mm').join(', ')}) ` + + `→ Link 'Base' in robot.json eintragen.`; + } + } else { + assignFixedBtn.disabled = true; + if (assignFixedInfo) assignFixedInfo.textContent = 'Alle erkannten Marker rotieren – kein fixer Marker gefunden.'; + } + } + + // ── Button 2: Joint-Origin Y/Z ─────────────────────────────────────────── + if (setOriginBtn) { + const [, ay, az] = msg.axisPoint; + setOriginBtn.disabled = false; + setOriginBtn.style.opacity = '1'; + setOriginBtn.style.cursor = 'pointer'; + setOriginBtn.title = `Joint '${robotLink}': origin[Y]=${ay.toFixed(1)} mm, origin[Z]=${az.toFixed(1)} mm`; + if (setOriginInfo) { + setOriginInfo.textContent = + `Berechnete Achse: Y = ${ay.toFixed(1)} mm · Z = ${az.toFixed(1)} mm ` + + `→ in robot.json links.${robotLink}.jointToParent.origin setzen.`; + } + } + } + + function disableCalibActions() { + if (assignFixedBtn) { + assignFixedBtn.disabled = true; + assignFixedBtn.style.opacity = '.45'; + assignFixedBtn.style.cursor = 'not-allowed'; + } + if (setOriginBtn) { + setOriginBtn.disabled = true; + setOriginBtn.style.opacity = '.45'; + setOriginBtn.style.cursor = 'not-allowed'; + } + } + + if (assignFixedBtn) { + assignFixedBtn.addEventListener('click', async () => { + if (!_lastYAxisMsg) return; + const skipped = _lastYAxisMsg.skipped ?? []; + const markerIds = skipped.map(s => s.id); + const measuredPositions = skipped + .filter(s => Array.isArray(s.posA)) + .map(s => ({ id: s.id, position_mm: s.posA })); + + log(`🔄 Ordne Marker [${markerIds.join(', ')}] dem Link 'Base' zu …`); + assignFixedBtn.disabled = true; + try { + const r = await fetch('/api/robot/assign-fixed-markers', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ markerIds, targetLink: 'Base', measuredPositions }), + }); + const data = await r.json(); + if (!r.ok) { log(`❌ Fehler: ${data.error ?? r.status}`); return; } + log(`✅ Zugeordnet: ${data.numAdded} neu, ${data.numAlreadyPresent} bereits vorhanden`); + data.changes.forEach(c => { + if (c.action === 'added') log(` + Marker ${c.markerId} → ${c.targetLink}`); + else if (c.action === 'already-present') log(` ○ Marker ${c.markerId} bereits in '${c.existingLink}'`); + else if (c.action === 'skipped-no-position') log(` ⚠ Marker ${c.markerId}: keine Positions-Daten`); + }); + } catch (err) { + log(`❌ Netzwerkfehler: ${err}`); + } finally { + assignFixedBtn.disabled = false; + } + }); + } + + if (setOriginBtn) { + setOriginBtn.addEventListener('click', async () => { + if (!_lastYAxisMsg) return; + const [, ay, az] = _lastYAxisMsg.axisPoint; + log(`🔄 Setze ${robotLink}.jointToParent.origin: Y=${ay.toFixed(1)} Z=${az.toFixed(1)} …`); + setOriginBtn.disabled = true; + try { + const r = await fetch('/api/robot/set-joint-origin', { + method: 'POST', + headers: { 'Content-Type': 'application/json' }, + body: JSON.stringify({ linkName: robotLink, y: ay, z: az }), + }); + const data = await r.json(); + if (!r.ok) { log(`❌ Fehler: ${data.error ?? r.status}`); return; } + log(`✅ Joint-Origin gesetzt: [${data.oldOrigin.join(', ')}] → [${data.newOrigin.join(', ')}]`); + } catch (err) { + log(`❌ Netzwerkfehler: ${err}`); + } finally { + setOriginBtn.disabled = false; + } + }); + } + + // ── Achsen-Messung vom Viewer empfangen ─────────────────────────────────── window.addEventListener('message', (e) => { if (!frameEl || e.source !== frameEl.contentWindow) return; const msg = e.data; - if (msg?.type === 'yaxis-measurement' && Array.isArray(msg.axisDir)) { + if (msg?.type !== 'yaxis-measurement') return; + + if (Array.isArray(msg.axisDir)) { + _lastYAxisMsg = msg; const fmt = v => (v >= 0 ? '+' : '') + v.toFixed(3) + '°'; - log(`📐 Achse (${msg.numMarkers} Marker): dir=[${msg.axisDir.map(v => v.toFixed(4)).join(', ')}]` + + log(`📐 Achse (${msg.numMarkers}/${msg.numMarkersCommon} Marker): dir=[${msg.axisDir.map(v => v.toFixed(4)).join(', ')}]` + ` XY=${fmt(msg.tiltXY)} YZ=${fmt(msg.tiltYZ)}`); log(` Referenzpunkt: [${msg.axisPoint.map(v => v.toFixed(1)).join(', ')}] mm`); + if ((msg.skipped ?? []).length) { + log(` Gefiltert (zu geringe Bewegung): ${msg.skipped.map(s => `${s.id} (${s.maxMoveMm} mm)`).join(', ')}`); + } + enableCalibActions(msg); // In rotation_detection.json speichern (anhängen) fetch('/api/xaxis/save-rotation-detection', { @@ -579,6 +709,11 @@ function initArm(tab) { }).then(r => r.json()) .then(d => log(`💾 Gespeichert: ${d.file} (${d.total} Messungen)`)) .catch(e => log(`⚠ Speichern fehlgeschlagen: ${e.message}`)); + + } else { + // Kein gültiges Ergebnis → Buttons deaktivieren + _lastYAxisMsg = null; + disableCalibActions(); } }); } diff --git a/public/calibration_arm.html b/public/calibration_arm.html index 67a640b..bacb721 100644 --- a/public/calibration_arm.html +++ b/public/calibration_arm.html @@ -23,6 +23,8 @@

Aktionen

+ +
+ + +
diff --git a/public/yAxisCompute.js b/public/yAxisCompute.js new file mode 100644 index 0000000..b4bb0e6 --- /dev/null +++ b/public/yAxisCompute.js @@ -0,0 +1,192 @@ +/** + * yAxisCompute.js + * ================ + * Reine Mathematik zur Rotationsachsen-Berechnung aus drei Marker-Positionen. + * Keine DOM- oder Three.js-Abhängigkeit – läuft in Browser und Node.js. + * + * UMD-Pattern: + * Browser: window.YAxisCompute.computeYAxis(...) + * Node.js: const { computeYAxis } = require('./public/yAxisCompute') + * + * Eingabe (markersA/B/C): + * Array von { marker_id: number, position_mm: [x, y, z] } + * – entspricht den fremd-Markern (link !== 'Board') aus aruco_marker_poses.json + * + * Ausgabe bei Erfolg: + * { + * ok: true, + * axisDir: [x, y, z], // Einheitsvektor der Rotationsachse + * axisPoint: [x, y, z], // Referenzpunkt auf der Achse (mm) + * tiltXY: degrees, // Abweichung von Y in XY-Ebene + * tiltYZ: degrees, // Abweichung von Y in YZ-Ebene + * numMarkers: number, // Anzahl genutzte Marker + * numMarkersCommon: number, // Alle gemeinsamen Marker (inkl. gefiltert) + * skipped: [{id, reason, maxMoveMm, posA?}], + * markerData: [{markerId, posA, posB, posC, circumcenter, normal}], + * } + * + * Ausgabe bei Fehler: + * { ok: false, reason: string, skipped: [] } + */ +(function (exports) { + 'use strict'; + + /** Marker mit max. Zentren-Bewegung unter diesem Wert werden ignoriert. */ + const DEFAULT_MIN_MOVEMENT_MM = 10.0; + + // ── Hilfsfunktionen ────────────────────────────────────────────────────────── + + function dist2(P, Q) { + return (P[0] - Q[0]) ** 2 + (P[1] - Q[1]) ** 2 + (P[2] - Q[2]) ** 2; + } + + /** + * Berechnet: + * - Umkreismittelpunkt C des Dreiecks P1-P2-P3 (Punkt auf der Rotationsachse) + * - Normalenvektor n der Dreiecks-Ebene (= Richtung der Rotationsachse) + * + * Gibt null zurück wenn das Triplet degenerat ist (kollinear / zu nahe beieinander). + */ + function circumcenterAndNormal(P1, P2, P3) { + const v1 = [P2[0] - P1[0], P2[1] - P1[1], P2[2] - P1[2]]; + const v2 = [P3[0] - P1[0], P3[1] - P1[1], P3[2] - P1[2]]; + const cross = [ + v1[1] * v2[2] - v1[2] * v2[1], + v1[2] * v2[0] - v1[0] * v2[2], + v1[0] * v2[1] - v1[1] * v2[0], + ]; + const crossLen = Math.sqrt(cross[0] ** 2 + cross[1] ** 2 + cross[2] ** 2); + if (crossLen < 1e-3) return null; + + const n = cross.map(c => c / crossLen); + + // Baryzentrische Gewichte → Umkreismittelpunkt (doc/04_y_achse.md) + const a2 = dist2(P2, P3), b2 = dist2(P1, P3), c2 = dist2(P1, P2); + const w1 = a2 * (b2 + c2 - a2); + const w2 = b2 * (a2 + c2 - b2); + const w3 = c2 * (a2 + b2 - c2); + const wSum = w1 + w2 + w3; + if (Math.abs(wSum) < 1e-6) return null; + + const C = [ + (w1 * P1[0] + w2 * P2[0] + w3 * P3[0]) / wSum, + (w1 * P1[1] + w2 * P2[1] + w3 * P3[1]) / wSum, + (w1 * P1[2] + w2 * P2[2] + w3 * P3[2]) / wSum, + ]; + return { C, n }; + } + + // ── Kern-API ───────────────────────────────────────────────────────────────── + + /** + * Berechnet die Rotationsachse aus drei Sätzen fremd-Marker. + * + * @param {Array} markersA Fremd-Marker aus Pos A (link !== 'Board') + * @param {Array} markersB Fremd-Marker aus Pos B + * @param {Array} markersC Fremd-Marker aus Pos C + * @param {Object} options + * @param {number} [options.minMovementMm=10] Mindest-Bewegung in mm + */ + function computeYAxis(markersA, markersB, markersC, options) { + const minMovementMm = (options && options.minMovementMm != null) + ? options.minMovementMm + : DEFAULT_MIN_MOVEMENT_MM; + + const mapA = new Map(markersA.map(m => [m.marker_id, m])); + const mapB = new Map(markersB.map(m => [m.marker_id, m])); + const mapC = new Map(markersC.map(m => [m.marker_id, m])); + + const circumcenters = []; // [{id, C:[x,y,z]}] + const normals = []; // [[nx, ny, nz]] + const markerData = []; // [{markerId, posA, posB, posC, circumcenter, normal}] + const skipped = []; // [{id, reason, maxMoveMm, posA?}] + + for (const [id, ma] of mapA) { + const mb = mapB.get(id); + const mc = mapC.get(id); + if (!mb || !mc) continue; + + const P1 = ma.position_mm.map(Number); + const P2 = mb.position_mm.map(Number); + const P3 = mc.position_mm.map(Number); + + // ── Mindest-Bewegungs-Filter ────────────────────────────────────────── + const maxMoveMm = Math.max( + Math.sqrt(dist2(P1, P2)), + Math.sqrt(dist2(P2, P3)), + Math.sqrt(dist2(P1, P3)), + ); + if (maxMoveMm < minMovementMm) { + skipped.push({ + id, + reason: 'Bewegung zu gering (kein rotierender Marker)', + maxMoveMm: +maxMoveMm.toFixed(2), + posA: P1, // Positions-Info für späteres Zuordnen zum Base-Link + }); + continue; + } + + // ── Umkreismittelpunkt + Normalenvektor ─────────────────────────────── + const result = circumcenterAndNormal(P1, P2, P3); + if (!result) { + skipped.push({ id, reason: 'degenerat (kollinear / identisch)', maxMoveMm: +maxMoveMm.toFixed(2) }); + continue; + } + + const { C, n } = result; + circumcenters.push({ id, C }); + normals.push(n); + markerData.push({ markerId: id, posA: P1, posB: P2, posC: P3, circumcenter: C, normal: n }); + } + + if (circumcenters.length === 0) { + return { + ok: false, + reason: skipped.length + ? `Alle ${skipped.length} gemeinsamen Marker gefiltert (Bewegung < ${minMovementMm} mm)` + : 'Keine gemeinsamen fremd-Marker in Pos A+B+C gefunden', + skipped, + }; + } + + // ── Achsenrichtung: Mittlere Normale (Vorzeichen angleichen) ───────────── + const ref = normals[0]; + const aligned = normals.map(n => { + const dot = n[0] * ref[0] + n[1] * ref[1] + n[2] * ref[2]; + return dot >= 0 ? n : n.map(c => -c); + }); + const meanN = [0, 1, 2].map(i => + aligned.reduce((s, n) => s + n[i], 0) / aligned.length + ); + const meanNLen = Math.sqrt(meanN[0] ** 2 + meanN[1] ** 2 + meanN[2] ** 2); + const axisDir = meanN.map(c => c / meanNLen); + + // ── Referenzpunkt: Schwerpunkt der Umkreismittelpunkte ─────────────────── + const axisPoint = [0, 1, 2].map(i => + circumcenters.reduce((s, c) => s + c.C[i], 0) / circumcenters.length + ); + + // ── Kippwinkel gegen Y-Achse [0,1,0] ──────────────────────────────────── + const [ax, ay, az] = axisDir; + const tiltXY = Math.atan2(ax, ay) * 180 / Math.PI; + const tiltYZ = Math.atan2(az, ay) * 180 / Math.PI; + + return { + ok: true, + axisDir, + axisPoint, + tiltXY, + tiltYZ, + numMarkers: circumcenters.length, + numMarkersCommon: circumcenters.length + skipped.length, + skipped, + markerData, + }; + } + + // ── Exports ────────────────────────────────────────────────────────────────── + + exports.computeYAxis = computeYAxis; + exports.DEFAULT_MIN_MOVEMENT_MM = DEFAULT_MIN_MOVEMENT_MM; + +}(typeof module !== 'undefined' ? module.exports : (self.YAxisCompute = {}))); diff --git a/server/editRobot.js b/server/editRobot.js index ced4116..4ef101c 100644 --- a/server/editRobot.js +++ b/server/editRobot.js @@ -488,3 +488,124 @@ export async function adoptXAxis(robotPath, { direction }) { angleXZdeg: Math.round(Math.atan2(nz, nx) * 18000 / Math.PI) / 100, }; } + +// ── Aktion 6: Fixe Marker einem Link zuordnen ───────────────────────────────── + +/** + * Ordnet Marker, die sich bei einer Gelenk-Rotation kaum bewegen, dem angegebenen + * Link zu. Typischer Anwendungsfall: Marker auf dem Basis-Körper werden als + * "Base"-Marker in robot.json registriert. + * + * - markerIds: Array von Marker-IDs (number[]) + * - targetLink: Ziel-Link-Name (z. B. 'Base') + * - measuredPositions: Array von { id, position_mm:[x,y,z] } aus Pos A + * + * Gibt { numAdded, numAlreadyPresent, changes[] } zurück. + * Bestehende Einträge werden NICHT verschoben (überschreiben wäre destruktiv). + */ +export async function assignFixedMarkersToLink(robotPath, { markerIds, targetLink, measuredPositions = [] }) { + if (!targetLink) throw new Error('targetLink muss angegeben werden.'); + + const robot = await readRobot(robotPath); + const links = robot.links ?? {}; + + // Index: markerId → aktueller Link + const existingMap = new Map(); + for (const [linkName, linkData] of Object.entries(links)) { + for (const m of (linkData.markers ?? [])) { + existingMap.set(Number(m.id), linkName); + } + } + + // Pos-A-Positionen indizieren + const posMap = new Map(); + for (const p of measuredPositions) { + posMap.set(Number(p.id), p.position_mm); + } + + if (!links[targetLink]) links[targetLink] = { markers: [] }; + if (!links[targetLink].markers) links[targetLink].markers = []; + + const changes = []; + let numAdded = 0, numAlreadyPresent = 0; + + for (const rawId of markerIds) { + const id = Number(rawId); + if (existingMap.has(id)) { + numAlreadyPresent++; + changes.push({ action: 'already-present', markerId: id, existingLink: existingMap.get(id) }); + continue; + } + + const pos = posMap.get(id); + if (!pos || pos.length < 3) { + changes.push({ action: 'skipped-no-position', markerId: id }); + continue; + } + + links[targetLink].markers.push({ + id, + position: pos.map(v => Math.round(Number(v) * 10) / 10), // 1 Dezimalstelle (mm) + positionSource: 'calibration-fixed-detection', + }); + numAdded++; + changes.push({ action: 'added', markerId: id, targetLink }); + } + + if (numAdded > 0) { + robot.links = links; + await writeRobot(robotPath, robot); + } + + return { numAdded, numAlreadyPresent, changes }; +} + +// ── Aktion 7: Joint-Origin Y/Z aus Drehachse übernehmen ────────────────────── + +/** + * Setzt die Y- und Z-Koordinaten des jointToParent.origin eines Links. + * + * Der Ursprung der Drehachse (axisPoint_mm) liegt im Board-Koordinatensystem. + * Y und Z sind davon direkt auf den Joint-Origin übertragbar (X wird vom + * Slider bestimmt und bleibt unverändert). + * + * - linkName: Name des Links, dessen Joint aktualisiert wird (z. B. 'Arm1') + * - y: Neuer Y-Wert des Joint-Origins (mm) + * - z: Neuer Z-Wert des Joint-Origins (mm) + * + * Gibt { changed, linkName, oldOrigin, newOrigin } zurück. + */ +export async function setJointOriginYZ(robotPath, { linkName, y, z }) { + if (!linkName) throw new Error('linkName muss angegeben werden.'); + + const robot = await readRobot(robotPath); + const link = (robot.links ?? {})[linkName]; + if (!link) return { changed: false, error: `Link '${linkName}' nicht in robot.json gefunden.` }; + + const joint = link.jointToParent; + if (!joint) return { changed: false, error: `Link '${linkName}' hat kein jointToParent.` }; + + const oldOrigin = Array.isArray(joint.origin) ? [...joint.origin] : [null, null, null]; + + if (!Array.isArray(joint.origin) || joint.origin.length < 3) { + joint.origin = [oldOrigin[0] ?? 0, 0, 0]; + } + + joint.origin[1] = Math.round(Number(y) * 10) / 10; // Y (1 Dezimalstelle) + joint.origin[2] = Math.round(Number(z) * 10) / 10; // Z + + // Quelle dokumentieren + if (!joint.originSource) joint.originSource = [null, null, null]; + while (joint.originSource.length < 3) joint.originSource.push(null); + joint.originSource[1] = 'calibration-yaxis'; + joint.originSource[2] = 'calibration-yaxis'; + + await writeRobot(robotPath, robot); + + return { + changed: true, + linkName, + oldOrigin, + newOrigin: [...joint.origin], + }; +} diff --git a/server/server.js b/server/server.js index a7bd4ed..f440a20 100755 --- a/server/server.js +++ b/server/server.js @@ -8,7 +8,7 @@ import { fileURLToPath } from 'url'; import process from 'process'; import { spawn } from 'child_process'; import { WebcamClient } from './webcamClient.js'; -import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId, adoptXAxis } from './editRobot.js'; +import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId, adoptXAxis, assignFixedMarkersToLink, setJointOriginYZ } from './editRobot.js'; const __filename = fileURLToPath(import.meta.url); const __dirname = path.dirname(__filename); @@ -889,6 +889,61 @@ app.post('/api/robot/adopt-x-axis', async (req, res) => { } }); +/** + * POST /api/robot/assign-fixed-markers + * Ordnet Marker, die sich bei einer Gelenk-Rotation kaum bewegen, dem + * angegebenen Link zu (typisch: 'Base'). + * Body: { markerIds: number[], targetLink: string, measuredPositions: [{id, position_mm}] } + */ +app.post('/api/robot/assign-fixed-markers', async (req, res) => { + try { + const { markerIds, targetLink, measuredPositions = [] } = req.body ?? {}; + if (!Array.isArray(markerIds) || markerIds.length === 0) { + return res.status(400).json({ error: '"markerIds" muss ein nicht-leeres Array sein.' }); + } + if (!targetLink) { + return res.status(400).json({ error: '"targetLink" muss angegeben werden.' }); + } + const result = await assignFixedMarkersToLink(ROBOT_JSON, { markerIds, targetLink, measuredPositions }); + console.log( + `robot/assign-fixed-markers [${markerIds.join(',')}] → ${targetLink}` + + ` added=${result.numAdded} alreadyPresent=${result.numAlreadyPresent}`, + ); + return res.json(result); + } catch (err) { + console.error('robot/assign-fixed-markers error:', err); + return res.status(500).json({ error: String(err) }); + } +}); + +/** + * POST /api/robot/set-joint-origin + * Setzt Y und Z des jointToParent.origin eines Links aus der berechneten + * Drehachsen-Position. + * Body: { linkName: string, y: number, z: number } + */ +app.post('/api/robot/set-joint-origin', async (req, res) => { + try { + const { linkName, y, z } = req.body ?? {}; + if (!linkName) return res.status(400).json({ error: '"linkName" muss angegeben werden.' }); + if (!Number.isFinite(Number(y)) || !Number.isFinite(Number(z))) { + return res.status(400).json({ error: '"y" und "z" müssen Zahlen sein.' }); + } + const result = await setJointOriginYZ(ROBOT_JSON, { linkName, y: Number(y), z: Number(z) }); + if (!result.changed) { + return res.status(400).json({ error: result.error }); + } + console.log( + `robot/set-joint-origin ${linkName}: ` + + `[${result.oldOrigin.join(', ')}] → [${result.newOrigin.join(', ')}]`, + ); + return res.json(result); + } catch (err) { + console.error('robot/set-joint-origin error:', err); + return res.status(500).json({ error: String(err) }); + } +}); + /** * POST /api/calibration/upload-npz * Liest {camera}_calibration.npz aus der aktuellen Session und diff --git a/test/yAxisComputeJs.test.js b/test/yAxisComputeJs.test.js new file mode 100644 index 0000000..90ccda5 --- /dev/null +++ b/test/yAxisComputeJs.test.js @@ -0,0 +1,195 @@ +/** + * yAxisComputeJs.test.js + * ======================= + * Unit-Test für public/yAxisCompute.js (reine JS-Berechnungslogik). + * + * Dieselben drei Timestamps wie der Python-Test: + * test/y-axis-finder-examples/20260612_190019 (Pos A) + * test/y-axis-finder-examples/20260612_190104 (Pos B) + * test/y-axis-finder-examples/20260612_190241 (Pos C) + * + * Erwartete Werte (aus verifiziertem Python-Lauf und boardViewer-Logik): + * Marker 197, 218, 219 → rotieren (3 Marker genutzt) + * Marker 201, 204, 242 → gefiltert (Bewegung < 10 mm) + * axisDir ≈ [0.9995, 0.029, -0.015] (fast reine X-Achse des Roboters) + * + * Hinweis: Im Gegensatz zum Python-Skript verwendet computeYAxis() nur die + * Marker-Zentren (position_mm), nicht alle vier Ecken. Deshalb: + * numMarkers = 3 (nicht 5 wie Python) + * Residuen etwas größer (Python hat 4× mehr Punkte je Marker) + */ + +const fs = require('fs'); +const path = require('path'); + +const { computeYAxis, DEFAULT_MIN_MOVEMENT_MM } = require('../public/yAxisCompute'); + +// ── Hilfsfunktionen ─────────────────────────────────────────────────────────── + +const EXAMPLES = path.join(__dirname, 'y-axis-finder-examples'); + +/** Lädt aruco_marker_poses.json und gibt die fremd-Marker zurück (link !== 'Board'). */ +function loadFremdMarkers(timestamp) { + const file = path.join(EXAMPLES, timestamp, 'aruco_marker_poses.json'); + const data = JSON.parse(fs.readFileSync(file, 'utf8')); + return (data.markers ?? []).filter(m => m.link !== 'Board'); +} + +const fremdA = loadFremdMarkers('20260612_190019'); +const fremdB = loadFremdMarkers('20260612_190104'); +const fremdC = loadFremdMarkers('20260612_190241'); + +// ── Haupt-Suite ─────────────────────────────────────────────────────────────── + +describe('computeYAxis – Arm1-Testdaten (190019 / 190104 / 190241)', () => { + + let r; + beforeAll(() => { + r = computeYAxis(fremdA, fremdB, fremdC); + }); + + // Grundstatus + test('Berechnung erfolgreich (ok: true)', () => { + expect(r.ok).toBe(true); + }); + + // Marker-Anzahlen + test('numMarkersCommon erfasst alle gemeinsamen fremd-Marker', () => { + // Alle drei Timestamps haben dieselben 6 fremd-Marker in common: + // 197, 201, 204, 218, 219, 242 + expect(r.numMarkersCommon).toBeGreaterThanOrEqual(4); + }); + + test('3 Marker tatsächlich genutzt (197, 218, 219)', () => { + expect(r.numMarkers).toBe(3); + const usedIds = r.markerData.map(m => m.markerId).sort((a, b) => a - b); + expect(usedIds).toEqual([197, 218, 219]); + }); + + // Filterung + test('kaum-bewegende Marker werden gefiltert (201, 204 mindestens)', () => { + const skippedIds = r.skipped.map(s => s.id).sort((a, b) => a - b); + expect(skippedIds).toContain(201); + expect(skippedIds).toContain(204); + }); + + test('gefilterte Marker haben Bewegung < DEFAULT_MIN_MOVEMENT_MM', () => { + r.skipped.forEach(s => { + expect(s.maxMoveMm).toBeLessThan(DEFAULT_MIN_MOVEMENT_MM); + expect(s.reason).toMatch(/Bewegung zu gering/); + }); + }); + + test('gefilterte Marker enthalten posA (für späteres Base-Zuordnen)', () => { + r.skipped.forEach(s => { + expect(Array.isArray(s.posA)).toBe(true); + expect(s.posA).toHaveLength(3); + }); + }); + + // Achsenrichtung + test('axisDir ist Einheitsvektor (|axisDir| ≈ 1)', () => { + const [ax, ay, az] = r.axisDir; + const len = Math.sqrt(ax * ax + ay * ay + az * az); + expect(len).toBeCloseTo(1.0, 4); + }); + + test('Achse liegt fast auf X-Achse des Roboters (axisDir[0] > 0.99)', () => { + // Arm1 schwingt auf/ab → Drehachse = X-Richtung + expect(r.axisDir[0]).toBeGreaterThan(0.99); + }); + + test('axisDir hat 3 Komponenten', () => { + expect(r.axisDir).toHaveLength(3); + }); + + test('axisPoint hat 3 Komponenten', () => { + expect(r.axisPoint).toHaveLength(3); + }); + + // Kippwinkel + test('tiltXY und tiltYZ sind finite Zahlen', () => { + expect(Number.isFinite(r.tiltXY)).toBe(true); + expect(Number.isFinite(r.tiltYZ)).toBe(true); + }); + + // markerData-Struktur + test('jeder markerData-Eintrag hat posA/posB/posC/circumcenter/normal', () => { + r.markerData.forEach(md => { + expect(md.posA).toHaveLength(3); + expect(md.posB).toHaveLength(3); + expect(md.posC).toHaveLength(3); + expect(md.circumcenter).toHaveLength(3); + expect(md.normal).toHaveLength(3); + }); + }); +}); + +// ── Parametertest: minMovementMm ────────────────────────────────────────────── + +describe('computeYAxis – minMovementMm-Parameter', () => { + + test('minMovementMm = 0 → alle gemeinsamen Marker werden genutzt, keine Skips', () => { + const r = computeYAxis(fremdA, fremdB, fremdC, { minMovementMm: 0 }); + expect(r.ok).toBe(true); + // Kein Marker sollte wegen Bewegung gefiltert sein + const movementSkips = r.skipped.filter(s => s.reason.includes('Bewegung zu gering')); + expect(movementSkips).toHaveLength(0); + // Mehr Marker als im Standardfall + expect(r.numMarkers).toBeGreaterThanOrEqual(3); + }); + + test('minMovementMm = 9999 → alle Marker gefiltert, ok: false', () => { + const r = computeYAxis(fremdA, fremdB, fremdC, { minMovementMm: 9999 }); + expect(r.ok).toBe(false); + expect(r.reason).toMatch(/gefiltert|gefunden/i); + expect(Array.isArray(r.skipped)).toBe(true); + }); + + test('DEFAULT_MIN_MOVEMENT_MM ist 10', () => { + expect(DEFAULT_MIN_MOVEMENT_MM).toBe(10.0); + }); +}); + +// ── Edge-Cases ──────────────────────────────────────────────────────────────── + +describe('computeYAxis – Edge Cases', () => { + + test('leere Eingaben → ok: false', () => { + const r = computeYAxis([], [], []); + expect(r.ok).toBe(false); + }); + + test('keine gemeinsamen Marker → ok: false', () => { + const a = [{ marker_id: 1, position_mm: [0, 0, 0] }]; + const b = [{ marker_id: 2, position_mm: [1, 0, 0] }]; + const c = [{ marker_id: 3, position_mm: [2, 0, 0] }]; + const r = computeYAxis(a, b, c); + expect(r.ok).toBe(false); + }); + + test('drei kollineare Punkte → degenerat, ok: false (einzelner Marker)', () => { + // Marker bewegt sich auf einer Linie → Kreisebene undefiniert + const a = [{ marker_id: 99, position_mm: [0, 0, 100] }]; + const b = [{ marker_id: 99, position_mm: [0, 100, 100] }]; // nur Y ändert sich + const c = [{ marker_id: 99, position_mm: [0, 200, 100] }]; // weiterhin nur Y + // Bewegung ist 200 mm (> 10 mm Threshold), aber Punkte sind kollinear + const r = computeYAxis(a, b, c, { minMovementMm: 0 }); + expect(r.ok).toBe(false); + }); + + test('ein einzelner gültiger Marker → Achse berechenbar', () => { + // Punkt bewegt sich auf einem Kreisbogen (einfache 2D-Rotation um Z-Achse) + const R = 100; // Radius 100 mm + const angle = (deg) => deg * Math.PI / 180; + const mk = (deg) => ({ + marker_id: 42, + position_mm: [R * Math.cos(angle(deg)), R * Math.sin(angle(deg)), 50], + }); + const r = computeYAxis([mk(0)], [mk(90)], [mk(180)]); + expect(r.ok).toBe(true); + expect(r.numMarkers).toBe(1); + // Achse sollte in Z-Richtung zeigen [0, 0, ±1] + expect(Math.abs(r.axisDir[2])).toBeGreaterThan(0.99); + }); +});