Introduction to Robotics (Fag 3480)

Vår 2011

Robert Wood (Harward Engineeering and Applied Sciences-Basis)

Ole Jakob Elle, PhD (Modified for IFI/UIO)
Førsteamanuensis II, Institutt for Informatikk
Universitetet i Oslo
Seksjonsleder Teknologi,
Intervensjonssenteret, Oslo Universitetssykehus (Rikshospitalet)
 Ch. 3: Forward and Inverse
Kinematics

side 2

Fag 3480 - Introduction to Robotics

Recap: The Denavit-Hartenberg (DH)
Convention
Representing each individual homogeneous transformation as the
product of four basic transformations:
Ai = Rot z,θ i Trans z,d i Trans x ,ai Rot x ,α i
cθ i − sθ i 0 0 1 0 0 0  1 0 0 ai   1 0 0 0
s cθ i 0 0 0 1 0 0  0 1 0 0  0 cα i − sα i 0
=  θi
0 0 1 0 0 0 1 d i  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 cθ i cα i − cθ i sα i ai sθ i 
=  θi
0 sα i cα i di 
 
0 0 0 1 

side 3

Fag 3480 - Introduction to Robotics

 Recap: the physical basis for DH
parameters
ai: link length, distance between the o0 and o1 (projected along x1)

αi: link twist, angle between z0 and z1 (measured around x1)

di: link offset, distance between o0 and o1 (projected along z0)

θi: joint angle, angle between x0 and x1 (measured around z0)

side 4

Fag 3480 - Introduction to Robotics

General procedure for
determining forward kinematics
• Label joint axes as z0, …, zn-1 (axis zi is joint axis for joint i+1)
• Choose base frame: set o0 on z0 and choose x0 and y0 using right-handed convention
• For i=1:n-1,
• Place oi where the normal to zi and zi-1 intersects zi. If zi intersects zi-1, put oi at intersection. If
zi and zi-1 are parallel, place oi along zi such that di=0
• xi is the common normal through oi, or normal to the plane formed by zi-1 and zi if the two
intersect
• Determine yi using right-handed convention

• Place the tool frame: set zn parallel to zn-1

• For i=1:n, fill in the table of DH parameters
• Form homogeneous transformation matrices, Ai
• Create Tn0 that gives the position and orientation of the end-effector in the inertial
frame
side 5

Fag 3480 - Introduction to Robotics

 Example 2: three-link
cylindrical robot
3DOF: need to assign four coordinate frames
Choose z0 axis (axis of rotation for joint 1, base frame)

Choose z1 axis (axis of translation for joint 2)

Choose z2 axis (axis of translation for joint 3)

Choose z3 axis (tool frame)

This is again arbitrary for this case since we have described no wrist/gripper

Instead, define z3 as parallel to z2

side 6

Fag 3480 - Introduction to Robotics

 Example 2: three-link
cylindrical robot link ai αi di θi
Now define DH parameters 1 0 0 d1 θ1

First, define the constant parameters ai, αi 2 0 -90 d2 0

Second, define the variable parameters θi, di 3 0 0 d3 0

c1 − s1 0 0 1 0 0 0 1 0 0 0
s c1 0 0  0 0 1 0  0 1 0 0 
A1 =  1 , A2 =  , A3 = 
0 0 1 d1  0 − 1 0 d2  0 0 1 d3 
     
0 0 0 1 0 0 0 1 0 0 0 1

c1 0 − s1 − s1d 3 
s 0 c1 c1d 3 
T3 = A1A2 A3 =
0  1

 0 − 1 0 d1 + d 2 
 
0 0 0 1 

side 7

Fag 3480 - Introduction to Robotics

 Example 3: spherical wrist
3DOF: need to assign four coordinate frames
yaw, pitch, roll (θ4, θ5, θ6) all intersecting at one point o (wrist center)

Choose z3 axis (axis of rotation for joint 4)

Choose z4 axis (axis of rotation for joint 5)

Choose z5 axis (axis of rotation for joint 6)

Choose tool frame:

z6 (a) is collinear with z5

y6 (s) is in the direction the gripper closes

x6 (n) is chosen with a right-handed convention

side 8

Fag 3480 - Introduction to Robotics

 Example 3: spherical wrist
Now define DH parameters link ai αi di θi
First, define the constant parameters ai, αi 4 0 -90 0 θ4
Second, define the variable parameters θi, di 5 0 90 0 θ5
c 4 0 − s4 0 c5 0 − s5 0 c 6 − s6 0 0 6 0 0 d6 θ6
s 0 c4 0 s 0 c5 0 s c6 0 0 
A4 =  4 , A5 =  5 , A6 =  6
0 −1 0 0 0 −1 0 0 0 0 1 d6 
     
0 0 0 1 0 0 0 1 0 0 0 1

c 4c5c 6 − s4s6 − c 4c 5s6 − s 4c 6 c 4 s5 c 4 s5 d 6 

s c c + c s − s 4 c 5 s6 + c 4 c 6 s 4 s5 s4s5d 6 
T6 = A4 A5 A6 =  4 5 6
3 4 6

 − s5 c 6 s5 c 6 c5 c5d 6 
 
 0 0 0 1 

side 9

Fag 3480 - Introduction to Robotics

 Example 4: cylindrical robot
with spherical wrist
6DOF: need to assign seven coordinate frames
But we already did this for the previous two examples, so we can fill in
the table of DH parameters:
link ai αi di θi

1 0 0 d1 θ1

2 0 -90 d2 0

3 0 0 d3 0

4 0 -90 0 θ4
o3, o4, o5 are all at
5 0 90 0 θ5
the same point oc
6 0 0 d6 θ6

side 10

Fag 3480 - Introduction to Robotics

 Example 4: cylindrical robot
with spherical wrist
Note that z3 (axis for joint 4) is collinear with z2 (axis for joint 3),
thus we can make the following combination:
r11 r12 r13 dx  r11 = c1c 4c 5c 6 − c1s 4s6 + s1s5c 6
r r r23 d y  r21 = s1c 4c 5c 6 − s1s 4s6 − c1s5c 6
T6 = T3 T6 =  21 22
0 0 3
r31 = −s 4c 5c 6 − c 4s6
r31 r32 r33 dz 
  r12 = −c1c 4c 5s6 − c1s 4c 6 − s1s5c 6
0 0 0 1
r22 = −s1c 4c 5s6 − s1s 4s6 + c1s5c 6
r32 = s 4c 5c 6 − c 4c 6
r13 = c1c 4s5 − s1c5
r23 = s1c 4s5 + c1c5
r33 = −s 4s5
d x = c1c 4s5d 6 − s1c5d 6 − s1d 3
d y = s1c 4s5d 6 + c1c5d 6 + c1d 3
d z = −s4s5d 6 + d1 + d 2

side 11

Fag 3480 - Introduction to Robotics

 Example 5: the Stanford
manipulator
6DOF: need to assign seven coordinate frames:
Choose z0 axis (axis of rotation for joint 1, base frame)

Choose z1-z5 axes (axes of rotation/translation for joints 2-6)

Choose xi axes

Choose tool frame

Fill in table of DH parameters:

link ai αi di θi

1 0 -90 0 θ1

2 0 90 d2 θ2

3 0 0 d3 0

4 0 -90 0 θ4

5 0 90 0 θ5
6 0 0 d6 θ6 side 12

Fag 3480 - Introduction to Robotics

 Example 5: the Stanford
manipulator
Now determine the individual homogeneous
transformations:

c1 0 − s1 0 c 2 0 s2 0 1 0 0 0
s 0 c1 0 s 0 − c20  0 1 0 0 
A1 =  1 , A2 =  2 , A3 = 
0 −1 0 0 0 1 0 d2  0 0 1 d3 
     
0 0 0 1 0 0 0 1 0 0 0 1
c 4 0 − s 4 0 c5 0 s5 0  c 6 − s6 0 0
s 0 c4 0 s 0 − c5 0 s c6 0 0 
A4 =  4 , A5 =  5 , A6 =  6
 0 −1 0 0 0 − 1 0 0 0 0 1 d6 
     
0 0 0 1 0 0 0 1 0 0 0 1

side 13

Fag 3480 - Introduction to Robotics

 Example 5: the Stanford
manipulator
Finally, combine to give the complete
description of the forward kinematics:
r11 = c1[c 2 (c 4c5c 6 − s4s6 ) − s2s5c 6 ] − d 2 (s4c5c 6 + c 4s6 )
r21 = s1[c 2 (c 4c5c 6 − s4s6 ) − s2s5c 6 ] + c1 (s4c5c 6 + c 4s6 )
r31 = −s2 (c 4c5c 6 − s4s6 ) − c 2s5c 6
r12 = c1[− c 2 (c 4c5s6 + s4c 6 ) + s2s5s6 ] − s1 (− s4c5s6 + c 4c 6 )
r11 r12 r13 dx  r22 = −s1[− c 2 (c 4c5s6 − s4c 6 ) − s2s5s6 ] + c1 (− s4c5s6 + c 4s6 )
r r r23 d y  r32 = s2 (c 4c5s6 + s4c 6 ) + c 2s5s6
T6 = A1 ⋅ ⋅ ⋅ A6 =  21 22
0
r13 = c1 (c 2c 4s5 + s2c5 ) − s1s4s5
r31 r32 r33 dz 

0 0 0 1
 r23 = s1 (c 2c 4s5 + s2c5 ) + c1s4s5
r33 = −s2c 4s5 + c 2c5
d x = c1s2d 3 − s1d 2 + d 6 (c1c 2c 4s5 + c1c5s2 − s1s4s5 )
d y = s1s2d 3 + c1d 2 + d 6 (c1s4s5 + c 2c 4s1s5 + c5s1s2 )
d z = c 2d 3 + d 6 (c 2c5 − c 4s2s5 ) side 14

Fag 3480 - Introduction to Robotics

 Example 6: the SCARA
manipulator
4DOF: need to assign five coordinate frames:
Choose z0 axis (axis of rotation for joint 1, base frame)

Choose z1-z3 axes (axes of rotation/translation for joints 2-4)

Choose xi axes

Choose tool frame

Fill in table of DH parameters:

link ai αi di θi
1 a1 0 0 θ1
2 a2 180 0 θ2
3 0 0 d3 0
4 0 0 d4 θ4
side 15

Fag 3480 - Introduction to Robotics

 Example 6: the SCARA
manipulator
Now determine the individual homogeneous
transformations:
c1 − s1 0 a1c1  c 2 s2 0 a2c 2  1 0 0 0 c 4 − s4 0 0
s c1 0 a1s1  s − c2 0 a2s2  0 1 0 0  s c4 0 0 
A1 =  1 , A2 =  2 , A3 =  , A4 =  4
0 0 1 0  0 0 −1 0  0 0 1 d3  0 0 1 d4 
       
0 0 0 1  0 0 0 1  0 0 0 1 0 0 0 1

c12c 4 + s12s 4 − c12s 4 + s12c 4 0 a1c1 + a2c12 

s c − c s − s12s4 − c12c 4 0 a1s1 + a2s12 
T4 = A1 ⋅ ⋅ ⋅ A4 =  12 4
0 12 4

 0 0 − 1 − d3 − d 4 
 
 0 0 0 1 

side 16

Fag 3480 - Introduction to Robotics

 Industrial robots
High precision and repetitive tasks
Pick and place, painting, etc

Hazardous environments

side 17

Fag 3480 - Introduction to Robotics

 Common configurations: elbow
manipulator
Anthropomorphic arm: ABB IRB1400 or KUKA

Very similar to the lab arm NACHI (RRR)

side 18

Fag 3480 - Introduction to Robotics

Simple example: control of a 2DOF
planar manipulator
Move from ‘home’ position and follow the path AB with a constant contact
force F all using visual feedback

side 19

Fag 3480 - Introduction to Robotics

Coordinate frames & forward kinematics
Three coordinate frames: 0 1 2
Positions:

 x1  a1 cos(θ1 )
 y  =  a sin(θ ) 
 1  1 1 
2
 x 2  a1 cos(θ1 ) + a2 cos(θ1 + θ 2 )  x 
 y  =  a sin(θ ) + a sin(θ + θ )  ≡  
 2  1 1 2 1 2  y  t
1 0
xˆ 0 =  , yˆ 0 =  
0 1 0 1
Orientation of the tool frame:

cos(θ1 + θ 2 ) − sin(θ1 + θ 2 )
xˆ 2 =  ˆ =
 2  cos(θ + θ ) 
, y
 sin( θ 1 + θ 2 
)  1 2 

 xˆ ⋅ xˆ yˆ 2 ⋅ xˆ 0  cos(θ1 + θ 2 ) − sin(θ1 + θ 2 )
R20 =  2 0  =  sin(θ + θ ) cos(θ + θ ) 
 x2 ⋅ y 0 y 2 ⋅ y 0  
ˆ ˆ ˆ ˆ 1 2 1 2 

side 20

Fag 3480 - Introduction to Robotics

Inverse Kinematics
Find the values of joint parameters that will put the tool frame at a desired
position and orientation (within the workspace)
Given H:

R o 
H=  ∈ SE (3 )
 0 1
Find all solutions to:

Tn0 (q1,..., qn ) = H
Noting that:
Tn0 (q1,..., qn ) = A1 (q1 ) ⋅ ⋅ ⋅ An (qn )

This gives 12 (nontrivial) equations with n unknowns

side 21

Fag 3480 - Introduction to Robotics

 Example: the Stanford
manipulator
For a given H: c1[c 2 (c 4c5c 6 − s4s6 ) − s2s5c 6 ] − d 2 (s4c5c 6 + c 4s6 ) = 0
s1[c 2 (c 4c5c 6 − s4s6 ) − s2s5c 6 ] + c1 (s4c5c 6 + c 4s6 ) = 0
0 1 0 − 0.154 
0 − s2 (c 4c5c 6 − s4s6 ) − c 2s5c 6 = 1
0 1 0.763 
H= c1[− c 2 (c 4c5s6 + s4c 6 ) + s2s5s6 ] − s1 (− s4c5s6 + c 4c6 ) = 1
1 0 0 0 
  − s1[− c2 (c 4c5s6 − s4c6 ) − s2s5s6 ] + c1 (− s4c5s6 + c 4s6 ) = 0
0 0 0 1 
s2 (c 4c5s6 + s4c6 ) + c2s5s6 = 0
Find θ1, θ2, d3, θ4, θ5, θ6: c1 (c2c 4s5 + s2c5 ) − s1s4s5 = 0
s1 (c2c 4s5 + s2c5 ) + c1s4s5 = 1
− s 2 c 4 s5 + c 2 c 5 = 0
c1s2d 3 − s1d 2 + d 6 (c1c 2c 4s5 + c1c5s2 − s1s4s5 ) = −0.154
s1s2d 3 + c1d 2 + d 6 (c1s4s5 + c 2c 4s1s5 + c5s1s2 ) = 0.763
c 2d 3 + d 6 (c 2c5 − c 4s2s5 ) = 0

One solution: θ1 = π/2, θ2 = π/2, d3 = 0.5, θ4 = π/2, θ5 = 0, θ6 = π/2

side 22

Fag 3480 - Introduction to Robotics

Inverse Kinematics
• The previous example shows how difficult it would be to obtain a closed-form
solution to the 12 equations

• Instead, we develop systematic methods based upon the manipulator

configuration

• For the forward kinematics there is always a unique solution

• Potentially complex nonlinear functions

• The inverse kinematics may or may not have a solution

• Solutions may or may not be unique

• Solutions may violate joint limits

• Closed-form solutions are ideal!

side 23

Fag 3480 - Introduction to Robotics

 Overview: kinematic
decoupling
Appropriate for systems that have an arm a wrist
Such that the wrist joint axes are aligned at a point

For such systems, we can split the inverse kinematics problem into two parts:
Inverse position kinematics: position of the wrist center

Inverse orientation kinematics: orientation of the wrist

First, assume 6DOF, the last three intersecting at oc

R60 (q1,..., q6 ) = R
o60 (q1,..., q6 ) = o

Use the position of the wrist center to determine the first three joint angles…

side 24

Fag 3480 - Introduction to Robotics

 Overview: kinematic
decoupling
Now, origin of tool frame, o6, is a distance d6 translated along z5 (since z5 and z6 are collinear)
Thus, the third column of R is the direction of z6 (w/ respect to the base frame) and we can write:

0
o = o60 = oco + d 6R 0
1

Rearranging: 0
oco = o − d 6R 0
 1

Calling o = [ox oy oz]T, oc0 = [xc yc zc]T

 xc  o x − d 6 r13 
 y  = o − d r 
 c  y 6 23 

 zc  oz − d 6 r33 

side 25

Fag 3480 - Introduction to Robotics

 Overview: kinematic
decoupling
Since [xc yc zc]T are determined from the first three joint angles, our forward kinematics
expression now allows us to solve for the first three joint angles decoupled from the final
three.
Thus we now have R30

Note that:
R = R30R63
To solve for the final three joint angles:

( )
R63 = R30
−1
( ) T
R = R30 R

Since the last three joints for a

spherical wrist, we can use a set of

Euler angles to solve for them

side 26

Fag 3480 - Introduction to Robotics

 Inverse position
Now that we have [xc yc zc]T we need to find q1,
q 2, q 3
Solve for qi by projecting onto the xi-1, yi-1 plane,
solve trig problem

Two examples: elbow (RRR) and spherical (RRP)

manipulators

For example, for an elbow manipulator, to solve for

θ1, project the arm onto the x0, y0 plane
side 27

Fag 3480 - Introduction to Robotics

Background: two argument atan
We use atan2(·) instead of atan(·) to account for the
full range of angular solutions
Called ‘four-quadrant’ arctan

− atan2(− y , x ) y <0
  y
π − atan −  y ≥ 0, x < 0
  x

atan2(y , x ) =  atan y  y ≥ 0, x ≥ 0
 x
 π
 y > 0, x = 0
2
 undefined y = 0, x = 0


side 28

Fag 3480 - Introduction to Robotics

 Example: RRR manipulator
To solve for θ1, project the arm onto the x0, y0 plane
θ1 = atan2(xc , y c )

θ1 = π + atan2(xc , y c )

side 29

Fag 3480 - Introduction to Robotics

 Caveats: singular
configurations, offsets
If xc=yc=0, θ1 is undefined If there is an offset, then we will have
two solutions for θ1: left arm and
i.e. any value of θ1 will work right arm
However, wrist centers cannot intersect
z0

side 30

Fag 3480 - Introduction to Robotics

 Left arm and right arm
solutions
Left arm: Right arm:
θ1 = α + β
θ1 = φ - α
α = atan2(xc , y c )
φ = atan2(xc , y c )
β = π + atan2 xc + y c − d 2 , d 
2 2

α = atan2 xc + y c − d 2 , d   
2 2

 
= atan2 − xc + y c − d 2 ,−d 
2 2

 

side 31

Fag 3480 - Introduction to Robotics

 Left arm and right arm solutions
Therefore there are in general two solutions for θ1

Finding θ2 and θ3 is identical to the planar two-link manipulator we have seen previously

r 2 + s 2 − a2 − a3
2 2

cos θ 3 =
2a2a3
r 2 = xc + y c − d 2
2 2

s = zc − d1
xc + y c − d 2 + (zc − d1 ) − a2 − a3
2 2 2 2 2

⇒ cos θ 3 = ≡D
2a2a3

Therefore we can find two solutions for θ3:

(
θ 3 = atan2 D,± 1 − D 2 )
side 32

Fag 3480 - Introduction to Robotics

 Left arm and right arm solutions
The two solutions for θ3 correspond to the elbow-down and elbow-up positions
respectively

Now solve for θ2:

θ 2 = atan2(r , s ) − atan2(a2 + a3c3 , a3s3 )

= atan2 xc + y c − d 2 , zc − d1  − atan2(a2 + a3c3 , a3s3 )
2 2

 

Thus there are two solutions for the pair (θ2, θ3)

side 33

Fag 3480 - Introduction to Robotics

 RRR: Four total solutions
In general, there will be a maximum of four solutions to the inverse
position kinematics of an elbow manipulator
Ex: PUMA

side 34

Fag 3480 - Introduction to Robotics

 Example: RRP manipulator
Spherical configuration
Solve for θ1 using same method as with RRR

θ1 = atan2(xc , y c )
Again, if there is an offset, there

will be left-arm and right-arm solutions

Solve for θ2:

θ 2 = atan2(s, r )
r 2 = xc + y c
2 2

s = zc − d1

Solve for d3:

d3 = r 2 + s 2
= xc + y c + (zc − d1 )
2 2 2

side 35

Fag 3480 - Introduction to Robotics

Next class…
Complete the discussion of inverse kinematics
Inverse orientation

Introduction to other methods

Introduction to velocity kinematics and the Jacobian

side 36

Fag 3480 - Introduction to Robotics