├── .babelrc ├── src ├── index.js └── kinematics.js ├── .github └── FUNDING.yml ├── .gitignore ├── .travis.yml ├── .eslintrc.yml ├── LICENSE.md ├── package.json ├── test └── kinematics.test.js └── README.md /.babelrc: -------------------------------------------------------------------------------- 1 | { 2 | "presets": ["es2015", "stage-2"] 3 | } -------------------------------------------------------------------------------- /src/index.js: -------------------------------------------------------------------------------- 1 | import kin from './kinematics' 2 | 3 | export default kin 4 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | github: [glumb] 2 | custom: ['paypal.me/MaximilianBeck'] 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | #npm 2 | node_modules 3 | npm-debug.log 4 | 5 | #build 6 | dist 7 | 8 | #phpstorm 9 | .idea 10 | 11 | #coverage 12 | .nyc_output 13 | coverage.lcov 14 | coverage 15 | 16 | #mac 17 | .DS_Store 18 | 19 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: false 2 | language: node_js 3 | cache: 4 | directories: 5 | - node_modules 6 | notifications: 7 | email: false 8 | node_js: 9 | - '6' 10 | before_install: 11 | - npm i -g npm@^2.0.0 12 | before_script: 13 | - npm prune 14 | script: 15 | - npm run build 16 | - npm run cover 17 | after_success: 18 | - npm run report-coverage 19 | - bash <(curl -s https://codecov.io/bash) 20 | - npm run semantic-release 21 | branches: 22 | except: 23 | - /^v\d+\.\d+\.\d+$/ 24 | -------------------------------------------------------------------------------- /.eslintrc.yml: -------------------------------------------------------------------------------- 1 | extends: airbnb 2 | parser: "babel-eslint" 3 | env: 4 | es6: true 5 | browser: true 6 | plugins: 7 | - react 8 | - jsx-a11y 9 | - import 10 | rules: 11 | semi: ["error", "never"] 12 | no-plusplus: 0 13 | no-restricted-syntax: ["off", "ForInStatement"] 14 | max-len: 0 15 | newline-per-chained-call: 0 16 | no-undef: 1 17 | "no-param-reassign": [2, {"props": false}] 18 | no-mixed-operators: 0 19 | camelcase: 0 20 | no-console: 0 21 | globals: 22 | 23 | # document: true 24 | # THREE: true 25 | # window: true 26 | # debug: true 27 | # THREERobot: true 28 | # define: true 29 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Maximilian Beck 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "kinematics", 3 | "description": "Robot kinematics library", 4 | "main": "dist/index.js", 5 | "version": "0.0.0-semantic-release-controlled", 6 | "scripts": { 7 | "commit": "git-cz", 8 | "test": "./node_modules/.bin/mocha ./test/*.test.js --compilers js:babel-register", 9 | "watch:test": "npm t -- -w", 10 | "eslint": "./node_modules/.bin/eslint './src/*.js' './test/*.js' --fix", 11 | "build": "babel --out-dir dist src", 12 | "cover": "nyc npm t", 13 | "report-coverage": "nyc report --reporter=text-lcov > coverage.lcov", 14 | "check-coverage": "nyc check-coverage --functions 100", 15 | "semantic-release": "semantic-release pre && npm publish && semantic-release post" 16 | }, 17 | "repository": { 18 | "type": "git", 19 | "url": "https://github.com/glumb/kinematics.git" 20 | }, 21 | "keywords": [ 22 | "robotics", 23 | "inverse kinematics", 24 | "kinematcis" 25 | ], 26 | "files": [ 27 | "dist" 28 | ], 29 | "author": "Maximilian Beck (http://glumb.de/)", 30 | "license": "MIT", 31 | "bugs": { 32 | "url": "https://github.com/glumb/kinematics/issues" 33 | }, 34 | "homepage": "https://github.com/glumb/kinematics#readme", 35 | "devDependencies": { 36 | "babel-cli": "6.18.0", 37 | "babel-core": "6.18.2", 38 | "babel-eslint": "7.1.0", 39 | "babel-polyfill": "6.16.0", 40 | "babel-preset-es2015": "6.18.0", 41 | "babel-preset-stage-2": "6.18.0", 42 | "babel-register": "6.18.0", 43 | "chai": "3.5.0", 44 | "codecov.io": "0.1.6", 45 | "commitizen": "2.8.6", 46 | "cz-conventional-changelog": "1.2.0", 47 | "eslint": "3.10.0", 48 | "eslint-config-airbnb": "13.0.0", 49 | "eslint-plugin-import": "2.2.0", 50 | "eslint-plugin-jsx-a11y": "^2.2.3", 51 | "eslint-plugin-react": "6.6.0", 52 | "istanbul": "0.4.5", 53 | "mocha": "3.1.2", 54 | "nyc": "8.4.0", 55 | "semantic-release": "^4.3.5" 56 | }, 57 | "config": { 58 | "commitizen": { 59 | "path": "./node_modules/cz-conventional-changelog" 60 | } 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /test/kinematics.test.js: -------------------------------------------------------------------------------- 1 | // import 'babel-polyfill' 2 | import 'mocha/mocha' 3 | import { assert } from 'chai' 4 | import K from '../src/index' 5 | 6 | const geometries = [ 7 | [ 8 | [1, 1, 0], 9 | [0, 10, 0], 10 | [5, 0, 0], 11 | [3, 0, 0], 12 | [0, -3, 0], 13 | ], 14 | [ 15 | [1, 1, 1], 16 | [0, 8, 2], 17 | [0, 10, 0], 18 | [5, 0, 0], 19 | [0, -6, 0], 20 | ], 21 | ] 22 | 23 | const targetPoses = [ 24 | [1, 1, 2, 1, 2, 3], 25 | [3, 8, 2, 4, 1, 3], 26 | [6, -6, -2, 0, 1, 3], 27 | [3, 8, 3, 4, 0, 3], 28 | ] 29 | 30 | function eulerToVec(b, c) { 31 | const cb = Math.cos(b) 32 | const sb = Math.sin(b) 33 | const cc = Math.cos(c) 34 | const sc = Math.sin(c) 35 | 36 | return [ 37 | cb * cc, 38 | cb * sc, -sb, 39 | ] 40 | } 41 | 42 | describe('#kinematics', () => { 43 | geometries.forEach((geometry) => { 44 | // if ik and fk break in the same way this test may not be sufficient 45 | // but this is very unlikely. :D 46 | const kin = new K(geometry) 47 | 48 | targetPoses.forEach((targetPose) => { 49 | it(`match inverse and forward kinematics ${targetPose}`, () => { 50 | const angles = kin.inverse(...targetPose) 51 | // get the TCP position [5] 52 | const pose = kin.forward(...angles)[5] 53 | 54 | const TOLERANCE = 0.000001 55 | 56 | // x 57 | assert.isBelow((pose[0] - targetPose[0]), TOLERANCE) 58 | // y 59 | assert.isBelow((pose[1] - targetPose[1]), TOLERANCE) 60 | // z 61 | assert.isBelow((pose[2] - targetPose[2]), TOLERANCE) 62 | // rotation a 63 | assert.isBelow((pose[3] - targetPose[3]), TOLERANCE) 64 | 65 | // direction - different Euler angles may point to the same direction 66 | const vecTarget = eulerToVec(targetPose[1], targetPose[2]) 67 | const vecPose = eulerToVec(pose[1], pose[2]) 68 | 69 | assert.isBelow((vecTarget[0] - vecPose[0]), TOLERANCE) 70 | assert.isBelow((vecTarget[1] - vecPose[1]), TOLERANCE) 71 | }) 72 | }) 73 | }) 74 | it('use debug mode to cover everything', () => { 75 | const kin = new K(geometries[0]) 76 | kin.debug = true 77 | kin.inverse(1, 1, 1, 0, 0, 0) 78 | kin.forward(0, 0, 0, 0, 0, 0) 79 | 80 | assert.equal(kin.debug, true) 81 | }) 82 | 83 | }) 84 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kinematics.js 2 | 3 | [![GitHub license](https://img.shields.io/badge/license-MIT-blue.svg)](https://raw.githubusercontent.com/glumb/kinematics/master/LICENSE.md) 4 | [![Travis](https://img.shields.io/travis/glumb/kinematics.svg)](https://travis-ci.org/glumb/kinematics) 5 | [![npm](https://img.shields.io/npm/v/kinematics.svg)](https://www.npmjs.com/package/kinematics) 6 | [![Codecov](https://img.shields.io/codecov/c/github/glumb/kinematics.svg)](https://codecov.io/github/glumb/kinematics) 7 | 8 | 6DOF robot kinematics in JavaScript. 9 | 10 | ## Install 11 | ```console 12 | npm install kinematics --save 13 | ``` 14 | 15 | ## Use 16 | ```js 17 | const Kinematics = require('kinematics').default 18 | 19 | const geometry = [ 20 | [1, 1, 0], // V0: 1x 1y 21 | [0, 10, 0], // V1: 10y 22 | [5, 0, 0], // V2: 5x 23 | [3, 0, 0], // V3: 3x 24 | [0, -3, 0], // V4: -3y 25 | ] 26 | 27 | const RobotKin = new Kinematics(geometry) 28 | 29 | let angles = [1.57, 1.2, 0, 0.3, 2.2, 1.1] 30 | 31 | const pose = RobotKin.forward(...angles)[5] 32 | 33 | angles = RobotKin.inverse(...pose) 34 | ``` 35 | ## Geometry 36 | The geometry array consists of 5 entries describing the links *V0-V5*. Each *Vn* is a tuple of 3 coordinates from *Jn* to *Jn+1*. 37 | One constraint: The y,z of *V3* and x,z of *V4* must be 0 for the kinematics to work. 38 |

39 | 40 |

41 |

42 | 43 |

44 | 45 | ## API 46 | 47 | **forward** 48 | 49 | ```js 50 | RobotKin.forward(R0, R1, R2, R3, R4, R5) 51 | ``` 52 | returns 53 | ``` 54 | [ 55 | [ 0, 0, 0 ], //J0 56 | [ 0.5, 1, -0.8 ], //J1 57 | [ -0.2, -8.8, 0.3 ], //J2 58 | [ 1.8, -5.6, -2.8 ], //J3 59 | [ 3.0, -3.6, -4.7 ], //J4 60 | [ 4.7, -1.3, -5.5, 1, 6, -2.8 ] //J5 + TCP Euler angles 61 | ] 62 | ``` 63 | 64 | **inverse** 65 | 66 | *X,Y,Z* coordinates, *A,B,C* Euler angles in order 'abc'. 67 | 68 | ```js 69 | RobotKin.inverse(X, Y, Z, A, B, C) 70 | ``` 71 | returns 72 | ``` 73 | [ 2, 1.6, 2.1, -3.5, 1, -1.5 ] //array of angles 74 | [ 1, 2.3, 3.1, NaN, NaN, NaN ] //NaN for out of reach angles 75 | ``` 76 | 77 | ## kinematic coupling 78 | kinematics.js assumes a robot with a series of joints. Some robots may have different kinematics. The depicted robot has a hinge at *J1* and R1/R2 are at the same kinematic position. Therefore moving *R1* also changes the angle at *J2*. To account for that, *R2* has to move the same amount. 79 | 80 | ![sr_geometry_kinematic_coupling](https://cloud.githubusercontent.com/assets/3062564/20247618/cd029290-a9d0-11e6-92ed-ef0f43a16e9b.png) 81 | 82 | Using that information, you can use kinematics.js to calculate the initial angles and correct them according to your kinematics. 83 | 84 | ```js 85 | let angles = RobotKin.inverse(...pose) 86 | angles[2] += angles[1] 87 | //set angles, do stuff 🤖 88 | ``` 89 | 90 | ## TODO 91 | - robot configuration 92 | - comply with DH for TCP orientation? 93 | - more kinematic chains 94 | 95 | ## Demo 96 | 97 | See the kinematics in action: http://robot.glumb.de (Use Chrome or FF due to ES6 features) 98 | ![robot-gui](https://user-images.githubusercontent.com/3062564/31865318-7d97605e-b76d-11e7-8ab4-7c2a9e17be3d.png) 99 | The gui is available in this repo:https://github.com/glumb/robot-gui 100 | -------------------------------------------------------------------------------- /src/kinematics.js: -------------------------------------------------------------------------------- 1 | export default class Kinematics { 2 | /** 3 | * @param {Array} geometry [5][3] Array including geometry information [x,y,z] 4 | */ 5 | constructor(geometry) { 6 | if (geometry.length !== 5) { 7 | throw new Error('geometry array must have 5 entries') 8 | } 9 | 10 | if (geometry[3][1] !== 0 || geometry[3][2] !== 0 || geometry[4][0] !== 0 || geometry[4][2] !== 0) { 11 | throw new Error('geometry 3 and 4 must be one dimensional geo[3] = [a,0,0] geo[4] = [0,b,0]') 12 | } 13 | 14 | this.V1_length_x_y = Math.sqrt((geometry[1][0] ** 2) + (geometry[1][1] ** 2)) 15 | this.V4_length_x_y_z = Math.sqrt((geometry[4][0] ** 2) + (geometry[4][1] ** 2) + (geometry[4][2] ** 2)) 16 | 17 | this.geometry = geometry 18 | 19 | this.J_initial_absolute = [] 20 | const tmpPos = [0, 0, 0] 21 | for (let i = 0; i < geometry.length; i++) { 22 | this.J_initial_absolute.push([tmpPos[0], tmpPos[1], tmpPos[2]]) 23 | tmpPos[0] += geometry[i][0] 24 | tmpPos[1] += geometry[i][1] 25 | tmpPos[2] += geometry[i][2] 26 | } 27 | 28 | this.R_corrected = [0, 0, 0, 0, 0, 0] 29 | 30 | this.R_corrected[1] -= Math.PI / 2 31 | this.R_corrected[1] += Math.atan2(geometry[1][0], geometry[1][1]) // correct offset bone 32 | 33 | this.R_corrected[2] -= Math.PI / 2 34 | this.R_corrected[2] -= Math.atan2((geometry[2][1] + geometry[3][1]), (geometry[2][0] + geometry[3][0])) // correct offset bone V2,V3 35 | this.R_corrected[2] -= Math.atan2(geometry[1][0], geometry[1][1]) // correct bone offset of V1 36 | 37 | this.R_corrected[4] += Math.atan2(geometry[4][1], geometry[4][0]) 38 | } 39 | 40 | /** 41 | * calculateAngles - calculate robot angles based on TCP and geometry 42 | * 43 | * @param {number} x coordinate 44 | * @param {number} y coordinate 45 | * @param {number} z coordinate 46 | * @param {number} a euler angle rotation order abc 47 | * @param {number} b euler angle rotation order abc 48 | * @param {number} c euler angle rotation order abc 49 | * todo @param {String} configuration S1 S2 S3 50 | * @return {Array} angles 51 | */ 52 | inverse(x, y, z, a, b, c) { 53 | if (this.debug) { 54 | console.log(x, y, z, a, b, c) 55 | } 56 | 57 | const ca = Math.cos(a) 58 | const sa = Math.sin(a) 59 | const cb = Math.cos(b) 60 | const sb = Math.sin(b) 61 | const cc = Math.cos(c) 62 | const sc = Math.sin(c) 63 | 64 | const targetVectorX = [ 65 | cb * cc, 66 | cb * sc, -sb, 67 | ] 68 | 69 | const R = [ 70 | this.R_corrected[0], 71 | this.R_corrected[1], 72 | this.R_corrected[2], 73 | this.R_corrected[3], 74 | this.R_corrected[4], 75 | this.R_corrected[5], 76 | ] 77 | 78 | const J = [ 79 | [0, 0, 0], 80 | [0, 0, 0], 81 | [0, 0, 0], 82 | [0, 0, 0], 83 | [0, 0, 0], 84 | [0, 0, 0], 85 | ] 86 | 87 | // ---- J5 ---- 88 | 89 | J[5][0] = x 90 | J[5][1] = y 91 | J[5][2] = z 92 | 93 | 94 | // ---- J4 ---- 95 | // vector 96 | 97 | J[4][0] = x - this.V4_length_x_y_z * targetVectorX[0] 98 | J[4][1] = y - this.V4_length_x_y_z * targetVectorX[1] 99 | J[4][2] = z - this.V4_length_x_y_z * targetVectorX[2] 100 | 101 | 102 | // ---- R0 ---- 103 | // # J4 104 | 105 | R[0] += Math.PI / 2 - Math.acos(this.J_initial_absolute[4][2] / Kinematics.length2(J[4][2], J[4][0])) 106 | R[0] += Math.atan2(-J[4][2], J[4][0]) 107 | 108 | if (this.J_initial_absolute[4][2] > Kinematics.length2(J[4][2], J[4][0]) && this.debug) { 109 | console.log('out of reach') 110 | } 111 | 112 | // ---- J1 ---- 113 | // # R0 114 | 115 | J[1][0] = Math.cos(R[0]) * this.geometry[0][0] + Math.sin(R[0]) * this.geometry[0][2] 116 | J[1][1] = this.geometry[0][1] 117 | J[1][2] = -Math.sin(R[0]) * this.geometry[0][0] + Math.cos(R[0]) * this.geometry[0][2] 118 | 119 | 120 | // ---- rotate J4 into x,y plane ---- 121 | // # J4 R0 122 | 123 | const J4_x_y = [] 124 | 125 | J4_x_y[0] = Math.cos(R[0]) * J[4][0] + -Math.sin(R[0]) * J[4][2] 126 | J4_x_y[1] = J[4][1] 127 | J4_x_y[2] = Math.sin(R[0]) * J[4][0] + Math.cos(R[0]) * J[4][2] 128 | 129 | // ---- J1J4_projected_length_square ---- 130 | // # J4 R0 131 | 132 | const J1J4_projected_length_square = ((J4_x_y[0] - this.J_initial_absolute[1][0]) ** 2) + ((J4_x_y[1] - this.J_initial_absolute[1][1]) ** 2) // not using Math.sqrt 133 | 134 | // ---- R2 ---- 135 | // # J4 R0 136 | 137 | const J2J4_length_x_y = Kinematics.length2(this.geometry[2][0] + this.geometry[3][0], this.geometry[2][1] + this.geometry[3][1]) 138 | R[2] += Math.acos((-J1J4_projected_length_square + (J2J4_length_x_y ** 2) + (this.V1_length_x_y ** 2)) / (2.0 * (J2J4_length_x_y) * this.V1_length_x_y)) 139 | 140 | // ---- R1 ---- 141 | // # J4 R0 142 | 143 | const J1J4_projected_length = Math.sqrt(J1J4_projected_length_square) 144 | R[1] += Math.atan2((J4_x_y[1] - this.J_initial_absolute[1][1]), (J4_x_y[0] - this.J_initial_absolute[1][0])) 145 | R[1] += Math.acos((+J1J4_projected_length_square - (J2J4_length_x_y ** 2) + (this.V1_length_x_y ** 2)) / (2.0 * J1J4_projected_length * this.V1_length_x_y)) 146 | 147 | // ---- J2 ---- 148 | // # R1 R0 149 | 150 | const ta = Math.cos(R[0]) 151 | const tb = Math.sin(R[0]) 152 | const tc = this.geometry[0][0] 153 | const d = this.geometry[0][1] 154 | const e = this.geometry[0][2] 155 | const f = Math.cos(R[1]) 156 | const g = Math.sin(R[1]) 157 | const h = this.geometry[1][0] 158 | const i = this.geometry[1][1] 159 | const j = this.geometry[1][2] 160 | const k = Math.cos(R[2]) 161 | const l = Math.sin(R[2]) 162 | const m = this.geometry[2][0] 163 | const n = this.geometry[2][1] 164 | const o = this.geometry[2][2] 165 | 166 | J[2][0] = ta * tc + tb * e + ta * f * h - ta * g * i + tb * j 167 | J[2][1] = d + g * h + f * i 168 | J[2][2] = -tb * tc + ta * e - tb * f * h + tb * g * i + ta * j 169 | 170 | // ---- J3 ---- 171 | // # R0 R1 R2 172 | 173 | J[3][0] = ta * tc + tb * e + ta * f * h - ta * g * i + tb * j + ta * f * k * m - ta * g * l * m - ta * g * k * n - ta * f * l * n + tb * o 174 | J[3][1] = d + g * h + f * i + g * k * m + f * l * m + f * k * n - g * l * n 175 | J[3][2] = -tb * tc + ta * e - tb * f * h + tb * g * i + ta * j - tb * f * k * m + tb * g * l * m + tb * g * k * n + tb * f * l * n + ta * o 176 | 177 | // ---- J4J3 J4J5 ---- 178 | // # J3 J4 J5 179 | 180 | const J4J5_vector = [J[5][0] - J[4][0], J[5][1] - J[4][1], J[5][2] - J[4][2]] 181 | const J4J3_vector = [J[3][0] - J[4][0], J[3][1] - J[4][1], J[3][2] - J[4][2]] 182 | 183 | // ---- R3 ---- 184 | // # J3 J4 J5 185 | 186 | const J4J5_J4J3_normal_vector = Kinematics.cross(J4J5_vector, J4J3_vector) 187 | const XZ_parallel_aligned_vector = [ 188 | 10 * Math.cos(R[0] + (Math.PI / 2)), 189 | 0, -10 * Math.sin(R[0] + (Math.PI / 2)), 190 | ] 191 | 192 | const reference = Kinematics.cross(XZ_parallel_aligned_vector, J4J3_vector) 193 | 194 | R[3] = Kinematics.angleBetween(J4J5_J4J3_normal_vector, XZ_parallel_aligned_vector, reference) 195 | 196 | // ---- R4 ---- 197 | // # J4 J3 J5 198 | 199 | const referenceVector = Kinematics.cross(J4J3_vector, J4J5_J4J3_normal_vector) 200 | 201 | R[4] += Kinematics.angleBetween(J4J5_vector, J4J3_vector, referenceVector) 202 | 203 | // ---- R5 ---- 204 | // # J3 J4 J5 205 | 206 | const targetVectorY = [ 207 | sa * sb * cc - sc * ca, 208 | sa * sb * sc + cc * ca, 209 | sa * cb, 210 | ] 211 | 212 | R[5] += Math.PI / 2 213 | R[5] -= Kinematics.angleBetween(J4J5_J4J3_normal_vector, targetVectorY, Kinematics.cross(targetVectorY, targetVectorX)) 214 | 215 | // --- return angles --- 216 | return R 217 | } 218 | 219 | calculateTCP(R0, R1, R2, R3, R4, R5, jointsResult) { 220 | const joints = this.calculateCoordinates(R0, R1, R2, R3, R4, R5) 221 | jointsResult[0] = joints[5][0] 222 | jointsResult[1] = joints[5][1] 223 | jointsResult[2] = joints[5][2] 224 | jointsResult[3] = joints[5][3] 225 | jointsResult[4] = joints[5][4] 226 | jointsResult[5] = joints[5][5] 227 | } 228 | 229 | 230 | /** 231 | * calculateCoordinates - calculate joint coordinates based on angles 232 | * 233 | * @param {number} R0 angle for joint 0 234 | * @param {number} R1 angle for joint 1 235 | * @param {number} R2 angle for joint 2 236 | * @param {number} R3 angle for joint 3 237 | * @param {number} R4 angle for joint 4 238 | * @param {number} R5 angle for joint 5 239 | * @return {Array} [[x,z,z]...[x,y,z,a,b,c]] 240 | */ 241 | forward(R0, R1, R2, R3, R4, R5) { 242 | const a = Math.cos(R0) 243 | const b = Math.sin(R0) 244 | const c = this.geometry[0][0] 245 | const d = this.geometry[0][1] 246 | const e = this.geometry[0][2] 247 | const f = Math.cos(R1) 248 | const g = Math.sin(R1) 249 | const h = this.geometry[1][0] 250 | const i = this.geometry[1][1] 251 | const j = this.geometry[1][2] 252 | const k = Math.cos(R2) 253 | const l = Math.sin(R2) 254 | const m = this.geometry[2][0] 255 | const n = this.geometry[2][1] 256 | const o = this.geometry[2][2] 257 | const p = Math.cos(R3) 258 | const q = Math.sin(R3) 259 | const r = this.geometry[3][0] 260 | const s = this.geometry[3][1] 261 | const t = this.geometry[3][2] 262 | const u = Math.cos(R4) 263 | const v = Math.sin(R4) 264 | const w = this.geometry[4][0] 265 | const x = this.geometry[4][1] 266 | const y = this.geometry[4][2] 267 | const A = Math.cos(R5) 268 | const B = Math.sin(R5) 269 | // const C = 0 // this.geometry[5][0] 270 | // const D = 0 // this.geometry[5][1] 271 | // const E = 0 // this.geometry[5][2] 272 | 273 | const jointsResult = [[], [], [], [], [], []] 274 | 275 | jointsResult[0][0] = 0 276 | jointsResult[0][1] = 0 277 | jointsResult[0][2] = 0 278 | 279 | jointsResult[1][0] = jointsResult[0][0] + a * c + b * e 280 | jointsResult[1][1] = jointsResult[0][1] + d 281 | jointsResult[1][2] = jointsResult[0][2] + -b * c + a * e 282 | 283 | jointsResult[2][0] = jointsResult[1][0] + a * f * h - a * g * i + b * j 284 | jointsResult[2][1] = jointsResult[1][1] + g * h + f * i 285 | jointsResult[2][2] = jointsResult[1][2] - b * f * h + b * g * i + a * j 286 | 287 | jointsResult[3][0] = jointsResult[2][0] + a * f * k * m - a * g * l * m - a * g * k * n - a * f * l * n + b * o 288 | jointsResult[3][1] = jointsResult[2][1] + g * k * m + f * l * m + f * k * n - g * l * n 289 | jointsResult[3][2] = jointsResult[2][2] - b * f * k * m + b * g * l * m + b * g * k * n + b * f * l * n + a * o 290 | 291 | jointsResult[4][0] = jointsResult[3][0] + a * f * k * r - a * g * l * r - a * g * k * p * s - a * f * l * p * s + b * q * s + b * p * t + a * g * k * q * t + a * f * l * q * t 292 | jointsResult[4][1] = jointsResult[3][1] + g * k * r + f * l * r + f * k * p * s - g * l * p * s - f * k * q * t + g * l * q * t 293 | jointsResult[4][2] = jointsResult[3][2] - b * f * k * r + b * g * l * r + b * g * k * p * s + b * f * l * p * s + a * q * s + a * p * t - b * g * k * q * t - b * f * l * q * t 294 | 295 | jointsResult[5][0] = jointsResult[4][0] + a * f * k * u * w - a * g * l * u * w - a * g * k * p * v * w - a * f * l * p * v * w + b * q * v * w - a * g * k * p * u * x - a * f * l * p * u * x + b * q * u * x - a * f * k * v * x + a * g * l * v * x + b * p * y + a * g * k * q * y + a * f * l * q * y 296 | jointsResult[5][1] = jointsResult[4][1] + g * k * u * w + f * l * u * w + f * k * p * v * w - g * l * p * v * w + f * k * p * u * x - g * l * p * u * x - g * k * v * x - f * l * v * x - f * k * q * y + g * l * q * y 297 | jointsResult[5][2] = jointsResult[4][2] - b * f * k * u * w + b * g * l * u * w + b * g * k * p * v * w + b * f * l * p * v * w + a * q * v * w + b * g * k * p * u * x + b * f * l * p * u * x + a * q * u * x + b * f * k * v * x - b * g * l * v * x + a * p * y - b * g * k * q * y - b * f * l * q * y 298 | 299 | const M = [ 300 | [a * g * k * p * u + a * f * l * p * u - b * q * u + a * f * k * v - a * g * l * v, 301 | -B * b * p - B * a * g * k * q - B * a * f * l * q + A * a * f * k * u - A * a * g * l * u - A * a * g * k * p * v - A * a * f * l * p * v + A * b * q * v, 302 | A * b * p + A * a * g * k * q + A * a * f * l * q + B * a * f * k * u - B * a * g * l * u - B * a * g * k * p * v - B * a * f * l * p * v + B * b * q * v], 303 | [-f * k * p * u + g * l * p * u + g * k * v + f * l * v, 304 | B * f * k * q - B * g * l * q + A * g * k * u + A * f * l * u + A * f * k * p * v - A * g * l * p * v, 305 | -A * f * k * q + A * g * l * q + B * g * k * u + B * f * l * u + B * f * k * p * v - B * g * l * p * v], 306 | [-b * g * k * p * u - b * f * l * p * u - a * q * u - b * f * k * v + b * g * l * v, 307 | -B * a * p + B * b * g * k * q + B * b * f * l * q - A * b * f * k * u + A * b * g * l * u + A * b * g * k * p * v + A * b * f * l * p * v + A * a * q * v, 308 | A * a * p - A * b * g * k * q - A * b * f * l * q - B * b * f * k * u + B * b * g * l * u + B * b * g * k * p * v + B * b * f * l * p * v + B * a * q * v], 309 | ] 310 | 311 | // http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf 312 | 313 | let θ = 0 314 | let ψ = 0 315 | let φ = 0 316 | if (M[2][0] !== 1 || M[2][0] !== -1) { 317 | θ = Math.PI + Math.asin(M[2][0]) 318 | ψ = Math.atan2(M[2][1] / Math.cos(θ), M[2][2] / Math.cos(θ)) 319 | φ = Math.atan2(M[1][0] / Math.cos(θ), M[0][0] / Math.cos(θ)) 320 | } else { 321 | φ = 0 // anything; can set to 322 | if (M[2][0] === -1) { 323 | θ = Math.PI / 2 324 | ψ = φ + Math.atan2(M[0][1], M[0][2]) 325 | } else { 326 | θ = -Math.PI / 2 327 | ψ = -φ + Math.atan2(-M[0][1], -M[0][2]) 328 | } 329 | } 330 | 331 | jointsResult[5][3] = ψ 332 | jointsResult[5][4] = θ 333 | jointsResult[5][5] = φ 334 | 335 | if (this.debug) { 336 | console.log('+++++++++forward KINEMATICS++++++++++') 337 | console.log(`J0 X ${jointsResult[0][0]} Y ${jointsResult[0][1]} Z ${jointsResult[0][2]}`) 338 | console.log(`J1 X ${jointsResult[1][0]} Y ${jointsResult[1][1]} Z ${jointsResult[1][2]}`) 339 | console.log(`J2 X ${jointsResult[2][0]} Y ${jointsResult[2][1]} Z ${jointsResult[2][2]}`) 340 | console.log(`J4 X ${jointsResult[4][0]} Y ${jointsResult[4][1]} Z ${jointsResult[4][2]}`) 341 | console.log(`J5 X ${jointsResult[5][0]} Y ${jointsResult[5][1]} Z ${jointsResult[5][2]}`) 342 | console.log(`J5 A ${jointsResult[5][3]} B ${jointsResult[5][4]} C ${jointsResult[5][5]}`) 343 | console.log(`---------forward KINEMATICS----------${jointsResult[1][1]}`) 344 | } 345 | 346 | return jointsResult 347 | } 348 | 349 | static cross(vectorA, vectorB) { 350 | return [ 351 | vectorA[1] * vectorB[2] - vectorA[2] * vectorB[1], 352 | vectorA[2] * vectorB[0] - vectorA[0] * vectorB[2], 353 | vectorA[0] * vectorB[1] - vectorA[1] * vectorB[0], 354 | ] 355 | } 356 | 357 | static dot(vectorA, vectorB) { 358 | return vectorA[0] * vectorB[0] + vectorA[1] * vectorB[1] + vectorA[2] * vectorB[2] 359 | } 360 | 361 | /** 362 | * @param {Array} vectorA angle from 363 | * @param {Array} vectorB angle to 364 | * @param {Array} referenceVector angle to set 0 degree from. coplanar with vecA and vecB 365 | * @return {number} description 366 | * @example angleBetween([1,0,0],[0,1,0],[0,0,1]) // PI/2 367 | */ 368 | static angleBetween(vectorA, vectorB, referenceVector) { 369 | // angle = atan2(norm(cross(a, b)), dot(a, b)) 370 | 371 | const norm = Kinematics.length3(Kinematics.cross(vectorA, vectorB)) 372 | 373 | const angle = Math.atan2(norm, (this.dot(vectorA, vectorB))) 374 | 375 | const tmp = referenceVector[0] * vectorA[0] + referenceVector[1] * vectorA[1] + referenceVector[2] * vectorA[2] 376 | 377 | const sign = (tmp > 0) ? 1.0 : -1.0 378 | 379 | return angle * sign 380 | } 381 | 382 | static length3(vector) { 383 | return Math.sqrt((vector[0] ** 2) + (vector[1] ** 2) + (vector[2] ** 2)) 384 | } 385 | 386 | static length2(a, b) { 387 | return Math.sqrt((a ** 2) + (b ** 2)) 388 | } 389 | } 390 | 391 | --------------------------------------------------------------------------------