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

Y-Axis checks

Browse Source
This commit is contained in:
chk
2026-06-13 06:13:31 +02:00
parent 1762a771cf
commit 7f17427e0a
7 changed files with 765 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
public/boardViewer.html
View File

@@ -149,6 +149,8 @@
} }
} }
</script> </script>
<!-- reine Berechnungslogik (kein DOM/Three.js) – auch von Jest-Tests genutzt -->
<script src="/yAxisCompute.js"></script>
</head> </head>
<body> <body>
@@ -736,10 +738,6 @@ function buildCompareLines() {
// ── Y-Achsen-Berechnung aus drei Positionen ─────────────────────────────────── // ── 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() { function computeAndShowYAxis() {
clearGroup(gYAxis); clearGroup(gYAxis);
@@ -751,125 +749,47 @@ function computeAndShowYAxis() {
return; return;
} }
const mapA = new Map(_primaryFremdMarkers .map(m => [m.marker_id, m])); // ── Berechnung via yAxisCompute.js (kein DOM/Three.js) ───────────────────
const mapB = new Map(_compareFremdMarkers .map(m => [m.marker_id, m])); const result = YAxisCompute.computeYAxis(
const mapC = new Map(_positionCFremdMarkers.map(m => [m.marker_id, m])); _primaryFremdMarkers,
_compareFremdMarkers,
function dist2(P, Q) { return (P[0]-Q[0])**2 + (P[1]-Q[1])**2 + (P[2]-Q[2])**2; } _positionCFremdMarkers,
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 if (!result.ok) {
const v1 = [P2[0]-P1[0], P2[1]-P1[1], P2[2]-P1[2]]; vlog(`Y-Achse: ${result.reason}`, 'warn');
const v2 = [P3[0]-P1[0], P3[1]-P1[1], P3[2]-P1[2]]; window.parent.postMessage({ type: 'yaxis-measurement', axisDir: null, skipped: result.skipped }, '*');
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 }, '*');
return; return;
} }
// Achsenrichtung: Mittlere Normale (Vorzeichen angleichen) const { axisDir, axisPoint, tiltXY, tiltYZ, skipped, markerData } = result;
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
// Referenzpunkt: Schwerpunkt der Umkreismittelpunkte // ── Visualisierung (Three.js) ─────────────────────────────────────────────
const axisPoint = [0, 1, 2].map(i => for (const { posB: P2, posC: P3, circumcenter: C } of markerData) {
circumcenters.reduce((s, c) => s + c.C[i], 0) / circumcenters.length 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 L = 500;
const p1mm = axisPoint.map((v, i) => v - L * axisDir[i]); const p1mm = axisPoint.map((v, i) => v - L * axisDir[i]);
const p2mm = 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(makeLine(r2vArr(p1mm), r2vArr(p2mm), 0xe879f9, 0.9));
gYAxis.add(makeSphere(r2vArr(axisPoint), 0.011, 0xe879f9)); gYAxis.add(makeSphere(r2vArr(axisPoint), 0.011, 0xe879f9));
// Abweichung von der idealen Y-Achse [0,1,0] in Roboter-Koordinaten // ── Logging ───────────────────────────────────────────────────────────────
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
const fmt = v => (v >= 0 ? '+' : '') + v.toFixed(3) + '°'; const fmt = v => (v >= 0 ? '+' : '') + v.toFixed(3) + '°';
const good = Math.abs(tiltXY) < 0.5 && Math.abs(tiltYZ) < 0.5; const good = Math.abs(tiltXY) < 0.5 && Math.abs(tiltYZ) < 0.5;
const usedIds = markerData.map(m => m.markerId);
const usedIds = circumcenters.map(c => c.id);
const skippedIds = skipped.map(s => s.id); const skippedIds = skipped.map(s => s.id);
vlog(`Y-Achse: ${usedIds.length} Marker genutzt (${usedIds.join(', ')})` + vlog(`Y-Achse: ${usedIds.length} Marker genutzt (${usedIds.join(', ')})` +
(skippedIds.length ? ` · ${skippedIds.length} gefiltert (${skippedIds.join(', ')})` : '')); (skippedIds.length ? ` · ${skippedIds.length} gefiltert (${skippedIds.join(', ')})` : ''));
vlog(` dir=[${axisDir.map(v => v.toFixed(4)).join(', ')}]`); vlog(` dir=[${axisDir.map(v => v.toFixed(4)).join(', ')}]`);
vlog(` Referenzpunkt: [${axisPoint.map(v => v.toFixed(1)).join(', ')}] mm`); vlog(` Referenzpunkt: [${axisPoint.map(v => v.toFixed(1)).join(', ')}] mm`);
vlog(` Abw. von Y-Achse: XY ${fmt(tiltXY)} YZ ${fmt(tiltYZ)}`, good ? 'ok' : 'warn'); 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({ window.parent.postMessage({
type: 'yaxis-measurement', type: 'yaxis-measurement',
@@ -877,8 +797,8 @@ function computeAndShowYAxis() {
axisPoint, axisPoint,
tiltXY, tiltXY,
tiltYZ, tiltYZ,
numMarkers: circumcenters.length, numMarkers: result.numMarkers,
numMarkersCommon: circumcenters.length + skipped.length, numMarkersCommon: result.numMarkersCommon,
skipped, skipped,
// Für rotation_detection.json: Run-Referenzen und Marker-Rohdaten // Für rotation_detection.json: Run-Referenzen und Marker-Rohdaten
runA: document.getElementById('sel-run-primary')?.value ?? null, runA: document.getElementById('sel-run-primary')?.value ?? null,

141
public/calibration.js
View File

@@ -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) => { window.addEventListener('message', (e) => {
if (!frameEl || e.source !== frameEl.contentWindow) return; if (!frameEl || e.source !== frameEl.contentWindow) return;
const msg = e.data; 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) + '°'; 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)}`); ` XY=${fmt(msg.tiltXY)} YZ=${fmt(msg.tiltYZ)}`);
log(` Referenzpunkt: [${msg.axisPoint.map(v => v.toFixed(1)).join(', ')}] mm`); 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) // In rotation_detection.json speichern (anhängen)
fetch('/api/xaxis/save-rotation-detection', { fetch('/api/xaxis/save-rotation-detection', {
@@ -579,6 +709,11 @@ function initArm(tab) {
}).then(r => r.json()) }).then(r => r.json())
.then(d => log(`💾 Gespeichert: ${d.file} (${d.total} Messungen)`)) .then(d => log(`💾 Gespeichert: ${d.file} (${d.total} Messungen)`))
.catch(e => log(`⚠ Speichern fehlgeschlagen: ${e.message}`)); .catch(e => log(`⚠ Speichern fehlgeschlagen: ${e.message}`));
} else {
// Kein gültiges Ergebnis → Buttons deaktivieren
_lastYAxisMsg = null;
disableCalibActions();
} }
}); });
} }

37
public/calibration_arm.html
View File

@@ -23,6 +23,8 @@
<!-- ── Aktionen ───────────────────────────────────────────────────────────── --> <!-- ── Aktionen ───────────────────────────────────────────────────────────── -->
<div class="section full"> <div class="section full">
<h2>Aktionen</h2> <h2>Aktionen</h2>
<!-- Roboter-Bewegung -->
<div style="margin-top:14px;display:flex;align-items:center;gap:20px;flex-wrap:wrap"> <div style="margin-top:14px;display:flex;align-items:center;gap:20px;flex-wrap:wrap">
<button id="btn-arm1-ccw" style="font-size:18px;padding:6px 22px" title="Bieps rauf"> <button id="btn-arm1-ccw" style="font-size:18px;padding:6px 22px" title="Bieps rauf">
⤴ Rauf ⤴ Rauf
@@ -32,6 +34,41 @@
Runter ⤵ Runter ⤵
</button> </button>
</div> </div>
<!-- Kalibrierungs-Aktionen (erscheinen wenn Rotation erkannt wurde) -->
<div id="arm1-calib-actions" style="display:none;margin-top:20px;border-top:1px solid var(--border);padding-top:16px">
<p style="font-size:11px;color:var(--muted);margin-bottom:12px">
Aktionen aus erkannter Rotation (3 Positionen):
</p>
<!-- Aktion 1: Fixe Marker dem Base-Link zuordnen -->
<div style="display:flex;align-items:flex-start;gap:12px;margin-bottom:14px;flex-wrap:wrap">
<button id="btn-arm1-assign-fixed" disabled
style="min-width:220px;padding:6px 14px;opacity:.45;cursor:not-allowed"
title="Marker, die sich kaum bewegen, dem Link 'Base' zuordnen">
Fixe Marker → Link «Base»
</button>
<span id="arm1-assign-fixed-info"
style="font-size:11px;color:var(--muted);max-width:360px;line-height:1.5">
Marker, die sich kaum bewegen, sind physisch am Basis-Körper befestigt.
Diese Aktion trägt sie in robot.json unter «Base» ein.
</span>
</div>
<!-- Aktion 2: Joint-Origin Y/Z aus Drehachse -->
<div style="display:flex;align-items:flex-start;gap:12px;flex-wrap:wrap">
<button id="btn-arm1-set-origin" disabled
style="min-width:220px;padding:6px 14px;opacity:.45;cursor:not-allowed"
title="Y und Z des Schulter-Joints aus der berechneten Drehachse setzen">
Joint-Origin Y/Z übernehmen
</button>
<span id="arm1-set-origin-info"
style="font-size:11px;color:var(--muted);max-width:360px;line-height:1.5">
Setzt Y und Z des Arm1-Joints (Schulter) in robot.json auf die
berechnete Drehachsen-Position.
</span>
</div>
</div>
</div> </div>
<div class="section full"> <div class="section full">

192
public/yAxisCompute.js Normal file
View File

@@ -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 = {})));

121
server/editRobot.js
View File

@@ -488,3 +488,124 @@ export async function adoptXAxis(robotPath, { direction }) {
angleXZdeg: Math.round(Math.atan2(nz, nx) * 18000 / Math.PI) / 100, 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],
};
}

57
server/server.js
View File

@@ -8,7 +8,7 @@ import { fileURLToPath } from 'url';
import process from 'process'; import process from 'process';
import { spawn } from 'child_process'; import { spawn } from 'child_process';
import { WebcamClient } from './webcamClient.js'; 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 __filename = fileURLToPath(import.meta.url);
const __dirname = path.dirname(__filename); 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 * POST /api/calibration/upload-npz
* Liest {camera}_calibration.npz aus der aktuellen Session und * Liest {camera}_calibration.npz aus der aktuellen Session und

195
test/yAxisComputeJs.test.js Normal file
View File

@@ -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);
});
});