# Robot's inverse kinematics

## 1. Introduction

Project shown here is a complete solution of robot’s inverse kinematics. Subject of the project is an example robot. To make possible calculation of inverse kinematics, firstly, the forward kinematics must be calculated. Additionally, beside forward and inverse kinematics of robot, there are drawn trajectories of robot’ motion. Trajectory was calculated in MATLAB environment. A complete source code for simulation in MATLAB is placed in this project.
Known: θi(t), di(t)
Unknown: x0(t), y0(t), z0(t)
In brief forward kinematics can be presented in one line by notation:
i(t), di(t)} → {x0(t), y0(t), z0(t)}

### 1.1. Forward kinematics of robot – purpose

Forward kinematics can be described in brief as a case when angles and shifts of robot’s elements are known. As a result of forward kinematics we want to obtain position defined in the last coordinates system (tool coordinate system) in the base coordinates system.

### 1.1. Inverse kinematics of robot – purpose

Inverse kinematics can be described in brief as a case when position of tool in the base coordinate system is known and as a result, angles and shifts of robot’s elements must be calculated. Known: x0(t), y0(t), z0(t)
Unknown: θi(t), di(t)
In brief inverse kinematics can be presented in one line by notation:
{x0(t), y0(t), z0(t)} → {θi(t), di(t)}

### 1.3. Static analysis of robot - purpose

In this project static analysis of robot is also performed. Performing of static analysis will give an answer what forces and torques have to be produced by robot’s drives to keep its construction in equilibrium during resting. Note that real robot has brakes on drives which are responsible for keeping it in balance during resting, however, knowledge about static load is important. Moreover, as a result of static analysis required strength of robot’s construction will be known. Please note that it is only static analysis. During normal work of robot dynamic loads will appear. Analysis of dynamics load is performed in another project which is only about robot’s dynamics. Project about robot’s dynamics may be found on website dynamics of robot.

## 2. Forward kinematics of robot

Forward kinematics will be solved first for an example robot.

### 2.1. Project of robot

Considered robot has four angular drives and one linear drive.

### 2.2. Denavit-Hartenberg notation

Table with Denavit-Hartenberg parameters.

### 2.3. Calculation of trajectory in MATLAB program

Trajectory was calculated in matlab program. To do this, firstly a function for calculation transformation matrixes was created. Function takes as input parameters Denavit-Hartenberg parameters.
function[T]=ma_trans(alfa,a,d,theta)
cA=cos(alfa);
sA=sin(alfa);
cQ=cos(theta);
sQ=sin(theta);
T=[ cQ -sQ 0 a ;
sQ*cA cQ*cA -sA -sA*d ;
sQ*sA cQ*sA cA cA*d ;
0 0 0 1 ] ;

Y axes of graphs, which are shown above, are described with different symbols. Following configuration variables are assigned to these symbols:
q_1(t) → θ1(t)
q_2 (t) → θ2(t)
q_3 (t) → θ3(t)
d_4 (t) → d4(t)
q_5 (t) → θ5(t)

## 3. Inverse kinematics of robot

In this elaboration robot’s inverse kinematics will be designated only to the third coordinates system. Reader may use calculations from this project to calculate inverse kinematics with consideration of all robot’s coordinates systems.
Purpose of inverse kinematics is to find angles and shifts of robot’s elements for Cartesian position which is defined in base coordinates system. As it is written above inverse kinematics will be calculated only to the third coordinates system.

### 3.1. General form of transformation matrix

Matrix i-1iT describes transformation from coordinates system „i” to coordinates system „i-1”

Abbreviated symbols of trigonometric functions are applied in matrix above.

### 3.2. Transformation matrixes for consecutive coordinates systems

Transformation matrix between coordinates systems „1” and „0”

Where: S1=sinθ1, C1=cosθ1

Transformation matrix between coordinates systems „2” and „1”

Where: S2=sinθ2, C2=cosθ2

Transformation matrix between coordinates systems „3” and „2”

Where: S3=sinθ3, C3=cosθ3

If consecutive transformation matrixes between coordinates systems are known then it is possible to calculate transformation matrix between base’s coordinates system and the last coordinates system.

After consecutive multiply operations between matrixes the result below is obtained:

Obtained matrix describes by symbolic notation rotations and shifts between third coordinates system and base’s coordinates system.

### 3.3.General form of setpoint matrix

General form of setpoint matrix.

### 3.4. Comparison of setpoint matrix and transformation matrix

In robot’s model, which is the subject of this elaboration, motion is in two dimensions. Matrix 03TD in the general case for motion in two dimensions looks as follows:

Matrix 03TD is called a setpoint matrix. Matrix 03TD has to be compared with obtained earlier matrix 03T to make possible expression angles and shifts as functions of cartesian coordinates relative to base coordinates system. Through a comparison it is understood the comparison of indexes of these matrixes and equate them to each other. As a result the following system of equations is obtained:

Two first equations are simplifying after application of trigonometric reduction formulas; thereby, as a result is obtained:

Subsequently notation of equations for X_0 and Y_0 is changed as follows:

Mentioned earlier equations are raised square root and are added by sides:

Expressions, which are inside equations, are multiplied:

Relation is used:

Substitutions are applied:

The first equation is written after taking into account substitutions:

Thus:

Where:

### 3.5. Calculation of configuration variables

Expression for θ1(t) is obtained by inserting relations, which were calculated above, into general formula:

Remaining variables are calculated in analogously way:

Designation of the second configuration variable → θ2(t)

Designation of the third configuration variable → θ3(t)

At this point, all expressions for configuration variables in function of cartesian coordinates are known. It is essential to designate domains of functions. Without any issues it is easy to note that lengths of arms have to bigger than nil.
L1>0; L2>0;L4>0
Obviously it is not an end of functions domains designations. Remaining domains designation is left to reader. Various positions of robot has to considered during designation of domains (range of angles values) because of possible collisions.