Click here to Skip to main content
15,943,477 members
Articles / General Programming / Algorithms

Simulator 6 Axis Articulated Robots

Rate me:
Please Sign up or sign in to vote.
5.00/5 (21 votes)
7 Apr 2014CPOL10 min read 59.2K   6K   35   7
A small simulator for a 6 axis articulated robot


I was looking for some application to do with controller, it looks like that one of the dream jobs is the robot.

So I decide to write a small article about robots. "robots for all".

Work with robots, is interesting, but is not easy. To make an exhaustive explanation, I should write a book. I try to explain the logic, but I must give you a lot of link to study in deepen the arguments.There is a lot of calculation to do. But we can utilize a program of Computer Algebra System. I utilize Maxima: So we can concentrate our effort on the program, and make easily each kind of try. When you will copy from maxima, it is already C/C++ formatted. So if you give the right name of the variables, copy paste becomes very easy.

Image 1

Robot type

Exists two big family of robots. Parallel also called close cinematic, the have small room of operation but can reach very high speed and force. Open cinematic or serial, such the cartesian, the scara, or our articled arm.

The parallel is heavier to treat, because we can not generalize, and must studied time for time. Meanwhile with the open cinematic, we can track a general road.

This is our champion:

Image 2

Problem Type

The problem in the robot kinematic are two:

  • Direct kinematic
  • Inverse kinematic

The direct problem is retrieve the position of the end-effector (the last arm, with your tool to keep, weld, paint etc.), given the constraint of the motors.

The inverse problem is retrieve the constraints of the motors know the position and orientation in the space. This is, I presume, the normal situation when we work. "The piece is there, we must bring the robot there" But is also hard to solve. And not ever is it possible, analytically. There is some particular configuration that allow us to solve it, ideal for small micro-controller, with small calculation power. Our configuration is an "anthropomorphic arm" that allow us to solve analytically.

Direct Problem

For the direct problem, we can just utilize matrix. It looks hard, but after understood the logic it is not impossible. It is just an algorithm to learn.

First you must know what are the rotation matrix. These are the basic rotation matrix, Our system matrix is obtained with opportune multiplication of these matrix.

We need to define an common notation. The notation normally in use, is the Denavit-Hartenberg-notation.

These notation defines the Denavit-Hartenberg parameters. (these are simplistic definitions, refer to the link for deepen ones) :

  • the z-axis is direct along the axis of the joint. Important the z-axis is the axis of rotation of the motor. Translations in z-axis are called dn
  • the x-axis points to the next motor, if z-axis points otherwise. Translations in x are called an

Practically we choose the coordinate system so that for describe the whole robot, we move just along z or x axis. The y-axis does not matter.

The Denavit-Hartenberg matrix. is a 4x4 Matrix. The minor 3x3 of the matrix, is a normal rotation matrix, the 4th column is the translation matrix, and the 4th row, is filled with zeroes and one to obtain a square matrix.

Image 3

Important the translation column gives instantly the position (x,y,z) in the space, without any other calculation to do

So we begin:

  • We choose an absolute coordinate system. For convenience we choose, ground level, with z-axis parallel to the first motor(better say joint.Actually with gears, or other technical solution, the motor normally is not aligned with the rotation axis. But for explain we say motors)
  • From ground, we translate along z-axis, for h mm (or inches). Matrix T0
  • At h there is a driven rotational joint. We express the angle as variable t1. Matrix RZ1
  • If we multiply: M1=T0*Rz1, we obtain the transformation matrix, which given the angles give us back the position and orientation of the coordinate system "attached" to the top of the shaft of the motor.
  • Moving ahead the second joint, we translate d1 mm along z and a1 mm along x. Matrix T1.
  • We must rotate our coordinate system so that our z-axis is parallel to the point of rotation of the joint, and the x-axis point to the third joint. To do this, it is enough a rotation of 90°(pi/2) degrees around the x-axis. This is not a variable, but a constant,after the multiplication "disappears". Matrix Rpx1
  • Now we can introduce the angle of the motor. Totally similar to the first motor, but this time the variable is called t2 Matrix RZ2
  • The matrix M2(attached at the top of the shaft of motor 2) is given by multiplication from the first matrix M2=M1*T1*Rpx1*Rz2
  • ... and now go on until the last arm

Image 4

If you look the 4th column, it gives you exactly the position of the top of the motor in our absolute coordinate system. (the second motor has still no effect for the position)

Here the set of instructions for Maxima. The first set of instructions are the definition of the matrix (they can be run at once), the second are their multiplication (to run arm for arm, the comment mess the syntax, or delete the void row with comments):

T2x:matrix([1,0,0, _x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T5x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0 ,0,0,1]);
T5:matrix([1,0,0,a5],[0,1,0,0],[0,0,1,0],[0 ,0,0,1]);

M1:T0.Rz1;/* position of coordinate system first motor*/

M1x:M1.T1x;/* Draws 1st  Arm*/
M1_1:M1.T1;/* moves to second joint*/
M1_2:M1_1.Rpx1; /*aligns the coordinate system to the next motor */
M2:M1_2.Rz2;/* coordinate system second motor*/

M2x:M2.T2x; /* Draws 2nd Arm*/
M2_1:M2.T2; /* translates to third joint*/
M3:M2_1.Rz3;/* coordinates system 3rd motor*/
M3:matrix([cos(t1)*cos(t2+t3), -cos(t1)*sin(t2+t3),sin(t1), a2*cos(t1)*cos(t2)+a1*cos(t1)],

M3x:M3.T3x;/* Draws 3rd Arm*/
M3_1:M3.T3;/* translates to 4th joint*/
M3_2:M3_1.Rpy3;/*aligns the coordinate system to the next motor */
M4:M3_2.Rz4;/* coordinates system 4th motor*/

M4x:M4.T4x;/* draws  4th arm*/
M4_1:M4.T4;/* translates to 5th joint*/
M4_2:M4_1.Rpy4;/*aligns the coordinate system to the next motor*/
M5:M4_2.Rz5;/* coordinates system 5th motor*/

M5x:M5.T5x;/* draws  5th arm*/
M5_1:M5.T5;/* translates to 6th joint*/
M5_2:M5_1.Rpy6;/*aligns the coordinate system to the next joint*/
M6:M5_2.R6z;/* coordinate system 6th motor*/

If you look, I redefine the matrix M3 after his calculation, to allow us to introduce some important simplifications, that allow us the reduce the amount of sinus and co-sinus (at end become huge)

The matrix Tnx, are the matrix with generic variables( _x,_y,_z), these matrix allow us to draw points attached in the system of coordinates of the motor. Practically allow us the draw the arm, united at the motor. Therefore in the multiplication: Mnx:Mn.Tnx; there is the comment /*draws the n-th arm*/

Inverse Problem

This was the easy part. I know, it looks hard, but is just an algorithm. After you understand how it works, comes automatically.

The inverse problem, you can not solve with algorithm but you have to think about to find an analytically solution. Not ever possible. Sometimes you must solve with numerical analysis, such Newton, depend from your hardware configuration.

Where do you wanna go?

This is a 6-free degree robot. You must give him 6 parameters. The first three, easy, are ( x,y,z). The other three are the orientation. But what you mean with these three numbers?

There are several ways to define an orientation in the space:

The most used for these kind of application are the Euler angles(citation needed). Because follows the actual configuration of the wrist (the last three arms, are called wrist)

I used in this application Roll-Pitch-Yaw. For two reasons: first it is user friendly. It is more immediately, such an air plane, you have three numbers, and approximately you understand the asset of the plane. Second to try something different. Of Euler Angles inverse kinematic internet is full, with a fast research you will find may be also the routine.


This kind of geometry, allow us to divide the problem in two parts. The first three arms, are responsible of the positioning of the wrist:

Image 5

We know where the wrist must be. Because is our desired target, and we can make some geometrical considerations: the motor 4th, does not give translation contributes, it is a whole piece with the arm3 it can give just rotational contributes. The same does the motor-6. So if we multiply the vector parallel to the x-axis of the motor 5 by our RPY matrix, we can retrieve the absolute position of the wrist.

Ryp:Rz.Ryp_1; /* this is our Roll-Pitch-Yaw Matrix*/

N:Ryp.ex;/* this is the vector of motor 5, what interests us is the x-component oriented to the top of end effector*/
Pw:(a5+d6)*N;/* now we multiply his value for the effective distance, gives as sum of the distance from motor 5 to motor 6 + distance from motor 6 to end effector*/

This operation gives us the follow wrist position (pWx,pWy,pWz)

pWx = x  - cos(a)*cos(b)*(d6 + a5);
pWy = y - sin(a)*cos(b)*(d6 + a5);
pWz = z + sin(b)*(d6 + a5);

Where x,y,z,a,b are our desired target point.

Due the geometries of our robot, we can determine instantly, with normal trigonometric considerations, the first three angles:

    p->t1 = atan2(pWy, pWx);/*the rotation of the first arm, is given by the atan2 of the y and x coordinate of the wrist */
    /*we calculate the position of the joint 2, because this is a vertex of the triangle*/
    px = pWx -a1 * cos(p->t1);
    py = pWy - a1 * sin(p->t1);
    pz = pWz - d1 - h;
    /*we apply the theorem of cousins to define the anlge of the motor 3*/
    c3 = (px*px + py*py + pz*pz - pow(a3 + d4, 2) - a2*a2) / (2 * (a3 + d4) * a2);
    s3 =- sqrt(1 - c3*c3);
    p->t3 = atan2(s3, c3);

/*with similar consideration we find the angle of the 2nd motor*/    
 //  pag. 50
        xi = atan2(pz, sg*sqrt(px *px + py*py));// sg is my addition
        fi = atan2((a3 + d4) * s3, ( a2 + (a3 + d4) * c3));

        p->t2 = xi -  fi;

The function atan2, already included in C++, (I included it also in my code, but quoted) It is an atan, defined in the whole interval 2*pi, :

The solution of the last three angles. Must be done with the matrix:

Image 6

The rotation of the first three motors, will also give an inevitable contribute to the final orientation, therefore we must find which is the actual orientation of the wrist, and from this position set the angles of the last three motors consequently. Small note about the follow notation with the name Ri-j, I indicate the rotation matrix from i to j. The unknown is R3-6 , and we must find a relation between this matrix and some known terms. The matrix R0-6, is known. Is our target. Our desired point.But expressed in matrix form via our Roll-Pitch-Yawn matrix. And also R0-3, is known, we already calculated this angles.


This article, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

Written By
Italy Italy
This member has not yet provided a Biography. Assume it's interesting and varied, and probably something to do with programming.

Comments and Discussions

Questionimplementation d3 Pin
MBinnemann8-Aug-16 21:22
MBinnemann8-Aug-16 21:22 
Questiondoubt about "a" and "b" Pin
Guilherme Albrecht Gazzo4-Feb-16 9:03
Guilherme Albrecht Gazzo4-Feb-16 9:03 
GeneralMy vote of 5 Pin
Member 1134126729-Dec-14 20:01
Member 1134126729-Dec-14 20:01 
GeneralNice Pin
basilvde23-Apr-14 3:25
basilvde23-Apr-14 3:25 
GeneralRe: Nice Pin
jurhas23-Apr-14 20:24
jurhas23-Apr-14 20:24 
GeneralMy vote of 5 Pin
Joppe8914-Apr-14 11:44
professionalJoppe8914-Apr-14 11:44 
GeneralRe: My vote of 5 Pin
jurhas15-Apr-14 20:22
jurhas15-Apr-14 20:22 

General General    News News    Suggestion Suggestion    Question Question    Bug Bug    Answer Answer    Joke Joke    Praise Praise    Rant Rant    Admin Admin   

Use Ctrl+Left/Right to switch messages, Ctrl+Up/Down to switch threads, Ctrl+Shift+Left/Right to switch pages.