An example could be the simulation of a robotic arm with the XNA framework. This chapter should worry more about the mathematical background, whereas the chapter Character Animation will deal more with the models coming from 3D modellers.

If you want to move an arm of robotics or animated characters to a certain direction this entity is mostly modeled as a rigid multibody system consisting of a set of rigid objects which are called links. These links are connected by joints. To control the movement of this rigid multibody and get it into the destined direction inverse kinematics is often used.

The goal of inverse kinematics is to place each joint at its target. For that the right settings for the joint angles need to be found. The angles are represented by a vector [1].

Inverse kinematics is very challenging since there may be several possible solutions for the angle or none. In case of a solution complex and expensive computations could be required to find it [2]. Many different approaches for solving that problem exist:

Jacobian transpose method

Pseudoinverse method

Damped Least Squares (DLS)

Selectively Damped Least Square (SDLS)

Cyclic Coordinate Descent

It is a big effort to implement the Jacobian based methods because they require enormous mathematical knowledge and many prerequisites like classes for matrices with m columns and n rows or singular value decomposition. An Example for implementation can be found here. It was created by Samuel R. Buss and Jin-Su Kim.

All methods mentioned above except the Cyclic Coordinate Descent are based upon the Jacobian matrix which is a function of the joint angle values and used to determine the end position. They discuss the question of how to choose the angle. The values of the angles need to be altered until a value approximately equal to the target value is reached.

Updating the values of the joint angles can be used in two ways:

1) Each step perform a single update of the angle values (using equation) so that joint follows the target position.
2) The angles are updated iteratively until it is close to a solution [1]

The Jacobian can only be used as an approximation near a position. The process of calculating the Jacobian must therefore be repeated in small steps until the desired end position is reached.

Pseudo Code:

while (e is too far from g) {

Compute J(e,Φ) for the current pose Φ

Compute J-1 // invert the Jacobian matrix

Δe = β(g - e) // pick approximate step to take

ΔΦ = J-1 • Δe // compute change in joint DOFs

Φ = Φ + ΔΦ // apply change to DOFs

Compute new e vector // apply forward kinematics to see where we ended up

The idea of the Jacobian transpose method is to update the angles by equation using the transpose instead of the inverse or pseudoinverse (since an inversion is not always possible)[1] . With this method the change to an angle can be computed directly by looping through it. It avoids expensive inversion and singularity problems but converges towards a solution very slowly. The motion of this method closely matches the physics unlike other inverse kinematics solutions which can result in an unnatural motion [3].

This method sets the angle values to the pseudoinverse of the Jacobian. It tries to find a matrix which effectively inverts a non-square matrix. It has singularity issues which tend to the fact that certain directions are not reachable. The problem is that the method first loops through all angles and then needs to compute and store the Jacobian, pseudoinvert it, calculate the changes in the angle and last apply the changes [4].

This method avoids certain problems of the pseudoinverse method. It finds the value of the angle that minimizes the quantity rather than just the one finding the minimum vector. The damping constant must be chosen carefully to make the equation stable [1].

The algorithms based on the inverse Jacobian Matrix are sometimes unstable and fail to converge. Therefore another method exists. The Cyclic Coordinate Descent adjusts one joint angle at a time. It starts at the last link in the chain and works backwards iteratively through all of the adjustable angles until the desired position is reached or the loop has repeated a set number of times. The algorithm uses two vectors to determine the angle in order to rotate the model to the desired spot. This is solved by the inverse cosine of the dot product. Additionally, to define the rotation direction the cross product is used [5]. A concept demonstration of the method can be watched here

Here is a sample implementation:

First we need an object that represents a joint.

usingSystem;usingSystem.Collections.Generic;usingSystem.Linq;usingSystem.Text;usingMicrosoft.Xna.Framework;usingMicrosoft.Xna.Framework.Graphics;namespaceInverseKinematics{/// <summary>/// Represents a chain link of the class BoneChain/// </summary>publicclassBone{/// <summary>/// the bone's appearance/// </summary>privateCuboidcuboid;/// <summary>/// the bone's last calculated angle if errors occure like not a number/// this will be used instead/// </summary>publicfloatlastAngle=0;privateVector3worldCoordinate,destination;/// <summary>/// where the bone does point at/// </summary>publicVector3Destination{get{returndestination;}set{destination=value;}}/// <summary>/// the bone's source position/// </summary>publicVector3WorldCoordinate{get{returnworldCoordinate;}set{worldCoordinate=value;}}/// <summary>/// Generates a bone by another bone's end/// </summary>/// <param name="lastBone">the bone's end for this bone's source</param>/// <param name="destination"></param>publicBone(BonelastBone,Vector3destination):this(lastBone.Effector,destination){}/// <summary>/// Generates a bone at a coordinate in /// </summary>/// <param name="worldCoordinate"></param>/// <param name="destination"></param>publicBone(Vector3worldCoordinate,Vector3destination){cuboid=newCuboid();this.worldCoordinate=worldCoordinate;this.destination=destination;}

These are the fields and constructors which we need for our bone class. The field cuboid is the 3D model which represents our bone. The destination and worldCoordinate describe the joints. The worldCoordinate shows the position of the bone. The destination is the targeted position. The first constructor contains the settings for both vectors. The second constructor takes the world position and the target position (also called end effector) and generates a new world position for the new bone from them.

/// <summary>/// calculate's the bone's appearance appropiate to its world position/// and its destination/// </summary>publicvoidUpdate(){Vector3direction=newVector3(destination.Length()/2,0,0);cuboid.Scale(newVector3(destination.Length()/2,5f,5f));cuboid.Translate(direction);cuboid.Rotate(SphereCoordinateOrientation(destination));cuboid.Translate(worldCoordinate);cuboid.Update();}

The update method scales the cuboid with the length of the destination vector with the width of 5 and depth of 5. It translates the cuboid by its half length to get the rotation pivot and rotates it by the sphere coordinate angles of the destination vector and translates it to its world coordinate.

/// <summary>/// the bone's destination is local and points to the world's destination/// so this function just subtract's the bone's world coordinate from the world's destination/// and gets the bone's local destination vector/// </summary>/// <param name="destination">The destination in the world coordinate system</param>publicvoidSetLocalDestinationbyAWorldDestination(Vector3destination){this.destination=destination-worldCoordinate;}/// <summary>/// the bone's source plus the bone's destination vector/// </summary>/// <returns></returns>publicVector3Effector{get{returnworldCoordinate+destination;}}}}

The rest of the bone class is getters and setters.

usingSystem;usingSystem.Collections.Generic;usingSystem.Linq;usingSystem.Text;usingMicrosoft.Xna.Framework.Graphics;usingMicrosoft.Xna.Framework;namespaceInverseKinematics{/// <summary>/// The BoneChain class repressents a list of bones which are always connected once./// On the one hand you can add new bones and every bone's source is the last bone's destination/// on the other hand you can use the cyclic coordinate descent to change the bones' positions. /// </summary>publicclassBoneChain{/// <summary>/// The last bone that were created/// </summary>privateBonelastBone;/// <summary>/// All the concatenated bones /// </summary>privateList<Bone>bones;/// <summary>/// Creates an empty bone chain/// Added Bones will be affected by inverse kinematics/// </summary>publicBoneChain(){this.bones=newList<Bone>();}

The BoneChain class repressents a list of bones which are always connected once. On the one hand you can add new bones and every bone's source is the last bone's destination on the other hand you can use the cyclic coordinate descent to change the bones' positions. The class works with a list which contains the bones their coordinates. The class has two modes. The first is the creation mode where one bone is created after another and they keep connected. The other mode is the CCD (described further below).

/// <summary>/// Draws all the bones in this chain/// </summary>/// <param name="device"></param>publicvoidDraw(GraphicsDevicedevice){foreach(Boneboneinbones)bone.Draw(device);}

/// <summary>/// Creates a bone/// Every bone's destination is the next bone's source /// </summary>/// <param name="v">the bone's destination</param>/// <param name="click">if true it sets the bone with its coordinate and adds the next bone</param>publicvoidCreateBone(Vector3v,boolclick){if(click){//if it is the first bone it will create the bone's source at the destination point//so it need not to start at the coordinates(0/0/0)if(bones.Count==0){lastBone=newBone(v,Vector3.Zero);bones.Add(lastBone);}else{Bonetemp=newBone(lastBone,v);bones.Add(temp);lastBone=temp;}}if(lastBone!=null){lastBone.SetLocalDestinationbyAWorldDestination(v);}}

This is the method for creating the bones (creation mode)

/// <summary>/// The Cyclic Coordinate Descent/// </summary>/// <param name="destination">Where the bones should be adjusted</param>/// <param name="gameTime"></param>publicvoidCalculateCCD(Vector3destination,GameTimegameTime){// iterating the bones reverseintindex=bones.Count-1;while(index>=0){//getting the vector between the new destination and the joint's world positionVector3jointWorldPositionToDestination=destination-bones.ElementAt(index).WorldCoordinate;//getting the vector between the end effector and the joint's world positionVector3boneWorldToEndEffector=bones.Last().Effector-bones.ElementAt(index).WorldCoordinate;//calculate the rotation axis which is the cross product of the destinationVector3cross=Vector3.Cross(jointWorldPositionToDestination,boneWorldToEndEffector);//normalizing that rotation axiscross.Normalize();//check if there occured divisions by 0 if(float.IsNaN(cross.X)||float.IsNaN(cross.Y)||float.IsNaN(cross.Z))//take a temporary vectorcross=Vector3.UnitZ;// calculate the angle between jointWorldPositionToDestination and boneWorldToEndEffector// in regard of the rotation axisfloatangle=CalculateAngle(jointWorldPositionToDestination,boneWorldToEndEffector,cross);if(float.IsNaN(angle))angle=0;//create a matrix for the roation of this bone's destinationMatrixm=Matrix.CreateFromAxisAngle(cross,angle);// rotate the destinationbones.ElementAt(index).Destination=Vector3.Transform(bones.ElementAt(index).Destination,m);// update all bones which are affected by this boneUpdateBones(index);index--;}}

This is one possible version of the CCD Algorithm.

/// <summary>/// While CalculateCCD changes the destinations of all the bones, /// every affected adjacent bone's WorldCoordinate must be updated to keep the bone chain together./// </summary>/// <param name="index">when the bones should updated, because CalculateCCD changed their destinations</param>privatevoidUpdateBones(intindex){for(intj=index;j<bones.Count-1;j++){bones.ElementAt(j+1).WorldCoordinate=(bones.ElementAt(j).Effector);}}/// <summary>/// Updates all the representation parameters for every bone /// including orienations and positionsin this bonechain /// </summary>publicvoidUpdate(){foreach(Boneboneinbones)bone.Update();}/// <summary>/// This function calculates an angle between two vectors/// the cross product which is orthogonal to the two vectors is the most common orientation vector /// for specifing the angle's direction./// </summary>/// <param name="v0">the first vector </param>/// <param name="v1">the second vector </param>/// <param name="crossProductOfV0andV1">the cross product of the first and second vector </param>/// <returns>the angle between the two vectors in radians</returns>privatefloatCalculateAngle(Vector3v0,Vector3v1,Vector3crossProductOfV0andV1){Vector3n0=Vector3.Normalize(v0);Vector3n1=Vector3.Normalize(v1);Vector3NCross=Vector3.Cross(n1,n0);NCross.Normalize();floatNDot=Vector3.Dot(n0,n1);if(float.IsNaN(NDot))NDot=0;if(NDot>1)NDot=1;if(NDot<-1)NDot=-1;floata=(float)Math.Acos(NDot);if((n0+n1).Length()<0.01f)return(float)Math.PI;returnVector3.Dot(NCross,crossProductOfV0andV1)>=0?a:-a;}}}