Angle y

Arbeiten um aus der robot Configuration die Y Winkel-Infos zu erhalten

public/calculateAngles.js

@@ -80,10 +80,7 @@ function optimizeRobot(listFoundMarkers, jsonRobot) {
     }
 
     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
-    );
+    const deviation = Math.sqrt(mm.reduce((sum, x) => sum + Math.pow(x - average, 2), 0) / n);
 
     return {
       ...entry,
@@ -95,7 +92,7 @@ function optimizeRobot(listFoundMarkers, jsonRobot) {
 
   return withStats;
 }
-function calculateRotAngle(listIdAndX, jsonRobot, jointName){
+function calculateRotationAngle(listIdAndX, jsonRobot, jointName, method = "tan") {
   // Achse finden
   const jointInfo = jsonRobot.Joints[jointName];
   if(!jointInfo){return null, null; }    
@@ -104,19 +101,72 @@ function calculateRotAngle(listIdAndX, jsonRobot, jointName){
   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;
+  }
+  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; }
 
-  var markerFound = []
-  for (const m of markerUsed){
-    markerFound.push(listIdAndX.get(m.id));
+  
+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 angleOneSin = Math.asin(db / hypotenuse) * (180 / Math.PI);
+        const deltaAngleSin = angleOneSin - angleZero;
+        angles.push({ deltaAngleSin });
+      }
+      // Arbeiten mit cos und hypotenuse
+      else{
+        const angleOneCos = Math.acos(db / hypotenuse) * (180 / Math.PI);
+        const deltaAngleCos = -(angleOneCos - angleZero);
+        angles.push({ deltaAngleCos });
+      }
+    }
   }
 
   
-  var angle = 0;
-  var varAngle = 0;
+  const n = angles.length;
+  if(n === 0){ return null, null; }
 
-  return angle, varAngle;
+  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;
 }
 
 function calculate(foundMarkers, jsonRobot) {
@@ -124,7 +174,7 @@ 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);
 
-  const { meanY: y, stdDevY: vary } = calculateRotAngle(foundById, jsonRobot, "jointB");
+  const { meanY: y, stdDevY: vary } = calculateRotationAngle(foundById, jsonRobot, "jointB", "tan");
 
   return {
     meta: {