Simulator 6 Axis Articulated Robots

• Download Robot_Simulator-noexe.zip - 2.2 MB
• Download Robot_Simulator.zip - 2.4 MB

• Introduction
• Robot type
• Problem Type(important to read)
• Direct Problem
• Inverse Problem
• The Simulator

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: http://maxima.sourceforge.net/. 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.

Exists two big family of robots. Parallel also called close cinematic http://en.wikipedia.org/wiki/Parallel_manipulator, the have small room of operation but can reach very high speed and force. Open cinematic or serial http://en.wikipedia.org/wiki/Robotic_arm#Types, 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:

First you must know what are the rotation matrix. These are the basic rotation matrix, http://en.wikipedia.org/wiki/Rotation_matrix#Basic_rotations. 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. http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters

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.

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

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):

 T0:matrix([1,0,0,0],[0,1,0,0],[0,0,1,H],[0,0,0,1]);
Rz1:matrix([cos(t1),-sin(t1),0,0],[sin(t1),cos(t1),0,0],[0,0,1,0],[0,0,0,1]);
T1x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T1:matrix([1,0,0,a1],[0,1,0,0],[0,0,1,d1],[0,0,0,1]);
Rpx1:matrix([1,0,0,0],[0,0,-1,0],[0,1,0,0],[0,0,0,1]);
Rz2:matrix([cos(t2),-sin(t2),0,0],[sin(t2),cos(t2),0,0],[0,0,1,0],[0,0,0,1]);
T2x:matrix([1,0,0, _x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T2:matrix([1,0,0,a2],[0,1,0,0],[0,0,1,0],[0,0,0,1]);
Rz3:matrix([cos(t3),-sin(t3),0,0],[sin(t3),cos(t3),0,0],[0,0,1,0],[0,0,0,1]);
T3x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T3:matrix([1,0,0,a3],[0,1,0,0],[0,0,1,0],[0,0,0,1]);
Rpy3:matrix([0,0,1,0],[0,1,0,0],[-1,0,0,0],[0,0,0,1]);
Rz4:matrix([cos(t4),-sin(t4),0,0],[sin(t4),cos(t4),0,0],[0,0,1,0],[0,0,0,1]);
T4x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);
T4:matrix([1,0,0,0],[0,1,0,0],[0,0,1,d4],[0,0,0,1]);
Rpy4:matrix([0,0,-1,0],[0,1,0,0],[1,0,0,0],[0,0,0,1]);
Rz5:matrix([cos(t5),-sin(t5),0,0],[sin(t5),cos(t5),0,0],[0,0,1,0],[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]);
Rpy6:matrix([0,0,1,0],[0,1,0,0],[-1,0,0,0],[0,0,0,1]);
R6z:matrix([cos(t6),-sin(t6),0,0],[sin(t6),cos(t6),0,0],[0,0,1,0],[0,0,0,1]);
T6x:matrix([1,0,0,_x],[0,1,0,_y],[0,0,1,_z],[0,0,0,1]);

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

/*1-2*/
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*/

/*2-3*/
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)],
[sin(t1)*cos(t2+t3),-sin(t1)*sin(t2+t3),-cos(t1),a2*sin(t1)*cos(t2)+a1*sin(t1)],[sin(t2+t3),cos(t2+t3),0,H+a2*sin(t2)+d1],[0,0,0,1]);

/*3-4*/
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*/


/*4-5*/
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*/

/*5-6*/
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*/
/*end-effector*/
M6x:M6.T6x; 

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*/

Ry:matrix([cos(b),0,sin(b)],[0,1,0],[-sin(b),0,cos(b)]);
Rz:matrix([cos(a),-sin(a),0],[sin(a),cos(a),0],[0,0,1]);
Rxx:matrix([1,0,0],[0,cos(c),-sin(c)],[0,sin(c),cos(c)]);
Ryp_1:Ry.Rxx;
Ryp:Rz.Ryp_1; /* this is our Roll-Pitch-Yaw Matrix*/

 
ex:transpose(matrix([1,0,0]));
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*/

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

    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*/    
 // http://www.rob.uni-luebeck.de/Lehre/2008w/Robotik/Vorlesung/Robotik1VL5_1_vers1.pdf  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;