tests
This commit is contained in:
chk
@@ -5,6 +5,43 @@
|
||||
*/
|
||||
|
||||
|
||||
|
||||
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");
|
||||
@@ -123,7 +160,7 @@ function optimizeRobot(listFoundMarkers, jsonRobot) {
|
||||
return withStats;
|
||||
}
|
||||
|
||||
function calculateRotationAngleByTwoMarkers(listIdAndX, jsonRobot, jointName){
|
||||
function calculateAngle_byRelativePositionOfMarker(listRecoginize, jsonRobot, jointName){
|
||||
|
||||
// Achse finden
|
||||
const jointInfo = jsonRobot.Joints[jointName];
|
||||
@@ -133,52 +170,70 @@ function calculateRotationAngleByTwoMarkers(listIdAndX, jsonRobot, jointName){
|
||||
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}]`)
|
||||
appendToAnalysis(`${achsisName}Ang - RelativePosition Started with n=[${jointInfo.axis}]`)
|
||||
|
||||
markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
|
||||
if(markerUsed.length === 0){
|
||||
appendToAnalysis(`${achsisName}Ang - Double no Markers found`)
|
||||
if(markerUsed.length < 2 ){
|
||||
appendToAnalysis(`${achsisName}Ang - RelativePosition no Marker-Pairs on Robot`)
|
||||
return null, null;
|
||||
}
|
||||
|
||||
const markerFound = markerUsed
|
||||
.map(m => [m.id, listIdAndX.get(m.id)])
|
||||
.map(m => [m.id, listRecoginize.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(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))))
|
||||
|
||||
for(i in pairs){
|
||||
m0 = listIdAndX.get(i[0]);
|
||||
m1 = listIdAndX.get(i[1]);
|
||||
const markerMap = Object.fromEntries(
|
||||
jsonRobot.Marker.map(m => [m.id, m])
|
||||
);
|
||||
|
||||
appendToAnalysis(`${achsisName}Ang - Double ${JSON.stringify(m0)} and ${JSON.stringify(m1)} -- ${i}`);
|
||||
}
|
||||
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,
|
||||
rotationAngle: angleMarker - angleFound,
|
||||
lengthProjM,
|
||||
lengthProjF
|
||||
};
|
||||
});
|
||||
|
||||
|
||||
appendToAnalysis(`${achsisName}Ang - Pairs with Angles: ${JSON.stringify(pairsWithAngles)}`);
|
||||
}
|
||||
|
||||
function calculateRotationAngle(listIdAndX, jsonRobot, jointName, method = "tan") {
|
||||
function calculateAngle_byPosAndAxis(listIdAndX, jsonRobot, jointName, method = "tan") {
|
||||
|
||||
// Achse finden
|
||||
const jointInfo = jsonRobot.Joints[jointName];
|
||||
@@ -289,21 +344,17 @@ async function calculate(foundMarkers, jsonRobot) {
|
||||
|
||||
jsonRobot.recognized.x = x;
|
||||
|
||||
const { average: y, deviation: vary } = calculateRotationAngle(foundById, jsonRobot, "jointB", "tan");
|
||||
const { average: y, deviation: vary } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointB", "tan");
|
||||
|
||||
calculateAngle_byRelativePositionOfMarker(foundById, jsonRobot, "jointB");
|
||||
|
||||
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");
|
||||
const { average: a, deviation: vara } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointD", "sin");
|
||||
|
||||
jsonRobot.Joints["jointC"].origin[0] = x;
|
||||
jsonRobot.Joints["jointC"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180)
|
||||
|
||||
@@ -23,10 +23,10 @@
|
||||
{"id":201,"on":"Base","relPos":[-164.8, 97.5, 74.5], "relPosSource":["226_FotoAverage_5_2026",null,null]},
|
||||
{"id":204,"on":"Base","relPos":[-158.5,152.5,111]},
|
||||
|
||||
{"id":198,"on":"Arm1", "relPos":[-93.4,-160, 35], "relPosSource":["226_FotoAverage_5_2026",null,null]},
|
||||
{"id":229,"on":"Arm1", "relPos":[-90.6,-250, 35], "relPosSource":["226_FotoAverage_5_2026",null,null]},
|
||||
{"id":242,"on":"Arm1", "relPos":[-89.5,-250,-35]},
|
||||
{"id":243,"on":"Arm1", "relPos":[-89.5,-285, 0]},
|
||||
{"id":198,"on":"Arm1","relPos":[-89.5,-160, 35],"relPosSource":["Fusion",null,null]},
|
||||
{"id":229,"on":"Arm1","relPos":[-89.5,-250, 35],"relPosSource":["Fusion",null,null]},
|
||||
{"id":242,"on":"Arm1","relPos":[-89.5,-250,-35]},
|
||||
{"id":243,"on":"Arm1","relPos":[-89.5,-285, 0]},
|
||||
|
||||
{"id":222,"on":"Joint1", "relPos":[0,0, -35]},
|
||||
{"id":226,"on":"Joint1", "relPos":[0,0, 35]},
|
||||
|
||||
Reference in New Issue
Block a user