655 lines
16 KiB
JavaScript
Executable File
655 lines
16 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;
|
|
}
|
|
|
|
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;
|
|
|
|
// Oberarm / shoulder
|
|
const row243 = getRow(243, 3);
|
|
const row229 = getRow(229, 3);
|
|
const row198 = getRow(198, 3);
|
|
const row197 = getRow(197);
|
|
const row218 = getRow(218, 3);
|
|
const row219 = getRow(219, 3);
|
|
const row226 = getRow(226, 3);
|
|
const row242 = getRow(242, 3);
|
|
const row200 = getRow(200, 3);
|
|
const row204 = getRow(204, 3);
|
|
const row222 = getRow(222, 3);
|
|
|
|
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
|
|
);
|
|
}
|
|
|
|
// 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(row222){
|
|
x226 += row222.x_mm * 5;
|
|
xCount += 5;
|
|
}
|
|
if (row226) {
|
|
x226 += row226.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
|
|
}
|
|
});
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
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,
|
|
calculateAngleFromPosition,
|
|
calculateAngleFromRollColumn,
|
|
calculateAngleFromRelativePosition
|
|
};
|
|
} |