Inverse Kinematics in action

Posted August 06 2012, tagged math, robots, canvas,

Here is a simple demo showing Inverse Kinematics in action.   The full calculation can be seen on my previous post, Inverse Kinematics and Robotic arms

Click within the maximum radius of the arms to calculate the angles needed to position them to the end point. It should work in most browsers that are compatible with canvas. However I've only tested it in Chrome.

 

 

Javascript code

To save you having to View source, here is the code I've written for this demo. Feel free to play with it as you want.

 
        var L1 = 4.25;
        var L2 = 6.25;
        var world = {'x':14,'y':14}
        var originOffset = { 'x':1, 'y': 1};
        var screenToWorldFactor = 10;
        
        function w2screen(p) {
            // Convert a co-ordinte in the world to a screen co-ordinate
            // The world co-ordinate has the ordinate in the lower left hand corner
            var canvas = document.getElementById('Canvas')
            var scaleX = Math.max(world.x, canvas.width) / Math.min(world.x, canvas.width);
            var scaleY = Math.max(world.y, canvas.height) / Math.min(world.y, canvas.height);
            
            var screenX = (p.x + originOffset.x) * scaleX ;
            var screenY = (canvas.height - (p.y + originOffset.y) * scaleY) ;
            
            return {'x':screenX, 'y':screenY};
        }
        
        function s2world(p) {
            // Converts a screen co-ordinate in to the relevant world co-ordinate
            var canvas = document.getElementById('Canvas')
            var scaleX = Math.max(world.x, canvas.width) / Math.min(world.x, canvas.width);
            var scaleY = Math.max(world.y, canvas.height) / Math.min(world.y, canvas.height);
            
            var worldX = p.x / scaleX - originOffset.x;
            var worldY = (canvas.height - p.y) / scaleY - originOffset.y;  
            
            return {'x':Math.round(worldX * screenToWorldFactor) / screenToWorldFactor, 'y': Math.round(worldY * screenToWorldFactor) / screenToWorldFactor};
            
        }
        
        function degToRad(ang) {
            // Utility to convert degrees to Radians
            return ang * Math.PI /180;
        }
        
        function destLocation(origin, ang, length) {
            // Calculates the world position given an origin, angle and length
            // this is used when we want to go from one poisition to another
            var x2 = Math.cos(ang) * length + origin.x;
            var y2 = Math.sin(ang) * length + origin.y;
            return {'x':x2,'y': y2};
        }
        
        function clearCanvas() {
            // Just clears the canvas
            var canvas = document.getElementById("Canvas");
            var context = canvas.getContext("2d");
            context.clearRect ( 0 , 0 , canvas.width , canvas.height );
        }
        
        function drawGrid() {
            // Draw the world grid, including axis points, and limits of the arm
            var canvas = document.getElementById("Canvas");
            var context = canvas.getContext("2d");
            var from, to, radScreen;
            
            context.lineWidth = 1;
            
           
            // draw a radius showing the arms extents
            from = w2screen({'x':0, 'y': 0});
            radScreen = Math.max(world.x, canvas.width) / Math.min(world.x, canvas.width);;
            context.beginPath();
            context.arc(from.x, from.y, radScreen * (L1 + L2), 0, 2 * Math.PI, false);
            context.strokeStyle = "#c00";
            context.stroke();

            // X grid lines           
            context.strokeStyle = '#000';
            context.font = "italic 20px Calibri";
            context.fillStyle = "#000";
            for (var x = 0; x < world.x - originOffset.x; x++) {
                from = w2screen({'x': x, 'y':0});
                to = w2screen({'x':x, 'y': world.y});
                
                context.beginPath();
                context.moveTo(from.x,from.y);
                context.lineTo(to.x, to.y);
                context.stroke();
                
                context.fillText(x,from.x, w2screen({'x':0,'y':-originOffset.y * 0.5}).y );
            }

            // Y grid lines
            for (var y = 0; y < world.y - originOffset.y; y++) {
                from = w2screen({'x': 0, 'y': y});
                to = w2screen({'x': world.x,'y':y });
                
                context.beginPath();
                context.moveTo(from.x,from.y);
                context.lineTo(to.x, to.y);
                context.stroke();
                
                context.fillText(y,w2screen({'x':-originOffset.x * 0.5,'y':0}).x ,from.y );
            }
            
        }
        
        function drawArms(ang_1,ang_2) {
                
            var canvas = document.getElementById("Canvas");
            var context = canvas.getContext("2d");
            var points = new Array();

            // Convert ang_2 so that it is relative to ang_1
            // e.g. if ang_2 is 180 then it is the same as ang_1
            ang_2 = ang_2 - degToRad(180) + ang_1;
            
            // Calculate and push the points on to the points array, 
            points.push({'x':0,'y':0});
            points.push(destLocation(points[0], ang_1, L1));
            points.push(destLocation(points[1], ang_2, L2));

            // Convert each of the points in to screen co-ordinates
            var screenCoords = new Array();
            for (p in points) {
                screenCoords.push(w2screen(points[p]));
            }
            
            // Now draw the line
            context.beginPath();
            
            context.lineWidth = 10;
            context.strokeStyle = "#0000ff";
            context.lineCap = "round";
            context.moveTo(screenCoords[0].x, screenCoords[0].y);
            for (i = 1; i < screenCoords.length; i++) {
                context.lineTo(screenCoords[i].x, screenCoords[i].y);
                
                context.stroke();
                context.lineWidth = 10 - i * 5;
            }
            
            // Highlight the joints
            for (p in screenCoords) { 
                context.beginPath();
                context.arc(screenCoords[p].x, screenCoords[p].y, 10 - p* 2, 0, 2 * Math.PI, false);
                context.fillStyle = "#fff";
                context.fill();
                context.lineWidth = 5;
                context.strokeStyle = "black";
                context.stroke();
            }
        }
        
        function inverseKinematics(x,y) {
            // This is where the real work is done
            var B2 = x*x + y*y // The length of the hypotenus squared
            var B = Math.sqrt(B2);
            
            var ang_1 = Math.atan(y/ x) + Math.acos((L1*L1 - L2*L2 + x*x + y*y) / (2 *  L1 * B))
            var ang_2 = Math.acos((L1*L1 + L2*L2-B2)/(2 * L1 * L2));
            return {'ang_1': ang_1, 'ang_2': ang_2};
        }
        
        window.onload = function() {
            
            moveTo();
            
            document.getElementById('Canvas').addEventListener('click', function(e) {
                var canvas = document.getElementById('Canvas');
                var x = event.pageX - canvas.offsetLeft;
                var y = event.pageY - canvas.offsetTop;

                var context = canvas.getContext('2d');
                var target = s2world({'x':x, 'y':y});
                document.getElementById('Xpos').value = target.x;
                document.getElementById('Ypos').value = target.y;
                
                moveTo();
            }, false);
            
            document.getElementById('Xpos').addEventListener('click',moveTo, false)
            document.getElementById('Ypos').addEventListener('click',moveTo, false)

        };
        
        function moveTo() {
            var xPos = document.getElementById('Xpos').value;
            var yPos = document.getElementById('Ypos').value;
            
            // ensure that the positions, do not exceed the max length of the arms
            var arms_length = Math.sqrt(xPos * xPos + yPos * yPos);
            if (arms_length <= L1 + L2) { 
                var ang = inverseKinematics(xPos, yPos);
                
                clearCanvas();
                drawGrid();
                drawArms(ang.ang_1, ang.ang_2);
            }
            
        }

comments powered by Disqus