--- /dev/null
+<!DOCTYPE html>\r
+<html>\r
+\r
+<head>\r
+ <title></title>\r
+\r
+ <style type="text/css">\r
+ body {\r
+ margin: 0;\r
+ }\r
+\r
+ input {\r
+ width: 98%;\r
+ }\r
+ </style>\r
+\r
+ <script src="https://cdnjs.cloudflare.com/ajax/libs/mathjs/8.1.0/math.js"\r
+ integrity="sha512-ne87j5uORxbrU7+bsqeJJWfWj5in65R9PCjaQL161xtH5cesZgULVbeVAkzAAN7hnYOcrHeBas9Wbd/Lm8gXFA=="\r
+ crossorigin="anonymous"></script>\r
+ <script src="https://cdn.jsdelivr.net/npm/vue@2.6.12/dist/vue.js"></script>\r
+ <script type="text/javascript">\r
+ let ctx;\r
+\r
+ class Point {\r
+ constructor(x, y) {\r
+ this.x = x;\r
+ this.y = y;\r
+ }\r
+\r
+ add(that) {\r
+ return new Point(\r
+ this.x + that.x,\r
+ this.y + that.y\r
+ );\r
+ }\r
+\r
+ sub(that) {\r
+ return new Point(\r
+ this.x - that.x,\r
+ this.y - that.y\r
+ );\r
+ }\r
+\r
+ dist(that) {\r
+ let a = this.x - that.x;\r
+ let b = this.y - that.y;\r
+ return Math.sqrt(a * a + b * b)\r
+ }\r
+ }\r
+\r
+ class Segment {\r
+ constructor(length, angle, minAngle, maxAngle) {\r
+ this.length = length;\r
+ this.angle = angle;\r
+ this.minAngle = minAngle;\r
+ this.maxAngle = maxAngle;\r
+ }\r
+\r
+ restrictAngle() {\r
+ if (this.angle < this.minAngle)\r
+ this.angle = this.minAngle;\r
+ if (this.angle > this.maxAngle)\r
+ this.angle = this.maxAngle;\r
+ }\r
+ }\r
+\r
+ class Arm {\r
+ constructor(base, segments) {\r
+ this.base = base;\r
+ this.segments = segments;\r
+ }\r
+\r
+ draw() {\r
+ let currentPoint = this.base;\r
+ drawCircle(currentPoint, 10, '#0000FF');\r
+\r
+ let angleSum = 0;\r
+\r
+ for (let i in this.segments) {\r
+ let segment = this.segments[i];\r
+\r
+ angleSum += segment.angle;\r
+\r
+ let endPoint = currentPoint.add(new Point(\r
+ Math.cos(angleSum * Math.PI / 180) * segment.length,\r
+ Math.sin(angleSum * Math.PI / 180) * segment.length\r
+ ));\r
+\r
+ drawLine(currentPoint, endPoint);\r
+ drawCircle(currentPoint, 10);\r
+\r
+ currentPoint = endPoint;\r
+ }\r
+ drawCircle(currentPoint, 10, '#FF0000');\r
+ }\r
+\r
+ moveTo(targetPoint) {\r
+ const H = 0.00002;\r
+ const EPS = 5;\r
+\r
+ let iteration = 0;\r
+\r
+ while (targetPoint.dist(this.getEndPoint()) > EPS) {\r
+ if (++iteration > 100) break;\r
+ let dO = this.getDeltaOrientation(targetPoint);\r
+ for (let i in this.segments) {\r
+ this.segments[i].angle += dO._data[i] * H;\r
+ if (this.segments[i].angle < -360) this.segments[i].angle = this.segments[i].angle % -360;\r
+ if (this.segments[i].angle > +360) this.segments[i].angle = this.segments[i].angle % +360;\r
+\r
+ this.segments[i].restrictAngle();\r
+ }\r
+ }\r
+ }\r
+\r
+ getDeltaOrientation(targetPoint) {\r
+ let Jt = this.getJacobianTranspose(targetPoint);\r
+ let V = targetPoint.sub(this.getEndPoint());\r
+ return math.multiply(Jt, [V.x, V.y, 1]);\r
+ }\r
+\r
+ getJacobianTranspose(targetPoint) {\r
+ let rows = [];\r
+ for (let i in this.segments) {\r
+ let Ra = [0, 0, 1];\r
+ let E = this.getEndPoint();\r
+ let A = this.getSegmentEndpoint(i);\r
+\r
+ let diff = [E.x - A.x, E.y - A.y, 0];\r
+\r
+ let row = math.cross(Ra, diff);\r
+ rows.push(row);\r
+ }\r
+ let transpose = math.matrix(rows);\r
+ //let jacobi = math.transpose(transpose);\r
+ //let pseudo = math.multiply(math.inv(math.multiply(transpose, jacobi)), transpose);\r
+ //return pseudo;\r
+ return transpose;\r
+ }\r
+\r
+ getEndPoint() {\r
+ return this.getSegmentEndpoint(this.segments.length);\r
+ }\r
+\r
+ getSegmentEndpoint(index) {\r
+ let currentPoint = this.base;\r
+ let angleSum = 0;\r
+\r
+ for (let i in this.segments) {\r
+ if (index == i) break;\r
+\r
+ let segment = this.segments[i];\r
+\r
+ angleSum += segment.angle;\r
+\r
+ let endPoint = currentPoint.add(new Point(\r
+ Math.cos(angleSum * Math.PI / 180) * segment.length,\r
+ Math.sin(angleSum * Math.PI / 180) * segment.length\r
+ ));\r
+\r
+ currentPoint = endPoint;\r
+ }\r
+\r
+ return currentPoint;\r
+ }\r
+ }\r
+\r
+ let mousePos = new Point(0, 0);\r
+\r
+ let arm = new Arm(\r
+ new Point(window.innerWidth / 2, window.innerHeight / 2),\r
+ []\r
+ );\r
+\r
+ for (let i = 0; i < 3; i++)\r
+ AddSegment();\r
+\r
+ function draw(dt) {\r
+ ctx.clearRect(0, 0, w, h);\r
+\r
+ arm.moveTo(mousePos);\r
+ arm.draw();\r
+\r
+ window.requestAnimationFrame(draw);\r
+ }\r
+\r
+ function drawCircle(pos, radius, color) {\r
+ let filled = false;\r
+ if (color) {\r
+ filled = true;\r
+ ctx.fillStyle = color;\r
+ }\r
+ ctx.beginPath();\r
+\r
+ ctx.arc(pos.x, pos.y, radius, 0, 2 * Math.PI);\r
+\r
+ if (filled)\r
+ ctx.fill();\r
+\r
+ ctx.stroke();\r
+ }\r
+\r
+ function drawLine(from, to) {\r
+ ctx.beginPath();\r
+ ctx.moveTo(from.x, from.y);\r
+ ctx.lineTo(to.x, to.y);\r
+ ctx.stroke();\r
+ }\r
+\r
+ function init() {\r
+ let canvas = document.getElementById('canvas');\r
+ let resize = function () {\r
+ w = window.innerWidth;\r
+ h = window.innerHeight - 200;\r
+ canvas.width = w;\r
+ canvas.height = h;\r
+ document.getElementById('inputBaseX').setAttribute('max', w);\r
+ document.getElementById('inputBaseY').setAttribute('max', h);\r
+ }\r
+ window.onresize = resize;\r
+ resize();\r
+ if (canvas.getContext) {\r
+ ctx = canvas.getContext('2d');\r
+ draw(performance.now());\r
+ }\r
+ canvas.onmousemove = (evt) => {\r
+ mousePos.x = evt.clientX;\r
+ mousePos.y = evt.clientY;\r
+ };\r
+ }\r
+\r
+ function AddSegment() {\r
+ arm.segments.push(new Segment(100, 0, -360, 360));\r
+ }\r
+ </script>\r
+</head>\r
+\r
+<body onload="init();">\r
+ <canvas id="canvas"></canvas>\r
+ <div id="app">\r
+ Base X\r
+ <input v-model.number="arm.base.x" type="range" min="0" id="inputBaseX" style="width: 100px;" />\r
+ Base Y\r
+ <input v-model.number="arm.base.y" type="range" min="0" id="inputBaseY" style="width: 100px;" />\r
+ <br />\r
+ <table style="width: 100%;">\r
+ <colgroup>\r
+ <col span="1" style="width: 20%;">\r
+ <col span="1" style="width: 20%;">\r
+ <col span="1" style="width: 20%;">\r
+ <col span="1" style="width: 20%;">\r
+ <col span="1" style="width: 20%;">\r
+ </colgroup>\r
+ <tr>\r
+ <th>Length</th>\r
+ <th>Min. Angle</th>\r
+ <th>Max. Angle</th>\r
+ <th>Angle</th>\r
+ </tr>\r
+ <tr v-for="segment in arm.segments">\r
+ <td><input v-model.number="segment.length" type="range" max="300" /></td>\r
+ <td><input v-model.number="segment.minAngle" min="-360" max="0" type="range" /></td>\r
+ <td><input v-model.number="segment.maxAngle" min="0" max="360" type="range" /></td>\r
+ <td>{{ Number(segment.angle).toFixed(4) }}</td>\r
+ <td><button v-on:click="arm.segments.splice(arm.segments.indexOf(segment), 1)"\r
+ v-if="arm.segments.length > 1">Delete</button></td>\r
+ </tr>\r
+ </table>\r
+ <button onclick="AddSegment()">Add</button>\r
+ </div>\r
+ <script>\r
+ var app = new Vue({\r
+ el: '#app',\r
+ data: {\r
+ arm: arm\r
+ }\r
+ })\r
+ </script>\r
+</body>\r
+\r
+</html>\r
projects / inverse_kinematics / commitdiff