91 lines
3.6 KiB
JavaScript
91 lines
3.6 KiB
JavaScript
/**
|
||
* homingXEstimate.cjs
|
||
* ===================
|
||
* Reine Geometrie-Logik zur Schätzung der Slider-X-Position (kein I/O, kein fs).
|
||
*
|
||
* Bewusst CommonJS (`.cjs`): so kann sowohl der ESM-Server
|
||
* (`server/homingOrchestrator.js` via Node-Interop `import { … } from './…cjs'`)
|
||
* als auch Jest (`require('../server/homingXEstimate.cjs')`) dieselbe Logik nutzen.
|
||
* Folgt damit dem Repo-Muster „pure Logik herauslösen" (vgl. `public/yAxisCompute.js`),
|
||
* nur als `.cjs`, weil der Importeur ein ESM-Modul unter `"type":"module"` ist.
|
||
*/
|
||
|
||
/**
|
||
* 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. Sobald ein revolute-Gelenk um eine andere Achse dreht, hängt
|
||
* das Welt-x vom (hier unbekannten) Winkel ab → xSafe=false.
|
||
*
|
||
* @param {object} links robot.json `links`
|
||
* @param {string} linkName Link des Markers
|
||
* @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 bereits geparsten Daten.
|
||
*
|
||
* 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 kinematische
|
||
* Gelenk-Offset (z.B. Arm1.origin.x = 110 mm) korrekt herausgerechnet.
|
||
*
|
||
* Übersprungen werden: Board-Marker, Marker ohne Modell-Eintrag (unbekannte ID),
|
||
* Marker nicht-x-zuverlässiger Ketten und Marker ohne beobachtetes x.
|
||
*
|
||
* Fallback (keine zuverlässigen Marker, z.B. ohne robot.json): roher Mittelwert
|
||
* der world-x aller Nicht-Board-Marker – nur Notlösung.
|
||
*
|
||
* @param {{ markers?: Array<{ marker_id:number, link?:string, position_mm?:number[] }> }} arucoData
|
||
* @param {object} links robot.json `links` (oder {} falls nicht vorhanden)
|
||
* @returns {number} x_mm
|
||
*/
|
||
function estimateXFromParsed(arucoData, links) {
|
||
const markers = arucoData?.markers ?? [];
|
||
const samples = [];
|
||
|
||
for (const obs of 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 = markers.filter(m => m.link && m.link !== 'Board');
|
||
if (armMarkers.length === 0) return 0.0;
|
||
return armMarkers.reduce((s, m) => s + (m.position_mm?.[0] ?? 0), 0) / armMarkers.length;
|
||
}
|
||
|
||
module.exports = { modelWorldXAtSliderZero, estimateXFromParsed };
|