Experiment 6: Inverse kinematics of a 6-DoFs Industrial Robot.

The Unimation PUMA 560

As an example of the algebraic solution technique applied to a manipulator with six
degrees of freedom, we will solve the kinematic equations of the PUMA 560, which
were developed in Chapter 3. This solution is in the style of [5].
We wish to solve
 
r11 r12 r13 px
0
 r21 r22 r23 py 
6
T =
r r r p 

31 32 33 z
0 0 0 1
= 01 T (θ1 )12 T (θ2 )23 T (θ3 )34 T (θ4 )45 T (θ5 )56 T (θ6 ) (4.54)

for θi when 06 T is given as numeric values.

A restatement of (4.54) that puts the dependence on θ1 on the left-hand side
of the equation is
[01 T (θ1 )]−1 06 T = 12 T (θ2 )23 T (θ3 )34 T (θ4 )45 T (θ5 )56 T (θ6 ). (4.55)

Inverting 01 T , we write (4.55) as

  
c1 s 1 0 0 r11 r12 r13 px
 −s c 0 0  r21 r22 r23 py 
 1 1   = 1T , (4.56)
 0 0 1 0   r31 r32 r33 pz  6
0 0 0 1 0 0 0 1

where 16 T is given by equation (3.13) developed in Chapter 3. This simple technique

of multiplying each side of a transform equation by an inverse is often used to
advantage in separating out variables in the search for a solvable equation.
Equating the (2, 4) elements from both sides of (4.56), we have
−s1 px + c1 py = d3 . (4.57)
To solve an equation of this form, we make the trigonometric substitutions

px = ρ cos φ,
py = ρ sin φ, (4.58)

where

ρ = px2 + py2 ,

φ = Atan2(py , px ). (4.59)

Substituting (4.58) into (4.57), we obtain

d3
c1 s φ − s 1 cφ = . (4.60)
ρ
From the difference-of-angles formula,
d3
sin(φ − θ1 ) = . (4.61)
ρ
Hence,
d32
cos(φ − θ1 ) = ± 1 − , (4.62)
ρ2
and so  
d 2
d
φ − θ1 = Atan2  3 , ± 1 − 32  . (4.63)
ρ ρ

Finally, the solution for θ1 may be written as


θ1 = Atan2(py , px ) − Atan2 d3 , ± px2 + py2 − d32 . (4.64)

Note that we have found two possible solutions for θ1 , corresponding to the plus-
or-minus sign in (4.64). Now that θ1 is known, the left-hand side of (4.56) is known.
If we equate both the (1,4) elements and the (3,4) elements from the two sides of
(4.56), we obtain

c1 px + s1 py = a3 c23 − d4 s23 + a2 c2 ,
−px = a3 s23 + d4 c23 + a2 s2 . (4.65)

If we square equations (4.65) and (4.57) and add the resulting equations, we obtain

a3 c3 − d4 s3 = K, (4.66)

where
px2 + py2 + px2 − a22 − a32 − d32 − d42
K= . (4.67)
2a2
Note that dependence on θ1 has been removed from (4.66). Equation (4.66) is of
the same form as (4.57) and so can be solved by the same kind of trigonometric
substitution to yield a solution for θ3 :

θ3 = Atan2(a3 , d4 ) − Atan2(K, ± a32 + d42 − K 2 ). (4.68)

The plus-or-minus sign in (4.68) leads to two different solutions for θ3 . If we consider
(4.54) again, we can now rewrite it so that all the left-hand side is a function of only
knowns and θ2 :
[03 T (θ2 )]−106 T = 34 T (θ4 )45 T (θ5 )56 T (θ6 ), (4.69)
or   
c1 c23 s1 c23 −s23 −a2 c3 r11 r12 r13 px
 −c s −s s −c a s   r21 r22 r23 py 
 1 23 1 23 23 2 3   = 3T , (4.70)
 −s1 c1 0 −d3   r31 r32 r33 pz  6
0 0 0 1 0 0 0 1
where 36 T is given by equation (3.11) developed in Chapter 3. Equating both
the (1,4) elements and the (2,4) elements from the two sides of (4.70), we get
c1 c23 px + s1 c23 py − s23 pz − a2 c3 = a3 ,
−c1 s23 px − s1 s23 py − c23 pz + a2 s3 = d4 . (4.71)
These equations can be solved simultaneously for s23 and c23 , resulting in
(−a3 − a2 c3 )pz + (c1 px + s1 py )(a2 s3 − d4 )
s23 = ,
pz2 + (c1 px + s1 py )2
(a2 s3 − d4 )pz − (a3 + a2 c3 )(c1 px + s1 py )
c23 = . (4.72)
pz2 + (c1 px + s1 py )2

The denominators are equal and positive, so we solve for the sum of θ2 and θ3 as
θ23 = Atan2[(−a3 − a2 c3 )pz − (c1 px + s1 py )(d4 − a2 s3 ),
(a2 s3 − d4 )pz − (a3 + a2 c3 )(c1 px + s1 py )]. (4.73)

Equation (4.73) computes four values of θ23 , according to the four possible combina-
tions of solutions for θ1 and θ3 ; then, four possible solutions for θ2 are computed as
θ2 = θ23 − θ3 , (4.74)
where the appropriate solution for θ3 is used when forming the difference.
Now the entire left side of (4.70) is known. Equating both the (1,3) elements
and the (3,3) elements from the two sides of (4.70), we get
r13 c1 c23 + r23 s1 c23 − r33 s23 = −c4 s5 ,
−r13 s1 + r23 c1 = s4 s5 . (4.75)
As long as s5  = 0, we can solve for θ4 as
θ4 = Atan2(−r13 s1 + r23 c1 , −r13 c1 c23 − r23 s1 c23 + r33 s23 ). (4.76)
When θ5 = 0, the manipulator is in a singular configuration in which joint axes 4 and
6 line up and cause the same motion of the last link of the robot. In this case, all that
matters (and all that can be solved for) is the sum or difference of θ4 and θ6 . This
situation is detected by checking whether both arguments of the Atan2 in (4.76) are
near zero. If so, θ4 is chosen arbitrarily,4 and when θ6 is computed later, it will be
computed accordingly.
If we consider (4.54) again, we can now rewrite it so that all the left-hand side
is a function of only knowns and θ4 , by rewriting it as

[04 T (θ4 )]−1 60 T = 45 T (θ5 )56 T (θ6 ), (4.77)

where [04 T (θ 4)]−1 is given by

 
c1 c23 c4 + s1 s4 s1 c23 c4 − c1 s4 −s23 c4 −a2 c3 c4 + d3 s4 − a3 c4
 −c c s + s c −s c s − c c s s a2 c3 s4 + d3 c4 + a3 s4 
 1 23 4 1 4 1 23 4 1 4 23 4 , (4.78)
 −c1 s23 −s1 s23 −c23 a2 s3 − d4 
0 0 0 1

and 46 T is given by equation (3.10) developed in Chapter 3. Equating both the (1,3)
elements and the (3,3) elements from the two sides of (4.77), we get

r13 (c1 c23 c4 + s1 s4 ) + r23 (s1 c23 c4 − c1 s4 ) − r33 (s23 c4 ) = −s5 ,

r13 (−c1 s23 ) + r23 (−s1 s23 ) + r33 (−c23 ) = c5 . (4.79)

Hence, we can solve for θ5 as

θ5 = Atan2(s5 , c5 ), (4.80)

where s5 and c5 are given by (4.79).

Applying the same method one more time, we compute (05 T )−1 and write
(4.54) in the form
(05 T )−1 60 T = 56 T (θ6 ). (4.81)
Equating both the (3,1) elements and the (1,1) elements from the two sides of (4.77)
as we have done before, we get

θ6 = Atan2(s6 , c6 ), (4.82)

where

s6 = −r11 (c1 c23 s4 − s1 c4 ) − r21 (s1 c23 s4 + c1 c4 ) + r31 (s23 s4 ),

c6 = r11 [(c1 c23 c4 + s1 s4 )c5 − c1 s23 s5 ] + r21 [(s1 c23 c4 − c1 s4 )c5 − s1 s23 s5 ]
−r31 (s23 c4 c5 + c23 s5 ).

Because of the plus-or-minus signs appearing in (4.64) and (4.68), these equations
compute four solutions. Additionally, there are four more solutions obtained by
 Section 4.7 Examples of inverse manipulator kinematics 121

‘‘flipping’’ the wrist of the manipulator. For each of the four solutions computed
above, we obtain the flipped solution by
θ4′ = θ4 + 180◦ ,
θ5′ = −θ5 , (4.83)
θ6′ = θ6 + 180◦ .
After all eight solutions have been computed, some (or even all) of them might have
to be discarded because of joint-limit violations. Of any remaining valid solutions,
usually the one closest to the present manipulator configuration is chosen.

FIGURE : Four solutions of the PUMA 560.