Bolona Italy Robot Tutorial

Kinematic Model of Robot Manipulators
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)
Universit`a di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS) Kinematic Model 1 / 164
Summary
1
Kinematic Model
2
Direct Kinematic Model
3
Inverse Kinematic Model
4
Dierential Kinematics
5
Statics - Singularities - Inverse dierential kinematics
6
Inverse kinematics algorithms
7
Measures of performance
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-eector in
a given reference frame (e.g. a Cartesian frame).
= Forward kinematic model:
a function f dened between the joint space IR
n
and the work space IR
m
:
x = f(q) x IR
m
, q IR
n
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
from IR
m
to IR
n
:
q = g(x) = f
1
(x) q IR
n
, x IR
m
Some common (somehow arbitrary) denitions must be adopted for the
same manipulator, dierent (although equivalent) kinematic models can be
dened.
Kinematic Model Example: a 2 dof planar robot
Forward kinematic model:
x = l
1
cos
1
+ l
2
cos(
1
+
2
)
y = l
1
sin
1
+ l
2
sin(
1
+
2
)
=
1
+
2
An easy problem...
Inverse kinematic model:
cos
2
=
x
2
0
+ y
2
0
l
2
1
l
2
2
2l
1
l
2
, sin
2
=
(1 cos
2
2
)
2
= atan2(sin
2
, cos
2
)
k
1
= l
1
+ l
2
cos
2
, k
2
= l
2
sin
2
sin
1
=
y
0
k
1
x
0
k
2
k
2
1
+ k
2
2
, cos
1
=
y
0
k
1
sin
1
k
2
1
= atan2(sin
1
, cos
1
)
The solution is not so simple.
Two possible solutions (sign of sin
2
).
Kinematic Model
Homogeneous Transformations are used for the denition 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.
Kinematic Model
A convention for the description of robots.
Each link is numbered from 0 to n, in order to be univocally identied in the
kinematic chain: L
0
, L
1
, . . . , L
n
.
= Conventionally, L
0
is the base link, and L
n
is the nal (distal) link.
Each joint is numbered, from 1 to n, starting from the base joint: J
1
, J
2
, . . . , J
n
.
= According to this convention, joint J
i
connects link L
i 1
to link L
i
.
A manipulator with n + 1 links has n joints.
Kinematic Model
The motion of the joints changes the end-eector position/orientation in the work
space.
The position and the orientation of the end-eector result to be a (non linear)
function of the n joint variables q
1
, q
2
, ..., q
n
, i.e.
p = f(q
1
, q
2
, ..., q
n
) = f(q)
q = [q
1
q
2
. . . q
n
]
T
is dened in the joint space IR
n
,
p is dened in the work space IR
m
.
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).
Kinematic Model Denavit-Hartenberg Parameters
Kinematic Model
Need of dening a systematic and possibly unique method for the denition 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.
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 F
b
in F
a
.
Under some hypotheses, only 4 parameters are required: the Denavit-Hartenberg
Parameters.
Given two reference frames F
0
and F
6
in the 3D space, 4 cases are possible:
Most general case: Skew Axes.
PROBLEM: Find a sequence of elementary homogeneous transformations relating
two generic reference frames F
0
e F
6
, with skew axes z
0
and z
6
.
SOLUTION: Innite solutions are possible.
It is desirable to dene A sequence so that the kinematic model is dened
univocally and using the minimum number of parameters.
Denavit-Hartenberg Parameters - Procedure
A common normal n exists among two skew z axes. Let us dene:
d the distance between the origin of F
0
and the intersection point of z
0
with n
a the distance between z
0
and z
6
along n
Apply the following sequence of translations/rotations:
1
Translate the origin of F
0
along z
0
for the quantity d: the frame F
1
is obtained
2
Rotate (ccw) F
1
about z
1
by the angle until x
1
is aligned with n:F
2
is obtained
3
Translate F
2
along x
2
(= n) for a: F
3
is obtained, with origin on the z
6
axis
4
Rotate (ccw) F
3
about x
3
by , so that z
3
is aligned with z
6
: F
4
is obtained
5
Translate F
4
along z
4
for the quantity b until F
6
: the frame F
5
is obtained
6
Rotate F
5
about z
5
by the angle : F
6
is reached
Six cyclic transformations have been employed to move from F
0
to F
6
: 3
translations and 3 rotations.
There is a translation-rotation pattern:
0
T
6
= Tras(z
0
, d)Rot(z
1
, )Tras(x
2
, a)Rot(x
3
, )Tras(z
4
, b)Rot(z
5
, ) (1)
The rst 4 transformations are of particular interest: 2 couples of translations and
rotations about two axes (note that z
0
= z
1
and x
2
= x
3
):
0
H
4
= Tras(z
0
, d)Rot(z
1
, )Tras(x
2
, a)Rot(x
3
, )
=
aC
aS
0 S
d
0 0 0 1

Matrix
0
T
6
can be expressed in terms of H matrices by adding to (1)
a null translation along x
6
, obtaining the frame F
7
a null rotation about x
7
, obtaining the frame F
8
Therefore we have
0
T
8
= Tras(z
0
, d)Rot(z
1
, )Tras(x
2
, a)Rot(x
3
, )
Tras(z
4
, b)Rot(z
5
, )Tras(x
6
, 0)Rot(x
7
, 0)
that is expressed by cyclic transformations.
If another frame F
12
is given, it is possible to move from F
6
to F
12
by means of a
sequence similar to (1). Then, the transformation from F
0
to F
12
is
0
T
12
=
0
H
4
Tras(z
4
, b)Rot(z
5
, )Tras(z
6
, d
)Rot(z
7
,
)Tras(x
8
, a
)Rot(x
9
,
)
Tras(z
10
, b
)Rot(z
11
,
)Tras(x
12
, 0)Rot(x
13
, 0)
Since a translation and a rotation about the same axis may commute, i.e.
Rot(z
5
, )Tras(z
6
, d
) = Tras(z
6
, d
)Rot(z
5
, )
we have that
Tras(z
4
, b)Rot(z
5
, )Tras(z
6
, d
)Rot(z
7
,
) = Tras(z
4
, b)Tras(z
6
, d
)Rot(z
5
, )Rot(z
7
,
)
= Tras(z
4
, b + d
)Rot(z
5
, +
)
In conclusion, the transformation between F
0
and F
12
is expressed by two DH
transformations expressed by H matrices:
the rst one with parameters d, , a, ,
the second one with parameters (b + d
), ( +
), a
(and a third one with parameters b
, 0, 0).
In general, only frames F
0
and F
4
are of interest, and not the intermediate ones
(F
1
-F
3
). Therefore, F
4
will be indicated from now as F
1
. The transformation
0
H
4
is then indicated as
0
H
1
.
0
H
1
= Tras(z
0
, d)Rot(z
1
, )Tras(x
2
, a)Rot(x
3
, )
=
aC
aS
0 S
d
0 0 0 1
The frames associated to each link are used only for the denition 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 denition of the kinematic model.
As a matter of fact, if F
0
and F
6
are two frames associated to two consecutive
links, and the position and orientation of F
6
are not constrained by other
considerations, it is possible to choose F
4
as the frame of the second link (NOT
F
6
), reducing in this manner to 4 the number of parameters: b and are not
necessary.
Then, the transformation between two consecutive links is
0
H
4
.
In conclusion:
Although in general 6 parameters are necessary to specify the relative position and
orientation of two frames F
0
and F
1
, only 4 parameters are sucient (d, , a, )
by assuming that:
1
The axis x
1
intersects z
0
2
The axis x
1
is perpendicular to z
0
These parameters are known as the Denavit-Hartenberg Parameters.
Consider now a generic manipula-
tor.
L
i 1
, L
i
: consecutive links
J
i
ed J
i +1
i relative joints
The motion axis of J
i
denes the direction of
z
i 1
(frame F
i 1
) associated to the proximal
link
z
i
(F
i
) is aligned with the motion axis of the
following joint
The origin of F
i
is at the intersection of z
i
with
the common normal a
i
between z
i 1
and z
i
If a common normal does not exist (a
i
= 0),
the origin of F
i
is placed on z
i 1
If the two axes intersect, the origin is placed at
the intersection
If the two axes coincide, also the origins of
F
i 1
and F
i
coincide
x
i
(F
i
) is directed along the common normal
y
i
is chosen in order to obtain a proper frame.
Conclusion: the position and the orientation of two consecutive frames, and
therefore of the related links, may be dened by the four Denavit-Hartenberg
parameters:
a
i
= length of the common normal between the axes of two consecutive joints
i
= ccw angle between z
i 1
the axis of joint i , and z
i
, axis of joint i + 1
d
i
= distance between the origin o
i 1
of F
i 1
and the point p
i
,
i
= ccw angle between the axis x
i 1
and the common normal p
i
o
i
about
z
i 1
.
The parameters a
i
and
i
are constant and depend only on the link geometry:
a
i
is the link length
i
is the link twist angle
between the joints axes.
Considering the two other parameters, depending on the joint type one is
constant and the other one may change in time:
prismatic joint: d
i
is the joint variable and
i
is constant;
rotational joint:
i
is the joint variable and d
i
is constant.
The homogeneous transformation matrix relating the frames F
i 1
and F
i
is
i 1
H
i
= Trasl (z
i 1
, d
i
) Rot(z
i 1
,
i
) Trasl (x
i
, a
i
) Rot(x
i
,
i
)
=
1 0 0 0
0 1 0 0
0 0 1 d
i
0 0 0 1
C
i
S
i
0 0
S
i
C
i
0 0
0 0 1 0
0 0 0 1
1 0 0 a
i
0 1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 C
i
S
i
0
0 S
i
C
i
0
0 0 0 1
C
i
S
i
C
i
S
i
S
i
a
i
C
i
S
i
C
i
C
i
C
i
S
i
a
i
S
i
0 S
i
C
i
d
i
0 0 0 1
known as Canonical Transformation.

In literature, matrix
i 1
H
i
is also indicated as
i 1
A
i
.
Each matrix
i 1
H
i
is a function of the i-th joint variable, d
i
or
i
depending on the
joint type. For notational ease, the joint variable is generically indicated as q
i
, i.e.:
q
i
= d
i
for prismatic joints
q
i
=
i
for rotational joints
Therefore:
i 1
H
i
=
i 1
H
i
(q
i
).
In case of a manipulator with n joints, the relationship between frame F
0
and
frame F
n
is:
0
T
n
=
0
H
1
(q
1
)
1
H
2
(q
2
)...
n1
H
n
(q
n
)
This equation expresses the position and orientation of the last link wrt the base
frame, once the joint variables q
1
, q
2
, . . . , q
n
are known.
This equation is the kinematic model of the manipulator.
Reference Conguration of a Canonical Transformation
A generic homogenous transformation
0
T
n
may be expressed as a function of n
canonical transformations
0
T
n
=
n
i =1
i 1
H
i
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. d
j
= min(d
j
) (with
j
= 0), the so-called
Reference Conguration for
0
T
n
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 conguration may be non physically reachable by the
manipulator.
Kinematic Model
In the reference conguration, the matrices
i 1
H
i
are:
i 1
H
i
=
i 1
H
i
|
i
=0
=
1 0 0 a
i
0 C
i
S
i
0
0 S
i
C
i
d
i
0 0 0 1
rotational joints
i 1
H
i
=
i 1
H
i
|
i
=0; d
i
=min(d
i
)
=
1 0 0 a
i
0 C
i
S
i
0
0 S
i
C
i
min(d
i
)
0 0 0 1
prismatic joints
The rotational part of these matrices indicates a rotation about the x
i
axis.
Therefore, by composing all the
i 1
H
i
, the x
i
axes results only translated (their
orientation does not change).
In this conguration, all the x
i
axes have the same direction (they are aligned).
Direct Kinematic Model Procedure for assigning frames
Direct Kinematic Model
Claudio Melchiorri
Kinematic Model
A procedure to assign frames to the links of a manipulator
Need of common conventions, in order to dene univocally the kinematic equations.
First step: denition of the base frame F
0
. In this case it is usually posible to consider
not only the kinematic conguration of the manipulator but also other considerations,
related e.g. to the work space. However, according to the DH convention, usually F
0
is
chosen so that z
0
coincides with the motion axis of J
1
.
F
0
= ?
F
n
= ?
Also F
n
is assigned considering not only the robots kinematics, since a motion axis for
the last link does not exist. = F
0
and F
n
are arbitrarily chosen!
Kinematic Model
The Denavit-Hartenberg convention does not dene 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
F
0
: only the direction of z
0
may be univocally dened, while in general the
origin o
0
and the orientation of x
0
and y
0
are not assigned;
2
F
n
: only x
n
is constrained to be perpendicular to z
n1
(i.e. to J
n
). Since the
joint n + 1 does not exist, it is not possible to dene the other elements;
3
parallel consecutive axes: it is not dened univocally the common normal line;
4
intersecting consecutive axes: the direction of x
i
is not dened;
5
prismatic joint: only z
i
is dened.
In these cases, it is possible to exploit the arbitrariness in order to simplify the
kinematic model, for example by posing the origin of consecutive frames in the
same point, or aligning axes of consecutive frames, and so on.
Procedure for assigning the frames
The frames are assigned to the links with the following procedure:
1. The joints and links are numbered (joints from 1 to n; links from 0 to n).
Links L
i 1
and L
i
are adjacent (proximal and distal, respectively), connected
by the joint J
i
(whose variable is q
i
);
2. Denition of the axes z
i
, i = 0, . . . , n 1, aligned with the joint motion
directions (rotation/translation);
(N.B. z
i
is the motion direction of joint J
i +1
: z
0
J
1
; z
1
J
2
; . . .)
3. Denition of F
0
, with origin in any point of z
0
, and axes x
0
, y
0
properly
chosen;
Steps 4 - 6 are repeated for i = 1, . . . , n 1
4. Denition of F
i
. Three cases are possible:
a) the axes of joints J
i
and J
i +1
have a common normal:
the origin of F
i
is placed at the intersection point of z
i
with the common
normal between z
i 1
and z
i
b) the axes of J
i
e J
i +1
intersect:
the origin of F
i
is placed at the intersection point between z
i 1
and z
i
c) the axes of joints J
i
and J
i +1
are coincident or parallel:
if J
i
is rotational, the origin of F
i
is chosen so that d
i
= 0
if J
i
is prismatic, the origin of F
i
is placed at a joint limit
5. Denition of x
i
along the common normal between z
i 1
and z
i
(if exists)
with positive direction from J
i
to J
i +1
; if z
i 1
intersects z
i
, then the following
joints are considered;
6. Denition of y
i
in order to obtain a proper frame.
Finally:
7. Dene o
n
coincident with o
n1
;
8. Dene x
n
perpendicular to z
n1
;
9. If J
n
is rotational, choose z
n
parallel to z
n1
;
If J
n
is prismatic, it is possible to choose z
n
freely;
10. Dene y
n
in order to obtain a proper frame.
Note that:
The position of o
n
and its orientation z
n
are arbitrary
In this manner, the frame F
n
is dierent wrt the frame of the end-eector
t
T
(with axes n, s, a). Therefore, it is in general necessary to dene a constant
homogeneous transform matrix to take into account this dierence.
Once the n frames F
i
(i = 1, . . . , n) are dened, the corresponding 4n DH
parameters d
i
, a
i
,
i
,
i
can be easily determined, and therefore also the
matrices
i 1
H
i
can be computed. The kinematic model is then obtained.
Then:
a) Dene the DH Parameters Table
b) Compute the homogeneous transformation matrices
i 1
H
i
, i = 1, . . . , n
c) Compute the direct kinematic function
0
T
n
=
0
H
1
1
H
2
. . .
n1
H
n
Direct Kinematic Model Examples
Example
Let us consider the following 3 dof manipulator:
Example
Step 1: Assign numbers to joints and links
Example
Step 2: Choice of the z
i
axes (joint rotation/translation motion axes)
Example
Step 3: Choice of F
0
Example
Steps 4 - m: Denition of F
1
... F
n
Example
Finally (optional): choice of the tool frame
Example
Lets consider a 2 dof planar manipulator:
Denavit-Hartenberg parameters
d a
L1 0
1
a
1
0
o
L2 0
2
a
2
0
o
The
i 1
H
i
matrices result:
0
H
1
=
C
1
S
1
0 a
1
C
1
S
1
C
1
0 a
1
S
1
0 0 1 0
0 0 0 1
1
H
2
=
C
2
S
2
0 a
2
C
2
S
2
C
2
0 a
2
S
2
0 0 1 0
0 0 0 1

Example
Then
0
T
2
=
0
H
1
1
H
2
=
n s a p
0 0 0 1
C
12
S
12
0 a
1
C
1
+ a
2
C
12
S
12
C
12
0 a
1
S
1
+ a
2
S
12
0 0 1 0
0 0 0 1
The vectors n, s, a express the orientation of the manipulator (rotation about z

0
),
while p denes the end eector position (plane x
0
y
0
).
Example
z
i 1
axes aligned with the motion direc-
tion of J
i
Note that:
d
i
= 0: distances among common
normal lines
a
i
: distances among the joint axes
J
i
i
: angle between z
i 1
and z
i
about x
i
With the DH convention, the origin of F
2
is coincident with F
1
. In this case, one
obtains:
0
T
2
=
C
12
S
12
0 a
1
C
1
S
12
C
12
0 a
1
S
1
0 0 1 0
0 0 0 1
Then
2
T
t
=
1 0 0 a
2
0 1 0 0
0 0 1 0
0 0 0 1

Example: 3 dof anthropomorphic manipulator
Table of the Denavit-Hartenberg parameters
d a
L1 0
1
0 90
o
L2 0
2
a
2
0
o
L3 0
3
a
3
0
o
Matrices
i 1
H
i
0
H
1
=
C
1
0 S
1
0
S
1
0 C
1
0
0 1 0 0
0 0 0 1
,
1
H
2
=
C
2
S
2
0 a
2
C
2
S
2
C
2
0 a
2
S
2
0 0 1 0
0 0 0 1
,
2
H
3
=
C
3
S
3
0 a
3
C
3
S
3
C
3
0 a
3
S
3
0 0 1 0
0 0 0 1

Kinematic model:
0
T
3
=
C
1
C
23
C
1
S
23
S
1
C
1
(a
2
C
2
+ a
3
C
23
)
S
1
C
23
S
1
S
23
C
1
S
1
(a
2
C
2
+ a
3
C
23
)
S
23
C
23
0 a
2
S
2
+ a
3
S
23
0 0 0 1
The orientation of z
3
depends only on the
rst joint J
1
; p
z
does not depend on
1
.
Check if the model is correct
With
1
=
2
=
3
= 0
o
0
T
3
=
1 0 0 a
2
+ a
3
0 0 1 0
0 1 0 0
0 0 0 1
x
0
z
0
y
0
F
0
a
2
a
3
y
3
z
3
F
3
With
1
=
2
=
3
= 90
o
0
T
3
=
0 0 1 0
1 0 0 a
3
0 1 0 a
2
0 0 0 1
x
0
z
0
y
0
F
0
a
2
a
3
z
3
y
3
x
3
F
3
Another choice for the last frame could be
In this case, the last frame does not re-
spect the DH convention:
= x
3
does not intersect z
2
!
There are two possible manners to obtain the kinematic model.
There are two possible manners to obtain the kinematic model:
1) Use the previous model and add a constant rotation, in this case
T =
0 0 1 0
1 0 0 0
0 1 0 0
0 0 0 1
and then
0
T
3,new
=
0
T
3
T =
C
1
S
23
S
1
C
1
C
23
C
1
(a
2
C
2
+ a
3
C
23
)
S
1
C
23
C
1
S
1
C
23
S
1
(a
2
C
2
+ a
3
C
23
)
C
23
0 S
23
a
2
S
2
+ a
3
S
23
0 0 0 1
The unit vector s depends only on the rst joint. The position p
z
does not depend
on
1
.
2) Use the DH convention by adding suitable (ctitious)
i 1
H
i
matrices.
In this case, it is necessary to add a rotation of /2 about z and a rotation of /2
about x and therefore the DH angles = = /2.
x
2
y
2
z
2
J
3
L
3
x
3
y
3
z
3
y
3
x
3
z
3
z
3
x
3
y
3
The new DH parameters table (the joint angle
3
and the new angle are dened
about the same axis and then it is possible to simply add them together):
d a
L1 0
1
0 90
o
L2 0
2
a
2
0
o
L3 0
3
+ 90
o
a
3
90
o
The
i 1
H
i
matrices are
0
H
1
=
C
1
0 S
1
0
S
1
0 C
1
0
0 1 0 0
0 0 0 1
,
1
H
2
=
C
2
S
2
0 a
2
C
2
S
2
C
2
0 a
2
S
2
0 0 1 0
0 0 0 1
,
2
H
3
=
S
3
0 C
3
a
3
C
3
C
3
0 S
3
a
3
S
3
0 1 0 0
0 0 0 1

The kinematic model results:
0
T
3,new
=
C
1
S
23
S
1
C
1
C
23
C
1
(a
2
C
2
+ a
3
C
23
)
S
1
C
23
C
1
S
1
C
23
S
1
(a
2
C
2
+ a
3
C
23
)
C
23
0 S
23
a
2
S
2
+ a
3
S
23
0 0 0 1
The unit vector s depends only on the rst joint. The position p
z
does not depend
on
1
.
Example: 3 dof spherical manipulator
Denavit-Hartenberg parameters:
d a
L1 0
1
0 90
o
L2 d
2

2
0 90
o
L3 d
3
0 0 0
o
The Denavit-Hartenberg matrices are:
0
H
1
=
C
1
0 S
1
0
S
1
0 C
1
0
0 1 0 0
0 0 0 1
,
1
H
2
=
C
2
0 S
2
0
S
2
0 C
2
0
0 1 0 d
2
0 0 0 1
,
2
H
3
=
1 0 0 0
0 1 0 0
0 0 1 d
3
0 0 0 1

The kinematic model results:
0
T
3
=
n s a p
0 0 0 1
C
1
C
2
S
1
C
1
S
2
d
2
S
1
+ d
3
C
1
S
2
C
2
S
1
C
1
S
1
S
2
d
2
C
1
+ d
3
S
1
S
2
S
2
0 C
2
d
3
C
2
0 0 0 1
The third joint J

3
does not aect the orientation, s depends only on J
1
.
If
1
=
2
= 0
o
, d
3
= d
0
T
3
=
1 0 0 0
0 1 0 d
2
0 0 1 d
0 0 0 1
y
0
z
0
x
0
F
0
d
2
d
y
3
z
3
x
3
F
3
If
1
=
2
= 90
o
, d
3
= d
0
T
3
=
0 1 0 d
2
0 0 1 d
1 0 0 0
0 0 0 1
y
0
z
0
x
0
F
0
d
2
d
z
3
x
3
y
3
F
3
Example: 3 dof spherical wrist
Denavit-Hartenberg parameters
d a
L4 0
4
0 90
o
L5 0
5
0 90
o
L6 d
6

6
0 0
o
Note the numbers starting from 4...
Then
3
H
4
=
C
4
0 S
4
0
S
4
0 C
4
0
0 1 0 0
0 0 0 1
,
4
H
5
=
C
5
0 S
5
0
S
5
0 C
5
0
0 1 0 0
0 0 0 1
,
5
H
6
=
C
6
S
6
0 0
S
6
C
6
0 0
0 0 1 d
6
0 0 0 1

The kinematic model is:
3
T
6
=
C
4
C
5
C
6
S
4
S
6
S
4
C
6
C
4
C
5
S
6
C
4
S
5
d
6
C
4
S
5
S
4
C
5
C
6
+ C
4
S
6
C
4
C
6
S
4
C
5
S
6
S
4
S
5
d
6
S
4
S
5
S
5
C
6
S
5
S
6
C
5
d
6
C
5
0 0 0 1
In this case, the rotation matrix has the same expression as an Euler ZYZ rotation
matrix.
R
Euler
(, , ) = Rot(z
0
, )Rot(y
1
, )Rot(z
2
, )
=
+ C
+ C
It means that the manipulators joints

4
,
5
and
6
are equivalent to the Euler
ZYZ angles.
If
1
=
2
=
3
= 0
o
3
T
6
=
1 0 0 0
0 1 0 0
0 0 1 d
6
0 0 0 1
If
1
=
2
=
3
= 90
o
3
T
6
=
1 0 0 0
0 0 1 d
6
0 1 0 0
0 0 0 1

Example: Stanford manipulator
By composing the 3 dof spherical manipu-
lator with the spherical wrist, the so-called
Stanford manipulator is obtained, a 6
dof robot.
Since the frames have been dened in a
consistent manner, the kinematic model is
simply obtained by multiplying the matri-
ces
0
T
3
of the arm and
3
T
6
of the wrist.
Then
0
T
6
=
0
T
3
3
T
6
=
n s a p
0 0 0 1

Example: Stanford manipulator
where
n =
S
1
(S
4
C
5
C
6
+ C
4
S
6
) + C
1
(C
2
(C
4
C
5
C
6
S
4
S
6
) S
2
S
5
C
6
)
C
1
(S
4
C
5
C
6
+ C
4
S
6
) + S
1
(S
2
S
5
C
6
+ C
2
(C
4
C
5
C
6
S
4
S
6
))
C
2
S
5
C
6
S
2
(C
4
C
5
C
6
S
4
S
6
)
o =
S
1
(C
4
C
6
S
4
C
5
S
6
) + C
1
(C
2
(S
4
C
6
+ C
4
C
5
S
6
) + S
2
S
5
S
6
)
C
1
(C
4
C
6
S
4
C
5
S
6
) + S
1
(S
2
S
5
S
6
C
2
(S
4
C
6
+ C
4
C
5
S
6
))
C
2
S
5
S
6
+ S
2
(S
4
C
6
+ C
4
C
5
S
6
)
a =
S
1
S
4
S
5
+ C
1
(S
2
C
5
+ C
2
C
4
S
5
)
C
1
S
4
S
5
+ S
1
(S
2
C
5
+ C
2
C
4
S
5
)
C
2
C
5
S
2
C
4
S
5
p =
d
2
S
1
+ d
3
C
1
S
2
+ d
6
(C
1
S
2
C
5
+ C
1
C
2
C
4
S
5
S
1
S
4
S
5
)
d
2
C
1
+ d
3
S
1
S
2
+ d
6
(S
1
S
2
C
5
+ S
1
C
2
C
4
S
5
+ C
1
S
4
S
5
)
d
3
C
2
+ d
6
(C
2
C
5
S
2
C
4
S
5
)

Example: PUMA 260
Joint variables
i
are dened about the
z
i 1
axes; a
2
is the distance between z
1
and z
2
(in this case parallel), d
3
is the
oset between the origins of F
2
and F
3
,
and d
4
is the oset between the origins of
F
3
and F
4
. Frames F
4
and F
5
coincides.
The
i
angles are either 0
o
or 90
o
.
d a
L1 0
1
0 90
o
L2 0
2
a
2
0
o
L3 d
3

3
0 90
o
L4 d
4

4
0 90
o
L5 0
5
0 90
o
L6 d
6

6
0 0
o
Example: PUMA 260
Canonical transformation matrices:
0
H
1
=
C
1
0 S
1
0
S
1
0 C
1
0
0 1 0 0
0 0 0 1
,
1
H
2
=
C
2
S
2
0 a
2
C
2
S
2
C
2
0 a
2
S
2
0 0 1 0
0 0 0 1
,
2
H
3
=
C
3
0 S
3
0
S
3
0 C
3
0
0 1 0 d
3
0 0 0 1
3
H
4
=
C
4
0 S
4
0
S
4
0 C
4
0
0
1
0 d
4
0 0 0 1
,
4
H
5
=
C
5
0 S
5
0
S
5
0 C
5
0
0 1 0 0
0 0 0 1
,
5
H
6
=
C
6
S
6
0 0
S
6
C
6
0 0
0 0 1 d
6
0 0 0 1

Example: PUMA 260
0
T
3
=
C
1
C
2
C
3
C
1
S
2
S
3
S
1
C
1
C
3
S
2
+ C
1
C
2
S
3
a
2
C
1
C
2
d
3
S
1
C
2
C
3
S
1
S
1
S
2
S
3
C
1
C
3
S
1
S
2
+ C
2
S
1
S
3
C
1
d
3
+ a
2
C
2
S
1
C
3
S
2
+ C
2
S
3
0 (C
2
C
3
) + S
2
S
3
a
2
S
2
0 0 0 1
3
T
6
=
C
4
C
5
C
6
S
4
S
6
(C
6
S
4
) C
4
C
5
S
6
C
4
S
5
C
4
d
6
S
5
C
5
C
6
S
4
+ C
4
S
6
C
4
C
6
C
5
S
4
S
6
S
4
S
5
d
6
S
4
S
5
(C
6
S
5
) S
5
S
6
C
5
d
4
+ C
5
d
6
0 0 0 1
0
T
6
=
0
T
6
=
0
T
3
3
T
6
=
n s a p
0 0 0 1

Example: PUMA 260
n =
S
1
(C
5
C
6
S
4
+ C
4
S
6
) + C
1
(C
2
((C
6
S
3
S
5
) + C
3
(C
4
C
5
C
6
S
4
S
6
)) S
2
(C
3
C
6
S
5
+ S
3
(C
4
C
5
C
6
S
4
S
6
)))
(C
1
(C
5
C
6
S
4
+ C
4
S
6
)) + S
1
(C
2
((C
6
S
3
S
5
) + C
3
(C
4
C
5
C
6
S
4
S
6
)) S
2
(C
3
C
6
S
5
+ S
3
(C
4
C
5
C
6
S
4
S
6
)))
S
2
((C
6
S
3
S
5
) + C
3
(C
4
C
5
C
6
S
4
S
6
)) + C
2
(C
3
C
6
S
5
+ S
3
(C
4
C
5
C
6
S
4
S
6
))
s =
S
1
(C
4
C
6
C
5
S
4
S
6
)+C
1
(C
2
(S
3
S
5
S
6
+ C
3
((C
6
S
4
) C
4
C
5
S
6
)) S
2
((C
3
S
5
S
6
) + S
3
((C
6
S
4
) C
4
C
5
S
6
)))
(C
1
(C
4
C
6
C
5
S
4
S
6
))+S
1
(C
2
(S
3
S
5
S
6
+ C
3
((C
6
S
4
) C
4
C
5
S
6
))S
2
((C
3
S
5
S
6
)+S
3
((C
6
S
4
) C
4
C
5
S
6
)))
S
2
(S
3
S
5
S
6
+ C
3
((C
6
S
4
) C
4
C
5
S
6
)) + C
2
((C
3
S
5
S
6
)+S
3
((C
6
S
4
) C
4
C
5
S
6
))
a =
S
1
S
4
S
5
+ C
1
(C
2
(C
5
S
3
+ C
3
C
4
S
5
) S
2
((C
3
C
5
) + C
4
S
3
S
5
))
(C
1
S
4
S
5
) + S
1
(C
2
(C
5
S
3
+ C
3
C
4
S
5
) S
2
((C
3
C
5
) + C
4
S
3
S
5
))
S
2
(C
5
S
3
+ C
3
C
4
S
5
) + C
2
((C
3
C
5
) + C
4
S
3
S
5
)
p =
S
1
(d
3
+ d
6
S
4
S
5
) + C
1
(a
2
C
2
+ C
2
((d
4
+ C
5
d
6
)S
3
+ C
3
C
4
d
6
S
5
) S
2
((C
3
(d
4
+ C
5
d
6
)) + C
4
d
6
S
3
S
5
))
(C
1
(d
3
+ d
6
S
4
S
5
)) + S
1
(a
2
C
2
+ C
2
((d
4
+ C
5
d
6
)S
3
+ C
3
C
4
d
6
S
5
) S
2
((C
3
(d
4
+ C
5
d
6
)) + C
4
d
6
S
3
S
5
))
a
2
S
2
+ S
2
((d
4
+ C
5
d
6
)S
3
+ C
3
C
4
d
6
S
5
) + C
2
((C
3
(d
4
+ C
5
d
6
)) + C
4
d
6
S
3
S
5
)

Example: planar 4 dof manipulator (redundant)
DH parameters
d a
L1 0
1
a
1
0
o
L2 0
2
a
2
0
o
L3 0
3
a
3
0
o
L4 0
4
a
4
0
o
All the
i 1
H
i
matrices have the same
structure
i 1
H
i
=
C
i
S
i
0 a
i
C
i
S
i
C
i
0 a
i
S
i
0 0 1 0
0 0 0 1

Example: planar 4 dof manipulator (redundant)
Then
0
T
4
=
0
H
1
1
H
2
2
H
3
3
H
4
=
n s a p
0 0 0 1
C
1234
S
1234
0 a
1
C
1
+ a
2
C
12
+ a
3
C
123
+ a
4
C
1234
S
1234
C
1234
0 a
1
S
1
+ a
2
S
12
+ a
3
S
123
+ a
4
S
1234
0 0 1 0
0 0 0 1
The vectors n, s, a dene the end-eector orientation (rotation about z), while p
denes its position (on the x y plane, p
z
= 0).
= The procedure can be applied also to redundant manipulators.
Claudio Melchiorri
Inverse Kinematic Model Introduction
Direct Kinematic Model:
The direct kinematic model consists in a function f(q) mapping the joint
position variables q IR
n
to the position/orientation of the end eector.
The denition of f(q) is conceptually simple, and a general approach for its
computation has been dened.
Inverse Kinematic Model:
The inverse kinematics consists in nding a function g(x) mapping the
position/orientation of the end-eector to the corresponding joint variables q:
the problem is not simple!
A general approach for the solution of this problem does not exist
On the other hand, for the most common kinematic structures, a scheme for
obtaining the solution has been found. Unfortunately
The solution is not unique. In general we have:
No solution (e.g. starting with a position x not in the workspace);
A nite set of solutions (one or more);
Innite solutions.
We seek for closed form solutions, and not based on numerical techniques:
The analytic solution is more ecient 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.
In order to obtain a closed form solution to the inverse kinematic problem, two
approaches are possible:
An algebraic approach, i.e. elaborations of the kinematic equations until a
suitable set of (simple) equations is obtained for the solution
A geometric approach based, when possible, on geometrical considerations,
dependent on the kinematic structure of the manipulator and that may help
in the solution.
Inverse Kinematic Model Algebraic Approach
Algebraic Approach
For a 6 dof manipulator, the kinematic model is described by the equation
0
T
6
=
0
H
1
(q
1
)
1
H
2
(q
2
) . . .
5
H
6
(q
6
)
equivalent to 12 equations in the 6 unknowns q
i
, i = 1, . . . , 6.
Example: spherical manipulator (only 3 dof)
T=
0.5868 0.6428 0.4394 0.4231

0.5265 0.7660 0.3687 0.9504
0.5736 0.0000 0.8192 0.4096
0 0 0 1
C
1
C
2
S
1
C
1
S
2
d
2
S
1
+ d
3
C
1
S
2
C
2
S
1
C
1
S
1
S
2
d
2
C
1
+ d
3
S
1
S
2
S
2
0 C
2
d
3
C
2
0 0 0 1
Since both the numerical values of

0
T
6
and the structure of the
i 1
H
i
matrices
are known, by suitable pre- / post-multiplications it is possible to obtain
[
0
H
1
(q
1
) . . .
i 1
H
i
(q
i
)]
1 0
T
6
[
j
H
j +1
(q
j +1
) . . .
5
H
6
(q
6
)]
1
=
i
H
i +1
(q
i +1
) . . .
j 1
H
j
(q
j
), i < j
obtaining 12 new equations for each couple (i , j ), i < j .
By selecting the most simple equations among all those obtained, it might be
possible to obtain a solution to the problem.
Inverse Kinematic Model Geometric Approach
Geometric Approach
General considerations that may help in nding 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
Inverse kinematics for the position (x, y, z) q
1
, q
2
, q
3
2
Inverse kinematics for the orientation R q
4
, q
5
, q
6
.
Geometric Approach
PIEPER APPROACH: Given a 6 dof manipulator, sucient condition to nd 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.
Geometric Approach
In many 6 dof industrial manipulators, the rst 3 dof are usually devoted to
position the wrist, that has 3 additional dof give the correct orientation to the
end-eector.
In these cases, it is quite simple to decompose the IK problem in the two
sub-problems (position and orientation).
Geometric Approach
In case of a manipulator with a spherical wrist, a natural choice is to decompose
the problem in
1
IK for the point p
p
(center of the spherical wrist)
2
solution of the orientation IK problem
Therefore:
1
The point p
p
is computed since
0
T
6
is known (submatrices R and p):
p
p
= p d
6
a
p
p
depends only on the joint variables q
1
, q
2
, q
3
;
2
The IK problem is solved for q
1
, q
2
, q
3
;
3
The rotation matrix
0
R
3
related to the rst three joints is computed;
4
The matrix
3
R
6
=
0
R
T
3
R is computed;
5
The IK problem for the rotational part is solved (Euler).
Inverse Kinematic Model Examples
Solution of the spherical manipulator
Direct kinematic model:
0
T
3
=
n s a p
0 0 0 1
C
1
C
2
S
1
C
1
S
2
d
2
S
1
+ d
3
C
1
S
2
C
2
S
1
C
1
S
1
S
2
d
2
C
1
+ d
3
S
1
S
2
S
2
0 C
2
d
3
C
2
0 0 0 1
If the whole matrix

0
T
3
is known, it is possible to compute:
1
= atan2 (s
x
, s
y
)
2
= atan2 (n
z
, a
z
)
d
3
= p
z
/ cos
2
=
Unfortunately, according to
the Pieper approach only p
is known!
We known only the position vector p.
From
0
T
3
=
0
H
1
1
H
2
2
H
3
we have
(
0
H
1
)
1 0
T
3
=
C
1
S
1
0 0
0 0 1 0
S
1
C
1
0 0
0 0 0 1
n
x
s
x
a
x
p
x
n
y
s
y
a
y
p
y
n
z
s
z
a
z
p
z
0 0 0 1
C
2
0 S
2
d
3
S
2
S
2
0 C
2
d
3
C
2
0 1 0 d
2
0 0 0 1
=
1
H
2
2
H
3
By equating the position vectors,
1
p
p
=
p
x
C
1
+ p
y
S
1
p
z
p
x
S
1
+ p
y
C
1
d
3
S
2
d
3
C
2
d
2
The vector
1
p
p
depends only on
2
and d
3
! Lets dene a = tan
1
/2, then
C
1
=
1 a
2
1 + a
2
S
1
=
2a
1 + a
2
By substitution in the last element of
1
p
p
(d
2
+ p
y
)a
2
+ 2p
x
a + d
2
p
y
= 0 = a =
p
x

p
2
x
+ p
2
y
d
2
2
d
2
+ p
y
Two possible solutions! ((p
2
x
+ p
2
y
d
2
2
) > 0??)
Then
1
= 2 atan2 (p
x

p
2
x
+ p
2
y
d
2
2
, d
2
+ p
y
)
Vector
1
p
p
is dened as
1
p
p
=
p
x
C
1
+ p
y
S
1
p
z
p
x
S
1
+ p
y
C
1
d
3
S
2
d
3
C
2
d
2
From the rst two elements

p
x
C
1
+ p
y
S
1
p
z
=
d
3
S
2
d
3
C
2
from which
2
= atan2 (p
x
C
1
+ p
y
S
1
, p
z
)
Finally, if the rst two elements are squared and added together
d
3
=
(p
x
C
1
+ p
y
S
1
)
2
+ p
2
z
Note that two possible solutions exist considering the position of the end-eector
(wrist) only. If also the orientation is considered, the solution (if exists) is unique.
We have seen that the relation (p
2
x
+ p
2
y
d
2
2
) > 0 must hold:
Numerical example: Given a spherical manipulator with d
2
= 0.8 m in the pose
1
= 20
o
,
2
= 30
o
, d
3
= 0.5 m
we have
0
T
3
=
0.8138 0.342 0.4698 0.0387

0.2962 0.9397 0.171 0.8373
0.5 0 0.866 0.433
0 0 0 1
The solution based on the whole matrix

0
T
3
is:
1
= 20
o
,
2
= 30
o
, d
3
= 0.5.
The solution based on the vector p gives:
a)
1
= 20
o
,
2
= 30
o
, d
3
= 0.5 (with the + sign).
b)
1
= 14.7
o
,
2
= 30
o
, d
3
= 0.5 (with the - sign).
The solution based on the vector p gives:
a)
1
= 20
o
,
2
= 30
o
, d
3
= 0.5 (with the + sign).
b)
1
= 14.7
o
,
2
= 30
o
, d
3
= 0.5 (with the - sign).
Solution of the 3 dof anthropomorphic manipulator
From the kinematic structure, one obtains
1
= atan2 (p
y
, p
x
)
Concerning
2
and
3
, note that the arm
moves in a plane dened by
1
only.
One obtains
C
3
=
p
2
x
+ p
2
y
+ p
2
z
a
2
2
a
2
3
2a
2
a
3
S
3
=
1 C
2
3
3
= atan2 (S
3
, C
3
)
Moreover, by geometrical arguments, it is possible to state that:
2
= atan2 (p
z
,
p
2
x
+ p
2
y
) atan2 (a
3
S
3
, a
2
+ a
3
C
3
)
Solution of the 3 dof anthropomorphic manipulator
Note that also the solution is valid
1
= + atan2 (p
y
, p
x
),
2
=
2
Then, FOUR possible solutions exist:
shoulder right - elbow up; shoulder right - elbow down;
shoulder left - elbow up; shoulder left - elbow down;
Same position, but dierent orientation!
Note that the conditions p
x
= 0, p
y
= 0 must hold (singular conguration).
Solution of the spherical wrist
Let us consider the matrix
3
R
6
=
n
x
s
x
a
x
n
y
s
y
a
y
n
z
s
z
a
z
From the direct kinematic equations one obtains

3
R
6
=
C
4
C
5
C
6
S
4
S
6
S
4
C
6
C
4
C
5
S
6
C
4
S
5
S
4
C
5
C
6
+ C
4
S
6
C
4
C
6
S
4
C
5
S
6
S
4
S
5
S
5
C
6
S
5
S
6
C
5

3
R
6
=
C
4
C
5
C
6
S
4
S
6
S
4
C
6
C
4
C
5
S
6
C
4
S
5
S
4
C
5
C
6
+ C
4
S
6
C
4
C
6
S
4
C
5
S
6
S
4
S
5
S
5
C
6
S
5
S
6
C
5
The solution is then computed as (ZYZ Euler angles):
5
[0, ]:
4
= atan2 (a
y
, a
x
)
5
= atan2 (
a
2
x
+ a
2
y
, a
z
)
6
= atan2 (s
z
, n
z
)
5
[, 0]:
4
= atan2 (a
y
, a
x
)
5
= atan2 (
a
2
x
+ a
2
y
, a
z
)
6
= atan2 (s
z
, n
z
)
Numerical example: Let us consider a spherical wrist in the pose
4
= 10
o
5
= 20
o
,
6
= 30
o
Then
3
R
6
=
0.7146 0.6131 0.3368

0.6337 0.7713 0.0594
0.2962 0.1710 0.9397
Therefore, if

5
[0, ]
4
= 10
o
5
= 20
o
,
6
= 30
o

5
[, 0]
4
= 170
o
5
= 20
o
,
6
= 30
o
Note that the PUMA has an anthropomorphic structure (4 solutions) and a
spherical wrist (2 solutions):
= 8 dierent congurations are possible!
Claudio Melchiorri
Dierential Kinematics Introduction
Dierential Kinematics: the Jacobian matrix
In robotics it is of interest to dene, besides the
mapping between the joint and workspace posi-
tion/orientation (i.e. the kinematic equations),
also
The relationship between the joints and
end-eector velocities:
q
The relationship between the force applied
on the environment by the manipulator and
the corresponding joint torques
f
n

These two relationships are based on a linear operator, a matrix, called the
Jacobian of the manipulator.
Dierential Kinematics Introduction
The Jacobian matrix
In robotics, the Jacobian is used for several purposes:
To dene the relationship between joint and workspace velocities
To dene the relationship between forces/torques between the spaces
To study the singular congurations
To dene numerical procedures for the solution of the IK problem
To study the manipulability properties.
Dierential Kinematics The Jacobian Matrix
Velocity domain
The translational and rotational velocities are considered separately.
Let us consider two frames F
0
(base frame) and F
1
(integral with the rigid body).
The translational velocity of point p of the rigid body, with respect to F
0
, is
dened as the derivative (with respect to time) of p, denoted as p:
p =
d p
dt
Velocity domain
With respect to the rotational velocity, two dierent denitions are possible:
1
A triple IR
3
giving the orientation of F
1
with respect to F
0
(Euler,
RPY,... angles) is adopted, and its derivative is used to dene the rotational
velocity
=
d
dt
2
An angular velocity vector is dened, giving the rotational velocity of a
third frame F
2
with origin coincident with F
0
and axes parallel to F
1
.
The velocity vector is placed in the ori-
gin, and its direction coincides with the
instantaneous rotation axis of the rigid
body.
Analytical and Geometrical expressions of the Jacobian
The two descriptions lead to dierent results concerning the expression of the
Jacobian matrix, in particular in the part relative to the rotational velocity.
One obtains respectively the:
Analytic Jacobian
Geometric Jacobian
Two problems:
1) Integration of the rotational velocity
dt = : orientation of the rigid body
dt = ??
Example: Lets consider a rigid body and the following rotational velocities
Case a)
= [/2, 0, 0]
T
0 t 1
= [0, /2, 0]
T
1 < t 2
Case b)
= [0, /2, 0]
T
0 t 1
= [/2, 0, 0]
T
1 < t 2
By integrating the velocities in the two case, one obtains:
2
0
dt = [/2, /2, 0]
T
Case a)
= [/2, 0, 0]
T
0 t 1
= [0, /2, 0]
T
1 < t 2
Case b)
= [0, /2, 0]
T
0 t 1
= [/2, 0, 0]
T
1 < t 2
2
0
dt = [/2, /2, 0]
T
On the other hand, the rotation matrices in the two cases are:
R
a
=
0 1 0
0 0 1
1 0 0
R
b
=
0 0 1
1 0 0
0 1 0
The integration of does not have a clear physical interpretation.

2) On the other hand:
represents the velocity components about the three axes of F
0
,
the elements of are dened with respect to frame that: a) is not Cartesian
(its axes are not orthogonal each other); b) varies in time according to .
The two expressions of the Jacobian matrix physically dene the same
phenomenon (velocity of the manipulator), and therefore a relationship between
them must exist. For example, if the Euler angles , , are used for the triple ,
it is possible to show that
=
0 sin cos sin

0 cos sin sin
1 0 cos
= T()
Note that matrix T() is singular when
sin = 0.
In this case, some rotational velocities
may be expressed by and not by , e.g.
= [cos , sin , 0]
T
.
These cases are called representation singu-
larities of .
Denition of matrix T():
: [
x
,
y
,
z
]
T
=

0
0
1

z
=

: [
x
,
y
,
z
]
T
=

x
= S
y
= C
: [
x
,
y
,
z
]
T
=

z
= C
x
= S
y
= S

If sin = 0, then the components perpendicular to z of the velocity expressed by
are linearly dependent (
2
x
+
2
y
=

2
), while physically this constraint may not
exist!
From:
=
0 sin cos sin

0 cos sin sin
1 0 cos

one obtains:
0 S
0
0 C
0
1 0 1

2
x
+
2
y
=

z
=

+


In general, given a triple of angles , a transformation matrix T() exists such that
= T()
Once matrix T() is known, it is possible to relate the analytical and geometrical
expressions of the Jacobian matrix:
I 0
0 T()

p

Then
J
G
=
I 0
0 T()
J
A
= T
A
()J
A
Dierential Kinematics Analytical Jacobian
Analitycal Jacobian
The analytical expression of the Jacobian is obtained by dierentiating a vector
x = f(q) IR
6
, that denes the position and orientation (according to some
convention) of the manipulator in F
0
.
By dierentiating f(q), one obtains
dx =
f(q)
q
dq
= J(q)dq
where the m n matrix
J(q) =
f(q)
q
=
f
1
q
1
f
1
q
2
. . .
f
1
q
n
. . .
f
m
q
1
f
m
q
2
. . .
f
m
q
n
J(q) IR
mn
is called Jacobian matrix or JACOBIAN of the manipulator.
Analitycal Jacobian
If the innitesimal period of time dt is considered, on obtains
d x
dt
= J(q)
d q
dt
that is
x =
= J(q) q
relating the velocity vector x expressed in F
0
and the joint velocity vector q.
The elements J
i ,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) IR
mn
).
Example: 2 dof manipulator
d a
L1 0
1
a
1
0
o
L2 0
2
a
2
0
o
The end-eector position is
p
x
= a
1
C
1
+ a
2
C
12
p
y
= a
1
S
1
+ a
2
S
12
p
z
= 0
If is composed by the Euler angles , , dened about axes z
0
, y
1
, z
2
, and
considering that the z axes of the base frame and of the end eector are parallel,
the total rotation is equivalent to a single rotation about z
0
and therefore:
1
+
2
0
0

Euler angles:
1
+
2
0
0
By dierentiation of the expressions of p and one obtains
a
1
S
1
a
2
S
12
a
2
S
12
a
1
C
1
+ a
2
C
12
a
2
C
12
0 0
1 1
0 0
0 0
q
= J(q) q
Dierential Kinematics Geometric Jacobian
Geometric Expression of the Jacobian
The geometric expression of the Jacobian is obtained considering the rotational
velocity vector .
Each column of the Jacobian matrix denes the eect of the i -th joint on the
end-eector velocity. It is divided in two terms.
The rst term considers the eect of q
i
on the linear velocity v, while the second
one on the rotational velocity , i.e.
= J q = J =
J
v1
J
v2
. . . J
vn
J
1
J
2
. . . J
n
Therefore
v = J
v1
q
1
+J
v2
q
2
+ . . . +J
vn
q
n
= J
1
q
1
+J
2
q
2
+ . . . +J
n
q
n
The analytic and geometric Jacobian dier for the rotational part.
In order to obtain the geometric Jacobian, a general method based on the
geometrical structure of the manipulator is adopted.
Derivative of a Rotation Matrix
Lets consider a rotation matrix R = R(t) and R(t)R
T
(t) = I .
Lets derive the equation: R(t)R
T
(t) = I =

R(t)R
T
(t) +R(t)

R
T
(t) = 0
A 3 3 (skew-symmetric) matrix S(t) is obtained
S(t) =

R(t)R
T
(t)
As a matter of fact
S(t) +S
T
(t) = 0 = S =
0
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.
Physical interpretation:
Matrix S(t) is expressed as a function of a vector (t) = [
x
,
y
,
z
]
T
representing the angular velocity of R(t).
Consider a constant vector p
and the vector p(t) = R(t)p
. The time derivative

of p(t) is
p(t) =

R(t)p
which can be written as

p(t) = S(t)R(t)p
= (R(t) p
)
This last results, i.e. p(t) = (R(t) p
), is well known from the classic

mechanics of rigid bodies.
Moreover
R S() R
T
= S(R )
i.e. the matrix form of S() in a frame rotated by R is the same as the
skew-symmetric matrix S(R ) of the vector rotated by R.
Consider two frames F and F, which dier by the rotation R (
= R ). Then
S(
) operates on vectors in F and S() on vectors in F.

Consider a vector v
a
in F and assume that some operations must be performed
on that vector in F (then using S). It is necessary to:
1
Transform the vectors from F to F (by R
T
)
2
Use S()
3
Transform back the result to F (by R)
That is: v
b
= R S() R
T
v
a
equivalent to the transformation using S(
): v
b
= S(
) v
a
Example
Consider the elementary rotation about z
Rot(z, ) =
cos sin 0
sin cos 0
0 0 1
If is a function of time
S(t) =
sin
cos 0
cos
sin 0
0 0 0
cos sin 0
sin cos 0
0 0 1
0 0
0 0 0
= S((t))
Then
=
0
0
i.e. a rotational velocity about z.

Geometric Jacobian
The end-eector velocity is a linear composition of the joint velocities
v = J
v1
q
1
+J
v2
q
2
+ . . . +J
vn
q
n
= J
1
q
1
+J
2
q
2
+ . . . +J
n
q
n
Each column of the Jacobian
matrix denes the eect of the
i -th joint on the end-eector ve-
locity.
Geometric Jacobian
It is possible to show (see Appendix) that the i -th column of the Jacobian can be
computed as
J
vi
J
i
0
z
i 1
(
0
p
n
0
p
i 1
)
0
z
i 1
revolute joint
J
vi
J
i
0
z
i 1
0
prismatic joint
where
0
z
i 1
and
0
r
i 1,n
=
0
p
n
0
p
i 1
depend on the joint variables
q
1
, q
2
, ..., q
n
. In particular:
0
p
n
is the end-eector position, dened in the rst three elements of the last
column of
0
T
n
=
0
H
1
(q
1
) . . .
n1
H
n
(q
n
);
0
p
i 1
is the position of F
i 1
, dened in the rst three elements of the last
column of
0
T
i 1
=
0
H
1
(q
1
) . . .
i 2
H
i 1
(q
i 1
);
0
z
i 1
is the third column of
0
R
i 1
:
0
R
i 1
=
0
R
1
(q
1
)
1
R
2
(q
2
) . . .
i 2
R
i 1
(q
i 1
)
Dierential Kinematics Examples
The Jacobian is computed as
J =
z
0
(p
2
p
0
) z
1
(p
2
p
1
)
z
0
z
1
The origins of the frames are

p
0
=
0
0
0
p
1
=
a
1
C
1
a
1
S
1
0
p
2
=
a
1
C
1
+ a
2
C
12
a
1
S
1
+ a
2
S
12
0
and the rotational axes are

z
0
= z
1
=
0
0
1

Then
z
0
(p
2
p
0
) =
0 0 a
1
S
1
+ a
2
S
12
0 0 a
1
C
1
a
2
C
12
a
1
S
1
a
2
S
12
a
1
C
1
+ a
2
C
12
0
0
0
1
a
1
S
1
a
2
S
12
a
1
C
1
+ a
2
C
12
0
z
1
(p
2
p
1
) =
0 0 a
2
S
12
0 0 a
2
C
12
a
2
S
12
a
2
C
12
0
0
0
1
a
2
S
12
a
2
C
12
0
In conclusion:
J(q) =
a
1
S
1
a
2
S
12
a
2
S
12
a
1
C
1
+ a
2
C
12
a
2
C
12
0 0
0 0
0 0
1 1

Jacobian:
J(q) =
a
1
S
1
a
2
S
12
a
2
S
12
a
1
C
1
+ a
2
C
12
a
2
C
12
0 0
0 0
0 0
1 1
If the orientation is not of interest, only the rst two rows may be considered
J(q) =
a
1
S
1
a
2
S
12
a
2
S
12
a
1
C
1
+ a
2
C
12
a
2
C
12

Example: 3dof anthropomorphic manipulator
The canonical transformation matrices are
0
H
1
=
C
1
0 S
1
0
S
1
0 C
1
0
0 1 0 0
0 0 0 1
1
H
2
=
C
2
S
2
0 a
2
C
2
S
2
C
2
0 a
2
S
2
0 0 1 0
0 0 0 1
2
H
3
=
C
3
S
3
0 a
3
C
3
S
3
C
3
0 a
3
S
3
0 0 1 0
0 0 0 1
and the kinematic model

0
T
3
=
C
1
C
23
C
1
S
23
S
1
C
1
(a
2
C
2
+ a
3
C
23
)
S
1
C
23
S
1
S
23
C
1
S
1
(a
2
C
2
+ a
3
C
23
)
S
23
C
23
0 a
2
S
2
+ a
3
S
23
0 0 0 1

The Jacobian results
J =
z
0
(p
3
p
0
) z
1
(p
3
p
1
) z
2
(p
3
p
2
)
z
0
z
1
z
2
where
p
0
= p
1
=
0
0
0
p
2
=
a
2
C
1
C
2
a
2
S
1
S
2
a
2
S
2
p
3
=
C
1
(a
2
C
2
+ a
3
C
23
)
S
1
(a
2
C
2
+ a
3
C
23
)
a
2
S
2
+ a
3
S
23
The rotational axes are

z
0
=
0
0
1
z
1
= z
2
=
S
1
C
1
0

Therefore
J =
S
1
(a
2
C
2
+ a
3
C
23
) C
1
(a
2
S
2
+ a
3
S
23
) a
3
C
1
S
23
C
1
(a
2
C
2
+ a
3
C
23
) S
1
(a
2
S
2
+ a
3
S
23
) a
3
S
1
S
23
0 a
2
C
2
+ a
3
C
23
a
3
C
23
0 S
1
S
1
0 C
1
C
1
1 0 0
Only three rows are linearly independent (3dof). By considering the linear velocity
one obtains:
J =
S
1
(a
2
C
2
+ a
3
C
23
) C
1
(a
2
S
2
+ a
3
S
23
) a
3
C
1
S
23
C
1
(a
2
C
2
+ a
3
C
23
) S
1
(a
2
S
2
+ a
3
S
23
) a
3
S
1
S
23
0 a
2
C
2
+ a
3
C
23
a
3
C
23
Note that it is not possible to achieve any rotational velocity.

Moreover S
1
y
= C
1
x
Note that:
1
does not aect v
z
(nor
x
,
y
)
z
depends only by
1
S
1
y
= C
1
x
:
x
and
y
are not independent
the rst three rows may also be obtained by derivation of
0
p
3
In the position case
det(J) = a
2
a
3
S
3
(a
2
C
2
+ a
3
C
23
)
Therefore det(J) = 0 in two cases:
S
3
= 0 =
3
=
(a
2
C
2
+ a
3
C
23
) = 0 i.e. when the wrist is on the z axis (p
x
= p
y
= 0):
shoulder singularity
Example 3 dof spherical manipulator
Canonical transformation matrices
0
H
1
=
C
1
0 S
1
0
S
1
0 C
1
0
0 1 0 0
0 0 0 1
,
1
H
2
=
C
2
0 S
2
0
S
2
0 C
2
0
0 1 0 d
2
0 0 0 1
2
H
3
=
1 0 0 0
0 1 0 0
0 0 1 d
3
0 0 0 1
Kinematic model:
0
T
3
=
C
1
C
2
S
1
C
1
S
2
d
2
S
1
+ d
3
C
1
S
2
C
2
S
1
C
1
S
1
S
2
d
2
C
1
+ d
3
S
1
S
2
S
2
0 C
2
C
2
d
3
0 0 0 1

The Jacobian is
J =
z
0
(p
3
p
0
) z
1
(p
3
p
1
) z
2
z
0
z
1
0
with
z
0
=
0
0
1
z
1
=
S
1
C
1
0
z
2
=
C
1
S
2
S
1
S
2
C
2
and
p
0
= p
1
=
0
0
0
p
2
=
d
2
S
1
d
2
C
1
0
p
3
=
d
2
S
1
+ d
3
C
1
S
2
d
2
C
1
+ d
3
S
1
S
2
C
2
d
3

Then
J =
d
2
C
1
d
3
S
1
S
2
d
3
C
1
C
2
C
1
S
2
d
2
S
1
+ d
3
C
1
S
2
d
3
S
1
C
2
S
1
S
2
0 d
3
S
2
C
2
0 S
1
0
0 C
1
0
1 0 0
Note that:
q
3
does not aect ;
z
depends only on q
1
;
S
1
y
= C
1
x
.
J =
d
6
S
4
S
5
d
6
C
4
C
5
0
d
6
C
4
S
5
d
6
C
5
S
4
0
0 d
6
S
5
0
0 S
4
C
4
S
5
0 C
4
S
4
S
5
1 0 C
5
By choosing d
6
= 0, i.e. the origin of F
6
is in the intersection point of the three
joint axes, then
J =
0 0 0
0 0 0
0 0 0
0 S
4
C
4
S
5
0 C
4
S
4
S
5
1 0 C
5
With hteos expression, however, the

linear velocity of the end-eector is not
computed.
det(J) = 0 = S
5
= 0, i.e.
5
= 0, .
In this case it is not possible to determine
individually

4
and

6
.
Example: PUMA 260
Only revolute joints are present:
J =
z
0
(p
6
p
0
) z
1
(p
6
p
1
) z
2
(p
6
p
2
)
z
0
z
1
z
2
z
3
(p
6
p
3
) z
4
(p
6
p
4
) z
5
(p
6
p
5
)
z
3
z
4
z
5

Example: PUMA 260
If d
6
= 0:
J =
d
3
C
1
S
1
(a
2
C
2
+ d
4
S
23
) C
1
(d
4
C
23
a
2
S
2
) d
4
C
1
C
23
d
3
S
1
+ C
1
(a
2
C
2
+ d
4
S
23
) S
1
(d
4
C
23
a
2
S
2
) d
4
S
1
C
23
0 a
2
C
2
+ d
4
S
23
d
4
S
23
0 S
1
S
1
0 C
1
C
1
1 0 0
0 0 0
0 0 0
0 0 0
C
1
S
23
S
1
C
4
C
1
C
23
S
4
C
1
S
23
C
5
+ C
1
C
23
C
4
S
5
+ S
1
S
4
S
5
S
1
S
23
C
1
C
4
S
1
C
23
S
4
S
1
S
23
C
5
+ S
1
C
23
C
4
S
5
C
1
S
4
S
5
C
23
S
23
S
4
C
23
C
5
+ S
23
C
4
S
5

Claudio Melchiorri
Statics - Singularities - Inverse dierential kinematics Statics
Forces
The relation x = J q maps velocities dened 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:
w
T
dx =
T
dq
being w = [f
T
n
T
]
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.
Statics - Singularities - Inverse dierential kinematics Statics
Forces
By recalling that
dx = J(q)dq
one obtains
= J
T
(q)w
dening the relationship between the joint torque vector and the force vector w,
applied to the manipulator.
J(q) = mapping between q and [v
T
,
T
]
T
J
T
(q) = mapping between [f
T
, n
T
]
T
and
q ()
J(q)
J
T
(q)
x (w)
Statics - Singularities - Inverse dierential kinematics Transformation of reference frame for the Jacobian
Transformation of reference frame for the Jacobian
Lets consider two frames F
a
and
F
b
attached to a rigid body (robot).
If
a
x is the end-eector velocity in F
a
, then
a
x =
a
J q
It is known that it is possible to transform the same velocity in another frame
F
b
as
b
x =
b
G
a
a
x
where
b
G
a
is the transformation matrix between the two frames:
b
G
a
=
b
R
a

b
R
a
a
P
ab
0
b
R
a

Statics - Singularities - Inverse dierential kinematics Transformation of reference frame for the Jacobian
Transformation of reference frame for the Jacobian
Then
b
x =
b
J q =
b
G
a
a
x =
b
G
a
a
J q
and therefore the transformation for the Jacobian between the two frames F
a
and
F
b
is given by
b
J =
b
G
a
a
J
Similar considerations hold for the force domain (where the tanspose Jacobian is
used).
Note that if the two frames are not rigidly attached to the robot, then the
Jacobian transformation between them is dened only by the rotation matrix
b
R
a
:
b
J =
b
R
a
0
0
b
R
a
a
J
Statics - Singularities - Inverse dierential kinematics Kinematic singularities
Singular congurations
The Jacobian is a 6 n matrix mapping the IR
n
joint velocity space to the IR
6
operational velocity space:
x = J(q) q
From an innitesimal point of view, this relationship is expressed as
dx = J(q)dq
that can be interpreted as a relationship between innitesimal displacements in
IR
n
and IR
6
.
Singular congurations 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 congurations exist such that the Jacobian
looses rank. These congurations are termed kinematic singularities.
In these congurations, there are directions (vectors x) in IR
6
without any
correspondent direction ( q) in IR
n
: these directions cannot be actuated and the
robot looses a motion capability.
Singular congurations
The singular congurations are important for several reasons:
1
They represents congurations in which the motion capabilities of the robot are
reduced: it is not possible to impose arbitrary motions of the end-eector
2
In the proximity of a singularity, small velocities in the operational space may
generate large (innite) velocities in the joint space
3
They correspond to congurations that have not a well dened solution to the
inverse kinematic problem: either no solution or innite solutions.
There are two types of singularities:
1
Boundary singularities, that correspond to points on the border of the workspace,
i.e. when the robot is either fully extended or retracted. These singularities may be
easily avoided by not driving the manipulator to the border of its workspace
2
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-eector congurations. These singularities constitute a serious
problem, as they can be encountered anywhere in the reachable workspace for a
planned path in the operational space.
Example
Consider the Jacobian J(q)
J =
1 1
0 0
Then rank(J) = 1 and

dx = dq
1
+ dq
2
dy = 0
For any value of dq
1
, dq
2
, then dy = 0.
Any vector dx whose second element is not null represents an instantaneous
motion that cannot be achieved.
Example
Jacobian:
J =
1 1
0 0
In the force domain

= J
T
f =
1 0
1 0

f
x
f
y
then
1
= f
x
2
= f
x
then, f
y
does not aect the joint torques, and any torque vector such that
1
=
2
does not generate any force on the environment.
Example
Consider the 2 dof planar manipulator:
J(q) =
a
1
S
1
a
2
S
12
a
2
S
12
a
1
C
1
+ a
2
C
12
a
2
C
12
If
1
=
2
= 0, then
J(q) =
0 0
a
1
+ a
2
a
2
0 0
2 1
if a
1
= a
2
= 1
Therefore

dx = 0
dy = 2 dq
1
+ dq
2
Moreover
J
T
(q) =
0 2
0 1

1
= 2 f
y
2
= f
y
If
2
= 2
1
f
x
= f
y
= 0.
Example
J(q) =
a
1
S
1
a
2
S
12
a
2
S
12
a
1
C
1
+ a
2
C
12
a
2
C
12
Consider the velocity vector x = [1, 1]

T
. By computing q = J
1
(q) x:
If
1
= 0
o
,
2
= 1
o
then q =
58.9
115.59
(rad/sec) =
3374.7
6622.8
(
o
/sec)
If
1
= 0
o
,
2
= 10
o
then q =
6.67
12.43
(rad/sec) =
382.16
712.18
(
o
/sec)
Example
J(q) =
a
1
S
1
a
2
S
12
a
2
S
12
a
1
C
1
+ a
2
C
12
a
2
C
12
Plot of q = J(q)
1
x
with x =
1
1
and
1
= 0,
2
[0.0005, 0.01] rad
0 0.001 0.002 0.003 0.004 0.005 0.006 0.007 0.008 0.009 0.01
3000
2000
1000
0
1000
2000
3000
4000
5000
q2 [rad]
d
q
1
,

d
q
2

[
r
a
d
/
s
e
c
]
Velocita di giunto (dq2 dash)
Singularity decoupling
In case of complex structures, the analysis of the kinematic singularities via the
Jacobian determinant det(J) may result quite dicult.
In case of manipulators with spherical wrist, by similarity with the inverse
kinematics, it is possible to decompose the study of the singular congurations
into two sub-problems:
arm singularities (e.g. the rst three joints)
wrist singularities
If J IR
6n
then
J =
J
11
J
12
J
21
J
22
where, since the last three joints are of the revolute type:
J
12
= [z
3
(p
6
p
3
), z
4
(p
6
p
4
), z
5
(p
6
p
5
)] J
22
= [z
3
, z
4
, z
5
]
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-eector frame at the intersection of the wrist axes, where also the last frames
are placed. Then:
J
12
= [0, 0, 0] = J =
J
11
0
J
21
J
22
In this manner, J is a block lower-triangular matrix, and

det(J) = det(J
11
)det(J
22
)
The singularities are then decoupled, since
det(J
11
) = 0 gives the arm singularities, while
det(J
22
) = 0 gives the wrist singularities.
Statics - Singularities - Inverse dierential kinematics Inverse Dierential Kinematics
Inverse Dierential Kinematics
The direct relationship between joint and operational space velocities:
x = J(q) q
is dened by the m n Jacobian matrix J.
Inverse problem: x = q
A solution of the linear system x = J(q) q is seeked.
In case m = n, the inverse of the Jacobian matrix is employed:
q = J
1
(q) x
Otherwise, it is necessary to use its (Moore-Penrose) pseudo-inverse
q = J
+
(q) x
where:
J
+
= J
T
(JJ
T
)
1
if m < n (right pseudo-inverse : JJ
+
= I)
J
+
= (J
T
J)
1
J
T
if m > n (left pseudo-inverse: J
+
J = I)
If m = n there are two cases:
J
a) m < n
J
b) m > n
a): JJ
+
= I
m
J
J
T
(JJ
T
)
1
= I
m
= J
+
= J
T
(JJ
T
)
1
b): J
+
J = I
n
(J
T
J)
1
J
T
J = I
n
= J
+
= (JJ
T
)
1
J
T
Solution of x = J q if m = n, det(J) = 0
J(q)
J
1
(q)
The equation x = J q (as well as q = J
1
x) has an unique solution:
x
o
! q
o
= J
1
x
o
such that x
o
= J q
o
= J J
1
x
o
Solution of x = J q if m < n
N(J)
IR
n
0
J(q)
IR
m
rank(J) = min(m, n) = m R(J) = IR
m
x q such that x = J q (multiple solutions!)
q = J
+
x N(J) such that q N(J) x = J q = 0
q = J
+
x + q
N
x = J (J
+
x + q
N
) = x, q
N
N(J)
q = J
+
x +(I J
+
J)y general expression of the solution (I J
+
J) base of N(J)
q = J
+
x has minimum norm
Solution o x = J q if m > n
IR
n
R(J)
IR
m
rank(J) = min(m, n) = n
q ! x such that x = J q
x R(J) ! q such that x = J q ( q = J
1
x)
If x R(J) q such that x = J q
If x
0
R(J) q
0
= J
+
x
0
x = J q
0
= JJ
+
x
0
= x
0
(JJ
+
= I)
x x
0
is minimum
Solution of x = J q in general if m = n
N(J)
IR
n
R(J)
IR
m
rank(J) = p < min(m, n)
The solution given by the pseudo-inverse q
s
= J
+
x is such that ( x
s
= J q
s
):
x
s
x the norm of the error is minimum
q
s
the norm of the solution is minimum
Statics - Singularities - Inverse dierential kinematics Accelerations
Accelerations
If accelerations are of interest, by dierentiating x = J q one obtains
x = J(q) q +
d J(q)
dt
q
If an acceleration vector x is given in the operational space, the corresponding
vector q is computed as a solution of
b = J(q) q
being b = x
d J(q)
dt
q. Then
q = J
1
b if the inverse of the Jacobian exists
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 = J q (b = J q) exists i x R(J) (b R(J)). A
vector a is in R(A) i
rank([A, a]) = rank[A]
Claudio Melchiorri
Inverse kinematics algorithms Introduction
The Jacobian matrix can be used also for the solution of the inverse kinematic
problem.
If a desired trajectory is known in terms of the velocity v(kT) = v
k
, a simple
approach is to consider
q
k+1
= q
k
+J
1
(q
k
)v
k
T
equivalent to a numerical integration over time of the velocity. This operation has
two major drawbacks aecting the computation of the exact solution:
numerical drifts
initialization problems
These problems may be avoided by implementing a feedback scheme accounting
for the operational space errors e = x
d
x. Then
e = x
d
x = x
d
J
a
(q) q (2)
The vector q must be chosen so that the error e converges to zero.
Inverse kinematics algorithms Jacobian (pseudo)-inverse
Scheme 1: Jacobian (pseudo)-inverse
Assuming that J
a
is invertible, then q = J
1
a
(q) ( x
d
+Ke)
By substituting this expression in (2) one obtains
e +Ke = 0
representing, if K is positive denite, an asymptotically stable system. Note that
the convergence velocity depends on K.
K
J
1
a
(q)
f()
j j - -
- - - -
6
?
x
d
x
d
e q
q
x
Inverse kinematics algorithms Jacobian transpose
Scheme 2: Jacobian transpose
This scheme is based on the Lyapunov approach. A Lyapunov function must be
found guaranteeing the convergence to zero of the error e.
Lets assume
V(e) =
1
2
e
T
K e
with K symmetric and positive denite. In this manner
V(e) > 0, e = 0, V(0) = 0
By dierentiating V(e) one obtains
V = e
T
K e = e
T
K x
d
e
T
K x
= e
T
K x
d
e
T
KJ
a
(q) q
Scheme 2: Jacobian transpose
From
V = e
T
K x
d
e
T
KJ
a
(q) q
By choosing
q = J
T
a
(q)Ke
one obtains
V = e
T
K x
d
e
T
KJ
a
(q)J
T
a
(q)Ke
If x
d
= 0 then

V < 0 and the system is asymptotically stable if J
a
is full rank.
If J
a
is not full rank, then the condition q = 0 may be obtained also with e = 0
(Ke null (J
T
a
)).
K J
T
a
(q)
f()
j - -
- -
6
x
d
e q
q
x
-
Example
Consider the non linear function
z = f(q) =
x
y
q
3
1
+ sin(q
1
q
2
)
q
1
q
3
2
+ sin(q
2
1
+ 2q
2
)
The Jacobian is
J(q) =
3q
2
1
+ q
2
cos(q
1
q
2
) q
1
cos(q
1
q
2
)
q
3
2
+ 2q
1
cos(q
2
1
+ 2q
2
) 3q
1
q
2
2
+ 2cos(q
2
1
+ 2q
2
)
Assuming z
0
= [0.1, 0.2]
T
, nd q
0
= f
1
(z
0
).
Example
With null initial conditions (q
1
= q
2
= 0), the following results are computed with
the two algorithms (T
s
= 0.001 s).
K = 100
(pseudo-) Inverse J
1
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2
0.6
0.4
0.2
0
0.2
Algoritmo J^1; valori x, y (dash)
time [s]
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2
1.5
1
0.5
0
Valori q1, q2 (dash)
time [s]
Transpose J
T
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2
0.6
0.4
0.2
0
0.2
Algoritmo J^T; valori x, y (dash)
time [s]
0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18 0.2
1.5
1
0.5
0
time [s]
Example
K = 1000
(pseudo-) Inverse J
1
0 0.002 0.004 0.006 0.008 0.01 0.012 0.014 0.016 0.018 0.02
0.5
0
0.5
Algoritmo J^1; valori x, y (dash)
time [s]
0 0.002 0.004 0.006 0.008 0.01 0.012 0.014 0.016 0.018 0.02
1.5
1
0.5
0
time [s]
Transpose J
T
Unstable
Claudio Melchiorri
Measures of performance Introduction
Manipulator performance
Denition of criteria to compute the performance in the velocity and force
domains achievable by a manipulator.
Attitude of a manipulator in a given conguration to perform a given task.
Considered criteria:
Manipulability ellipsoids;
Manipulability measures;
Polytopes.
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 Consider a unit sphere in the joint velocity space
q
T
q = 1
This sphere may be considered as a cost (energy entering in the system); it is of
interest to compute the corresponding entity in the operational space (i.e. the achieved
performance). From x = J q we have
x
T
(JJ
T
)
+
x = 1
This is a quadratic form representing, in the operational space IR
m
, an ellipsoid.
As known from geometry, this ellipsoid has shape and orientation dened by the kernel of
the quadratic form, i.e. by (JJ
T
)
+
:
Direction of the principal axes: dened by the eigenvectos u
i
of JJ
T
;
Length of the principal axes: given by the singular values of J,
i
=
i
(JJ
T
) .
As a matter of fact, from the singular value decomposition it follows that:
J = USV
T
JJ
T
= US
2
U
T
(JJ
T
)
+
= US
2
U
T
FORCE
Consider the mapping in the force/torque domain
T
= 1
from which ( = J
T
w):
w
T
JJ
T
w = 1
This equation denes an ellipsoid in the operational force space IR
m
.
Similarly to the velocity case, we have that:
The principal axes of the ellipsoid are directed along the eigenvectors u
i
di JJ
T
;
Their length are proportional to the inverse of the singular values
i
di J.
This is an important and remarkable result, that conrms 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
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)
Example: 2 dof planar manipulator
Velocity ellipsoids:
-3 -2 -1 0 1 2 3
-3
-2
-1
0
1
2
3
Force ellipsoids:
-3 -2 -1 0 1 2 3
-3
-2
-1
0
1
2
3
Other manipulability measures
1
Manipulability measure
w(q) =
det(JJ
T
)
proportional to the volume of the velocity manipulability ellipsoid. If the
manipulator is not redundant, then
w(q) = |det(J)|
2
Another criterion is
w(q) =

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).
3
It is also possible to consider
w(q) =
min
giving the minimum achievable performance (along any direction in IR
m
).
Measures of performance Velocity and force polytopes
Velocity and force polytopes
The actuation system has physical limits both in the velocity and force/torque
domain (maximum values):
q
i ,min
q
i
q
i ,max
i ,min

i

i ,max
These bounds geometrically represent polytopes (e.g. volumes delimited by planar
surfaces) in the joint velocity and force spaces IR
n
: P
q
, P
.
The Jacobian matrix allows to transform these polytopes from the joint to the
operational space. Since the mapping is linear (expressed through the matrix
operator J(q)), one obtains volumes in the operational space still delimited by
planar surfaces: the polytopes P
v
, P
w
in IR
m
.
Velocity and force polytopes
Directions along which the maximum performance are obtained in the operational
space are those corresponding to one of the vertices of P
v
, P
w
.
These directions may be computed with techniques and algorithms from the
operational research eld, and are less elegant with respect those employed to
compute the manipulability ellipsoids.
As general rule, however, we can observe that the maximum performance in the
operational space are achieved in the vertices of the polytopes P
q
, P
.
Example: 2 dof planar manipulator
Velocity polytopes:
-3 -2 -1 0 1 2 3
-3
-2
-1
0
1
2
3
Force polytopes:
-3 -2 -1 0 1 2 3
-3
-2
-1
0
1
2
3
Example
Velocity ellipsoid and polytope
-0.5 0 0.5 1 1.5 2 2.5
-1
-0.5
0
0.5
1
1.5
2
2.5
Force ellipsoid and polytope
-0.5 0 0.5 1 1.5 2 2.5
-1
-0.5
0
0.5
1
1.5
2
2.5
Example
Velocity and force ellipsoids and polytopes
-0.5 0 0.5 1 1.5 2 2.5
-1
-0.5
0
0.5
1
1.5
2
2.5

Bolona Italy Robot Tutorial

Hochgeladen von

Dokumentinformationen

Originalbeschreibung:

Copyright

Verfügbare Formate

Dieses Dokument teilen

Dokument teilen oder einbetten

Freigabeoptionen

Stufen Sie dieses Dokument als nützlich ein?

Sind diese Inhalte unangemessen?

Copyright:

Verfügbare Formate

Bolona Italy Robot Tutorial

Hochgeladen von

Copyright:

Verfügbare Formate

Kinematic Model of Robot Manipulators

C. Melchiorri (DEIS) Kinematic Model 14 / 164

(and a third one with parameters b

known as Canonical Transformation.

C. Melchiorri (DEIS) Kinematic Model 41 / 164

The vectors n, s, a express the orientation of the manipulator (rotation about z

C. Melchiorri (DEIS) Kinematic Model 43 / 164

C. Melchiorri (DEIS) Kinematic Model 44 / 164

C. Melchiorri (DEIS) Kinematic Model 50 / 164

C. Melchiorri (DEIS) Kinematic Model 52 / 164

The third joint J

C. Melchiorri (DEIS) Kinematic Model 55 / 164

It means that the manipulators joints

C. Melchiorri (DEIS) Kinematic Model 57 / 164

C. Melchiorri (DEIS) Kinematic Model 58 / 164

C. Melchiorri (DEIS) Kinematic Model 59 / 164

C. Melchiorri (DEIS) Kinematic Model 61 / 164

C. Melchiorri (DEIS) Kinematic Model 62 / 164

C. Melchiorri (DEIS) Kinematic Model 63 / 164

C. Melchiorri (DEIS) Kinematic Model 64 / 164

0.5868 0.6428 0.4394 0.4231

Since both the numerical values of

If the whole matrix

From the rst two elements

0.8138 0.342 0.4698 0.0387

The solution based on the whole matrix

From the direct kinematic equations one obtains

C. Melchiorri (DEIS) Kinematic Model 84 / 164

The solution is then computed as (ZYZ Euler angles):

0.7146 0.6131 0.3368

dt = : orientation of the rigid body

The integration of does not have a clear physical interpretation.

0 sin cos sin

C. Melchiorri (DEIS) Kinematic Model 97 / 164

0 sin cos sin

C. Melchiorri (DEIS) Kinematic Model 98 / 164

C. Melchiorri (DEIS) Kinematic Model 102 / 164

By dierentiation of the expressions of p and one obtains

and the vector p(t) = R(t)p

. The time derivative

which can be written as

), is well known from the classic

) operates on vectors in F and S() on vectors in F.

i.e. a rotational velocity about z.

The origins of the frames are

and the rotational axes are

C. Melchiorri (DEIS) Kinematic Model 111 / 164

C. Melchiorri (DEIS) Kinematic Model 112 / 164

C. Melchiorri (DEIS) Kinematic Model 113 / 164

and the kinematic model

C. Melchiorri (DEIS) Kinematic Model 114 / 164

The rotational axes are

C. Melchiorri (DEIS) Kinematic Model 115 / 164

Note that it is not possible to achieve any rotational velocity.

C. Melchiorri (DEIS) Kinematic Model 118 / 164

C. Melchiorri (DEIS) Kinematic Model 119 / 164

With hteos expression, however, the

C. Melchiorri (DEIS) Kinematic Model 122 / 164

C. Melchiorri (DEIS) Kinematic Model 123 / 164

C. Melchiorri (DEIS) Kinematic Model 127 / 164

Then rank(J) = 1 and

In the force domain

Consider the velocity vector x = [1, 1]