405 lines
12 KiB
JavaScript
405 lines
12 KiB
JavaScript
/**
|
|
* calculateAngles module
|
|
*
|
|
* Browser + Server + Jest (CJS) kompatibel
|
|
*/
|
|
|
|
|
|
|
|
function v_multiplication(vec, scalar){
|
|
if(vec.length == 3){
|
|
return [vec[0]*scalar, vec[1]*scalar, vec[2]*scalar]
|
|
}
|
|
}
|
|
|
|
function v_dot(v1, v2) {
|
|
return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
|
|
}
|
|
|
|
function v_length(v) {
|
|
return Math.sqrt(v_dot(v, v));
|
|
}
|
|
|
|
function v_minus(v1, v2){
|
|
if(v1.length == 3 && v2.length==3)
|
|
return [v1[0] - v2[0], v1[1] - v2[1], v1[2] - v2[2]]
|
|
else
|
|
throw "Vektor Length is wrong";
|
|
}
|
|
|
|
function v_angle(v1, v2) {
|
|
const cosTheta = v_dot(v1, v2) / (v_length(v1) * v_length(v2));
|
|
|
|
// kleine numerische Fehler abfangen
|
|
const clamped = Math.max(-1, Math.min(1, cosTheta));
|
|
|
|
return Math.acos(clamped); // Winkel in Radiant
|
|
}
|
|
|
|
function v_getProjectionToPlane(vec, normal){
|
|
return v_minus(vec, v_multiplication(normal, v_dot(vec, normal)) )
|
|
}
|
|
|
|
|
|
|
|
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("xPos");
|
|
|
|
const partsMovingFixedX = new Set(['Base', 'Arm1', 'Joint1']);
|
|
const markersMovingFixedX = jsonRobot.Marker.filter(m => partsMovingFixedX.has(m.on));
|
|
|
|
appendToAnalysis(`xPos found ${markersMovingFixedX.length} Markers`);
|
|
// 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);
|
|
|
|
|
|
appendToAnalysis(`xPos > ${meanPx.toFixed(2)} ± ${stdDevPx.toFixed(2)}`)
|
|
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 calculateAngle_byRelativePositionOfMarker(listRecoginize, jsonRobot, jointName){
|
|
|
|
// Achse finden
|
|
const jointInfo = jsonRobot.Joints[jointName];
|
|
if(!jointInfo){return null, null; }
|
|
if(jointInfo.type !== 'revolute'){ return null, null; }
|
|
|
|
if(!(jointInfo.axis)){ return null, null; }
|
|
if(!(jointInfo.child)){ return null, null; }
|
|
|
|
const achsisName = (jointName == "jointB") ? "y" : (jointName == "jointC") ? "z" : "a"
|
|
|
|
appendToAnalysis(`${achsisName}Ang - RelativePosition Started with n=[${jointInfo.axis}]`)
|
|
|
|
markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
|
|
if(markerUsed.length < 2 ){
|
|
appendToAnalysis(`${achsisName}Ang - RelativePosition no Marker-Pairs on Robot`)
|
|
return null, null;
|
|
}
|
|
|
|
const markerFound = markerUsed
|
|
.map(m => [m.id, listRecoginize.get(m.id)])
|
|
.filter(v => v !== undefined && v[1] !== undefined);
|
|
|
|
if(markerFound.length < 2 ){
|
|
appendToAnalysis(`${achsisName}Ang - RelativePosition no Marker-Pairs found in Fotos`)
|
|
return null, null;
|
|
}
|
|
|
|
|
|
const pairs = markerFound.flatMap((a, i) =>
|
|
markerFound.slice(i + 1).map(b => [a[0], b[0]])
|
|
);
|
|
|
|
|
|
if(pairs == []){ appendToAnalysis(`${achsisName}Ang - Double not found`); return; }
|
|
|
|
const n = v_multiplication(jointInfo.axis, 1/(Math.sqrt(v_dot(jointInfo.axis,jointInfo.axis))))
|
|
|
|
const markerMap = Object.fromEntries(
|
|
jsonRobot.Marker.map(m => [m.id, m])
|
|
);
|
|
|
|
const pairsWithAngles = pairs.map(([id0, id1]) => {
|
|
|
|
// Point in real World
|
|
const m0p = v_getProjectionToPlane(listRecoginize.get(id0), n);
|
|
const m1p = v_getProjectionToPlane(listRecoginize.get(id1), n);
|
|
|
|
// Point in Robot Model
|
|
const f0p = v_getProjectionToPlane(markerMap[id0].relPos, n);
|
|
const f1p = v_getProjectionToPlane(markerMap[id1].relPos, n);
|
|
|
|
const angleMarker = v_angle(m0p, m1p) * 180 / Math.PI;
|
|
const angleFound = v_angle(f0p, f1p) * 180 / Math.PI;
|
|
|
|
const lengthProjM = v_length(v_minus(m0p, m1p));
|
|
const lengthProjF = v_length(v_minus(f0p, f1p));
|
|
|
|
return {
|
|
pair: [id0, id1],
|
|
angleMarker,
|
|
angleFound,
|
|
angleRotation: angleMarker - angleFound,
|
|
lengthProjM,
|
|
lengthProjF
|
|
};
|
|
});
|
|
|
|
|
|
|
|
const formatted = JSON.stringify(pairsWithAngles, (key, value) => {
|
|
if (typeof value === "number") {
|
|
return Number(value.toFixed(3));
|
|
}
|
|
return value;
|
|
});
|
|
|
|
appendToAnalysis(`${achsisName}Ang - Pairs with Angles: ${formatted}`);
|
|
}
|
|
|
|
function calculateAngle_byPosAndAxis(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 achsisName = (jointName == "jointB") ? "y" : (jointName == "jointC") ? "z" : "a"
|
|
appendToAnalysis(`${achsisName}Ang - Started with ${method} on dir [${a}, ${b}]`)
|
|
|
|
const jointA = jointInfo.origin[a];
|
|
const jointB = jointInfo.origin[b];
|
|
|
|
appendToAnalysis(`${achsisName}Ang - Axis: (${jointA.toFixed(2)}, ${jointB.toFixed(2)})`);
|
|
|
|
markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
|
|
if(markerUsed.length === 0){ return {average: null, deviation: null}; }
|
|
|
|
|
|
const markerFound = markerUsed
|
|
.map(m => [m.id, listIdAndX.get(m.id)])
|
|
.filter(v => v !== undefined && v[1] !== undefined); // Nur Marker, die gefunden wurden
|
|
|
|
appendToAnalysis(`${achsisName}Ang found ${markerFound.length} markers`);
|
|
|
|
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);
|
|
|
|
appendToAnalysis(`${achsisName}Ang tan: ${mRobot.id} Pos=(${pos[1][a].toFixed(2)}, ${pos[1][b].toFixed(2)}) Δ=(${da.toFixed(2)},${db.toFixed(2)}) α°=${angleZero.toFixed(2)} ${achsisName}=${deltaAngleTan.toFixed(2)}`);
|
|
}
|
|
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);
|
|
|
|
appendToAnalysis(`${achsisName}Ang sin: ${mRobot.id} Pos=(${pos[1][a].toFixed(2)}, ${pos[1][b].toFixed(2)}) Δ=${db.toFixed(2)} hyp=${hypotenuse.toFixed(2)} α°=${angleOneSin.toFixed(2)} ${achsisName}=${deltaAngleSin.toFixed(2)}`);
|
|
}
|
|
// 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);
|
|
|
|
appendToAnalysis(`${achsisName}Ang cos: ${mRobot.id} Pos=(${pos[1][a].toFixed(2)}, ${pos[1][b].toFixed(2)}) Δ=${db.toFixed(2)} hyp=${hypotenuse.toFixed(2)} α°=${angleOneCos.toFixed(2)} ${achsisName}=${deltaAngleCos.toFixed(2)}`);
|
|
}
|
|
}
|
|
}
|
|
|
|
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);
|
|
|
|
appendToAnalysis(`${achsisName}Ang ${achsisName}=${average.toFixed(2)} ± ${deviation.toFixed(2)}`)
|
|
return {average, deviation};
|
|
}
|
|
|
|
async function calculate(foundMarkers, jsonRobot) {
|
|
|
|
if(foundMarkers == undefined || jsonRobot == undefined){console.warn("calculateAngles mit falschen Parametern aufgerufen.");}
|
|
|
|
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 } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointB", "tan");
|
|
|
|
calculateAngle_byRelativePositionOfMarker(foundById, jsonRobot, "jointB");
|
|
|
|
jsonRobot.recognized.y = y;
|
|
// ToDo ! callibration
|
|
if(jsonRobot.Joints["jointD"] !== undefined && jsonRobot.ElementLength !== undefined){
|
|
jsonRobot.Joints["jointD"].origin[0] = x;
|
|
jsonRobot.Joints["jointD"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180) + jsonRobot.Joints["jointB"].origin[1];
|
|
jsonRobot.Joints["jointD"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180) + jsonRobot.Joints["jointB"].origin[2];
|
|
|
|
const { average: a, deviation: vara } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointD", "sin");
|
|
}
|
|
if(jsonRobot.Joints["jointC"] !== undefined && jsonRobot.ElementLength !== undefined){
|
|
jsonRobot.Joints["jointC"].origin[0] = x;
|
|
jsonRobot.Joints["jointC"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180)+ jsonRobot.Joints["jointB"].origin[1]
|
|
jsonRobot.Joints["jointC"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180)+ jsonRobot.Joints["jointB"].origin[2]
|
|
}
|
|
|
|
|
|
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
|
|
};
|
|
} |