Kinematic model of industrial robots

Transcription

Kinematic Model of Robot Manipulators
Claudio Melchiorri
Dipartimento di Ingegneria dell’Energia Elettrica e dell’Informazione (DEI)
Università 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
Differential Kinematics
5
Statics - Singularities - Inverse differential kinematics
6
Inverse kinematics algorithms
7
Measures of performance
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)
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
from IRm to IRn :
q ∈ IRn , x ∈ IRm
q = g(x) = f −1 (x)
Some common (somehow arbitrary) definitions must be adopted ⇒ for the
same manipulator, different (although equivalent) kinematic models can be
defined.
Kinematic Model
4 / 164
Kinematic Model
Introduction
Kinematic Model – Example: a 2 dof planar robot
Forward kinematic model:
x
=
l1 cos θ1 + l2 cos(θ1 + θ2 )
y
=
l1 sin θ1 + l2 sin(θ1 + θ2 )
Inverse kinematic model:
cos θ2 =
x02 + y02 − l12 − l22
,
2l1 l2
sin θ2 = ±
q
(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 ).
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.
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.
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).
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.
Kinematic Model
9 / 164
Kinematic Model
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:
Kinematic Model
10 / 164
Kinematic Model
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.
Kinematic Model
11 / 164
Kinematic Model
Denavit-Hartenberg Parameters - Procedure
A common normal n exists among two skew z axes. Let us define:
d the distance between the origin of F0 and the intersection point of z0 with n
a the distance between z0 and z6 along n
Apply the following sequence of translations/rotations:
1
Translate the origin of F0 along z0 for the quantity d: the frame F1 is obtained
2
Rotate (ccw) F1 about z1 by the angle θ until x1 is aligned with n:F2 is obtained
3
Translate F2 along x2 (= n) for a: F3 is obtained, with origin on the z6 axis
4
Rotate (ccw) F3 about x3 by α, so that z3 is aligned with z6 : F4 is obtained
5
Translate F4 along z4 for the quantity b until F6 : the frame F5 is obtained
6
Rotate F5 about z5 by the angle φ: F6 is reached
Kinematic Model
12 / 164
Kinematic Model
Kinematic Model
13 / 164
Kinematic Model
Kinematic Model
13 / 164
Kinematic Model
Kinematic Model
13 / 164
Kinematic Model
Kinematic Model
13 / 164
Kinematic Model
Kinematic Model
13 / 164
Kinematic Model
Kinematic Model
13 / 164
Kinematic Model
Kinematic Model
13 / 164
Kinematic Model
Kinematic Model
13 / 164
Kinematic Model
Six cyclic transformations have been employed to move from F0 to F6 : 3
translations and 3 rotations.
There is a translation-rotation pattern:
0
T6 = Tras(z0 , d)Rot(z1 , θ)Tras(x2 , a)Rot(x3 , α)Tras(z4 , b)Rot(z5, φ)
(1)
The first 4 transformations are of particular interest: 2 couples of translations and
rotations about two axes (note that z0 = z1 and x2 = x3 ):
0
H4
=
=
Tras(z0 , d)Rot(z1 , θ)Tras(x2 , a)Rot(x3 , α)


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
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
= Tras(z0 , d)Rot(z1 , θ)Tras(x2 , a)Rot(x3, α)
Tras(z4 , b)Rot(z5 , φ)Tras(x6 , 0)Rot(x7 , 0)
that is expressed by cyclic transformations.
Kinematic Model
15 / 164
Kinematic Model
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
=
0
H4 Tras(z4 , b)Rot(z5 , φ)Tras(z6 , d ′ )Rot(z7 , θ′ )Tras(x8 , a′ )Rot(x9 , α′ )
Tras(z10 , b ′ )Rot(z11 , φ′ )Tras(x12 , 0)Rot(x13 , 0)
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 , φ + θ′ )
Kinematic Model
16 / 164
Kinematic Model
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).
Kinematic Model
17 / 164
Kinematic Model
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
=
=
Tras(z0 , d)Rot(z1 , θ)Tras(x2 , a)Rot(x3 , α)


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.
Kinematic Model
18 / 164
Kinematic Model
As a matter of fact, if F0 and F6 are two frames associated to two consecutive
links, and the position and orientation of F6 are not constrained by other
considerations, it is possible to choose F4 as the frame of the second link (NOT
F6 ), reducing in this manner to 4 the number of parameters: b and φ are not
necessary.
Then, the transformation between two consecutive links is 0 H4 .
Kinematic Model
19 / 164
Kinematic Model
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
The axis x1 intersects z0
The axis x1 is perpendicular to z0
These parameters are known as the Denavit-Hartenberg Parameters.
Kinematic Model
20 / 164
Kinematic Model
Consider now a generic manipulator.
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
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 piˆoi about
zi −1 .
Kinematic Model
22 / 164
Kinematic Model
The parameters ai and αi are constant and depend only on the link geometry:
ai
αi
is the link length
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:
rotational joint:
di is the joint variable and
θi is the joint variable and
Kinematic Model
θi is constant;
di is constant.
23 / 164
Kinematic Model
The homogeneous transformation matrix relating the frames Fi −1 and Fi is
i −1
Hi = Trasl(zi −1 , di ) Rot(zi −1 , θi ) Trasl(xi , ai ) Rot(xi , αi )

1
 0

=
0
0

Cθi
 Sθi
=
 0
0
0
1
0
0
0
0
1
0


Cθi
0
 Sθi
0 


 0
di 
0
1
−Sθi Cαi
Cθi Cαi
Sαi
0
−Sθi
Cθi
0
0
Sθi Sαi
−Cθi Sαi
Cαi
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
Cαi
Sαi
0
0
−Sαi
Cαi
0

0
0 

0 
1

ai Cθi
ai Sθi 

di 
1
known as Canonical Transformation.
In literature, matrix
i −1
Hi is also indicated as
Kinematic Model
i −1
Ai .
24 / 164
Kinematic Model
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
for prismatic joints
q i = θi
for rotational joints
i −1
Hi =
i −1
Hi (qi ).
In case of a manipulator with n joints, the relationship between frame F0 and
frame Fn is:
0
Tn =
0
H1 (q1 ) 1 H2 (q2 )...n−1 Hn (qn )
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.
Kinematic Model
25 / 164
Kinematic Model
Reference Configuration of a Canonical Transformation
A generic homogenous transformation 0 Tn may be expressed as a function of n
canonical transformations
0
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.
Kinematic Model
26 / 164
Kinematic Model
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
Cαi
Sαi
0
Hi are:
0
Cαi
Sαi
0
0
−Sαi
Cαi
0
0
−Sαi
Cαi
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
Hi , the xi axes results only translated (their
In this configuration, all the xi axes have the same direction (they are aligned).
Kinematic Model
27 / 164
Procedure for assigning frames
Claudio Melchiorri
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 robot’s kinematics, since a motion axis for
the last link does not exist.
=⇒ F0 and Fn are arbitrarily chosen!
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
F0 : only the direction of z0 may be univocally defined, while in general the
origin o0 and the orientation of x0 and y0 are not assigned;
2
Fn : only xn is constrained to be perpendicular to zn−1 (i.e. to Jn ). Since the
joint n + 1 does not exist, it is not possible to define the other elements;
3
parallel consecutive axes: it is not defined univocally the common normal line;
4
intersecting consecutive axes: the direction of xi is not defined;
5
prismatic joint: only zi is defined.
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.
Kinematic Model
30 / 164
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 Li −1 and Li are adjacent (proximal and distal, respectively), connected
by the joint Ji (whose variable is qi );
2. Definition of the axes zi , i = 0, . . . , n − 1, aligned with the joint motion
directions (rotation/translation);
(N.B. zi is the motion direction of joint Ji +1 : z0 → J1 ; z1 → J2 ; . . .)
3. Definition of F0 , with origin in any point of z0 , and axes x0 , y0 ‘properly’
chosen;
Kinematic Model
31 / 164
Steps 4 - 6 are repeated for i = 1, . . . , n − 1
4. Definition of Fi . Three cases are possible:
a) the axes of joints Ji and Ji +1 have a common normal:
the origin of Fi is placed at the intersection point of zi with the common
normal between zi −1 and zi
b) the axes of Ji e Ji +1 intersect:
the origin of Fi is placed at the intersection point between zi −1 and zi
c) the axes of joints Ji and Ji +1 are coincident or parallel:
if Ji is rotational, the origin of Fi is chosen so that di = 0
if Ji is prismatic, the origin of Fi is placed at a joint limit
5. Definition of xi along the common normal between zi −1 and zi (if exists)
with positive direction from Ji to Ji +1 ; if zi −1 intersects zi , then the following
joints are considered;
6. Definition of yi in order to obtain a proper frame.
Kinematic Model
32 / 164
Finally:
7. Define on coincident with on−1 ;
8. Define xn perpendicular to zn−1 ;
9. If Jn is rotational, choose zn parallel to zn−1 ;
If Jn is prismatic, it is possible to choose zn freely;
10. Define yn in order to obtain a proper frame.
Note that:
The position of on and its orientation zn are arbitrary
In this manner, the frame Fn is different wrt the frame of the end-effector t T
(with axes n, s, a). Therefore, it is in general necessary to define a constant
homogeneous transform matrix to take into account this difference.
Kinematic Model
33 / 164
Once the n frames Fi (i = 1, . . . , n) are defined, the corresponding 4n DH
parameters di , ai , αi , θi can be easily determined, and therefore also the
matrices i −1 Hi can be computed. The kinematic model is then obtained.
Then:
a) Define the DH Parameters Table
b) Compute the homogeneous transformation matrices
i −1
Hi ,
i = 1, . . . , n
c) Compute the direct kinematic function
0
Tn =
0
H1 1 H2 . . .
Kinematic Model
n−1
Hn
34 / 164
Examples
Example
Let us consider the following 3 dof manipulator:
Kinematic Model
35 / 164
Examples
Example
Step 1: Assign numbers to joints and links
Kinematic Model
36 / 164
Examples
Example
Step 2: Choice of the zi axes (joint rotation/translation motion axes)
Kinematic Model
37 / 164
Examples
Example
Step 3: Choice of F0
Kinematic Model
38 / 164
Examples
Example
Steps 4 - m: Definition of F1 ... Fn
Kinematic Model
39 / 164
Examples
Example
Finally (optional): choice of the tool frame
Kinematic Model
40 / 164
Examples
Example
Let’s 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
−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 =
0
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
The vectors n, s, a express the orientation of the manipulator (rotation about z0 ),
while p defines the end effector position (plane x0 − y0 ).
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
With the DH convention, the origin of
obtains:

C12 −S12 0 a1 C1
C12
0 a1 S 1
 S
0
T2 =  012
0
1
0
0
0
0
1
F2 is coincident with F1 . In this case, one



Then
Kinematic Model

1
 0
2
Tt =  0
0
0
1
0
0
0
0
1
0

a2
0 
0 
1
43 / 164
Examples
Example: 3 dof anthropomorphic manipulator
Table of the Denavit-Hartenberg parameters
L1
L2
L3
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


0

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
Kinematic model:

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
The orientation of z3 depends only on the
first joint J1 ; pz does not depend on θ1 .
Kinematic Model
45 / 164
Examples
Check if the model is correct
z0
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
a3
x3
F3
y3
F3
a2
z3
z0
y0
F0
Kinematic Model
x0
46 / 164
Examples
Another choice for the last frame could be
In this case, the last frame does not respect the DH convention:
=⇒ x3 does not intersect z2 !
There are two possible manners to obtain the kinematic model.
Kinematic Model
47 / 164
Examples
There are two possible manners to obtain the kinematic model:
1) Use the previous model and add a constant

0 0 1
 1 0 0
T=
 0 1 0
0 0 0
and then
0
T3,new

−C1 S23

−S
1 C23
= 0 T3 T = 
 C23
0
S1
−C1
0
0
rotation, in this case

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 .
Kinematic Model
48 / 164
Examples
2) Use the DH convention by adding suitable (fictitious)
i −1
Hi 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.
y2
x′3
y3
x′′3
L3
x2
z2 J3
x3
y3′
z′′3
z′3
z3
Kinematic Model
y3′′
49 / 164
Examples
The new DH parameters table (the joint angle θ3 and the new angle θ are defined
about the same axis and then it is possible to simply add them together):
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


0

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
The kinematic model results:
0
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 .
Kinematic Model
51 / 164
Examples
Example: 3 dof spherical manipulator
Denavit-Hartenberg parameters:
L1
L2
L3
d
0
d2
d3
θ
θ1
θ2
0
a
0
0
0
α
−90o
90o
0o
The Denavit-Hartenberg matrices are:

C1
 S1
0

H1 = 
0
0
0
0
−1
0
−S1
C1
0
0


0

0 
 , 1 H2 = 

0 
1
C2
S2
0
0
0
0
1
0
Kinematic Model
S2
−C2
0
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
The kinematic model results:
0
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 .
Kinematic Model
53 / 164
Examples
z3
If θ1 = θ2 = 0o , d3

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

0
d2 
d 
1
d
y0
d2
F0
x0
y3
d
=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
Example: 3 dof spherical wrist
Denavit-Hartenberg parameters
L4
L5
L6
d
0
0
d6
θ
θ4
θ5
θ6
a
0
0
0
α
−90o
90o
0o
Note the numbers starting from 4...
Then

C4
 S4
3

H4 = 
0
0
0
0
−1
0
−S4
C4
0
0


0

0
 ,4 H5 = 

0
1
C5
S5
0
0
0
0
1
0
S5
−C5
0
0
Kinematic Model


0

0
 ,5 H6 = 

0
1
C6
S6
0
0
−S6
C6
0
0
0
0
1
0

0
0 

d6 
1
55 / 164
Examples
The kinematic model is:

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 (φ, θ, ψ)
= Rot(z0 , φ)Rot(y1 , θ)Rot(z2 , ψ)

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 manipulator’s joints θ4 , θ5 and θ6 are equivalent to the Euler
ZYZ angles.
Kinematic Model
56 / 164
Examples
If θ1 = θ2 = θ3 = 0o

1
 0
3
T6 = 
 0
0
If θ1 = θ2 = θ3 = 90o

−1
 0
3
T6 = 
 0
0
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
Example: Stanford manipulator
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
T6 =
0
T3 3 T6 =
n
0
Kinematic Model
s a
0 0
p
1
58 / 164
Examples
Example: Stanford manipulator
where

−S1 (S4 C5 C6 + C4 S6 ) + C1 (C2 (C4 C5 C6 − S4 S6 ) − S2 S5 C6 )
n =  C1 (S4 C5 C6 + C4 S6 ) + S1 (−S2 S5 C6 + C2 (C4 C5 C6 − S4 S6 )) 
−C2 S5 C6 − S2 (C4 C5 C6 − S4 S6 )


−S1 (C4 C6 − S4 C5 S6 ) + C1 (−C2 (S4 C6 + C4 C5 S6 ) + S2 S5 S6 )
o =  C1 (C4 C6 − S4 C5 S6 ) + S1 (S2 S5 S6 − C2 (S4 C6 + C4 C5 S6 )) 
C2 S5 S6 + S2 (S4 C6 + C4 C5 S6 )


−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 )

Kinematic Model
59 / 164
Examples
Example: PUMA 260
Joint variables θi are defined about the
zi −1 axes; a2 is the distance between z1
and z2 (in this case parallel), d3 is the
offset between the origins of F2 and F3 ,
and d4 is the offset between the origins of
F3 and F4 . Frames F4 and F5 coincides.
The αi angles are either 0o or ±90o .
L1
L2
L3
L4
L5
L6
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
Example: PUMA 260
Canonical transformation matrices:

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


0

0
 ,1 H2=

0
1
C2
S2
0
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

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
Example: PUMA 260

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
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
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
Example: PUMA 260
n=
"
S1 (C5 C6 S4 + C4 S6 ) + C1 (C2 (−(C6 S3 S5 ) + C3 (C4 C5 C6 − S4 S6 )) − S2 (C3 C6 S5 + S3 (C4 C5 C6 − S4 S6 )))
−(C1 (C5 C6 S4 + C4 S6 )) + S1 (C2 (−(C6 S3 S5 ) + C3 (C4 C5 C6 − S4 S6 )) − S2 (C3 C6 S5 + S3 (C4 C5 C6 − S4 S6 )))
S2 (−(C6 S3 S5 ) + C3 (C4 C5 C6 − S4 S6 )) + C2 (C3 C6 S5 + S3 (C4 C5 C6 − S4 S6 ))
#
"
#
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=
"
"
S1 S4 S5 + C1 (C2 (C5 S3 + C3 C4 S5 ) − S2 (−(C3 C5 ) + C4 S3 S5 ))
−(C1 S4 S5 ) + S1 (C2 (C5 S3 + C3 C4 S5 ) − S2 (−(C3 C5 ) + C4 S3 S5 ))
S2 (C5 S3 + C3 C4 S5 ) + C2 (−(C3 C5 ) + C4 S3 S5 )
#
S1 (−d3 + d6 S4 S5 ) + C1 (a2 C2 + C2 ((d4 + C5 d6 )S3 + C3 C4 d6 S5 ) − S2 (−(C3 (d4 + C5 d6 )) + C4 d6 S3 S5 ))
−(C1 (−d3 + d6 S4 S5 )) + S1 (a2 C2 + C2 ((d4 + C5 d6 )S3 + C3 C4 d6 S5 ) − S2 (−(C3 (d4 + C5 d6 )) + C4 d6 S3 S5 ))
a2 S2 + S2 ((d4 + C5 d6 )S3 + C3 C4 d6 S5 ) + C2 (−(C3 (d4 + C5 d6 )) + C4 d6 S3 S5 )
Kinematic Model
#
63 / 164
Examples
Example: planar 4 dof manipulator (redundant)
DH parameters
L1
L2
L3
L4
d
0
0
0
0
θ
θ1
θ2
θ3
θ4
a
a1
a2
a3
a4
α
0o
0o
0o
0o
All the i −1 Hi matrices have the same
structure


Ci −Si 0 ai Ci
 Si
Ci 0 ai Si 
i −1

Hi = 
 0
0
1
0 
0
0
0
1
Kinematic Model
64 / 164
Examples
Example: planar 4 dof manipulator (redundant)
Then
0
T4
=
0

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

a1 C1 + a2 C12 + a3 C123 + a4 C1234
a1 S1 + a2 S12 + a3 S123 + a4 S1234 


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.
Kinematic Model
65 / 164
Claudio Melchiorri
Kinematic Model
66 / 164
Introduction
Direct Kinematic Model:
The direct kinematic model consists in a function f(q) mapping the joint
position variables q ∈ IRn to the position/orientation of the end effector.
The definition of f(q) is conceptually simple, and a general approach for its
computation has been defined.
Kinematic Model
67 / 164
Introduction
Inverse Kinematic Model:
The inverse kinematics consists in finding a function g(x) mapping the
position/orientation of the end-effector 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 finite set of solutions (one or more);
Infinite solutions.
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.
Kinematic Model
68 / 164
Introduction
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.
Kinematic Model
69 / 164
Algebraic Approach
Algebraic Approach
For a 6 dof manipulator, the kinematic model is described by the equation
0
T6 =
0
H1 (q1 ) 1 H2 (q2 ) . . . 5 H6 (q6 )
equivalent to 12 equations in the 6 unknowns qi , i = 1, . . . , 6.
Example: spherical manipulator (only 3 dof)

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 ) . . .j−1 Hj (qj ), 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.
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
Inverse kinematics for the position
(x, y , z) → q1 , q2 , q3
2
Inverse kinematics for the orientation
R → q4 , q5 , q6 .
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.
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).
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
IK for the point pp (center of the spherical wrist)
2
solution of the orientation IK problem
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 IK problem is solved for q1 , q2 , q3 ;
3
The rotation matrix 0 R3 related to the first three joints is computed;
4
The matrix
5
The IK problem for the rotational part is solved (Euler).
3
R6 =
0
RT
3 R
is computed;
Kinematic Model
74 / 164
Examples
Solution of the spherical manipulator
Direct kinematic model:
n s a p
0
T3 =
0 0 0 1

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
If the whole matrix 0 T3 is known, it is possible to compute:

 θ1 = atan2 (−sx , sy )
Unfortunately, according to
=⇒ the Pieper approach only p
θ2 = atan2 (−nz , az )

d3 = pz / cos θ2
is known!
Kinematic Model
75 / 164
Examples
We known only the position vector p.
From
(0 H1 )−1 0 T3

C1
 0
= 
 −S1
0
=
1
S1
0
C1
0
H2 2 H3
0
−1
0
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
By equating the position vectors,

 

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 ! Let’s define a = tan θ1 /2, then
C1 =
1 − a2
1 + a2
S1 =
2a
1 + a2
By substitution in the last element of 1 pp
2
(d2 + py )a + 2px a + d2 − py = 0
=⇒
a=
−px ±
p
px2 + py2 − d22
d2 + py
Two possible solutions! ((px2 + py2 − d22 ) > 0??)
Then
θ1 = 2 atan2 (−px ±
q
px2 + py2 − d22 , d2 + py )
Kinematic Model
77 / 164
Examples
Vector 1 pp is defined as

 
d3 S2
px C1 + py S1
1
 =  −d3 C2 
−pz
pp = 
d2
−px S1 + py C1

From the first two elements
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
Kinematic Model
78 / 164
Examples
Note that two possible solutions exist considering the position of the end-effector
(wrist) only. If also the orientation is considered, the solution (if exists) is unique.
We have seen that the relation (px2 + py2 − d22 ) > 0 must hold:
Kinematic Model
79 / 164
Examples
Numerical example: Given a spherical manipulator with d2 = 0.8 m in the pose
θ1 = 20o ,
we have

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
(with the “+” sign).
(with the “-” sign).
Kinematic Model
80 / 164
Examples
• The solution based on the vector p gives:
a) θ1 = 20o , θ2 = 30o , d3 = 0.5
(with the “+” sign).
b) θ1 = −14.7o , θ2 = −30o , d3 = 0.5
(with the “-” sign).
Kinematic Model
81 / 164
Examples
Solution of the 3 dof anthropomorphic manipulator
From the kinematic structure, one obtains
θ1 = atan2 (py , px )
Concerning θ2 and θ3 , note that the arm
moves in a plane defined by θ1 only.
One obtains
C3 =
px2 + py2 + pz2 − a22 − a32
2a2 a3
S3 = ±
q
1 − C32
θ3 = atan2 (S3 , C3 )
Moreover, by geometrical arguments, it is possible to state that:
θ2 = atan2 (pz ,
q
px2 + py2 ) − atan2 (a3 S3 , a2 + a3 C3 )
Kinematic Model
82 / 164
Examples
Solution of the 3 dof anthropomorphic manipulator
Note that also the solution is valid
θ2′ = π − θ2
θ1 = π + atan2 (py , px ),
Then, FOUR possible solutions exist:
shoulder right - elbow up;
shoulder left - elbow up;
shoulder right - elbow down;
shoulder left - elbow down;
Same position, but different orientation!
Note that the conditions px 6= 0, py 6= 0
must hold (singular configuration).
Kinematic Model
83 / 164
Examples
Solution of the spherical wrist
Let us consider the matrix


nx s x ax
3
R6 =  ny sy ay 
nz s z az
From the direct kinematic equations one obtains
3

C4 C5 C6 − S4 S6
R6 =  S4 C5 C6 + C4 S6
−S5 C6
−S4 C6 − C4 C5 S6
C4 C6 − S4 C5 S6
S5 S6
Kinematic Model

C4 S5
S4 S5 
C5
84 / 164
Examples
3
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
#
The solution is then computed as (ZYZ Euler angles):
θ5 ∈ [0, π]:
θ4
=
θ5
=
θ6
=
atan2 (ay , ax )
q
atan2 ( ax2 + ay2 , az )
atan2 (sz , −nz )
θ5 ∈ [−π, 0]:
θ4
=
θ5
=
θ6
=
atan2 (−ay , −ax )
q
atan2 (− ax2 + ay2 , az )
atan2 (−sz , nz )
Kinematic Model
85 / 164
Examples
Numerical example: Let us consider a spherical wrist in the pose
θ4 = 10o
Then
3
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
Note that the PUMA has an anthropomorphic structure (4 solutions) and a
spherical wrist (2 solutions):
=⇒ 8 different configurations are possible!
Kinematic Model
86 / 164
Claudio Melchiorri
Kinematic Model
87 / 164
Introduction
Differential Kinematics: the Jacobian matrix
In robotics it is of interest to define, besides the
mapping between the joint and workspace position/orientation (i.e. the kinematic equations),
also
The relationship between the joints and
end-effector velocities:
v
⇐⇒
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 J, called the
Jacobian of the manipulator.
Kinematic Model
88 / 164
Introduction
The Jacobian matrix
In robotics, the Jacobian is used for several purposes:
To define the relationship between joint and workspace velocities
To define the relationship between forces/torques between the spaces
To study the singular configurations
To define numerical procedures for the solution of the IK problem
To study the manipulability properties.
Kinematic Model
89 / 164
The Jacobian Matrix
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).
The translational velocity of point p of the rigid body, with respect to F0 , is
defined as the derivative (with respect to time) of p, denoted as ṗ:
ṗ =
d p
dt
Kinematic Model
90 / 164
The Jacobian Matrix
Velocity domain
With respect to the rotational velocity, two different definitions are possible:
1
2
A triple γ ∈ IR3 giving the orientation of F1 with respect to F0 (Euler,
RPY,... angles) is adopted, and its derivative is used to define the rotational
velocity γ̇
dγ
γ̇ =
dt
An angular velocity vector ω is defined, giving the rotational velocity of a
third frame F2 with origin coincident with F0 and axes parallel to F1 .
The velocity vector ω is placed in the origin, and its direction coincides with the
instantaneous rotation axis of the rigid
body.
Kinematic Model
91 / 164
The Jacobian Matrix
Analytical and Geometrical expressions of the Jacobian
The two descriptions lead to different results concerning the expression of the
Jacobian matrix, in particular in the part relative to the rotational velocity.
One obtains respectively the:
Analytic Jacobian JA
Geometric Jacobian JG
Two problems:
1) Integration of the rotational velocity
Z
γ̇dt
=⇒
γ: orientation of the rigid body
Z
ωdt
=⇒
??
Kinematic Model
92 / 164
The Jacobian Matrix
Example: Let’s 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
ω
= [0, π/2, 0]T
0≤t≤1
ω
= [π/2, 0, 0]T
1<t≤2
Case b)
By integrating the velocities in the two cases, one obtains:
Z
2
ωdt = [π/2, π/2, 0]T
0
Kinematic Model
93 / 164
The Jacobian Matrix
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
Z
2
ωdt = [π/2, π/2, 0]T
0
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
The integration of ω does not have a clear physical interpretation.
Kinematic Model
94 / 164
The Jacobian Matrix
2) On the other hand:
ω represents the velocity components about the three axes of F0 ,
the elements of γ̇ are defined with respect to frame that: a) is not Cartesian
(its axes are not orthogonal each other); b) varies in time according to γ.
Kinematic Model
95 / 164
The Jacobian Matrix
The two expressions of the Jacobian matrix physically define 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 θ  γ̇ = T(γ) γ̇
1
0
cos θ
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 singularities of γ.
Kinematic Model
96 / 164
The Jacobian Matrix
Definition of matrix T(γ):
φ̇ :
θ̇ :
ψ̇ :
T
[ωx , ωy , ωz ] = φ̇
[ωx , ωy , ωz ]T = θ̇
[ωx , ωy , ωz ]T = ψ̇
"
"
"
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
The Jacobian Matrix
If sin θ = 0, then the components perpendicular to z of the velocity expressed by γ̇
are linearly dependent (ωx2 + ωy2 = θ̇2 ), while physically this constraint may not
exist!
From:

one obtains:

0 −Sφ
 0 Cφ
1
0
0
ω= 0
1

− sin φ cos φ sin θ
cos φ sin φ sin θ  γ̇
0
cos θ

 



φ̇
−Sφ θ̇
0
ωx
0   θ̇  =⇒  Cφ θ̇  =  ωy 
1
ωz
ψ̇
φ̇ + ψ̇
Kinematic Model
=⇒
ωx2 + ωy2 = θ̇2
ωz = φ̇ + ψ̇
98 / 164
The Jacobian Matrix
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:
v
I
0
ṗ
=
ω
0 T(γ)
γ̇
Then
JG =
I
0
0
T(γ)
JA = TA (γ)JA
Kinematic Model
99 / 164
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
where the m × n matrix
 ∂f1
∂f(q)  ∂q1
...
=
J(q) =
∂q
∂fm
∂q1
∂f1
∂q2
∂fm
∂q2
...
...
∂f1
∂qn
∂fm
∂qn


J(q) ∈ IRm×n
is called Jacobian matrix or JACOBIAN of the manipulator.
Kinematic Model
100 / 164
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̇ =
v
γ̇
= 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) ∈ IRm×n ).
Kinematic Model
101 / 164
Analytical Jacobian
Example: 2 dof manipulator
L1
L2
d
0
0
θ
θ1
θ2
a
a1
a2
α
0o
0o
The end-effector position is
px
py
=
=
a1 C1 + a2 C12
a1 S1 + a2 S12
pz
=
0
If γ is composed by the Euler angles φ, θ, ψ defined about axes z0 , y1 , z2 , and
considering that the z axes of the base frame and of the end effector are parallel,
the total rotation is equivalent to a single rotation about z0 and therefore:

 

φ
θ1 + θ2
 θ =

0
ψ
0
Kinematic Model
102 / 164
Analytical Jacobian
Euler angles:

 

φ
θ1 + θ2
 θ =

0
ψ
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̇
Kinematic Model
103 / 164
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 defines the effect of the i-th joint on the
end-effector velocity. It is divided in two terms.
The first term considers the effect of q̇i on the linear velocity v, while the second
one on therotational
velocity ω, i.e.
v
Jv1 Jv2 . . . Jvn
= J q̇
=⇒
J=
ω
Jω1 Jω2 . . . Jωn
Therefore
v
=
Jv1 q̇1 + Jv2 q̇2 + . . . + Jvn q̇n
ω
=
Jω1 q̇1 + Jω2 q̇2 + . . . + Jωn 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.
Kinematic Model
104 / 164
Geometric Jacobian
Derivative of a Rotation Matrix
Let’s consider a rotation matrix
Let’s derive the equation:
R = R(t)
and
R(t)RT (t) = I
=⇒
R(t)RT (t) = I .
Ṙ(t)RT (t) + R(t)ṘT (t) = 0
A 3 × 3 (skew-symmetric) matrix S(t) is obtained
S(t) = Ṙ(t)RT (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.
Kinematic Model
105 / 164
Geometric Jacobian
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.
Kinematic Model
106 / 164
Geometric Jacobian
Moreover
R S(ω) RT = 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 differ by the rotation R (ω ′ = R ω). Then
S(ω ′ ) operates on vectors in F ’ and S(ω) on vectors in F .
Consider a vector va′ in F ’ and assume that some operations must be performed
on that vector in F (then using S). It is necessary to:
1
2
3
Transform the vectors from F ’ to F (by RT )
Use S(ω)
Transform back the result to F ’ (by R)
That is:
equivalent to the transformation using S(ω ′ ):
Kinematic Model
vb′ = R S(ω) RT va′
vb′ = S(ω ′ ) va′
107 / 164
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
Then
−θ̇ cos θ
−θ̇ sin θ
0


0
ω= 0 
θ̇

0
cos θ
0   − sin θ
0
0
sin θ
cos θ
0

0
0 
1
 
0
0
0 = θ̇
1
0
−θ̇
0
0

0
0  = S(ω(t))
0
i.e. a rotational velocity about z.
Kinematic Model
108 / 164
Geometric Jacobian
Geometric Jacobian
The end-effector velocity is a linear composition of the joint velocities
v
ω
=
=
Jv1 q̇1 + Jv2 q̇2 + . . . + Jvn q̇n
Jω1 q̇1 + Jω2 q̇2 + . . . + Jωn q̇n
Each column of the Jacobian
matrix defines the effect of the
i-th joint on the end-effector velocity.
Kinematic Model
109 / 164
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
Jωi
0
zi −1
Jvi
=
prismatic joint
0
Jωi
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 ) . . . n−1 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
Ri −1 =
0
R1 (q1 ) 1 R2 (q2 ) . . .
Kinematic Model
i −2
Ri −1 (qi −1 )
110 / 164
Examples
The Jacobian is computed as
z0 × (p2 − p0 ) z1 × (p2 − p1 )
J=
z0
z1
The origins of the frames are
 


0
a1 C1
p0 =  0 
p1 =  a1 S1 
0
0


a1 C1 + a2 C12
p2 =  a1 S1 + a2 S12 
0
and the rotational axes are


0
z0 = z1 =  0 
1
Kinematic Model
111 / 164
Examples
Then
z0 × (p2 − p0 )
=
=
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:

−a1 S1 − a2 S12
 a1 C1 + a2 C12


0
J(q) = 

0


0
1
Kinematic Model
−a2 S12
a2 C12
0
0
0
1








112 / 164
Examples
Jacobian:




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
Kinematic Model
113 / 164
Examples
Example: 3dof anthropomorphic manipulator
The canonical transformation matrices are

C1
 S1
0
H1 = 
 0
0
and the kinematic model

C1 C23
 S1 C23
0
T3 = 
 S23
0
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

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
Examples
The Jacobian results
z0 × (p3 − p0 ) z1 × (p3 − p1 ) z2 × (p3 − p2 )
J=
z0
z1
z2
where


0
p0 = p1 =  0 
0

a2 C1 C2
p2 =  a2 S1 S2 
a2 S2

The rotational axes are


0
z0 =  0 
1

C1 (a2 C2 + a3 C23 )
p3 =  S1 (a2 C2 + a3 C23 ) 
a2 S2 + a3 S23



S1
z1 = z2 =  −C1 
0
Kinematic Model
115 / 164
Examples
Therefore




J=



−S1 (a2 C2 + a3 C23 ) −C1 (a2 S2 + a3 S23 ) −a3 C1 S23
C1 (a2 C2 + a3 C23 ) −S1 (a2 S2 + a3 S23 ) −a3 S1 S23
0
a2 C2 + a3 C23
a3 C23
0
S1
S1
0
−C1
−C1
1
0
0








- Only three rows are linearly independent (3 dof).
- Note that it is not possible to achieve all the rotational velocities ω in IR3 .
- Moreover S1 ωy = −C1 ωx (ωx = S1 θ̇2 + S1 θ̇3 , ωy = −C1 θ̇2 − C1 θ̇3 , ).
By considering the linear velocity only, one obtains:


−S1 (a2 C2 + a3 C23 ) −C1 (a2 S2 + a3 S23 ) −a3 C1 S23
J =  C1 (a2 C2 + a3 C23 ) −S1 (a2 S2 + a3 S23 ) −a3 S1 S23 
0
a2 C2 + a3 C23
a3 C23
Kinematic Model
116 / 164
Examples
Note that:
θ̇1 does not affect vz (nor ωx , ωy )
ωz depends only by θ̇1
S1 ωy = −C1 ωx :
ωx and ωy are not independent
the first three rows may also be obtained by derivation of 0 p3
In the “linear velocity” case (i.e. the first three rows only)
det(J) = −a2 a3 S3 (a2 C2 + a3 C23 )
Therefore det(J) = 0 in two cases:
0
S3 = 0
=⇒
θ3 =
π
(a2 C2 + a3 C23 ) = 0 i.e. when the wrist is on the z axis (px = py = 0):
shoulder singularity
Kinematic Model
117 / 164
Examples
Example 3 dof spherical manipulator
Canonical transformation matrices

C1
 S1
0
H1 = 
 0
0
−S1
C1
0
0
0
0
−1
0

1
 0
2
H3 = 
 0
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
−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
Examples
The Jacobian is
J=
with

0
z0 =  0 
1

and
z0 × (p3 − p0 )
z0

0
p0 = p1 =  0 
0

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
Examples
Then




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 .
Kinematic Model
120 / 164
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








By choosing d6 = 0, i.e. the origin of F6 is in the intersection point of the three
joint axes, then



J=


0
0
0
0
0
1
0
0
0
−S4
C4
0
0
0
0
C4 S5
S4 S5
C5






With this expression, however, the linear
velocity of the end-effector is not computed.
det(J) = 0 =⇒ S5 = 0, i.e. θ5 = 0, π.
In this case it is not possible to determine
individually θ̇4 and θ̇6 .
Kinematic Model
121 / 164
Examples
Example: PUMA 260
Only revolute joints are present:
z0 × (p6 − p0 ) z1 × (p6 − p1 ) z2 × (p6 − p2 )
J=
z0
z1
z2
z3 × (p6 − p3 ) z4 × (p6 − p4 ) z5 × (p6 − p5 )
z3
z4
z5
Kinematic Model
122 / 164
Examples
Example: PUMA 260
If d6 = 0:
J
=
−d3 C1 − S1 (a2 C2 + d4 S23 )
 −d3 S1 + C1 (a2 C2 + d4 S23 )

0


0

0
1

0
0
0
C1 S23
S1 S23
−C23
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
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.
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)
=⇒
mapping between q̇ and [vT , ω T ]T
JT (q)
=⇒
mapping between [f T , nT ]T and τ
J(q)
q̇
(τ )
ẋ
(w)
JT (q)
Kinematic Model
126 / 164
Transformation of reference frame for the Jacobian
Let’s consider two frames Fa and
Fb attached to a rigid body (“robot”).
If a ẋ is the end-effector velocity in Fa , then
a
a
ẋ =
J q̇
It is known that it is possible to transform the same velocity in another frame
Fb as
b
ẋ =
b
Ga a ẋ
where b Ga is the transformation matrix between the two frames:
b
Ra − b Ra a Pab
b
Ga =
b
0
Ra
Kinematic Model
127 / 164
Then
b
ẋ =
b
J q̇ =
b
Ga a ẋ =
b
Ga a J q̇
and therefore the transformation for the Jacobian between the two frames Fa and
Fb is given by
b
J=
b
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
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 these configurations, there are “directions” (vectors ẋ) in IR6 without any
correspondent “direction” (q̇) in IRn : these directions cannot be actuated and the
robot ”looses” a motion capability.
Kinematic Model
129 / 164
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
2
In the proximity of a singularity, small velocities in the operational space may
generate large (infinite) velocities in the joint space
3
They correspond to configurations that have not a well defined solution to the
inverse kinematic problem: either no solution or infinite 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-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.
Kinematic Model
130 / 164
Example
Consider the Jacobian J(q)
J=
1 1
0 0
Then rank(J) = 1 and
dx
=
dq1 + dq2
dy
=
0
For any value of dq1 , dq2 , then dy = 0.
Any vector dx whose second element is not null represents an instantaneous
motion that cannot be achieved.
Kinematic Model
131 / 164
Example
Jacobian:
J=
1 1
0 0
τ = JT f =
1
1
In the force domain
0
0
fx
fy
then
τ1
= fx
τ2
= 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.
Kinematic Model
132 / 164
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.
Kinematic Model
133 / 164
Example
J(q) =
−a1 S1 − a2 S12
a1 C1 + a2 C12
−a2 S12
a2 C12
Consider the velocity vector ẋ = [−1, −1]T . By computing q̇ = J−1 (q)ẋ:
If θ1 = 0o , θ2 = 1o then
q̇ =
−58.9
115.59
If θ1 = 0o , θ2 = 10o then
q̇ =
−6.67
12.43
Kinematic Model
(rad/sec) =
(rad/sec) =
−3374.7
6622.8
−382.16
712.18
(o /sec)
(o /sec)
134 / 164
Example
J(q) =
−a1 S1 − a2 S12
a1 C1 + a2 C12
−a2 S12
a2 C12
Velocita‘ di giunto (dq2 dash)
5000
4000
3000
Plot of
q̇ = J(q) ẋ
−1
with ẋ = −1 and
θ1 = 0,
θ2 ∈ [0.0005, 0.01] rad
dq1, dq2 [rad/sec]
−1
2000
1000
0
−1000
−2000
−3000
0
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
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 ∈ IR6×n 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 )]
Kinematic Model
J22 = [z3 , z4 , z5 ]
136 / 164
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
gives the arm singularities, while
det(J22 ) = 0
gives the wrist singularities.
Kinematic Model
137 / 164
Inverse Differential Kinematics
The ‘direct’ relationship between joint and operational space velocities:
ẋ = J(q)q̇
is defined 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̇
J+ = JT (JJT )−1
if m < n (right pseudo-inverse : JJ+ = I)
where:
J+ = (JT J)−1 JT
if m > n (left pseudo-inverse: J+ J = I)
Kinematic Model
138 / 164
If m 6= n there are two cases:
J
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
=⇒
Kinematic Model
J+ = (JJT )−1 JT
139 / 164
Solution of ẋ = Jq̇
if m = n,
det(J) 6= 0
J(q)
J−1 (q)
The equation ẋ = Jq̇ (as well as q̇ = J−1 ẋ) has an unique solution:
∀ ẋo → ∃ ! q̇o = J−1 ẋo
such that
Kinematic Model
ẋo = Jq̇o = J J−1 ẋo
140 / 164
if m < n
J(q)
0
N (J)
IRn
IRm
• rank(J) = min(m, n) = m
• ∀ ẋ ∃ q̇ such that ẋ = Jq̇
+
→
R(J) = IRm
(multiple solutions!)
∃ N (J) such that ∀ q̇ ∈ N (J) → ẋ = J q̇ = 0
• q̇ = J ẋ
+
→ ẋ = J (J ẋ + q̇N ) = ẋ,
+
+
→ q̇ = J ẋ + qN
+
∀q̇N ∈ N (J)
→ q̇ = J ẋ + (I − J J)y general expression of the solution
• q̇ = J+ ẋ
has
dim(N (J)) = n − m
(I − J+ J) base of N (J)
minimum norm
Kinematic Model
141 / 164
Solution ofi ẋ = Jq̇
if m > n
R(J)
IRm
IRn
• rank(J) = min(m, n) = n
• ∀ q̇ ∃ ! ẋ such that ẋ = Jq̇ ∈ R(J)
• ∀ ẋ ∈ R(J) ∃ ! q̇ such that ẋ = Jq̇ (q̇ = J−1 ẋ)
• If ẋ 6∈ R(J) →6 ∃ q̇ such that ẋ = Jq̇
• If ẋ0 6∈ R(J) → ∃ q̇0 = J+ ẋ0
→ ẋ = Jq̇0 = JJ+ ẋ0 6= ẋ0
(JJ+ 6= I)
• kẋ − ẋ0 k is minimum
Kinematic Model
142 / 164
in general if m 6= n
N (J)
R(J)
IRn
IRm
• rank(J) = p < min(m, n)
• The solution given by the pseudo-inverse q̇s = J+ ẋ is such that (ẋs = Jq̇s ):
(
kẋs − ẋk
the norm of the error is minimum
kq̇s k
the norm of the solution is minimum
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
vector q̈ is computed as a solution of
ẍ = J(q)q̈ +
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̇ = Jq̇ (b = Jq̈) exists iff ẋ ∈ R(J) (b ∈ R(J)).
A vector a is in R(A) iff
rank([A, a]) = rank[A]
Kinematic Model
144 / 164
Claudio Melchiorri
Kinematic Model
145 / 164
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 ) = vk , a simple
approach is to consider
qk+1 = qk + J−1 (qk )vk T
equivalent to a numerical integration over time of the velocity. This operation has
two major drawbacks affecting 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 = xd − x. Then
ė = ẋd − ẋ = ẋd − Ja (q)q̇
(2)
The vector q̇ must be chosen so that the error e converges to zero.
Kinematic Model
146 / 164
Jacobian (pseudo)-inverse
Scheme 1: Jacobian (pseudo)-inverse
Assuming that Ja 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 definite, an asymptotically stable system. Note that
the convergence velocity depends on K.
ẋd
xd
e
✲ ❥
✲
− ✻
K
x
❄
✲ ❥ ✲ J−1 (q)
a
f(·)
Kinematic Model
q̇ ✲
R
q
✲
✛
147 / 164
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.
Let’s assume
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
By differentiating V (e) one obtains
V̇
=
eT Kė = eT Kẋd − eT Kẋ
=
eT Kẋd − eT KJa (q)q̇
Kinematic Model
148 / 164
Jacobian transpose
Scheme 2: Jacobian transpose
From
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
If ẋd = 0 then V̇ < 0 and the system is asymptotically stable if Ja is full rank.
If Ja is not full rank, then the condition q̇ = 0 may be obtained also with e 6= 0
(Ke ∈ null(JT
a )).
xd
✲ ❥
e ✲
− ✻
x
K
q̇ ✲
✲ JT (q)
a
f(·)
Kinematic Model
R
q
✲
✛
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 )
q23 + 2q1 cos(q12 + 2q2 )
3q1 q22 + 2cos(q12 + 2q2 )
#
Assuming z0 = [0.1, 0.2]T , find q0 = f −1 (z0 ).
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 J−1
Transpose JT
Algoritmo J^T; valori x, y (dash)
Algoritmo J^−1; valori x, y (dash)
0.2
0.2
0
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
0
−0.5
−0.5
−1
−1
−1.5
0
0.02
0.04
0.06
0.08
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]
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 J−1
Algoritmo J^−1; valori x, y (dash)
0.5
0
Unstable
−0.5
0
0.002
0.004
0.006
0.008
0.01
0.012
time [s]
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
−0.5
−1
−1.5
0
0.01
0.012
time [s]
Kinematic Model
152 / 164
Claudio Melchiorri
Kinematic Model
153 / 164
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.
Kinematic Model
154 / 164
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̇ = Jq̇ we have
ẋT (JJT )+ ẋ = 1
This is a quadratic form representing, in the operational space IRm , an ellipsoid.
As known from geometry, this ellipsoid has shape and orientation defined by the kernel of
the quadratic form, i.e. by (JJT )+ :
Direction of the principal axes: defined by the eigenvectos ui of JJT ;
p
Length of the principal axes: given by the singular values of J, σi = λi (JJT ) .
As a matter of fact, from the singular value decomposition it follows that:
J = USVT
JJT = US2 UT
Kinematic Model
(JJT )+ = US−2 UT
155 / 164
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
Kinematic Model
156 / 164
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)
Kinematic Model
157 / 164
Example: 2 dof planar manipulator
Velocity ellipsoids:
Force ellipsoids:
3
3
2
2
1
1
0
0
-1
-1
-2
-2
-3
-3
-2
-1
0
1
2
3
-3
-3
Kinematic Model
-2
-1
0
1
2
3
158 / 164
Other manipulability measures
1
Manipulability measure
w (q) =
p
det(JJT )
proportional to the volume of the velocity manipulability ellipsoid. If the
manipulator is not redundant, then
w (q) = |det(J)|
2
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) =
3
It is also possible to consider
w (q) = σmin
giving the “minimum” achievable performance (along any direction in IRm ).
Kinematic Model
159 / 164
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 ,max
τi
These bounds geometrically represent polytopes (e.g. volumes delimited by planar
surfaces) in the joint velocity and force spaces IRn : Pq , 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 Pv , Pw in IRm .
Kinematic Model
160 / 164
Directions along which the maximum performance are obtained in the operational
space are those corresponding to one of the vertices of Pv , Pw .
These directions may be computed with techniques and algorithms from the
operational research field, 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 Pq , Pτ .
Kinematic Model
161 / 164
Example: 2 dof planar manipulator
Velocity polytopes:
Force polytopes:
3
3
2
2
1
1
0
0
-1
-1
-2
-2
-3
-3
-2
-1
0
1
2
3
-3
-3
Kinematic Model
-2
-1
0
1
2
3
162 / 164
Example
Velocity ellipsoid and polytope
Force ellipsoid and polytope
2.5
2.5
2
2
1.5
1.5
1
1
0.5
0.5
0
0
-0.5
-0.5
-1
-0.5
0
0.5
1
1.5
2
-1
-0.5
2.5
Kinematic Model
0
0.5
1
1.5
2
2.5
163 / 164
Example
Velocity and force ellipsoids and polytopes
2.5
2
1.5
1
0.5
0
-0.5
-1
-0.5
0
0.5
1
1.5
2
2.5
Kinematic Model
164 / 164

Kinematic model of industrial robots

Transcription

Similar documents

REVerse engineering of audio-VIsual coNtent Data

here - Jan-Frederik Pietschmann

Conference talk

HiMAP

cabine di radioscopia industriale

5047 - The Wheel Horse Tractor Manual and Documentation Website