Katedra Robotyki i Mechatroniki, WIMIR, AGH Krakw

Modelling of Robots and Manipulators

Exercise B2

Geometrical model inverse kinematics

1. Analytical formulation of an algorithm

For the selected kinematic structure of a manipulator the following tasks are to be completed.

T1. Determine analytically relationships expressing the joint coordinates qi i=1..3 in terms of
position and orientation of the last link.
In case of rotary joints the arctan function or a pair of arcsin and arccos (unless an assumed
joint motion range enables the use of only one of arcsin or arccos) functions should be used.

The report should contain complete derivation of the relationships starting from the position
and orientation of the last link described by a proper homogeneous transformation matrix 0Te
in the following general symbolic form:

N x Ox Ax Px
N Oy Ay Py
Te = y
o

Nz Oz Az Pz

0 0 0 1
All the elements of the above matrix are numbers denoted by symbols to make the analysis
general. The matrix and the constant parameters of the geometrical model constitute the input
data for the algorithm.
Note that in case when the 3rd coordinate frame is located at the beginning of the 3rd link the
location of the tip of the 3rd link with respect to the reference fame might be determined by
the following product:

0
0
pa = 0 T 3
d 4

1

The above formula leads to 3 numbers (Cartesian coordinates pax, pay, paz) defining the
location of the tip of the 3rd link.
The manipulator geometrical model defines the way of dependence of these coordinates on
joint coordinates algebraic, usually non-linear functions f1(q1,q2,q3), f2(q1,q2,q3) and
f3(q1,q2,q3) that lead to the following set of equations:

pax = f1 (q1 , q2 , q3 )
pay = f 2 (q1 , q2 , q3 )
paz = f 3 (q1 , q2 , q3 )

1
Katedra Robotyki i Mechatroniki, WIMIR, AGH Krakw

like in the following example:

pax = d 3C1S 2
pay = d 3 S1S 2
paz = d1 + d 3C2

Solution of the above equations leads to formulas expressing joint coordinates q1, q2, q3 in
terms of the already known values of pax, pay and paz.

like in the following example:

d 3 S1S 2 pay p
= tg (1 ) = ay
d 3C1S 2 pax pax

The solutions for q1, q2 and q3 should be introduced into the algorithm.
In case of a 3 link planar manipulator one of the pax, pay and paz Cartesian coordinates equals
always 0. Then the 3rd input data must be the orientation angle of the 3rd link, and some
elements of the orientation part of the 0T3 matrix should be used to formulate the third
equation of the set.

T2. Find a number of solutions of the inverse problem for the considered example of a
manipulator. In case of the multiple solution present all the solutions graphically on the
drawing of the kinematic chain of the manipulator. Indicate the solution within and outside of
the selected joint variables motion ranges.

T3. Analyze conditions of existence of the achieved solutions to the inverse kinematics.

2. Implementation of the algorithm

For the selected kinematic structure of a manipulator the following task is to be completed.

T4. Using the formulated algorithm prepare a Matlab procedure for calculation of values of
joint coordinates basing on the position and orientation of a 3rd link defined by a
corresponding homogeneous transformation matrix.

To test the procedure, please assume some values of the joint coordinates. Calculate elements
of the corresponding homogeneous transformation matrix that expresses position and
orientation of the end-effector. Next use this matrix as the input data for the procedure that
solves the inverse kinematic problem. One of the achieved solutions should be identical with
the values of the joint coordinates that were assumed at the start of the testing procedure.

The structure of the procedure:

Direct kinematics (preparation of the input data)
1. Setting the values of joint coordinates and the geometrical model constants
2. Calculation of position and orientation of the end effector

Inverse kinematics (actual calculations)

1. Calculation of values of q1, q2 and q3 coordinates with use of the derived relationships
2. Checking if the values of the joint variables lie within their assumed ranges of motion

2
Katedra Robotyki i Mechatroniki, WIMIR, AGH Krakw
3. Checking if the values of pax, pay and paz achieved by substitution of the calculated
values of q1, q2 and q3 to formulas on pax, pay and paz are consistent with the values
obtained during preparation of the input data.

Some examples of commands useful in the prepared script are listed below.

The elements of the homogeneous transformation matrix are referred to as follows:

pwx=T0E(1,3)*pwd;
..
pax=T0E(1,4)-pwx;
..
The function atan2 is called like in the following lines:
th21=atan2((d1-paz),sqrt(paz^2+pay^2))
th22=atan2((d1-paz),-sqrt(paz^2+pay^2))

An example of a definition of the motion ranges:

z=[-pi/2,-pi/2,0.5;pi/2,pi/3,1;0.1,0.5]

Creation of vectors t1, t2 i t3 containing various sets of solutions:

t1(1)=th11
t1(2)=th12
t2(1)=th21
t2(2)=th22
t3(1)=d31
t3(2)=d32

Calculation of pa values for the achieved values of the joint variables:

pas=subs(pa,{th1,th2,d3,a1},{t1,t2,t3,a1s});

dpax dpay dpaz should be equal 0

dpax=pax-pas(1,1);
..

Checking of the motion ranges (determined by the matrix z):

k=1
for i=1:2
if (t1(i)>z(1,1)) && (t1(i)<z(2,1)) && (t2(i)>z(1,2)) && (t2(i)<z(2,2)) &&
(t3(i)<z(1,3)) && (t3(i)<z(2,3)) && abs(dpax)<0.00001 &&

qr(k,1)=t1(i)
qr(k,2)=t2(i)
qr(k,3)=t2(i)
end
end

Please note that all the angles used with the trigonometric functions should be expressed in
radians as well as all the distances in meters.

3
Katedra Robotyki i Mechatroniki, WIMIR, AGH Krakw
3. Numerical solution
T5. Determine values of the 3 first joint coordinates by solving the set of equations:

p ax = f1 (q1 , q 2 , q3 )
p ay = f 2 (q1 , q 2 , q3 )
p az = f 3 (q1 , q 2 , q3 )

with use of a numerical procedure.

All the parameters (values of pax, pay and paz as well as the constant values) should be
substituted before the solving of the set of equations.
An example of the solution for an RRR manipulator arm structure:

% kinematic structure of the arm

syms th1 th2 th3 d1 a2 a3
A1=mA(th1,d1,0,sym(pi/2));
A2=mA(th2,0,a2,0);
A3=mA(th3,0,a3,0)
% position and orientation of the arm tip
T03=A1*A2*A3

The following script should be added:

% select numerical presentation of results (8 digits)

digits(8)

% substitute constants (if there are any)

T03s=subs(T03,{d1,a2,a3},{0.2,0.5,0.3})

% define the set of equations

r1=pax-T03s(1,4)
r2=pay-T03s(2,4)
r3=paz-T03s(3,4)

% solve the set of equations

res=solve(r1,r2,r3,'th1,th2,th3')

% present the solutions (numerical values)

q1=double(vpa(res.th1))
q2=double(vpa(res.th2))
q3=double(vpa(res.th3))

Remark:
All the angles should be introduced in radians and distances in meters.

The report should contain:

- derivation of the analytical solution to the inverse kinematics of the mechanism
- discussion of number and existence of the solution
- report of testing of the implemented algorithm for at least 3 different sets of data
- results of numerical solutions achieved for the same sets of data
- comparison of the analytical and numerical results
- conclusions