Robot control and modelling

Omar TAHRI
University of Burgundy
[email protected]
 Useful formulas for robot modelling
• Rotation representation
• Homogeneous coordinates/Rigid transformation
Notations
 2-D EUCLIDEAN SPACE
Rotation in a plane

• Let’s us consider the frame 𝐹′defined by the doublet (𝒆′𝟏 , 𝒆′𝟐 )

obtained by rotating by an angle 𝜃 the frame 𝐹 defined by the doublet
(𝒆𝟏 , 𝒆𝟐 )

𝒆𝟐
𝒆′𝟏
𝒆′𝟐

𝜃
𝒆𝟏

Express (𝒆′𝟏 , 𝒆′𝟐 ) as a function of (𝒆𝟏 , 𝒆𝟐 ) and the angle 𝜃

 2-D EUCLIDEAN SPACE
Rotation in a plane
• Let’s us consider the frame 𝐹 ′ defined by the doublet (𝒆′𝟏 , 𝒆′𝟐 )
obtained by rotating by an angle 𝜃 the frame 𝐹 defined by the
doublet (𝒆𝟏 , 𝒆𝟐 )
𝒆𝟐

𝒆′𝟏
𝒆′𝟐 𝜃
𝜃
𝒆𝟏

𝒆′𝟏 = 𝑐𝑜𝑠(𝛳)𝒆𝟏 + 𝑠𝑖𝑛(𝛳)𝒆𝟐

𝒆′𝟐 = −𝑠𝑖𝑛(𝛳)𝒆𝟏 + 𝑐𝑜𝑠(𝛳)𝒆𝟐
the two frame basis are linked by the matrix
𝑐𝛳 −𝑠𝛳
𝐑= ; c𝜃 = cos 𝜃 𝑎𝑛𝑑 s𝜃 = sin 𝜃
𝑠𝛳 𝑐𝛳
 2-D EUCLIDEAN SPACE
Rotation in a plane

𝑐𝛳 −𝑠𝛳
𝐑= ; c𝜃 = cos 𝜃 𝑎𝑛𝑑 s𝜃 = sin 𝜃
𝑠𝛳 𝑐𝛳
det 𝑹 ? 𝑎𝑛𝑑 𝑹−1 ?
 2-D EUCLIDEAN SPACE
Rotation in a plane

𝑐𝛳 −𝑠𝛳
𝐑= ; c𝜃 = cos 𝜃 𝑎𝑛𝑑 s𝜃 = sin 𝜃
𝑠𝛳 𝑐𝛳
det 𝑹 ? 𝑎𝑛𝑑 𝑹−1 ?

det 𝑹 = c𝜃 2 + s𝜃 2 = 1 (1)
c𝜃 𝑠𝜃
𝑹−1 = = 𝑹⊤
−𝑠𝜃 c𝜃
𝑹𝑹⊤ = 𝑰 (2)

The matrix 𝑹 holds the conditions (1) and (2)

it describes an orthogonal transformation in 2D space

and is called a rotation matrix
 2-D EUCLIDEAN SPACE
Point coordinates transformation after a rotation

𝒆𝟐
𝒆′𝟏
𝒆′𝟐
𝜃
𝒆𝟏
𝑥′
• Let be the coordinates of a point 𝒑 in the frame defined
𝑦′
by the doublet (𝒆′𝟏 , 𝒆′𝟐 ).

What are the coordinates of 𝒑 in the frame defined by the

doublet (𝒆𝟏 , 𝒆𝟐 )?
 2-D EUCLIDEAN SPACE
Point coordinates transformation after a rotation
𝒆𝟐
𝒆′𝟐 𝒑 𝑦
𝑦′ 𝒆′𝟏

𝑥′ 𝜃
𝑥 𝒆𝟏
𝑥′
• Let be the coordinates of a point 𝒑 in the frame defined by the doublet
𝑦′
(𝒆′𝟏 , 𝒆′𝟐 ) :
𝒑 = 𝒙′ 𝒆′𝟏 + 𝒚′ 𝒆′𝟐 = 𝒙′ 𝒄θ𝒆𝟏 + 𝒔θ𝒆𝟐 + 𝒚′ −𝒔θ𝒆𝟏 + 𝒄θ𝒆𝟐
𝒑 = (𝑥 ′ 𝑐𝜃 − 𝑠𝜃𝑦′)𝒆𝟏 + (𝑥 ′ 𝑠𝜃 + 𝑐𝜃𝑦′)𝒆𝟐

▪ The coordinates of 𝒑 in the frame defined by the doublet (𝒆𝟏 , 𝒆𝟐 ) can be

obtained by:
𝑥 c𝜃 −𝑠𝜃 𝑥′ 𝑥′ c𝜃 𝑠𝜃 𝑥
𝑦 = =
𝑠𝜃 c𝜃 𝑦′ 𝑦′ −𝑠𝜃 c𝜃 𝑦
𝒆′𝟏 𝒆′𝟐
 2-D EUCLIDEAN SPACE
Point coordinates transformation after a rotation
𝒆𝟐
𝒆′𝟐 𝒑 𝑦
𝑦′ 𝒆′𝟏

𝑥′ 𝜃
𝑥 𝒆𝟏
𝑥′
• Let be the coordinates of a point 𝒑 in the frame defined by the doublet
𝑦′
(𝒆′𝟏 , 𝒆′𝟐 ) :
𝒑 = 𝒙′ 𝒆′𝟏 + 𝒚′ 𝒆′𝟐 = 𝒙′ 𝒄θ𝒆𝟏 + 𝒔θ𝒆𝟐 + 𝒚′ −𝒔θ𝒆𝟏 + 𝒄θ𝒆𝟐
𝒑 = (𝑥 ′ 𝑐𝜃 − 𝑠𝜃𝑦′)𝒆𝟏 + (𝑥 ′ 𝑐𝜃 + 𝑐𝜃𝑦′)𝒆𝟐

▪ The coordinates of 𝒑 in the frame defined by the doublet (𝒆𝟏 , 𝒆𝟐 ) can be

obtained by:
𝑥 c𝜃 −𝑠𝜃 𝑥′ 𝑥′ c𝜃 𝑠𝜃 𝑥
𝑦 = = 𝑦
𝑠𝜃 c𝜃 𝑦′ 𝑦′ −𝑠𝜃 c𝜃
𝑥 ′ 𝑥′
▪ We also have: 𝑦 = 𝒆𝟏 𝒆′𝟐
𝑦′
 2-D EUCLIDEAN SPACE
Rigid transformation: rotation + translation
𝒆𝟐𝒕
𝒑
𝑦
𝒆′𝟐 𝒆′𝟏
𝑦′

𝑥′ 𝜃
𝑡𝑦 𝒆𝟏𝒕
𝒆𝟐 The frame 𝐹′defined by (𝒆′𝟏 , 𝒆′𝟐 ) is obtained by
translating, then rotating the frame F defined
(𝒆𝟏 , 𝒆𝟐 )

𝒆𝟏 𝑥 𝑡
𝑥

• We can show
𝑥 c𝜃 −𝑠𝜃 𝑡𝑥 𝑥′
𝑥 c𝜃 −𝑠𝜃 𝑥′ 𝑡𝑥 𝑦 = s𝜃 c𝜃 𝑡𝑦 𝑦′
𝑦 = + 𝑡
𝑠𝜃 c𝜃 𝑦′ 𝑦 1 0 0 1 1
 2-D EUCLIDEAN SPACE
Point coordinates transformation after a rotational and a translational
𝒆𝟐𝒕 motions
𝒑
𝑦
𝒆′𝟐 𝒆′𝟏
𝑦′

𝑥′ 𝜃
𝑡𝑦 𝒆𝟏𝒕
𝒆𝟐 The frame 𝐹′defined by (𝒆′𝟏 , 𝒆′𝟐 ) is obtained by
translating, then rotating the frame F defined
(𝒆𝟏 , 𝒆𝟐 )

𝒆𝟏 𝑥 𝑡
𝑥
• We can show 𝑥 c𝜃 −𝑠𝜃 𝑡𝑥 𝑥′ 𝑥′
𝑥 c𝜃 −𝑠𝜃 𝑥′ 𝑡𝑥 𝑦 = s𝜃 c𝜃 𝑡𝑦
= + 𝑡 𝑦′ = 𝑻 𝑦′
𝑦 𝑠𝜃 c𝜃 𝑦′ 𝑦 1 0 0 1 1 1
𝑹 𝒕
Where 𝐓 = is an homogenous transformation
0 0 1
 2-D EUCLIDEAN SPACE

• Combination of two consecutives rotations

𝒑′ = 𝑹𝟏 𝒑
𝒑′′ = 𝑹𝟐 𝑹𝟏 𝒑
𝒑′′ = 𝑹𝟐 𝒑′

▪ The product of two rotation matrix is a rotation matrix?

▪ 𝑹𝟐 𝑹𝟏 =𝑹𝟏 𝑹𝟐 ?

• Combination of two homogenous transformation

𝒑′ = 𝑹𝟏 𝒑 + 𝒕𝟏
𝒑′′ = 𝑹𝟐 𝒑′ + 𝒕𝟐

▪ Find the transformation that links 𝒑 𝑡𝑜 𝒑′′

o Is it an homogenous transformation?
 2-D EUCLIDEAN
′′
SPACE
𝒆𝟐
𝜃2 𝒆′′
𝟏

𝒆′𝟐 𝒆′𝟏
𝑡𝑦2 𝑡𝑥2
𝜃1
𝑡𝑦1

𝒆𝟐

𝒆𝟏 𝑡𝑥1

• The homogenous transformation the frames and ?

 2-D EUCLIDEAN
′′
SPACE
𝒆𝟐
𝒆′′
𝟏

𝒆′𝟐 𝒆′𝟏
𝑡𝑥2
𝜃1
𝑡𝑦1

𝒆𝟐

𝒆𝟏

• The homogenous transformation the frames and ?

 3-D EUCLIDEAN SPACE
Cartesian coordinates of a point
• Standard base vectors

• Coordinates of a point 𝑷 in space:

𝑷
3-D EUCLIDEAN SPACE
Spherical coordinates

𝜃 𝑟

𝜑
3-D EUCLIDEAN SPACE
Cylindrical coordinates

𝜑 𝜌
 3-D EUCLIDEAN SPACE - Vectors

•A vector can be defined by a pair of points:

•Coordinates of the vector :

• Cross Product:
:  v 2 u3 − v 3 u 2   0 −v 3 v 
2

v  u =  v u − v u  = v  u with v  = v 0 −v

   
3 1 1 3 3 1

 v u − v u 
1 2 2 1  − v 2 v1 0 
 3D rotations

• 3D Rotation matrix
 3D rotations

• 3D Rotation matrix
 3D rotations

• 3D Rotation matrix

• Coordinates of a point in the two frames are related by

• The same relation applies for a vector

 3D rotations

• The rotation matrix can be seen as the coordinates of a

orthonormal triad in another triad

• Combination of two rotations is a rotation defined by

 3D rotations

• The rotation matrix can be seen as the coordinates of a

orthonormal triad in another triad

• Combination of two rotation is a rotation defined by

• The definition of a rotation by rotation matrix (9 parameters) is not

minimal form since it is only 3 degree o freedom
▪ 6 constraints because of the orthogonality of
 3D rotations

• The definition of a rotation by rotation matrix (9 parameters) is not

minimal form since it is only 3 degree o freedom:

✓ Other forms to define rotations exist:

▪ Euler angles
▪ Rotation vector
▪ Quaternions
 Rotations
• Rotation from Euler Angles
✓ A rotation matrix can be obtained by 3 consecutive rotations as
follow (ZXZ):

R=R R R ( ) ( ) ( )

 cos sin  0  1 0 0   cos sin 0

     
R  =  − sin  cos 0  R  =  0 cos sin   R =  − sin cos 0
 0 − sin  cos   0
 0
 0 1 
   0 1 
 Rotations
• Rotation from Euler Angles
✓ A rotation matrix can be obtained by 3 consecutive
rotations as follow (ZXZ):

R=R R R ( ) ( ) ( )

 cos sin  0  1 0 0   cos sin 0

     
R  =  − sin  cos 0  R  =  0 cos sin   R =  − sin cos 0
 0 − sin  cos   0
 0
 0 1 
   0 1 

R=
 cos cos − sin cos sin  cos sin  + sin cos cos sin sin  
 
 − sin cos − cos cos sin  − sin sin  + cos cos cos cos sin  
 sin  sin  − sin  cos cos 

 Rotations
• Rotation from Euler angles:

✓ Different conventions for handling Euler angles (or no conventions): ZXZ,

YXZ,…

✓ When the basic rotations are a rotation around X, followed by a rotation 2

around Y, and then a rotation around Z, then they are called YAW-PITCH-
ROLL angles.

✓ Rotations not uniquely defined

✓ Gimbal Lock
 Rotations
• Rotation from Euler angles: Gimbal Lock poblem
▪ Consider a three consecutive rotation around ZXZ

▪ If β =0

Changing the values  or  will have the same effect: a rotation around
Z. This means a lost of one degree of freedom
• Other examples of Gimbal lock problem (Refer to the shared
Matlab files)
 Rotation defined by a rotation vector

u = u x uy uz 
 Right hand rule
 Rotation defined by a rotation vector  u

u = u x uy uz 

• Rodrigues’ formula

R = I + sin( ) u + (1 − cos( ))u u

0 − uz uy 
u =  u z 0 −u x

− u y ux 0 
 Rotation defined by a rotation vector  u

u = u x uy uz 

• Rodrigue’s formula
R = I + sin( ) u + (1 − cos( ))u u

0 − uz uy 
u =  u z 0 −u x

− u y ux 0 

•  and u can be obtained from the rotation matrix in the same way
as it is done for quaternions
 Rotation defined by a rotation vector  u

u = u x uy uz 

• Rodrigue’s formula
R = I + sin( ) u + (1 − cos( ))u u
Tapez une équation ici.
0 − uz uy 
u =  u z 0 −u x

− u y ux 0 

•  and u can be obtained from the rotation matrix in the same way
as for quaternions
• The rotation vector is an invariant to its corresponding rotation
matrix Ru = u
•  u suffers from discontinuity around π
 Rotations: quaternions
• Invented by Sir William Hamilton (1843)

• Do not suffer from Gimbal Lock

• You can think of quaternions as an extension of complex numbers

where there are three different square roots of -1.

q = q0 + q1i + q2 j + q3 k = q0 + q
𝑄
i 2 = j 2 = k 2 = −1
ij = − ji = k
jk = −kj = i
ki = −ik = j
 Basic operations on Quaternions
(similar to complex number )

• Addition/subtraction:
𝑞10 + 𝑞11 𝑖 + 𝑞12 𝑗 + 𝑞13 𝑘 + 𝑞20 + 𝑞21 𝑖 + 𝑞22 𝑗 + 𝑞23 𝑘 = (𝑞10 +𝑞20 ) + (𝑞11 +𝑞21 )i + ⋯
𝑞10 + 𝑞11 𝑖 + 𝑞12 𝑗 + 𝑞13 𝑘 − 𝑞10 + 𝑞21 𝑖 + 𝑞22 𝑗 + 𝑞23 𝑘 = (𝑞10 −𝑞20 ) + (𝑞11 −𝑞21 )i + ⋯

• Conjugate:
𝑄 = 𝑞0 + 𝑞1 𝑖 + 𝑞2 𝑗 + 𝑞3 𝑘
𝑄′ = 𝑞0 − 𝑞1 𝑖 − 𝑞2 𝑗 − 𝑞3 𝑘

• Multiplication with a scalar

β𝑄 = β𝑞0 + β𝑞1 𝑖 + β𝑞2 𝑗 + β𝑞3 𝑘
 Basic operations on Quaternions
(similar to complex number )
• Product of two quaternions:
𝑄1 𝑄2 = 𝑞10 + 𝑞11 𝑖 + 𝑞12 𝑗 + 𝑞13 𝑘 𝑞20 + 𝑞21 𝑖 + 𝑞22 𝑗 + 𝑞23 𝑘
= (𝑞10 𝑞20 − 𝑞11 𝑞21 − 𝑞12 𝑞22 − 𝑞13 𝑞23 )
+ 𝑞20 𝑞11 𝑖 + 𝑞12 𝑗 + 𝑞13 𝑘 + 𝑞10 𝑞21 𝑖 + 𝑞22 𝑗 + 𝑞23 𝑘
+(𝑞12 𝑞23 − 𝑞13 𝑞22 )i + (𝑞13 𝑞21 − 𝑞11 𝑞23 )j + (𝑞11 𝑞22 − 𝑞12 𝑞21 )k

𝑄1 𝑄2 = 𝑞10 𝑞20 -𝒗𝟏 . 𝒗𝟐 + 𝑞20 𝒗𝟏 +𝑞10 𝒗𝟐 + 𝒗𝟏 × 𝒗𝟐

Dot/inner product Cross product
𝒗𝟏 = 𝑞11 𝑖 + 𝑞12 𝑗 + 𝑞13 𝑘; 𝒗𝟐 = 𝑞21 𝑖 + 𝑞22 𝑗 + 𝑞23 𝑘
in general 𝑄1 𝑄2 ≠ 𝑄2 𝑄1
𝑖𝑓 𝒗𝟏 //𝒗𝟐 𝒗𝟏 × 𝒗𝟐 = 𝟎 𝑄1 𝑄2 = 𝑄2 𝑄1

𝑄𝑄′ = 𝑞02 +𝑞12 +𝑞22 + 𝑞32 = 𝑄 2

• Inverse of a quaternion:
𝑄1 𝑄1−1 = 1; 𝑄1−1 ?
𝑄𝑄′ = 𝑄 2 𝑄1−1 = 𝑄′ / 𝑄 2
 From quaternion vector to rotation matrix R

• A rotation Rot (n𝑢ˆ, )  q = cos 2 + sin 2 n𝑢ˆ ; q =1

• Matrix R represented with quaternions

q + q − q − q 2( q q − q q ) 2( q q + q q ) 
2 2 2 2
0 1 2 3 1 2 0 3 1 3 0 2

R =  2( q q + q q ) q −q +q −q
2 2 2 2
2( q q − q q ) 
 1 2 0 3 0 1 2 3 2 3 0 1

 2( q q − q q ) 2( q q + q q ) q −q −q +q 
2 2 2 2
1 3 0 2 2 3 0 1 0 1 2 3

• Example: we substitute values of q

q = (q0 q1 q2 q3 ) = ( 2
2
0 0 2
2
)
0 − 1 0
R = 1 0 0
0 0 1
 From rotation matrix to quaternions

• Relations between qi and rij

q2
0 = (1 + r11 + r22 + r33 )
1
q0 q1 = 1
4
(r32 − r23 )
4
q0 q2 = 14 (r13 − r31 )
= (1 + r11 − r22 − r33 )
1
q12
4 q0 q3 = 14 (r21 − r12 )
q1q2 = 14 (r12 + r21 )
= (1 − r11 + r22 − r33 )
1
q22
4 q1q3 = 14 (r13 + r31 )
q32 = (1 − r11 − r22 + r33 )
1 q2 q3 = 14 (r23 + r32 )
4
 Rotation of a point/vector using quaternions
• Let us consider :

• Let us consider the quaternion’s product

▪ The scalar part of is null (1)

▪ The imaginary part (vector part) is equal to the rotated coordinates of 𝑷
(2)

• Ex: Prove (1) and (2) using the quaternion product formula and
the expression of the rotation matrix from quaternions
 Rotation of a point/vector using quaternions
• Lets us consider two rotational motions corresponding to two
unitary quaternions 𝑸𝟏 and 𝑸𝟐

▪ The product 𝑸𝟐 𝑸𝟏 is a unitary quaternion that defined the

combination of the two rotational motions

▪ A lecture about quaternions in French

 Advantages of Quaternions

• Avoids Gimbal Lock

• Faster multiplication algorithms to combine successive rotations

than using rotation matrices

• Easier to normalize than rotation matrices

• Mathematically stable
 Frame change : rigid motion
• Rigid motion: combination of a rotation and a translation:
• The relation between the coordinates of point in two different frames is
given by

• For a vector: ?
 Homogenous transformation in 3D
• Rigid transformation: combination of a rotation and a translation:
• The relation between the coordinates of point in two different frames is
given by

• Homogenous 3D coordinates

• Composition of rigid transformations

Lecture 3 : Kinematics of Manipulators
Denavit and Hartenberg method (D-H method)
 Kinematics of Manipulators
Forward and Inverse Kinematics

The forward kinematics problem : express the position and

orientation of the reference system at the end-point with respect
to some fixed global system of reference, given the values of the
positions of each joints and the types of joints and the geometry of
the elements that connect the joints.

The inverse kinematics problem : determine the needed values for

each of the joint positions, given a particular of the reference
system at the end-point with respect to some fixed global system
of reference, given the values of the positions of each joints and
the types of joints and the geometry of the elements that connect
the joints.
 Kinematics of Manipulators
Forward and Inverse Kinematics
Serial robot:
• The forward kinematics problem (unique solution)
𝑌
The coordinates of the end-point 𝑥𝑒 , 𝑦𝑒 are
𝑙2 uniquely defined for given joint angles θ1 , θ2
𝑦𝑒 and element lengths 𝑙1 and 𝑙2
𝑙1 θ2
θ1 𝑋
𝑥𝑒

• The inverse kinematics problem : multiple possible solutions

There are different solutions for joint values θ1 , θ2 to

𝑦𝑒
such that of the end-point reaches the coordinates 𝑥𝑒 , 𝑦𝑒

𝑥𝑒
 Kinematics of Manipulators
Forward and Inverse Kinematics

• Serial robot:
▪ The forward kinematics problem has a unique solution
▪ The inverse kinematic problem for that same robot arm may have more
than one solution.
• Parallel manipulators,
▪ the forward kinematics has more than one solution

THE STEWART-GOUGH PLATFORM OF GENERAL GEOMETRY CAN HAVE 40 REAL POSTURES

▪ The inverse kinematics has only one solution.

 Kinematics of Manipulators
Forward and Inverse Kinematics
Ex : find 𝑻02

Link 1
𝑥1 Link 2
𝑦2
𝑧1 𝑦
1
𝛼 𝛽 𝑶𝟐
𝑙2 𝑙3
𝑥2
𝑶𝟏 𝑧0 𝑧2
Link 0 𝑥0
𝑑>0
𝑙1
𝑦0
𝑶𝟎

Base
 Kinematics of Manipulators
Forward and Inverse Kinematics
Ex : find 𝑻02

Link 1
𝑥1 Link 2
𝑦2
𝑧1 𝑦
1
𝛼 𝛽 𝑶𝟐
𝑙2 𝑙3
𝑥2
𝑶𝟏 𝑧0 𝑧2
Link 0 𝑥0
𝑑>0 • is rigidly attached to the base
• is rigidly attached to the link 1
𝑙1 • is rigidly attached to the link2
𝑦0 • 𝛼 = 𝛽 = 0 : the robot arm is in a
𝑶𝟎
horizontal posture

Base
 Kinematics of Manipulators
Forward and Inverse Kinematics
Ex : find 𝑻02

Link 1
𝑥1 Link 2
𝑦2
𝑧1 𝑦
1
𝛼 𝛽 𝑶𝟐
𝑙2 𝑙3
𝑥2
𝑶𝟏 𝑧0 𝑧2
Link 0 𝑥0
𝑑>0
𝑙1
𝑦0
𝑶𝟎

𝑇02 = 𝑇01 𝑇12

Base
 Kinematics of Manipulators
Forward and Inverse Kinematics
Ex : find 𝑻02

Link 1
𝑥1 Link 2
𝑦2
𝑧1 𝑦
1
𝛼 𝛽 𝑶𝟐
𝑙2 𝑙3
𝑥2
𝑶𝟏 𝑧0 𝑧2
Link 0 𝑥0
𝑑>0
𝑙1
• Check for 𝛼 = 𝛽 = 0 : horizontal posture
𝑦0 𝜋
𝑶𝟎 • Check for 𝛼 = − 2 𝑎𝑛𝑑 𝛽 = 0 : vertical posture

Base
 Kinematics of Manipulators
Link n
Link n-1

𝑞2 𝑞𝑛
Link 1
𝑞1

Link 0

General idea
• Attach a frame to each link of the robot
• Compute the homogeneous transformation between consecutive frames
• Deduce the homogeneous transformation between the robot base and the
end-effector/tools

Link: corps
Joint: articulation
 Kinematics of Manipulators
Denavit/Hartenberg parameters : 4 parameters to describe the
motion between two consecutives links

• A joint axis 𝑖 (unique) is direction vector, where the positive

direction is used to define:
▪ The positive rotational motion (counterclockwise for rotational joint)
▪ The positive direction of translational motion (linear joint).
of the link 𝑖 with respect to the link 𝑖 − 1

• Link parameters
▪ The link length (𝑎𝑖−1 ): is the shortest distance
between the two joint axes 𝑖 and 𝑖 + 1

▪ The link twist (𝛼𝑖−1 ) : the relative rotation, or

twist between two joint axes 𝑖 and 𝑖 + 1
 Kinematics of Manipulators
Denavit and Hartenberg parameters : 4 parameters to describe the
motion between two consecutives links

• Joint parameters:

▪ The link offset: the distance along the common joint axis from the
end of the link 𝑖 − 1 to the beginning of the link 𝑖.

▪ The joint angle: the relative rotation of link 𝑖 with respect to link
𝑖 − 1 , around the common joint axis 𝑖.
 Kinematics of Manipulators
Defining the Link Reference Systems (DH method)

• A reference system is fixed to link 𝑖.

• For intermediate links (with joints on both sides)

a. The origin of is defined as the intersection of the joint axis 𝑖 and the
line of the link length 𝑎𝑖

b. The axis 𝒛𝒊 is defined to be along the joint axis 𝑖 with a direction taking into
account the positive displacement unique definition

c. The axis 𝒙𝒊 of fig is defined to be along the line of the link length 𝑎𝑖 , with
the positive direction being from the joint axis 𝑖 to the joint axis 𝑖 + 1.

d. From 𝒙𝒊 and 𝒛𝒊 , the axis 𝒚𝒊 is defined such that the corresponding frame a
right-handed reference system.
 Kinematics of Manipulators
Homogeneous transformation 𝑻𝑖−1,𝑖 from DH parameters

• To bring to , we need to apply:

▪ Translation along 𝑥𝑖−1 by 𝑎𝑖−1 to cancel the distance between 𝑧𝑖 and
𝑧𝑖−1

▪ A rotation around 𝑥 to have to align 𝑧𝑖 and 𝑧𝑖−1

▪ A translation along 𝑧 to make the origins of f and coincide

▪ A rotation around 𝑧 to make the two frames coincide

 Kinematics of Manipulators
Homogeneous transformation 𝑻𝑖−1,𝑖 from DH parameters

𝑻𝑖−1,𝑖 = 𝑻𝑡𝑟𝑎𝑛𝑥 𝑎𝑖−1 𝑻𝑟𝑜𝑡𝑥 (𝜶𝒊−𝟏 ) 𝑻𝑡𝑟𝑎𝑛𝑧 𝑑𝑖 𝑻𝑟𝑜𝑡𝑧 𝜃𝑖

𝑻𝑖−1,𝑖 =
 Kinematics of Manipulators
Defining the Link Reference Systems (DH method)

• For the first link 0: the fixed base of the robot arm
a. The axis 𝒛𝟎 is defined to be along the joint 1

b. The reference system is then defined to be completely coincident with the ,

𝑑1 or 𝜃1 , is zero

• For the first link 𝑛: to which the gripper/end-effector is attached

a.

b.
 Kinematics of Manipulators
Defining the Link Reference Systems (DH method)

• For the first link 𝑛: to which the gripper/end-effector is attached

a.

b.
 Kinematics of Manipulators
Denavit and Hartenberg method (D-H method)
• Example 6: six dofs Anthropomorphic robot (IRB 1400)

6 dofs robot
Lab : implement symbolic Matlab program to compute the forward kinematics:
▪ Define the reference systems according to DH
▪ Study the 3 first dofs and 3 last (wrist)
▪ Check your obtained result
 Kinematics of Manipulators
Inverse Kinematics
• Problem: find the joint positions that are needed to put the
reference system in a specified position and orientation, with
respect to the base reference system.

• There are basically two kinds of methods for obtaining solutions for
the inverse kinematics:

▪ Analytical methods that give closed form solutions: not always possible
▪ Numerical methods that give iterative solutions.
 Kinematics of Manipulators
Inverse Kinematics
• Find 𝛼 𝛽 𝑑 such that 𝑶𝒆 coincides with the point

𝛼 = 𝛽 = 0 : vertical posture
Link 2 𝛽
Link 3
𝑦𝑒
𝑧𝑒
𝛼 𝑶𝒆𝟐
𝑙2 𝑙3
𝑥𝑒
𝑧0
Link 1 𝑥0
𝑑>0
𝑙1
𝑦0
𝑶𝟎
𝑈𝑠𝑖𝑛𝑔 𝑚𝑎𝑡𝑙𝑎𝑏 𝑓𝑖𝑙𝑒𝑠

Base
 Kinematics of Manipulators
Inverse Kinematics
• Find 𝛼 𝛽 𝑑 such that 𝑶𝒆 coincides with the point

(1)
(2)

(3)
(4)

(3)+(4)

𝐼𝑓 − 1 ≤ 𝑐 ≤ 1
 Kinematics of Manipulators
Inverse Kinematics
• Find 𝛼 𝛽 𝑑 such that 𝑶𝒆 coincides with the point

(3) and (4) can written :

With:
IRB 1400
Axis 3
Axis 1
𝒙𝟒 𝒙𝟔
𝑑1 𝑑4 𝒙𝟓
𝒙𝟑
𝒛𝟒
Axis 4 Axis 6
𝑑3
𝒛𝟔
𝒛𝟑 𝒛𝟓
𝑑2
𝒛𝟏 𝒙𝟐

Axis 5
𝒙𝟏
𝒛𝟎 𝑑0
𝒛𝟐

𝒙𝟎

Axis 2
 Kinematics of Manipulators
Inverse Kinematics
• Find 𝛼 𝛽 𝑑 such that 𝑶𝒆 coincides with the point

𝛼 = 𝛽 = 0 : vertical posture
Link 2 𝛽
Link 3
𝑦𝑒
𝑧𝑒
𝛼 𝑶𝒆𝟐
𝑙2 𝑙3
𝑥𝑒
𝑧0
Link 1 𝑥0
𝑑>0
𝑙1
𝑦0
𝑶𝟎
𝑈𝑠𝑖𝑛𝑔 𝑚𝑎𝑡𝑙𝑎𝑏 𝑓𝑖𝑙𝑒𝑠

Base
 Kinematics of Manipulators
Cartesian speeds

𝑑𝑡𝑥
𝑡ሶ𝑥 = =?
𝑡𝑥 = 𝑙1 + 𝑙3 cos(𝛼 + 𝛽) + 𝑙2 cos(𝛼) 𝑑𝑡
𝑑𝑡𝑦
൞ 𝑡𝑦 = 𝑙1 + 𝑙3 sin(𝛼 + 𝛽) + 𝑙2 sin(𝛼) 𝑡ሶ𝑦 = =?
𝑡𝑧 = 𝑑 𝑑𝑡
𝑑𝑡𝑧
𝑡ሶ𝑧 = =?
𝑑𝑡
 Kinematics of Manipulators
Cartesian speeds

𝑑𝑡𝑥
𝑡ሶ𝑥 = = −𝑙3 𝛼ሶ + 𝛽ሶ sin 𝛼 + 𝛽 − 𝑙2 𝛼ሶ sin 𝛼
𝑡𝑥 = 𝑙1 + 𝑙3 cos(𝛼 + 𝛽) + 𝑙2 cos(𝛼) 𝑑𝑡
𝑑𝑡𝑦
൞ 𝑡𝑦 = 𝑙1 + 𝑙3 sin(𝛼 + 𝛽) + 𝑙2 sin(𝛼) 𝑡ሶ𝑦 = = 𝑙3 𝛼ሶ + 𝛽ሶ cos 𝛼 + 𝛽 + 𝑙2 𝛼ሶ cos 𝛼
𝑑𝑡
𝑡𝑧 = 𝑑 𝑑𝑡𝑧
𝑡ሶ𝑧 = = 𝑑ሶ
𝑑𝑡

𝑡ሶ𝑥 −𝑙3 sin 𝛼 + 𝛽 − 𝑙2 sin 𝛼 −𝑙3 sin 𝛼 + 𝛽 0 𝛼ሶ

ቐ 𝑡ሶ𝑦 = 𝑙3 cos 𝛼 + 𝛽 + 𝑙2 cos 𝛼 𝑙3 cos 𝛼 + 𝛽 0 𝛽ሶ =𝒗𝒐𝟐
𝑡ሶ𝑧 0 0 1 𝑑ሶ

Translational speed of the end−effector expressed on the base frame

 Kinematics of Manipulators
Cartesian speeds

What about the rotationnel speeds?

 Jacobian matrix
Link between joint speed and cartesian speed of the end-point

The Jacobian Matrix 𝐽 express the relationship is between the joint

value rates of change 𝒒ሶ = 𝑞ሶ 1 𝑞ሶ 𝑛 … 𝑞ሶ 𝑛 and the rate of change of the
location of end-point 𝑷ሶ 𝑒 = 𝑃𝑥ሶ 𝑃ሶ 𝑦 𝑃ሶ 𝑧 𝜔𝑥 𝜔𝑦 𝜔𝑧 :
𝑷ሶ 𝒆 = 𝑱 𝒒ሶ The Forward Jacobian

𝒒ሶ = 𝑱+ 𝑷ሶ 𝒆 The Inverse Jacobian

 Jacobian matrix
Link between joint speed and cartesian speed of the end-point

𝒒ሶ = 𝑱+ 𝑷ሶ 𝒆 The Inverse Jacobian