Professional Documents
Culture Documents
Bruno SICILIANO
KINEMATICS
Rotation matrix
Representations of orientation
Homogeneous transformations
Direct kinematics
Kinematic calibration
Position
ox
o = oy
oz
Orientation
x = xx x + xy y + xz z
y = yx x + yy y + yz z
z = zx x + zy y + zz z
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
ROTATION MATRIX
xT x y T x z T x
R = x y z = xT y y T y z T y
xT z y T z z T z
RT R = I
RT = R1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Elementary rotations
rotation of about z
cos sin 0
Rz () = sin cos 0
0 0 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
rotation of about y
cos 0 sin
Ry () = 0 1 0
sin 0 cos
rotation of about x
1 0 0
Rx () = 0 cos sin
0 sin cos
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Representation of a vector
px px
p = py p = py
pz pz
p = x y z p
= Rp
p = RT p
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Example
px = px cos py sin
py = px sin + py cos
pz = pz
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Rotation of a vector
p = Rp
pT p = pT RT Rp
Example
px = px cos py sin
py = px sin + py cos
pz = pz
p = Rz ()p
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Rotation matrix
it describes the mutual orientation between two coordinate
frames; its column vectors are the direction cosines of the
axes of the rotated frame with respect to the original frame
it represents the coordinate transformation between the
coordinates of a point expressed in two different frames
(with common origin)
it is the operator that allows the rotation of a vector in the
same coordinate frame
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
p1 = R21 p2
p0 = R10 p1
p0 = R20 p2
Example
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
EULER ANGLES
rotation matrix
9 parameters with 6 constraints
ZYZ angles
Inverse problem
Given
r11 r12 r13
R = r21 r22 r23
r31 r32 r33
= Atan2(r23 , r13 )
q
= Atan2 2 + r2 , r
r13 23 33
= Atan2(r32 , r31 )
or ( (, 0))
= Atan2(r23 , r13 )
q
= Atan2 r13 2 + r2 , r
23 33
= Atan2(r32 , r31 )
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
RPY angles
Inverse problem
Given
r11 r12 r13
R = r21 r22 r23
r31 r32 r33
= Atan2(r21 , r11 )
q
= Atan2 r31 , r32 2 + r2
33
= Atan2(r32 , r33 )
or ( (/2, 3/2))
= Atan2(r21 , r11 )
q
= Atan2 r31 , r32 2 + r2
33
= Atan2(r32 , r33 )
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
ry rx
sin = q cos = q
rx2 + ry2 rx2 + ry2
q
sin = rx2 + ry2 cos = rz
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
rx2 (1 c ) + c rx ry (1 c ) rz s
R(, r) = rx ry (1 c ) + rz s ry2 (1 c ) + c
rx rz (1 c ) ry s ry rz (1 c ) + rx s
rx rz (1 c ) + ry s
ry rz (1 c ) rx s
rz2 (1 c ) + c
R(, r) = R(, r)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Inverse problem
Given
r11 r12 r13
R = r21 r22 r23
r31 r32 r33
r11 + r22 + r33 1
= cos 1
2
r r23
1 32
r= r13 r31
2 sin
r21 r12
with
rx2 + ry2 + rz2 = 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
UNIT QUATERNION
4-parameter representation Q = {, }
= cos
2
= sin r
2
2 + 2x + 2y + 2z = 1
2( 2 + 2x ) 1
2(x y z ) 2(x z + y )
R(, ) = 2(x y + z ) 2( 2 + 2y ) 1 2(y z x )
2(x z y ) 2(y z + x ) 2( 2 + 2z ) 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Inverse problem
Given
r11 r12 r13
R = r21 r22 r23
r31 r32 r33
the quaternion is ( 0)
1
= r11 + r22 + r33 + 1
2
sgn (r32 r23 ) r11 r22 r33 + 1
1
= sgn (r13 r31 ) r22 r33 r11 + 1
2
sgn (r21 r12 ) r33 r11 r22 + 1
Q1 = {, }
quaternion product
Q1 Q2 = {1 2 T1 2 , 1 2 + 2 1 + 1 2 }
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
HOMOGENEOUS TRANSFORMATIONS
p0 = o01 + R10 p1
Inverse transformation
Homogeneous representation
p
=
p
1
R10 o01
A01 =
0T 1
Coordinate transformation
0 = A01 p
p 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Inverse transformation
1
p 0 = A01
1 = A10 p 0
p
ove
R01 R01 o01
A10 =
0T 1
A1 6= AT
DIRECT KINEMATICS
Manipulator
series of links connected by means of joints
Degree of freedom
associated with a joint articulation = joint variable
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
nbe sbe abe pbe
Teb (q) =
0 0 0 1
0 s12 c12 a1 c1 + a2 c12
0 c12 s12 a1 s1 + a2 s12
=
1 0 0 0
0 0 0 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Open chain
DenavitHartenberg convention
DenavitHartenberg parameters
Coordinate transformation
ci si 0 0
s ci 0 0
Ai1
i = i
0 0 1 di
0 0 0 1
1 0 0 ai
0 ci si 0
Aii =
0 si ci 0
0 0 0 1
ci si ci si si ai ci
i1 i si ci ci ci si ai si
Ai1
i (q i ) = A i A i =
0 si ci di
0 0 0 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Procedure
1. Find and number consecutively the joint axes; set the directions
of axes z0 , . . . , zn1
2. Choose Frame 0 by locating the origin on axis z0 ; axes x0 and
y0 are chosen so as to obtain a right-handed frame. If feasible,
it is worth choosing Frame 0 to coincide with the base frame
Execute steps from 3 to 5 for i = 1, . . . , n 1:
3. Locate the origin Oi at the intersection of zi with the common
normal to axes zi1 and zi . If axes zi1 and zi are parallel and
Joint i is revolute, then locate Oi so that di = 0; if Joint i is
prismatic, locate Oi at a reference position for the joint range,
e.g., a mechanical limit
4. Choose axis xi along the common normal to axes zi1 and zi
with direction from Joint i to Joint i + 1
5. Choose axis yi so as to obtain a right-handed frame
To complete:
6. Choose Frame n; if Joint n is revolute, then align zn with zn1 ,
otherwise, if Joint n is prismatic, then choose zn arbitrarily.
Axis xn is set according to step 4
7. For i = 1, . . . , n, form the table of parameters ai , di , i , i
8. On the basis of the parameters in 7, compute the homogeneous
transformation matrices Ai1
i (qi ) for i = 1, . . . , n
9. Compute the homogeneous transformation Tn0 (q) =
A01 . . . An1
n that yields the position and orientation of Frame n
with respect to Frame 0
10. Given T0b and Ten , compute the direct kinematics function as
Teb (q) = T0b Tn0 Ten that yields the position and orientation of
the end-effector frame with respect to the base frame
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Link ai i di i
1 a1 0 0 1
2 a2 0 0 2
3 a3 0 0 3
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
ci si 0 ai ci
s ci 0 ai si
Ai1
i = i i = 1, 2, 3
0 0 1 0
0 0 0 1
Spherical arm
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Link ai i di i
1 0 /2 0 1
2 0 /2 d2 2
3 0 0 d3 0
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
c1 0 s1 0 c2 0 s2 0
s 0 c1 0 s 0 c2 0
A01 = 1 A12 = 2
0 1 0 0 0 1 0 d2
0 0 0 1 0 0 0 1
1 0 0 0
0 1 0 0
A23 =
0 0 1 d3
0 0 0 1
c1 c2 s1 c1 s2 c1 s2 d3 s1 d2
s c c1 s1 s2 s1 s2 d3 + c1 d2
= 1 2
s2 0 c2 c2 d3
0 0 0 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Anthropomorphic arm
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Link ai i di i
1 0 /2 0 1
2 a2 0 0 2
3 a3 0 0 3
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
c1 0 s1 0
s 0 c1 0
A01 = 1
0 1 0 0
0 0 0 1
ci si 0 ai ci
s ci 0 ai si
Ai1
i = i i = 2, 3
0 0 1 0
0 0 0 1
Spherical wrist
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Link ai i di i
4 0 /2 0 4
5 0 /2 0 5
6 0 0 d6 6
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
c4 0 s4 0 c5 0 s5 0
s 0 c4 0 s 0 c5 0
A34 = 4 A45 = 5
0 1 0 0 0 1 0 0
0 0 0 1 0 0 0 1
c6 s6 0 0
s c6 0 0
A56 = 6
0 0 1 d6
0 0 0 1
c4 c5 c6 s4 s6 c4 c5 s6 s4 c6 c4 s5 c4 s5 d6
s c c + c4 s6 s4 c5 s6 + c4 c6 s4 s5 s4 s5 d6
= 4 5 6
s5 c6 s5 s6 c5 c5 d6
0 0 0 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Stanford manipulator
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
n0 s0 a0 p0
T60 = T30 T63 =
0 0 0 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
c1 s2 d3 s1 d2 + c1 (c2 c4 s5 + s2 c5 ) s1 s4 s5 d6
0
p = s1 s2 d3 + c1 d2 + s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5 d6
c2 d3 + (s2 c4 s5 + c2 c5 )d6
c1 c2 (c4 c5 c6 s4 s6 ) s2 s5 c6 s1 (s4 c5 c6 + c4 s6 )
n0 = s1 c2 (c4 c5 c6 s4 s6 ) s2 s5 c6 + c1 (s4 c5 c6 + c4 s6 )
s2 (c4 c5 c6 s4 s6 ) c2 s5 c6
c1 c2 (c4 c5 s6 + s4 c6 ) + s2 s5 s6 s1 (s4 c5 s6 + c4 c6 )
s0 = s1 c2 (c4 c5 s6 + s4 c6 ) + s2 s5 s6 + c1 (s4 c5 s6 + c4 c6 )
s2 (c4 c5 s6 + s4 c6 ) + c2 s5 s6
c1 (c2 c4 s5 + s2 c5 ) s1 s4 s5
a0 = s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5
s2 c4 s5 + c2 c5
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Link ai i di i
1 0 /2 0 1
2 a2 0 0 2
3 0 /2 0 3
4 0 /2 d4 4
5 0 /2 0 5
6 0 0 d6 6
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
c3 0 s3 0 c4 0 s4 0
s 0 c3 0 s 0 c4 0
A23 = 3 A34 = 4
0 1 0 0 0 1 0 d4
0 0 0 1 0 0 0 1
a2 c1 c2 + d4 c1 s23 + d6 c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5
p0 = a2 s1 c2 + d4 s1 s23 + d6 s1 (c23 c4 s5 + s23 c5 ) c1 s4 s5
a2 s2 d4 c23 + d6 (s23 c4 s5 c23 c5 )
c1 c23 (c4 c5 c6 s4 s6 ) s23 s5 c6 + s1 (s4 c5 c6 + c4 s6 )
0
n = s1 c23 (c4 c5 c6 s4 s6 ) s23 s5 c6 c1 (s4 c5 c6 + c4 s6 )
s23 (c4 c5 c6 s4 s6 ) + c23 s5 c6
c1 c23 (c4 c5 s6 + s4 c6 ) + s23 s5 s6 + s1 (s4 c5 s6 + c4 c6 )
s0 = s1 c23 (c4 c5 s6 + s4 c6 ) + s23 s5 s6 c1 (s4 c5 s6 + c4 c6 )
s23 (c4 c5 s6 + s4 c6 ) c23 s5 s6
c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5
a0 = s1 (c23 c4 s5 + s23 c5 ) c1 s4 s5
s23 c4 s5 c23 c5
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
DLR manipulator
Link ai i di i
1 0 /2 0 1
2 0 /2 0 2
3 0 /2 d3 3
4 0 /2 0 4
5 0 /2 d5 5
6 0 /2 0 6
7 0 0 d7 7
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
ci 0 si 0
si 0 ci 0
Ai1
i =
0
i = 1, . . . , 6
1 0 di
0 0 0 1
c7 s7 0 0
6
s7 c7 0 0
A7 =
0
0 1 d7
0 0 0 1
d3 xd3 + d5 xd5 + d7 xd7
p07 = d3 yd3 + d5 yd5 + d7 yd7
d 3 zd3 + d 5 zd5 + d 7 zd7
xd3 = c1 s2
xd5 = c1 (c2 c3 s4 s2 c4 ) + s1 s3 s4
xd7 = c1 (c2 k1 + s2 k2 ) + s1 k3
yd3 = s1 s2
yd5 = s1 (c2 c3 s4 s2 c4 ) c1 s3 s4
yd7 = s1 (c2 k1 + s2 k2 ) c1 k3
zd3 = c2
zd5 = c2 c4 + s2 c3 s4
zd7 = s2 (c3 (c4 c5 s6 s4 c6 ) + s3 s5 s6 ) c2 k2
k1 = c3 (c4 c5 s6 s4 c6 ) + s3 s5 s6
k2 = s4 c5 s6 + c4 c6
k3 = s3 (c4 c5 s6 s4 c6 ) c3 s5 s6
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
((xa c5 + xc s5 )c6 + xb s6 )c7 + (xa s5 xc c5 )s7
n07 = ((ya c5 + yc s5 )c6 + yb s6 )c7 + (ya s5 yc c5 )s7
(za c6 + zc s6 )c7 + zb s7
((xa c5 + xc s5 )c6 + xb s6 )s7 + (xa s5 xc c5 )c7
((ya c5 + yc s5 )c6 + yb s6 )s7 + (ya s5 yc c5 )c7
s07 =
(za c6 + zc s6 )s7 + zb c7
(xa c5 + xc s5 )s6 xb c6
a07 = (ya c5 + yc s5 )s6 yb c6
za s6 zc c6
xa = (c1 c2 c3 + s1 s3 )c4 + c1 s2 s4
xb = (c1 c2 c3 + s1 s3 )s4 c1 s2 c4
xc = c1 c2 s3 s1 c3
ya = (s1 c2 c3 c1 s3 )c4 + s1 s2 s4
yb = (s1 c2 c3 c1 s3 )s4 s1 s2 c4
yc = s1 c2 s3 + c1 c3
za = (s2 c3 c4 c2 s4 )c5 + s2 s3 s5
zb = (s2 c3 s4 + c2 c4 )s5 s2 s3 c5
zc = s2 c3 s4 + c2 c4
se 7 = /2
c7 0 s7 a7 c7
6
s7 0 c7 a7 s7
A7 =
0
0 1 0
0 0 0 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Humanoid manipulator
Direct kinematics
0
Trh = T30 Tt3 Trt Trh
r
0
Tlh = T30 Tt3 Tlt Tlh
l
c23 s23 0 0
s c23 0 0
Tt3 = 23
0 0 1 0
0 0 0 1
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Joint space
q1
.
q = ..
qn
qi = i (revolute joint)
qi = di (prismatic joint)
Operational space
p
x=
p (position)
(orientation)
x = k(q)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Example
px a1 c1 + a2 c12 + a3 c123
x = py = k(q) = a1 s1 + a2 s12 + a3 s123
1 + 2 + 3
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Workspace
Reachable workspace
Dexterous workspace
different orientations
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Example
admissible configurations
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
workspace
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Accuracy
deviation between the position reached in the assigned
posture and the position computed via direct kinematics
typical values: (0.2, 1) mm
Repeatability
measure of manipulators ability to return to a previously
reached position
typical values: (0.02, 0.2) mm
Kinematic redundancy
m < n (intrinsic)
r < m = n (functional)
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
KINEMATIC CALIBRATION
x = k(a, , d, )
xm measured pose
k k k k
x = a + + d +
a d
= (n )
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
x1 1
. .
x = .. = .. =
xl l
Solution
= ( T
1
T ) x
= n +
. . . until converges
Start-up
Direct kinematics
q = T
q = x
Inverse kinematics
T = q
x = q
Complexity
closed-form solution
multiple solutions
infinite solutions
no admissible solutions
Intuition
algebraic
geometric
Numerical techniques
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Algebraic solution
= 1 + 2 + 3
pW x = px a3 c = a1 c1 + a2 c12
pW y = py a3 s = a1 s1 + a2 s12
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
2 = Atan2(s2 , c2 )
(a1 + a2 c2 )pW y a2 s2 pW x
s1 =
p2W x + p2W y
(a1 + a2 c2 )pW x + a2 s2 pW y
c1 =
p2W y + p2W y
1 = Atan2(s1 , c1 )
3 = 1 2
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
Geometric solution
2 = cos 1 (c2 )
= Atan2(pW y , pW x )
q
c p2W x + p2W y = a1 + a2 c2
1
p2W x + p2W y + a21 a22
= cos q
2a1 p2W x + p2W y
1 =
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
pW = p d6 a
Decoupled solution
compute wrist position pW (q1 , q2 , q3 )
solve inverse kinematics for (q1 , q2 , q3 )
compute R30 (q1 , q2 , q3 )
compute R63 (4 , 5 , 6 ) = R30 T R
solve inverse kinematics for orientation (4 , 5 , 6 )
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
pW x c1 + pW y s1 d3 s2
p1W = pW z = d3 c2
pW x s1 + pW y c1 d2
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
1 t2 2t
c1 = s1 =
1 + t2 1 + t2
q
2 2 2
1 = 2Atan2 pW x pW x + pW y d2 , d2 + pW y
pW x c1 + pW y s1 d3 s2
=
pW z d3 c2
2 = Atan2(pW x c1 + pW y s1 , pW z )
q
d3 = (pW x c1 + pW y s1 )2 + p2W z
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
pW x = c1 (a2 c2 + a3 c23 )
pW y = s1 (a2 c2 + a3 c23 )
pW z = a2 s2 + a3 s23
3 = Atan2(s3 , c3 )
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
3,I [, ]
3,II = 3,I
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
q
p2W x + p2W y (a2 + a3 c3 ) + pW z a3 s3
c2 =
a22 + a23 + 2a2 a3 c3
q
pW z (a2 + a3 c3 ) p2W x + p2W y a3 s3
s2 =
a22 + a23 + 2a2 a3 c3
2 = Atan2(s2 , c2 )
p
for s+
3 = 1 c23 :
q
2,I = Atan2 (a2 + a3 c3 )pW z a3 s+
p2W x + p2W y ,
3
q
2 2 +
(a2 + a3 c3 ) pW x + pW y + a3 s3 pW z
q
+
2,II = Atan2 (a2 + a3 c3 )pW z + a3 s3 p2W x + p2W y ,
q
2 2 +
(a2 + a3 c3 ) pW x + pW y + a3 s3 pW z
p
for s
3 = 1 c23 :
q
2,III = Atan2 (a2 + a3 c3 )pW z a3 s3 p2W x + p2W y ,
q
2 2
(a2 + a3 c3 ) pW x + pW y + a3 s3 pW z
q
2,IV = Atan2 (a2 + a3 c3 )pW z + a3 s3 p2W x + p2W y ,
q
2 2
(a2 + a3 c3 ) pW x + pW y + a3 s3 pW z
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
1,I = Atan2(pW y , pW x )
1,II = Atan2(pW y , pW x )
pW x = 0 pW y = 0
INDUSTRIAL ROBOTICS Prof. Bruno SICILIANO
4 = Atan2(a3y , a3x )
q
3
5 = Atan2 (a3x )2 + (a3y )2 , az
6 = Atan2(s3z , n3z )
4 = Atan2(a3y , a3x )
q
3
5 = Atan2 (a3x )2 + (a3y )2 , az
6 = Atan2(s3z , n3z )