From e6f3786ffd6e291c2abf10b9521609ca29a4e47d Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Sat, 3 Feb 2024 18:01:21 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=8F=83=E2=80=8D=E2=99=80=EF=B8=8F=20Adds?= =?UTF-8?q?=20mock=20spot=20server=20to=20develop=20of=20API?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mock/.gitignore | 1 + mock/kinematic.js | 397 ++++++++++++++++++++++++++++++++++++ mock/package.json | 18 ++ mock/pnpm-lock.yaml | 483 ++++++++++++++++++++++++++++++++++++++++++++ mock/server.js | 316 +++++++++++++++++++++++++++++ 5 files changed, 1215 insertions(+) create mode 100644 mock/.gitignore create mode 100644 mock/kinematic.js create mode 100644 mock/package.json create mode 100644 mock/pnpm-lock.yaml create mode 100644 mock/server.js diff --git a/mock/.gitignore b/mock/.gitignore new file mode 100644 index 0000000..30bc162 --- /dev/null +++ b/mock/.gitignore @@ -0,0 +1 @@ +/node_modules \ No newline at end of file diff --git a/mock/kinematic.js b/mock/kinematic.js new file mode 100644 index 0000000..41de371 --- /dev/null +++ b/mock/kinematic.js @@ -0,0 +1,397 @@ +export default class Kinematic { + l1; + l2; + l3; + l4; + + L; + W; + + constructor() { + this.l1 = 50; + this.l2 = 20; + this.l3 = 120; + this.l4 = 155; + + this.L = 140; + this.W = 75; + } + + bodyIK(omega, phi, psi, xm, ym, zm) { + const { cos, sin } = Math; + + const Rx = [ + [1, 0, 0, 0], + [0, cos(omega), -sin(omega), 0], + [0, sin(omega), cos(omega), 0], + [0, 0, 0, 1], + ]; + const Ry = [ + [cos(phi), 0, sin(phi), 0], + [0, 1, 0, 0], + [-sin(phi), 0, cos(phi), 0], + [0, 0, 0, 1], + ]; + const Rz = [ + [cos(psi), -sin(psi), 0, 0], + [sin(psi), cos(psi), 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ]; + const Rxyz = this.matrixMultiply(this.matrixMultiply(Rx, Ry), Rz); + + const T = [ + [0, 0, 0, xm], + [0, 0, 0, ym], + [0, 0, 0, zm], + [0, 0, 0, 0], + ]; + const Tm = this.matrixAdd(T, Rxyz); + + const sHp = sin(Math.PI / 2); + const cHp = cos(Math.PI / 2); + + return [ + this.matrixMultiply(Tm, [ + [cHp, 0, sHp, this.L / 2], + [0, 1, 0, 0], + [-sHp, 0, cHp, this.W / 2], + [0, 0, 0, 1], + ]), + this.matrixMultiply(Tm, [ + [cHp, 0, sHp, this.L / 2], + [0, 1, 0, 0], + [-sHp, 0, cHp, -this.W / 2], + [0, 0, 0, 1], + ]), + this.matrixMultiply(Tm, [ + [cHp, 0, sHp, -this.L / 2], + [0, 1, 0, 0], + [-sHp, 0, cHp, this.W / 2], + [0, 0, 0, 1], + ]), + this.matrixMultiply(Tm, [ + [cHp, 0, sHp, -this.L / 2], + [0, 1, 0, 0], + [-sHp, 0, cHp, -this.W / 2], + [0, 0, 0, 1], + ]), + ]; + } + + legIK(point) { + const [x, y, z] = point; + const { atan2, cos, sin, sqrt, acos } = Math; + + let F; + + try { + F = sqrt(x ** 2 + y ** 2 - this.l1 ** 2); + if (isNaN(F)) throw new Error("F is NaN"); + } catch (error) { + F = this.l1; + } + const G = F - this.l2; + const H = sqrt(G ** 2 + z ** 2); + + const theta1 = -atan2(y, x) - atan2(F, -this.l1); + let theta3; + try { + theta3 = acos( + (H ** 2 - this.l3 ** 2 - this.l4 ** 2) / (2 * this.l3 * this.l4) + ); + if (isNaN(theta3)) throw new Error("theta3 is NaN"); + } catch (error) { + theta3 = 0; + } + const theta2 = + atan2(z, G) - + atan2(this.l4 * sin(theta3), this.l3 + this.l4 * cos(theta3)); + + return [theta1, theta2, theta3]; + } + + matrixMultiply(a, b) { + const result = []; + + for (let i = 0; i < a.length; i++) { + const row = []; + + for (let j = 0; j < b[0].length; j++) { + let sum = 0; + + for (let k = 0; k < a[i].length; k++) { + sum += a[i][k] * b[k][j]; + } + + row.push(sum); + } + + result.push(row); + } + + return result; + } + + multiplyVector(matrix, vector) { + const rows = matrix.length; + const cols = matrix[0].length; + const vectorLength = vector.length; + + if (cols !== vectorLength) { + throw new Error( + "Matrix and vector dimensions do not match for multiplication." + ); + } + + const result = []; + + for (let i = 0; i < rows; i++) { + let sum = 0; + + for (let j = 0; j < cols; j++) { + sum += matrix[i][j] * vector[j]; + } + + result.push(sum); + } + + return result; + } + + matrixAdd(a, b) { + const result = []; + + for (let i = 0; i < a.length; i++) { + const row = []; + + for (let j = 0; j < a[i].length; j++) { + row.push(a[i][j] + b[i][j]); + } + + result.push(row); + } + + return result; + } + + calcLegPoints(angles) { + const [theta1, theta2, theta3] = angles; + const theta23 = theta2 + theta3; + + const T0 = [0, 0, 0, 1]; + const T1 = this.vectorAdd(T0, [ + -this.l1 * Math.cos(theta1), + this.l1 * Math.sin(theta1), + 0, + 0, + ]); + const T2 = this.vectorAdd(T1, [ + -this.l2 * Math.sin(theta1), + -this.l2 * Math.cos(theta1), + 0, + 0, + ]); + const T3 = this.vectorAdd(T2, [ + -this.l3 * Math.sin(theta1) * Math.cos(theta2), + -this.l3 * Math.cos(theta1) * Math.cos(theta2), + this.l3 * Math.sin(theta2), + 0, + ]); + const T4 = this.vectorAdd(T3, [ + -this.l4 * Math.sin(theta1) * Math.cos(theta23), + -this.l4 * Math.cos(theta1) * Math.cos(theta23), + this.l4 * Math.sin(theta23), + 0, + ]); + + return [T0, T1, T2, T3, T4]; + } + + calcIK(Lp, angles, center) { + const [omega, phi, psi] = angles; + const [xm, ym, zm] = center; + + const [Tlf, Trf, Tlb, Trb] = this.bodyIK(omega, phi, psi, xm, ym, zm); + + const Ix = [ + [-1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1], + ]; + + return [ + this.legIK(this.multiplyVector(this.matrixInverse(Tlf), Lp[0])), + this.legIK( + this.multiplyVector( + Ix, + this.multiplyVector(this.matrixInverse(Trf), Lp[1]) + ) + ), + this.legIK(this.multiplyVector(this.matrixInverse(Tlb), Lp[2])), + this.legIK( + this.multiplyVector( + Ix, + this.multiplyVector(this.matrixInverse(Trb), Lp[3]) + ) + ), + ]; + } + + vectorAdd(a, b) { + return a.map((val, index) => val + b[index]); + } + + matrixInverse(matrix) { + const det = this.determinant(matrix); + const adjugate = this.adjugate(matrix); + const scalar = 1 / det; + const inverse = []; + + for (let i = 0; i < matrix.length; i++) { + const row = []; + + for (let j = 0; j < matrix[i].length; j++) { + row.push(adjugate[i][j] * scalar); + } + + inverse.push(row); + } + + return inverse; + } + + determinant(matrix) { + if (matrix.length !== matrix[0].length) { + throw new Error("The matrix is not square."); + } + + if (matrix.length === 2) { + return matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]; + } + + let det = 0; + + for (let i = 0; i < matrix.length; i++) { + const sign = i % 2 === 0 ? 1 : -1; + const subMatrix = []; + + for (let j = 1; j < matrix.length; j++) { + const row = []; + + for (let k = 0; k < matrix.length; k++) { + if (k !== i) { + row.push(matrix[j][k]); + } + } + + subMatrix.push(row); + } + + det += sign * matrix[0][i] * this.determinant(subMatrix); + } + + return det; + } + + adjugate(matrix) { + if (matrix.length !== matrix[0].length) { + throw new Error("The matrix is not square."); + } + + const adjugate = []; + + for (let i = 0; i < matrix.length; i++) { + const row = []; + + for (let j = 0; j < matrix[i].length; j++) { + const sign = (i + j) % 2 === 0 ? 1 : -1; + const subMatrix = []; + + for (let k = 0; k < matrix.length; k++) { + if (k !== i) { + const subRow = []; + + for (let l = 0; l < matrix.length; l++) { + if (l !== j) { + subRow.push(matrix[k][l]); + } + } + + subMatrix.push(subRow); + } + } + + const cofactor = sign * this.determinant(subMatrix); + row.push(cofactor); + } + + adjugate.push(row); + } + + return this.transpose(adjugate); + } + + transpose(matrix) { + const transposed = []; + + for (let i = 0; i < matrix.length; i++) { + const row = []; + + for (let j = 0; j < matrix[i].length; j++) { + row.push(matrix[j][i]); + } + + transposed.push(row); + } + + return transposed; + } +} + +class ForwardKinematics { + l1; + l2; + l3; + l4; + + constructor() { + this.l1 = 50; + this.l2 = 20; + this.l3 = 120; + this.l4 = 155; + } + + calculateFootpoint(theta1, theta2, theta3) { + const { cos, sin } = Math; + + const x = + this.l1 * cos(theta1) + + this.l2 * cos(theta1) + + this.l3 * cos(theta1 + theta2) + + this.l4 * cos(theta1 + theta2 + theta3); + const y = + this.l1 * sin(theta1) + + this.l2 * sin(theta1) + + this.l3 * sin(theta1 + theta2) + + this.l4 * sin(theta1 + theta2 + theta3); + const z = 0; + + return [x, y, z]; + } + + calculateFootpoints(angles) { + const footpoints = []; + + for (let i = 0; i < angles.length; i += 3) { + const theta1 = angles[i]; + const theta2 = angles[i + 1]; + const theta3 = angles[i + 2]; + const footpoint = this.calculateFootpoint(theta1, theta2, theta3); + footpoints.push(footpoint); + } + + return footpoints; + } +} diff --git a/mock/package.json b/mock/package.json new file mode 100644 index 0000000..ab6414c --- /dev/null +++ b/mock/package.json @@ -0,0 +1,18 @@ +{ + "name": "mock", + "version": "1.0.0", + "description": "", + "main": "index.js", + "scripts": { + "test": "echo \"Error: no test specified\" && exit 1" + }, + "keywords": [], + "author": "", + "license": "ISC", + "type": "module", + "dependencies": { + "cors": "^2.8.5", + "express": "^4.18.2", + "ws": "^8.16.0" + } +} diff --git a/mock/pnpm-lock.yaml b/mock/pnpm-lock.yaml new file mode 100644 index 0000000..30d71be --- /dev/null +++ b/mock/pnpm-lock.yaml @@ -0,0 +1,483 @@ +lockfileVersion: '6.0' + +settings: + autoInstallPeers: true + excludeLinksFromLockfile: false + +dependencies: + cors: + specifier: ^2.8.5 + version: 2.8.5 + express: + specifier: ^4.18.2 + version: 4.18.2 + ws: + specifier: ^8.16.0 + version: 8.16.0 + +packages: + + /accepts@1.3.8: + resolution: {integrity: sha512-PYAthTa2m2VKxuvSD3DPC/Gy+U+sOA1LAuT8mkmRuvw+NACSaeXEQ+NHcVF7rONl6qcaxV3Uuemwawk+7+SJLw==} + engines: {node: '>= 0.6'} + dependencies: + mime-types: 2.1.35 + negotiator: 0.6.3 + dev: false + + /array-flatten@1.1.1: + resolution: {integrity: sha512-PCVAQswWemu6UdxsDFFX/+gVeYqKAod3D3UVm91jHwynguOwAvYPhx8nNlM++NqRcK6CxxpUafjmhIdKiHibqg==} + dev: false + + /body-parser@1.20.1: + resolution: {integrity: sha512-jWi7abTbYwajOytWCQc37VulmWiRae5RyTpaCyDcS5/lMdtwSz5lOpDE67srw/HYe35f1z3fDQw+3txg7gNtWw==} + engines: {node: '>= 0.8', npm: 1.2.8000 || >= 1.4.16} + dependencies: + bytes: 3.1.2 + content-type: 1.0.5 + debug: 2.6.9 + depd: 2.0.0 + destroy: 1.2.0 + http-errors: 2.0.0 + iconv-lite: 0.4.24 + on-finished: 2.4.1 + qs: 6.11.0 + raw-body: 2.5.1 + type-is: 1.6.18 + unpipe: 1.0.0 + transitivePeerDependencies: + - supports-color + dev: false + + /bytes@3.1.2: + resolution: {integrity: sha512-/Nf7TyzTx6S3yRJObOAV7956r8cr2+Oj8AC5dt8wSP3BQAoeX58NoHyCU8P8zGkNXStjTSi6fzO6F0pBdcYbEg==} + engines: {node: '>= 0.8'} + dev: false + + /call-bind@1.0.5: + resolution: {integrity: sha512-C3nQxfFZxFRVoJoGKKI8y3MOEo129NQ+FgQ08iye+Mk4zNZZGdjfs06bVTr+DBSlA66Q2VEcMki/cUCP4SercQ==} + dependencies: + function-bind: 1.1.2 + get-intrinsic: 1.2.2 + set-function-length: 1.2.0 + dev: false + + /content-disposition@0.5.4: + resolution: {integrity: sha512-FveZTNuGw04cxlAiWbzi6zTAL/lhehaWbTtgluJh4/E95DqMwTmha3KZN1aAWA8cFIhHzMZUvLevkw5Rqk+tSQ==} + engines: {node: '>= 0.6'} + dependencies: + safe-buffer: 5.2.1 + dev: false + + /content-type@1.0.5: + resolution: {integrity: sha512-nTjqfcBFEipKdXCv4YDQWCfmcLZKm81ldF0pAopTvyrFGVbcR6P/VAAd5G7N+0tTr8QqiU0tFadD6FK4NtJwOA==} + engines: {node: '>= 0.6'} + dev: false + + /cookie-signature@1.0.6: + resolution: {integrity: sha512-QADzlaHc8icV8I7vbaJXJwod9HWYp8uCqf1xa4OfNu1T7JVxQIrUgOWtHdNDtPiywmFbiS12VjotIXLrKM3orQ==} + dev: false + + /cookie@0.5.0: + resolution: {integrity: sha512-YZ3GUyn/o8gfKJlnlX7g7xq4gyO6OSuhGPKaaGssGB2qgDUS0gPgtTvoyZLTt9Ab6dC4hfc9dV5arkvc/OCmrw==} + engines: {node: '>= 0.6'} + dev: false + + /cors@2.8.5: + resolution: {integrity: sha512-KIHbLJqu73RGr/hnbrO9uBeixNGuvSQjul/jdFvS/KFSIH1hWVd1ng7zOHx+YrEfInLG7q4n6GHQ9cDtxv/P6g==} + engines: {node: '>= 0.10'} + dependencies: + object-assign: 4.1.1 + vary: 1.1.2 + dev: false + + /debug@2.6.9: + resolution: {integrity: sha512-bC7ElrdJaJnPbAP+1EotYvqZsb3ecl5wi6Bfi6BJTUcNowp6cvspg0jXznRTKDjm/E7AdgFBVeAPVMNcKGsHMA==} + peerDependencies: + supports-color: '*' + peerDependenciesMeta: + supports-color: + optional: true + dependencies: + ms: 2.0.0 + dev: false + + /define-data-property@1.1.1: + resolution: {integrity: sha512-E7uGkTzkk1d0ByLeSc6ZsFS79Axg+m1P/VsgYsxHgiuc3tFSj+MjMIwe90FC4lOAZzNBdY7kkO2P2wKdsQ1vgQ==} + engines: {node: '>= 0.4'} + dependencies: + get-intrinsic: 1.2.2 + gopd: 1.0.1 + has-property-descriptors: 1.0.1 + dev: false + + /depd@2.0.0: + resolution: {integrity: sha512-g7nH6P6dyDioJogAAGprGpCtVImJhpPk/roCzdb3fIh61/s/nPsfR6onyMwkCAR/OlC3yBC0lESvUoQEAssIrw==} + engines: {node: '>= 0.8'} + dev: false + + /destroy@1.2.0: + resolution: {integrity: sha512-2sJGJTaXIIaR1w4iJSNoN0hnMY7Gpc/n8D4qSCJw8QqFWXf7cuAgnEHxBpweaVcPevC2l3KpjYCx3NypQQgaJg==} + engines: {node: '>= 0.8', npm: 1.2.8000 || >= 1.4.16} + dev: false + + /ee-first@1.1.1: + resolution: {integrity: sha512-WMwm9LhRUo+WUaRN+vRuETqG89IgZphVSNkdFgeb6sS/E4OrDIN7t48CAewSHXc6C8lefD8KKfr5vY61brQlow==} + dev: false + + /encodeurl@1.0.2: + resolution: {integrity: sha512-TPJXq8JqFaVYm2CWmPvnP2Iyo4ZSM7/QKcSmuMLDObfpH5fi7RUGmd/rTDf+rut/saiDiQEeVTNgAmJEdAOx0w==} + engines: {node: '>= 0.8'} + dev: false + + /escape-html@1.0.3: + resolution: {integrity: sha512-NiSupZ4OeuGwr68lGIeym/ksIZMJodUGOSCZ/FSnTxcrekbvqrgdUxlJOMpijaKZVjAJrWrGs/6Jy8OMuyj9ow==} + dev: false + + /etag@1.8.1: + resolution: {integrity: sha512-aIL5Fx7mawVa300al2BnEE4iNvo1qETxLrPI/o05L7z6go7fCw1J6EQmbK4FmJ2AS7kgVF/KEZWufBfdClMcPg==} + engines: {node: '>= 0.6'} + dev: false + + /express@4.18.2: + resolution: {integrity: sha512-5/PsL6iGPdfQ/lKM1UuielYgv3BUoJfz1aUwU9vHZ+J7gyvwdQXFEBIEIaxeGf0GIcreATNyBExtalisDbuMqQ==} + engines: {node: '>= 0.10.0'} + dependencies: + accepts: 1.3.8 + array-flatten: 1.1.1 + body-parser: 1.20.1 + content-disposition: 0.5.4 + content-type: 1.0.5 + cookie: 0.5.0 + cookie-signature: 1.0.6 + debug: 2.6.9 + depd: 2.0.0 + encodeurl: 1.0.2 + escape-html: 1.0.3 + etag: 1.8.1 + finalhandler: 1.2.0 + fresh: 0.5.2 + http-errors: 2.0.0 + merge-descriptors: 1.0.1 + methods: 1.1.2 + on-finished: 2.4.1 + parseurl: 1.3.3 + path-to-regexp: 0.1.7 + proxy-addr: 2.0.7 + qs: 6.11.0 + range-parser: 1.2.1 + safe-buffer: 5.2.1 + send: 0.18.0 + serve-static: 1.15.0 + setprototypeof: 1.2.0 + statuses: 2.0.1 + type-is: 1.6.18 + utils-merge: 1.0.1 + vary: 1.1.2 + transitivePeerDependencies: + - supports-color + dev: false + + /finalhandler@1.2.0: + resolution: {integrity: sha512-5uXcUVftlQMFnWC9qu/svkWv3GTd2PfUhK/3PLkYNAe7FbqJMt3515HaxE6eRL74GdsriiwujiawdaB1BpEISg==} + engines: {node: '>= 0.8'} + dependencies: + debug: 2.6.9 + encodeurl: 1.0.2 + escape-html: 1.0.3 + on-finished: 2.4.1 + parseurl: 1.3.3 + statuses: 2.0.1 + unpipe: 1.0.0 + transitivePeerDependencies: + - supports-color + dev: false + + /forwarded@0.2.0: + resolution: {integrity: sha512-buRG0fpBtRHSTCOASe6hD258tEubFoRLb4ZNA6NxMVHNw2gOcwHo9wyablzMzOA5z9xA9L1KNjk/Nt6MT9aYow==} + engines: {node: '>= 0.6'} + dev: false + + /fresh@0.5.2: + resolution: {integrity: sha512-zJ2mQYM18rEFOudeV4GShTGIQ7RbzA7ozbU9I/XBpm7kqgMywgmylMwXHxZJmkVoYkna9d2pVXVXPdYTP9ej8Q==} + engines: {node: '>= 0.6'} + dev: false + + /function-bind@1.1.2: + resolution: {integrity: sha512-7XHNxH7qX9xG5mIwxkhumTox/MIRNcOgDrxWsMt2pAr23WHp6MrRlN7FBSFpCpr+oVO0F744iUgR82nJMfG2SA==} + dev: false + + /get-intrinsic@1.2.2: + resolution: {integrity: sha512-0gSo4ml/0j98Y3lngkFEot/zhiCeWsbYIlZ+uZOVgzLyLaUw7wxUL+nCTP0XJvJg1AXulJRI3UJi8GsbDuxdGA==} + dependencies: + function-bind: 1.1.2 + has-proto: 1.0.1 + has-symbols: 1.0.3 + hasown: 2.0.0 + dev: false + + /gopd@1.0.1: + resolution: {integrity: sha512-d65bNlIadxvpb/A2abVdlqKqV563juRnZ1Wtk6s1sIR8uNsXR70xqIzVqxVf1eTqDunwT2MkczEeaezCKTZhwA==} + dependencies: + get-intrinsic: 1.2.2 + dev: false + + /has-property-descriptors@1.0.1: + resolution: {integrity: sha512-VsX8eaIewvas0xnvinAe9bw4WfIeODpGYikiWYLH+dma0Jw6KHYqWiWfhQlgOVK8D6PvjubK5Uc4P0iIhIcNVg==} + dependencies: + get-intrinsic: 1.2.2 + dev: false + + /has-proto@1.0.1: + resolution: {integrity: sha512-7qE+iP+O+bgF9clE5+UoBFzE65mlBiVj3tKCrlNQ0Ogwm0BjpT/gK4SlLYDMybDh5I3TCTKnPPa0oMG7JDYrhg==} + engines: {node: '>= 0.4'} + dev: false + + /has-symbols@1.0.3: + resolution: {integrity: sha512-l3LCuF6MgDNwTDKkdYGEihYjt5pRPbEg46rtlmnSPlUbgmB8LOIrKJbYYFBSbnPaJexMKtiPO8hmeRjRz2Td+A==} + engines: {node: '>= 0.4'} + dev: false + + /hasown@2.0.0: + resolution: {integrity: sha512-vUptKVTpIJhcczKBbgnS+RtcuYMB8+oNzPK2/Hp3hanz8JmpATdmmgLgSaadVREkDm+e2giHwY3ZRkyjSIDDFA==} + engines: {node: '>= 0.4'} + dependencies: + function-bind: 1.1.2 + dev: false + + /http-errors@2.0.0: + resolution: {integrity: sha512-FtwrG/euBzaEjYeRqOgly7G0qviiXoJWnvEH2Z1plBdXgbyjv34pHTSb9zoeHMyDy33+DWy5Wt9Wo+TURtOYSQ==} + engines: {node: '>= 0.8'} + dependencies: + depd: 2.0.0 + inherits: 2.0.4 + setprototypeof: 1.2.0 + statuses: 2.0.1 + toidentifier: 1.0.1 + dev: false + + /iconv-lite@0.4.24: + resolution: {integrity: sha512-v3MXnZAcvnywkTUEZomIActle7RXXeedOR31wwl7VlyoXO4Qi9arvSenNQWne1TcRwhCL1HwLI21bEqdpj8/rA==} + engines: {node: '>=0.10.0'} + dependencies: + safer-buffer: 2.1.2 + dev: false + + /inherits@2.0.4: + resolution: {integrity: sha512-k/vGaX4/Yla3WzyMCvTQOXYeIHvqOKtnqBduzTHpzpQZzAskKMhZ2K+EnBiSM9zGSoIFeMpXKxa4dYeZIQqewQ==} + dev: false + + /ipaddr.js@1.9.1: + resolution: {integrity: sha512-0KI/607xoxSToH7GjN1FfSbLoU0+btTicjsQSWQlh/hZykN8KpmMf7uYwPW3R+akZ6R/w18ZlXSHBYXiYUPO3g==} + engines: {node: '>= 0.10'} + dev: false + + /media-typer@0.3.0: + resolution: {integrity: sha512-dq+qelQ9akHpcOl/gUVRTxVIOkAJ1wR3QAvb4RsVjS8oVoFjDGTc679wJYmUmknUF5HwMLOgb5O+a3KxfWapPQ==} + engines: {node: '>= 0.6'} + dev: false + + /merge-descriptors@1.0.1: + resolution: {integrity: sha512-cCi6g3/Zr1iqQi6ySbseM1Xvooa98N0w31jzUYrXPX2xqObmFGHJ0tQ5u74H3mVh7wLouTseZyYIq39g8cNp1w==} + dev: false + + /methods@1.1.2: + resolution: {integrity: sha512-iclAHeNqNm68zFtnZ0e+1L2yUIdvzNoauKU4WBA3VvH/vPFieF7qfRlwUZU+DA9P9bPXIS90ulxoUoCH23sV2w==} + engines: {node: '>= 0.6'} + dev: false + + /mime-db@1.52.0: + resolution: {integrity: sha512-sPU4uV7dYlvtWJxwwxHD0PuihVNiE7TyAbQ5SWxDCB9mUYvOgroQOwYQQOKPJ8CIbE+1ETVlOoK1UC2nU3gYvg==} + engines: {node: '>= 0.6'} + dev: false + + /mime-types@2.1.35: + resolution: {integrity: sha512-ZDY+bPm5zTTF+YpCrAU9nK0UgICYPT0QtT1NZWFv4s++TNkcgVaT0g6+4R2uI4MjQjzysHB1zxuWL50hzaeXiw==} + engines: {node: '>= 0.6'} + dependencies: + mime-db: 1.52.0 + dev: false + + /mime@1.6.0: + resolution: {integrity: sha512-x0Vn8spI+wuJ1O6S7gnbaQg8Pxh4NNHb7KSINmEWKiPE4RKOplvijn+NkmYmmRgP68mc70j2EbeTFRsrswaQeg==} + engines: {node: '>=4'} + hasBin: true + dev: false + + /ms@2.0.0: + resolution: {integrity: sha512-Tpp60P6IUJDTuOq/5Z8cdskzJujfwqfOTkrwIwj7IRISpnkJnT6SyJ4PCPnGMoFjC9ddhal5KVIYtAt97ix05A==} + dev: false + + /ms@2.1.3: + resolution: {integrity: sha512-6FlzubTLZG3J2a/NVCAleEhjzq5oxgHyaCU9yYXvcLsvoVaHJq/s5xXI6/XXP6tz7R9xAOtHnSO/tXtF3WRTlA==} + dev: false + + /negotiator@0.6.3: + resolution: {integrity: sha512-+EUsqGPLsM+j/zdChZjsnX51g4XrHFOIXwfnCVPGlQk/k5giakcKsuxCObBRu6DSm9opw/O6slWbJdghQM4bBg==} + engines: {node: '>= 0.6'} + dev: false + + /object-assign@4.1.1: + resolution: {integrity: sha512-rJgTQnkUnH1sFw8yT6VSU3zD3sWmu6sZhIseY8VX+GRu3P6F7Fu+JNDoXfklElbLJSnc3FUQHVe4cU5hj+BcUg==} + engines: {node: '>=0.10.0'} + dev: false + + /object-inspect@1.13.1: + resolution: {integrity: sha512-5qoj1RUiKOMsCCNLV1CBiPYE10sziTsnmNxkAI/rZhiD63CF7IqdFGC/XzjWjpSgLf0LxXX3bDFIh0E18f6UhQ==} + dev: false + + /on-finished@2.4.1: + resolution: {integrity: sha512-oVlzkg3ENAhCk2zdv7IJwd/QUD4z2RxRwpkcGY8psCVcCYZNq4wYnVWALHM+brtuJjePWiYF/ClmuDr8Ch5+kg==} + engines: {node: '>= 0.8'} + dependencies: + ee-first: 1.1.1 + dev: false + + /parseurl@1.3.3: + resolution: {integrity: sha512-CiyeOxFT/JZyN5m0z9PfXw4SCBJ6Sygz1Dpl0wqjlhDEGGBP1GnsUVEL0p63hoG1fcj3fHynXi9NYO4nWOL+qQ==} + engines: {node: '>= 0.8'} + dev: false + + /path-to-regexp@0.1.7: + resolution: {integrity: sha512-5DFkuoqlv1uYQKxy8omFBeJPQcdoE07Kv2sferDCrAq1ohOU+MSDswDIbnx3YAM60qIOnYa53wBhXW0EbMonrQ==} + dev: false + + /proxy-addr@2.0.7: + resolution: {integrity: sha512-llQsMLSUDUPT44jdrU/O37qlnifitDP+ZwrmmZcoSKyLKvtZxpyV0n2/bD/N4tBAAZ/gJEdZU7KMraoK1+XYAg==} + engines: {node: '>= 0.10'} + dependencies: + forwarded: 0.2.0 + ipaddr.js: 1.9.1 + dev: false + + /qs@6.11.0: + resolution: {integrity: sha512-MvjoMCJwEarSbUYk5O+nmoSzSutSsTwF85zcHPQ9OrlFoZOYIjaqBAJIqIXjptyD5vThxGq52Xu/MaJzRkIk4Q==} + engines: {node: '>=0.6'} + dependencies: + side-channel: 1.0.4 + dev: false + + /range-parser@1.2.1: + resolution: {integrity: sha512-Hrgsx+orqoygnmhFbKaHE6c296J+HTAQXoxEF6gNupROmmGJRoyzfG3ccAveqCBrwr/2yxQ5BVd/GTl5agOwSg==} + engines: {node: '>= 0.6'} + dev: false + + /raw-body@2.5.1: + resolution: {integrity: sha512-qqJBtEyVgS0ZmPGdCFPWJ3FreoqvG4MVQln/kCgF7Olq95IbOp0/BWyMwbdtn4VTvkM8Y7khCQ2Xgk/tcrCXig==} + engines: {node: '>= 0.8'} + dependencies: + bytes: 3.1.2 + http-errors: 2.0.0 + iconv-lite: 0.4.24 + unpipe: 1.0.0 + dev: false + + /safe-buffer@5.2.1: + resolution: {integrity: sha512-rp3So07KcdmmKbGvgaNxQSJr7bGVSVk5S9Eq1F+ppbRo70+YeaDxkw5Dd8NPN+GD6bjnYm2VuPuCXmpuYvmCXQ==} + dev: false + + /safer-buffer@2.1.2: + resolution: {integrity: sha512-YZo3K82SD7Riyi0E1EQPojLz7kpepnSQI9IyPbHHg1XXXevb5dJI7tpyN2ADxGcQbHG7vcyRHk0cbwqcQriUtg==} + dev: false + + /send@0.18.0: + resolution: {integrity: sha512-qqWzuOjSFOuqPjFe4NOsMLafToQQwBSOEpS+FwEt3A2V3vKubTquT3vmLTQpFgMXp8AlFWFuP1qKaJZOtPpVXg==} + engines: {node: '>= 0.8.0'} + dependencies: + debug: 2.6.9 + depd: 2.0.0 + destroy: 1.2.0 + encodeurl: 1.0.2 + escape-html: 1.0.3 + etag: 1.8.1 + fresh: 0.5.2 + http-errors: 2.0.0 + mime: 1.6.0 + ms: 2.1.3 + on-finished: 2.4.1 + range-parser: 1.2.1 + statuses: 2.0.1 + transitivePeerDependencies: + - supports-color + dev: false + + /serve-static@1.15.0: + resolution: {integrity: sha512-XGuRDNjXUijsUL0vl6nSD7cwURuzEgglbOaFuZM9g3kwDXOWVTck0jLzjPzGD+TazWbboZYu52/9/XPdUgne9g==} + engines: {node: '>= 0.8.0'} + dependencies: + encodeurl: 1.0.2 + escape-html: 1.0.3 + parseurl: 1.3.3 + send: 0.18.0 + transitivePeerDependencies: + - supports-color + dev: false + + /set-function-length@1.2.0: + resolution: {integrity: sha512-4DBHDoyHlM1IRPGYcoxexgh67y4ueR53FKV1yyxwFMY7aCqcN/38M1+SwZ/qJQ8iLv7+ck385ot4CcisOAPT9w==} + engines: {node: '>= 0.4'} + dependencies: + define-data-property: 1.1.1 + function-bind: 1.1.2 + get-intrinsic: 1.2.2 + gopd: 1.0.1 + has-property-descriptors: 1.0.1 + dev: false + + /setprototypeof@1.2.0: + resolution: {integrity: sha512-E5LDX7Wrp85Kil5bhZv46j8jOeboKq5JMmYM3gVGdGH8xFpPWXUMsNrlODCrkoxMEeNi/XZIwuRvY4XNwYMJpw==} + dev: false + + /side-channel@1.0.4: + resolution: {integrity: sha512-q5XPytqFEIKHkGdiMIrY10mvLRvnQh42/+GoBlFW3b2LXLE2xxJpZFdm94we0BaoV3RwJyGqg5wS7epxTv0Zvw==} + dependencies: + call-bind: 1.0.5 + get-intrinsic: 1.2.2 + object-inspect: 1.13.1 + dev: false + + /statuses@2.0.1: + resolution: {integrity: sha512-RwNA9Z/7PrK06rYLIzFMlaF+l73iwpzsqRIFgbMLbTcLD6cOao82TaWefPXQvB2fOC4AjuYSEndS7N/mTCbkdQ==} + engines: {node: '>= 0.8'} + dev: false + + /toidentifier@1.0.1: + resolution: {integrity: sha512-o5sSPKEkg/DIQNmH43V0/uerLrpzVedkUh8tGNvaeXpfpuwjKenlSox/2O/BTlZUtEe+JG7s5YhEz608PlAHRA==} + engines: {node: '>=0.6'} + dev: false + + /type-is@1.6.18: + resolution: {integrity: sha512-TkRKr9sUTxEH8MdfuCSP7VizJyzRNMjj2J2do2Jr3Kym598JVdEksuzPQCnlFPW4ky9Q+iA+ma9BGm06XQBy8g==} + engines: {node: '>= 0.6'} + dependencies: + media-typer: 0.3.0 + mime-types: 2.1.35 + dev: false + + /unpipe@1.0.0: + resolution: {integrity: sha512-pjy2bYhSsufwWlKwPc+l3cN7+wuJlK6uz0YdJEOlQDbl6jo/YlPi4mb8agUkVC8BF7V8NuzeyPNqRksA3hztKQ==} + engines: {node: '>= 0.8'} + dev: false + + /utils-merge@1.0.1: + resolution: {integrity: sha512-pMZTvIkT1d+TFGvDOqodOclx0QWkkgi6Tdoa8gC8ffGAAqz9pzPTZWAybbsHHoED/ztMtkv/VoYTYyShUn81hA==} + engines: {node: '>= 0.4.0'} + dev: false + + /vary@1.1.2: + resolution: {integrity: sha512-BNGbWLfd0eUPabhkXUVm0j8uuvREyTh5ovRa/dyow/BqAbZJyC+5fU+IzQOzmAKzYqYRAISoRhdQr3eIZ/PXqg==} + engines: {node: '>= 0.8'} + dev: false + + /ws@8.16.0: + resolution: {integrity: sha512-HS0c//TP7Ina87TfiPUz1rQzMhHrl/SG2guqRcTOIUYD2q8uhUdNHZYJUaQ8aTGPzCh+c6oawMKW35nFl1dxyQ==} + engines: {node: '>=10.0.0'} + peerDependencies: + bufferutil: ^4.0.1 + utf-8-validate: '>=5.0.2' + peerDependenciesMeta: + bufferutil: + optional: true + utf-8-validate: + optional: true + dev: false diff --git a/mock/server.js b/mock/server.js new file mode 100644 index 0000000..44ada30 --- /dev/null +++ b/mock/server.js @@ -0,0 +1,316 @@ +import express from "express"; +import cors from "cors"; +import Kinematic from "./kinematic.js"; +import { WebSocketServer } from "ws"; + +const app = express(); +const kinematic = new Kinematic(); +const wss = new WebSocketServer({ port: 8080 }); + +app.use(cors()); +app.use(express.json()); + +const port = 3000; +const subscriptions = {}; + +const randomFloatFromInterval = (min, max) => + Math.floor((Math.random() * (max - min + 1) + min) * 100) / 100; + +const radToDeg = (val) => val * (180 / Math.PI); +const degToRad = (val) => val * (Math.PI / 180); + +function subscribeClientToCategory(ws, category) { + if (!subscriptions[category]) { + subscriptions[category] = new Set(); + } + subscriptions[category].add(ws); +} + +const unsubscribeClientFromCategory = (ws, category) => { + if (!subscriptions[category]) return; + subscriptions[category].delete(ws); + if (subscriptions[category].size === 0) { + delete subscriptions[category].size; + } +}; + +const sendUpdateToSubscribers = (category, data) => { + if (subscriptions[category]) { + const message = JSON.stringify(data); + for (const client of subscriptions[category]) { + client.send(message); + } + } +}; + +const model = { + battery: { + voltage: randomFloatFromInterval(7.6, 8.2), + ampere: randomFloatFromInterval(0.2, 3), + power_button: false, + }, + servos: { + angles: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + dir: [-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1], + on: true, + }, + mpu: { + x: 0, + y: 0, + z: 0, + heading: 240, + temperature: 21, + }, + display: [[]], + distance_sensors: { + left: 22, + right: 23, + }, + appTime: 1123321, + connectivity: { + ssid: "best network", + ip: "192.168.0.118", + mDNS: "leika.local", + rssi: 100, + }, + running: true, +}; + +const settings = { + useMetric: true, + name: "Leika", + ssid: "Rune private network", + pass: "12345678", + ap: "Leika", + apPass: "12345678", + apChannel: 1, +}; + +const logs = [ + "Booting up", + "Starting webserver", + "Loading setting", + "Connected to Rune private network", +]; + +const system = { + HeapSize: 400000, + HeapFree: 0, + HeapMin: 0, + DmaHeapSize: 0, + DmaHeapFree: 0, + DmaHeapMin: 0, + PsramSize: 400000, + PsramFree: 0, + PsramMin: 0, + ChipModel: 0, + ChipRevision: 0, + ChipCores: 2, + CpuFreqMHz: 80, + SketchSize: 0, + FreeSketchSpace: 10200, + FlashChipSize: 0, + CpuUsed: 0, + CpuUsedCore0: 0, + CpuUsedCore1: 0, + arduinoVersion: "3.2.1", +}; + +const updateBattery = () => { + model.battery.voltage = randomFloatFromInterval(7.6, 8.2); + model.battery.ampere = randomFloatFromInterval(0.2, 3); + return model.battery; +}; + +const updateMpu = () => { + model.mpu.x = randomFloatFromInterval(0, 1); + model.mpu.y = randomFloatFromInterval(0, 1); + model.mpu.z = randomFloatFromInterval(0, 1); + model.mpu.temperature = randomFloatFromInterval(20, 22); + return model.mpu; +}; + +const updateDistances = () => { + model.distance_sensors = { + left: randomFloatFromInterval(10, 220), + right: randomFloatFromInterval(10, 220), + }; + return model.distance_sensors; +}; + +const updateDistance = (position) => { + model.distance_sensors[position] = randomFloatFromInterval(10, 220); + return model.distance_sensors[position]; +}; + +const updateSystem = () => { + system.CpuUsedCore0 = randomFloatFromInterval(0, 100); + system.CpuUsedCore1 = randomFloatFromInterval(0, 100); + system.CpuUsed = + Math.floor((system.CpuUsedCore0 + system.CpuUsedCore1) / 0.02) / 100; + system.HeapFree = randomFloatFromInterval(0, 20000); + system.HeapMin = randomFloatFromInterval(0, 20000); + return system; +}; + +const updateBodyState = (angles, position) => { + const Lp = [ + [100, -100, 100, 1], + [100, -100, -100, 1], + [-100, -100, 100, 1], + [-100, -100, -100, 1], + ]; + + model.servos.angles = kinematic + .calcIK( + Lp, + angles.map((x) => degToRad(x)), + position + ) + .flat() + .map((x, i) => radToDeg(x * model.servos.dir[i])); + return model.servos.angles; +}; + +const updateAngle = (id, angle) => { + model.servos.angles[id] = angle; + return model.servos.angles; +}; + +const updateAngles = (angles) => { + model.servos.angles = angles; + return model.servos.angles; +}; + +wss.on("connection", (ws) => { + ws.on("error", console.error); + + ws.on("message", (message) => { + let data = message; + try { + data = JSON.parse(message); + } catch (error) { + return; + } + switch (data.type) { + case "subscribe": + subscribeClientToCategory(ws, data.category); + break; + case "unsubscribe": + unsubscribeClientFromCategory(ws, data.category); + break; + case "sensor/battery": + ws.send({ type: "battery", battery: JSON.stringify(updateBattery()) }); + break; + case "sensor/mpu": + ws.send({ type: "battery", mpu: JSON.stringify(updateMpu()) }); + break; + case "sensor/distances": + ws.send(JSON.stringify(updateDistances())); + break; + case "sensor/distance": + ws.send(JSON.stringify({ distance: updateDistance(data.position) })); + break; + case "kinematic/angle": + if (data.angle && data.id) { + model.servos.angles[data.id] = data.angle; + ws.send( + JSON.stringify({ type: "angles", angles: model.servos.angles }) + ); + } else { + ws.send(JSON.stringify(updateAngle(data.id, data.angle))); + } + break; + case "kinematic/angles": + if (data.angles) { + model.servos.angles = data.angles; + ws.send( + JSON.stringify({ + type: "angles", + angles: model.servos.angles, + }) + ); + } else { + ws.send(JSON.stringify(updateAngles(data.angles))); + } + break; + case "kinematic/bodystate": + if (data.angles) { + ws.send( + JSON.stringify({ + type: "angles", + angles: updateBodyState(data.angles, data.position), + }) + ); + } else { + ws.send(JSON.stringify({ angles: model.servos.angles })); + } + break; + case "system/log": + ws.send(JSON.stringify(logs)); + break; + case "system/info": + ws.send(JSON.stringify(updateSystem())); + break; + case "system/settings": + if (data.settings) { + Object.entries(data.settings).forEach( + ([key, value]) => (settings[key] = value) + ); + ws.send(JSON.stringify(settings)); + } else { + ws.send(JSON.stringify(settings)); + } + break; + case "system/stop": + model.running = false; + ws.send(JSON.stringify(settings)); + break; + default: + ws.send(JSON.stringify({ error: "Unknown request type" })); + } + }); + + ws.on("close", () => { + for (const category in subscriptions) { + unsubscribeClientFromCategory(ws, category); + } + }); +}); + +app.get("/sensor/battery", (req, res) => res.send(updateBattery())); +app.get("/sensor/mpu", (req, res) => res.send(updateMpu())); +app.get("/sensor/distances", (req, res) => res.send(updateDistances())); +app.get("/sensor/distance/:position", (req, res) => + res.send({ distance: updateDistance(req.params.position) }) +); + +// ----------------------------------------------------------- // + +app.post("/kinematic/angle/:id", (req, res) => + res.send(updateAngle(req.params.id, req.body.angle)) +); +app.post("/kinematic/angles/", (req, res) => + res.send(updateAngles(req.body.angles)) +); +app.get("/kinematic/bodystate", (req, res) => res.send(model.servos.angles)); +app.post("/kinematic/bodystate", (req, res) => { + sendUpdateToSubscribers("angles", model.servos.angles); + res.send(updateBodyState(req.body.angles, req.body.position)); +}); + +// ----------------------------------------------------------- // + +app.get("/system/log", (req, res) => res.send(logs)); +app.get("/system/info", (req, res) => res.send(updateSystem())); +app.get("/system/settings", (req, res) => res.send(settings)); +app.post("/system/settings", (req, res) => { + Object.entries(req.body).forEach((x) => (settings[x[0]] = x[1])); + res.send(settings); +}); +app.post("/system/stop", (req, res) => { + model.running = false; + model.res.send(settings); +}); + +app.listen(port, () => console.log(`Open at http://localhost:${port}`));