Files
appRobotHoming/public/calculateActions.js
2026-05-15 14:20:18 +02:00

723 lines
19 KiB
JavaScript
Executable File

// calculateActions.js
// Berechnung + nachvollziehbare Result-Struktur + Live-Logs in analysis-log
function getAnalysisLogEl() {
if (typeof document === "undefined") return null;
return document.getElementById("analysis-log");
}
function appendToAnalysis(line) {
const el = getAnalysisLogEl();
if (!el) return;
const now = new Date().toISOString();
el.value += `[${now}] ${line}\n`;
el.scrollTop = el.scrollHeight;
}
function createAnalysisResult(meta = {}) {
return {
meta: {
timestamp: new Date().toISOString(),
...meta
},
inputs: {
source: null,
headers: [],
rowCount: 0
},
observations: [],
calculations: [],
features: {},
commands: [],
logs: [],
errors: [],
summary: {
observationCount: 0,
calculationCount: 0,
featureCount: 0,
commandCount: 0
},
status: "ok"
};
}
function rowSnapshot(row) {
return {
id: row?.id,
seen_by: row?.seen_by,
x_mm: row?.x_mm,
y_mm: row?.y_mm,
z_mm: row?.z_mm,
roll_deg: row?.roll_deg,
pitch_deg: row?.pitch_deg
};
}
function addLog(result, message, level = "info") {
const entry = {
timestamp: new Date().toISOString(),
level,
message
};
result.logs.push(entry);
appendToAnalysis(message);
}
function addError(result, message, error = null) {
result.status = "error";
result.errors.push({
timestamp: new Date().toISOString(),
message,
details: error ? String(error) : null
});
addLog(result, `Fehler: ${message}`, "error");
}
function addObservation(result, key, row, confidence = 1.0, notes = []) {
result.observations.push({
key,
source: {
rowId: row?.id,
seenBy: row?.seen_by
},
values: rowSnapshot(row),
confidence,
notes
});
}
function addCalculation(result, calc) {
result.calculations.push({
timestamp: new Date().toISOString(),
...calc
});
}
function addFeature(result, key, feature) {
result.features[key] = feature;
}
function addCommand(result, id, command, basedOn = [], confidence = 1.0, format = "gcode") {
result.commands.push({
id,
command,
basedOn,
confidence,
format
});
}
function weightedAverage(candidates) {
const weightSum = candidates.reduce((sum, c) => sum + c.weight, 0);
if (!weightSum) return null;
const valueRad = candidates.reduce((sum, c) => sum + (c.valueRad * c.weight), 0) / weightSum;
const spreadRad = candidates.length > 1
? Math.sqrt(
candidates.reduce(
(sum, c) => sum + c.weight * Math.pow(c.valueRad - valueRad, 2),
0
) / weightSum
)
: 0;
const confidence = Math.max(
0.2,
Math.min(
0.98,
0.55 +
Math.min(candidates.length * 0.1, 0.25) -
Math.min(spreadRad / 1.0, 0.2)
)
);
return { valueRad, weightSum, spreadRad, confidence };
}
async function fetchCSV() {
const res = await fetch("/api/latest-snapshot");
if (!res.ok) throw new Error("Fehler beim Laden des Snapshots");
let data;
if (res.headers.get("content-type")?.includes("application/json")) {
data = await res.json();
} else {
const csvData = await res.text();
data = {
filename: "latest.csv",
mtime: new Date().toISOString(),
content: csvData
};
}
const lines = data.content.trim().split(/\r?\n/).filter(Boolean);
if (lines.length < 2) {
throw new Error("Keine oder unvollständige Daten");
}
const headers = lines[0].split(",").map(h => h.trim());
const rows = lines.slice(1).map(line => {
const cells = line.split(",");
const obj = {};
headers.forEach((h, i) => {
const raw = (cells[i] ?? "").trim();
const numeric = Number(raw);
obj[h] = raw !== "" && Number.isFinite(numeric) ? numeric : raw;
});
return obj;
});
return { data, headers, rows };
}
function calculateAngleFromPosition(result, row, axisY, axisZ, deltaYangle0, deltaZangle0 = 0, label = "angle") {
const y = parseFloat(row.y_mm);
const z = parseFloat(row.z_mm);
const dy = -(y - axisY);
const dz = z - axisZ;
let angle0Rad = 0;
if (deltaZangle0 !== 0 && deltaYangle0 !== 0) {
angle0Rad = Math.atan(deltaZangle0 / deltaYangle0);
}
const angleRad = Math.atan2(dz, dy) - angle0Rad;
const angleDeg = angleRad * 180 / Math.PI;
const message = `(${label} = ${angleDeg.toFixed(2)}° = ${angleRad.toFixed(4)} rad) aus Position von ID = ${row.id}`;
addLog(result, message);
addCalculation(result, {
type: "angleFromPosition",
label,
source: {
rowId: row.id,
seenBy: row.seen_by
},
input: {
axisY,
axisZ,
deltaYangle0,
deltaZangle0,
y_mm: y,
z_mm: z,
dy,
dz,
angle0Rad
},
output: {
angleRad,
angleDeg
}
});
return angleRad;
}
function calculateAngleFromRollColumn(result, row, roll0 = 0, pitch0 = 0, yaw0 = 0, strMotor = "motor") {
const rollDeg = -parseFloat(row.roll_deg) + roll0;
const rollRad = rollDeg * Math.PI / 180;
const message = `(${strMotor} = ${rollDeg.toFixed(2)}° = ${rollRad.toFixed(4)} rad) aus roll_deg von ID = ${row.id}`;
addLog(result, message);
addCalculation(result, {
type: "angleFromRollColumn",
label: strMotor,
source: {
rowId: row.id,
seenBy: row.seen_by
},
input: {
roll0,
pitch0,
yaw0,
roll_deg: row.roll_deg
},
output: {
angleRad: rollRad,
angleDeg: rollDeg
}
});
return rollRad;
}
function calculateAngleFromRelativePosition(result, row1, row2, strMotor = "motor") {
const y1 = parseFloat(row1.y_mm);
const z1 = parseFloat(row1.z_mm);
const y2 = parseFloat(row2.y_mm);
const z2 = parseFloat(row2.z_mm);
const dy = y1 - y2;
const dz = z1 - z2;
const angleRad = -Math.atan2(dz, dy);
const angleDeg = angleRad * 180 / Math.PI;
const message = `(${strMotor} = ${angleDeg.toFixed(2)}° = ${angleRad.toFixed(4)} rad) aus relativer Position von ID = ${row1.id} und ID = ${row2.id}`;
addLog(result, message);
addCalculation(result, {
type: "angleFromRelativePosition",
label: strMotor,
source: {
rowIds: [row1.id, row2.id],
seenBy: [row1.seen_by, row2.seen_by]
},
input: {
y1_mm: y1,
z1_mm: z1,
y2_mm: y2,
z2_mm: z2,
dy,
dz
},
output: {
angleRad,
angleDeg
}
});
return angleRad;
}
function buildFeatureFromCandidates(result, key, title, method, candidates) {
const summary = weightedAverage(candidates);
if (!summary) return null;
const feature = {
title,
method,
valueRad: summary.valueRad,
valueDeg: summary.valueRad * 180 / Math.PI,
confidence: summary.confidence,
spreadRad: summary.spreadRad,
weightSum: summary.weightSum,
evidence: candidates.map(c => c.source),
parts: candidates
};
addFeature(result, key, feature);
addCalculation(result, {
type: "featureSummary",
key,
title,
method,
input: candidates,
output: {
valueRad: feature.valueRad,
valueDeg: feature.valueDeg,
confidence: feature.confidence,
spreadRad: feature.spreadRad,
weightSum: feature.weightSum
}
});
return feature;
}
function calculate_angleY(row243, row229, row198, row197, shoulderAxisY, shoulderAxisZ, result) {
const angleYCandidates = [];
if (row243) {
const a1 = calculateAngleFromPosition(result, row243, shoulderAxisY, shoulderAxisZ, 285, 0, "yMotor");
const a2 = calculateAngleFromRollColumn(result, row243, 90, 0, 0, "yMotor");
angleYCandidates.push({
source: "row243.position",
valueRad: a1,
weight: 1
});
/*
angleYCandidates.push({
source: "row243.roll_deg",
valueRad: a2,
weight: 1
});
*/
}
if (row229) {
const a = calculateAngleFromPosition(result, row229, shoulderAxisY, shoulderAxisZ, 250, 35, "yMotor");
angleYCandidates.push({
source: "row229.position",
valueRad: a,
weight: 1
});
}
if (row198) {
const a = calculateAngleFromPosition(result, row198, shoulderAxisY, shoulderAxisZ, 165, 35, "yMotor");
angleYCandidates.push({
source: "row198.position",
valueRad: a,
weight: 1
});
}
if (row198 && row229) {
const a = calculateAngleFromRelativePosition(result, row198, row229, "yMotor");
angleYCandidates.push({
source: "row198-row229.relative",
valueRad: a,
weight: 3
});
}
if (row197) {
const angleDeg = 90 - parseFloat(row197.pitch_deg);
const angleRad = angleDeg * Math.PI / 180;
addLog(result, `(yMotor = ${angleDeg.toFixed(2)}° = ${angleRad.toFixed(4)} rad ) aus Pitch von ${row197.id}`);
addCalculation(result, {
type: "angleFromPitch",
label: "yMotor",
source: {
rowId: row197.id,
seenBy: row197.seen_by
},
input: {
pitch_deg: row197.pitch_deg
},
output: {
angleRad,
angleDeg
}
});
angleYCandidates.push({
source: "row197.pitch_deg",
valueRad: angleRad,
weight: 1
});
}
if (angleYCandidates.length > 0) {
buildFeatureFromCandidates(
result,
"shoulder.angleY",
"Schulter Y",
"weighted-average",
angleYCandidates
);
}
}
async function calculate() {
const result = createAnalysisResult({
sourceScript: "calculateActions.js"
});
try {
addLog(result, "Starte Berechnung...");
const { data, headers, rows } = await fetchCSV();
result.inputs = {
source: {
filename: data.filename,
mtime: data.mtime
},
headers,
rowCount: rows.length
};
addLog(result, `CSV-Daten geladen: ${rows.length} Zeilen, ${headers.length} Spalten.`);
const getRow = (id, seenBy = null) => {
const row = rows.find(r => r.id == id && (seenBy === null || r.seen_by == seenBy));
if (row) {
addObservation(
result,
`row-${id}${seenBy !== null ? `-seenBy-${seenBy}` : ""}`,
row,
seenBy === 3 ? 0.95 : 0.75,
[
`id=${id}`,
seenBy !== null ? `seen_by=${seenBy}` : "seen_by=any"
]
);
} else {
addLog(result, `Zeile nicht gefunden: id=${id}${seenBy !== null ? `, seen_by=${seenBy}` : ""}`, "warn");
}
return row;
};
const shoulderAxisY = 115;
const shoulderAxisZ = 61;
const row200 = getRow(200, 3); // Base
const row204 = getRow(204, 3); // Base
const row201 = getRow(201, 3); // Base
const row198 = getRow(198, 3); // Bizeps oben-hinten
const row242 = getRow(242, 3); // Bizeps unten
const row243 = getRow(243, 3); // Bizeps vorne
const row229 = getRow(229, 3); // Bizeps oben
const row197 = getRow(197); // Bizeps Seiten-Abdeckung
const row222 = getRow(222, 3); // Ellbow
const row226 = getRow(226, 3); // Ellbow
const row223 = getRow(223, 3); // Forearm
const row228 = getRow(228, 3); // Forearm
const row218 = getRow(218, 3); // Forearm 90°
const row219 = getRow(219, 3); // Forearm 90°
const row212 = getRow(212, 3); // Hand
// Bizeps > Y-Achse der Schulter: angleYCandidates
calculate_angleY(row243, row229, row198, row197, shoulderAxisY, shoulderAxisZ, result);
// Forearm rotation: a axis:
listForearmID = [row223, row228, row218, row219];
listForearmAngle = [60, 145, -90, -90];
addLog(result, "Berechnung der Unterarm-Rotation (a axis) aus x Positionen von Forearm und Ellbogen...");
for(let r = 0; r < listForearmID.length; r++){
if (listForearmID[r] && (row222 || row226)) {
dx = listForearmID[r].x_mm - (row222 ? row222.x_mm : row226.x_mm);
if(dx > 35 || dx < -35){
addLog(result, `dx=${dx.toFixed(2)} mm ist zu groß für eine realistische Unterarm-Rotation. Überspringe...`, "warn");
continue;
}
addLog(result, `dx between ${listForearmID[r].id} and Ellbow: ${dx.toFixed(2)} mm results in angle: ${Math.asin(dx / 35)*180/Math.PI}°`);
var angleRad = Math.asin(dx / 35) + listForearmAngle[r] * Math.PI / 180
if(row222){angleRad = -angleRad }
const angleDeg = angleRad * 180 / Math.PI;
addLog(result, `(a axis from ${listForearmID[r].id} with dx ${dx.toFixed(2)} mm: ${angleDeg.toFixed(2)}° = ${angleRad.toFixed(4)} rad ) aus x von ${listForearmID[r].id} und Ellbogen`);
addCalculation(result, {
type: "forearmrotationAFromX",
input: {
dx,
r
}
});
}
}
// Unterarm / z
const angleZCandidates = [];
if (row218 && row219) {
const lowerArmAngle = calculateAngleFromRelativePosition(result, row218, row219, "zMotor");
angleZCandidates.push({
source: "row218-row219.relative",
valueRad: lowerArmAngle,
weight: 200 // Hohes gewicht, da ich sicher bin, dass es korrekt ist
});
}
if (row226) {
const a = calculateAngleFromRollColumn(result, row226, 0, 0, 0, "zMotor");
angleZCandidates.push({
source: "row226.roll_deg",
valueRad: a,
weight: 1
});
}
if (angleZCandidates.length > 0) {
buildFeatureFromCandidates(
result,
"forearm.angleZ",
"Unterarm Z",
"combined",
angleZCandidates
);
}
// Commands
const angleY = result.features["shoulder.angleY"];
const angleZ = result.features["forearm.angleZ"];
if (angleY && angleZ) {
const cmdConfidence = Math.min(angleY.confidence, angleZ.confidence);
addCommand(
result,
"set-coord-angles-deg",
`G92 y${angleY.valueDeg.toFixed(1)} z${angleZ.valueDeg.toFixed(1)} (Set Coord. Angles in deg)`,
["shoulder.angleY", "forearm.angleZ"],
cmdConfidence
);
addCommand(
result,
"set-coord-angles-rad",
`M92 y${angleY.valueRad.toFixed(3)} z${angleZ.valueRad.toFixed(3)} (Set Coord. Angles in rad)`,
["shoulder.angleY", "forearm.angleZ"],
cmdConfidence
);
}
// Ellbow / Zusatzschätzung
if (row222 || row226 || row229 || row198 || row243 || row242 || row200 || row204) {
let x226 = 0;
let xCount = 0;
if (row226) {
x226 += row226.x_mm * 5;
xCount += 5;
}
if(row222){
/// 222 should have the same as x226
x226 += row222.x_mm * 5;
xCount += 5;
}
if (row229) {
x226 += row229.x_mm + 90;
xCount += 1;
}
if (row198) {
x226 += row198.x_mm + 90;
xCount += 1;
}
if (row243) {
x226 += row243.x_mm + 90;
xCount += 1;
}
if (row242) {
x226 += row242.x_mm + 90;
xCount += 1;
}
if (row200) {
x226 += row200.x_mm + 154;
xCount += 1;
}
if (row204) {
x226 += row204.x_mm + 160;
xCount += 1;
}
if (xCount > 0) {
x226 = x226 / xCount;
addLog(result, `Ellbogen x226=${x226}`);
addCalculation(result, {
type: "elbowXEstimate",
source: {
rowIds: [row226, row229, row198, row243, row242, row200, row204].filter(Boolean).map(r => r.id)
},
output: {
x226
}
});
}
if (row218 || row219) {
let x219 = 0;
let x219Count = 0;
if (row218) {
x219 += row218.x_mm;
x219Count += 1;
}
if (row219) {
x219 += row219.x_mm;
x219Count += 1;
}
if (x219Count > 0) {
x219 = x219 / x219Count;
addLog(result, `Ellbogen x219=${x219}`);
const xDelta = x219 - x226;
addCalculation(result, {
type: "elbowXComparison",
input: {
x226,
x219,
xDelta
}
});
if (Math.abs(xDelta) < 35) {
addLog(result, `Ellbogen xDelta / 35=${xDelta / 35}`);
const angleRad = Math.asin(xDelta / 35);
const angleDeg = 90 - angleRad * 180 / Math.PI;
addLog(result, `(xEllbow = ${angleDeg.toFixed(2)}° = ${angleRad.toFixed(4)} rad ) aus x von 218 bzw. 219`);
addCalculation(result, {
type: "elbowAngleFromX",
input: {
x226,
x219,
xDelta
},
output: {
angleRad,
angleDeg
}
});
}
}
}
// Ellbow-Rotation wenn X Position OK und 223 oder so bekannt ist.
if(row223 && xCount > 2){
// unterarm-roll => a aus der position von 223
dx = row223.x_mm - x226;
// aus der X Position kann der Unterarm-Winkel (a) berechnet werden
const angleRad = Math.asin(dx / 35);
const angleDeg = 90 - angleRad * 180 / Math.PI;
addLog(result, `(xEllbowRotation = ${angleDeg.toFixed(2)}° = ${angleRad.toFixed(4)} rad ) aus x von 223 und Ellbogen x226`);
addCalculation(result, {
type: "elbowRotationAFromX223",
input: {
x223: row223.x_mm,
x226
},
output: {
angleRad, angleDeg
}
});
}
}
result.summary = {
observationCount: result.observations.length,
calculationCount: result.calculations.length,
featureCount: Object.keys(result.features).length,
commandCount: result.commands.length
};
addLog(
result,
`Berechnung fertig: ${result.summary.featureCount} Features, ${result.summary.commandCount} Commands.`
);
return result;
} catch (err) {
addError(result, err.message, err);
return result;
}
}
if (typeof window !== "undefined") {
window.calculate = calculate;
window.createAnalysisResult = createAnalysisResult;
}
if (typeof module !== "undefined") {
module.exports = {
calculate,
createAnalysisResult
};
}