Create a Mechanical Snake With Inverse Kinematics

Imagine a chain of particles animating in symphony together: A train moving as all attached compartments follow suit; a puppet dancing as its master pulls its string; even your arms, when your parents hold your hands as they lead you in an evening walk. Movevment ripples down from the last node to the origin, abiding to constraints as it goes. This is inverse kinematics (IK), a mathematical algorithm that calculates necessary motions. Here, we'll use it to create a snake that's a little more advanced than the one from Nokia games.

Final Result Preview

Let's take a look at the final result we will be working towards. Press and hold UP, LEFT and RIGHT keys to make it move.

Step 1: Relationships in a Chain

A chain is constructed of nodes. Each node represents a point in the chain where translation and rotation may happen. In IK chain, motion ripples down in reverse from the last node (last child) to the first node (root node) as opposed to Forward Kinematics (FK) where kinematics traverse from the root node to the last child.

All chains begins with the root node. This root node is the acting parent unto which a new child node is attached. In turn, this first child will parent the second child in the chain, and this is repeated until the last child is added. The animation below depicts such a relationship.

Step 2: Remembering Relationships

The IKshape class implements the notion of a node in our chain. Instances of IKshape class remember their parent and child nodes, with the exceptions of the root node which has no parent node and the last node which has no child node. Below are the private properties of the IKshape.

private var childNode:IKshape;
private var parentNode:IKshape;
private var vec2Parent:Vector2D;

Accessors of these properties are shown as below:

public function set IKchild(childSprite:IKshape):void 
{
	childNode = childSprite;
}

public function get IKchild ():IKshape 
{
	return childNode
}

public function set IKparent(parentSprite:IKshape):void 
{
	parentNode = parentSprite;
}

public function get IKparent():IKshape
{
	return parentNode;
}

Step 3: Vector from Child to Parent

You may notice that this class does store a Vector2D that points from child node to parent node. The rationale for this direction is due to motion flowing from child to parent. Vector2D is used because the magnitude and direction of vector pointing from child to parent will be manipulated frequently while implementing behaviour of an IK chain. Thus, keeping track of such data is necessary. Below are methods to maniplate vector quantities for IKshape.

public function calcVec2Parent():void
{
	var xlength:Number = parentNode.x - this.x;
	var ylength:Number = parentNode.y - this.y;

	vec2Parent = new Vector2D(xlength, ylength);
}

public function setVec2Parent(vec:Vector2D):void
{
	vec2Parent = vec.duplicate();
}

public function getVec2Parent():Vector2D
{
	return vec2Parent.duplicate();
}

public function getAng2Parent():Number
{
	return vec2Parent.getAngle();
}

Step 4: Drawing Node

Last but not least, we need a method to draw our shape. We shall draw a rectangle to represent each node. However, any other preferences can be put in by overriding the draw method here. Iv included an example of a class overriding the default draw method, the Ball class. (A quick switch between shapes will be demonstrated at the end of this tutorial.) With this, we complete the creation of the Ikshape class.

protected function draw():void 
{
	var col:Number = 0x00FF00;
	var w:Number = 50;
	var h:Number = 10;
	graphics.beginFill(col);
	graphics.drawRect(-w/2, -h/2, w, h);
	graphics.endFill();
}

Step 5: The IK Chain

IKine class implements behaviour of an IK chain. Explanation regarding this class follows this order

1. Introduction to private variables in this class.
2. Basic methods used in this class.
3. Mathematical explanation on the workings of specific functions.
4. Implementation of those specific functions.

Step 6: The Data in a Chain

Code below shows the IKine class private variables.

private var IKineChain:Vector.<IKshape>;		//members of chain

//Data structure for constraints
private var constraintDistance:Vector.<Number>;		//distance between nodes
private var constraintRangeStart:Vector.<Number>;	//start of rotational freedom
private var constraintRangeEnd:Vector.<Number>;		//end of rotational freedom

Step 7: Instantiate the Chain

IKine chain will store a Sprite datatype that remembers the relationship of its parent and child. These sprites are instances of IKshape. The resultant chain sees the root node at index 0, the next child at index 1, ... until the last child in sequential manner. However, construction of chain is not from root to last child; it is from last child to root.

Assuming that the chain is of length n, construction follows this sequence: n-th node, (n-1)-th node, (n-2)-th node ... 0-th node. The animation below depicts this sequence.

Upon instantiation of IK chain, the last node is inserted. Parent nodes will be appended later. The last node appended is the root. The code below are methods of IK chain construction, appending and removing nodes to chain.

public function IKine (lastChild:IKshape, distance:Number)
{
	//initiate all private variables
	IKineChain = new Vector.<IKshape>();
	constraintDistance = new Vector.<Number>();
	constraintRangeStart = new Vector.<Number>();
	constraintRangeEnd = new Vector.<Number>();

	//Set constraints
	this.IKineChain[0] = lastChild;
	this.constraintDistance[0] = distance;
	this.constraintRangeStart[0] = 0;
	this.constraintRangeEnd[0] = 0;
}

/*Methods to manipulate IK chain
*/
public function appendNode(nodeNext:IKshape, distance:Number = 60, angleStart:Number = -1*Math.PI, angleEnd:Number = Math.PI):void
{
	this.IKineChain.unshift(nodeNext);
	this.constraintDistance.unshift(distance);
	this.constraintRangeStart.unshift(angleStart);
	this.constraintRangeEnd.unshift(angleEnd);
}

public function removeNode(node:Number):void
{
	this.IKineChain.splice(node, 1);
	this.constraintDistance.splice(node, 1);
	this.constraintRangeStart.splice(node, 1);
	this.constraintRangeEnd.splice(node, 1);
}

Step 8: Getting Chain Nodes

These following methods are used to retrieve nodes from the chain whenever there's a need.

public function getRootNode():IKshape 
{
	return this.IKineChain[0];
}

public function getLastNode():IKshape 
{
	return this.IKineChain[IKineChain.length - 1];
}

public function getNode(node:Number):IKshape
{
	return this.IKineChain[node];
}

Step 9: Constraints

We have seen how the chain of nodes is being represented in an array: Root node at index 0, ... (n-1)-th node at index (n-2), n-th node at index (n-1), n being length of chain. We can conveniently arrange our constraints in such order as well. Constraints come in two forms: distance between nodes and degree of bending freedom between nodes.

Distance to maintain between nodes is recognised as a child node's constraint upon its parent. For the sake of referencing convenience, we can store this value as constraintDistance array with index similar to that of the child node's. Note that the root node has no parent. However, distance constraint should be registered upon appending the root node so that if the chain is extended later, the newly-appended "parent" of this root node can utilise its data.

Next, the angle of bending for a parent node is restricted to a range. We shall store the start and end point for range in constraintRangeStart and ConstraintRangeEnd array. Figure below shows a child node in green and two parent nodes in blue. Only the node marked "OK" is allowed because it lies within the angle constraint. We can use similar approach in referencing values in these arrays. Note again that root node's angle constraints should be registered even though not in use due to similar rationale as previous. Plus, angle constraints does not apply to the last child because we want flexibility in control.

Step 10: Constraints: Getting and Setting

The methods as follow may prove useful when you have initiated constraints on a node but would like to alter the value in future.

/*Manipulating corresponding constraints
*/
public function getDistance(node:Number):Number
{
	return this.constraintDistance[node];
}

public function setDistance(newDistance:Number, node:Number):void
{
	this.constraintDistance[node] = newDistance;
}

public function getAngleStart(node:Number):Number
{
	return this.constraintRangeStart[node];
}

public function setAngleStart(newAngleStart:Number, node:Number):void
{
	this.constraintRangeStart[node] = newAngleStart;
}

public function getAngleRange(node:Number):Number
{
	return this.constraintRangeEnd[node];
}

public function setAngleRange(newAngleRange:Number, node:Number):void
{
	this.constraintRangeEnd[node] = newAngleRange;
}

Step 11: Length Constraint, Concept

 The following animation shows the calculation of length constraint.

Step 12: Length Constraint, Formula

In this step, we'll have a look at commands in a method that help to constrain distance between nodes. Note the highlighted lines. You may notice only the last child is applied this constraint. Well, as far as the command goes, this is true. Parent nodes are required to fulfill not only length but angle constraints. All these are handled with the implementation of method vecWithinRange(). Last child need not be constrained in angle because we need maximum bend flexibility.

private function updateParentPosition():void
{
	for (var i:uint = IKineChain.length - 1; i > 0; i--)
	{
		IKineChain[i].calcVec2Parent();
		var vec:Vector2D;

		//handling the last child
		if ( i == IKineChain.length - 1) 
		{
			var ang:Number = IKineChain[i].getAng2Parent();
			vec = new Vector2D(0, 0);
			vec.redefine(this.constraintDistance[IKineChain.length - 1], ang);
		}
		else
		{
			vec = this.vecWithinRange(i);
		}

		IKineChain[i].setVec2Parent(vec);
		IKineChain[i].IKparent.x = IKineChain[i].x + IKineChain[i].getVec2Parent().x;
		IKineChain[i].IKparent.y = IKineChain[i].y + IKineChain[i].getVec2Parent().y;
	}
}

Step 13: Angle Constraint, Concept

First, we calculate the current angle sandwiched between the two vectors, vec1 and vec2. If the angle is not within the constrained range, assign the minimum or maximum limit to the angle. Once an angle is defined, we can calculate a vector that is rotated from vec1 together with the constraint of distance (magnitude).

 The following animation offers another alternative to visualising the idea.

Step 14: Angle Constraint, Formula

The implementation of the angle constraints is as below.

private function vecWithinRange(currentNode:Number):Vector2D
{
	//getting the appropriate vectors
	var child2Me:Vector2D = IKineChain[currentNode].IKchild.getVec2Parent();
	var me2Parent:Vector2D = IKineChain[currentNode].getVec2Parent();

	//Implement angle bounds limitation
	var currentAng:Number = child2Me.angleBetween(me2Parent);
	var currentStart:Number = this.constraintRangeStart[currentNode];
	var currentEnd:Number = this.constraintRangeEnd[currentNode];
	var limitedAng:Number = Math2.implementBound(currentStart, currentEnd, currentAng);

	//Implement distance limitation
	child2Me.setMagnitude(this.constraintDistance[currentNode]);
	child2Me.rotate(limitedAng);

	return child2Me
}

Step 15: Angle with Directions

Perhaps it is worthy to go through here the idea of getting an angle that interprets clockwise and counter-clockwise direction. The angle sandwiched between two vectors, say vec1 and vec2, can be easily obtained from the dot product of those two vectors. The output will be the shortest angle to rotate vec1 to vec2. However, there is no notion of direction as the answer is always positive. Therefore modification on the regular output should be done. Before outputing the angle, I used vector product between vec1 and vec2 to determine whether the current sequence is positive or negative rotation and incorporated the sign into the angle. I have highlighted the directional feature in lines of code below.

public function vectorProduct(vec2:Vector2D):Number 
{
	return this.vec_x * vec2.y - this.vec_y * vec2.x;
}

public function angleBetween(vec2:Vector2D):Number
{ 
	var angle:Number = Math.acos(this.normalise().dotProduct(vec2.normalise()));

	var vec1:Vector2D = this.duplicate();
	if (vec1.vectorProduct(vec2) < 0)
	{
		angle *= -1;
	}

	return angle;
}

Step 16: Orienting Nodes

Nodes that are boxes needs to be oriented to the direction of their vectors so that they look nice. Otherwise, you will see a chain like below. (Use the arrow keys to move.)

The function below implements the right orientation of nodes.

private function updateOrientation():void
{
	for (var i:uint = 0; i < IKineChain.length - 1; i++)
	{
		var orientation:Number = IKineChain[i].IKchild.getVec2Parent().getAngle();

		IKineChain[i].rotation = Math2.degreeOf(orientation);
	}
}

Step 17: Last Bit

Now that everything is set, we can animate our chain using animate(). This is a composite function making calls to updateParentPosition() and updateOrientation(). However before that can be achieved, we have to update relationships on all nodes. We make a call to updateRelationships(). Again, updateRelationships() is a composite function making calls to defineParent() and defineChild(). This is done once and whenever there's a change in the chain structure, eg nodes are added or dropped at runtime.

Step 18: Essential Methods in IKine

In order to make IKine class work for you, these are the few methods you should look into. I've documented them in a table form.

Note that angle inputs are in radians not degrees.

Step 19: Creating a Snake

Now lets create a project in FlashDevelop. In the src folder you will see Main.as. This is the sequence of tasks you should do:

1. Initiate copies of IKshape or classes that extend from IKshape on the stage.
2. Initiate IKine and use it to chain up copies of IKshape on stage.
3. Update relationships on all nodes in chain.
4. Implement user controls.
5. Animate!

Step 20: Draw Objects

Object is drawn as we construct IKshape. This is done in a loop. Note if you'd like to change the outlook of the drawing to a circle, enable comment on line 56 and disable comment on line 57. (You'll need to download my source files in order for this to work.)

private function drawObjects():void 
{
	for (var i:uint = 0; i < totalNodes; i++) {

	var currentObj:IKshape = new IKshape();
	//var currentObj:Ball = new Ball();
	currentObj.name = &quot;b&quot; + i;
	addChild(currentObj);
	}
}

Step 21: Initialise Chain

Before initialising the IKine class to construct the chain, private variables of Main.as are created.

private var currentChain:IKine;
private var lastNode:IKshape;
private var totalNodes:uint = 10;

For the case here, all nodes are constrained to a distance of 40 between nodes.

private function initChain():void 
{
	this.lastNode = this.getChildByName("b" + (totalNodes - 1)) as IKshape;
	currentChain = new IKine(lastNode, 40);

	for (var i:uint = 2; i <= totalNodes; i++)
	{
		currentChain.appendNode(this.getChildByName(&quot;b&quot; + (totalNodes - i)) as IKshape, 40, Math2.radianOf(-30), Math2.radianOf(30));
	}
	currentChain.updateRelationships();
	
	//center snake on the stage.
	currentChain.getLastNode().x = stage.stageWidth / 2;
	currentChain.getLastNode().y = stage.stageHeight /2
}

Step 22: Add Keyboard Controls

Next, we declare variables to be utilised by our keyboard control.

private var leadingVec:Vector2D;
private var currentMagnitude:Number = 0;
private var currentAngle:Number = 0;

private var increaseAng:Number = 5;
private var increaseMag:Number = 1;
private var decreaseMag:Number = 0.8;
private var capMag:Number = 10;

private var pressedUp:Boolean = false;
private var pressedLeft:Boolean = false;
private var pressedRight:Boolean = false;

Attach onto stage the main loop and keyboard listeners. I've highlighted them.

private function init(e:Event = null):void 
{
	removeEventListener(Event.ADDED_TO_STAGE, init);
	// entry point

	this.drawObjects();
	this.initChain();
	leadingVec = new Vector2D(0, 0);

	stage.addEventListener(Event.ENTER_FRAME, handleEnterFrame);
	stage.addEventListener(KeyboardEvent.KEY_DOWN, handleKeyDown);
	stage.addEventListener(KeyboardEvent.KEY_UP, handleKeyUp);
}

Write the listeners.

private function handleEnterFrame(e:Event):void 
{
	if (pressedUp == true)
	{
		currentMagnitude += increaseMag;
		currentMagnitude = Math.min(currentMagnitude, capMag);
	}
	else
	{
		currentMagnitude *= decreaseMag;
	}

	if (pressedLeft == true) 
	{
		currentAngle -= Math2.radianOf(increaseAng);
	}

	if (pressedRight == true) 
	{
	currentAngle += Math2.radianOf(increaseAng);
	}

	leadingVec.redefine(currentMagnitude, currentAngle);
	var futureX:Number = leadingVec.x + lastNode.x;
	var futureY:Number = leadingVec.y + lastNode.y;

	futureX = Math2.implementBound(0, stage.stageWidth, futureX);
	futureY = Math2.implementBound(0, stage.stageHeight, futureY);

	lastNode.x = futureX;
	lastNode.y = futureY;
	lastNode.rotation = Math2.degreeOf(leadingVec.getAngle());

	currentChain.animate();
}

private function handleKeyDown(e:KeyboardEvent):void 
{
	if (e.keyCode == Keyboard.UP)
	{
		pressedUp = true;
	}

	if (e.keyCode == Keyboard.LEFT)
	{
		pressedLeft = true;
	}
	else if (e.keyCode == Keyboard.RIGHT)
	{
		pressedRight = true;
	}
}

private function handleKeyUp(e:KeyboardEvent):void 
{
	if (e.keyCode == Keyboard.UP)
	{
		pressedUp = false;
	}
	
	if (e.keyCode == Keyboard.LEFT)
	{
		pressedLeft = false;
	}
	else if (e.keyCode == Keyboard.RIGHT)
	{
		pressedRight = false;
	}
}

Notice that I've used a Vector2D instance to lead the snake moving around the stage. I've also constrained this vector within the boundary of stage so it will not move out. The Actionscript performing this constraint is highlighted.

Step 23: Animate!

Press Ctrl+Enter to see your snake animate!. Control its movement using the arrow keys.

Conclusion

This tutorial does require some knowledge in Vector Analysis. For readers who would like to get a familiar look at vectors, do have a read on the post by Daniel Sidhon. Hope this helps you in understanding and implementing inverse kinematics. Thanks for the read. Do drop suggestions and comments as Im always eager to hear from audiences. Terima Kasih.