diff --git a/public/calculateAngles.js b/public/calculateAngles.js index 5da8e61..d3cde74 100644 --- a/public/calculateAngles.js +++ b/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: { diff --git a/public/robot.json b/public/robot.json index 5285df2..db82d94 100644 --- a/public/robot.json +++ b/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"}, - "jointD":{"name":"EllbowTwist","type":"revolute","axis":[0,1,0],"parent":"Joint1","child":"Arm2"} + "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", "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]} diff --git a/test/calculateAngles2.test.js b/test/calculateAngles2.test.js index c990241..51018da 100644 --- a/test/calculateAngles2.test.js +++ b/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(); }); diff --git a/test/optimizeRobot.test.js b/test/optimizeRobot.test.js index 3abe506..efe32f8 100644 --- a/test/optimizeRobot.test.js +++ b/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');