The picture bellows shows the robot arm used for this example. It has 5 degrees of freedom:

1. Base Rotate

2. Shoulder

3. Elbow

4. Wrist

5. Wrist Rotate

We will use the following standard coordinate system used in robotics:

All degrees of freedom of this type of arm act in the same vertical plane. This vertical plane can rotate around the z axis as seen in the following diagram.

We can obtain the base rotate angle (A0) as a simple arcTan function of the desired actuator target xT and yT coordinates.

A0 = arctan ( yT / xT )

This is possible with this type of robotic arm because there are no degrees of freedom that go out of the vertical plane.

Once we have calculated A0 we can calculate the value of wT by using the Pythagoras Theorem .

wT = sqrt ( sq ( xT ) + sq ( yT ))

Once we have wT we can do all calculations in the two dimensional plane wz.

Note that wrist rotate angle is not required for the calculation of the remaining angles. The wrist rotate angle will be determined by the particular need to have the gripper rotated in a certain position to grab a particular shaped object. This may or may not be relevant to all applications. If wrist rotate is important then it should be calculated after all other angles have been calculated.

The following diagram shows the robotic arm simplified to 3 degrees of freedom in the wz plane.

In most applications we will like the gripper to act on a certain fixed angle. For example to pick up a chess piece we will normally want the gripper to act vertically (-90 degrees). In order to have control over the Wrist angle and to keep simplifying the model we will use a discrete set of angles for the last link. In this case we will use 90, 45, 0, -45 or -90 degrees.

If we know the target gripper coordinates, wT and zT, and the desired angle for the actuator link (AG = GripperAngle) then we can use simple trigonometry to calculate coordinate w2, z2.

w2 = wT – L3 cos (AG)

z2 = zT – L3 sin (AG)

We now have the simplified 2 Degrees of Freedom problem shown in the following diagram. Where we know the coordinate of the end of Link 2 (w2, z2) and we know the coordinate of the beginning of Link 1 (w0, z0) and we just need to calculate the angles for both links.

A1, A2 and A3 are the three angles that we need to obtain. We can use the law of cosines to obtain angle A1-A12 as we have the length of the three triangle sides L1, L2 and

L12 (this last one obtained using Pythagoras Theorem).

L12 = sqrt ( sq ( w2 ) + sq ( z2 ))

A12 = arctan (z2 / w2)

If L12 > L1 + L2 then there is no solution as arm can not reach the target.

A1 = arccos ( ( sq ( L1 ) + sq ( L12 ) – sq ( L2)) / (2 * L1 * L12 )) + A12

with A1 calculate w1,z1

w1 = L1 cos (A1)

z1 = L1 sin (A1)

A2 = arctan ( ( z2 – z1 ) / ( w2 – w1 ) ) – A1

with A12 calculate the Angle of Link 3, A3

A3 = AG – A1 – A2

To illustrate the algorithm and to confirm that it works before implementing it on a robot arm I wrote a small program in Processing language: Inverse Kinematics Processing Simulation

Processing is an open source programming language and environment for people who want to create images, animations, and interactions.

Processing.js is the sister project of the Processing visual programming language, designed for the web. Processing.js makes your data visualizations, digital art, interactive animations, educational graphs, video games, etc. work using web standards and without any plug-ins.

Bellow is the processing code:

/*
 * Inverse_Kinematics_1 
 * Programm used to validate an inverse kinematics algorithm for a Robotic Arm
 *
 * Author: Diego Pontones
 * Date: February 17, 2011
 * Copyright: Diego Pontones
 * email:
 */

int nLinks = 3;
int nGripperAngles = 5;
int GripperAngle[] = {0, -45, -90, 45, 90};
int linkStrokeW[] = {28, 18, 12};
int linkColor[] = {#00D000, #0000FF, #FF0000};
int l[] = {0, 100, 100, 60};
int h0 = 180; //Arm Origen in Screen Coordinates
int v0 = 360; //Arm Origen in Screen Coordinates
int currGripper = 0;

float[] w = new float[nLinks]; //horizontal coordinate, corresponds to P0, P1, P2 etc
float[] z = new float[nLinks]; //vertical coordinate
float[] a = new float[nLinks]; //angle for the link, reference is previous link
float tw, tz; //target coordinate for the actuator (gripper)
float tw0, tz0; //Previous target coordinate for the actuator (gripper) that was inside the robot reach
float l12;
float a12;

void setup() {
 size(480, 560); //drawing window size
 smooth(); 
 w[0] = 0; // Set Origen
 z[0] = 0; // Set Origen
 tw0=100; // Initiakl Target
 tz0=100;
}

void draw() {
 background(240);
 checkMouseInMenu(mouseX, mouseY);
 drawGripperMenu();
 drawAxes();
 tw = mouseX - h0;
 tz = v0 - mouseY ;
 calcP2();
 if (l12>l[1]+l[2]){
 textAlign(CENTER); 
 fill(#FF0000);
 text("No Solution",240,70);
 text("Move mouse pointer towards the origin.",240,84);
 tw=tw0;
 tz=tz0;
 calcP2();
 }
 calcP1();
 drawLinks();
 tw0=tw;
 tz0=tz;
}

void drawAxes(){
 strokeWeight(1);
 stroke(0);
 line(h0, v0, h0+259,v0);
 line(h0, v0, h0,v0-259);
}

void checkMouseInMenu(float hin, float vin){
 int H = 120;
 if((vin>436) && (vin<454)) { 
 for(int i=0; i<nGripperAngles; i++) { 
 if((hin>H-20) && (hin<H+20)) {
 currGripper=i;
 }
 H=H+44; 
 }
 }
}

void drawGripperMenu() {
 textAlign(CENTER);
 text("Robot Arm Inverse Kinematics Algorithm Simulation",250,34);
 text("www.maquinapensante.com",250,50);
 int H = 60;
 int V = 424;
 fill(0);
 textAlign(LEFT); 
 text("To change gripper angle roll mouse over new value",H,V);
 textAlign(CENTER);
 H = 120;
 V = 450;
 for(int i=0; i<nGripperAngles; i++) { 
 if(i == currGripper) {
 fill(0);
 }else {
 fill(180); 
 }
 text(GripperAngle[i], H, V);
 H = H+44; 
 }
}

void calcP2(){
 w[2]=tw-cos(radians(GripperAngle[currGripper]))*l[3];
 z[2]=tz-sin(radians(GripperAngle[currGripper]))*l[3];
 l12 = sqrt(sq(w[2])+sq(z[2]));
}

void calcP1(){
 a12=atan2(z[2],w[2]);
 a[1]=acos((sq(l[1])+sq(l12)-sq(l[2]))/(2*l[1]*l12))+a12;
 w[1]=cos(a[1])*l[1];
 z[1]=sin(a[1])*l[1];
}

void drawLinks(){
 for(int i=0; i<2; i++) { 
 strokeWeight(linkStrokeW[i]);
 stroke(linkColor[i]);
 line(h0+w[i], v0-z[i],h0+w[i+1], v0-z[i+1]);
 }
 strokeWeight(linkStrokeW[2]);
 stroke(linkColor[2]);
 line(h0+w[2], v0-z[2],h0+ tw,v0- tz);
 fill(0);
 strokeWeight(0);
 stroke(0);
 ellipse(h0+ tw,v0- tz,10,10);
}

As can be seen the algorithm successfully calculates the angles for the joints however the following needs to be considered when implementing it:

• As mentioned before the wrist rotate angle would have to be calculated if required in order to compensate for base rotation and for wrist (L3) angle.

• Need to check and limit the angle values so that the arm segments and the gripper do no not hit the surface or other segments.

• For example for a gripper orientation (AG) of -90 degrees the wrist rotation angle will be a simple function of the base rotate angle (A0).