├── .gitignore ├── README.md ├── babel.config.js ├── deploy.sh ├── package.json ├── public ├── favicon.ico └── index.html ├── src ├── App.vue ├── assets │ ├── logo.png │ └── refresh.svg ├── components │ ├── Acrobot.vue │ ├── SimplePendulum.vue │ ├── acrobot.js │ ├── controllers │ │ ├── LQR.js │ │ ├── MPC.js │ │ ├── controller.js │ │ └── openLoopController.js │ ├── environments │ │ ├── DirectCollocation │ │ │ ├── DirectCollocation.vue │ │ │ └── DirectCollocationPlugin.vue │ │ ├── Flatness │ │ │ ├── Flatness.vue │ │ │ └── FlatnessPlugin.vue │ │ ├── KalmanFilter │ │ │ ├── KalmanFilter.vue │ │ │ └── KalmanFilterPlugin.vue │ │ ├── LQR │ │ │ ├── LQR.vue │ │ │ └── LQRPlugin.vue │ │ ├── MPC │ │ │ ├── MPC.vue │ │ │ └── MPCPlugin.vue │ │ ├── ParticleFilter │ │ │ ├── ParticleFilter.vue │ │ │ └── ParticleFilterPlugin.vue │ │ ├── RRT │ │ │ ├── RRT.vue │ │ │ └── RRTPlugin.vue │ │ ├── ValueIteration │ │ │ ├── ValueIteration.vue │ │ │ └── ValueIterationPlugin.vue │ │ ├── pluginMixin.js │ │ └── utils │ │ │ ├── ArrayInput.vue │ │ │ ├── Blocks.vue │ │ │ ├── MatrixInput.vue │ │ │ ├── Section.vue │ │ │ ├── Sensors.vue │ │ │ └── ValueInput.vue │ ├── estimators │ │ ├── kalmanFilter.js │ │ └── particleFilter.js │ ├── math.js │ ├── models │ │ ├── ControlBar.vue │ │ ├── ModelLayout.vue │ │ ├── PlotSheet.vue │ │ ├── arm │ │ │ └── arm.js │ │ ├── car │ │ │ ├── Car.vue │ │ │ └── car.js │ │ ├── cartPole │ │ │ ├── CartPole.vue │ │ │ ├── cartPole.js │ │ │ └── trajectory.js │ │ ├── doublePendulum │ │ │ ├── DoublePendulum.vue │ │ │ ├── doublePendulum.js │ │ │ └── trajectories.js │ │ ├── linearSystem.js │ │ ├── model.js │ │ ├── quadrotor2D │ │ │ ├── Quadrotor2D.vue │ │ │ ├── Quadrotor2Dold.vue │ │ │ ├── quadrotor2D.js │ │ │ └── trajectories.js │ │ ├── secondOrder │ │ │ ├── SecondOrder.vue │ │ │ ├── secondOrder.js │ │ │ ├── trajectories.js │ │ │ └── viTable.json │ │ ├── simplePendulum │ │ │ ├── simplePendulum.js │ │ │ ├── trajectories.js │ │ │ └── viTable.json │ │ ├── systems.js │ │ └── utils.js │ ├── nav │ │ ├── Drawer.vue │ │ └── Toolbar.vue │ ├── planners │ │ ├── directCollocation.js │ │ ├── interactivePath.js │ │ ├── rrt.js │ │ ├── trajectory.js │ │ ├── utils.js │ │ ├── valueIterationPlanner.js │ │ └── valueIterationPlanner2D.js │ ├── plots │ │ ├── KalmanPlot.vue │ │ ├── ParticlePlot.vue │ │ ├── RRTPlot.vue │ │ ├── TrajPlot.vue │ │ └── ValueIterationPlot.vue │ ├── plugins │ │ ├── ParticleFilterPlugin.vue │ │ └── PluginGroup.vue │ ├── systemMixin.js │ ├── twoUtils.js │ └── worldMixin.js ├── main.js ├── plugins │ └── vuetify.js ├── router │ └── router.js ├── store │ └── index.js └── views │ ├── Demo.vue │ └── Home.vue ├── vue.config.js └── yarn.lock /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | node_modules 3 | lib 4 | /dist 5 | 6 | # local env files 7 | .env.local 8 | .env.*.local 9 | 10 | # Log files 11 | npm-debug.log* 12 | yarn-debug.log* 13 | yarn-error.log* 14 | 15 | # Editor directories and files 16 | .idea 17 | .vscode 18 | *.suo 19 | *.ntvs* 20 | *.njsproj 21 | *.sln 22 | *.sw? 23 | 24 | # vscode 25 | *.code-workspace 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Website shields.io](https://img.shields.io/website-up-down-green-red/http/shields.io.svg)](https://bertrandbev.github.io/controls-js/#/) 2 | [![GitHub license](https://img.shields.io/github/license/Naereen/StrapDown.js.svg)](https://github.com/Naereen/StrapDown.js/blob/master/LICENSE) 3 | 4 |

5 | 6 |

7 | 8 | # Controls.js 9 | 10 | Controls.js is a sandbox showcasing a few modern controls techiques directly in the browser. 11 | 12 | It harnesses [eigen-js](https://github.com/BertrandBev/eigen-js) for all linear algebra and quadratic programming, and [nlopt-js](https://github.com/BertrandBev/nlopt-js) for non-linear optimization. 13 | 14 | [Home](https://bertrandbev.github.io/controls-js/#/) 15 | 16 | ## Environments 17 | 18 | ![alt text](https://api.iconify.design/mdi-matrix.svg?color=purple&width=25&height=25)   [Linear quadratic regulation](https://bertrandbev.github.io/controls-js/#/lqr/cartPole) 19 | 20 | > This environment demonstrates [linear quadratic regulation](https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator) of a few test systems around unstable trim points 21 | 22 | ![alt text](https://api.iconify.design/mdi-restore.svg?color=purple&width=25&height=25)   [Value iteration](https://bertrandbev.github.io/controls-js/#/valueIteration/simplePendulum) 23 | 24 | > A simple pendulum dynamics phase plane is discretized and [the value iteration](https://en.wikipedia.org/wiki/Markov_decision_process#Value_iteration) algorithm is run to obtain a swingup policy 25 | 26 | ![alt text](https://api.iconify.design/mdi-infinity.svg?color=purple&width=25&height=25)   [Differential flatness](https://bertrandbev.github.io/controls-js/#/flatness/quadrotor2D) 27 | 28 | > A [differentially flat](https://en.wikipedia.org/wiki/Flatness_(systems_theory)) system, a 2D quadcopter, follows a custom spatial trajectory 29 | 30 | ![alt text](https://api.iconify.design/mdi-vector-curve.svg?color=purple&width=25&height=25)   [Direct collocation](https://bertrandbev.github.io/controls-js/#/directCollocation/cartPole) 31 | 32 | > Nonlinear trajectory optimisation is demonstrated on a few dynamical systems using [direct collocation](https://en.wikipedia.org/wiki/Trajectory_optimization) 33 | 34 | ![alt text](https://api.iconify.design/mdi-camera-timer.svg?color=purple&width=25&height=25)   [Model predictive control](https://bertrandbev.github.io/controls-js/#/mpc/cartPole) 35 | 36 | > [Model predictive control](https://en.wikipedia.org/wiki/Model_predictive_control) is used to dynamically stabilize the systems in the previous section around their optimized trajectories for robust tracking. 37 | 38 | ![alt text](https://api.iconify.design/mdi-chart-bell-curve.svg?color=purple&width=25&height=25)   [Kalman Filter](https://bertrandbev.github.io/controls-js/#/kalmanFilter/car) 39 | 40 | > A [kalman filter](https://en.wikipedia.org/wiki/Kalman_filter) estimator tracks a 2d top down car's state based on radar measurements 41 | 42 | ![alt text](https://api.iconify.design/mdi-dots-hexagon.svg?color=purple&width=25&height=25)   [Particle Filter](https://bertrandbev.github.io/controls-js/#/particleFilter/car) 43 | 44 | > A [particle filter](https://en.wikipedia.org/wiki/Particle_filter) estimator tracks a 2d top down car's state based on radar measurements using a swarm of simulated particles 45 | -------------------------------------------------------------------------------- /babel.config.js: -------------------------------------------------------------------------------- 1 | module.exports = { 2 | presets: [ 3 | '@vue/cli-plugin-babel/preset' 4 | ], 5 | sourceType: 'unambiguous' 6 | } 7 | -------------------------------------------------------------------------------- /deploy.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # package.json "deploy": "yarn build; push-dir --dir=dist --branch=gh-pages --cleanup" 3 | 4 | # abort on errors 5 | set -e 6 | 7 | # build 8 | yarn build 9 | 10 | # navigate into the build output directory 11 | cd dist 12 | 13 | # Commit repo 14 | git init 15 | git add -A 16 | git commit -m 'deploy' 17 | git push -f git@github.com:BertrandBev/controls-js.git master:gh-pages 18 | 19 | # Nav back 20 | cd - -------------------------------------------------------------------------------- /package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "nl-controls", 3 | "version": "0.1.0", 4 | "private": true, 5 | "scripts": { 6 | "serve": "vue-cli-service serve", 7 | "build": "vue-cli-service build", 8 | "lint": "vue-cli-service lint" 9 | }, 10 | "dependencies": { 11 | "animejs": "^3.2.1", 12 | "arraybuffer-loader": "^1.0.8", 13 | "chart.js": "^2.9.4", 14 | "chroma-js": "^2.1.0", 15 | "core-js": "^3.8.2", 16 | "eigen": "^0.2.0", 17 | "lodash": "^4.17.20", 18 | "markdown-it": "^12.0.4", 19 | "nlopt-js": "^0.1.1", 20 | "plotly.js-dist": "^1.58.4", 21 | "two.js": "^0.7.0-stable.1", 22 | "vue": "^2.6.12", 23 | "vue-chartjs": "^3.5.1", 24 | "vue-notification": "^1.3.20", 25 | "vue-router": "^3.4.9", 26 | "vue-svg-loader": "^0.16.0", 27 | "vuetify": "^2.4.2", 28 | "vuex": "^3.6.0", 29 | "wasm-loader": "^1.3.0" 30 | }, 31 | "devDependencies": { 32 | "@babel/plugin-syntax-import-meta": "^7.10.4", 33 | "@vue/cli-plugin-babel": "^4.5.10", 34 | "@vue/cli-plugin-eslint": "^4.5.10", 35 | "@vue/cli-plugin-router": "^4.5.10", 36 | "@vue/cli-plugin-vuex": "^4.5.10", 37 | "@vue/cli-service": "^4.5.10", 38 | "babel-eslint": "^10.1.0", 39 | "eslint": "^7.17.0", 40 | "eslint-plugin-vue": "^7.4.1", 41 | "html-loader": "^1.3.2", 42 | "sass": "^1.32.2", 43 | "sass-loader": "^10.1.0", 44 | "vue-cli-plugin-pug": "^2.0.0", 45 | "vue-cli-plugin-vuetify": "^2.0.9", 46 | "vue-template-compiler": "^2.6.12", 47 | "vuetify-loader": "^1.6.0" 48 | }, 49 | "eslintConfig": { 50 | "root": true, 51 | "env": { 52 | "node": true 53 | }, 54 | "extends": [ 55 | "plugin:vue/essential", 56 | "eslint:recommended" 57 | ], 58 | "rules": { 59 | "no-console": "off", 60 | "no-unused-vars": "off", 61 | "vue/no-mutating-props": "off" 62 | }, 63 | "parserOptions": { 64 | "parser": "babel-eslint" 65 | } 66 | }, 67 | "eslintIgnore": [ 68 | "lib/*" 69 | ], 70 | "postcss": { 71 | "plugins": { 72 | "autoprefixer": {} 73 | } 74 | }, 75 | "browserslist": [ 76 | "> 1%", 77 | "last 2 versions" 78 | ] 79 | } 80 | -------------------------------------------------------------------------------- /public/favicon.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BertrandBev/controls-js/d141c78dbb1064cd3e56037b0fc7ff32177d3071/public/favicon.ico -------------------------------------------------------------------------------- /public/index.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | nl-controls 10 | 11 | 12 | 13 | 14 | 15 | 19 |
20 | 21 | 22 | 23 | 26 | 27 | -------------------------------------------------------------------------------- /src/App.vue: -------------------------------------------------------------------------------- 1 | 25 | 26 | 69 | 70 | -------------------------------------------------------------------------------- /src/assets/logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/BertrandBev/controls-js/d141c78dbb1064cd3e56037b0fc7ff32177d3071/src/assets/logo.png -------------------------------------------------------------------------------- /src/assets/refresh.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/components/Acrobot.vue: -------------------------------------------------------------------------------- 1 | 6 | 7 | 105 | 106 | -------------------------------------------------------------------------------- /src/components/SimplePendulum.vue: -------------------------------------------------------------------------------- 1 | 8 | 9 | 158 | 159 | -------------------------------------------------------------------------------- /src/components/acrobot.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen' 2 | import _ from 'lodash' 3 | import LQR, { wrapAngle, sqr } from './controllers/LQR.js' 4 | 5 | class Acrobot { 6 | constructor(params = {}) { 7 | this.params = { 8 | g: 9.81, 9 | l1: 1, 10 | l2: 1, 11 | lc1: 0.5, 12 | lc2: 0.5, 13 | m1: 1, 14 | m2: 1, 15 | I1: 1, 16 | I2: 1, 17 | mu1: 0.5, 18 | mu2: 0.5, 19 | ...params 20 | } 21 | this.params = { 22 | g: 9.81, 23 | l1: 1, 24 | l2: 1, 25 | lc1: 0.5, 26 | lc2: 0.5, 27 | m1: 1, 28 | m2: 1, 29 | I1: 1, 30 | I2: 1, 31 | mu1: 0.5, 32 | mu2: 0.5, 33 | ...params 34 | } 35 | const x = params.x0 || new eig.Matrix(4, 1); 36 | eig.GC.set(this, 'x', x); 37 | this.target = null 38 | } 39 | 40 | /** 41 | * Get the shape of a system 42 | * @returns {Array} shape [xn, un] 43 | */ 44 | shape() { 45 | return [4, 1] 46 | } 47 | 48 | /** 49 | * Returns dx/dt 50 | * @param {Matrix} x 51 | * @param {Matrix} u 52 | * @returns {Matrix} dx 53 | */ 54 | dynamics(x, u) { 55 | // x = [theta1, theta2, dtheta1, dtheta2] 56 | const p = this.params 57 | const [c1, s1] = [Math.cos(x.get(0)), Math.sin(x.get(0))] 58 | const [c2, s2] = [Math.cos(x.get(1)), Math.sin(x.get(1))] 59 | const [q1, q2] = [x.get(2), x.get(3)] 60 | const s12 = Math.sin(x.get(0) + x.get(1)) 61 | 62 | // Mass matrix 63 | const M = new eig.Matrix([[ 64 | p.I1 + p.I2 + p.m2 * sqr(p.l1) + 2 * p.m2 * p.l1 * p.lc2 * c2, 65 | p.I2 + p.m2 * p.l1 * p.lc2 * c2 66 | ], [ 67 | p.I2 + p.m2 * p.l1 * p.lc2 * c2, 68 | p.I2 69 | ]]) 70 | const Minv = M.inverse() 71 | // Coriolis matrix 72 | const C = new eig.Matrix([[ 73 | -2 * p.m2 * p.l1 * p.lc2 * s2 * q2, -p.m2 * p.l1 * p.lc2 * s2 * q2 74 | ], [ 75 | p.m2 * p.l1 * p.lc2 * s2 * q1, 0 76 | ]]) 77 | // Gravity matrix 78 | const G = new eig.Matrix([ 79 | -p.m1 * p.g * p.lc1 * s1 - p.m2 * p.g * (p.l1 * s1 + p.lc2 * s12), 80 | - p.m2 * p.g * p.l2 * s12 81 | ]) 82 | // Controls matrix 83 | const B = new eig.Matrix([0, 1]) 84 | // Damping matrix 85 | const D = new eig.Matrix([[-p.mu1, 0], [0, -p.mu2]]) 86 | // From standard manipulator form 87 | // M(q)ddq + C(q, dq)dq = Tg(q) + Bu 88 | // http://underactuated.mit.edu/underactuated.html?chapter=acrobot 89 | const dx = new eig.Matrix([q1, q2]) 90 | const ddx = Minv.matMul(G.matAdd(B.matMul(u)).matSub(C.matMul(dx))).matAdd(D.matMul(dx)) 91 | return dx.vcat(ddx) 92 | } 93 | 94 | 95 | /** 96 | * Execute a step 97 | * @param {Matrix} u controls effort 98 | * @param {Number} dt delta time 99 | */ 100 | step(u, dt) { 101 | const dx = this.dynamics(this.x, u) 102 | // Override x if target tracking 103 | if (this.target) { 104 | const theta = -wrapAngle(Math.atan2(this.target.y, this.target.x) - Math.PI / 2) 105 | this.x.set(2, -10 * wrapAngle(this.x.get(0) - theta)) 106 | this.x.set(3, -10 * wrapAngle(this.x.get(1))) 107 | dx.set(0, this.x.get(2)) 108 | dx.set(1, this.x.get(3)) 109 | dx.set(2, 0) 110 | dx.set(3, 0) 111 | } 112 | // console.log('x', x, 'xDot', xDot) 113 | const newX = this.x.matAdd(dx.mul(dt)) 114 | newX.set(0, wrapAngle(newX.get(0))) 115 | newX.set(1, wrapAngle(newX.get(1))) 116 | eig.GC.set(this, 'x', newX) 117 | } 118 | } 119 | 120 | export { Acrobot } -------------------------------------------------------------------------------- /src/components/controllers/LQR.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen' 2 | import _ from 'lodash' 3 | import Controller from './controller.js' 4 | import LinearSystem from '@/components/models/linearSystem.js' 5 | 6 | class LQR extends Controller { 7 | /** 8 | * Build a LQR controller to stabilize the system around x0 9 | * @param {Object} system system of interest 10 | * @param {Array} x0 equilibrium state 11 | * @param {Array} u0 equilibrium command 12 | */ 13 | constructor(system, x0, u0) { 14 | super(system) 15 | this.system = system 16 | eig.GC.set(this, 'x0', x0) 17 | eig.GC.set(this, 'u0', u0) 18 | const [xn, un] = this.system.shape 19 | } 20 | 21 | ready() { 22 | return !!this.K 23 | } 24 | 25 | solve(params) { 26 | const [Jx, Ju] = LinearSystem.linearizeSystem(this.system, this.x0, this.u0) 27 | const Q = new eig.Matrix(params.Q) 28 | const R = new eig.Matrix(params.R) 29 | const sol = eig.Solvers.careSolve(Jx, Ju, Q, R); 30 | eig.GC.set(this, 'K', sol.K) 31 | } 32 | 33 | simulate(dx, dt, duration) { 34 | const xn = this.system.shape[0] 35 | const x0 = this.x0.matAdd(eig.Matrix.ones(xn, 1).mul(dx)) 36 | const arr = this.system.simulate(this, x0, dt, duration) 37 | const xu0 = this.x0.vcat(this.u0) 38 | arr.forEach(x => x.matSubSelf(xu0)) 39 | return arr 40 | } 41 | 42 | linearSimulate(dx, dt, duration) { 43 | const linsys = LinearSystem.fromModel(this.system, this.x0, this.u0) 44 | const xn = this.system.shape[0] 45 | const x0 = this.x0.matAdd(eig.Matrix.ones(xn, 1).mul(dx)) 46 | const arr = linsys.simulate(this, x0, dt, duration) 47 | const xu0 = this.x0.vcat(this.u0) 48 | arr.forEach(x => x.matSubSelf(xu0)) 49 | linsys.delete(); 50 | return arr 51 | } 52 | 53 | /** 54 | * Get command 55 | * @returns {Array} u command 56 | */ 57 | getCommand(x, t) { 58 | const delta_x = this.x0.matSub(x || this.system.x) 59 | this.system.bound(delta_x) 60 | return this.K.matMul(delta_x).matAdd(this.u0).clamp(-1000, 1000) // TODO: use system actuator limits 61 | } 62 | } 63 | 64 | export default LQR -------------------------------------------------------------------------------- /src/components/controllers/MPC.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen'; 2 | import Controller from './controller.js'; 3 | import LinearSystem from '../models/linearSystem.js'; 4 | 5 | class MPC extends Controller { 6 | static DEBUG = false; 7 | /** 8 | * Create a MPC instance 9 | * @param {Object} system 10 | * @param {Trajectory} trajectory 11 | * @param {Number} dt 12 | */ 13 | constructor(system, trajectory, dt, n, uBounds) { 14 | super(system); 15 | this.system = system; 16 | this.trajectory = trajectory; 17 | this.dt = dt; 18 | this.n = MPC.DEBUG ? 3 : n; 19 | this.uBounds = uBounds; 20 | // TODO: pass as argument 21 | this.params = this.system.mpcParams(); 22 | } 23 | 24 | optimizeInner(refTraj, linTraj, dx0) { 25 | // Minimize 0.5 xT.P.x + qT.x 26 | // Suject to l <= Ax <= u 27 | 28 | // Solve MPC starting at t 29 | const a = Date.now(); 30 | const [xn, un] = this.system.shape; 31 | const dim = (xn + un) * this.n; 32 | 33 | // Create cost matrix 34 | const P = eig.SparseMatrix.identity(dim, dim).mul(1); 35 | for (let n = 0; n < xn * this.n; n += 1) { 36 | if (this.params.xWeight) { 37 | // Use params weighting 38 | P.set(n, n, this.params.xWeight[n % xn]); 39 | } else if ((n % xn) < xn / 2) { 40 | // Default weighting 41 | P.set(n, n, 100); 42 | } 43 | } 44 | for (let n = xn * this.n; n < dim; n++) { 45 | P.set(n, n, 0.01); 46 | } 47 | const q = new eig.Matrix(dim, 1); 48 | 49 | // Create dynamic constraints 50 | const At = new eig.TripletVector(20); // TODO figure out reserve 51 | const In = eig.Matrix.identity(xn, xn); 52 | const negOnes = eig.Matrix.ones(xn, 1).mul(-1); 53 | for (let n = 0; n < this.n; n++) { 54 | const pt = linTraj[n]; 55 | const x0 = pt.block(0, 0, xn, 1); 56 | const u0 = pt.block(xn, 0, un, 1); 57 | if (n >= this.n - 1) { 58 | continue; 59 | } 60 | 61 | // Build constraint matrices 62 | const Ab = this.system.xJacobian(x0, u0).mul(this.dt).matAdd(In); 63 | const Bb = this.system.uJacobian(x0, u0).mul(this.dt); 64 | 65 | if (MPC.DEBUG) { 66 | x0.print("x0"); 67 | u0.print("u0"); 68 | Ab.print(`A_${n}`); 69 | Bb.print(`B_${n}`); 70 | } 71 | 72 | At.addBlock(n * xn, n * xn, Ab); 73 | At.addDiag(n * xn, (n + 1) * xn, negOnes); 74 | At.addBlock(n * xn, this.n * xn + n * un, Bb); 75 | 76 | if (MPC.DEBUG) { 77 | console.log('=== testing jacobian'); 78 | LinearSystem.testJacobian(this.system); 79 | console.log('=== jacobian tested!'); 80 | } 81 | } 82 | At.addDiag((this.n - 1) * xn, 0, negOnes.mul(-1)); 83 | const uDiag = eig.Matrix.ones(un * this.n, 1); 84 | At.addDiag(this.n * xn, this.n * xn, uDiag); 85 | const A = new eig.SparseMatrix(dim, dim, At); 86 | const lb = new eig.Matrix(dim, 1); 87 | const ub = new eig.Matrix(dim, 1); 88 | this.system.bound(dx0); 89 | // dx0.print('dx0'); 90 | lb.setBlock((this.n - 1) * xn, 0, dx0); 91 | ub.setBlock((this.n - 1) * xn, 0, dx0); 92 | for (let k = this.n * xn; k < dim; k++) { 93 | lb.set(k, this.uBounds.min[k % un]); 94 | ub.set(k, this.uBounds.max[k % un]); 95 | } 96 | 97 | // Solve quadratic program 98 | const x = eig.Solvers.quadProgSolve(P, q, A, lb, ub); 99 | 100 | if (MPC.DEBUG) { 101 | dx0.print("dx0"); 102 | (new eig.SparseMatrix(P)).print("P"); 103 | q.print("q"); 104 | (new eig.SparseMatrix(A)).print("A"); 105 | lb.print('lb'); 106 | ub.print('ub'); 107 | x.print('result'); 108 | } 109 | 110 | const xTraj = []; 111 | for (let n = 0; n < this.n; n += 1) { 112 | const pt = refTraj[n]; 113 | const x0 = pt.block(0, 0, xn, 1); 114 | const u0 = pt.block(xn, 0, un, 1); 115 | const x_n = x.block(n * xn, 0, xn, 1).matAdd(x0); 116 | const u_n = x.block(this.n * xn + n * un, 0, un, 1).matAdd(u0); 117 | xTraj.push(x_n.vcat(u_n)); 118 | } 119 | 120 | return xTraj; 121 | } 122 | 123 | optimize(t) { 124 | // Prepare first pass 125 | const ptList = []; 126 | for (let n = 0; n < this.n; n++) { 127 | const pt = this.trajectory.get(t + n * this.dt); 128 | ptList.push(pt); 129 | } 130 | const [xn, un] = this.system.shape; 131 | const x0 = ptList[0].block(0, 0, xn, 1); 132 | const dx0 = this.system.x.matSub(x0); 133 | 134 | // const traj = this.optimizeInner(ptList, ptList, dx0); 135 | return this.optimizeInner(ptList, ptList, dx0); 136 | } 137 | } 138 | 139 | export default MPC; -------------------------------------------------------------------------------- /src/components/controllers/controller.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen' 2 | 3 | class Controller { 4 | constructor(system) { 5 | this.system = system 6 | } 7 | } 8 | 9 | export default Controller -------------------------------------------------------------------------------- /src/components/controllers/openLoopController.js: -------------------------------------------------------------------------------- 1 | import Controller from './controller.js' 2 | 3 | class OpenLoopController extends Controller { 4 | constructor(system, trajectory) { 5 | super(system) 6 | this.trajectory = trajectory 7 | } 8 | 9 | reset() { 10 | if (!this.trajectory.ready()) { 11 | return console.error('The trajectory must be ready') 12 | } 13 | this.trajectory.reset() 14 | let [xn, un] = this.system.shape 15 | const x = this.trajectory.array[0].block(0, 0, xn, 1); 16 | this.system.setState(x); 17 | } 18 | 19 | getCommand(x, t) { 20 | let [xn, un] = this.system.shape 21 | const xTraj = this.trajectory.get(t) 22 | return xTraj.block(xn, 0, un, 1) 23 | } 24 | } 25 | 26 | export default OpenLoopController -------------------------------------------------------------------------------- /src/components/environments/DirectCollocation/DirectCollocation.vue: -------------------------------------------------------------------------------- 1 | 23 | 24 | 93 | 94 | -------------------------------------------------------------------------------- /src/components/environments/DirectCollocation/DirectCollocationPlugin.vue: -------------------------------------------------------------------------------- 1 | 42 | 43 | -------------------------------------------------------------------------------- /src/components/environments/Flatness/Flatness.vue: -------------------------------------------------------------------------------- 1 | 24 | 25 | 98 | 99 | -------------------------------------------------------------------------------- /src/components/environments/Flatness/FlatnessPlugin.vue: -------------------------------------------------------------------------------- 1 | 9 | 10 | -------------------------------------------------------------------------------- /src/components/environments/KalmanFilter/KalmanFilter.vue: -------------------------------------------------------------------------------- 1 | 19 | 20 | 88 | 89 | -------------------------------------------------------------------------------- /src/components/environments/KalmanFilter/KalmanFilterPlugin.vue: -------------------------------------------------------------------------------- 1 | 14 | 15 | 152 | 153 | -------------------------------------------------------------------------------- /src/components/environments/LQR/LQR.vue: -------------------------------------------------------------------------------- 1 | 21 | 22 | 90 | 91 | -------------------------------------------------------------------------------- /src/components/environments/LQR/LQRPlugin.vue: -------------------------------------------------------------------------------- 1 | 23 | 24 | -------------------------------------------------------------------------------- /src/components/environments/MPC/MPC.vue: -------------------------------------------------------------------------------- 1 | 22 | 23 | 104 | 105 | -------------------------------------------------------------------------------- /src/components/environments/MPC/MPCPlugin.vue: -------------------------------------------------------------------------------- 1 | 25 | 26 | -------------------------------------------------------------------------------- /src/components/environments/ParticleFilter/ParticleFilter.vue: -------------------------------------------------------------------------------- 1 | 18 | 19 | 85 | 86 | -------------------------------------------------------------------------------- /src/components/environments/ParticleFilter/ParticleFilterPlugin.vue: -------------------------------------------------------------------------------- 1 | 20 | 21 | 190 | 191 | -------------------------------------------------------------------------------- /src/components/environments/RRT/RRT.vue: -------------------------------------------------------------------------------- 1 | 18 | 19 | 79 | 80 | -------------------------------------------------------------------------------- /src/components/environments/RRT/RRTPlugin.vue: -------------------------------------------------------------------------------- 1 | 12 | 13 | -------------------------------------------------------------------------------- /src/components/environments/ValueIteration/ValueIteration.vue: -------------------------------------------------------------------------------- 1 | 25 | 26 | 106 | 107 | -------------------------------------------------------------------------------- /src/components/environments/ValueIteration/ValueIterationPlugin.vue: -------------------------------------------------------------------------------- 1 | 25 | 26 | -------------------------------------------------------------------------------- /src/components/environments/pluginMixin.js: -------------------------------------------------------------------------------- 1 | export default { 2 | data: () => ({ 3 | active: false 4 | }), 5 | 6 | computed: { 7 | name() { 8 | return this.$options.name 9 | }, 10 | 11 | mouseTargetEnabled() { 12 | return true; // Override if needed 13 | } 14 | }, 15 | 16 | methods: { 17 | createGraphics(two) { 18 | // Override if needed 19 | }, 20 | 21 | update(t, dt) { 22 | // Override if needed 23 | }, 24 | 25 | stepSystem() { 26 | // Override if needed 27 | return true; 28 | }, 29 | 30 | reset() { 31 | throw new Error('must be overridden') 32 | }, 33 | 34 | ready() { 35 | throw new Error('must be overridden') 36 | }, 37 | 38 | updateSystem(t, dt) { 39 | throw new Error('must be overridden') 40 | } 41 | } 42 | } -------------------------------------------------------------------------------- /src/components/environments/utils/ArrayInput.vue: -------------------------------------------------------------------------------- 1 | 4 | 5 | -------------------------------------------------------------------------------- /src/components/environments/utils/Blocks.vue: -------------------------------------------------------------------------------- 1 | 20 | 21 | -------------------------------------------------------------------------------- /src/components/environments/utils/MatrixInput.vue: -------------------------------------------------------------------------------- 1 | 40 | 41 | -------------------------------------------------------------------------------- /src/components/environments/utils/Section.vue: -------------------------------------------------------------------------------- 1 | 9 | 10 | -------------------------------------------------------------------------------- /src/components/environments/utils/Sensors.vue: -------------------------------------------------------------------------------- 1 | 21 | 22 | -------------------------------------------------------------------------------- /src/components/environments/utils/ValueInput.vue: -------------------------------------------------------------------------------- 1 | 8 | 9 | -------------------------------------------------------------------------------- /src/components/estimators/kalmanFilter.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen'; 2 | import LinearSystem from '@/components/models/linearSystem.js'; 3 | import { addNoise } from '@/components/math.js'; 4 | 5 | class KalmanFilter { 6 | constructor(system) { 7 | this.system = system; 8 | this.watchers = new Set(); 9 | } 10 | 11 | ready() { 12 | return !!this.P && !!this.Q; 13 | } 14 | 15 | reset(P, Q) { 16 | // Init mean & covariance 17 | const x = new eig.Matrix(this.system.x); 18 | // Store data 19 | eig.GC.set(this, 'x', x); 20 | eig.GC.set(this, 'P', P); 21 | eig.GC.set(this, 'Q', Q); 22 | } 23 | 24 | predict(u, dt) { 25 | // Retrieve system jacobian 26 | const [Jx, Ju] = LinearSystem.linearizeSystem(this.system, this.x, u); // TODO: use real jacobian if available 27 | // Transform mean & covariance matrices 28 | const dx = this.system.dynamics(this.x, u).mul(dt); 29 | this.x.matAddSelf(dx); 30 | this.system.bound(this.x); 31 | const dP = Jx.matMul(this.P).matMul(Jx.transpose()).matAdd(this.Q).mul(dt); 32 | this.P.matAddSelf(dP); 33 | // Notify watchers 34 | this.watchers.forEach(fun => fun()); 35 | } 36 | 37 | update(sensor) { 38 | // Take measurement 39 | const z = sensor.measurement(sensor, this.system.x); 40 | const zHat = sensor.measurement(sensor, this.x); 41 | // Add measurement noise 42 | const cov = new eig.Matrix(sensor.noise); 43 | addNoise(z, cov); 44 | const H = LinearSystem.linearize(x => sensor.measurement(sensor, x), this.x); 45 | const R = new eig.Matrix(sensor.noise); 46 | // Compute Kalman gain 47 | const PHt = this.P.matMul(H.transpose()); 48 | const K = PHt.matMul((H.matMul(PHt).matAdd(R)).inverse()); 49 | // Update mean & covariance 50 | this.x.matAddSelf(K.matMul(z.matSub(zHat))); 51 | this.P.matSubSelf(K.matMul(H).matMul(this.P)); 52 | // Notify watchers 53 | this.watchers.forEach(fun => fun()); 54 | } 55 | 56 | /** 57 | * Add update callback 58 | */ 59 | addWatcher(fun) { 60 | this.watchers.add(fun); 61 | } 62 | 63 | /** 64 | * Add update callback 65 | */ 66 | removeWatcher(fun) { 67 | this.watchers.delete(fun); 68 | } 69 | } 70 | 71 | export default KalmanFilter; -------------------------------------------------------------------------------- /src/components/estimators/particleFilter.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen'; 2 | import _ from 'lodash'; 3 | import { addNoise, Gaussian } from '@/components/math.js'; 4 | 5 | 6 | class Particle { 7 | constructor(system, params) { 8 | this.params = params; 9 | this.system = new system.constructor(); 10 | this.weight = 1; 11 | switch (params.distribution) { 12 | case 'exact': 13 | // Copy state 14 | this.system.setState(system.x); 15 | break; 16 | case 'uniform': { 17 | // Sample state uniformely 18 | const x = eig.Matrix.random(system.shape[0], 1); 19 | for (let k = 0; k < x.rows(); k++) { 20 | const min = params.range.min[k]; 21 | const max = params.range.max[k]; 22 | this.system.x.set(k, min + (max - min) * x.get(k)); 23 | } 24 | break; 25 | } 26 | case 'normal': { 27 | // Sample state from a multimodal gaussian density 28 | break; 29 | } 30 | } 31 | } 32 | 33 | clone() { 34 | const newParticle = new Particle(this.system, this.params); 35 | newParticle.system.setState(this.system.x); 36 | return newParticle; 37 | } 38 | 39 | delete() { 40 | this.system.delete(); 41 | } 42 | 43 | predict(u, dt) { 44 | this.system.step(u, dt); 45 | // Add process noise 46 | const cov = new eig.Matrix(this.params.processNoise).mul(dt); 47 | addNoise(this.system.x, cov); 48 | } 49 | 50 | update(sensor, z) { 51 | // Simulate measurement 52 | const zHat = sensor.measurement(sensor, this.system.x); 53 | // Create gaussian if needed TODO: clear on parameter update 54 | if (!sensor.gaussian) { 55 | const mean = new eig.Matrix(zHat.rows(), 1); 56 | const cov = new eig.Matrix(sensor.noise); 57 | sensor.gaussian = new Gaussian(mean, cov); 58 | } 59 | // Update weight according to measurement likelihood 60 | const val = sensor.gaussian.density(zHat.matSub(z)); 61 | this.weight *= val; 62 | } 63 | } 64 | 65 | class ParticleFilter { 66 | constructor(system) { 67 | this.system = system; 68 | this.particles = []; 69 | this.watchers = new Set(); 70 | } 71 | 72 | ready() { 73 | return true; 74 | } 75 | 76 | reset(params) { 77 | this.params = _.cloneDeep(params); 78 | this.particles.forEach(p => p.delete()); 79 | this.particles = []; 80 | for (let k = 0; k < params.nPts; k++) { 81 | const p = new Particle(this.system, params); 82 | this.particles.push(p); 83 | } 84 | this.normalizeWeights(); 85 | } 86 | 87 | predict(u, dt) { 88 | this.particles.forEach(particle => particle.predict(u, dt)); 89 | // Notify watchers 90 | this.watchers.forEach(fun => fun()); 91 | } 92 | 93 | normalizeWeights() { 94 | // Pseudo-normalize weight (~soft max?) 95 | const weights = this.particles.map(p => p.weight); 96 | const max = _.max(weights); 97 | const W = max; // TEMP (find better way) 98 | this.particles.forEach(p => p.weight /= W); 99 | } 100 | 101 | update(sensor) { 102 | // Take measurement 103 | const z = sensor.measurement(sensor, this.system.x); 104 | // Add measurement noise 105 | const cov = new eig.Matrix(sensor.noise); 106 | addNoise(z, cov); 107 | // Update particles 108 | this.particles.forEach(particle => particle.update(sensor, z)); 109 | this.normalizeWeights(); 110 | // Notify watchers 111 | this.watchers.forEach(fun => fun()); 112 | } 113 | 114 | resample() { 115 | // Stochastic sampling algorithm 116 | const n = this.particles.length; 117 | const newParticles = []; 118 | let index = Math.floor(Math.random() * n); 119 | let beta = 0.0; 120 | const weights = this.particles.map(p => p.weight); 121 | const mw = _.max(weights); 122 | for (let k = 0; k < n; k++) { 123 | beta += Math.random() * 2.0 * mw; 124 | while (beta > weights[index]) { 125 | beta -= weights[index]; 126 | index = (index + 1) % n; 127 | } 128 | const newParticle = this.particles[index].clone(); 129 | newParticles.push(newParticle); 130 | } 131 | this.particles.forEach(p => p.delete()); 132 | this.particles = newParticles; 133 | } 134 | 135 | 136 | /** 137 | * Add update callback 138 | */ 139 | addWatcher(fun) { 140 | this.watchers.add(fun); 141 | } 142 | 143 | /** 144 | * Add update callback 145 | */ 146 | removeWatcher(fun) { 147 | this.watchers.delete(fun); 148 | } 149 | } 150 | 151 | export default ParticleFilter; -------------------------------------------------------------------------------- /src/components/math.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen' 2 | 3 | function wrapAngle(angle) { 4 | let mod = (angle + Math.PI) % (2 * Math.PI) 5 | if (mod < 0) { mod += 2 * Math.PI } 6 | return mod - Math.PI 7 | } 8 | 9 | function sqr(val) { 10 | return Math.pow(val, 2) 11 | } 12 | 13 | function matFromDiag(diag) { 14 | const n = diag.length 15 | const mat = [...Array(n)].map(() => [...Array(n)].map(() => 0)) 16 | for (let k = 0; k < n; k++) { 17 | mat[k][k] = diag[k] 18 | } 19 | return mat 20 | } 21 | 22 | function addNoise(x, cov) { 23 | // Add measurement noise (Consider adding that in a library) 24 | const mean = new eig.Matrix(x.rows(), 1); 25 | const rdn = eig.Random.normal(mean, cov, 1); 26 | x.matAddSelf(rdn); 27 | } 28 | 29 | // A multivariate gaussian class 30 | class Gaussian { 31 | constructor(mean, cov) { 32 | this.setMean(mean) 33 | this.setCov(cov) 34 | } 35 | 36 | setMean(mean) { 37 | eig.GC.set(this, 'mean', mean) 38 | } 39 | 40 | setCov(cov) { 41 | this.absDet = Math.abs(cov.det()); 42 | const invCov = cov.inverse(); 43 | eig.GC.set(this, 'invCov', invCov) 44 | } 45 | 46 | delete() { 47 | eig.GC.popException(this.mean) 48 | eig.GC.popException(this.invCov) 49 | } 50 | 51 | density(x) { 52 | const k = this.mean.rows(); 53 | const delta = x.matSub(this.mean); 54 | const scalar = delta.transpose().matMul(this.invCov).matMul(delta).get(0, 0); 55 | return Math.exp(-0.5 * scalar) / Math.sqrt(Math.pow(2 * Math.PI, k) * this.absDet) 56 | } 57 | } 58 | 59 | 60 | /** 61 | * TODO: add to shared lib 62 | * @param {Array} traj 63 | * @param {Number} dt 64 | * @param {Number} tau Decay characteristic time for low-pass 65 | */ 66 | function differenciate(traj, dt, tau = 1e-8, loop = false) { 67 | const dTraj = [] 68 | for (let k = 0; k < traj.length; k++) { 69 | const div = traj[(k + 1) % traj.length].matSub(traj[k % traj.length]).div(dt) 70 | dTraj.push(div) 71 | } 72 | return smooth(dTraj, dt, tau) 73 | } 74 | 75 | /** 76 | * 77 | * @param {Array} traj 78 | * @param {Number} dt 79 | * @param {Number} tau Decay characteristic time for low-pass 80 | * @param {Boolean} wrap Wether the trajectory smoothing should wrap 81 | */ 82 | function smooth(traj, dt, tau = 1e-8, wrap = true) { 83 | if (traj.length === 0) { 84 | return [] 85 | } 86 | const decay = Math.exp(-2 * Math.PI * dt / tau) 87 | let val = traj[0] 88 | const smoothed = [] 89 | for (let k = 0; k < (wrap ? 2 : 1) * traj.length; k++) { 90 | val = val.mul(decay).matAdd(traj[k % traj.length].mul(1 - decay)) 91 | if (!wrap || k >= traj.length) { 92 | smoothed.push(val) 93 | } 94 | } 95 | return smoothed 96 | } 97 | 98 | export { wrapAngle, sqr, matFromDiag, addNoise, Gaussian, differenciate, smooth } -------------------------------------------------------------------------------- /src/components/models/ControlBar.vue: -------------------------------------------------------------------------------- 1 | 10 | 11 | -------------------------------------------------------------------------------- /src/components/models/ModelLayout.vue: -------------------------------------------------------------------------------- 1 | 31 | 32 | 140 | 141 | -------------------------------------------------------------------------------- /src/components/models/PlotSheet.vue: -------------------------------------------------------------------------------- 1 | 10 | 11 | -------------------------------------------------------------------------------- /src/components/models/car/Car.vue: -------------------------------------------------------------------------------- 1 | 21 | 22 | 83 | 84 | -------------------------------------------------------------------------------- /src/components/models/cartPole/CartPole.vue: -------------------------------------------------------------------------------- 1 | 11 | 12 | 177 | 178 | -------------------------------------------------------------------------------- /src/components/models/doublePendulum/DoublePendulum.vue: -------------------------------------------------------------------------------- 1 | 20 | 21 | 77 | 78 | -------------------------------------------------------------------------------- /src/components/models/linearSystem.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen' 2 | import Model from './model.js' 3 | 4 | class LinearSystem extends Model { 5 | constructor(x0, u0, A, B, states, commands) { 6 | super(states, commands, {}) 7 | eig.GC.set(this, 'x0', x0) 8 | eig.GC.set(this, 'u0', u0) 9 | eig.GC.set(this, 'A', A) 10 | eig.GC.set(this, 'B', B) 11 | } 12 | 13 | delete() { 14 | ['x0', 'u0', 'A', 'B'].forEach(k => eig.GC.popException(this[k])) 15 | } 16 | 17 | trim() { 18 | return { x: new eig.Matrix(this.shape[0], 1), u: new eig.Matrix(this.shape[1], 1) } 19 | } 20 | 21 | /** 22 | * Returns dx/dt 23 | * @param {Matrix} x 24 | * @param {Matrix} u 25 | * @returns {Matrix} dx 26 | */ 27 | dynamics(x, u) { 28 | const dx = x.matSub(this.x0) 29 | const du = u.matSub(this.u0) 30 | return this.A.matMul(dx).matAdd(this.B.matMul(du)) 31 | } 32 | 33 | /** 34 | * Create linear system from system 35 | * @param {Object} system 36 | * @param {Matrix} x0 37 | * @param {Matrix} u0 38 | */ 39 | static fromModel(system, x0, u0) { 40 | const [Jx, Ju] = LinearSystem.linearizeSystem(system, x0, u0) 41 | return new LinearSystem(x0, u0, Jx, Ju, system.states, system.commands) 42 | } 43 | 44 | /** 45 | * Linearize function about x0 46 | * @param {Function} fun function of interest 47 | * @param {Matrix} x0 equilibrium state 48 | * @returns {Matrix} A (df/dx at x0) 49 | */ 50 | static linearize(fun, x0) { 51 | const eps = 1e-8 52 | const dx0 = fun(x0); 53 | const [m, n] = [dx0.rows(), x0.rows()] 54 | // TODO: extract in C lib ? 55 | function setCol(mat, row, vec) { 56 | for (let k = 0; k < vec.rows(); k++) { 57 | mat.set(k, row, vec.get(k)) 58 | } 59 | } 60 | // Populate A matrix 61 | let A = new eig.Matrix(m, n); 62 | for (let k = 0; k < n; k += 1) { 63 | const x = new eig.Matrix(x0); 64 | x.set(k, x.get(k) + eps); 65 | const dx = fun(x).matSub(dx0).div(eps); 66 | setCol(A, k, dx); 67 | } 68 | return A; 69 | } 70 | 71 | /** 72 | * Linearize system about x0 & u0 73 | * @param {Object} system system of interest 74 | * @param {Matrix} x0 equilibrium state 75 | * @param {Matrix} u0 equilibrium command 76 | * @returns {Matrix} [Jx, Ju] 77 | */ 78 | static linearizeSystem(system, x0, u0) { 79 | const Jx = LinearSystem.linearize(x => system.dynamics(x, u0), x0); 80 | const Ju = LinearSystem.linearize(u => system.dynamics(x0, u), u0); 81 | return [Jx, Ju] 82 | } 83 | 84 | /** 85 | * Test jacobian functions 86 | */ 87 | static testJacobian(system) { 88 | const [xn, un] = system.shape 89 | const x0 = new eig.Matrix(xn, 1); 90 | for (let i = 0; i < xn; i++) { 91 | x0.set(i, i * 2.8 + 13.7); 92 | } 93 | const u0 = new eig.Matrix(un, 1); 94 | for (let i = 0; i < un; i++) { 95 | u0.set(i, i * 2.8 + 13.7); 96 | } 97 | const [Jxn, Jun] = LinearSystem.linearizeSystem(system, x0, u0) 98 | const Jx = system.xJacobian(x0, u0) 99 | const Ju = system.uJacobian(x0, u0) 100 | Jx.matSub(Jxn).print('Jx diff') 101 | Ju.matSub(Jun).print('Ju diff') 102 | } 103 | } 104 | 105 | export default LinearSystem -------------------------------------------------------------------------------- /src/components/models/model.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen' 2 | 3 | class Model { 4 | static STATE_BOUNDS = 1000 5 | 6 | constructor(states, commands, params) { 7 | // Populate properties 8 | this.states = states 9 | this.commands = commands 10 | this.shape = [this.states.length, this.commands.length] 11 | this.statesCommands = [...states, ...commands] 12 | this.params = params 13 | // Set state 14 | const x0 = params.x0 ? new eig.Matrix(params.x0) : this.trim().x; 15 | this.setState(x0) 16 | // Init graphics 17 | this.graphics = {} 18 | } 19 | 20 | delete() { 21 | eig.GC.popException(this.x); 22 | } 23 | 24 | trim() { 25 | return { x: new eig.Matrix(this.shape[0], 1), u: new eig.Matrix(this.shape[1], 1) } 26 | } 27 | 28 | /** 29 | * Set state 30 | * @param {Matrix} x 31 | */ 32 | setState(x) { 33 | eig.GC.set(this, 'x', x) 34 | } 35 | 36 | /** 37 | * Bound state x to appropriate domain 38 | * @param {Matrix} x 39 | */ 40 | bound(x) { 41 | x.clampSelf(-Model.STATE_BOUNDS, Model.STATE_BOUNDS) 42 | } 43 | 44 | /** 45 | * Returns dx/dt 46 | * @param {Matrix} x 47 | * @param {Matrix} u 48 | * @returns {Matrix} dx 49 | */ 50 | dynamics(x, u) { 51 | throw new Error('Must be overridden') 52 | } 53 | 54 | /** 55 | * Get next state from some state 56 | * @param {Matrix} x - State 57 | * @param {Matrix} u - Command 58 | * @param {Number} dt - Timestep 59 | * @param {Boolean} bound - Bound state to system domain 60 | */ 61 | xNext(x, u, dt, bound = true) { 62 | const dx = this.dynamics(x, u) 63 | const xNext = x.matAdd(dx.mul(dt)) 64 | if (bound) 65 | this.bound(xNext) 66 | return xNext 67 | } 68 | 69 | /** 70 | * Propagate state one timestep forward 71 | * @param {Matrix} u - Command 72 | * @param {Number} dt - Timestep 73 | */ 74 | step(u, dt) { 75 | const xNext = this.xNext(this.x, u, dt) 76 | this.setState(xNext) 77 | } 78 | 79 | /** 80 | * Reverse trajectory array 81 | * @param {Array} array 82 | */ 83 | reverse(array) { 84 | const backward = array.map(val => eig.Matrix(val)); 85 | backward.reverse(); 86 | const [xn, un] = this.shape 87 | // Shift backwards commands and tranform directions 88 | for (let k = 0; k < backward.length; k++) { 89 | if (k > 0) { 90 | const block = backward[k].block(xn, 0, un, 1); 91 | backward[k - 1].setBlock(xn, 0, block); 92 | } 93 | this.statesCommands.forEach((val, idx) => { 94 | if (val.derivative) backward[k].set(idx, -backward[k].get(idx)) 95 | }) 96 | } 97 | const forward = [...array] 98 | forward.splice(array.length - 1, 1) 99 | return [...forward, ...backward] 100 | } 101 | 102 | /** 103 | * Simulate 104 | */ 105 | simulate(controller, x0, dt, duration) { 106 | let x = new eig.Matrix(x0) 107 | const arr = [] 108 | for (let t = 0; t < duration; t += dt) { 109 | const u = controller.getCommand(x, t); 110 | const dx = this.dynamics(x, u); 111 | arr.push(x.vcat(u)) 112 | x.matAddSelf(dx.mul(dt)) 113 | } 114 | return arr 115 | } 116 | } 117 | 118 | export default Model -------------------------------------------------------------------------------- /src/components/models/quadrotor2D/Quadrotor2D.vue: -------------------------------------------------------------------------------- 1 | 22 | 23 | 93 | 94 | -------------------------------------------------------------------------------- /src/components/models/secondOrder/SecondOrder.vue: -------------------------------------------------------------------------------- 1 | 21 | 22 | 78 | 79 | -------------------------------------------------------------------------------- /src/components/models/secondOrder/secondOrder.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen'; 2 | import _ from 'lodash'; 3 | import colors from 'vuetify/lib/util/colors'; 4 | import Model from '@/components/models/model.js'; 5 | import { matFromDiag } from '@/components/math.js'; 6 | import { bounceTraj } from './trajectories.js'; 7 | import { createMarker, createTraj } from '../utils.js'; 8 | 9 | class SecondOrder extends Model { 10 | static NAME = 'second order'; 11 | static TAG = 'secondOrder'; 12 | static STATES = Object.freeze([ 13 | { name: 'x', show: true }, 14 | { name: 'xDot', show: true, derivative: true }, 15 | ]); 16 | static COMMANDS = Object.freeze([ 17 | { name: 'force' } 18 | ]); 19 | 20 | constructor(params = {}) { 21 | super(SecondOrder.STATES, SecondOrder.COMMANDS, { 22 | ...params, 23 | m: 1 24 | }); 25 | } 26 | 27 | /** 28 | * Bound state x to appropriate domain 29 | * @param {Matrix} x 30 | */ 31 | bound(x) { 32 | } 33 | 34 | /** 35 | * Returns dx/dt 36 | * @param {Matrix} x 37 | * @param {Matrix} u 38 | * @returns {Matrix} dx 39 | */ 40 | dynamics(x, u) { 41 | return new eig.Matrix([ 42 | x.get(1), 43 | u.get(0) / this.params.m 44 | ]); 45 | } 46 | 47 | /** 48 | * Returns df/dx 49 | * @param {Matrix} x 50 | * @param {Matrix} u 51 | * @returns {Matrix} df/dx 52 | */ 53 | xJacobian(x, u) { 54 | return new eig.Matrix([ 55 | [0, 1], [0, 0] 56 | ]); 57 | } 58 | 59 | /** 60 | * Returns df/du 61 | * @param {Matrix} x 62 | * @param {Matrix} u 63 | * @returns {Matrix} df/du 64 | */ 65 | uJacobian(x, u) { 66 | return new eig.Matrix([ 67 | [0], [1 / this.params.m] 68 | ]); 69 | } 70 | 71 | /** 72 | * Mouse step 73 | * @param {Number} dt 74 | * @param {Array} mouseTarget 75 | */ 76 | trackMouse(mouseTarget, dt) { 77 | const { u } = this.trim(); 78 | const dx = this.dynamics(this.x, u); 79 | const xVel = 10 * (mouseTarget[0] - this.x.get(0)); 80 | this.x.set(1, _.clamp(xVel, -15, 15)); 81 | dx.set(0, this.x.get(1)); 82 | // TODO: extract in schema 83 | const newX = this.x.matAdd(dx.mul(dt)); 84 | this.bound(newX); 85 | this.setState(newX); 86 | return { u }; 87 | } 88 | 89 | /** 90 | * Draw model 91 | */ 92 | createGraphics(two, scale) { 93 | const GEOM = { 94 | cartWidth: scale, 95 | cartHeight: scale / 2, 96 | mr: scale / 10 // Marker radius 97 | }; 98 | // Cart 99 | const cart = two.makeRectangle(0, 0, GEOM.cartWidth, GEOM.cartHeight); 100 | cart.fill = colors.teal.base; 101 | cart.linewidth = 2; 102 | 103 | // Forces 104 | const sides = [-GEOM.cartWidth / 2, GEOM.cartWidth / 2].map(x => { 105 | const fLine = two.makeLine(x, 0, x, 0); 106 | fLine.linewidth = 2; 107 | fLine.stroke = colors.red.base; 108 | const fHead = two.makePolygon(0, 0, 6, 3); 109 | fHead.rotation = (Math.PI / 2) * Math.sign(x); 110 | fHead.fill = colors.red.base; 111 | return { group: two.makeGroup(fLine, fHead), fLine, fHead }; 112 | }); 113 | 114 | this.graphics.showControl = true; 115 | this.graphics.setControl = u => { 116 | sides.forEach((side, idx) => { 117 | const uh = _.clamp(u.get(0) * 5, -100, 100); 118 | side.group.visible = this.graphics.showControl && 119 | (idx - 0.5) * uh > 0 && 120 | Math.abs(uh) > 0.1; 121 | side.fHead.translation.x = (Math.sign(uh) * GEOM.cartWidth) / 2 + uh; 122 | side.fLine.vertices[1].x = side.fHead.translation.x; 123 | }); 124 | }; 125 | this.graphics.cart = two.makeGroup( 126 | cart, 127 | sides[0].group, 128 | sides[1].group 129 | ); 130 | 131 | // Create marker 132 | const marker = createMarker(two, GEOM.mr, colors.green.darken4, 4); 133 | this.graphics.cart.add(marker); 134 | 135 | // Create traj 136 | this.graphics.traj = createTraj(two, this.mpcParams().nPts); 137 | } 138 | 139 | /** 140 | * Update model 141 | */ 142 | updateGraphics(worldToCanvas, params) { 143 | const { u } = params; 144 | const x = this.x; 145 | this.graphics.cart.translation.set(...worldToCanvas([x.get(0), 0])); 146 | if (u) 147 | this.graphics.setControl(u); 148 | this.graphics.cart.opacity = params.ghost ? 0.3 : 1; 149 | this.graphics.traj.update((pt) => { 150 | return worldToCanvas([pt.get(0), 0.1]); 151 | }); 152 | } 153 | 154 | /** 155 | * LQR Params 156 | */ 157 | lqrParams() { 158 | return { 159 | x0: [-2, 0], 160 | Q: matFromDiag([10, 1]), 161 | R: matFromDiag([1]), 162 | simEps: 1, 163 | simDuration: 4, 164 | disengage: false, 165 | divergenceThres: 1e10, 166 | }; 167 | } 168 | 169 | /** 170 | * Plugin params 171 | */ 172 | valueIterationParams() { 173 | return { 174 | x: { min: [-5, -2], max: [5, 2], nPts: [100, 100], targets: [[0, 0]] }, 175 | u: { min: [-2], max: [2], nPts: [2] }, 176 | dt: 0.05, 177 | x0: [-2, 0], 178 | dump: () => import('./viTable.json') 179 | }; 180 | } 181 | 182 | /** 183 | * Direct collocation params 184 | */ 185 | directCollocationParams() { 186 | return { 187 | nPts: 20, 188 | uBounds: { min: [-5], max: [5] }, 189 | anchors: [{ t: 0, x: [-2, 0] }, { t: 1, x: [2, 0] }], 190 | reverse: true, 191 | traj: bounceTraj 192 | }; 193 | } 194 | 195 | /** 196 | * MPC Params 197 | */ 198 | mpcParams() { 199 | return { 200 | nPts: 10, 201 | uBounds: { min: [-10], max: [10] }, 202 | dt: 1 / 10, 203 | traj: bounceTraj 204 | }; 205 | } 206 | 207 | /** 208 | * Kalman filter plugin parameters 209 | */ 210 | kalmanFilterParams() { 211 | function measurement(params, x) { 212 | const pos = new eig.Matrix(params.pos); 213 | const x0 = new eig.Matrix([x.get(0), 0]); 214 | const dist = pos.matSub(x0).norm(); 215 | return new eig.Matrix([dist]); 216 | } 217 | return { 218 | covariance: [[5, 0], [0, 5]], 219 | processNoise: [[0, 0], [0, 0]], 220 | inputNoise: [[0, 0], [0, 0]], 221 | sensors: [ 222 | { type: 'radar', dt: 1, pos: [-2, 2], measurement, noise: [[5]] } 223 | ] 224 | }; 225 | } 226 | } 227 | 228 | const traj = []; 229 | 230 | export { traj }; 231 | export default SecondOrder; -------------------------------------------------------------------------------- /src/components/models/secondOrder/trajectories.js: -------------------------------------------------------------------------------- 1 | 2 | const bounceTraj = { 3 | dt: 0.04472625790685786, 4 | x: [[-2.0000, 0.0000, 5.0000], 5 | [-1.9947, 0.2294, 5.0000], 6 | [-1.9790, 0.4587, 5.0000], 7 | [-1.9527, 0.6881, 5.0000], 8 | [-1.9158, 0.9175, 5.0000], 9 | [-1.8685, 1.1468, 5.0000], 10 | [-1.8106, 1.3762, 5.0000], 11 | [-1.7422, 1.6056, 5.0000], 12 | [-1.6633, 1.8349, 5.0000], 13 | [-1.5739, 2.0643, 5.0000], 14 | [-1.4739, 2.2937, 5.0000], 15 | [-1.3634, 2.5230, 5.0000], 16 | [-1.2424, 2.7524, 5.0000], 17 | [-1.1109, 2.9818, 5.0000], 18 | [-0.9689, 3.2111, 5.0000], 19 | [-0.8163, 3.4405, 5.0000], 20 | [-0.6532, 3.6698, 5.0000], 21 | [-0.4796, 3.8992, 5.0000], 22 | [-0.2955, 4.1286, 5.0000], 23 | [-0.1008, 4.3579, 5.0000], 24 | [0.1008, 4.3579, -5.0000], 25 | [0.2955, 4.1286, -5.0000], 26 | [0.4796, 3.8992, -5.0000], 27 | [0.6532, 3.6698, -5.0000], 28 | [0.8163, 3.4405, -5.0000], 29 | [0.9689, 3.2111, -5.0000], 30 | [1.1109, 2.9818, -5.0000], 31 | [1.2424, 2.7524, -5.0000], 32 | [1.3634, 2.5230, -5.0000], 33 | [1.4739, 2.2937, -5.0000], 34 | [1.5739, 2.0643, -5.0000], 35 | [1.6633, 1.8349, -5.0000], 36 | [1.7422, 1.6056, -5.0000], 37 | [1.8106, 1.3762, -5.0000], 38 | [1.8685, 1.1468, -5.0000], 39 | [1.9158, 0.9175, -5.0000], 40 | [1.9527, 0.6881, -5.0000], 41 | [1.9790, 0.4587, -5.0000], 42 | [1.9947, 0.2294, -5.0000], 43 | [2.0000, 0.0000, -5.0000], 44 | [1.9947, -0.2294, -5.0000], 45 | [1.9790, -0.4587, -5.0000], 46 | [1.9527, -0.6881, -5.0000], 47 | [1.9158, -0.9175, -5.0000], 48 | [1.8685, -1.1468, -5.0000], 49 | [1.8106, -1.3762, -5.0000], 50 | [1.7422, -1.6056, -5.0000], 51 | [1.6633, -1.8349, -5.0000], 52 | [1.5739, -2.0643, -5.0000], 53 | [1.4739, -2.2937, -5.0000], 54 | [1.3634, -2.5230, -5.0000], 55 | [1.2424, -2.7524, -5.0000], 56 | [1.1109, -2.9818, -5.0000], 57 | [0.9689, -3.2111, -5.0000], 58 | [0.8163, -3.4405, -5.0000], 59 | [0.6532, -3.6698, -5.0000], 60 | [0.4796, -3.8992, -5.0000], 61 | [0.2955, -4.1286, -5.0000], 62 | [0.1008, -4.3579, 5.0000], 63 | [-0.1008, -4.3579, 5.0000], 64 | [-0.2955, -4.1286, 5.0000], 65 | [-0.4796, -3.8992, 5.0000], 66 | [-0.6532, -3.6698, 5.0000], 67 | [-0.8163, -3.4405, 5.0000], 68 | [-0.9689, -3.2111, 5.0000], 69 | [-1.1109, -2.9818, 5.0000], 70 | [-1.2424, -2.7524, 5.0000], 71 | [-1.3634, -2.5230, 5.0000], 72 | [-1.4739, -2.2937, 5.0000], 73 | [-1.5739, -2.0643, 5.0000], 74 | [-1.6633, -1.8349, 5.0000], 75 | [-1.7422, -1.6056, 5.0000], 76 | [-1.8106, -1.3762, 5.0000], 77 | [-1.8685, -1.1468, 5.0000], 78 | [-1.9158, -0.9175, 5.0000], 79 | [-1.9527, -0.6881, 5.0000], 80 | [-1.9790, -0.4587, 5.0000], 81 | [-1.9947, -0.2294, 5.0000], 82 | [-2.0000, 0.0000, 5.0000]] 83 | } 84 | 85 | export { bounceTraj } -------------------------------------------------------------------------------- /src/components/models/simplePendulum/simplePendulum.js: -------------------------------------------------------------------------------- 1 | import eig from '@eigen'; 2 | import _ from 'lodash'; 3 | import { wrapAngle, sqr, matFromDiag } from '@/components/math.js'; 4 | import colors from 'vuetify/lib/util/colors'; 5 | import Model from '@/components/models/model.js'; 6 | import { swingup } from './trajectories'; 7 | import Arm from '../arm/arm.js'; 8 | import { createTraj } from '../utils.js'; 9 | 10 | class SimplePendulum extends Model { 11 | static NAME = 'simple pendulum'; 12 | static TAG = 'simplePendulum'; 13 | static STATES = Object.freeze([ 14 | { name: 'theta', show: true }, 15 | { name: 'thetaDot', show: true, derivative: true }, 16 | ]); 17 | static COMMANDS = Object.freeze([ 18 | { name: 'torque' } 19 | ]); 20 | 21 | constructor(params = {}) { 22 | super(SimplePendulum.STATES, SimplePendulum.COMMANDS, { 23 | ...params, 24 | g: 9.81, 25 | l: 1, 26 | m: 1, 27 | mu: 0.5, 28 | }); 29 | // const x = params.x0 || new eig.Matrix(2, 1); 30 | // eig.GC.set(this, 'x', x) 31 | } 32 | 33 | trim() { 34 | return { 35 | x: new eig.Matrix([Math.PI, 0]), 36 | u: new eig.Matrix([0]) 37 | }; 38 | } 39 | 40 | /** 41 | * Bound state x to appropriate domain 42 | * @param {Matrix} x 43 | */ 44 | bound(x) { 45 | super.bound(x); 46 | x.set(0, wrapAngle(x.get(0))); 47 | } 48 | 49 | /** 50 | * Returns dx/dt 51 | * @param {Matrix} x 52 | * @param {Matrix} u 53 | * @returns {Matrix} dx 54 | */ 55 | dynamics(x, u) { 56 | // x = [theta, thetaDot] 57 | const p = this.params; 58 | const dx = new eig.Matrix(2, 1); 59 | const s = Math.sin(x.get(0)); 60 | const ddx = (-p.m * p.g * p.l * s - p.mu * x.get(1) + u.get(0)) / (p.m * Math.pow(p.l, 2)); 61 | dx.set(0, x.get(1)); 62 | dx.set(1, ddx); 63 | return dx; 64 | } 65 | 66 | /** 67 | * Returns df/dx 68 | * @param {Matrix} x 69 | * @param {Matrix} u 70 | * @returns {Matrix} df/dx 71 | */ 72 | xJacobian(x, u) { 73 | const p = this.params; 74 | const c = Math.cos(x.get(0)); 75 | return new eig.Matrix([ 76 | [0, 1], 77 | [-p.g / p.l * c, - p.mu / p.m * p.l] 78 | ]); 79 | } 80 | 81 | /** 82 | * Returns df/du 83 | * @param {Matrix} x 84 | * @param {Matrix} u 85 | * @returns {Matrix} df/du 86 | */ 87 | uJacobian(x, u) { 88 | const p = this.params; 89 | return new eig.Matrix([ 90 | [0], [1 / p.m / sqr(p.l)] 91 | ]); 92 | } 93 | 94 | /** 95 | * Mouse step 96 | * @param {Array} mouseTarget 97 | * @param {Number} dt 98 | */ 99 | trackMouse(mouseTarget, dt) { 100 | const { u } = this.trim(); 101 | const dx = this.dynamics(this.x, u); 102 | const theta = Math.atan2(mouseTarget[1], mouseTarget[0]) + Math.PI / 2; 103 | this.x.set(1, 10 * wrapAngle(theta - this.x.get(0))); 104 | dx.set(0, this.x.get(1)); 105 | dx.set(1, 0); 106 | const newX = this.x.matAdd(dx.mul(dt)); 107 | this.bound(newX); 108 | this.setState(newX); 109 | return { u }; 110 | } 111 | 112 | /** 113 | * Draw model 114 | */ 115 | createGraphics(two, scale) { 116 | const GEOM = { 117 | length: scale, 118 | thickness: 8, 119 | radius: 16, 120 | }; 121 | this.graphics.arm = Arm.createArm(two, GEOM, colors.green.lighten2, colors.green.darken4, 1); 122 | 123 | // Create traj 124 | this.graphics.scale = scale; 125 | this.graphics.traj = createTraj(two, this.mpcParams().nPts); 126 | } 127 | 128 | /** 129 | * Update model 130 | */ 131 | updateGraphics(worldToCanvas, params) { 132 | // TODO: draw torque 133 | const { u, ghost } = params; 134 | Arm.updateArm(this.graphics.arm, worldToCanvas, this.x, u); 135 | this.graphics.arm[0].opacity = ghost ? 0.3 : 1; 136 | const center = worldToCanvas([0, 0]); 137 | this.graphics.traj.update((pt) => { 138 | return [ 139 | center[0] + this.graphics.scale * Math.sin(pt.get(0)), 140 | center[1] + this.graphics.scale * Math.cos(pt.get(0)) 141 | ]; 142 | }); 143 | } 144 | 145 | /** 146 | * LQR Params 147 | */ 148 | lqrParams() { 149 | return { 150 | Q: matFromDiag([10, 1]), 151 | R: matFromDiag([1]), 152 | simEps: 1e-1, 153 | simDuration: 3, 154 | disengage: false, 155 | divergenceThres: 500, 156 | }; 157 | } 158 | 159 | /** 160 | * Plugin params 161 | */ 162 | valueIterationParams() { 163 | const maxThetaDot = 2 * Math.sqrt(this.params.g / this.params.l); 164 | return { 165 | x: { 166 | min: [-Math.PI, -maxThetaDot], 167 | max: [Math.PI, maxThetaDot], 168 | nPts: [100, 100], 169 | targets: [[Math.PI, 0], [-Math.PI, 0]] 170 | }, 171 | u: { min: [-2], max: [2], nPts: [2] }, 172 | dt: 0.06, 173 | x0: [0, 0], 174 | dump: () => import('./viTable.json') 175 | }; 176 | } 177 | 178 | /** 179 | * Direct collocation params 180 | */ 181 | directCollocationParams() { 182 | return { 183 | nPts: 30, 184 | uBounds: { min: [-5], max: [5] }, 185 | anchors: [{ t: 0, x: [0, 0] }, { t: 1, x: [3.14, 0] }], 186 | holdTime: 1, 187 | reverse: true, 188 | traj: swingup 189 | }; 190 | } 191 | 192 | /** 193 | * MPC Params 194 | */ 195 | mpcParams() { 196 | return { 197 | nPts: 10, 198 | uBounds: { min: [-5], max: [5] }, 199 | dt: 1 / 10, 200 | traj: swingup 201 | }; 202 | } 203 | } 204 | 205 | export default SimplePendulum; -------------------------------------------------------------------------------- /src/components/models/simplePendulum/trajectories.js: -------------------------------------------------------------------------------- 1 | const swingup = { 2 | dt: 0.10915679928353673, 3 | x: [[0.0000, 0.0000, 0.0000], 4 | [0.0000, 0.0000, 0.0000], 5 | [0.0000, 0.0000, 0.0000], 6 | [0.0000, 0.0000, 0.0000], 7 | [0.0000, 0.0000, 0.0000], 8 | [0.0000, 0.0000, 0.0000], 9 | [0.0000, 0.0000, 0.0000], 10 | [0.0000, 0.0000, 0.0000], 11 | [0.0000, 0.0000, 0.0000], 12 | [0.0000, 0.0000, 0.0000], 13 | [0.0000, 0.0000, 5.0000], 14 | [0.0310, 0.5376, 5.0000], 15 | [0.1178, 0.9810, 5.0000], 16 | [0.2472, 1.2832, 5.0000], 17 | [0.4014, 1.4195, 5.0000], 18 | [0.5407, 0.8422, -5.0000], 19 | [0.5692, -0.3306, -5.0000], 20 | [0.4697, -1.4049, -5.0000], 21 | [0.2599, -2.2674, -5.0000], 22 | [-0.0306, -2.8188, -5.0000], 23 | [-0.3628, -3.0037, -5.0000], 24 | [-0.6958, -2.8435, -5.0000], 25 | [-0.9954, -2.4296, -5.0000], 26 | [-1.2393, -1.8763, -5.0000], 27 | [-1.4126, -1.1475, -2.6690], 28 | [-1.4783, 0.1174, 5.0000], 29 | [-1.3732, 1.7272, 5.0000], 30 | [-1.0933, 3.1987, 5.0000], 31 | [-0.6608, 4.4000, 5.0000], 32 | [-0.1180, 5.1155, 5.0000], 33 | [0.4702, 5.1955, 5.0000], 34 | [1.0344, 4.7275, 5.0000], 35 | [1.5278, 3.9923, 5.0000], 36 | [1.9367, 3.2680, 5.0000], 37 | [2.2726, 2.7184, 5.0000], 38 | [2.5597, 2.4095, 5.0000], 39 | [2.8132, 2.0054, -1.3786], 40 | [3.0015, 1.3015, -5.0000], 41 | [3.1071, 0.5949, -5.0000], 42 | [3.1400, 0.0000, -5.0000], 43 | [3.1400, 0.0000, 0.0000], 44 | [3.1400, 0.0000, 0.0000], 45 | [3.1400, 0.0000, 0.0000], 46 | [3.1400, 0.0000, 0.0000], 47 | [3.1400, 0.0000, 0.0000], 48 | [3.1400, 0.0000, 0.0000], 49 | [3.1400, 0.0000, 0.0000], 50 | [3.1400, 0.0000, 0.0000], 51 | [3.1400, 0.0000, 0.0000], 52 | [3.1400, 0.0000, 0.0000], 53 | [3.1400, 0.0000, 0.0000], 54 | [3.1400, 0.0000, 0.0000], 55 | [3.1400, 0.0000, 0.0000], 56 | [3.1400, 0.0000, 0.0000], 57 | [3.1400, 0.0000, 0.0000], 58 | [3.1400, 0.0000, 0.0000], 59 | [3.1400, 0.0000, 0.0000], 60 | [3.1400, 0.0000, 0.0000], 61 | [3.1400, 0.0000, -5.0000], 62 | [3.1400, 0.0000, -5.0000], 63 | [3.1071, -0.5949, -5.0000], 64 | [3.0015, -1.3015, -1.3786], 65 | [2.8132, -2.0054, 5.0000], 66 | [2.5597, -2.4095, 5.0000], 67 | [2.2726, -2.7184, 5.0000], 68 | [1.9367, -3.2680, 5.0000], 69 | [1.5278, -3.9923, 5.0000], 70 | [1.0344, -4.7275, 5.0000], 71 | [0.4702, -5.1955, 5.0000], 72 | [-0.1180, -5.1155, 5.0000], 73 | [-0.6608, -4.4000, 5.0000], 74 | [-1.0933, -3.1987, 5.0000], 75 | [-1.3732, -1.7272, 5.0000], 76 | [-1.4783, -0.1174, -2.6690], 77 | [-1.4126, 1.1475, -5.0000], 78 | [-1.2393, 1.8763, -5.0000], 79 | [-0.9954, 2.4296, -5.0000], 80 | [-0.6958, 2.8435, -5.0000], 81 | [-0.3628, 3.0037, -5.0000], 82 | [-0.0306, 2.8188, -5.0000], 83 | [0.2599, 2.2674, -5.0000], 84 | [0.4697, 1.4049, -5.0000], 85 | [0.5692, 0.3306, -5.0000], 86 | [0.5407, -0.8422, 5.0000], 87 | [0.4014, -1.4195, 5.0000], 88 | [0.2472, -1.2832, 5.0000], 89 | [0.1178, -0.9810, 5.0000], 90 | [0.0310, -0.5376, 5.0000], 91 | [0.0000, 0.0000, 0.0000], 92 | [0.0000, 0.0000, 0.0000], 93 | [0.0000, 0.0000, 0.0000], 94 | [0.0000, 0.0000, 0.0000], 95 | [0.0000, 0.0000, 0.0000], 96 | [0.0000, 0.0000, 0.0000], 97 | [0.0000, 0.0000, 0.0000], 98 | [0.0000, 0.0000, 0.0000], 99 | [0.0000, 0.0000, 0.0000], 100 | [0.0000, 0.0000, 0.0000], 101 | [0.0000, 0.0000, 0.0000]] 102 | }; 103 | 104 | const trim = { 105 | dt: 0.10915679928353673, 106 | x: [[3.1400, 0.0000, 0.0000], 107 | [3.1400, 0.0000, 0.0000]] 108 | }; 109 | 110 | export { trim, swingup }; -------------------------------------------------------------------------------- /src/components/models/systems.js: -------------------------------------------------------------------------------- 1 | import SecondOrder from "@/components/models/secondOrder/secondOrder.js"; 2 | import SimplePendulum from "@/components/models/simplePendulum/simplePendulum.js"; 3 | import DoublePendulum from "@/components/models/doublePendulum/doublePendulum.js"; 4 | import Quadrotor2D from "@/components/models/quadrotor2D/quadrotor2D.js"; 5 | import CartPole from "@/components/models/cartPole/cartPole.js"; 6 | import Arm from "@/components/models/arm/arm.js"; 7 | import Car from "@/components/models/car/car.js"; 8 | 9 | 10 | const systems = { 11 | [SecondOrder.TAG]: SecondOrder, 12 | [SimplePendulum.TAG]: SimplePendulum, 13 | [DoublePendulum.TAG]: DoublePendulum, 14 | [Quadrotor2D.TAG]: Quadrotor2D, 15 | [CartPole.TAG]: CartPole, 16 | [Arm.TAG]: Arm, 17 | [Car.TAG]: Car, 18 | }; 19 | 20 | export default systems -------------------------------------------------------------------------------- /src/components/models/utils.js: -------------------------------------------------------------------------------- 1 | const COLORS = { 2 | }; 3 | 4 | function createMarker(two, radius, color, stroke = 0) { 5 | const marker = two.makeGroup(); 6 | if (stroke > 0) { 7 | const circle = two.makeCircle(0, 0, radius); 8 | circle.stroke = color; 9 | circle.linewidth = stroke; 10 | marker.add(circle); 11 | } 12 | for (let k = 0; k < 4; k++) { 13 | const [sa, ea] = [k * Math.PI / 2, (k + 1) * Math.PI / 2]; 14 | const segment = two.makeArcSegment(0, 0, 0, radius, sa, ea); 15 | segment.fill = k % 2 === 0 ? '#ffffff' : color; 16 | segment.noStroke(); 17 | // segment.linewidth = 3; 18 | // segment.stroke = color; 19 | marker.add(segment); 20 | } 21 | return marker; 22 | } 23 | 24 | function createCircularForce(two, radius, color) { 25 | const theta = -5 * Math.PI / 4; 26 | const [sa, ea] = [Math.PI / 4, theta]; 27 | const r = radius; 28 | const fArc = two.makeArcSegment(0, 0, r, r, sa, ea); 29 | fArc.stroke = color; 30 | fArc.linewidth = 2; 31 | fArc.noFill(); 32 | const fHead = two.makePolygon(0, 0, 6, 3); 33 | fHead.fill = color; 34 | const force = two.makeGroup(fArc, fHead); 35 | const setControl = u => { 36 | force.visible = !!u; 37 | if (!u) return; 38 | const opacity = Math.min(Math.abs(u) / 10, 1); 39 | fArc.opacity = opacity; 40 | fHead.opacity = opacity; 41 | const headAngle = u < 0 ? sa : ea; 42 | fHead.rotation = headAngle + (u < 0 ? Math.PI : 0); 43 | fHead.translation.set( 44 | r * Math.cos(headAngle), 45 | r * Math.sin(headAngle) 46 | ); 47 | }; 48 | setControl(0); 49 | return { force, setControl }; 50 | } 51 | 52 | function createTraj(two, nPts) { 53 | const traj = {}; 54 | traj.data = []; 55 | traj.lines = [...Array(nPts)].map(() => { 56 | const line = two.makeLine(0, 0, 0, 0); 57 | return line; 58 | }); 59 | traj.update = (transform) => { 60 | traj.data.forEach((x, idx) => { 61 | const xy = transform(x); 62 | traj.lines[idx].vertices[0].x = xy[0]; 63 | traj.lines[idx].vertices[0].y = xy[1]; 64 | traj.lines[idx].vertices[1].x = xy[0]; 65 | traj.lines[idx].vertices[1].y = xy[1]; 66 | if (idx > 0) { 67 | traj.lines[idx - 1].vertices[1].x = xy[0]; 68 | traj.lines[idx - 1].vertices[1].y = xy[1]; 69 | } 70 | }); 71 | }; 72 | return traj; 73 | } 74 | 75 | export { createMarker, createCircularForce, createTraj, COLORS }; -------------------------------------------------------------------------------- /src/components/nav/Drawer.vue: -------------------------------------------------------------------------------- 1 | 34 | 35 | 99 | 100 | -------------------------------------------------------------------------------- /src/components/nav/Toolbar.vue: -------------------------------------------------------------------------------- 1 | 28 | 29 | -------------------------------------------------------------------------------- /src/components/planners/interactivePath.js: -------------------------------------------------------------------------------- 1 | import Two from "two.js"; 2 | import _ from 'lodash' 3 | import Vue from 'vue' 4 | import colors from 'vuetify/lib/util/colors' 5 | import eig from "@eigen" 6 | import { setDraggable } from '@/components/twoUtils.js' 7 | 8 | // TODO: allow for open path (and non differentiable ?) 9 | class InteractivePath { 10 | constructor(two, worldToCanvas, canvasToWorld) { 11 | this.worldToCanvas = worldToCanvas 12 | this.canvasToWorld = canvasToWorld 13 | this.updateListeners = [] 14 | 15 | const length = 300 16 | const vHandle = 100 17 | const dHandle = 140 18 | 19 | this.path = new Two.Path([ 20 | // new Two.Anchor(0, 0), 21 | new Two.Anchor(-length, 0, 0, vHandle, 0, -vHandle, Two.Commands.curve), 22 | new Two.Anchor(0, 0, -dHandle, -dHandle, dHandle, dHandle, Two.Commands.curve), 23 | new Two.Anchor(length, 0, 0, vHandle, 0, -vHandle, Two.Commands.curve), 24 | new Two.Anchor(0, 0, dHandle, -dHandle, -dHandle, dHandle, Two.Commands.curve), 25 | new Two.Anchor(-length, 0, 0, vHandle, 0, -vHandle, Two.Commands.curve) 26 | ], false) 27 | this.path.automatic = false; 28 | this.path.noFill() 29 | this.path.stroke = colors.purple.base; 30 | this.path.linewidth = 3 31 | this.group = two.makeGroup(this.path) 32 | 33 | const vertices = this.path.vertices 34 | for (let k = 1; k < vertices.length; k++) { 35 | const anchor = vertices[k] 36 | 37 | const radius = 20; 38 | const editColor = colors.red.base; 39 | 40 | 41 | const handle = two.makeCircle(0, 0, radius / 4); 42 | const l = two.makeCircle(0, 0, radius / 4); 43 | // var r = two.makeCircle(0, 0, radius / 4); 44 | 45 | handle.translation.copy(anchor); 46 | l.translation.copy(anchor.controls.left).addSelf(anchor); 47 | // r.translation.copy(anchor.controls.right).addSelf(anchor); 48 | handle.noStroke().fill = l.noStroke().fill = editColor; 49 | // r.noStroke().fill 50 | 51 | const ll = new Two.Path([ 52 | new Two.Anchor().copy(handle.translation), 53 | new Two.Anchor().copy(l.translation) 54 | ]); 55 | // var rl = new Two.Path([ 56 | // new Two.Anchor().copy(handle.translation), 57 | // new Two.Anchor().copy(r.translation) 58 | // ]); 59 | ll.noFill().stroke = editColor; 60 | // rl.noFill().stroke = editColor; 61 | 62 | this.group.add(ll, handle, l); 63 | 64 | const _this = this 65 | handle.translation.bind(Two.Events.change, function () { 66 | anchor.copy(this); 67 | l.translation.copy(anchor.controls.left).addSelf(this); 68 | ll.vertices[0].copy(this); 69 | ll.vertices[1].copy(l.translation); 70 | if (k === vertices.length - 1) { 71 | vertices[0].copy(this) 72 | } 73 | _this.onUpdate() 74 | // r.translation.copy(anchor.controls.right).addSelf(this); 75 | // rl.vertices[0].copy(this); 76 | // rl.vertices[1].copy(r.translation); 77 | }); 78 | l.translation.bind(Two.Events.change, function () { 79 | anchor.controls.left.copy(this).subSelf(anchor); 80 | let controls = k === vertices.length - 1 ? vertices[0].controls.right : anchor.controls.right; 81 | controls.copy(this).subSelf(anchor).multiplyScalar(-1); 82 | ll.vertices[1].copy(this); 83 | _this.onUpdate() 84 | }); 85 | // r.translation.bind(Two.Events.change, function () { 86 | // anchor.controls.right.copy(this).subSelf(anchor); 87 | // anchor.controls.left.copy(this).subSelf(anchor).multiplyScalar(-1); 88 | // rl.vertices[1].copy(this); 89 | // }); 90 | 91 | // Add Interactivity 92 | Vue.nextTick(() => { 93 | setDraggable(handle); 94 | setDraggable(l); 95 | }) 96 | // setDraggable(r); 97 | } 98 | 99 | // Center path 100 | const [cx, cy] = worldToCanvas([0, 0]) 101 | this.group.translation.set(cx, cy); 102 | } 103 | 104 | addUpdateListener(listener) { 105 | this.updateListeners.push(listener) 106 | } 107 | 108 | onUpdate() { 109 | this.updateListeners.forEach(l => l()) 110 | } 111 | 112 | discretize(nPts) { 113 | const traj = [] 114 | for (let k = 0; k < nPts; k++) { 115 | const t = k / nPts; 116 | traj.push(this.path.getPointAt(t)) 117 | } 118 | return traj 119 | } 120 | 121 | getTraj(nPts) { 122 | const [cx, cy] = this.worldToCanvas([0, 0]) 123 | return this.discretize(nPts).map(val => { 124 | return new eig.Matrix(this.canvasToWorld([val.x + cx, val.y + cy])); 125 | }); 126 | } 127 | } 128 | 129 | export default InteractivePath -------------------------------------------------------------------------------- /src/components/planners/rrt.js: -------------------------------------------------------------------------------- 1 | import _ from 'lodash' 2 | import eig from '@eigen' 3 | 4 | class RRT { 5 | constructor(system) { 6 | this.system = system; 7 | this.params = system.rrtParams(); 8 | this.watchers = new Set(); 9 | // Initialize graph 10 | this.clear(); 11 | } 12 | 13 | /** 14 | * Add update callback 15 | */ 16 | addWatcher(fun) { 17 | this.watchers.add(fun) 18 | } 19 | 20 | /** 21 | * Add update callback 22 | */ 23 | removeWatcher(fun) { 24 | this.watchers.delete(fun) 25 | } 26 | 27 | sample() { 28 | const n = this.system.shape[0] / 2; 29 | const x = eig.Matrix.random(n, 1); 30 | const xMin = this.params.xMin; 31 | const xMax = this.params.xMax; 32 | for (let k = 0; k < n; k++) 33 | x.set(k, xMin[k] + (x.get(k) + 1) / 2 * (xMax[k] - xMin[k])); 34 | return x; 35 | } 36 | 37 | step(x1, x2) { 38 | let dx = x2.matSub(x1); 39 | const x22 = new eig.Matrix(x2); 40 | for (let k = 0; k < dx.length(); k++) { 41 | if (!this.params.wrap[k]) continue; 42 | const rng = this.params.xMax[k] - this.params.xMin[k]; 43 | const adx = Math.abs(dx.get(k)); 44 | if (adx > rng / 2) 45 | x22.set(k, x22.get(k) - Math.sign(dx.get(k)) * rng); 46 | } 47 | dx = x22.matSub(x1); 48 | const dxn = dx.norm(); 49 | const xNext = eig.Matrix(x1); 50 | for (let k = 0; k < dx.length(); k++) { 51 | const eps = Math.min(this.params.deltas[k], dxn); 52 | xNext.set(k, xNext.get(k) + dx.get(k) / dxn * eps); 53 | } 54 | this.system.bound(xNext); 55 | 56 | 57 | // Handle wrapping 58 | // const dx = x2.matSub(x1); 59 | // const x2n = x2.matAdd(dx); 60 | // this.system.bound(x2n); 61 | // x2n.matSubSelf(x1); 62 | // const x1next = new eig.Matrix(x1); 63 | // for (let k = 0; k < dx.length(); k++) { 64 | // const sign = Math.abs(x2n.get(k)) < Math.abs(dx.get(k)) ? 1 : -1; 65 | // const eps = this.params.deltas[k]; 66 | // x1next.set(k, x1next.get(k) + sign * eps); 67 | // } 68 | return xNext; 69 | } 70 | 71 | dist(x1, x2) { 72 | // Norm in wrapped space 73 | const dx = x2.matSub(x1); 74 | for (let k = 0; k < dx.length(); k++) { 75 | if (!this.params.wrap[k]) continue; 76 | const rng = this.params.xMax[k] - this.params.xMin[k]; 77 | const adx = Math.abs(dx.get(k)); 78 | if (adx > rng / 2) 79 | dx.set(k, rng - adx); 80 | } 81 | return dx.norm(); 82 | } 83 | 84 | findClosestNode(x) { 85 | let minDist = Infinity; 86 | let minNode = null; 87 | this.nodes.forEach(node => { 88 | const dist = this.dist(x, node.x); 89 | if (dist < minDist) { 90 | minNode = node; 91 | minDist = dist; 92 | } 93 | }); 94 | return minNode; 95 | } 96 | 97 | append(x, node = null) { 98 | // Optional node 99 | const newNode = { 100 | next: [] 101 | }; 102 | eig.GC.set(newNode, 'x', x); 103 | if (node) 104 | node.next.push(newNode); 105 | this.nodes.push(newNode); 106 | } 107 | 108 | clear() { 109 | eig.GC.popException(this.nodes); 110 | this.nodes = []; 111 | this.append(this.system.x.block(0, 0, this.params.n, 1)) 112 | } 113 | 114 | extend(x) { 115 | const node = this.findClosestNode(x); 116 | if (node) { 117 | // Take a step in x direction 118 | const xNext = this.step(node.x, x); 119 | this.append(xNext, node); 120 | } else { 121 | // Only append x for now 122 | this.append(x); 123 | } 124 | } 125 | 126 | run(validFun, iterations) { 127 | // Clear graph 128 | this.clear(); 129 | for (let k = 0; k < iterations; k++) { 130 | const x = this.sample(); 131 | if (!validFun(x)) 132 | continue; 133 | this.extend(x) 134 | } 135 | console.log('nodes', this.nodes) 136 | this.watchers.forEach(fun => fun()); 137 | } 138 | } 139 | 140 | export default RRT -------------------------------------------------------------------------------- /src/components/planners/trajectory.js: -------------------------------------------------------------------------------- 1 | import _ from 'lodash'; 2 | import eig from "@eigen"; 3 | 4 | class Trajectory { 5 | /** 6 | * Constructor 7 | * @param {Bool} loop whether the time wraps around 8 | */ 9 | constructor(system, loop = true) { 10 | this.array = []; 11 | this.loop = loop; 12 | this.watchers = new Set(); 13 | this.system = system; 14 | } 15 | 16 | getLegend() { 17 | return this.system.statesCommands; 18 | } 19 | 20 | /** 21 | * Check if the interoplator is ready 22 | */ 23 | ready() { 24 | return this.array.length > 0; 25 | } 26 | 27 | /** 28 | * Return trajectory dimension 29 | */ 30 | dim() { 31 | return !this.ready() ? 0 : this.array[0].rows(); 32 | } 33 | 34 | /** 35 | * Reset starting time 36 | */ 37 | reset(t) { 38 | this.tStart = t; // Date.now() / 1000 39 | } 40 | 41 | /** 42 | * Clear traj 43 | */ 44 | clear() { 45 | this.array = []; 46 | } 47 | 48 | /** 49 | * Add update callback 50 | */ 51 | addWatcher(fun) { 52 | this.watchers.add(fun); 53 | } 54 | 55 | /** 56 | * Add update callback 57 | */ 58 | removeWatcher(fun) { 59 | this.watchers.delete(fun); 60 | } 61 | 62 | /** 63 | * Create a trajectory based on an array of matrices 64 | * @param {Array} array of matrices 65 | * @param {Number} dt 66 | */ 67 | set(array, dt) { 68 | eig.GC.popException(this.array); 69 | console.assert(dt > 0, 'The time must be positive'); 70 | console.assert(array.length > 0, 'The array must have at least one element'); 71 | // Control array content 72 | const rows = this.system.shape[0] + this.system.shape[1]; 73 | array.forEach(val => { 74 | console.assert(val.rows() === rows && val.cols() === 1, 75 | `The values must be of shape [x; u]; expected ${[rows, 1]}, got ${[val.rows(), val.cols()]}`); 76 | }); 77 | eig.GC.set(this, 'array', array); 78 | this.dt = dt; 79 | this.tStart = 0; //Date.now() / 1000 80 | this.duration = (this.array.length - 1) * this.dt; 81 | this.watchers.forEach(fun => fun()); 82 | } 83 | 84 | /** 85 | * Get interpolated value 86 | * @param {Number} t time of interpolation 87 | */ 88 | get(t) { 89 | t -= this.tStart; 90 | // t -= Math.floor(t / this.duration) * this.duration 91 | let idx = Math.max(0, Math.floor(t / this.dt)); 92 | idx = this.loop ? idx % this.array.length : Math.min(this.array.length - 1, idx); 93 | var nextIdx = (idx + 1) % this.array.length; 94 | if (!this.loop && nextIdx < idx) nextIdx = idx; 95 | let ratio = (t - idx * this.dt) / this.dt; 96 | ratio -= Math.floor(ratio); 97 | return this.array.length < 2 ? 98 | this.array[0] : 99 | this.array[idx].mul(1 - ratio).matAdd(this.array[nextIdx].mul(ratio)); 100 | } 101 | 102 | /** 103 | * Get interpolated state 104 | * @param {Number} t 105 | */ 106 | getState(t) { 107 | return this.get(t).block(0, 0, this.system.shape[0], 1); 108 | } 109 | 110 | /** 111 | * Get interpolated command 112 | * @param {Number} t 113 | */ 114 | getCommand(t) { 115 | return this.get(t).block(this.system.shape[0], 0, this.system.shape[1], 1); 116 | } 117 | 118 | /** 119 | * Delete trajectory 120 | */ 121 | delete() { 122 | eig.GC.popException(this.array); 123 | this.array = null; 124 | } 125 | 126 | /** 127 | * Load trajectory from a dump 128 | * @param {Object} dump 129 | */ 130 | load(dump) { 131 | this.set(dump.x.map(x => new eig.Matrix(x)), dump.dt); 132 | } 133 | 134 | /** 135 | * Get a string version of the trajectory 136 | */ 137 | dump() { 138 | let rows = `dt: ${this.dt},\n` + 'x: ['; 139 | this.array.forEach((vec, idx) => { 140 | rows += '['; 141 | for (let k = 0; k < vec.length(); k += 1) { 142 | rows += `${vec.get(k).toFixed(4)}` + (k < vec.length() - 1 ? ',' : ''); 143 | } 144 | rows += ']' + (idx === this.array.length - 1 ? ']' : ',\n'); 145 | }); 146 | return rows; 147 | } 148 | } 149 | 150 | /** 151 | * Test // TODO: use a test framework TODO: refactor 152 | */ 153 | function testTrajectory() { 154 | const array = [1, 2, 3, 7]; 155 | const ip = new Trajectory(array, 0.5); 156 | let val = [ip.get(0), ip.get(1), ip.get(1.5), ip.get(0.25)]; 157 | let expected = [1, 3, 7, 1.5]; 158 | console.assert(_.isEqual(val, expected), `Expected ${expected}, got ${val}`); 159 | console.log('Test successful'); 160 | } 161 | 162 | export default Trajectory; -------------------------------------------------------------------------------- /src/components/planners/utils.js: -------------------------------------------------------------------------------- 1 | import _ from 'lodash'; 2 | import eig from '@eigen'; 3 | 4 | class Tensor { 5 | /** 6 | * Create a tensor 7 | * @param {Array} dims 8 | */ 9 | constructor(dims) { 10 | this.dims = dims; 11 | this.length = _.reduce(dims, (p, n) => p * n, 1); 12 | this.data = [...Array(this.length)].fill(0); 13 | } 14 | 15 | /** 16 | * [indices] -> k 17 | * @param {Array} indices 18 | */ 19 | pack(indices) { 20 | let k = 0; 21 | this.dims.forEach((dim, idx) => { 22 | k = k * dim + indices[idx]; 23 | }); 24 | return k; 25 | } 26 | 27 | /** 28 | * k -> [indices] 29 | * @param {Number} k 30 | */ 31 | unpack(k) { 32 | let prod = this.length; 33 | return this.dims.map(dim => { 34 | prod /= dim; 35 | const newK = k % prod; 36 | const idx = (k - newK) / prod; 37 | k = newK; 38 | return idx; 39 | }); 40 | } 41 | 42 | /** 43 | * Get Value at vector 44 | * @param {Array} indices 45 | */ 46 | get(indices) { 47 | return this.data[this.pack(indices)]; 48 | } 49 | 50 | /** 51 | * Set Value at vector 52 | * @param {Array} indices 53 | * @param {Number} val 54 | */ 55 | set(indices, val) { 56 | this.data[this.pack(indices)] = val; 57 | } 58 | 59 | /** 60 | * Initialize with value 61 | * @param {Number} val 62 | */ 63 | clear(val) { 64 | for (let k = 0; k < this.data.length; k++) 65 | this.data[k] = val; 66 | } 67 | 68 | /** 69 | * Iterator 70 | */ 71 | forEach(fun) { 72 | for (let k = 0; k < this.length; k++) { 73 | const ind = this.unpack(k); 74 | fun(k, ind); 75 | } 76 | } 77 | 78 | /** 79 | * Print tensor 80 | * @param {String} title 81 | */ 82 | print() { 83 | console.log(this.data); 84 | } 85 | 86 | /** 87 | * Get matrix form 88 | */ 89 | getMatrix() { 90 | console.assert(this.dims.length === 2, `The tensor dimension (${this.dims}) must be 2`); 91 | return [...Array(this.dims[1])].map((val, j) => { 92 | return [...Array(this.dims[0])].map((val, i) => this.get([i, j])); 93 | }); 94 | } 95 | } 96 | 97 | /** 98 | * Test // TODO: use a test framework 99 | */ 100 | function testTensor() { 101 | const t = new Tensor([2, 3, 4]); 102 | [...Array(2)].map((_, i) => i).forEach(i => { 103 | [...Array(3)].map((_, i) => i).forEach(j => { 104 | [...Array(4)].map((_, i) => i).forEach(k => { 105 | const indices = [i, j, k]; 106 | const val = i + j + k; 107 | const flat = t.pack(indices); 108 | const expanded = t.unpack(flat); 109 | t.set(indices, val); 110 | console.assert(_.isEqual(expanded, indices), 'Index packing, expected %s, got %s', indices, expanded); 111 | console.assert(t.get(indices) === val, 'Value error, expected %d, got %d', val, t.get(indices)); 112 | }); 113 | }); 114 | }); 115 | console.log('Test successful'); 116 | } 117 | 118 | class Grid { 119 | /** 120 | * 121 | * @param {Array} grid [{min, max, nPts}, ...] spec for each dimension 122 | */ 123 | constructor(grid) { 124 | this.grid = []; 125 | for (let k = 0; k < grid.min.length; k++) 126 | this.grid.push({ min: grid.min[k], max: grid.max[k], nPts: grid.nPts[k] }); 127 | this.tensor = new Tensor(this.grid.map(val => val.nPts)); 128 | } 129 | 130 | /** 131 | * Clamp vector to bounds 132 | * @param {Matrix} vec 133 | */ 134 | clamp(vec) { 135 | const clamped = new eig.Matrix(vec); 136 | this.grid.forEach((val, idx) => { 137 | const v = Math.max(val.min, Math.min(val.max, clamped.get(idx))); 138 | clamped.set(idx, v); 139 | }); 140 | return clamped; 141 | } 142 | 143 | /** 144 | * Snap vector value to tensor index 145 | * @param {Matrix} vec 146 | */ 147 | pack(vec) { 148 | const ind = this.toGrid(vec); 149 | return ind ? this.tensor.pack(ind) : null; 150 | } 151 | 152 | /** 153 | * Unpack tensor idx 154 | * @param {Number} idx 155 | */ 156 | unpack(idx) { 157 | const ind = this.tensor.unpack(idx); 158 | return this.fromGrid(ind); 159 | } 160 | 161 | /** 162 | * Snap vector value to grid indices 163 | * @param {Matrix} vec 164 | */ 165 | toGrid(vec) { 166 | let oob = false; 167 | const ind = this.grid.map((val, idx) => { 168 | const scalar = (vec.get(idx) - val.min) / (val.max - val.min); 169 | const k = Math.floor(scalar * (val.nPts - 1)); 170 | oob |= k < 0 || k >= val.nPts; 171 | return Math.max(0, Math.min(val.nPts - 1, k)); 172 | }); 173 | return oob ? null : ind; 174 | } 175 | 176 | /** 177 | * Get cell mean position 178 | * @param {Array} indices 179 | */ 180 | fromGrid(indices) { 181 | const vec = this.grid.map((val, idx) => { 182 | const interval = (val.max - val.min) / (val.nPts - 1); 183 | const factor = Math.max(0, Math.min(val.nPts - 1, indices[idx])); 184 | return val.min + interval * factor; 185 | }); 186 | return new eig.Matrix(vec); 187 | } 188 | 189 | /** 190 | * Get tensor value at 191 | * @param {Matrix} vec 192 | */ 193 | get(vec) { 194 | const ind = this.toGrid(vec); 195 | return this.tensor.get(ind); 196 | } 197 | 198 | /** 199 | * Set tensor value at 200 | */ 201 | set(vec, val) { 202 | const ind = this.toGrid(vec); 203 | return this.tensor.set(ind, val); 204 | } 205 | 206 | /** 207 | * Iterator 208 | * @param {Function} fun - (k: Number, vec: Matrix) 209 | */ 210 | forEach(fun) { 211 | this.tensor.forEach((k, ind) => { 212 | const vec = this.fromGrid(ind); 213 | fun(k, vec); 214 | }); 215 | } 216 | } 217 | 218 | export { Tensor, Grid }; -------------------------------------------------------------------------------- /src/components/planners/valueIterationPlanner.js: -------------------------------------------------------------------------------- 1 | import _ from 'lodash' 2 | import eig from "@eigen"; 3 | import Trajectory from '@/components/planners/trajectory.js' 4 | import { Grid } from './utils.js' 5 | import { smooth } from '@/components/math.js' 6 | 7 | class ValueIterationPlanner { 8 | /** 9 | * Create a ValueIterationPlanner 10 | * @param {Object} system System of interest 11 | */ 12 | constructor(system) { 13 | this.system = system; 14 | this.watchers = new Set() 15 | } 16 | 17 | /** 18 | * Add update callback 19 | */ 20 | addWatcher(fun) { 21 | this.watchers.add(fun) 22 | } 23 | 24 | /** 25 | * Add update callback 26 | */ 27 | removeWatcher(fun) { 28 | this.watchers.delete(fun) 29 | } 30 | 31 | getMatrix() { 32 | return this.V.tensor.getMatrix(); 33 | } 34 | 35 | isTarget(x) { 36 | const kx = this.V.pack(x) 37 | return _.some(this.kxTargets.map(k => _.isEqual(k, kx))) 38 | } 39 | 40 | /** 41 | * Create transition table 42 | */ 43 | createTransitionTable() { 44 | const MAX_ITER = 20; // max lookahead iterations 45 | const a = Date.now() 46 | this.table = {} 47 | this.tableDt = {} 48 | let maxIterReached = 0 49 | let updateCount = 0 50 | this.V.forEach((kx, x) => { 51 | if (!this.isTarget(x)) { 52 | this.table[kx] = {} 53 | this.tableDt[kx] = {} 54 | this.U.forEach((ku, u) => { 55 | updateCount += 1 56 | let i = 0; 57 | let xIter = x; 58 | for (i = 0; i < MAX_ITER; i++) { 59 | xIter = this.system.xNext(xIter, u, this.dt, false); 60 | const kxn = this.V.pack(xIter); 61 | if (kxn == kx) 62 | continue; 63 | else if (kxn) { 64 | this.table[kx][ku] = kxn 65 | this.tableDt[kx][ku] = (i + 1) * this.dt; 66 | } 67 | break; 68 | } 69 | if (i == MAX_ITER) 70 | maxIterReached += 1; 71 | }) 72 | } 73 | }) 74 | eig.GC.flush() 75 | console.log('Table creation time:', Date.now() - a, 'ms') 76 | // console.log('table', this.table) 77 | console.log(`max iter reached ${maxIterReached}; ${maxIterReached / updateCount * 100}%`) 78 | } 79 | 80 | /** 81 | * Running cost associated with triplet x, u, dt 82 | */ 83 | cost() { 84 | return this.dt 85 | } 86 | 87 | valueIterationInit() { 88 | this.policy = {} 89 | this.V.tensor.clear(Infinity); 90 | this.kxTargets.forEach(k => { 91 | this.V.tensor.data[k] = 0; 92 | }); 93 | } 94 | 95 | /** 96 | * Run a step of value iteration 97 | */ 98 | valueIterationStep(iter) { 99 | let maxUpdate = 0 100 | _.forEach(this.table, (val, kx) => { 101 | let bestU = null; // TODO: pick random U 102 | let minV = this.V.tensor.data[kx] 103 | _.forEach(val, (kxn, ku) => { 104 | const nextV = this.cost() + this.V.tensor.data[kxn] 105 | if (nextV < minV) { 106 | minV = nextV; 107 | bestU = ku; 108 | } 109 | }) 110 | maxUpdate = Math.max(maxUpdate, Math.abs(this.V.tensor.data[kx] - minV) || 0) 111 | if (bestU) { 112 | this.V.tensor.data[kx] = minV 113 | this.policy[kx] = bestU; 114 | } 115 | }) 116 | if (iter > 0 && iter % 100 === 0) { 117 | console.log('max update', maxUpdate) 118 | } 119 | return maxUpdate < 10e-8; 120 | } 121 | 122 | run(params, maxIter = 1000) { 123 | this.V = new Grid(params.x) 124 | this.U = new Grid(params.u) 125 | this.kxTargets = params.x.targets.map(x => this.V.pack(new eig.Matrix(x))) 126 | this.dt = params.dt 127 | this.createTransitionTable(); 128 | this.valueIterationInit(); 129 | for (let k = 0; k < maxIter; k++) { 130 | const converged = this.valueIterationStep(k) 131 | if (converged) { 132 | console.log(`Converged in ${k + 1} iterations`) 133 | this.watchers.forEach(fun => fun()) 134 | return true; 135 | } 136 | } 137 | console.log(`Not converged after ${maxIter} iterations`) 138 | return false; 139 | } 140 | 141 | getControl(x) { 142 | x = this.V.clamp(x) 143 | const kx = this.V.pack(x) 144 | return this.U.unpack(this.policy[kx]) 145 | } 146 | 147 | /** 148 | * Simulate 149 | * @param {Trajectory} trajectory - the trajectory to be set 150 | * @param {Number} duration 151 | */ 152 | simulate(x0, trajectory, maxDuration) { 153 | const sequence = [] 154 | // Find closest value in table 155 | let dist = Infinity; 156 | let kx; 157 | _.forEach(this.table, (val, _kx) => { 158 | const x = this.V.unpack(_kx); 159 | const d = x0.matSub(x).normSqr(); 160 | if (d < dist) { 161 | dist = d; 162 | kx = _kx; 163 | } 164 | }) 165 | if (!kx) return; 166 | // Now start for x 167 | for (let t = 0; t <= maxDuration; t += this.dt) { 168 | const ku = this.policy[kx]; 169 | const x = this.V.unpack(kx); 170 | const u = this.U.unpack(ku); 171 | sequence.push(x.vcat(u)); 172 | // Check for target 173 | if (_.some(this.kxTargets.map(k => _.isEqual(k, kx)))) 174 | break; 175 | kx = this.table[kx][ku]; 176 | if (!kx || !ku) break; 177 | } 178 | trajectory.set(sequence, this.dt); 179 | } 180 | } 181 | 182 | export default ValueIterationPlanner -------------------------------------------------------------------------------- /src/components/plots/KalmanPlot.vue: -------------------------------------------------------------------------------- 1 | 6 | 7 | -------------------------------------------------------------------------------- /src/components/plots/ParticlePlot.vue: -------------------------------------------------------------------------------- 1 | 5 | 6 | -------------------------------------------------------------------------------- /src/components/plots/RRTPlot.vue: -------------------------------------------------------------------------------- 1 | 5 | 6 | -------------------------------------------------------------------------------- /src/components/plots/TrajPlot.vue: -------------------------------------------------------------------------------- 1 | 6 | 7 | -------------------------------------------------------------------------------- /src/components/plots/ValueIterationPlot.vue: -------------------------------------------------------------------------------- 1 | 4 | 5 | -------------------------------------------------------------------------------- /src/components/plugins/ParticleFilterPlugin.vue: -------------------------------------------------------------------------------- 1 | 26 | 27 | 196 | 197 | -------------------------------------------------------------------------------- /src/components/plugins/PluginGroup.vue: -------------------------------------------------------------------------------- 1 | 29 | 30 | -------------------------------------------------------------------------------- /src/components/systemMixin.js: -------------------------------------------------------------------------------- 1 | import _ from 'lodash' 2 | import eig from "@eigen"; 3 | 4 | export default { 5 | data: () => ({ 6 | system: null, 7 | isMounted: false 8 | }), 9 | 10 | computed: { 11 | }, 12 | 13 | mounted() { 14 | this.isMounted = true; 15 | this.createGraphics(); 16 | }, 17 | 18 | methods: { 19 | createGraphics() { 20 | this.system.createGraphics(this.two, this.scale); 21 | const plugin = this.$refs.plugin; 22 | plugin.createGraphics(this.two); 23 | }, 24 | 25 | reset() { 26 | this.$refs.plugin.reset(); 27 | }, 28 | 29 | update() { 30 | const plugin = this.$refs.plugin; 31 | // TODO: add FPS meter 32 | let params = {}; 33 | // Update system 34 | const stepSystem = !plugin || plugin.stepSystem(); 35 | const mouseTargetEnabled = !plugin || plugin.mouseTargetEnabled; 36 | if (mouseTargetEnabled && this.mouseTarget && stepSystem) { 37 | params = this.system.trackMouse(this.mouseTarget, this.dt); 38 | } else if (plugin && plugin.ready()) { 39 | params = plugin.updateSystem(this.t, this.dt); 40 | } else if (stepSystem) { 41 | params = this.system.trim(); 42 | this.system.step(params.u, this.dt); 43 | } 44 | params.t = this.t; 45 | params.dt = this.dt; 46 | // Update plugins 47 | if (plugin) plugin.update(params); 48 | // Graphic update 49 | this.system.updateGraphics(this.worldToCanvas, params); 50 | // Run GC 51 | eig.GC.flush(); 52 | } 53 | } 54 | } -------------------------------------------------------------------------------- /src/components/twoUtils.js: -------------------------------------------------------------------------------- 1 | 2 | function setDraggable(shape, params = {}) { 3 | const el = shape._renderer.elem 4 | 5 | // el.style.cursor = 'move' 6 | el.addEventListener('mousemove', e => { 7 | shape.scale = params.scale || 1.5 8 | el.style.cursor = 'pointer' 9 | }) 10 | el.addEventListener('mouseleave', e => { 11 | shape.scale = 1 12 | }) 13 | el.addEventListener('mousedown', e => { 14 | e.preventDefault(); 15 | let anchor = [e.clientX, e.clientY]; 16 | // Create functions 17 | function drag(e) { 18 | e.preventDefault(); 19 | shape.translation.x += e.clientX - anchor[0] 20 | shape.translation.y += e.clientY - anchor[1] 21 | anchor = [e.clientX, e.clientY]; 22 | shape.scale = params.scale || 1.5 23 | const pos = [shape.translation.x, shape.translation.y] 24 | if (params.mousemove) params.mousemove(pos); 25 | } 26 | function dragEnd(e) { 27 | e.preventDefault(); 28 | window.removeEventListener('mousemove', drag) 29 | window.removeEventListener('mouseup', dragEnd); 30 | shape.scale = 1 31 | if (params.mouseup) params.mouseup(); 32 | } 33 | window.addEventListener('mousemove', drag); 34 | window.addEventListener('mouseup', dragEnd); 35 | if (params.mousedown) params.mousedown(); 36 | }) 37 | } 38 | 39 | export { setDraggable } -------------------------------------------------------------------------------- /src/components/worldMixin.js: -------------------------------------------------------------------------------- 1 | import Two from "two.js"; 2 | 3 | const FPS_ALPHA = 0.98; 4 | const FRAME_COLOR = "#455A64"; 5 | 6 | export default { 7 | data: () => ({ 8 | // FPS computation 9 | fps: 60, 10 | lastUpdate: Date.now(), 11 | // Private variables 12 | t: 0, // Simulated time 13 | two: null, 14 | mouseTarget: null, 15 | loop: null, 16 | }), 17 | 18 | computed: { 19 | mouseDragging() { 20 | return this.mouseTarget !== null 21 | }, 22 | 23 | canvas() { 24 | throw new Error('This method must be overidden') 25 | }, 26 | 27 | height() { 28 | return this.canvas.parentElement.clientHeight; 29 | }, 30 | 31 | width() { 32 | return this.canvas.parentElement.clientWidth; 33 | }, 34 | 35 | scale() { 36 | throw new Error('This method must be overidden') 37 | }, 38 | 39 | dt() { 40 | throw new Error('This method must be overidden') 41 | } 42 | }, 43 | 44 | mounted() { 45 | const canvas = this.$refs.canvas; 46 | const params = { width: this.width, height: this.height }; 47 | this.two = new Two(params).appendTo(canvas); 48 | 49 | // Add helper functions 50 | this.two.scale = this.scale; 51 | this.two.canvas = canvas; 52 | this.two.worldToCanvas = this.worldToCanvas; 53 | this.two.canvasToWorld = this.canvasToWorld; 54 | 55 | // Add mouse events 56 | document.addEventListener("mouseup", ev => { 57 | this.dragging = false; 58 | this.mouseTarget = null; 59 | }); 60 | canvas.addEventListener("mousedown", ev => { 61 | this.dragging = true; 62 | this.mouseTarget = this.canvasToWorld([ev.offsetX, ev.offsetY]); 63 | }); 64 | canvas.addEventListener("mousemove", ev => { 65 | if (this.dragging) { 66 | this.mouseTarget = this.canvasToWorld([ev.offsetX, ev.offsetY]); 67 | } 68 | }); 69 | 70 | // Frame 71 | const frame = this.two.makeGroup( 72 | this.two.makeLine(-this.width / 3, 0, this.width / 3, 0), 73 | this.two.makeLine(0, -this.width / 6, 0, this.width / 6) 74 | ); 75 | frame.translation.set(this.width / 2, this.height / 2); 76 | frame.fill = FRAME_COLOR; 77 | 78 | // Start loop 79 | const updateFun = () => { 80 | const now = Date.now(); 81 | this.t += this.dt 82 | try { 83 | this.update(); 84 | } catch (e) { 85 | console.error(e) 86 | } 87 | this.two.update(); 88 | const dtMeas = (Date.now() - this.lastUpdate) / 1000 89 | this.fps = this.fps * FPS_ALPHA + (1 - FPS_ALPHA) / dtMeas 90 | this.lastUpdate = Date.now(); 91 | const updateTimeMs = Date.now() - now 92 | const targetDtMs = Math.max(0, this.dt * 1000 - updateTimeMs) 93 | this.loop = setTimeout(updateFun, targetDtMs); 94 | }; 95 | 96 | this.$nextTick(() => { 97 | updateFun(); 98 | }) 99 | }, 100 | 101 | beforeDestroy() { 102 | clearTimeout(this.loop) 103 | }, 104 | 105 | methods: { 106 | canvasToWorld(pos) { 107 | return [ 108 | (pos[0] - this.width / 2) / this.scale, 109 | -(pos[1] - this.height / 2) / this.scale 110 | ]; 111 | }, 112 | 113 | worldToCanvas(pos) { 114 | return [ 115 | pos[0] * this.scale + this.width / 2, 116 | -pos[1] * this.scale + this.height / 2 117 | ]; 118 | }, 119 | 120 | update() { 121 | throw new Error('This method must be overidden') 122 | }, 123 | 124 | reset() { 125 | this.t = 0 126 | } 127 | } 128 | } -------------------------------------------------------------------------------- /src/main.js: -------------------------------------------------------------------------------- 1 | import Vue from 'vue' 2 | import App from './App.vue' 3 | import router from './router/router.js' 4 | // import store from './store' 5 | import vuetify from './plugins/vuetify'; 6 | import Notifications from 'vue-notification' 7 | 8 | // Add simple store 9 | const store = new Vue({ 10 | data: () => ({ 11 | windowSize: { x: 0, y: 0 } 12 | }) 13 | }) 14 | Vue.prototype.$store = store 15 | 16 | // Add bus 17 | const bus = new Vue() 18 | Vue.prototype.$bus = bus 19 | 20 | // Add notification handler 21 | Vue.use(Notifications) 22 | bus.notify = (type, msg) => { 23 | Vue.notify({ 24 | group: "alert", 25 | title: { error: "Error", success: "Success" }[type], 26 | text: msg, 27 | type: { error: "type-error", success: "type-success" }[type], 28 | duration: 6000 29 | }); 30 | } 31 | 32 | Vue.config.productionTip = false 33 | new Vue({ 34 | router, 35 | store, 36 | vuetify, 37 | render: h => h(App) 38 | }).$mount('#app') 39 | -------------------------------------------------------------------------------- /src/plugins/vuetify.js: -------------------------------------------------------------------------------- 1 | import Vue from 'vue'; 2 | import Vuetify from 'vuetify/lib'; 3 | 4 | Vue.use(Vuetify); 5 | 6 | export default new Vuetify({ 7 | }); 8 | -------------------------------------------------------------------------------- /src/router/router.js: -------------------------------------------------------------------------------- 1 | import Vue from 'vue' 2 | import VueRouter from 'vue-router' 3 | // Environments 4 | import Home from "@/views/Home.vue"; 5 | import LQR from "@/components/environments/LQR/LQR.vue"; 6 | import ValueIteration from "@/components/environments/ValueIteration/ValueIteration.vue"; 7 | import RRT from "@/components/environments/RRT/RRT.vue"; 8 | import Flatness from "@/components/environments/Flatness/Flatness.vue"; 9 | import DirectCollocation from "@/components/environments/DirectCollocation/DirectCollocation.vue"; 10 | import MPC from "@/components/environments/MPC/MPC.vue"; 11 | import KalmanFilter from "@/components/environments/KalmanFilter/KalmanFilter.vue"; 12 | import ParticleFilter from "@/components/environments/ParticleFilter/ParticleFilter.vue"; 13 | 14 | Vue.use(VueRouter) 15 | 16 | const env = [ 17 | LQR, 18 | ValueIteration, 19 | // RRT, 20 | Flatness, 21 | DirectCollocation, 22 | MPC, 23 | KalmanFilter, 24 | ParticleFilter, 25 | ]; 26 | const envRoutes = env.map(env => ({ 27 | component: env, 28 | name: env.name, 29 | path: `/${env.name}/:systemName`, 30 | props: true, 31 | meta: { 32 | ...env.meta, 33 | rightDrawer: true, 34 | } 35 | })); 36 | 37 | const routes = [ 38 | { 39 | path: '/', 40 | component: Home, 41 | }, 42 | ...envRoutes 43 | ] 44 | 45 | const router = new VueRouter({ 46 | // mode: 'history', 47 | base: process.env.BASE_URL, 48 | routes 49 | }) 50 | 51 | export { envRoutes } 52 | export default router 53 | -------------------------------------------------------------------------------- /src/store/index.js: -------------------------------------------------------------------------------- 1 | import Vue from 'vue' 2 | import Vuex from 'vuex' 3 | 4 | Vue.use(Vuex) 5 | 6 | export default new Vuex.Store({ 7 | state: { 8 | }, 9 | mutations: { 10 | }, 11 | actions: { 12 | }, 13 | modules: { 14 | } 15 | }) 16 | -------------------------------------------------------------------------------- /src/views/Demo.vue: -------------------------------------------------------------------------------- 1 | 13 | 14 | 84 | 85 | -------------------------------------------------------------------------------- /src/views/Home.vue: -------------------------------------------------------------------------------- 1 | 12 | 13 | 57 | 58 | -------------------------------------------------------------------------------- /vue.config.js: -------------------------------------------------------------------------------- 1 | const path = require('path') 2 | 3 | module.exports = { 4 | publicPath: process.env.NODE_ENV === 'production' 5 | ? '/controls-js/' 6 | : '/', 7 | "transpileDependencies": [ 8 | "vuetify" 9 | ], 10 | chainWebpack: config => { 11 | config.resolve.symlinks(false); 12 | config.module 13 | .rule('wasm') 14 | .type('javascript/auto') 15 | .test(/\.wasm$/) 16 | .use('arraybuffer-loader') 17 | .loader('arraybuffer-loader') 18 | .end() 19 | 20 | 21 | const svgRule = config.module.rule('svg'); 22 | svgRule.uses.clear(); 23 | svgRule 24 | .use('babel-loader') 25 | .loader('babel-loader') 26 | .end() 27 | .use('html-loader') 28 | .loader('html-loader'); 29 | 30 | config.resolve.alias.set('@src', path.resolve(__dirname, 'src')) 31 | config.resolve.alias.set('@assets', path.resolve(__dirname, 'src/assets')) 32 | // Swap out with the below lines for local development 33 | config.resolve.alias.set('@eigen', 'eigen') 34 | config.resolve.alias.set('@nlopt', 'nlopt-js') 35 | // config.resolve.alias.set('@eigen', path.resolve(__dirname, '../eigen-js/dist/index.js')) 36 | // config.resolve.alias.set('@nlopt', path.resolve(__dirname, '../nlopt-js/dist/index.js')) 37 | } 38 | } --------------------------------------------------------------------------------