ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Angle Double

Browse Source
This commit is contained in:
ChK
2026-05-15 14:20:18 +02:00
parent e406f18169
commit 8410ce685b
14 changed files with 465 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
public/calculateAngles.js
View File

@@ -21,12 +21,16 @@ function appendToAnalysis(line) {
function calculateXPos(listIdAndX, jsonRobot){
appendToAnalysis("Calculate XPos");
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 => {
@@ -48,6 +52,8 @@ function calculateXPos(listIdAndX, jsonRobot){
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 };
}
@@ -117,7 +123,63 @@ function optimizeRobot(listFoundMarkers, jsonRobot) {
return withStats;
}
function calculateRotationAngleByTwoMarkers(listIdAndX, 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; }
var a = 1;
var b = 2;
if(jointInfo.axis == [1,0,0]){a=1, b=2}
if(jointInfo.axis == [0,1,0]){a=0, b=2}
if(jointInfo.axis == [0,0,1]){a=0, b=1}
const achsisName = (jointName == "jointB") ? "y" : (jointName == "jointC") ? "z" : "a"
appendToAnalysis(`${achsisName}Ang - Double Started with on dir [${a}, ${b}]`)
markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
if(markerUsed.length === 0){
appendToAnalysis(`${achsisName}Ang - Double no Markers found`)
return null, null;
}
const markerFound = markerUsed
.map(m => [m.id, listIdAndX.get(m.id)])
.filter(v => v !== undefined && v[1] !== undefined);
const pairs = [];
for (let i = 0; i < markerFound.length; i++) {
for (let j = i + 1; j < markerFound.length; j++) {
const [idA, posA] = markerFound[i];
const [idB, posB] = markerFound[j];
if (posA[a] === posB[a] && posA[b] === posB[b]) {
pairs.push([idA, idB]);
}
}
}
if(pairs == []){ appendToAnalysis(`${achsisName}Ang - Double not found`); return; }
for(i in pairs){
m0 = listIdAndX.get(i[0]);
m1 = listIdAndX.get(i[1]);
appendToAnalysis(`${achsisName}Ang - Double ${JSON.stringify(m0)} and ${JSON.stringify(m1)} -- ${i}`);
}
}
function calculateRotationAngle(listIdAndX, jsonRobot, jointName, method = "tan") {
// Achse finden
const jointInfo = jsonRobot.Joints[jointName];
if(!jointInfo){return null, null; }
@@ -143,19 +205,25 @@ function calculateRotationAngle(listIdAndX, jsonRobot, jointName, method = "tan"
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 null, null; }
const markerFound = markerUsed
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 = [];
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];
@@ -168,6 +236,8 @@ const markerFound = markerUsed
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);
@@ -176,15 +246,17 @@ const markerFound = markerUsed
if(method === "sin"){
const db = pos[1][b] - jointB;
var angleOneSin;
if(Math.abs(db) > hypotenuse && db < 1.3 * hypotenuse){angleOneSin = 180}
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 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{
@@ -192,10 +264,11 @@ const markerFound = markerUsed
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; }
@@ -203,25 +276,39 @@ const markerFound = markerUsed
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 } = calculateRotationAngle(foundById, jsonRobot, "jointB", "tan");
try{
calculateRotationAngleByTwoMarkers(foundById, jsonRobot, "jointB");
}
catch(e){
appendToAnalysis(e);
}
jsonRobot.recognized.y = y;
// ToDo ! callibration
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");
jsonRobot.Joints["jointC"].origin[0] = x;
jsonRobot.Joints["jointC"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180)
jsonRobot.Joints["jointC"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180)
return {