diff --git a/public/calculateAngles.js b/public/calculateAngles.js index 8f3f2d3..00ee7ee 100644 --- a/public/calculateAngles.js +++ b/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 - 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 } = calculateAngle_byPosAndAxis(foundById, jsonRobot, "jointD", "sin"); + 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["jointB"].origin[1]; + jsonRobot.Joints["jointD"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180) + jsonRobot.Joints["jointB"].origin[2]; - jsonRobot.Joints["jointC"].origin[0] = x; - jsonRobot.Joints["jointC"].origin[1] = -jsonRobot.ElementLength["Arm1"]*Math.cos(y*Math.PI/180) - jsonRobot.Joints["jointC"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180) - + 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["jointB"].origin[1] + jsonRobot.Joints["jointC"].origin[2] = jsonRobot.ElementLength["Arm1"]*Math.sin(y*Math.PI/180)+ jsonRobot.Joints["jointB"].origin[2] + } return { diff --git a/test/calculateAngles4.synthesized.a.test.js b/test/calculateAngles4.synthesized.a.test.js new file mode 100644 index 0000000..577219d --- /dev/null +++ b/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); + + }) + + + +}); \ No newline at end of file diff --git a/test/calculateAngles4.synthesized.y.test.js b/test/calculateAngles4.synthesized.y.test.js new file mode 100644 index 0000000..a8d45f9 --- /dev/null +++ b/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); + } + }) +}); \ No newline at end of file