ChangeAngles zweites Gelenk

Arbeiten am zweiten Gelenk

public/calculateAngles.js

@@ -133,6 +133,10 @@ function calculateRotationAngle(listIdAndX, jsonRobot, jointName, method = "tan"
     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;
@@ -163,22 +167,31 @@ const markerFound = markerUsed
       const db = pos[1][b] - jointB;
       const angleOne = Math.atan2(db, da) * (180 / Math.PI);
       const deltaAngleTan = angleOne - angleZero;
-      angles.push({ deltaAngleTan });
+      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 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 });
+        angles.push(deltaAngleSin);
       }
       // 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 });
+        angles.push(deltaAngleCos);
       }
     }
   }
@@ -191,7 +204,7 @@ const markerFound = markerUsed
   const deviation = Math.sqrt(angles.reduce((sum, x) => sum + Math.pow(x - average, 2), 0) / n);
 
 
-  return average, deviation;
+  return {average, deviation};
 }
 
 async function calculate(foundMarkers, jsonRobot) {
@@ -199,7 +212,17 @@ async 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 } = calculateRotationAngle(foundById, jsonRobot, "jointB", "tan");
+  jsonRobot.recognized.x = x;
+
+  const { average: y, deviation: vary } = calculateRotationAngle(foundById, jsonRobot, "jointB", "tan");
+
+  jsonRobot.recognized.y = y;
+  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");
+  
 
   return {
     meta: {

public/robot.json

@@ -1,10 +1,12 @@
 {
+    "recognized":{"x":null, "y":null, "z": null, "a":null, "b":null, "c":null, "e": null},
     "Elements":["Board","Base","Arm1","Joint1","Arm2","Finger1","Finger2"], 
+    "ElementLength":{"Arm1":250, "Arm2":250, "Finger1":100, "Finger2":100},
     "Joints":{
         "jointA":{"name":"Slider",  "type":"lninear", "axis":[1,0,0],"parent":"Board","child":"Base"},
         "jointB":{"name":"Shoulder","type":"revolute","axis":[1,0,0],"parent":"Base","child":"Arm1","origin":[-89.5, 115, 53], "originSource":[null, "229_198_Foto_5_2026", "229_198_Foto_5_2026"]},
-        "jointC":{"name":"EllbowLift","type":"revolute","axis":[1,0,0],"parent":"Arm1","child":"Joint1"},
+        "jointC":{"name":"EllbowLift","type":"revolute","axis":[1,0,0],"parent":"Arm1","child":"Joint1", "origin":[null, null, null]},
-        "jointD":{"name":"EllbowTwist","type":"revolute","axis":[0,1,0],"parent":"Joint1","child":"Arm2"}
+        "jointD":{"name":"EllbowTwist","type":"revolute","axis":[0,1,0],"parent":"Joint1","child":"Arm2", "origin":[null, null, null]}
     },
     "MarkerType":"DICT_4X4_250",
     "Marker":[
@@ -26,6 +28,16 @@
 
         {"id":222,"on":"Joint1", "relPos":[0,0, -35]},
         {"id":226,"on":"Joint1", "relPos":[0,0, 35]},
+
+        {"id":228,"on":"Arm2", "relPos":[-24.75, 112, 24.75],  "relPosSource":["Fusion365","Fusion365","Fusion365"]},
+        {"id":  0,"on":"Arm2", "relPos":[-24.75, 182, 24.75],  "relPosSource":["Fusion365","Fusion365","Fusion365"]},
+        {"id":  0,"on":"Arm2", "relPos":[-35,112,0],           "relPosSource":["Fusion365","Fusion365","Fusion365"]},
+        {"id":  0,"on":"Arm2", "relPos":[-35,219,0],           "relPosSource":["Fusion365","Fusion365","Fusion365"]},
+        {"id":223,"on":"Arm2", "relPos":[-28.67,112,-20.08],   "relPosSource":["Fusion365","Fusion365","Fusion365"]},
+        {"id":  0,"on":"Arm2", "relPos":[0,182,-30],           "relPosSource":["Fusion365","Fusion365","Fusion365"]},
+        {"id":218,"on":"Arm2", "relPos":[35,112,0],            "relPosSource":["Fusion365","Fusion365","Fusion365"]},
+        {"id":219,"on":"Arm2", "relPos":[35,219,0],            "relPosSource":["Fusion365","Fusion365","Fusion365"]},
+        {"id":  0,"on":"Arm2", "relPos":[24.75, 182, 24.75],   "relPosSource":["Fusion365","Fusion365","Fusion365"]},
         
         {"id":218,"on":"Finger1","name":"A1","relPos":[-1.70,-25.14, 38.04]},
         {"id":222,"on":"Finger1","name":"B1","relPos":[-14.55, 0.84, 74.79]}

test/calculateAngles2.test.js

@@ -30,6 +30,9 @@ describe("calculate() row223 Ellbow-Rotation Tests", () => {
 
         const result = await calculate(foundMarkers, jsonRobot);
 
+        expect(jsonRobot.recognized.y).toBeDefined();
+        expect(jsonRobot.Joints["jointD"].origin).toBeDefined();
+        expect(jsonRobot.Joints["jointD"].origin[0]).toBeDefined();
         expect(result.status).toBe('ok');
         expect(result.result).toBeDefined();
     });

test/optimizeRobot.test.js

@@ -23,9 +23,7 @@ describe("calculate() row223 Ellbow-Rotation Tests", () => {
 
 
   test('berechnet X-Durchschnitt für Base / Arm1 / Joint1', async () => {
-    //const markersPath1 = path.resolve('./test/snapshots/snapshot_video0_1777958128576_two_cam.json');
     const markersPath1 = path.resolve('./test/snapshots/snapshot_video0_1778406635059_two_cam.json');
-    //const markersPath2 = path.resolve('./test/snapshots/snapshot_video0_1777957783463_two_cam.json');
     const markersPath2 = path.resolve('./test/snapshots/snapshot_video0_1778406621349_two_cam.json');
     const markersPath3 = path.resolve('./test/snapshots/snapshot_video0_1778407153025_two_cam.json');
     const markersPath4 = path.resolve('./test/snapshots/snapshot_video0_1778407171886_two_cam.json');