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