EENG 428 Laboratory --- Lab Session 3

Description:

In this session, mathematical derivation for forward and inverse kinematic problems

are to be discussed, starting from simple coordinate systems, and ending with the

Denavit-Hartenberg convention.

Prerequisites:

Attending students are expected to know:

- Different forms of transformation matrices (Rotation about / Translation along

an axis) and their Matlab implementations using the robotic toolbox.

- D-H convention.

Contents:

1- Forward and inverse kinematics in simple coordinate systems.

2- Forward kinematic equation for a serial manipulator using D-H convention.

 1- Forward and inverse kinematics in simple coordinate systems:

Representing all points of 3D space can be achieved through infinite number of

different sets of coordinates. The most preferable coordinates are known to be

orthogonal. The most common orthogonal coordinates are the Cartesian, cylindrical

and the spherical coordinates. These coordinates offer to represent any point (zero

dimensional) in space.

Representing all directions of 3D space can be achieved through infinite number of

different sets of coordinates. These coordinates are also 3 dimensional and

articulated (only rotational). The most common coordinates are the Euler, and Roll-

Pitch-Yaw (RPY) coordinates. These coordinates offer to represent any direction

(with zero distance from the origin) in space.

1.1- Cartesian robot, forward and inverse kinematics:

A Cartesian robot consist of 3 prismatic joints (linear joints), that are placed in

orthogonal planes. The position of the tool (the last frame) w.r.t the base is given by:

BTH = Trans (Px,Py,Pz)

Both forward and inverse kinematics (for positions) are very simple to compute,

the Matlab representation for forward and inverse kinematics for the Cartesian

robot are as follows:

 syms px py pz

T=transl(px,py,pz) % Forward kinematics

X=T(1:3,4); % Inverse kinematics

x=X(1)
Y=X(2)
z=X(3)

1.2- Cylindrical robot, forward and inverse kinematics:

A cylindrical robot consist of one revolute joint and two prismatic joints that are

placed in orthogonal planes. The position of the tool (the last frame) w.r.t the base

is given by:

R
TP = Tcyl(r,α,l)= Trans(0,0,l)*Rot(z, α)*Trans(r,0,0)

Or: RTP = Tcyl(r,α,l)= Rot(z, α)*Trans(r,0,l)

Forward and inverse kinematics for cylindrical robots are only valid to define the

position of the last frame with respect to the base, where the orientation of the last

frame cannot be controlled.

syms r al l

Tcyl=transl(0,0,l)*trotz(al)*transl(r,0,0) %forward
kinematics
%Example:
Tcyl=subs(Tcyl,[r al l],[2 pi/3 5]);

%inverse kinematics
z= Tcyl(3,4)
theta= atan2(Tcyl(2,4),Tcyl(1,4))
 if(Tcyl(1,4)==0)
R=Tcyl(2,4)/sin(theta)
else
R=Tcyl(1,4)/cos(theta)
End

1.3- Spherical robot, forward and inverse kinematics:

A cylindrical robot consist of two revolute joints and one prismatic joint that are

placed in orthogonal planes. The position of the tool (the last frame) w.r.t the base

is given by:

R
TP = Tsph = Rot(z,γ)*Rot(y,β)*Trans(0,0,r)

Forward and inverse kinematics for spherical robots are only valid to define the

position of the last frame with respect to the base, where the orientation of the last

frame cannot be controlled.

Exercise 1 (Homework):

Write a simple Matlab code that allows to find forward and inverse kinematics for a

spherical manipulator. (go with symmetry with the code written for the cylindrical

manipulator in the lab sheet).

1.4- Forward and inverse kinematics in Roll-Pitch-Yaw (RPY) coordinates:

Any orientation in 3D space can be achieved by three sequential rotations about z, y

and x axes respectively.

 TRPY = Rot(z, Ɵz)*Rot(y, Ɵy)*Rot(x, Ɵx)

The angles Ɵz , Ɵy and Ɵx are known as the Roll-Pitch-Yaw angles respectively.

There is a possibility to achieve any orientation by reversing the order of rotations

(about x then y then z). Yet, the first order is more used in the course book.

- Forward kinematics: if RPY angles are given, the orientation of the last
frame can be directly computed, and it is unique.

syms tx ty tz
Trpy=trotz(tz)*troty(ty)*trotx(tx)

Substituting the angles with their numerical values gives the final orientation.

- Inverse kinematics: if the desired orientation is given, the values of the RPY
angles can be calculated to achieve the same orientation. Usually, there are
two set of solutions that give the same orientation.
𝑛𝑥 𝑜𝑥 𝑎𝑥 0
𝑛 𝑜𝑦 𝑎𝑦 0
For any RTp = [ 𝑦 ]
𝑛𝑧 𝑜𝑧 𝑎𝑧 0
0 0 0 1

TRPY should be equal to RTp, this will lead us to the following two sets of solutions:

Ɵy(1) = ATAN2(-𝑛𝑧 ,+√1 − 𝑛𝑧 2 ) Ɵy(2) = ATAN2(-𝑛𝑧 ,-√1 − 𝑛𝑧 2 )

Ɵx(1) = ATAN2(𝑜𝑧 /√1 − 𝑛𝑧 2 ,𝑎𝑧 / Ɵx(2) = ATAN2(−𝑜𝑧 /√1 − 𝑛𝑧 2 ,−𝑎𝑧 /
√1 − 𝑛𝑧 2 ) √1 − 𝑛𝑧 2 )
Ɵz(1) = ATAN2(𝑛𝑦 /√1 − 𝑛𝑧 2 ,𝑛𝑥 / Ɵz(2) = ATAN2(−𝑛𝑦 /√1 − 𝑛𝑧 2 ,−𝑛𝑥 /
√1 − 𝑛𝑧 2 ) √1 − 𝑛𝑧 2 )
Example:

0.354 −0.674 0.649 0

0.505 0.722 0.475 0
Given RTp = [ ], find all sets of the RPY angles that
−0.788 0.160 0.595 0
0 0 0 1

achieve the same orientation.

Solution:
clear all
close all
syms tx ty tz
T=[0.354 -0.674 0.649 0;0.505 0.722 0.475 0;-0.788 0.160
0.595 0; 0 0 0 1]
Trpy=trotz(tz)*troty(ty)*trotx(tx)

ty1=atan2(-T(3,1),sqrt(1-(T(3,1)^2)))
ty2=atan2(-T(3,1),-sqrt(1-(T(3,1)^2)))

tx1=atan2(T(3,2)/cos(ty1),T(3,3)/cos(ty1))
tx2=atan2(T(3,2)/cos(ty2),T(3,3)/cos(ty2))

tz1=atan2(T(2,1)/cos(ty1),T(1,1)/cos(ty1))
tz2=atan2(T(2,1)/cos(ty2),T(1,1)/cos(ty2))

sets=[180*[tx1 ty1 tz1]'/pi 180*[tx2 ty2 tz2]'/pi]

Matlab robotic toolbox offers two ready functions that can find both of forward and
inverse kinematics for RPY coordinates, these functions are rpy2tr()and tr2rpy(),
an example for the usage is given below:
rpy2tr(0.9594,0.9076,0.2627,'zyx') % forward kinematics
for RPY
rpy2tr(55,51,15,'deg','zyx')% forward kinematics for
RPY, given the angles in degree

T=[0.354 -0.674 0.649 0;0.505 0.722 0.475 0;-0.788 0.160

0.595 0; 0 0 0 1];
 % inverse kinematics for RPY
tr2rpy(T,'zyx')
tr2rpy(T,'deg','zyx')

1.5- Forward and inverse kinematics in Euler coordinates:

Any orientation in 3D space can be achieved by three sequential rotations about z, y

then z axes respectively.

T = Rot(z, Ɵ1)*Rot(y, Ɵ2)*Rot(z, Ɵ3)

The angles Ɵ1 , Ɵ2 and Ɵ3 are known as the Euler angles.

- Forward kinematics: if Euler angles are given, the orientation of the last
frame can be directly computed, and it is unique.

syms t1 t2 t3
Trpy=trotz(t1)*troty(t2)*trotz(t3)

Substituting the angles with their numerical values gives the final orientation.

- Inverse kinematics: if the desired orientation is given, the values of the Euler
angles can be calculated to achieve the same orientation. Usually, there are
two set of solutions that give the same orientation.
𝑛𝑥 𝑜𝑥 𝑎𝑥 0
𝑛 𝑜𝑦 𝑎𝑦 0
For any RTp = [ 𝑦 ]
𝑛𝑧 𝑜𝑧 𝑎𝑧 0
0 0 0 1

TEul should be equal to RTp.

Exercise 2 (Homework):

1- Derive the inverse kinematic formulas for the Euler angles that achieve any
desired orientation. (find two sets similar to the RPY angles) (Depend on
pages 64-66 in the course book).
0.354 −0.674 0.649 0
0.505 0.722 0.475 0
2- Given RTp = [ ], find all sets of the Euler
−0.788 0.160 0.595 0
0 0 0 1
angles that achieve the same orientation.

3- Matlab robotic toolbox offers two ready functions that can find both of
forward and inverse kinematics for Euler coordinates, these functions are
eul2tr()and tr2eul(). Give one example for the usage of these functions
(similar to the given example for RPY).

4- Both RPY and Euler angles can achieve any orientation in 3D space, these
angles are connected to each other by one transformation and some offset(s).
By inspection, find the angles that replaces the question marks in the
following formula:

𝑅𝑃𝑌(𝜃1 , 𝜃2 , 𝜃3 ) ∗ 𝑟𝑜𝑡(𝑦, 90°) = 𝐸𝑢𝑙𝑒𝑟(? , ? , ? )

(state clearly how did you find the unknowns).

1.6- Rotation about an arbitrary axis:

In previous, we discussed sequential rotations about x,y and z axes that can achieve

any orientation in 3D space (RPY and Euler coordinates).

It is also possible to achieve any orientation in 3D space by only one rotation about

a certain axis (a vector) with the correct angle. This is known by the rotation about

an arbitrary axis, and has many applications.

The transformation matrix that correspond to the rotation about a non-zero unit

vector k with an angle Ɵ is given by:

Rot (k, Ɵ) =

cos(𝜃) + 𝑘𝑥 2 (1 − cos(𝜃)) 𝑘𝑥 𝑘𝑦 (1 − cos(𝜃)) − 𝑘𝑧 sin(𝜃) 𝑘𝑥 𝑘𝑧 (1 − cos(𝜃)) + 𝑘𝑦 sin(𝜃) 0

𝑘𝑥 𝑘𝑦 (1 − cos(𝜃)) + 𝑘𝑧 sin(𝜃) cos(𝜃) + 𝑘𝑦 2 (1 − cos(𝜃)) 𝑘𝑦 𝑘𝑧 (1 − cos(𝜃)) − 𝑘𝑥 sin(𝜃) 0
𝑘𝑥 𝑘𝑧 (1 − cos(𝜃)) − 𝑘𝑦 sin(𝜃) 𝑘𝑦 𝑘𝑧 (1 − cos(𝜃)) + 𝑘𝑥 sin(𝜃) cos(𝜃) + 𝑘𝑦 2 (1 − cos(𝜃)) 0
[ 0 0 0 1]

- Forward kinematics is very easy to compute (by substitution in the above

matrix).

- Inverse kinematics:

Given a transformation matrix (the desired orientation) RTp , finding the angle

of rotation about a vector that achieve the same orientation in the inverse

kinematic problem.

𝑛𝑥 𝑜𝑥 𝑎𝑥 0
𝑛 𝑜𝑦 𝑎𝑦 0
For any RTp = [ 𝑦 ]:
𝑛𝑧 𝑜𝑧 𝑎𝑧 0
0 0 0 1

The sum of the diagonal elements:

 cos(𝜃) + 𝑘𝑥 2 (1 − cos(𝜃)) + cos(𝜃) + 𝑘𝑦 2 (1 − cos(𝜃)) +cos(𝜃) + 𝑘𝑦 2 (1 − cos(𝜃))

= 𝑛𝑥 + 𝑜𝑦 + 𝑎𝑧

As k is a unit vector:

2cos(𝜃) + 1 = 𝑛𝑥 + 𝑜𝑦 + 𝑎𝑧

And this gives:

𝜃 = cos −1 [( 𝑛𝑥 + 𝑜𝑦 + 𝑎𝑧 − 1)/2]

Then, by subtracting each non-diagonal element from its mirror gives:

2𝑘𝑧 sin(𝜃) = 𝑛𝑦 −𝑜𝑥

2𝑘𝑦 sin(𝜃) = 𝑎𝑥 −𝑛𝑧

2𝑘𝑥 sin(𝜃) = 𝑜𝑧 −𝑎𝑦

Then:

𝑘𝑥 (𝑜𝑧 −𝑎𝑦 )/(2 sin(𝜃))

[𝑘𝑦 ] = [ (𝑎𝑥 −𝑛𝑧 )/(2 sin(𝜃)) ]
𝑘𝑧 (𝑛𝑦 −𝑜𝑥 )/(2 sin(𝜃))

There are two values that 𝜃 can take, each value correspond to a different unit

vector.

Mathematically, the values of theta are can be found as:

𝜃1 = cos −1[( 𝑛𝑥 + 𝑜𝑦 + 𝑎𝑧 − 1)/2]

𝜃2 = −cos−1 [( 𝑛𝑥 + 𝑜𝑦 + 𝑎𝑧 − 1)/2]
Exercise 3 (Homework):

0.354 −0.674 0.649 0

0.505 0.722 0.475 0
1- Given RTp = [ ], find all sets of possible unit
−0.788 0.160 0.595 0
0 0 0 1
vectors with corresponding rotational angles that achieve the same
orientation.

2- Matlab robotic toolbox offers two ready functions that can find both of
forward and inverse kinematics for rotation about an arbitrary axis, these
functions are angvec2tr()and tr2angvec(). Give one example for the
usage of these functions.

3- The ready function tr2angvec() gives only one set of answers for the

inverse kinematic problem (rotation about an arbitrary axis). Generalize it by

writing a function that gives the two sets of vector-angle that can achieve a

desired (given) orientation.

2- Denavit-Hartenberg convention in matrix representation:

Denavit-Hartenberg convention is used to facilitate and standardize the way of

representing two or more sequential frames of serial robotic manipulators.

Using this convention, any two sequential frames are related to each other by only

two rotations and two translations as follows:

n
Tn+1 = An+1 = Rot(z,θn+1)*Trans(0,0,dn+1)*Trans(an+1,0,0)*Rot(x,αn+1)
(Refer to your lecture notes and pages 67-75 in the course book for all details)

Using Matlab, the D-H matrix (the A matrix) can be written as:

syms t al a d
A= trotz(t)*transl(a,0,d)*trotx(al)

If we have the D-H parameters, the function subs() will allow us to find the

representation of each link as in the following example.

Example:

Given the manipulator in Figure 2.26 in the course book, D-H parameters are given

the following table, find all the A matrices, then find UTH.

# θ d a α

1 θ1 0 0 90

2 θ2 0 a2 0

3 θ3 0 a3 0

4 θ4 0 a4 -90

5 θ5 0 0 90

6 θ6 0 0 0

Solution:

syms t al a d
A= trotz(t)*transl(a,0,d)*trotx(al);
 syms t1 t2 t3 t4 t5 t6 a2 a3 a4
A1=subs(A,[t d a al],[t1 0 0 pi/2])
A2=subs(A,[t d a al],[t2 0 a2 0])
A3=subs(A,[t d a al],[t3 0 a3 0])
A4=subs(A,[t d a al],[t4 0 a4 -pi/2])
A5=subs(A,[t d a al],[t5 0 0 pi/2])
A6=subs(A,[t d a al],[t6 0 0 0])
Atotal=A1*A2*A3*A4*A5*A6
Atotal=simplify(Atotal)

Forward kinematics: Finding UTH given all joint variables is straight forward (only

substitution in the product of the A matrices).

Note that, there are no ready functions in the Matlab robotic toolbox that can help

you to derive the forward of inverse kinematics in all symbolics formulas. The

method stated in the lab sheet will be very useful for future topics (Jacobian

analysis).

Exercise 4 (Homework): (Exercise 21 in chapter 2 in the course book).

For the SCARA-type robot shown in the next figure,

1- Assign coordinate frames for its links based on D-H representation.

2- Fill out the D-H parameters table.

3- Write all the A matrices separately.

4- Find the transformation matrix for the tool-frame coordinates w.r.t the base
(the product of the A matrices).
Only Email submissions are accepted (you have exactly 6 days to submit)

Paper solutions are not accepted under any circumstances

- Take clear photos of your paper solutions and combine them all in a single

pdf file.

- Send your Matlab codes in a separate file (m-file or word document).

Submit to the Email: [email protected]

- In case of emergency, contact me on the Email: (Don’t visit me in the office

before writing an Email explaining your problem)

[email protected]