Koordinaten + offset

bei den Achsen-Koordinaten hat ein Offset gefehlt.

Zudem braucht es systematische Tests

public/calculateAngles.js

@@ -223,14 +223,22 @@ function calculateAngle_byRelativePositionOfMarker(listRecoginize, jsonRobot, jo
       pair: [id0, id1],
       angleMarker,
       angleFound,
-      rotationAngle: angleMarker - angleFound,
+      angleRotation: angleMarker - angleFound,
       lengthProjM,
       lengthProjF
     };
   });
 
 
-  appendToAnalysis(`${achsisName}Ang - Pairs with Angles: ${JSON.stringify(pairsWithAngles)}`);
+  
+  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") {
@@ -269,7 +277,7 @@ function calculateAngle_byPosAndAxis(listIdAndX, jsonRobot, jointName, method =
   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; }
+  if(markerUsed.length === 0){ return {average: null, deviation: null}; }
 
   
   const markerFound = markerUsed
@@ -350,16 +358,18 @@ async function calculate(foundMarkers, jsonRobot) {
   
   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["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["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["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["jointC"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180)+ jsonRobot.Joints["jointB"].origin[2]
-  
+  }
   
 
   return {

test/calculateAngles4.synthesized.a.test.js

@@ -0,0 +1,55 @@
+const { calculate } = require('../public/calculateAngles');
+
+
+
+function foundMarkers(y, robot, a=0, translate=[0,0,0]){
+    
+    const alphaPi = y * Math.PI / 180;
+
+    const pos20 = [0,Math.sin(alphaPi)*10,Math.cos(alphaPi)*10]
+    
+    m = {"markers": [
+        {"id": 20, "position_mm": pos20 }
+    ]}
+
+    return m
+}
+
+robot = {
+    "Elements":["Base", "Arm1"],
+    "ElementLength":{"Arm1":250, "Joint1": 89.5, "Arm2":250, "Finger1":100, "Finger2":100},
+    "Joints":{
+        "jointB":{"name":"Shoulder","type":"revolute","axis":[1,0,0],"parent":"Base","child":"Arm1","origin":[-89.5,0,0], "originSource":[null, "test", "test"]},
+        "jointC":{"name":"Ellbow","type":"revolute","axis":[1,0,0],"parent":"Arm1","child":"Joint1", "origin":[-89.5, -250, 0]},
+        "jointD":{"name":"EllbowTwist","type":"revolute","axis":[0,1,0],"parent":"Joint1","child":"Arm2", "origin":[0, -250, 0]}
+    },
+    "Marker":[
+        {"id":20, "on":"Arm1", "relPos":[0,0,10]},
+        {"id":40, "on":"Arm2", "relPos":[0,200,35]},
+        {"id":41, "on":"Arm2", "relPos":[0,160,35]}
+    ],
+    "recognized":{"x":null, "y":null, "z":null}
+}
+
+
+
+function normalizeAngleDeg(angle) {
+  return ((angle % 360) + 360) % 360;
+}
+
+
+describe('calculateAngles minimal test', () => {
+  it('should run calculate() artificially created robot & testData', async () => {
+  
+
+            var result =  calculate(foundMarkers(45, robot), robot);
+
+            expect(result).toBeDefined();
+            expect(robot.recognized.y).toBeDefined();
+            expect(robot.recognized.y).toBeCloseTo(45,2);
+         
+  })
+
+
+
+});

test/calculateAngles4.synthesized.y.test.js

@@ -0,0 +1,56 @@
+const { calculate } = require('../public/calculateAngles');
+
+
+
+function foundMarkers(alpha, robot){
+    const alphaPi = alpha * Math.PI / 180;
+
+    m = {"markers": [{"id": 0, "position_mm":[0,Math.sin(alphaPi)*10,Math.cos(alphaPi)*10]}]}
+
+    return m
+}
+
+robot = {
+    "Elements":["Base", "Arm1"],
+    "ElementLength":{"Arm1":250, "Arm2":250, "Finger1":100, "Finger2":100},
+    "Joints":{
+        "jointB":{"name":"Shoulder","type":"revolute","axis":[1,0,0],"parent":"Base","child":"Arm1","origin":[0,0,0], "originSource":[null, "test", "test"]},
+    },
+    "Marker":[
+        {"id":0, "on":"Arm1", "relPos":[0,0,10]}
+    ],
+    "recognized":{"x":null, "y":null, "z":null}
+}
+
+
+
+function normalizeAngleDeg(angle) {
+  return ((angle % 360) + 360) % 360;
+}
+
+
+describe('calculateAngles minimal test', () => {
+  it('should run calculate() artificially created robot & testData', async () => {
+  
+        for (let alpha = -180; alpha <= 181; alpha += 15) {
+
+            var result =  calculate(foundMarkers(alpha, robot), robot);
+
+            expect(result).toBeDefined();
+            expect(robot.recognized.y).toBeDefined();
+            expect(robot.recognized.y).toBeCloseTo(alpha,2);
+        } 
+  })
+
+  it('should run calculate() artificially created robot & testData 2', async () => {
+  
+        for (let alpha = -30; alpha <= 761; alpha += 15) {
+
+            var result =  calculate(foundMarkers(alpha, robot), robot);
+
+            expect(result).toBeDefined();
+            expect(robot.recognized.y).toBeDefined();
+            expect(normalizeAngleDeg(robot.recognized.y)).toBeCloseTo(normalizeAngleDeg(alpha),2);
+        } 
+  })
+});