Professional Documents
Culture Documents
EjemplosDenavit Hartenberg
EjemplosDenavit Hartenberg
EjemplosDenavit Hartenberg
Claudio Melchiorri
Dipartimento di Ingegneria dellEnergia Elettrica e dellInformazione (DEI)
Universit`
a di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Kinematic Model
1 / 164
Summary
1
Kinematic Model
Differential Kinematics
Measures of performance
C. Melchiorri (DEIS)
Kinematic Model
2 / 164
Kinematic Model
Introduction
Kinematic Model
In robotics, there are two main kinematic problems:
1. Forward (direct) Kinematic Problem: once the joint position, velocity,
acceleration are known, compute the corresponding variables of the end-effector in
a given reference frame (e.g. a Cartesian frame).
= Forward kinematic model:
a function f defined between the joint space IRn and the work space IRm :
x = f(q)
C. Melchiorri (DEIS)
x IRm , q IRn
Kinematic Model
3 / 164
Kinematic Model
Introduction
Kinematic Model
2. Inverse Kinematic Problem: computation of the relevant variables
(positions, velocities, accelerations) from the work space to the joint space.
= Inverse Kinematic Model:
function
g = f 1
q = g(x) = f 1 (x)
C. Melchiorri (DEIS)
Kinematic Model
4 / 164
Kinematic Model
Introduction
l1 cos 1 + l2 cos(1 + 2 )
l1 sin 1 + l2 sin(1 + 2 )
cos 2 =
sin 2 =
(1 cos2 2 )
2 = atan2(sin 2 , cos 2 )
k1 = l1 + l2 cos 2 ,
1 + 2
sin 1 =
An easy problem...
y0 k1 x0 k2
,
k2 + k2
1
2
k2 = l2 sin 2
cos 1 =
y0 k1 sin 1
k2
1 = atan2(sin 1 , cos 1 )
The solution is not so simple.
Two possible solutions (sign of sin 2 ).
C. Melchiorri (DEIS)
Kinematic Model
5 / 164
Kinematic Model
Introduction
Kinematic Model
Homogeneous Transformations are used for the definition of the kinematic model.
A robotic manipulator is a mechanism composed by a chain of rigid bodies, the
links, connected by joints.
A reference frame is associated to each link, and homogeneous transformations
are used to describe their relative position/orientation.
C. Melchiorri (DEIS)
Kinematic Model
6 / 164
Kinematic Model
Introduction
Kinematic Model
A convention for the description of robots.
Each link is numbered from 0 to n, in order to be univocally identified in the
kinematic chain: L0 , L1 , . . . , Ln .
= Conventionally, L0 is the base link, and Ln is the final (distal) link.
Each joint is numbered, from 1 to n, starting from the base joint: J1 , J2 , . . . , Jn .
= According to this convention, joint Ji connects link Li 1 to link Li .
A manipulator with n + 1 links has n joints.
C. Melchiorri (DEIS)
Kinematic Model
7 / 164
Kinematic Model
Introduction
Kinematic Model
The motion of the joints changes the end-effector position/orientation in the work
space.
The position and the orientation of the end-effector result to be a (non linear)
function of the n joint variables q1 , q2 , ..., qn , i.e.
p = f(q1 , q2 , ..., qn ) = f(q)
q = [q1 q2 . . . qn ]T is defined in the joint space IRn ,
p is defined in the work space IRm .
Usually, p is composed by:
some position components (e.g. x, y , z, wrt a Cartesian reference frame)
some orientation components (e.g. Euler or RPY angles).
C. Melchiorri (DEIS)
Kinematic Model
8 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Kinematic Model
Need of defining a systematic and possibly unique method for the definition of the
kinematic model of a robot manipulator:
DENAVIT-HARTENBERG NOTATION
A reference frame is assigned to each link, and homogeneous transformations
matrices are used to describe the relative position/orientation of these frames.
The reference frames are assigned according to a particular convention, and
therefore the number of parameters needed to describe the pose of each link, and
consequently of the robot, is minimized.
C. Melchiorri (DEIS)
Kinematic Model
9 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
Problem:
How to assign frames to the links in order to minimize the number of parameters?
Generally speaking, 6 parameters are necessary to describe the position and the
orientation of a rigid body in the 3D space (a rigid body has 6 dof), and therefore
6 parameters are required to describe Fb in Fa .
Under some hypotheses, only 4 parameters are required: the Denavit-Hartenberg
Parameters.
Given two reference frames F0 and F6 in the 3D space, 4 cases are possible:
C. Melchiorri (DEIS)
Kinematic Model
10 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
Most general case: Skew Axes.
PROBLEM: Find a sequence of elementary homogeneous transformations relating
two generic reference frames F0 e F6 , with skew axes z0 and z6 .
SOLUTION: Infinite solutions are possible.
It is desirable to define A sequence so that the kinematic model is defined
univocally and using the minimum number of parameters.
C. Melchiorri (DEIS)
Kinematic Model
11 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
12 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
13 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
13 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
13 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
13 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
13 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
13 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
13 / 164
Kinematic Model
Denavit-Hartenberg Parameters
C. Melchiorri (DEIS)
Kinematic Model
13 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
Six cyclic transformations have been employed to move from F0 to F6 : 3
translations and 3 rotations.
There is a translation-rotation pattern:
0
(1)
H4
=
=
C. Melchiorri (DEIS)
C S C S S aC
S C C C S aS
0
S
C
d
0
0
0
1
Kinematic Model
14 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
Matrix 0 T6 can be expressed in terms of H matrices by adding to (1)
a null translation along x6 , obtaining the frame F7
a null rotation about x7 , obtaining the frame F8
Therefore we have
0
T8
C. Melchiorri (DEIS)
Kinematic Model
15 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
If another frame F12 is given, it is possible to move from F6 to F12 by means of a
sequence similar to (1). Then, the transformation from F0 to F12 is
0
T12
Since a translation and a rotation about the same axis may commute, i.e.
Rot(z5 , )Tras(z6 , d ) = Tras(z6 , d )Rot(z5 , )
we have that
Tras(z4 , b)Rot(z5 , )Tras(z6 , d )Rot(z7 , ) = Tras(z4 , b)Tras(z6 , d )Rot(z5 , )Rot(z7 , )
= Tras(z4 , b + d )Rot(z5 , + )
C. Melchiorri (DEIS)
Kinematic Model
16 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
In conclusion, the transformation between F0 and F12 is expressed by two DH
transformations expressed by H matrices:
the first one with parameters d, , a, ,
the second one with parameters (b + d ), ( + ), a ,
(and a third one with parameters b , , 0, 0).
C. Melchiorri (DEIS)
Kinematic Model
17 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
In general, only frames F0 and F4 are of interest, and not the intermediate ones
(F1 -F3 ). Therefore, F4 will be indicated from now as F1 . The transformation
0
H4 is then indicated as 0 H1 .
0
H1
C S C S S aC
S C C C S aS
0
S
C
d
0
0
0
1
The frames associated to each link are used only for the definition of the
kinematic model of the robot: usually their position/orientation may be freely
assigned and do not depend by other constraints.
Therefore, these frames are assigned in order to minimize the number of
parameters required for the definition of the kinematic model.
C. Melchiorri (DEIS)
Kinematic Model
18 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
Kinematic Model
19 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
In conclusion:
Although in general 6 parameters are necessary to specify the relative position and
orientation of two frames F0 and F1 , only 4 parameters are sufficient (d, , a, )
by assuming that:
1
2
C. Melchiorri (DEIS)
Kinematic Model
20 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
Consider now a generic manipulator.
C. Melchiorri (DEIS)
Li 1 , Li : consecutive links
Ji ed Ji +1 i relative joints
The motion axis of Ji defines the direction of
zi 1 (frame Fi 1 ) associated to the proximal
link
zi (Fi ) is aligned with the motion axis of the
following joint
The origin of Fi is at the intersection of zi with
the common normal ai between zi 1 and zi
If a common normal does not exist (ai = 0),
the origin of Fi is placed on zi 1
If the two axes intersect, the origin is placed at
the intersection
If the two axes coincide, also the origins of
Fi 1 and Fi coincide
xi (Fi ) is directed along the common normal
yi is chosen in order to obtain a proper frame.
Kinematic Model
21 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
Conclusion: the position and the orientation of two consecutive frames, and
therefore of the related links, may be defined by the four Denavit-Hartenberg
parameters:
ai = length of the common normal between the axes of two consecutive joints
i = ccw angle between zi 1 the axis of joint i, and zi , axis of joint i + 1
di = distance between the origin oi 1 of Fi 1 and the point pi ,
i = ccw angle between the axis xi 1 and the common normal pioi about
zi 1 .
C. Melchiorri (DEIS)
Kinematic Model
22 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
The parameters ai and i are constant and depend only on the link geometry:
ai
i
C. Melchiorri (DEIS)
Kinematic Model
i is constant;
di is constant.
23 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
The homogeneous transformation matrix relating the frames Fi 1 and Fi is
i 1
1
0
=
0
0
Ci
Si
=
0
0
0
1
0
0
0
0
1
0
Ci
0
Si
0
0
di
0
1
Si Ci
Ci Ci
Si
0
Si
Ci
0
0
Si Si
Ci Si
Ci
0
0
0
1
0
1
0
0
0
0
0
0
1
0
1
0
0
0
0
1
0
1
ai
0
0
0
0
0
1
0
Ci
Si
0
0
Si
Ci
0
0
0
0
1
ai Ci
ai Si
di
1
i 1
Hi is also indicated as
Kinematic Model
i 1
Ai .
24 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Denavit-Hartenberg Parameters
Each matrix i 1 Hi is a function of the i-th joint variable, di or i depending on the
joint type. For notational ease, the joint variable is generically indicated as qi , i.e.:
Therefore:
qi = di
q i = i
i 1
Hi =
i 1
Hi (qi ).
Tn =
This equation expresses the position and orientation of the last link wrt the base
frame, once the joint variables q1 , q2 , . . . , qn are known.
This equation is the kinematic model of the manipulator.
C. Melchiorri (DEIS)
Kinematic Model
25 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Tn =
n
Y
i 1
Hi
i =1
If all the rotational joint variables are null, i.e. i = 0, and all the prismatic joints
variables are at the minimum value, i.e. dj = min(dj ) (with j = 0), the so-called
Reference Configuration for 0 Tn is obtained.
Note that for prismatic joints the value j may be imposed by the manipulator
structure (and be not null). Also in these cases, it is arbitrarily considered null. A
similar consideration holds also for rotational joints (i = 0):
The reference configuration may be non physically reachable by the
manipulator.
C. Melchiorri (DEIS)
Kinematic Model
26 / 164
Kinematic Model
Denavit-Hartenberg Parameters
Kinematic Model
In the reference configuration, the matrices
i 1
i 1
Hi =
i 1
Hi =
i 1
Hi |i =0;
Hi |i =0
di =min(di )
i 1
1
0
0
0
1
0
0
0
0
Ci
Si
0
Hi are:
0
Ci
Si
0
0
Si
Ci
0
0
Si
Ci
0
ai
0
di
1
ai
0
min(di )
1
rotational joints
prismatic joints
The rotational part of these matrices indicates a rotation about the xi axis.
Therefore, by composing all the
orientation does not change).
i 1
In this configuration, all the xi axes have the same direction (they are aligned).
C. Melchiorri (DEIS)
Kinematic Model
27 / 164
C. Melchiorri (DEIS)
Kinematic Model
28 / 164
Kinematic Model
A procedure to assign frames to the links of a manipulator
Need of common conventions, in order to define univocally the kinematic equations.
First step: definition of the base frame F0 . In this case it is usually posible to consider
not only the kinematic configuration of the manipulator but also other considerations,
related e.g. to the work space. However, according to the DH convention, usually F0 is
chosen so that z0 coincides with the motion axis of J1 .
F0 = ?
Fn = ?
Also Fn is assigned considering not only the robots kinematics, since a motion axis for
the last link does not exist.
= F0 and Fn are arbitrarily chosen!
C. Melchiorri (DEIS)
Kinematic Model
29 / 164
Kinematic Model
The Denavit-Hartenberg convention does not define univocally the frames
associated to the links. As a matter of fact, the frames may be assigned with
some arbitrariness in the following cases:
1
parallel consecutive axes: it is not defined univocally the common normal line;
Kinematic Model
30 / 164
C. Melchiorri (DEIS)
Kinematic Model
31 / 164
Kinematic Model
32 / 164
C. Melchiorri (DEIS)
Kinematic Model
33 / 164
i 1
Hi ,
i = 1, . . . , n
C. Melchiorri (DEIS)
Tn =
H1 1 H2 . . .
Kinematic Model
n1
Hn
34 / 164
Examples
Example
Let us consider the following 3 dof manipulator:
C. Melchiorri (DEIS)
Kinematic Model
35 / 164
Examples
Example
Step 1: Assign numbers to joints and links
C. Melchiorri (DEIS)
Kinematic Model
36 / 164
Examples
Example
Step 2: Choice of the zi axes (joint rotation/translation motion axes)
C. Melchiorri (DEIS)
Kinematic Model
37 / 164
Examples
Example
Step 3: Choice of F0
C. Melchiorri (DEIS)
Kinematic Model
38 / 164
Examples
Example
Steps 4 - m: Definition of F1 ... Fn
C. Melchiorri (DEIS)
Kinematic Model
39 / 164
Examples
Example
Finally (optional): choice of the tool frame
C. Melchiorri (DEIS)
Kinematic Model
40 / 164
Examples
Example
Lets consider a 2 dof planar manipulator:
Denavit-Hartenberg parameters
L1
L2
The
i 1
d
0
0
1
2
a
a1
a2
0o
0o
Hi matrices result:
C1
S1
0
H1 =
0
0
C. Melchiorri (DEIS)
S1
C1
0
0
0
0
1
0
a1 C1
a1 S 1
0
1
C2
S2
1
H2 =
0
0
Kinematic Model
S2
C2
0
0
0
0
1
0
a2 C2
a2 S 2
0
1
41 / 164
Examples
Example
Then
0
T2 =
H1 1 H2 =
n s
0 0
a p
0 1
C12
S12
=
0
0
S12
C12
0
0
0
0
1
0
a1 C1 + a2 C12
a1 S1 + a2 S12
0
1
Kinematic Model
42 / 164
Examples
Example
zi 1 axes aligned with the motion direction of Ji
Note that:
di = 0: distances among common
normal lines
ai : distances among the joint axes
Ji
i : angle between zi 1 and zi
about xi
C12 S12 0 a1 C1
C12
0 a1 S 1
S
0
T2 = 012
0
1
0
0
0
0
1
C. Melchiorri (DEIS)
Then
Kinematic Model
1
0
2
Tt = 0
0
0
1
0
0
0
0
1
0
a2
0
0
1
43 / 164
Examples
Matrices
C1
S1
0
H1=
0
0
i 1
0
0
1
0
d
0
0
0
1
2
3
a
0
a2
a3
90o
0o
0o
Hi
S1
C1
0
0
C. Melchiorri (DEIS)
0
,1 H2=
0
1
C2
S2
0
0
S2
C2
0
0
0
0
1
0
Kinematic Model
a2 C2
a2 S 2
,2 H3=
0
1
C3
S3
0
0
S3
C3
0
0
0
0
1
0
a3 C3
a3 S 3
0
1
44 / 164
Examples
C1 C23
S1 C23
0
T3 =
S23
0
C1 S23
S1 S23
C23
0
S1
C1
0
0
C1 (a2 C2 + a3 C23 )
S1 (a2 C2 + a3 C23 )
a2 S2 + a3 S23
1
C. Melchiorri (DEIS)
Kinematic Model
45 / 164
Examples
With 1 = 2 = 3 = 0o
1 0
0
a2 + a3
0
0 0 1
0
T3 = 0 1
0
0
0 0
0
1
y3
y0
F0 x0
a2
z3
a3
With 1 = 2 = 3 = 90o
0
0
0
1
1
0
0
a
0
3
T3 = 0
1 0
a2
0
0
0
1
C. Melchiorri (DEIS)
a3
x3
F3
y3
F3
a2
z3
z0
y0
F0
Kinematic Model
x0
46 / 164
Examples
In this case, the last frame does not respect the DH convention:
= x3 does not intersect z2 !
C. Melchiorri (DEIS)
Kinematic Model
47 / 164
Examples
0 0 1
1 0 0
T=
0 1 0
0 0 0
and then
T3,new
C1 S23
S
1 C23
= 0 T3 T =
C23
0
S1
C1
0
0
0
0
0
1
C1 C23
S1 C23
S23
0
C1 (a2 C2 + a3 C23 )
S1 (a2 C2 + a3 C23 )
a2 S2 + a3 S23
1
The unit vector s depends only on the first joint. The position pz does not depend
on 1 .
C. Melchiorri (DEIS)
Kinematic Model
48 / 164
Examples
i 1
Hi matrices.
x3
y3
x3
L3
x2
z2 J3
C. Melchiorri (DEIS)
x3
y3
z3
z3
z3
Kinematic Model
y3
49 / 164
Examples
L1
L2
L3
The
i 1
d
0
0
0
1
2
3 + 90o
a
0
a2
a3
90o
0o
90o
Hi matrices are
C1
S1
0
H1=
0
0
0
0
1
0
S1
C1
0
0
C. Melchiorri (DEIS)
0
,1 H2=
0
1
C2
S2
0
0
S2
C2
0
0
0
0
1
0
Kinematic Model
a2 C2
S3
a2 S 2
,2 H3= C3
0
0
1
0
0
0
1
0
C3
S3
0
0
a3 C3
a3 S 3
0
1
50 / 164
Examples
T3,new
C1 S23
S1 C23
=
C23
0
S1
C1
0
0
C1 C23
S1 C23
S23
0
C1 (a2 C2 + a3 C23 )
S1 (a2 C2 + a3 C23 )
a2 S2 + a3 S23
1
The unit vector s depends only on the first joint. The position pz does not depend
on 1 .
C. Melchiorri (DEIS)
Kinematic Model
51 / 164
Examples
Denavit-Hartenberg parameters:
L1
L2
L3
d
0
d2
d3
1
2
0
a
0
0
0
90o
90o
0o
C1
S1
0
H1 =
0
0
0
0
1
0
C. Melchiorri (DEIS)
S1
C1
0
0
0
, 1 H2 =
0
1
C2
S2
0
0
0
0
1
0
Kinematic Model
S2
C2
0
0
0
, 2 H3 =
d2
1
1
0
0
0
0
1
0
0
0
0
1
0
0
0
d3
1
52 / 164
Examples
T3 =
n s
0 0
a p
0 1
C1 C2
C2 S1
=
S2
0
S1
C1
0
0
C1 S2
S1 S2
C2
0
d2 S1 + d3 C1 S2
d2 C1 + d3 S1 S2
d3 C2
1
The third joint J3 does not affect the orientation, s depends only on J1 .
C. Melchiorri (DEIS)
Kinematic Model
53 / 164
Examples
1 0
0 1
0
T3 = 0 0
0 0
F3
=d
0
0
1
0
If 1 = 2 = 90o , d3
0
1
0
0
0
T3 = 1
0
0
0
C. Melchiorri (DEIS)
0
d2
d
1
d
y0
d2
F0
x0
y3
=d
0
1
0
0
y3
x3
z0
d2
d
0
1
z0
F3 z3
d2
x3
y0
F0
x0
Kinematic Model
54 / 164
Examples
d
0
0
d6
4
5
6
a
0
0
0
90o
90o
0o
Then
C4
S4
3
H4 =
0
0
0
0
1
0
C. Melchiorri (DEIS)
S4
C4
0
0
0
,4 H5 =
0
1
C5
S5
0
0
0
0
1
0
S5
C5
0
0
Kinematic Model
0
,5 H6 =
0
1
C6
S6
0
0
S6
C6
0
0
0
0
1
0
0
0
d6
1
55 / 164
Examples
C4 C5 C6 S4 S6
S4 C5 C6 + C4 S6
3
T6 =
S5 C6
0
S4 C6 C4 C5 S6
C4 C6 S4 C5 S6
S5 S6
0
C4 S5
S4 S5
C5
0
d6 C4 S5
d6 S4 S5
d6 C5
1
In this case, the rotation matrix has the same expression as an Euler ZYZ rotation
matrix.
REuler (, , )
C C C S S C C S S C
= S C C + C S S C S + C C
S C
S S
C S
S S
C
It means that the manipulators joints 4 , 5 and 6 are equivalent to the Euler
ZYZ angles.
C. Melchiorri (DEIS)
Kinematic Model
56 / 164
Examples
1
0
3
T6 =
0
0
If 1 = 2 = 3 = 90o
1
0
3
T6 =
0
0
C. Melchiorri (DEIS)
0
1
0
0
0
0
1
0
0
0
1
0
0
1
0
0
0
0
d6
1
0
d6
0
1
Kinematic Model
57 / 164
Examples
By composing the 3 dof spherical manipulator with the spherical wrist, the so-called
Stanford manipulator is obtained, a 6
dof robot.
Since the frames have been defined in a
consistent manner, the kinematic model is
simply obtained by multiplying the matrices 0 T3 of the arm and 3 T6 of the wrist.
Then
0
C. Melchiorri (DEIS)
T6 =
T3 3 T6 =
n
0
Kinematic Model
s a
0 0
p
1
58 / 164
Examples
S1 S4 S5 + C1 (S2 C5 + C2 C4 S5 )
a = C1 S4 S5 + S1 (S2 C5 + C2 C4 S5 )
C2 C5 S2 C4 S5
d2 S1 + d3 C1 S2 + d6 (C1 S2 C5 + C1 C2 C4 S5 S1 S4 S5 )
p = d2 C1 + d3 S1 S2 + d6 (S1 S2 C5 + S1 C2 C4 S5 + C1 S4 S5 )
d3 C2 + d6 (C2 C5 S2 C4 S5 )
C. Melchiorri (DEIS)
Kinematic Model
59 / 164
Examples
L1
L2
L3
L4
L5
L6
C. Melchiorri (DEIS)
Kinematic Model
d
0
0
d3
d4
0
d6
1
2
3
4
5
6
a
0
a2
0
0
0
0
90o
0o
90o
90o
90o
0o
60 / 164
Examples
0
0
1
0
0
0
1
0
C1
S1
0
H1=
0
0
C4
S4
3
H4=
0
0
S1
C1
0
0
S4
C4
0
0
C. Melchiorri (DEIS)
0
,1 H2=
0
1
C2
S2
0
0
0
,4 H5=
d4
1
S2
C2
0
0
C5
S5
0
0
0
0
1
0
0
0
1
0
a2 C2
a2 S 2
,2 H3=
0
1
C3
S3
0
0
0
0
1
0
0
,5 H6=
0
1
C6
S6
0
0
S6
C6
0
0
S5
C5
0
0
Kinematic Model
S3
C3
0
0
0
0
1
0
0
0
d3
1
0
0
d6
1
61 / 164
Examples
C1 C2 C3 C1 S2 S3
C2 C3 S1 S1 S2 S3
0
T3 =
C3 S2 + C2 S3
0
C4 C5 C6 S4 S6
C5 C6 S4 + C4 S6
3
T6 =
(C6 S5 )
0
0
C. Melchiorri (DEIS)
T6 = 0 T6 =
S1
C1
0
0
C1 C3 S2 + C1 C2 S3
C3 S1 S2 + C2 S1 S3
(C2 C3 ) + S2 S3
0
(C6 S4 ) C4 C5 S6
C4 C6 C5 S4 S6
S5 S6
0
T3 3 T6 =
Kinematic Model
n s
0 0
C4 S5
S4 S5
C5
0
a p
0 1
a2 C1 C2 d3 S1
C1 d3 + a2 C2 S1
a2 S2
1
C4 d6 S5
d6 S4 S5
d4 + C5 d6
1
62 / 164
Examples
"
"
#
S1 (C4 C6 C5 S4 S6 )+C1 (C2 (S3 S5 S6 + C3 ((C6 S4 ) C4 C5 S6 )) S2 ((C3 S5 S6 ) + S3 ((C6 S4 ) C4 C5 S6 )))
s = (C1 (C4 C6 C5 S4 S6 ))+S1 (C2 (S3 S5 S6 + C3 ((C6 S4 ) C4 C5 S6 ))S2 ((C3 S5 S6 )+S3 ((C6 S4 ) C4 C5 S6 )))
S2 (S3 S5 S6 + C3 ((C6 S4 ) C4 C5 S6 )) + C2 ((C3 S5 S6 )+S3 ((C6 S4 ) C4 C5 S6 ))
a=
p=
"
"
C. Melchiorri (DEIS)
Kinematic Model
63 / 164
Examples
L1
L2
L3
L4
d
0
0
0
0
1
2
3
4
a
a1
a2
a3
a4
0o
0o
0o
0o
Ci Si 0 ai Ci
Si
Ci 0 ai Si
i 1
Hi =
0
0
1
0
0
0
0
1
C. Melchiorri (DEIS)
Kinematic Model
64 / 164
Examples
T4
H1 1 H2 2 H3 3 H4 =
C1234
S1234
=
0
0
S1234
C1234
0
0
0
0
1
0
n s
0 0
a
0
p
1
0
1
The vectors n, s, a define the end-effector orientation (rotation about z), while p
defines its position (on the x y plane, pz = 0).
= The procedure can be applied also to redundant manipulators.
C. Melchiorri (DEIS)
Kinematic Model
65 / 164
C. Melchiorri (DEIS)
Kinematic Model
66 / 164
Introduction
C. Melchiorri (DEIS)
Kinematic Model
67 / 164
Introduction
We seek for closed form solutions, and not based on numerical techniques:
The analytic solution is more efficient from the computational point of view;
If the solutions are known analytically, it is possible to select one of them on
the basis of proper criteria.
C. Melchiorri (DEIS)
Kinematic Model
68 / 164
Introduction
C. Melchiorri (DEIS)
Kinematic Model
69 / 164
Algebraic Approach
Algebraic Approach
For a 6 dof manipulator, the kinematic model is described by the equation
0
T6 =
0.5868
0.5265
T= 0.5736
0
0.6428
0.7660
0.0000
0
0.4394
0.3687
0.8192
0
C1 C2
0.4231
0.9504 C2 S1
=
S2
0.4096
0
1
S1
C1
0
0
C1 S2
S1 S2
C2
0
d2 S1 + d3 C1 S2
d2 C1 + d3 S1 S2
d3 C2
1
Since both the numerical values of 0 T6 and the structure of the i 1 Hi matrices
are known, by suitable pre- / post-multiplications it is possible to obtain
[ 0 H1 (q1 ) . . .i 1 Hi (qi )]1 0 T6 [ j Hj+1 (qj+1 ) . . .5 H6 (q6 )]1 = i Hi +1 (qi +1 ) . . .j1 Hj (qj ), i < j
Kinematic Model
70 / 164
Geometric Approach
Geometric Approach
General considerations that may help in finding solutions with algebraic techniques
do not exist, being these strictly related to the mathematical expression of the
direct kinematic model. On the other hand, it is often possible to exploit
considerations related to the geometrical structure of the manipulator.
PIEPER APPROACH
Many industrial manipulators have a kinematically decoupled structure, for
which it is possible to decompose the problem in two (simpler) sub-problems:
1
(x, y , z) q1 , q2 , q3
R q4 , q5 , q6 .
C. Melchiorri (DEIS)
Kinematic Model
71 / 164
Geometric Approach
Geometric Approach
PIEPER APPROACH: Given a 6 dof manipulator, sufficient condition to find a
closed form solution for the IK problem is that the kinematic structure presents:
three consecutive rotational joints with axes intersecting in a single point
or
three consecutive rotational joints with parallel axes.
C. Melchiorri (DEIS)
Kinematic Model
72 / 164
Geometric Approach
Geometric Approach
In many 6 dof industrial manipulators, the first 3 dof are usually devoted to
position the wrist, that has 3 additional dof give the correct orientation to the
end-effector.
In these cases, it is quite simple to decompose the IK problem in the two
sub-problems (position and orientation).
C. Melchiorri (DEIS)
Kinematic Model
73 / 164
Geometric Approach
Geometric Approach
In case of a manipulator with a spherical wrist, a natural choice is to decompose
the problem in
1
Therefore:
1
The point pp is computed since 0 T6 is known (submatrices R and p):
p p = p d6 a
pp depends only on the joint variables q1 , q2 , q3 ;
2
The matrix
R6 =
RT
3 R
is computed;
Kinematic Model
74 / 164
Examples
C1 C2 S1 C1 S2
C2 S1 C1 S1 S2
=
S2
0
C2
0
0
0
d2 S1 + d3 C1 S2
d2 C1 + d3 S1 S2
d3 C2
1
1 = atan2 (sx , sy )
Unfortunately, according to
= the Pieper approach only p
2 = atan2 (nz , az )
d3 = pz / cos 2
is known!
C. Melchiorri (DEIS)
Kinematic Model
75 / 164
Examples
From
(0 H1 )1 0 T3
C1
0
=
S1
0
C. Melchiorri (DEIS)
S1
0
C1
0
H2 2 H3
0
1
0
0
T 3 = 0 H1 1 H2 2 H3
nx
0
ny
0
0 nz
1
0
sx
sy
sz
0
Kinematic Model
ax
ay
az
0
we have
px
C2
S2
py
=
pz 0
1
0
0
0
1
0
S2
C2
0
0
d3 S2
d3 C2
d2
1
76 / 164
Examples
px C1 + py S1
d3 S2
1
= d3 C2
pz
pp =
px S1 + py C1
d2
The vector 1 pp depends only on 2 and d3 ! Lets define a = tan 1 /2, then
C1 =
1 a2
1 + a2
S1 =
2a
1 + a2
(d2 + py )a + 2px a + d2 py = 0
a=
px
q
px2 + py2 d22 , d2 + py )
Kinematic Model
77 / 164
Examples
d3 S2
px C1 + py S1
1
= d3 C2
pz
pp =
d2
px S1 + py C1
d3 S2
px C1 + py S1
=
pz
d3 C2
from which
2 = atan2 (px C1 + py S1 , pz )
Finally, if the first two elements are squared and added together
q
d3 = (px C1 + py S1 )2 + pz2
C. Melchiorri (DEIS)
Kinematic Model
78 / 164
Examples
C. Melchiorri (DEIS)
Kinematic Model
79 / 164
Examples
0.8138
0.2962
0
T3 =
0.5
0
2 = 30o ,
0.342
0.9397
0
0
d3 = 0.5 m
0.4698
0.171
0.866
0
0.0387
0.8373
0.433
1
The solution based on the whole matrix 0 T3 is: 1 = 20o , 2 = 30o , d3 = 0.5.
The solution based on the vector p gives:
a) 1 = 20o , 2 = 30o , d3 = 0.5
b) 1 = 14.7o , 2 = 30o , d3 = 0.5
C. Melchiorri (DEIS)
Kinematic Model
80 / 164
Examples
C. Melchiorri (DEIS)
Kinematic Model
81 / 164
Examples
S3 =
1 C32
3 = atan2 (S3 , C3 )
82 / 164
Examples
1 = + atan2 (py , px ),
Then, FOUR possible solutions exist:
shoulder right - elbow up;
shoulder left - elbow up;
Kinematic Model
83 / 164
Examples
nx s x ax
3
R6 = ny sy ay
nz s z az
C4 C5 C6 S4 S6
R6 = S4 C5 C6 + C4 S6
S5 C6
C. Melchiorri (DEIS)
S4 C6 C4 C5 S6
C4 C6 S4 C5 S6
S5 S6
Kinematic Model
C4 S5
S4 S5
C5
84 / 164
Examples
R6 =
"
C4 C5 C6 S4 S6
S4 C5 C6 + C4 S6
S5 C6
S4 C6 C4 C5 S6
C4 C6 S4 C5 S6
S5 S6
C4 S5
S4 S5
C5
atan2 (ay , ax )
q
atan2 ( ax2 + ay2 , az )
atan2 (sz , nz )
5 [, 0]:
C. Melchiorri (DEIS)
atan2 (ay , ax )
q
atan2 ( ax2 + ay2 , az )
atan2 (sz , nz )
Kinematic Model
85 / 164
Examples
R6 =
"
5 = 20o ,
0.7146
0.6337
0.2962
6 = 30o
0.6131
0.7713
0.1710
0.3368
0.0594
0.9397
Therefore, if
5 [0, ]
5 [, 0]
4 = 10o
4 = 170o
5 = 20o ,
6 = 30o
5 = 20o ,
6 = 30o
Kinematic Model
86 / 164
Differential Kinematics
C. Melchiorri (DEIS)
Kinematic Model
87 / 164
Differential Kinematics
Introduction
n
These two relationships are based on a linear operator, a matrix J, called the
Jacobian of the manipulator.
C. Melchiorri (DEIS)
Kinematic Model
88 / 164
Differential Kinematics
Introduction
C. Melchiorri (DEIS)
Kinematic Model
89 / 164
Differential Kinematics
Velocity domain
The translational and rotational velocities are considered separately.
Let us consider two frames F0 (base frame) and F1 (integral with the rigid body).
d p
dt
Kinematic Model
90 / 164
Differential Kinematics
Velocity domain
With respect to the rotational velocity, two different definitions are possible:
1
The velocity vector is placed in the origin, and its direction coincides with the
instantaneous rotation axis of the rigid
body.
C. Melchiorri (DEIS)
Kinematic Model
91 / 164
Differential Kinematics
=
: orientation of the rigid body
Z
dt
=
??
C. Melchiorri (DEIS)
Kinematic Model
92 / 164
Differential Kinematics
= [/2, 0, 0]T
0t1
1<t2
0t1
= [/2, 0, 0]T
1<t2
Case b)
C. Melchiorri (DEIS)
Kinematic Model
93 / 164
Differential Kinematics
[/2, 0, 0]T
0t1
1<t2
Case b)
0t1
[/2, 0, 0]T
1<t2
On the other hand, the rotation matrices in the two cases are:
0
Ra = 0
1
1
0
0
0
1
0
0
Rb = 1
0
0
0
1
1
0
0
Kinematic Model
94 / 164
Differential Kinematics
C. Melchiorri (DEIS)
Kinematic Model
95 / 164
Differential Kinematics
Kinematic Model
96 / 164
Differential Kinematics
[x , y , z ] =
T
[x , y , z ]T =
[x , y , z ]T =
C. Melchiorri (DEIS)
"
"
"
0
0
1
S
C
0
z =
C S
S S
C
x = S
y = C
z = C
= S C
x
y = S S
Kinematic Model
97 / 164
Differential Kinematics
one obtains:
0 S
0 C
1
0
0
= 0
1
0
0
1
C. Melchiorri (DEIS)
S
x
C = y
z
+
Kinematic Model
x2 + y2 = 2
z = +
98 / 164
Differential Kinematics
0 T()
Then
JG =
C. Melchiorri (DEIS)
I
0
0
T()
JA = TA ()JA
Kinematic Model
99 / 164
Differential Kinematics
Analytical Jacobian
Analitycal Jacobian
The analytical expression of the Jacobian is obtained by differentiating a vector
x = f(q) IR6 , that defines the position and orientation (according to some
convention) of the manipulator in F0 .
By differentiating f(q), one obtains
dx =
=
f(q)
dq
q
J(q)dq
f1
q2
fm
q2
...
...
f1
qn
fm
qn
J(q) IRmn
Kinematic Model
100 / 164
Differential Kinematics
Analytical Jacobian
Analitycal Jacobian
If the infinitesimal period of time dt is considered, on obtains
d x
d q
= J(q)
dt
dt
that is
x =
= J(q) q
relating the velocity vector x expressed in F0 and the joint velocity vector q.
The elements Ji ,j of the Jacobian are non linear functions of q(t): therefore
these elements change in time according to the value of the joint variables.
The Jacobian dimensions depend on the number n of joints and on the
dimension m of the considered operative space (J(q) IRmn ).
C. Melchiorri (DEIS)
Kinematic Model
101 / 164
Differential Kinematics
Analytical Jacobian
L1
L2
d
0
0
1
2
a
a1
a2
0o
0o
=
=
a1 C1 + a2 C12
a1 S1 + a2 S12
pz
1 + 2
=
0
C. Melchiorri (DEIS)
Kinematic Model
102 / 164
Differential Kinematics
Analytical Jacobian
1 + 2
=
0
By differentiation of the expressions of p and one obtains
a1 S1 a2 S12 a2 S12
a1 C1 + a2 C12
a2 C12
p
0
0
q
=
1
1
0
0
0
0
= J(q) q
C. Melchiorri (DEIS)
Kinematic Model
103 / 164
Differential Kinematics
Geometric Jacobian
J1 J2 . . . Jn
Therefore
v
J1 q 1 + J2 q 2 + . . . + Jn q n
The analytic and geometric Jacobian differ for the rotational part.
In order to obtain the geometric Jacobian, a general method based on the
geometrical structure of the manipulator is adopted.
C. Melchiorri (DEIS)
Kinematic Model
104 / 164
Differential Kinematics
Geometric Jacobian
R = R(t)
and
R(t)RT (t) = I
R(t)RT (t) = I .
T
R(t)R
(t) + R(t)R T (t) = 0
S(t) = R(t)R
(t)
As a matter of fact
S(t) + ST (t) = 0
0
S = z
y
z
0
x
y
x
0
Then
R(t)
= S(t)R(t)
This means that the derivative of a rotation matrix is expressed as a function of
the matrix itself.
C. Melchiorri (DEIS)
Kinematic Model
105 / 164
Differential Kinematics
Geometric Jacobian
p(t)
= R(t)p
which can be written as
p(t)
= S(t)R(t)p = (R(t) p )
C. Melchiorri (DEIS)
Kinematic Model
106 / 164
Differential Kinematics
Geometric Jacobian
That is:
equivalent to the transformation using S( ):
C. Melchiorri (DEIS)
Kinematic Model
vb = R S() RT va
vb = S( ) va
107 / 164
Differential Kinematics
Geometric Jacobian
Example
Consider the elementary rotation about z
cos
Rot(z, ) = sin
0
sin
cos
0
If is a function of time
sin
S(t) =
cos
0
cos
sin
0
Then
0
= 0
C. Melchiorri (DEIS)
0
cos
0 sin
0
0
sin
cos
0
0
0
1
0
0 =
1
0
0
0
0 = S((t))
0
Kinematic Model
108 / 164
Differential Kinematics
Geometric Jacobian
Geometric Jacobian
The end-effector velocity is a linear composition of the joint velocities
v
=
=
C. Melchiorri (DEIS)
Kinematic Model
109 / 164
Differential Kinematics
Geometric Jacobian
Geometric Jacobian
It is possible to show (see Appendix) that the i-th column of the Jacobian can be
computed as
0
zi 1 (0 pn 0 pi 1 )
Jvi
=
revolute joint
0
zi 1
Ji
0
zi 1
Jvi
=
prismatic joint
0
Ji
where 0 zi 1 and 0 ri 1,n = 0 pn 0 pi 1 depend on the joint variables
q1 , q2 , ..., qn . In particular:
0
pn is the end-effector position, defined in the first three elements of the last
column of 0 Tn = 0 H1 (q1 ) . . . n1 Hn (qn );
0
pi 1 is the position of F i 1 , defined in the first three elements of the last
column of 0 Ti 1 = 0 H1 (q1 ) . . . i 2 Hi 1 (qi 1 );
0
zi 1 is the third column of 0 Ri 1 :
0
C. Melchiorri (DEIS)
Ri 1 =
R1 (q1 ) 1 R2 (q2 ) . . .
Kinematic Model
i 2
Ri 1 (qi 1 )
110 / 164
Differential Kinematics
Examples
0
a1 C1
p0 = 0
p1 = a1 S1
0
0
a1 C1 + a2 C12
p2 = a1 S1 + a2 S12
0
C. Melchiorri (DEIS)
0
z0 = z1 = 0
1
Kinematic Model
111 / 164
Differential Kinematics
Examples
z1 (p2 p1 )
0
0
a1 S1 + a2 S12
0
0
0
a1 C1 a2 C12 0
a1 S1 a2 S12 a1 C1 + a2 C12
0
1
a1 S1 a2 S12
a1 C1 + a2 C12
0
0
0
a2 S12
a2 S12
0
0
0
a2 C12 0 = a2 C12
a2 S12 a2 C12
0
1
0
In conclusion:
C. Melchiorri (DEIS)
a1 S1 a2 S12
a1 C1 + a2 C12
0
J(q) =
0
1
Kinematic Model
a2 S12
a2 C12
0
0
0
1
112 / 164
Differential Kinematics
Examples
J(q) =
a1 S1 a2 S12
a1 C1 + a2 C12
0
0
0
1
a2 S12
a2 C12
0
0
0
1
If the orientation is not of interest, only the first two rows may be considered
a1 S1 a2 S12 a2 S12
J(q) =
a1 C1 + a2 C12
a2 C12
C. Melchiorri (DEIS)
Kinematic Model
113 / 164
Differential Kinematics
Examples
C1
S1
0
H1 =
0
0
C1 C23
S1 C23
0
T3 =
S23
0
C. Melchiorri (DEIS)
0
0
1
0
S1
C1
0
0
C3
S3
2
H3 =
0
0
C1 S23
S1 S23
C23
0
S1
C1
0
0
Kinematic Model
0
1 H2 =
0
1
S3
C3
0
0
0
0
1
0
C2
S2
0
0
S2
C2
0
0
a3 C3
a3 S 3
0
1
0
0
1
0
a2 C2
a2 S 2
0
1
C1 (a2 C2 + a3 C23 )
S1 (a2 C2 + a3 C23 )
a2 S2 + a3 S23
1
114 / 164
Differential Kinematics
Examples
0
p0 = p1 = 0
0
a2 C1 C2
p2 = a2 S1 S2
a2 S2
0
z0 = 0
1
C. Melchiorri (DEIS)
C1 (a2 C2 + a3 C23 )
p3 = S1 (a2 C2 + a3 C23 )
a2 S2 + a3 S23
S1
z1 = z2 = C1
0
Kinematic Model
115 / 164
Differential Kinematics
Examples
J=
Kinematic Model
116 / 164
Differential Kinematics
Examples
(a2 C2 + a3 C23 ) = 0 i.e. when the wrist is on the z axis (px = py = 0):
shoulder singularity
C. Melchiorri (DEIS)
Kinematic Model
117 / 164
Differential Kinematics
Examples
C1
S1
0
H1 =
0
0
S1
C1
0
0
0
0
1
0
1
0
2
H3 =
0
0
0
,1 H2 =
0
1
0
1
0
0
0
0
1
0
C2
S2
0
0
0
0
d3
1
0
0
1
0
S2
C2
0
0
0
0
d2
1
Kinematic model:
C1 C2
C2 S1
0
T3 =
S2
0
C. Melchiorri (DEIS)
S1
C1
0
0
C1 S2
S1 S2
C2
0
Kinematic Model
d2 S1 + d3 C1 S2
d2 C1 + d3 S1 S2
C2 d3
1
118 / 164
Differential Kinematics
Examples
with
0
z0 = 0
1
and
z0 (p3 p0 )
z0
0
p0 = p1 = 0
0
C. Melchiorri (DEIS)
z1 (p3 p1 )
z1
S1
z1 = C1
0
d2 S1
p2 = d2 C1
0
Kinematic Model
z2
0
C1 S2
z2 = S1 S2
C2
d2 S1 + d3 C1 S2
p3 = d2 C1 + d3 S1 S2
C2 d3
119 / 164
Differential Kinematics
Examples
J=
d2 C1 d3 S1 S2
d2 S1 + d3 C1 S2
0
0
0
1
d3 C1 C2
d3 S1 C2
d3 S2
S1
C1
0
C1 S2
S1 S2
C2
0
0
0
Note that:
q 3 does not affect ;
z depends only on q 1 ;
S1 y = C1 x .
C. Melchiorri (DEIS)
Kinematic Model
120 / 164
Differential Kinematics
Examples
J=
d6 S4 S5
d6 C4 S5
0
0
0
1
d6 C4 C5
d6 C5 S4
d6 S5
S4
C4
0
0
0
0
C4 S5
S4 S5
C5
J=
0
0
0
0
0
1
0
0
0
S4
C4
0
C. Melchiorri (DEIS)
0
0
0
C4 S5
S4 S5
C5
121 / 164
Differential Kinematics
Examples
Kinematic Model
122 / 164
Differential Kinematics
Examples
d3 C1 S1 (a2 C2 + d4 S23 )
d3 S1 + C1 (a2 C2 + d4 S23 )
0
1
0
0
0
C1 S23
S1 S23
C23
C. Melchiorri (DEIS)
0
0
0
S1 C4 C1 C23 S4
C1 C4 S1 C23 S4
S23 S4
C1 (d4 C23 a2 S2 )
S1 (d4 C23 a2 S2 )
a2 C2 + d4 S23
S1
C1
0
0
0
0
d4 C1 C23
d4 S1 C23
d4 S23
S1
C1
0
C1 S23 C5 + C1 C23 C4 S5 + S1 S4 S5
S1 S23 C5 + S1 C23 C4 S5 C1 S4 S5
C23 C5 + S23 C4 S5
Kinematic Model
123 / 164
Claudio Melchiorri
Dipartimento di Ingegneria dellEnergia Elettrica e dellInformazione (DEI)
Universit`
a di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Kinematic Model
124 / 164
Statics
Forces
The relation x = Jq maps velocities defined in the joint space to corresponding
velocities in the operational space. On this basis, exploiting the virtual work
principle, a similar mapping can be established considering the force domain.
Since the work, computed as the product between the applied force and the
displacement, is invariant with respect to the frame used to compute it, one
obtains:
wT dx = T dq
being w = [f T nT ]T a 6 1 vector composed by the linear forces f and torques n
applied to the manipulator, and the n 1 vector collecting the forces/torques
applied to the joints.
C. Melchiorri (DEIS)
Kinematic Model
125 / 164
Statics
Forces
By recalling that
dx = J(q)dq
one obtains
= JT (q)w
defining the relationship between the joint torque vector and the force vector w,
applied to the manipulator.
J(q)
JT (q)
( )
x
(w)
JT (q)
C. Melchiorri (DEIS)
Kinematic Model
126 / 164
x =
J q
x =
Ga a x
Kinematic Model
127 / 164
x =
J q =
Ga a x =
Ga a J q
and therefore the transformation for the Jacobian between the two frames Fa and
Fb is given by
b
J=
Ga a J
Similar considerations hold in the force domain (where the transpose Jacobian is
used).
Note that if the two frames are not rigidly attached to the robot, then the
Jacobian transformation between them is defined only by the rotation matrix b Ra :
b
Ra
0
b
a
J=
J
b
0
Ra
C. Melchiorri (DEIS)
Kinematic Model
128 / 164
Kinematic singularities
Singular configurations
The Jacobian is a 6 n matrix mapping the IRn joint velocity space to the IR6
operational velocity space:
x = J(q)q
From an infinitesimal point of view, this relationship is expressed as
dx = J(q)dq
that can be interpreted as a relationship between infinitesimal displacements in
IRn and IR6 .
Singular configurations or Singulariities
In general, rank(J(q)) = min (6, n). On the other hand, since the elements of J
are function of the joints, some robot configurations exist such that the Jacobian
looses rank. These configurations are denoted as kinematic singularities.
in IR6 without any
In these configurations, there are directions (vectors x)
n
in IR : these directions cannot be actuated and the
correspondent direction (q)
robot looses a motion capability.
C. Melchiorri (DEIS)
Kinematic Model
129 / 164
Kinematic singularities
Singular configurations
The singular configurations are important for several reasons:
1
They represents configurations in which the motion capabilities of the robot are
reduced: it is not possible to impose arbitrary motions of the end-effector
They correspond to configurations that have not a well defined solution to the
inverse kinematic problem: either no solution or infinite solutions.
Internal singularities, that occur inside the reachable workspace and are generally
caused by the alignment of two or more axes of motion, or else by the attainment
of particular end-effector configurations. These singularities constitute a serious
problem, as they can be encountered anywhere in the reachable workspace for a
planned path in the operational space.
C. Melchiorri (DEIS)
Kinematic Model
130 / 164
Kinematic singularities
Example
Consider the Jacobian J(q)
J=
1 1
0 0
dq1 + dq2
dy
C. Melchiorri (DEIS)
Kinematic Model
131 / 164
Kinematic singularities
Example
Jacobian:
J=
1 1
0 0
= JT f =
1
1
fx
fy
then
1
= fx
= fx
then, fy does not affect the joint torques, and any torque vector such that
1 = 2 does not generate any force on the environment.
C. Melchiorri (DEIS)
Kinematic Model
132 / 164
Kinematic singularities
Example
Consider the 2 dof planar manipulator:
J(q) =
If 1 = 2 = 0, then
J(q) =
0
a1 + a2
Therefore
Moreover
T
J (q) =
If
0
a2
dx
dy
0 2
0 1
0
2
a1 S1 a2 S12
a1 C1 + a2 C12
0
1
a2 S12
a2 C12
if a1 = a2 = 1
= 0
= 2 dq1 + dq2
1
2
=
=
2 fy
fy
2 = 2 1 fx = fy = 0.
C. Melchiorri (DEIS)
Kinematic Model
133 / 164
Kinematic singularities
Example
Consider the 2 dof planar manipulator:
J(q) =
a1 S1 a2 S12
a1 C1 + a2 C12
a2 S12
a2 C12
q =
58.9
115.59
If 1 = 0o , 2 = 10o then
q =
6.67
12.43
C. Melchiorri (DEIS)
Kinematic Model
(rad/sec) =
(rad/sec) =
3374.7
6622.8
382.16
712.18
(o /sec)
(o /sec)
134 / 164
Kinematic singularities
Example
Consider the 2 dof planar manipulator:
J(q) =
a1 S1 a2 S12
a1 C1 + a2 C12
a2 S12
a2 C12
4000
3000
q = J(q) x
1
with x = 1 and
Plot of
1 = 0,
2000
1000
1000
2000
3000
0
C. Melchiorri (DEIS)
0.001
0.002
Kinematic Model
0.003
0.004
0.005 0.006
q2 [rad]
0.007
0.008
0.009
0.01
135 / 164
Kinematic singularities
Singularity decoupling
In case of complex structures, the analysis of the kinematic singularities via the
Jacobian determinant det(J) may result quite difficult.
In case of manipulators with spherical wrist, by similarity with the inverse
kinematics, it is possible to decompose the study of the singular configurations
into two sub-problems:
arm singularities (e.g. the first three joints)
wrist singularities
If J IR6n then
J=
"
J11
J12
J21
J22
where, since the last three joints are of the revolute type:
J12 = [z3 (p6 p3 ), z4 (p6 p4 ), z5 (p6 p5 )]
C. Melchiorri (DEIS)
Kinematic Model
J22 = [z3 , z4 , z5 ]
136 / 164
Kinematic singularities
Singularity decoupling
Singularities depend on the mechanical structure, not on the frames chosen to
describe kinematics. Therefore, it is convenient to choose the origin of the
end-effector frame at the intersection of the wrist axes, where also the last frames
are placed. Then:
#
"
J11 0
J12 = [0, 0, 0]
=
J=
J21 J22
In this manner, J is a block lower-triangular matrix, and
det(J) = det(J11 )det(J22 )
The singularities are then decoupled, since
det(J11 ) = 0
det(J22 ) = 0
C. Melchiorri (DEIS)
Kinematic Model
137 / 164
Inverse problem:
where:
J+ = (JT J)1 JT
C. Melchiorri (DEIS)
138 / 164
J
a) m < n
a):
b) m > n
JJ+ = Im
J JT (JJT )1 = Im
b):
J+ = JT (JJT )1
J+ J = In
(JT J)1 JT J = In
C. Melchiorri (DEIS)
=
Kinematic Model
J+ = (JJT )1 JT
139 / 164
if m = n,
det(J) 6= 0
J(q)
J1 (q)
has an unique solution:
The equation x = Jq (as well as q = J1 x)
x o ! q o = J1 x o
C. Melchiorri (DEIS)
such that
Kinematic Model
x o = Jq o = J J1 x o
140 / 164
if m < n
J(q)
N (J)
IRn
IRm
rank(J) = min(m, n) = m
x q such that x = Jq
+
R(J) = IRm
(multiple solutions!)
q = J x
+
x = J (J x + q N ) = x,
q = J x + qN
q N N (J)
has
C. Melchiorri (DEIS)
dim(N (J)) = n m
(I J+ J) base of N (J)
minimum norm
Kinematic Model
141 / 164
if m > n
R(J)
IRm
IRn
rank(J) = min(m, n) = n
q ! x such that x = Jq R(J)
x = Jq 0 = JJ+ x 0 6= x 0
(JJ+ 6= I)
kx x 0 k is minimum
C. Melchiorri (DEIS)
Kinematic Model
142 / 164
in general if m 6= n
N (J)
R(J)
IRn
IRm
kx s xk
the norm of the error is minimum
kq s k
C. Melchiorri (DEIS)
Kinematic Model
143 / 164
Accelerations
Accelerations
If accelerations are of interest, by differentiating x = Jq one obtains
d J(q)
q
dt
If an acceleration vector x is given in the operational space, the corresponding
is computed as a solution of
vector q
x = J(q)
q+
b = J(q)
q
being b =
x
d J(q)
dt q.
Then
=
q
J1 b
=
q
J+ b
otherwise
If the Jacobian is not square (e.g. in case of redundant manipulators (m < n) or with
less than 6 dof), an exact solution of x = Jq (b = J
q) exists iff x R(J) (b R(J)).
A vector a is in R(A) iff
rank([A, a]) = rank[A]
C. Melchiorri (DEIS)
Kinematic Model
144 / 164
Claudio Melchiorri
Dipartimento di Ingegneria dellEnergia Elettrica e dellInformazione (DEI)
Universit`
a di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Kinematic Model
145 / 164
Introduction
(2)
Kinematic Model
146 / 164
Jacobian (pseudo)-inverse
d + Ke)
q = J1
a (q) (x
xd
C. Melchiorri (DEIS)
J1 (q)
a
f()
Kinematic Model
147 / 164
Jacobian transpose
1 T
e Ke
2
with K symmetric and positive definite. In this manner
V (e) =
V (e) > 0,
e 6= 0,
V (0) = 0
C. Melchiorri (DEIS)
eT Ke = eT Kx d eT Kx
eT Kx d eT KJa (q)q
Kinematic Model
148 / 164
Jacobian transpose
V = eT Kx d eT KJa (q)q
By choosing
q = JT
a (q)Ke
one obtains
V = eT Kx d eT KJa (q)JT
a (q)Ke
C. Melchiorri (DEIS)
JT (q)
a
f()
Kinematic Model
149 / 164
Jacobian transpose
Example
Consider the non linear function
z = f(q) =
x
y
"
q13 + sin(q1 q2 )
q1 q23 + sin(q12 + 2q2 )
The Jacobian is
J(q) =
"
3q12 + q2 cos(q1 q2 )
q1 cos(q1 q2 )
C. Melchiorri (DEIS)
Kinematic Model
150 / 164
Jacobian transpose
Example
With null initial conditions (q1 = q2 = 0), the following results are computed with
the two algorithms (Ts = 0.001 s).
K = 100
(pseudo-) Inverse J1
Transpose JT
Algoritmo J^T; valori x, y (dash)
0.2
0
0.2
0.2
0.4
0.4
0.6
0
0.02
0.04
0.06
0.08
0.1
0.12
time [s]
Valori q1, q2 (dash)
0.14
0.16
0.18
0.2
0.6
0
0.5
0.5
1.5
0
0.02
0.04
0.06
0.08
C. Melchiorri (DEIS)
0.1
0.12
time [s]
0.14
0.16
0.18
0.2
1.5
0
0.02
0.04
0.06
0.08
0.02
0.04
0.06
0.08
Kinematic Model
0.1
0.12
time [s]
Valori q1, q2 (dash)
0.1
0.12
time [s]
0.14
0.16
0.18
0.2
0.14
0.16
0.18
0.2
151 / 164
Jacobian transpose
Example
K = 1000
Transpose JT
(pseudo-) Inverse J1
Algoritmo J^1; valori x, y (dash)
0.5
Unstable
0.5
0
0.002
0.004
0.006
0.008
0.01
0.012
time [s]
Valori q1, q2 (dash)
0.002
0.004
0.006
0.008
0.014
0.016
0.018
0.02
0.014
0.016
0.018
0.02
0.5
1.5
0
C. Melchiorri (DEIS)
0.01
0.012
time [s]
Kinematic Model
152 / 164
Measures of performance
Claudio Melchiorri
Dipartimento di Ingegneria dellEnergia Elettrica e dellInformazione (DEI)
Universit`
a di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS)
Kinematic Model
153 / 164
Measures of performance
Introduction
Manipulator performance
Definition of criteria in order to compute the performance, in both the
velocity and force domains, achievable by a manipulator.
Attitude of a manipulator in a given configuration to perform a given task.
Considered criteria:
Manipulability ellipsoids;
Manipulability measures;
Polytopes.
C. Melchiorri (DEIS)
Kinematic Model
154 / 164
Measures of performance
Manipulability ellipsoids
Manipulability ellipsoids
The manipulability ellipsoids consider the mechanical gain of the manipulator, in terms
of both velocity and force.
VELOCITY
JJT = US2 UT
Kinematic Model
(JJT )+ = US2 UT
155 / 164
Measures of performance
Manipulability ellipsoids
Manipulability ellipsoids
FORCE
Consider the mapping in the force/torque domain
T = 1
from which ( = JT w):
wT JJT w = 1
This equation defines an ellipsoid in the operational force space IRm .
Similarly to the velocity case, we have that:
The principal axes of the ellipsoid are directed along the eigenvectors ui di JJT ;
Their length are proportional to the inverse of the singular values i di J.
This is an important and remarkable result, that confirms the duality of the velocity and
force spaces:
The directions along which good performance are obtained in the velocity domain are
directions along which poor performance are obtained in the force domain, and viceversa
C. Melchiorri (DEIS)
Kinematic Model
156 / 164
Measures of performance
Manipulability ellipsoids
Manipulability ellipsoids
Some considerations:
Actuation needs a large gain; it is better along directions corresponding to
large singular values;
Control: needs a small gain; it is better along directions whit smaller
singular values (better sensitivity).
The optimal direction for velocity (force) actuation is also the optimal
direction for controlling the force (velocity)
C. Melchiorri (DEIS)
Kinematic Model
157 / 164
Measures of performance
Manipulability ellipsoids
Velocity ellipsoids:
Force ellipsoids:
-1
-1
-2
-2
-3
-3
-2
-1
C. Melchiorri (DEIS)
-3
-3
Kinematic Model
-2
-1
158 / 164
Measures of performance
Manipulability ellipsoids
Manipulability measure
w (q) =
det(JJT )
Another criterion is
min
max
i.e. the ratio between the minimum and maximum eigenvalues (the inverse of the
conditioning number of J). The more this fraction is close to one, the more the
ellipsoid is close to a sphere, and all the directions give more or less the same
performance (no worst case directions).
w (q) =
C. Melchiorri (DEIS)
Kinematic Model
159 / 164
Measures of performance
q i ,min q i
q i ,max
i ,min
i ,max
Kinematic Model
160 / 164
Measures of performance
C. Melchiorri (DEIS)
Kinematic Model
161 / 164
Measures of performance
Velocity polytopes:
Force polytopes:
-1
-1
-2
-2
-3
-3
-2
-1
C. Melchiorri (DEIS)
-3
-3
Kinematic Model
-2
-1
162 / 164
Measures of performance
Example
2.5
2.5
1.5
1.5
0.5
0.5
-0.5
-0.5
-1
-0.5
0.5
C. Melchiorri (DEIS)
1.5
-1
-0.5
2.5
Kinematic Model
0.5
1.5
2.5
163 / 164
Measures of performance
Example
1.5
0.5
-0.5
-1
-0.5
C. Melchiorri (DEIS)
0.5
1.5
2.5
Kinematic Model
164 / 164