Inverse Kinematics

• Given a desired position (P)

& orientation (R) of the
end-effector z
Y  ( x, y, z , O, A, T )

q  (1 ,  2 ,  n ) y
Find the joint variables which
can bring the robot to the x
desired configuration.
 A Simple Example

Revolute and
Finding :
Prismatic Joints
y
Combined θ  arctan( )
x
More Specifically:
(x , y)
y arctan2() specifies that it’s in the
θ  arctan 2( ) first quadrant
x
Y

S
1 Finding S:
X

S  (x 2  y 2 )
Solvability
Given the numerical value of N0T we attempt to
find values of 1, 2 ,, n .
The PUMA 560:

Given 06T as 16 numerical values,

solve for6 joint angles, 1 , 2 ,, 6 .

12 equations and 6 unknowns

6 equations and 6 unknowns

(nonlinear, transcendental equations)
 Existence of Solutions

• A solution to the IKP exists if the target

belongs to the workspace.
• Workspace computation may be hard. In
practice it is made easy by special design of
the robot.
 Multiple Solutions
• The IKP may have more than one
solution. We need to be able to
calculate all the possible solutions.
• The system has to be able to choose
one. 2 solutions!
The closest solution: the
solution which minimizes
the amount that each
joint is required to move.
 Two Possible Solutions

In the absence of obstacles the upper configuration is

selected
 Number of Solutions
Depends upon the 8 solutions exits

number and range

of joints and also is
a function of link
parameters a,a,d

Another 4 solution
4  4  180 ,
5  5 ,
4 Solutions of the PUMA 560
6  6  180 .

Number of Solutions vs. Nonzero ai

a1  a3  a5  0 4
a3  a5  0 8
a3  0  16
All ai  0  16
The more the link length parameters are nonzero,
the bigger the maximum number of solutions!
 Solvability
• All systems with revolute and prismatic joints
having total of 6 D.O.F in a single series chain
are solvable.
• But this general solution is a numerical one.
• Robots with analytic solution: several
intersecting joint axes and/or many ai = 0, 90o.
 Methods of Solutions
• A manipulator is solvable if the joint
variables can be determined by an
algorithm. The algorithm should find all
possible solutions.

closed form solutions

• Solutions
numerical solutions
 Numerical Solutions
• Results in a numerical, iterative solution to system of
equations, for example Newton/Raphson techniques.
• Unknown number of operations to solve.
• Only returns a single solution.
• Accuracy is dictated by user.
• Because of these reasons, this is much less desirable
than a closed-form solution.
• Can be applied to all robots.
Closed-form solutions
Analytical solution to system of equations
Can be solved in a fixed number of
operations (therefore, computationally
fast/known speed)
Results in all possible solutions to the
manipulator kinematics
Often difficult or impossible to find
Most desirable for real-time control
Most desirable overall
Closed-form solutions
Given a 6 axis robot.
It can be proven that there exists a closed form
solution for inverse kinematics:
 If three adjacent revolute joint axes intersect at
a point (PUMA, Stanford).
 If three adjacent revolute joint axes are parallel
to one another (MINIMOVER).

In any case algebraic or geometric intuition is required to

obtain closed form solutions.
 Methods of Solutions
In general the IK problem can be solved by various
methods such as:
• Inverse Transform ( Paul, 1981)
• Screw Algebra (Kohli 1975)
• Dual Matrices (Denavit 1956)
• Iterative (Uicker 1964)
• Geometric approach (Lee 1984)
• Decoupling of position and orientation (Pieper
1968)
 Closed-form Solutions
We are interested in closed-form solutions:

1. Algebraic methods

2. Geometric methods
Algebraic solution
Consider a 3-link manipulator

We can derive
kinematic equations:

B
WT T T T T
0
3
0
1
1 2
2 3
Algebraic solution
D-H transformation 
X 3



Y0 Y3

 
Y1 Y2 X 2


X1 
X 0

i ai-1 ai-1 di i

1 0 0 0 1

2 0 L1 0 2

3 0 L2 0 3
 Algebraic Solution

 c1  s1 0 a0  c1  s1 0 0

 s ca c1ca 0  sa 0  sa 0 d1   s1 c1 0 0
0
T   1 0

1
 s1sa 0 c1sa 0 ca 0 ca 0 d1   0 0 1 0
   
 0 0 0 1   0 0 0 1
c 2  s 2 0 L1  c 3  s 3 0 L2 
 s c 2 0 0   s c 3 0 0 
1
T   2 2
T   3
2
 0 0 1 0
3
 0 0 1 0
   
 0 0 0 1  0 0 0 1
 Algebraic Solution
The kinematics of the example seen before are:
c123  s123 0 l1c1  l2 c12 
s c123 0 l1s1  l2 s12 
W T  3T 
B 0  123

 0 0 1 0 
 
 0 0 0 1 
Assume goal point is the
specification of wrist frame, c  s 0 x
specified by 3 numbers: s
  c 0 y 
B
T 
W
0 0 1 0
 
0 0 0 1
 Algebraic Solution
By comparison, we get c  c123 s  s123
the four equations: x  l1c1  l2 c12
y  l1s1  l2 s12

Summing the square

x 2  y 2  l12  l22  2l1l2c2
of the last 2 equations:
From here we get an x 2  y 2  l12  l22
expression for c2 c2 
2l1l2

And finally: s2   1  c 2
2   2  A tan 2( s2 , c2 ).
 Algebraic Solution
• The arc cosine function does not behave well
as its accuracy in determining the angle is
dependant on the angle ( cos(q)=cos(-q)).
• When sin(q) approaches zero, division by
sin(q) give inaccurate solutions.
• Therefore an arc tangent function which is
more consistent is used.

  A tan 2( s , c )
 Algebraic Solution
Using c12=c1c2-s1s2 and s12= c1s2-c2s1:

x  k1c1  k2 s1
y  k1s1  k2c1

where k1=l1+l2c2 and k2=l2s2.

To solve these eqs, set
r=+ k12+k22 and =Atan2(k2,k1).
 Algebraic Solution
k1

l2
 2 k2
l1
Then: k1=r cos  , k2=r sin  , and we can
write:
x/r= cos  cos 1 - sin  sin 1
y/r= cos  sin 1 + sin  cos 1
or: cos(+1) = x/r, sin(+1) =y/r
 Algebraic Solution
Therefore:
+1 = atan2(y/r,x/r) = atan2(y,x)
And so:
1 = atan2(y,x) - atan2(k2,k1)
Finally, 3 can be solved from:
1+ 2+ 3 = 
Examples: IK Solution for PUMA 560

Algebraic Solution for UNIMATION PUMA 560

We wish to Solve
 r11 r12 r13 px 
r r22 r23 p y  0
6T 
0  21
 1 T (1 ) 12T ( 2 ) 32T ( 3 ) 34T ( 4 ) 54T ( 5 ) 56T ( 6 ) (4.54)
r31 r32 r33 pz 
 
0 0 0 1

For i when 0 T is given as numeric values

6
 Examples: IK Solution for PUMA 560

• Paul (1981) suggest pre multiplying the above

matrix (4.54) by its unworn inverse transform
successively and determine the unknown
angle from the elements of the resultant
matrix equation.
Examples: IK solution for PUMA 560
By multiplying both sides of (4.54)

 T ( )
0
1 1
1 0
T  12T ( 2 ) 32T ( 3 ) 34T ( 4 ) 54T ( 5 ) 56T ( 6 )
6

Inverting 0 T we get:
1

 c1 s1 0 0  r11 r12 r13 px 

 s c 0 0 r21 r22 r23 p y  1
 1 1  6T
 0 0 1 0 r31 r32 r33 pz 
  
 0 0 0 1  0 0 0 1
(4.56)
 Examples: IK Solution for PUMA 560

• 1
6T from (3.13) is:

 c23[c6c4c5  s6 s4 ]  s23s5 s6  c23[ s6c4c5  c6 s4 ]  s23s5 s6  c23c4 s5  c5 s23 a2c2  a3c23  d 4 s23 
  s c s  c s s c s  c c s s d 
6T 
1  4 5 6 4 6 4 5 6 4 6 5 4 3 
 s23[c6c4c5  s6 s4 ]  c23s5c6 s23[ s6c4c5  c6 s4 ]  s23s5 s6 s23c4 s5  c5c23  a3 s23  a2 s2  d 4c23 
 
 0 0 0 1 
Examples: IK solution for PUMA 560
By adapting 16T from (3.13) and equating
elements we have  s1 p x  c1 p y  d 3

Using trigonometric substitution:

px   cos     p2  p2 
 x y 
p y   sin     A tan 2( p , p ) 
 y x 

d3
We obtain c1s  s1c  

d3
Using difference of angles: sin(   1 ) 

 Examples: IK Solution for PUMA 560

And so: cos(  1 )   1 

d32
2
d d 2 
  1  A tan 2 3 ,  1  32 
  
 
Solution for 1:


1  A tan 2 p y , px   A tan 2 d3 ,  px2  p y2  d32 
Note: two possible solution for 1
 Examples: IK Solution for PUMA 560

By equating elements (1,4) and also (3,4)

from Equ. (4.56):
c1 px  s1 p y  a3c23  d 4 s23  a2c2
 pz  a3 s23  d 4c23  a2 s2
By squaring and addition of resulting
equations and Equ. (4.57):
a3c3  d 4 s3  K
px2  p y2  pz2  a22  a32  d32  d 42
Where K
2a2
 Examples: IK Solution for PUMA 560

The above equation is only dependant on 3 so

with similar procedure we get:

 3  A tan 2(a3 , d 4 )  A tan 2 K ,  a32  d 42  K 2 
again two different solution for 3.
We may rewrite (4.54) as
 T ( )
0
3 2
1 0
T  34T ( 4 ) 45T ( 5 ) 56T ( 6 )
6

that is to say
 c1c23 s1c23  s23  a2 c3   r11 r 12 r13 px 
 c s  s1s23  c23 a2 s3  r21 r22 r23 p y  3
 1 23  6T
  s1 c1 0  d 3   r31 r32 r33 pz 
  
 0 0 0 1  0 0 0 1  (4.70)
 Examples: IK Solution for PUMA 560
By adapting 36T from (3.11) and equating (1,4) and
(2,4) we have
c1c23 p x  s1c23 p y  s23 p z  a2 c3  a3
 c1s23 p x  s1s23 p y  c23 p z  a2 s3  d 4
(a3  a2 c3 ) p z  (c1 p x  s1 p y )( a2 s3  d 4 )
s 23 
p z2  (c1 p x  s1 p y ) 2
(a2 s3  d 4 ) p z  (a3  a2 c3 )(c1 p x  s1 p y )
c23 
p z2  (c1 p x  s1 p y ) 2
We solve for 23 as
 23  A tan 2[(a3  a2c3 ) pz  (c1 px  s1 p y )(d 4  a2 s3 ),
(a2 s3  d 4 ) pz  (a3  a2c3 )(c1 px  s1 p y )]
Four possible
solutions for 2  2   23  3
 Examples: IK Solution for PUMA 560

By equating elements (1,3) and also (3,3)

from (4.70) :
r13c1c23  r23s1c23  r33s23  c4 s5
 r13s1  r23c1  s4 s5
if s5  0

 4  A tan 2(r13s1  r23c1 ,  r13c1c23  r23s1c23  r33s23 )

 Examples: IK Solution for PUMA 560

We continue similar procedure for rest of the

Parameters…
0
( T)
4 T  T ( 5 ) T ( 6 )
1 0
6
4
5
5
6

 5  A tan 2( s5 , c5 )

0
( T)
5
1 0
6 T  T ( 6 )
5
6

 6  A tan 2( s6 , c6 )
Examples: IK Solution for PUMA 560
Other possible solutions :
 4'   4  180
 5'   5
 6   6  180

After all eight solutions have been computed,

some of them may have to be discarded
because of joint limitations. Usually the one
closest to present manipulator configuration
is chosen.
 Inverse Kinematics
• Transformation Matrix 1 
 
 nx sx ax px   2
n sy ay p y   3 
T06   y  T01T12T23T34T45T56  
 nz

sz az pz 
  4 
0 0 0 1  5 
 
 6 
Robot dependent, Solutions not unique
Systematic closed-form solution in general is not available

 Special cases make the closed-form arm solution possible:

1. Three adjacent joint axes intersecting (PUMA, Stanford)

2. Three adjacent joint axes parallel to one another (MINIMOVER)

 Example
• Solving the inverse kinematics of Stanford arm
 nx sx ax px 
n sy ay p y 
T06   y  T01T12T23T34T45T56
 nz sz az pz 
 
0 0 0 1

(T01 ) 1T06  T12T23T34T45T56  T16

X X X C1 p x  S1 p y   X X X S 2  d 3 
X X X  pz  X X X  C 2  d 3 
T16    
X X X  S1 p x  C1 p y   X X X 0.1 
   
0 0 0 1  0 0 0 1 
 Example
• Solving the inverse kinematics of Stanford arm
 Sin1  p x  cos 1  p y  0.1 (1)
 cos 1  p x  sin 1  p y  sin  2  d 3 (2)

 pz   cos 2  d3 (3)
In Equ. (1), let
py
px  r  cos a , p y  r  sin a , r p p ,
2
x
2
y a  a tan 2( )
px
sin( a  1 )  0.1 

sin a  cos 1  sin 1  cos a  0 . 1  r 
r cos(a  1 )   1  (0.1 / r ) 
2

py 0.1
1  a tan 2( )  a tan 2( )
px  r  0.1
2 2

cos 1 p x  sin 1 p y pz
 2  a tan 2( ) d3 
pz cos  2
 Example
• Solving the inverse kinematics of Stanford arm
X X S 5 0
X X  C 5 0
(T34 ) 1 (T23 ) 1 (T12 ) 1 (T01 ) 1T06  T45T56  
X X 0 0
 
From term (3,3) 0 0 0 1

 S 4 [C 2 (C1a x  S1a y )  S 2 a z ]  C 4 ( S1a x  C1a y )  0

 S1a x  C1a y S 5
 4  a tan 2( )  5  a tan 2( )
C 2 (C1a x  S1a y )  S 2 a z C 5
From term (1,3),
(2,3)
S5  C 4 (C 2 (C1ax  S1a y )  S 2 az ]  S 4 (S1ax  C1a y )

 C5  S 2 (C1ax  S1a y )  C 2 az )
 Example
• Solving the inverse kinematics of Stanford arm
C 6  S 6 0 0
 S C 6 0 0
(T45 ) 1 (T34 ) 1 (T23 ) 1 (T12 ) 1 (T01 ) 1T06  T56   6
 0 0 1 0
 
 0 0 0 1

S 6  C5{C 4 [C 2 (C1sx  S1s y )  S 2 sz ]  S 4 (S1sx  C1s y )}  S5 [ S 2 (C1sx  S1s y )  C 2 sz ]

 C 6  S 4 [C 2 (C1sx  S1s y )  S 2 sz ]  C 4 (S1sx  C1s y )

S 6
 6  a tan 2( )
C 6
Pieper’s Solution When Three Axes Intersect
(E.G, Spherical Wrists)

• A completely general robot with six degrees of

freedom does not have a closed form solution,
however special cases can be solved.
• The technique involves decoupling the position
and orientation problems. The position problem
positions the wrist center, while the orientation
problem completes the desired orientation.
Pieper’s Solution
This method applies to
all six joints revolute,
with the last 3
intersection

It can be applied to
majority of industrial
robots
Pieper’s Solution: Basic Concept
First, the location of the wrist center, pc is
found from the given tool position (d) and
the tool pointing direction (here z6). Since
Z5
the wrist center location depends on the
first three joint variables, this results in Z4 d6
three equations and three unknowns which
are solved for 1 – 3. Then, the relative
wrist orientation R63 which is a function of Pc Z6
the last three joint variables, 4 – 6 is found
from the arm orientation R30 and the given
tool orientation R60. The relative wrist
d
orientation is set equal to the kinematic
description of R63 and the last 3 joint
variables are solved.
{0}
 Pieper’s Solution Step-by-step Procedure

• Start with the given tool pose, as T .

0
6
• Solve portions of the forward kinematics to find
find 03T (1 , 2 , 3 ), 63 R( 4 , 5 , 6. )
• Find the location of the wrist center, Pc as d 6 (last
column of 06T ) - d 6 (tool offset length) x Z 6 (third
column of 06T ).
• Set Pc = last column of 0T ( , , ) and solve for
for 1,2 ,.3 3 1 2 3

• Solve 63 R as 30 R 1 60 R (put 1,2 ,3 into 30 R (1 , 2 , 3 ) ).

• Set 63 R equal to 63 R( 4 ,5 ,6 ) and solve for  4 ,5 ,6.
• Along the way, keep track of the number of solutions
for each joint variable.
 Piper’s Solution – 3 axis intersect
• Solution for manipulators with 6DOF’s when
three consecutive axis intersect
• We will consider the case of revolute joints and
last three axis intersect
• Recall the transformation:
 c i  s i 0 ai 1 
 s ca c  c a  s a  s a d 
i 1  i i 1 i i 1 i 1 i 1 i 
iT 
 s i sa i 1 c i sa i 1 ca i 1 ca i 1d i 
 
 0 0 0 1 
 Piper’s Solution
0
P 4 O R G  01T 21T 23T 3P 4 O R G   x y z 1 T
The vector P4ORG in the 3-frame has the form:
 a3 
  sa d 
3
P4ORG   3 4
 ca 3 d 4 
 
 1 
And in the 3-frame:
 a3   f1  3    a 3 c3  d 4 s a 3 s 3  a 2 
  s a d   f    a ca s  d s a ca c  d s a ca  d s a 
2
P4 ORG  23T  3 4  2 3   3 2 3
  4 3 2 3 4 2 3 3 2
 ca 3 d 4   f 3  3   a3 s a 2 s3  d 4 s a 3 s a 2 c3  d 4 ca 2 ca 3  d 3 ca 2 
     
 1   1   1 
 Piper’s Solution (II)
Repeating patiently c1 g1  s1 g 2 
s g  c g 
0
P4ORG   1 1 1 2 
 g3 
 
 1 
g1  c 2 f1  s 2 f 2  a1
g 2  s 2 c a 1 f1  c 2 c a 1 f 2  s a 1 f 3  d 2 s a 1
g 3  s 2 s a 1 f1  c 2 s a 1 f 2  c a 1 f 3  d 2 c a 1
As mentioned before, for rotational joint0 P4 ORG
does not depend on 1:
0 2
r P4 OR G  x 2  y 2  z 2  g 12  g 22  g 32
Replace the gi’s, using z = g3 and work patiently:
r  k1c2  k 2 s2 2 a1  k3
z  k1s2  k 2 c2 sa1  k 4
k1  f1
k2   f 2
k3  f12  f 22  f 32  a12  d 22  2 d 2 f 3
k4  f 3ca1  d 2 ca1
1. If a1=0, r = k3(3). Solve for 3
2. If sin(a1)=0, z = k4(3). Solve for 3
3. Otherwise, eliminate s2 and c2 above to get
r  k3 2  z  k4 2  k12  k22
4a12 sa12
 Piper’s Solution (IV)

• 1 and 2 above give a quadratic equation in

tan(3/2)
• 3 gives an equation of degree four
• Having solve 3, can solve above for 2 and 1
• The remaining angles can be computed to give
the desired orientation.
 Example: PUMA560
• Want to solve:
 r11 r12 r13 px 
r r22 r22 p y 
6
0T   21
 r31 r32 r33 pz 
 
0 0 0 1 
 01T (1 ) 21T ( 2 ) 23T ( 3 ) 34T ( 4 ) 45T ( 5 ) 56T ( 6 )
• TRICK: Invert transformations to separate vbles:

 0
1T  1 6
( 1 ) 0 T  21 T ( 2 ) 23 T ( 3 ) 34 T ( 4 ) 45 T ( 5 ) 56 T ( 6 )
PUMA560: DK Solution
• Then:
 c1 s1 0 0   r11 r12 r13 px 
 s c 0 0   r21 r22 r22 p y 
 1 1
 0 0 1 0   r31 r32 r33 pz 
  
 0 0 0 1  0 0 0 1 
 61T ( 2  6 )
• Equating (2,4) element from two sides:
 s1 p x  c1 p y  d 3
• Equation can also be obtained from “geometrical”
arguments
• Two possible solutions
The size of the translation in {1} is independent of 1:

p x2  p 2y  p z2  a 22  a 32  d 32  d 42
a 3 c3  d 4 s3  K
2a2
• Geometrical meaning?
• Two possible solutions
• Write:
 0
1T ( 1 ) 21 T ( 2 ) 23 T  1 6
( 3 ) 0 T  34 T ( 4 ) 45 T ( 5 ) 56 T ( 6 )
• Repeat now a similar procedure
 Standard Frames

{W}

{B}
{T}

{G}
{S}
Repeatability and accuracy
Repeatability: how precisely a manipulator
can return to a taught point?
Accuracy: the precision with which a
computed point can be attained.