Refactor Claude
This commit is contained in:
chk
@@ -9,81 +9,24 @@
|
||||
import path from 'path';
|
||||
import fs from 'fs';
|
||||
import fsPromises from 'fs/promises';
|
||||
import { estimateXFromParsed } from './homingXEstimate.cjs';
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* Schätzt die Slider-X-Position aus den triangulierten Marker-Positionen.
|
||||
* Dünner I/O-Wrapper: liest die Dateien und delegiert die reine Geometrie an
|
||||
* `estimateXFromParsed()` (`server/homingXEstimate.cjs`, unit-getestet).
|
||||
*
|
||||
* @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).
|
||||
*
|
||||
* 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]
|
||||
* @param {string} arucoJsonPath Pfad zu aruco_marker_poses.json
|
||||
* @param {string} [robotJsonPath] Pfad zu robot.json (für den Gelenk-Offset)
|
||||
* @returns {number} x_mm
|
||||
*/
|
||||
export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) {
|
||||
try {
|
||||
const data = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8'));
|
||||
const arucoData = 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;
|
||||
return armMarkers.reduce((s, m) => s + (m.position_mm?.[0] ?? 0), 0) / armMarkers.length;
|
||||
return estimateXFromParsed(arucoData, links);
|
||||
} catch {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
90
server/homingXEstimate.cjs
Normal file
90
server/homingXEstimate.cjs
Normal file
@@ -0,0 +1,90 @@
|
||||
/**
|
||||
* 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 };
|
||||
Reference in New Issue
Block a user