Kinematics: Pose (position and

orientation) of a Rigid Body

 Recap of rotation matrices
a
p = Rb p
a b

 xˆa 
T
( yˆ B ) =  b T 
b
A
RB = A
xˆ B A

 yˆ a 
−1 T
b
Ra = Ra = Rb b a

A
RC = RB RC A B
Example 2: composition of rotation matrices
ẑ a x̂c
ŷa
φ
x̂b
θ
x̂a

 cθ − sθ 0  c− φ 0 s− φ   cφ 0 − sφ 
     
a
Rb =  sθ cθ 0 b
Rc =  0 1 0 =  0 1 0 
 0 1  − s
 0
 −φ 0 c− φ   sφ 0 cφ 
Example 2: composition of rotation matrices
ẑ a x̂c
ŷa
φ
x̂b
θ
x̂a

 cθ − sθ 0   cφ 0 − sφ   cθ cφ − sθ − cθ sφ 
    
a
Rc = Rb Rc =  sθ
a b
cθ 0  0 1 0  =  sθ cφ cθ − sθ sφ 
 0
 0 1   sφ 0 cφ   sφ 0 cφ 
Homogeneous transforms
Rotation matrices assume that the origins of
the two frames are co-located.
• What if they’re separated by a translation?

B
y
A
y

B
p
dA

A
x B
x
Homogeneous transform
B
yˆ
A
yˆ
(same point, two
reference frames)
B
p
A
dB
A
p

A
xˆ B
xˆ
A
p = ARB B p + Ad B
 Homogeneous transform
B
y
A
p = RB p + d B
A B A A
y

 A RB A
dB   B p 
=    a
db
p

 0 1   1 
A
 r11 r12 r13 A
dx  x B
x
  B
 r21 r22 r23 A

dy  p A   B
p
=     = TB  
 r31 r32 r33 A
d z   1   1 
 
 0 0 0 1 

always one
always zeros
Example 1: homogeneous transforms

B
B
y x
B
z
l

A
y

θ A
x
A
z
B
What’s TA ?
 Example 1: homogeneous transforms
A
B y
What’s TA ? B
y B
x
θ
 cos(θ ) − sin(θ ) 0  A
x
 
A
RB =  sin(θ ) cos(θ ) 0  A
z, B z
 0 0 1 


 − l 
 
B
RA B
dA 
B
dA =  0 
B

TA =   B
x
1 
B
y
 0  0 B
  z
l
 cos(θ ) sin(θ ) 0 − l
 
 − sin(θ ) cos(θ ) 0 0 A
y
B
TA = 
0 0 1 0
  θ
 0 0 0 1 
 A
x
A
z
 Example 2: homogeneous transforms
yb x
zb b  cθ 0 − sθ l 2c ( θ + π
4
)
 
 sθ 0 cθ l 2 s (θ + π
)
a
Tb = 4
l  0 −1 0 0 
 
 0 0 0 1 

l
ya This arm rotates about the z a axis.
θ
xa
za
a
What’s Tb ?
 Example 3: homogeneous transforms
A
yˆ c
yˆ c
xˆ

l c
zˆ
φ
θ A
xˆ

A
zˆ

 cθ 0 sθ   cφ − sφ 0   cθ cφ − sφ cθ sθ 
    
a
Rc = Rb Rc =  0
a b
1 0   sφ cφ 0  =  sφ cφ 0
− s
 θ 0 cθ   0 0 1   − sθ cφ sθ sφ cθ 
 Example 3: homogeneous transforms
A
yˆ c
yˆ c
xˆ

l c
zˆ
φ
θ A
xˆ

A
zˆ

 − l  cθ cφ − sφ cθ sθ   − l   lcθ cφ 
      
c
d=  0 a
d = − Rc d = −  sφ
a c
cφ 0   0  =  lsφ 
 0 − s c sθ sφ cθ   0   − lsθ cφ 
   θ φ 
 cθ cφ − sφ cθ sθ lcθ cφ 
 
 a Rc a
d   sφ cφ 0 lsφ 
a
Tc =  = 
 0 1  − sθ cφ sθ sφ cθ − lsθ cφ 
 
 0 0 0 1 

 Inverse of the Homogeneous transform

Can also derive it from the forward Homogeneous transform:

B
p = B R A A p + Bd A
A
p = RA
B T
( B
p − Bd A )
−1  B
p
A
p = TA 
B


 1 
 B
R
T
− BRA dA 
T B
TA =  
where B −1 A

 0 1 
Inverse of the Homogeneous transform
B
yˆ
A
yˆ
(same point, two
reference frames)
B
p
A
dB
A
p

A
xˆ B
xˆ
B
p = BRA A p − B RA A d B = B RA ( A p − Ad B )
 Example 1: homogeneous transform inverse

B
B
y x
B
z
l  cos(θ ) sin(θ ) 0 − l
 
 − sin(θ ) cos(θ ) 0 0
B
TA = 
A
y 0 0 1 0
 
 0 0 0 1 

θ A
x What’s
A
TB ?
A
z
Example 1: homogeneous transform inverse
 cos(θ ) − sin(θ ) 0 
 B
RA
T
− RA d A 
B T B
 
TA =  
B −1 A
RB =  sin(θ ) cos(θ ) 0 
  0 1 
 0 1   0

 − l  cos(θ ) − sin(θ ) 0   − l   l cos(θ ) 

      
B
dA =  0  − RB d A =  sin(θ ) cos(θ ) 0   0  =  l sin(θ ) 
A B

 0  0 0 1   0   0 
  

 cos(θ ) − sin(θ ) 0 l cos(θ ) 

 
−1 A  sin(θ ) cos(θ ) 0 l sin(θ ) 
B
TA = TB = 
0 0 1 0 
 
 0 0 0 1 

Example 2: homogeneous transform inverse

yb xb
zb  cθ 0 − sθ l 2c ( θ + π
4
)
 
 sθ 0 cθ l 2 s (θ + π
)
a
Tb = 4
l  0 −1 0 0 
 
 0 0 0 1 

b
What’s Ta ?
l
ya
θ
 cθ

sθ 0 (
− l 2 cθ cθ + π + sθ sθ + π 
4 4

)
 0 0 −1 0 
xa Ta =
( )
b
 − sθ cθ 0 l 2 sθ cθ + π − cθ sθ + π 
za  4 4 
 0 0 0 1 
 Forward Kinematics

• Where is the end effector w.r.t. the “base” frame?

 Composition of homogeneous transforms
3
x
Base to eff transform
3
y
l3
l2
0
T3 = 0T1 1T2 2T3 2
q3
x 1
1
x
y
q2

Transform associated w/ link 3 2

y 0
y
Transform associated w/ link 2
q1
0
Transform associated w/ link 1 x l1
0
z
 Forward kinematics: composition of
homogeneous transforms
3
x
0
T3 = T1 T2 T3
0 1 2

 c1 − s1 0 l1c1  3
  y
 s1 c1 0 l1s1  l3
0
T1 = 
0 0 1 0  q3 l2
  2
0 0 0 1  x 1
x
 1
y
q2
 c2 − s2 0 l 2 c2 
 
 s2 c2 0 l 2 s2 
1
T2 =  2
y
0 0 1 0  0
y
 
 0 0 0 1 
 q1
0
x l1
0
z
 Forward kinematics: composition of
homogeneous transforms
3
x
0
T3 = T1 T2 T3
0 1 2

3
 c3 − s3 0 l3c3  y
 
l3
 s3 c3 0 l3 s3 
2
T3 = 
1 0  q3 l2
0 0 2
  x 1
x
 0 0 1  1
 0 y
q2

2
y 0
y
q1
0
x l1
0
z
Remember those double-angle formulas…

sin (θ ± φ ) = sin (θ ) cos( φ ) ± cos(θ ) sin ( φ )

cos(θ ± φ ) = cos(θ ) cos( φ ) sin (θ ) sin ( φ )
 Forward kinematics: composition of
homogeneous transforms
0
T3 = 0T1 1T2 2T3
 c1 − s1 0 l1c1   c2 − s2 0 l2 c2   c3 − s3 0 l3c3 
   
 s1 c1 0 l1s1   s2 c2 0 l2 s2   s3 c3 0 l3 s3 
0
T3 = 
0 0 1 0  0 0 1 0  0 0 1 0 
   
0 0 0 1   0 0 0 1   0 0 0 1 


 c123 − s123 0 l1c1 + l2 c12 + l3c123 

 
 s123 c123 0 l1s1 + l2 s12 + l3 s123 
0
T3 =  
0 0 1 0
 
 0 0 0 1 
 
 DH parameters
• There are a large number of ways that homogeneous
transforms can encode the kinematics of a manipulator
• We will sacrifice some of this flexibility for a more systematic
approach: DH (Denavit-Hartenberg) parameters.
• DH parameters is a standard for describing a series of
transforms for arbitrary mechanisms.
3 3
x z
3
x
3
y
l3 l3
2
q3 l2 x
2 q3 l2
x 1
1
x 1
y 2
z z
q2
q2

2 1
y 0
y 0
x
y
q1
0 q1
x l1 0
0 z l1
z
0
x
 Forward kinematics: DH parameters

These four DH parameters,

( ai α i di θ i)

represent the following homogeneous matrix:

 cθ i − sθ i 0 0  1 0 0 0  1 0 0 ai   1 0 0 0
    
 sθ i cθ i 0 0  0 1 0 0  0 1 0 0   0 cα i − sα i 0
T= 
0 0 1 0  0 0 1 di   0 0 1 0   0 sα i cα i 0
    
 0 0 0 1   0 0 0 1   0 0 0 1   0 0 0 1 


First, translate by d i along z axis Then, translate by ai along x axis

and rotate by θ i about z axis and rotate byα i about x axis

 Forward kinematics: DH parameters

 cθ i − sθ i 0 0  1 0 0 0  1 0 0 ai   1 0 0 0
    
 sθ i cθ i 0 0  0 1 0 0  0 1 0 0   0 cα i − sα i 0
T= 
0 0 1 0  0 0 1 di   0 0 1 0   0 sα i cα i 0
    
 0 0 0 1   0 0 0 1   0 0 0 1   0 0 0 1 

 cθ i − sθ i cα i sθ i sα i ai cθ i 
 
 sθ i cθ i cα i − cθ i sα i ai sθ i 
= 
0 sα i cα i di 
 
 0 0 0 1 

 Forward kinematics: DH parameters
Four DH parameters: ( ai α i di θ i)
T = Trot ( z ,θ i )Ttrans ( z ,d i )Trot ( x ,α i )Ttrans ( x ,ai )
0
ŷ
1
1 ŷ
ŷ 2
ŷ
1
d
1
x̂ a x̂
0
x̂
θ α
1
0 1 ẑ 2
ẑ
ẑ ẑ

First, translate by d i along z axis Then, translate by ai along x axis

and rotate by θ i about z axis and rotate byα i about x axis

Forward kinematics: DH parameters

• A series of transforms is written as a table:

xform ai α i di θi
1
a1 α1 d1 θ1
2
a2 α 2 d2 θ2
 Example 1: DH parameters
3
x

3
y
l3
l2
ai α di θi 2
q3
i x 1
1
x
1 y
l1 0 0 q1 q2
2
l2 0 0 q2 2
y 0
y
3
l3 0 0 q3 q1
0
x l1
0
z
 Example 1: DH parameters
3
x
 cq1 − sq1 0 l1cq1 
 
s cq1 0 l1sq1  3
y
0
T1 =  q1
0 
l3
0 0 1
  q3 l2
 0 0 0 1 
2
x 1
x
 1
y
q2
 cq 2 − sq2 0 l2 cq2 
  2
s cq2 0 l2 sq2  y 0
y
1
T2 =  q2
0 0 1 0  q1
  0
 0 0 0 1 
x l1
 0
z
 c q3 − s q3 0 l3cq3 
 
 s q3 c q3 0 l3 s q 3 
2
T3 = 
0 0 1 0  ai α i di θi
 
 0 0 0 1 
 1 l1 0 0 q1
2 l2 0 0 q2
0
T3 = T1 T2 T3
0 1 2

3 l3 0 0 q3
Example 2: DH parameters

y2 x2
y3
q3
l3
z2

q2
l2 z3
x3
x1
z1 l1
z0
q1
x0
y0
 Example 2: DH parameters

y2 x2
y3
q3
l3
z2

q2
l2 z3
x3
x1
z1 l1
ai α i di θi z0
q1
x0
1
0 − π
2 l1 q1 y0

2 l2 0 0 q2 − π
2

3 l3 q3
0 0
Example 3: DH parameters

z1
q2
y1 q3
x2
x1
y2 x2
z0 q4
x2
q1 y0 l1 y2
x0 y2
 Example 3: DH parameters

z1
q2
y1 q3
ai α i di θi x2
1
x1
l2 0 l1 q1 y2 x2
2 l3 q2 z0 q4
π 0 x2
q1 y0 l1 y2
3 0 0 l4 q3
x0 y2
4 0 0 q4 0