257 lines
7.0 KiB
JavaScript
257 lines
7.0 KiB
JavaScript
/**
|
|
* calculateAngles module
|
|
*
|
|
* Browser + Server + Jest (CJS) kompatibel
|
|
*/
|
|
|
|
|
|
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 calculateXPos(listIdAndX, jsonRobot){
|
|
appendToAnalysis("Calculate XPos");
|
|
|
|
const partsMovingFixedX = new Set(['Base', 'Arm1', 'Joint1']);
|
|
const markersMovingFixedX = jsonRobot.Marker.filter(m => partsMovingFixedX.has(m.on));
|
|
// Join: Robot-Marker ↔ Found-Marker
|
|
|
|
const markersListeWithRobotInfo = jsonRobot.Marker
|
|
.filter(m => partsMovingFixedX.has(m.on)) // nur relevante Teile
|
|
.map(m => {
|
|
const found = listIdAndX.get(m.id);
|
|
if (!found) return null;
|
|
|
|
return {
|
|
id: m.id,
|
|
relPos: m.relPos, // oder m.relPos, je nach Struktur
|
|
position: found,
|
|
Px: found[0] - m.relPos[0]
|
|
};
|
|
})
|
|
.filter(Boolean); // nulls entfernen
|
|
|
|
const pxValues = markersListeWithRobotInfo.map(m => m.Px);
|
|
const meanPx = pxValues.reduce((sum, x) => sum + x, 0) / pxValues.length;
|
|
|
|
const variancePx = pxValues.reduce((sum, x) => sum + Math.pow(x - meanPx, 2), 0) / pxValues.length;
|
|
const stdDevPx = Math.sqrt(variancePx);
|
|
|
|
return { meanPx, stdDevPx };
|
|
}
|
|
|
|
|
|
function optimizeRobot(listFoundMarkers, jsonRobot) {
|
|
|
|
const map = new Map();
|
|
|
|
for (const foundMarkers of listFoundMarkers) {
|
|
var x_222_226 = null;
|
|
var x_222_226_count = 0;
|
|
|
|
for (const mark of foundMarkers.markers.filter(m => m.id === 226).map(f => [f.id, f.position_mm])) {
|
|
x_222_226 = mark[1][0];
|
|
x_222_226_count++;
|
|
}
|
|
if (x_222_226_count > 0) {
|
|
x_222_226 = x_222_226 / x_222_226_count;
|
|
}
|
|
else{
|
|
continue; // Wenn weder 222 noch 226 gefunden wurden, überspringen
|
|
}
|
|
|
|
for (const mark of foundMarkers.markers.map(f => [f.id, f.position_mm ])){
|
|
const id = mark[0];
|
|
const dx_222_226 = mark[1][0] - x_222_226;
|
|
if (!map.has(id)) {
|
|
map.set(id, []); // Initialisiere mit x_222_226, damit wir später die Abweichung berechnen können
|
|
}
|
|
map.get(id).push(dx_222_226);
|
|
}
|
|
}
|
|
|
|
const result = Array.from(map, ([id, mm]) => ({ id, mm })).filter(m => [198,200,204,229,243].includes(m.id));
|
|
|
|
|
|
const withStats = result.map(entry => {
|
|
const { mm } = entry;
|
|
const n = mm.length;
|
|
|
|
if (n === 0) {
|
|
return {
|
|
...entry,
|
|
n: 0,
|
|
average: null,
|
|
deviation: null,
|
|
result: "X",
|
|
status: "ok"
|
|
};
|
|
}
|
|
|
|
const average = mm.reduce((a, b) => a + b, 0) / n;
|
|
const deviation = Math.sqrt(mm.reduce((sum, x) => sum + Math.pow(x - average, 2), 0) / n);
|
|
|
|
return {
|
|
...entry,
|
|
n,
|
|
average,
|
|
deviation,
|
|
result: "X",
|
|
status: "ok"
|
|
};
|
|
});
|
|
withStats.status = "ok";
|
|
withStats.result = "X";
|
|
|
|
return withStats;
|
|
}
|
|
|
|
function calculateRotationAngle(listIdAndX, jsonRobot, jointName, method = "tan") {
|
|
// Achse finden
|
|
const jointInfo = jsonRobot.Joints[jointName];
|
|
if(!jointInfo){return null, null; }
|
|
if(jointInfo.type !== 'revolute'){ return null, null; }
|
|
if(!(jointInfo.origin)){ return null, null; }
|
|
if(!(jointInfo.axis)){ return null, null; }
|
|
if(!(jointInfo.child)){ return null, null; }
|
|
|
|
var a, b;
|
|
if(jointInfo.axis == [1,0,0]){
|
|
// Auf welche Elemente (x,y,z) zugegriffen wird.
|
|
// bei Rotation um a wird mit y=1 und z=2 gearbeitet.
|
|
a = 2;
|
|
b = 1;
|
|
}
|
|
if(JSON.stringify(jointInfo.axis) === JSON.stringify([0, 1, 0])){
|
|
a = 2;
|
|
b = 0;
|
|
}
|
|
else{
|
|
// Default: Rotationum X Achse
|
|
a = 2;
|
|
b = 1;
|
|
}
|
|
|
|
const jointA = jointInfo.origin[a];
|
|
const jointB = jointInfo.origin[b];
|
|
|
|
markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
|
|
if(markerUsed.length === 0){ return null, null; }
|
|
|
|
|
|
const markerFound = markerUsed
|
|
.map(m => [m.id, listIdAndX.get(m.id)])
|
|
.filter(v => v !== undefined && v[1] !== undefined); // Nur Marker, die gefunden wurden
|
|
|
|
var angles = [];
|
|
|
|
for(const pos of markerFound) {
|
|
const id = pos[0];
|
|
const mRobot = jsonRobot.Marker.filter(m => m.id === id)[0];
|
|
|
|
// Arbeiten mit x,y und Tan
|
|
const angleZero = Math.atan2(mRobot.relPos[b], mRobot.relPos[a]) * (180 / Math.PI);
|
|
if(method === "tan"){
|
|
const da = pos[1][a] - jointA;
|
|
const db = pos[1][b] - jointB;
|
|
const angleOne = Math.atan2(db, da) * (180 / Math.PI);
|
|
const deltaAngleTan = angleOne - angleZero;
|
|
angles.push(deltaAngleTan);
|
|
}
|
|
else{
|
|
const hypotenuse = Math.sqrt(mRobot.relPos[a]**2 + mRobot.relPos[b]**2);
|
|
|
|
// Arbetein mit sin und hypotenuse
|
|
if(method === "sin"){
|
|
const db = pos[1][b] - jointB;
|
|
var angleOneSin;
|
|
if(Math.abs(db) > hypotenuse && db < 1.3 * hypotenuse){angleOneSin = 180}
|
|
else if(Math.abs(db) < hypotenuse && -1*Math.abs(db) > -hypotenuse){
|
|
angleOneSin = Math.asin(db / hypotenuse) * (180 / Math.PI);
|
|
}
|
|
else if(Math.abs(db) < -1*hypotenuse && Math.abs(db) > 1.3*Math.abs(db)){angleOneSin = -180}
|
|
else angleOneSin = NaN;
|
|
|
|
const deltaAngleSin = angleOneSin - angleZero;
|
|
angles.push(deltaAngleSin);
|
|
}
|
|
// Arbeiten mit cos und hypotenuse
|
|
else{
|
|
const db = pos[1][b] - jointB;
|
|
const angleOneCos = Math.acos(db / hypotenuse) * (180 / Math.PI);
|
|
const deltaAngleCos = -(angleOneCos - angleZero);
|
|
angles.push(deltaAngleCos);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
const n = angles.length;
|
|
if(n === 0){ return null, null; }
|
|
|
|
const average = angles.reduce((a, b) => a + b, 0) / n;
|
|
const deviation = Math.sqrt(angles.reduce((sum, x) => sum + Math.pow(x - average, 2), 0) / n);
|
|
|
|
|
|
return {average, deviation};
|
|
}
|
|
|
|
async function calculate(foundMarkers, jsonRobot) {
|
|
|
|
const foundById = new Map(foundMarkers.markers.map(f => [f.id, f.position_mm ]));
|
|
const { meanPx: x, stdDevPx: varx } = calculateXPos(foundById, jsonRobot);
|
|
|
|
jsonRobot.recognized.x = x;
|
|
|
|
const { average: y, deviation: vary } = calculateRotationAngle(foundById, jsonRobot, "jointB", "tan");
|
|
|
|
jsonRobot.recognized.y = y;
|
|
jsonRobot.Joints["jointD"].origin[0] = x;
|
|
jsonRobot.Joints["jointD"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180)
|
|
jsonRobot.Joints["jointD"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180)
|
|
|
|
const { average: a, deviation: vara } = calculateRotationAngle(foundById, jsonRobot, "jointD", "sin");
|
|
|
|
|
|
return {
|
|
meta: {
|
|
module: 'calculateAngles',
|
|
timestamp: new Date().toISOString()
|
|
},
|
|
inputs: {
|
|
markers: foundMarkers ?? null,
|
|
robot: jsonRobot ?? null
|
|
},
|
|
status: 'ok',
|
|
result: {
|
|
x: x,
|
|
varx: varx
|
|
}
|
|
};
|
|
}
|
|
|
|
|
|
// export { calculate, optimizeRobot };
|
|
|
|
if (typeof window !== "undefined") {
|
|
window.calculate = calculate;
|
|
window.optimizeRobot = optimizeRobot;
|
|
}
|
|
|
|
if (typeof module !== "undefined") {
|
|
module.exports = {
|
|
calculate,
|
|
optimizeRobot
|
|
};
|
|
} |