Inverse Kinematics

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 x
robot to the desired
configuration.
 A Simple Example

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

S
1 Finding S:
X

S  (x 2  y2 )
Solvability
0
Given the numerical value of T we attempt to
N
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)
Inverse Kinematics
More difficult.
 The equations to solve are
nonlinear thus systematic closed-
form solution is not always
available.
 Solution not unique.
 Redundant robot.
 Elbow-up/elbow-down
configuration.
 Robot dependent.
2 solutions!
The Workspace

Workspace: volume of space which can be

reached by the end effector
Dextrous workspace: volume of space
where the end effector can be arbitrarily
oriented
Reachable workspace: volume of space
which the robot can reach in at least one
orientation
Two-link manipulator
If l1 = l2 reachable work
space is a disc of radius
2l1. The dextrous
Workspace is a point:
origin
If l1 l2 there is no
dextrous workspace.
The reachable work
space is a ring of outer
radius l1 + l2 and inner
radius l1 - l2
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.
The Closest Solution in Joint Space

Weights might be applied.

Selection favors moving smaller joints rather
than moving larger joints when a choice
exist.
The presence of obstacles.
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 ,

6  6  180 .
 4 Solutions of the PUMA 560
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 
X3


Y0 Y3

 
Y1 Y2 X2

X1 
X0

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 ca  sa 0  sa 0 d1   s1 c1 0 0
0
T   1 0 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 
2T 
1  2
3T 
2  3

 0 0 1 0  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 
B
T  0
T   123
W 3
 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  y  l  l  2l1l2c2
2 2
1
2 2
2
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  c22   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  k 2 s1
y  k1s1  k 2 c1

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 = 
Geometric Solution
Two-link Planar Manipulator

a2

a1

x
Geometric Solution
Applying the “law of cosines”:

We should try to avoid using the arccos function

because of inaccuracy.
Geometric Solution
The second joint angle 2is then found
accurately as:

Because of the square root, two solutions result:

Geometric Solution
1 is determined uniquely given 2
1=-y

Note the two different solutions for 1

corresponding to the elbow-down versus
elbow-up configurations
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 T from (3.13) is:
6

 c23[c6c4c5  s6 s4 ]  s23 s5 s6  c23[ s6c4c5  c6 s4 ]  s23 s5 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 ]  c23 s5c6 s23[ s6c4c5  c6 s4 ]  s23 s5 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:

p x   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 

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


1  A tan 2 p y , p x   A tan 2 d 3 ,  p x2  p y2  d 32 
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 p x  s1 p y  a3c23  d 4 s23  a2 c2
 p z  a3 s23  d 4 c23  a2 s2
By squaring and addition of resulting
equations and Equ. (4.57):
a3c3  d 4 s3  K
p x2  p y2  p z2  a22  a32  d 32  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 
   (4.70)
 0 0 0 1  0 0 0 1
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 ) p z  (c1 p x  s1 p y )(d 4  a2 s3 ),
(a2 s3  d 4 ) p z  (a3  a2 c3 )(c1 p x  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  r23 s1c23  r33 s23  c4 s5
 r13 s1  r23c1  s4 s5
if s5  0

 4  A tan 2(r13 s1  r23c1 ,  r13c1c23  r23 s1c23  r33 s23 )

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.
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
0
Start with the given tool pose, as T . 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.
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.