650 lines
23 KiB
JavaScript
650 lines
23 KiB
JavaScript
/**
|
||
* editRobot.js
|
||
* Hilfsfunktionen zum Bearbeiten von robot_xxx.json.
|
||
* Alle Schreibvorgänge machen ein Backup-Kommentar in der Datei (nein, aber
|
||
* atomisches Write per Temp-Datei ist hier nicht nötig – die Datei wird direkt
|
||
* überschrieben; bei Bedarf Backup-Strategie ergänzen).
|
||
*/
|
||
import { createRequire } from 'module';
|
||
import { fetchRobot, pushRobot } from './robotConfig.js';
|
||
const { normalizeSpinDeg } = createRequire(import.meta.url)('./spinNormalize.cjs');
|
||
|
||
// ── I/O ───────────────────────────────────────────────────────────────────────
|
||
|
||
async function readRobot(_robotPath) {
|
||
return fetchRobot();
|
||
}
|
||
|
||
async function writeRobot(_robotPath, data) {
|
||
return pushRobot(data);
|
||
}
|
||
|
||
// ── Aktion 1: Marker nach Z-Bereich zuordnen ─────────────────────────────────
|
||
|
||
/**
|
||
* Weist allen Markern, deren z-Position (mm) zwischen zMin und zMax liegt,
|
||
* das angegebene Set und/oder den angegebenen Link zu.
|
||
*
|
||
* - set (optional): Setzt den set-Wert des Markers.
|
||
* - link (optional): Verschiebt den Marker in diesen Link (wird ggf. angelegt).
|
||
* - extraMarkers (optional): Triangulierte Marker aus aruco_marker_poses.json –
|
||
* werden als neue Einträge in robot.json hinzugefügt,
|
||
* wenn sie noch nicht vorhanden sind.
|
||
*
|
||
* Gibt { numChanged, changes[] } zurück.
|
||
* changes[]: { markerId, action, oldLink, newLink, oldSet, newSet }
|
||
* action: 'updated' | 'added'
|
||
*/
|
||
export async function assignByZRange(robotPath, { zMin, zMax, set, link, extraMarkers = [] }) {
|
||
const robot = await readRobot(robotPath);
|
||
const links = robot.links ?? {};
|
||
const changes = [];
|
||
|
||
const zLo = Number(zMin);
|
||
const zHi = Number(zMax);
|
||
|
||
// ── Teil 1: Bestehende robot.json-Marker aktualisieren / verschieben ──────────
|
||
const snapshot = [];
|
||
for (const [linkName, linkData] of Object.entries(links)) {
|
||
for (const marker of (linkData.markers ?? [])) {
|
||
if (Array.isArray(marker.position)) snapshot.push({ id: marker.id, currentLink: linkName });
|
||
}
|
||
}
|
||
|
||
for (const { id, currentLink } of snapshot) {
|
||
const srcLinkData = links[currentLink];
|
||
const idx = (srcLinkData?.markers ?? []).findIndex(m => Number(m.id) === id);
|
||
if (idx === -1) continue;
|
||
|
||
const marker = srcLinkData.markers[idx];
|
||
const z = Number(marker.position[2]);
|
||
if (z < zLo || z > zHi) continue;
|
||
|
||
const change = {
|
||
action: 'updated',
|
||
markerId: marker.id,
|
||
oldLink: currentLink,
|
||
oldSet: marker.set ?? '',
|
||
newLink: currentLink,
|
||
newSet: marker.set ?? '',
|
||
};
|
||
|
||
if (set !== undefined && set !== '') { marker.set = set; change.newSet = set; }
|
||
|
||
if (link && link !== currentLink) {
|
||
srcLinkData.markers.splice(idx, 1);
|
||
if (!links[link]) links[link] = { markers: [] };
|
||
if (!links[link].markers) links[link].markers = [];
|
||
links[link].markers.push(marker);
|
||
change.newLink = link;
|
||
}
|
||
|
||
changes.push(change);
|
||
}
|
||
|
||
// ── Teil 2: Neue Marker aus 3b-Triangulation einfügen (noch nicht in robot.json) ──
|
||
// Diese haben keine Position in robot.json – wir übernehmen die gemessene Position.
|
||
if (link && extraMarkers.length > 0) {
|
||
const knownIds = new Set();
|
||
for (const ld of Object.values(links)) {
|
||
for (const m of (ld.markers ?? [])) knownIds.add(Number(m.id));
|
||
}
|
||
|
||
for (const em of extraMarkers) {
|
||
const emId = Number(em.marker_id);
|
||
const emPos = em.position_mm; // [x_mm, y_mm, z_mm] robot-Koordinaten
|
||
if (knownIds.has(emId)) continue; // bereits in robot.json (evtl. gerade hinzugefügt)
|
||
if (!Array.isArray(emPos) || emPos.length < 3) continue;
|
||
|
||
const z = Number(emPos[2]);
|
||
if (z < zLo || z > zHi) continue;
|
||
|
||
const newMarker = {
|
||
id: emId,
|
||
position: emPos.map(v => Math.round(Number(v) * 100) / 100), // 2 Dezimalstellen
|
||
};
|
||
if (set) newMarker.set = set;
|
||
|
||
if (!links[link]) links[link] = { markers: [] };
|
||
if (!links[link].markers) links[link].markers = [];
|
||
links[link].markers.push(newMarker);
|
||
knownIds.add(emId);
|
||
|
||
changes.push({
|
||
action: 'added',
|
||
markerId: emId,
|
||
oldLink: null,
|
||
oldSet: '',
|
||
newLink: link,
|
||
newSet: set ?? '',
|
||
});
|
||
}
|
||
}
|
||
|
||
if (changes.length > 0) {
|
||
robot.links = links;
|
||
await writeRobot(robotPath, robot);
|
||
}
|
||
|
||
return { numChanged: changes.length, changes };
|
||
}
|
||
|
||
// ── Aktion 2: Set oder Link-Zuordnung entfernen ───────────────────────────────
|
||
|
||
/**
|
||
* Entfernt die Set- oder Link-Zuordnung eines Markers.
|
||
*
|
||
* removeFrom: 'set' → löscht nur den set-Wert (Marker bleibt im Link)
|
||
* removeFrom: 'link' → entfernt Marker komplett aus dem Link
|
||
* (nur wenn set bereits leer/nicht gesetzt)
|
||
*
|
||
* Gibt { changed, markerId, action, link, [oldSet], [error] } zurück.
|
||
*/
|
||
export async function removeMarkerAssignment(robotPath, { markerId, removeFrom }) {
|
||
const id = Number(markerId);
|
||
const robot = await readRobot(robotPath);
|
||
const links = robot.links ?? {};
|
||
|
||
for (const [linkName, linkData] of Object.entries(links)) {
|
||
const markers = linkData.markers ?? [];
|
||
const idx = markers.findIndex(m => Number(m.id) === id);
|
||
if (idx === -1) continue;
|
||
|
||
const marker = markers[idx];
|
||
|
||
if (removeFrom === 'set') {
|
||
const oldSet = marker.set ?? '';
|
||
delete marker.set;
|
||
await writeRobot(robotPath, robot);
|
||
return { changed: true, markerId: id, action: 'set-removed', link: linkName, oldSet };
|
||
}
|
||
|
||
if (removeFrom === 'link') {
|
||
if (marker.set) {
|
||
return {
|
||
changed: false,
|
||
markerId: id,
|
||
error: `Marker ${id} hat noch Set "${marker.set}". Bitte zuerst Set entfernen.`,
|
||
};
|
||
}
|
||
markers.splice(idx, 1);
|
||
await writeRobot(robotPath, robot);
|
||
return { changed: true, markerId: id, action: 'link-removed', link: linkName };
|
||
}
|
||
|
||
return { changed: false, error: `Unbekannte Aktion: "${removeFrom}"` };
|
||
}
|
||
|
||
return { changed: false, error: `Marker-ID ${id} nicht gefunden.` };
|
||
}
|
||
|
||
// ── Aktion 3: Sets untereinander justieren (2D+Z Kabsch) ─────────────────────
|
||
|
||
/**
|
||
* Optimale 2D-Rotation (um Z-Achse) + 3D-Translation, die model→measured minimiert.
|
||
* Gibt { tx, ty, tz, theta, thetaDeg } zurück.
|
||
* modelPts / measuredPts: Arrays von [x, y, z] in mm.
|
||
*/
|
||
function computeRigid2DZ(modelPts, measuredPts) {
|
||
const n = modelPts.length;
|
||
if (n === 0) return { tx: 0, ty: 0, tz: 0, theta: 0, thetaDeg: 0 };
|
||
|
||
// Schwerpunkte
|
||
let mCx = 0, mCy = 0, mCz = 0, pCx = 0, pCy = 0, pCz = 0;
|
||
for (let i = 0; i < n; i++) {
|
||
mCx += modelPts[i][0]; mCy += modelPts[i][1]; mCz += modelPts[i][2];
|
||
pCx += measuredPts[i][0]; pCy += measuredPts[i][1]; pCz += measuredPts[i][2];
|
||
}
|
||
mCx /= n; mCy /= n; mCz /= n;
|
||
pCx /= n; pCy /= n; pCz /= n;
|
||
|
||
// 2D-Kreuzkovarianz für Kabsch-Rotation in XY-Ebene
|
||
let H00 = 0, H01 = 0, H10 = 0, H11 = 0;
|
||
for (let i = 0; i < n; i++) {
|
||
const ax = modelPts[i][0] - mCx, ay = modelPts[i][1] - mCy;
|
||
const bx = measuredPts[i][0] - pCx, by = measuredPts[i][1] - pCy;
|
||
H00 += ax * bx; H01 += ax * by;
|
||
H10 += ay * bx; H11 += ay * by;
|
||
}
|
||
|
||
// Optimaler Drehwinkel (2D-Sonderfall von Kabsch / SVD → atan2)
|
||
const theta = Math.atan2(H10 - H01, H00 + H11);
|
||
const cos = Math.cos(theta);
|
||
const sin = Math.sin(theta);
|
||
|
||
// Translation: gemessener Schwerpunkt − R × Modell-Schwerpunkt
|
||
const tx = pCx - (cos * mCx - sin * mCy);
|
||
const ty = pCy - (sin * mCx + cos * mCy);
|
||
const tz = pCz - mCz;
|
||
|
||
return { tx, ty, tz, theta, thetaDeg: theta * 180 / Math.PI };
|
||
}
|
||
|
||
function applyRigid2DZ([x, y, z], { tx, ty, tz, theta }) {
|
||
const cos = Math.cos(theta);
|
||
const sin = Math.sin(theta);
|
||
return [
|
||
Math.round((cos * x - sin * y + tx) * 100) / 100,
|
||
Math.round((sin * x + cos * y + ty) * 100) / 100,
|
||
Math.round((z + tz) * 100) / 100,
|
||
];
|
||
}
|
||
|
||
/**
|
||
* Richtet alle Marker des Sets `setToMove` rigid an ihren triangulieren 3b-Positionen aus.
|
||
* Das andere Set bleibt unberührt – die Transformation wird aus den Paaren
|
||
* (Modellposition, Messposition) der matching Marker berechnet.
|
||
*
|
||
* setToMove: Name des zu verschiebenden Sets (z.B. "rail")
|
||
* extraMarkers: Marker aus aruco_marker_poses.json (mit marker_id, position_mm)
|
||
*
|
||
* Gibt { numChanged, numMatchingPts, transform: {tx, ty, tz, thetaDeg} } zurück,
|
||
* oder { error } bei Fehler.
|
||
*/
|
||
export async function alignSetToMeasured(robotPath, { setToMove, extraMarkers = [] }) {
|
||
const robot = await readRobot(robotPath);
|
||
const links = robot.links ?? {};
|
||
|
||
// Alle Marker des zu verschiebenden Sets sammeln
|
||
const setMarkers = [];
|
||
for (const [linkName, linkData] of Object.entries(links)) {
|
||
for (const marker of (linkData.markers ?? [])) {
|
||
if (marker.set === setToMove) {
|
||
setMarkers.push({ marker, linkName });
|
||
}
|
||
}
|
||
}
|
||
|
||
if (setMarkers.length === 0) {
|
||
return { error: `Set "${setToMove}" nicht gefunden oder leer.` };
|
||
}
|
||
|
||
// Gemessene Positionen aus 3b indizieren
|
||
const measuredMap = new Map();
|
||
for (const em of extraMarkers) {
|
||
if (Array.isArray(em.position_mm) && em.position_mm.length >= 3) {
|
||
measuredMap.set(Number(em.marker_id), em.position_mm.map(Number));
|
||
}
|
||
}
|
||
|
||
// Matching-Paare für Kabsch (nur Marker, die sowohl Modellposition als auch Messung haben)
|
||
const modelPts = [], measuredPts = [];
|
||
for (const { marker } of setMarkers) {
|
||
if (!Array.isArray(marker.position) || marker.position.length < 3) continue;
|
||
const mpos = measuredMap.get(Number(marker.id));
|
||
if (!mpos) continue;
|
||
modelPts.push(marker.position.map(Number));
|
||
measuredPts.push(mpos);
|
||
}
|
||
|
||
if (modelPts.length < 2) {
|
||
return {
|
||
error: `Zu wenig Messpunkte für Set "${setToMove}" (${modelPts.length} von ${setMarkers.length} Markern gemessen). ` +
|
||
`Bitte Board-Run mit ≥2 Kameras durchführen.`,
|
||
};
|
||
}
|
||
|
||
const transform = computeRigid2DZ(modelPts, measuredPts);
|
||
|
||
// Transformation auf ALLE Marker des Sets anwenden (auch nicht gemessene)
|
||
let numChanged = 0;
|
||
for (const { marker } of setMarkers) {
|
||
if (!Array.isArray(marker.position) || marker.position.length < 3) continue;
|
||
marker.position = applyRigid2DZ(marker.position.map(Number), transform);
|
||
numChanged++;
|
||
}
|
||
|
||
robot.links = links;
|
||
await writeRobot(robotPath, robot);
|
||
|
||
return {
|
||
numChanged,
|
||
numMatchingPts: modelPts.length,
|
||
transform: {
|
||
tx: Math.round(transform.tx * 100) / 100,
|
||
ty: Math.round(transform.ty * 100) / 100,
|
||
tz: Math.round(transform.tz * 100) / 100,
|
||
thetaDeg: Math.round(transform.thetaDeg * 100) / 100,
|
||
},
|
||
};
|
||
}
|
||
|
||
// ── Aktion 4: Einzelnen Marker per ID hinzufügen / aktualisieren ──────────────
|
||
|
||
/**
|
||
* Fügt einen Marker per ID zu Set und Link hinzu oder aktualisiert einen bestehenden.
|
||
*
|
||
* - Existiert der Marker bereits in robot.json: set und/oder link werden aktualisiert.
|
||
* - Existiert er noch nicht: Position wird aus extraMarkers (3b-Output) geholt und
|
||
* der Marker wird dem angegebenen Link hinzugefügt.
|
||
*
|
||
* Gibt { changed, change } oder { changed: false, error } zurück.
|
||
*/
|
||
export async function assignMarkerId(robotPath, { markerId, set, link, extraMarkers = [] }) {
|
||
const id = Number(markerId);
|
||
if (!Number.isFinite(id) || id < 0) return { changed: false, error: 'Ungültige Marker-ID.' };
|
||
|
||
const robot = await readRobot(robotPath);
|
||
const links = robot.links ?? {};
|
||
|
||
// Marker in robot.json suchen
|
||
let foundMarker = null, foundLink = null;
|
||
outer: for (const [linkName, linkData] of Object.entries(links)) {
|
||
for (const m of (linkData.markers ?? [])) {
|
||
if (Number(m.id) === id) { foundMarker = m; foundLink = linkName; break outer; }
|
||
}
|
||
}
|
||
|
||
if (foundMarker) {
|
||
// Bestehenden Marker: Set und ggf. Link aktualisieren
|
||
const change = {
|
||
action: 'updated',
|
||
markerId: id,
|
||
oldLink: foundLink,
|
||
oldSet: foundMarker.set ?? '',
|
||
newLink: foundLink,
|
||
newSet: foundMarker.set ?? '',
|
||
};
|
||
|
||
if (set !== undefined && set !== '') { foundMarker.set = set; change.newSet = set; }
|
||
|
||
if (link && link !== foundLink) {
|
||
// In neuen Link verschieben
|
||
links[foundLink].markers = links[foundLink].markers.filter(m => Number(m.id) !== id);
|
||
if (!links[link]) links[link] = { markers: [] };
|
||
if (!links[link].markers) links[link].markers = [];
|
||
links[link].markers.push(foundMarker);
|
||
change.newLink = link;
|
||
}
|
||
|
||
robot.links = links;
|
||
await writeRobot(robotPath, robot);
|
||
return { changed: true, change };
|
||
}
|
||
|
||
// Neuer Marker: Position aus 3b-Output holen
|
||
const em = extraMarkers.find(m => Number(m.marker_id) === id);
|
||
if (!em) {
|
||
return {
|
||
changed: false,
|
||
error: `Marker ${id} ist nicht in robot.json und nicht im letzten 3b-Run vorhanden.`,
|
||
};
|
||
}
|
||
if (!link) {
|
||
return { changed: false, error: 'Link muss angegeben werden, um einen neuen Marker hinzuzufügen.' };
|
||
}
|
||
|
||
if (!Array.isArray(em.position_mm)) {
|
||
return {
|
||
changed: false,
|
||
error: `Marker ${id} hat keine triangulierte Position (position_mm fehlt – z.B. Einzelkamera-Marker).`,
|
||
};
|
||
}
|
||
const newMarker = {
|
||
id,
|
||
position: em.position_mm.map(v => Math.round(Number(v) * 100) / 100),
|
||
};
|
||
if (set) newMarker.set = set;
|
||
|
||
if (!links[link]) links[link] = { markers: [] };
|
||
if (!links[link].markers) links[link].markers = [];
|
||
links[link].markers.push(newMarker);
|
||
|
||
robot.links = links;
|
||
await writeRobot(robotPath, robot);
|
||
return {
|
||
changed: true,
|
||
change: { action: 'added', markerId: id, oldLink: null, oldSet: '', newLink: link, newSet: set ?? '' },
|
||
};
|
||
}
|
||
|
||
// ── Aktion 5: X-Achse übernehmen ─────────────────────────────────────────────
|
||
|
||
/**
|
||
* Rotiert alle Marker-Positionen in robot.json so, dass die übergebene Richtung
|
||
* zur neuen X-Achse [1,0,0] wird. Rotation um den Schwerpunkt aller A0-Marker
|
||
* (Origin bleibt erhalten).
|
||
*
|
||
* direction: [vx, vy, vz] – gemessene Ist-X-Richtung in Roboter-Koordinaten
|
||
*
|
||
* Algorithmus:
|
||
* 1. Normalisiere direction, ggf. Vorzeichen so dass vx > 0
|
||
* 2. Baue Orthonormalbasis: x_new = v,
|
||
* z_new = Gram-Schmidt([0,0,1] gegen v),
|
||
* y_new = cross(z_new, x_new)
|
||
* 3. Rotationsmatrix R (old→new): Zeilen = [x_new, y_new, z_new]
|
||
* 4. p_new = origin + R * (p_old − origin)
|
||
*/
|
||
export async function adoptXAxis(robotPath, { direction }) {
|
||
const [vx, vy, vz] = direction.map(Number);
|
||
const len = Math.sqrt(vx * vx + vy * vy + vz * vz);
|
||
if (len < 1e-9) throw new Error('Richtungsvektor zu klein (fast Null-Vektor).');
|
||
|
||
// Normalisieren, immer positive X-Komponente
|
||
let nx = vx / len, ny = vy / len, nz = vz / len;
|
||
if (nx < 0) { nx = -nx; ny = -ny; nz = -nz; }
|
||
|
||
// ── Orthonormalbasis ──────────────────────────────────────────────────────
|
||
// Z_new: Gram-Schmidt von [0,0,1] gegen x_new
|
||
const dotZ = nz; // dot([0,0,1], [nx,ny,nz])
|
||
let zx = -dotZ * nx, zy = -dotZ * ny, zz = 1 - dotZ * nz;
|
||
let zlen = Math.sqrt(zx * zx + zy * zy + zz * zz);
|
||
if (zlen < 1e-9) {
|
||
// Sonderfall: x_new fast parallel zu Z – Fallback auf [0,1,0]
|
||
const dotY = ny;
|
||
zx = -dotY * nx; zy = 1 - dotY * ny; zz = -dotY * nz;
|
||
zlen = Math.sqrt(zx * zx + zy * zy + zz * zz);
|
||
}
|
||
zx /= zlen; zy /= zlen; zz /= zlen;
|
||
|
||
// Y_new = cross(z_new, x_new) [rechte-Hand-Regel: ẑ × x̂ = ŷ]
|
||
const yx = zy * nz - zz * ny;
|
||
const yy = zz * nx - zx * nz;
|
||
const yz = zx * ny - zy * nx;
|
||
|
||
// Rotationsfunktion: p_rot = R * p (R hat Zeilen = neue Achsen in alten Koordinaten)
|
||
function rotVec(px, py, pz) {
|
||
return [
|
||
nx * px + ny * py + nz * pz, // neue X-Komponente
|
||
yx * px + yy * py + yz * pz, // neue Y-Komponente
|
||
zx * px + zy * py + zz * pz, // neue Z-Komponente
|
||
];
|
||
}
|
||
|
||
const robot = await readRobot(robotPath);
|
||
const links = robot.links ?? {};
|
||
|
||
// ── Ursprung: Schwerpunkt aller A0-Marker ────────────────────────────────
|
||
const a0Pos = [];
|
||
for (const ld of Object.values(links)) {
|
||
for (const m of (ld.markers ?? [])) {
|
||
if (m.set === 'A0' && Array.isArray(m.position) && m.position.length >= 3) {
|
||
a0Pos.push(m.position.map(Number));
|
||
}
|
||
}
|
||
}
|
||
let ox = 0, oy = 0, oz = 0;
|
||
if (a0Pos.length > 0) {
|
||
for (const [px, py, pz] of a0Pos) { ox += px; oy += py; oz += pz; }
|
||
ox /= a0Pos.length; oy /= a0Pos.length; oz /= a0Pos.length;
|
||
}
|
||
|
||
// ── Alle Marker rotieren ──────────────────────────────────────────────────
|
||
let numChanged = 0;
|
||
for (const ld of Object.values(links)) {
|
||
for (const m of (ld.markers ?? [])) {
|
||
if (!Array.isArray(m.position) || m.position.length < 3) continue;
|
||
const [px, py, pz] = m.position.map(Number);
|
||
const [rx, ry, rz] = rotVec(px - ox, py - oy, pz - oz);
|
||
m.position = [
|
||
Math.round((ox + rx) * 100) / 100,
|
||
Math.round((oy + ry) * 100) / 100,
|
||
Math.round((oz + rz) * 100) / 100,
|
||
];
|
||
numChanged++;
|
||
}
|
||
}
|
||
|
||
robot.links = links;
|
||
await writeRobot(robotPath, robot);
|
||
|
||
return {
|
||
numChanged,
|
||
origin: [ox, oy, oz].map(v => Math.round(v * 10) / 10),
|
||
newXAxis: [nx, ny, nz].map(v => Math.round(v * 10000) / 10000),
|
||
angleXYdeg: Math.round(Math.atan2(ny, 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],
|
||
};
|
||
}
|
||
|
||
// ── Aktion 8: Arm-Marker Spin setzen ─────────────────────────────────────────
|
||
|
||
/**
|
||
* Setzt den `spin`-Wert eines Arm-Markers in robot.json.
|
||
* Body: { linkName, markerId, spin }
|
||
* spin: absolute Gradzahl (0 / 90 / 180 / 270) oder relativer Delta (+90, -90, +180)
|
||
* — hier wird immer die *neue absolute* Gradzahl erwartet.
|
||
*
|
||
* @returns {{ changed, linkName, markerId, oldSpin, newSpin } | { changed: false, error }}
|
||
*/
|
||
export async function setArmMarkerSpin(robotPath, { linkName, markerId, spin }) {
|
||
const robot = await readRobot(robotPath);
|
||
const linkData = robot.links?.[linkName];
|
||
if (!linkData) return { changed: false, error: `Link '${linkName}' nicht gefunden.` };
|
||
|
||
const markers = linkData.markers ?? [];
|
||
const idx = markers.findIndex(m => Number(m.id) === Number(markerId));
|
||
if (idx === -1) {
|
||
return { changed: false, error: `Marker ${markerId} in Link '${linkName}' nicht gefunden.` };
|
||
}
|
||
|
||
const oldSpin = markers[idx].spin ?? 0;
|
||
const newSpin = normalizeSpinDeg(spin);
|
||
markers[idx].spin = newSpin;
|
||
|
||
await writeRobot(robotPath, robot);
|
||
|
||
return { changed: true, linkName, markerId: Number(markerId), oldSpin, newSpin };
|
||
}
|