gitweb.ps.run Git - inverse_kinematics/blob - index.html

   1 <!DOCTYPE html>\r
   2 <html>\r
   3 \r
   4 <head>\r
   5   <title></title>\r
   6 \r
   7   <style type="text/css">\r
   8     body {\r
   9       margin: 0;\r
  10     }\r
  11 \r
  12     input {\r
  13       width: 98%;\r
  14     }\r
  15   </style>\r
  16 \r
  17   <script src="https://cdnjs.cloudflare.com/ajax/libs/mathjs/8.1.0/math.js"\r
  18     integrity="sha512-ne87j5uORxbrU7+bsqeJJWfWj5in65R9PCjaQL161xtH5cesZgULVbeVAkzAAN7hnYOcrHeBas9Wbd/Lm8gXFA=="\r
  19     crossorigin="anonymous"></script>\r
  20   <script src="https://cdn.jsdelivr.net/npm/vue@2.6.12/dist/vue.js"></script>\r
  21   <script type="text/javascript">\r
  22     let ctx;\r
  23 \r
  24     class Point {\r
  25       constructor(x, y) {\r
  26         this.x = x;\r
  27         this.y = y;\r
  28       }\r
  29 \r
  30       add(that) {\r
  31         return new Point(\r
  32           this.x + that.x,\r
  33           this.y + that.y\r
  34         );\r
  35       }\r
  36 \r
  37       sub(that) {\r
  38         return new Point(\r
  39           this.x - that.x,\r
  40           this.y - that.y\r
  41         );\r
  42       }\r
  43 \r
  44       dist(that) {\r
  45         let a = this.x - that.x;\r
  46         let b = this.y - that.y;\r
  47         return Math.sqrt(a * a + b * b)\r
  48       }\r
  49     }\r
  50 \r
  51     class Segment {\r
  52       constructor(length, angle, minAngle, maxAngle) {\r
  53         this.length = length;\r
  54         this.angle = angle;\r
  55         this.minAngle = minAngle;\r
  56         this.maxAngle = maxAngle;\r
  57       }\r
  58 \r
  59       restrictAngle() {\r
  60         if (this.angle < this.minAngle)\r
  61           this.angle = this.minAngle;\r
  62         if (this.angle > this.maxAngle)\r
  63           this.angle = this.maxAngle;\r
  64       }\r
  65     }\r
  66 \r
  67     class Arm {\r
  68       constructor(base, segments) {\r
  69         this.base = base;\r
  70         this.segments = segments;\r
  71       }\r
  72 \r
  73       draw() {\r
  74         let currentPoint = this.base;\r
  75         drawCircle(currentPoint, 10, '#0000FF');\r
  76 \r
  77         let angleSum = 0;\r
  78 \r
  79         for (let i in this.segments) {\r
  80           let segment = this.segments[i];\r
  81 \r
  82           angleSum += segment.angle;\r
  83 \r
  84           let endPoint = currentPoint.add(new Point(\r
  85             Math.cos(angleSum * Math.PI / 180) * segment.length,\r
  86             Math.sin(angleSum * Math.PI / 180) * segment.length\r
  87           ));\r
  88 \r
  89           drawLine(currentPoint, endPoint);\r
  90           drawCircle(currentPoint, 10);\r
  91 \r
  92           currentPoint = endPoint;\r
  93         }\r
  94         drawCircle(currentPoint, 10, '#FF0000');\r
  95       }\r
  96 \r
  97       moveTo(targetPoint) {\r
  98         const H = 0.00002;\r
  99         const EPS = 5;\r
 100 \r
 101         let iteration = 0;\r
 102 \r
 103         while (targetPoint.dist(this.getEndPoint()) > EPS) {\r
 104           if (++iteration > 100) break;\r
 105           let dO = this.getDeltaOrientation(targetPoint);\r
 106           for (let i in this.segments) {\r
 107             this.segments[i].angle += dO._data[i] * H;\r
 108             if (this.segments[i].angle < -360) this.segments[i].angle = this.segments[i].angle % -360;\r
 109             if (this.segments[i].angle > +360) this.segments[i].angle = this.segments[i].angle % +360;\r
 110 \r
 111             this.segments[i].restrictAngle();\r
 112           }\r
 113         }\r
 114       }\r
 115 \r
 116       getDeltaOrientation(targetPoint) {\r
 117         let Jt = this.getJacobianTranspose(targetPoint);\r
 118         let V = targetPoint.sub(this.getEndPoint());\r
 119         return math.multiply(Jt, [V.x, V.y, 1]);\r
 120       }\r
 121 \r
 122       getJacobianTranspose(targetPoint) {\r
 123         let rows = [];\r
 124         for (let i in this.segments) {\r
 125           let Ra = [0, 0, 1];\r
 126           let E = this.getEndPoint();\r
 127           let A = this.getSegmentEndpoint(i);\r
 128 \r
 129           let diff = [E.x - A.x, E.y - A.y, 0];\r
 130 \r
 131           let row = math.cross(Ra, diff);\r
 132           rows.push(row);\r
 133         }\r
 134         let transpose = math.matrix(rows);\r
 135         //let jacobi = math.transpose(transpose);\r
 136         //let pseudo = math.multiply(math.inv(math.multiply(transpose, jacobi)), transpose);\r
 137         //return pseudo;\r
 138         return transpose;\r
 139       }\r
 140 \r
 141       getEndPoint() {\r
 142         return this.getSegmentEndpoint(this.segments.length);\r
 143       }\r
 144 \r
 145       getSegmentEndpoint(index) {\r
 146         let currentPoint = this.base;\r
 147         let angleSum = 0;\r
 148 \r
 149         for (let i in this.segments) {\r
 150           if (index == i) break;\r
 151 \r
 152           let segment = this.segments[i];\r
 153 \r
 154           angleSum += segment.angle;\r
 155 \r
 156           let endPoint = currentPoint.add(new Point(\r
 157             Math.cos(angleSum * Math.PI / 180) * segment.length,\r
 158             Math.sin(angleSum * Math.PI / 180) * segment.length\r
 159           ));\r
 160 \r
 161           currentPoint = endPoint;\r
 162         }\r
 163 \r
 164         return currentPoint;\r
 165       }\r
 166     }\r
 167 \r
 168     let mousePos = new Point(0, 0);\r
 169 \r
 170     let arm = new Arm(\r
 171       new Point(window.innerWidth / 2, window.innerHeight / 2),\r
 172       []\r
 173     );\r
 174 \r
 175     for (let i = 0; i < 3; i++)\r
 176       AddSegment();\r
 177 \r
 178     function draw(dt) {\r
 179       ctx.clearRect(0, 0, w, h);\r
 180 \r
 181       arm.moveTo(mousePos);\r
 182       arm.draw();\r
 183 \r
 184       window.requestAnimationFrame(draw);\r
 185     }\r
 186 \r
 187     function drawCircle(pos, radius, color) {\r
 188       let filled = false;\r
 189       if (color) {\r
 190         filled = true;\r
 191         ctx.fillStyle = color;\r
 192       }\r
 193       ctx.beginPath();\r
 194 \r
 195       ctx.arc(pos.x, pos.y, radius, 0, 2 * Math.PI);\r
 196 \r
 197       if (filled)\r
 198         ctx.fill();\r
 199 \r
 200       ctx.stroke();\r
 201     }\r
 202 \r
 203     function drawLine(from, to) {\r
 204       ctx.beginPath();\r
 205       ctx.moveTo(from.x, from.y);\r
 206       ctx.lineTo(to.x, to.y);\r
 207       ctx.stroke();\r
 208     }\r
 209 \r
 210     function init() {\r
 211       let canvas = document.getElementById('canvas');\r
 212       let resize = function () {\r
 213         w = window.innerWidth;\r
 214         h = window.innerHeight - 200;\r
 215         canvas.width = w;\r
 216         canvas.height = h;\r
 217         document.getElementById('inputBaseX').setAttribute('max', w);\r
 218         document.getElementById('inputBaseY').setAttribute('max', h);\r
 219       }\r
 220       window.onresize = resize;\r
 221       resize();\r
 222       if (canvas.getContext) {\r
 223         ctx = canvas.getContext('2d');\r
 224         draw(performance.now());\r
 225       }\r
 226       canvas.onmousemove = (evt) => {\r
 227         mousePos.x = evt.clientX;\r
 228         mousePos.y = evt.clientY;\r
 229       };\r
 230     }\r
 231 \r
 232     function AddSegment() {\r
 233       arm.segments.push(new Segment(100, 0, -360, 360));\r
 234     }\r
 235   </script>\r
 236 </head>\r
 237 \r
 238 <body onload="init();">\r
 239   <canvas id="canvas"></canvas>\r
 240   <div id="app">\r
 241     Base X\r
 242     <input v-model.number="arm.base.x" type="range" min="0" id="inputBaseX" style="width: 100px;" />\r
 243     Base Y\r
 244     <input v-model.number="arm.base.y" type="range" min="0" id="inputBaseY" style="width: 100px;" />\r
 245     <br />\r
 246     <table style="width: 100%;">\r
 247       <colgroup>\r
 248         <col span="1" style="width: 20%;">\r
 249         <col span="1" style="width: 20%;">\r
 250         <col span="1" style="width: 20%;">\r
 251         <col span="1" style="width: 20%;">\r
 252         <col span="1" style="width: 20%;">\r
 253       </colgroup>\r
 254       <tr>\r
 255         <th>Length</th>\r
 256         <th>Min. Angle</th>\r
 257         <th>Max. Angle</th>\r
 258         <th>Angle</th>\r
 259       </tr>\r
 260       <tr v-for="segment in arm.segments">\r
 261         <td><input v-model.number="segment.length" type="range" max="300" /></td>\r
 262         <td><input v-model.number="segment.minAngle" min="-360" max="0" type="range" /></td>\r
 263         <td><input v-model.number="segment.maxAngle" min="0" max="360" type="range" /></td>\r
 264         <td>{{ Number(segment.angle).toFixed(4) }}</td>\r
 265         <td><button v-on:click="arm.segments.splice(arm.segments.indexOf(segment), 1)"\r
 266             v-if="arm.segments.length > 1">Delete</button></td>\r
 267       </tr>\r
 268     </table>\r
 269     <button onclick="AddSegment()">Add</button>\r
 270   </div>\r
 271   <script>\r
 272     var app = new Vue({\r
 273       el: '#app',\r
 274       data: {\r
 275         arm: arm\r
 276       }\r
 277     })\r
 278   </script>\r
 279 </body>\r
 280 \r
 281 </html>\r