Chapter 4

Inverse Kinematics

Goal ..................................................................................................................................... 1
4.1 Solvability ................................................................................................................... 1
Manipulator Workspace: ................................................................................................. 2
4.2 The Algebraic Solution ............................................................................................... 3
Simple Trigonemtric Equations ....................................................................................... 4
4.3 Examples ..................................................................................................................... 6
Example 1: ....................................................................................................................... 6
Example 2: FANUC S-900W Robot ............................................................................... 7
Example 3: PUMA 560 Robot...................................................................................... 11
4.4 Repeatability and Accuracy ...................................................................................... 12
Chapter 4: Inverse Kinematics AER 525 - Robotics

Goal
To find the values of joint variables (manipulator configuration) that will place the end-
effector at a desired position and orientation relative to the base, given the manipulator
geometry (link lengths, offsets, twist angles, and the location of the base).

4.1 Solvability
0
Given the numerical value of the homogeneous transformation matrix Tn , we attempt to

find values of joint variables (  or d, depending on the type of the joint) q1, q2, …, qn.

If the dimension of the task space is m, then there are m independent equations with n
unknown joint variables:
a) m  n : that is, the number of robot degrees of freedom is not sufficient to provide all
possibilities of end-effector position and orientation. Hence, the inverse kinematics
problem may not have a solution.
b) m  n : that is, there are enough equations to solve for the unknowns. However,
these equations are nonlinear and transcendental. Hence, there may be one or more
solutions (but finite) to the inverse kinematics problem.
c) m  n : that is, there are more degrees of freedom than required to provide the
desired end-effector position and orientation. Hence, there are infinite solutions to the
inverse kinematics problem. This situation is called Redundancy.

(a) (b) (c)

m>n m=n m<n
(m=3 , n=2) (m=3 , n=3) (m=3 , n=4)

4-1
Chapter 4: Inverse Kinematics AER 525 - Robotics

NOTE: Generally, the solutions of the inverse kinematics problem are not necessarily
in the analytical closed form, but must be computed through numerical methods.
However, for a 6 d.o.f. manipulator, closed-form solutions are guaranteed if three
neighboring joint axes intersect at a point (Pieper's Theorem). This includes manipulators
with three consecutive parallel axes, since they meet at a point at infinity. The majority of
industrial robots have a spherical wrist attached to the end-effector providing its
orientation, and the first three joints assign the wrist point location in the space. Hence,
the end-effector orientation and the wrist-point position problems are de-coupled and can
be solved separately.
Manipulator Workspace:
The workspace of a manipulator is defined as the set of all end-effector locations
(positions and orientations of the end-effector frame) that can be reached by arbitrary
choices of joint variables within the corresponding ranges, regarding all the physical
limitations such as link lengths and joint limits. If both end-effector position and

Top Side Top Side

Cartesian Cylindrical
Top
Top

Side
Side

Spherical Articulated

4-2
Chapter 4: Inverse Kinematics AER 525 - Robotics

orientation are considered, the workspace is the complete or dexterous workspace;

disregarding the orientation of the end-effector gives the reachable workspace, i.e., the
volume of space which the manipulator can reach in at least one orientation. Obviously,
the dexterous workspace is a subset of reachable workspace.
NOTE: The necessary condition for a solution of inverse kinematics to exist is that the
specified goal point must lie within the reachable workspace and m  n .

4.2 The Algebraic Solution

For an n d.o.f. manipulator, the process of solving the inverse kinematics problem is as
follows:
STEP 1) Assign the Denavit-Hartenberg parameters and link coordinate frames, and
0 1 n 1
derive the homogenous transformation matrices T1 , T2 , …, Tn , and
0
obtain Tn as a function of joint variables. (Forward Kinematics Problem)

STEP 2) Start from the following matrix equation:

T 0
1
1 0
Tn 1T2 2T3 n  1 Tn ,
and equalize suitable elements of the matrices on both sides of the above
equation to reach to "simple" trigonometric equations for solving joint
variables.
STEP 3) If required, continue to the next equations as follows and repeat step 2, until
all joint variables are solved:

T  T  T  T T T
1
2
1 0
1
1 0
n
2
3
3
4
n 1
n

T  T  T  T  T T
2
3
1 1
2
1 0
1
1 0
n
3
4
n 1
n

4-3
Chapter 4: Inverse Kinematics AER 525 - Robotics

Simple Trigonemtric Equations

a) cos q  A ; (A is constant)
 1  A2 
2 solutions: q   Atan 2 .
 A 
 

b)  A sin q  B cos q  0 ; (A and B are constant)

  B
q1  Atan2 
2 solutions:   A
q  q  180 o
 2 1

NOTE: If both A and B are zero, the joint variable is undefined and one d.o.f. is lost
(singular configuration). This is known as a degeneracy, and the joint variable is
arbitrarily set to zero.

c)  A sin q  B cos q  C ; (A and B and C are constant)

r  A 2  B 2
 A  r cos  
Solution: Define:  ; where   B.
 B  r sin   Atan 2 
  A
 sin  q   C
 r

 
By substitution, obtain: 2 .
cos  q    1  C r

B  C 
2 solutions: q  Atan2   Atan2 
 A  r C
2 2


 A cos q1  B cosq1  q 2   C sinq1  q2   D

d)  (A, B, D, H are constant)
 A sin q1  B sinq1  q2   C cosq1  q2   H

Solution: take the square of both equations and add them up:

4-4
Chapter 4: Inverse Kinematics AER 525 - Robotics

2 2 2 2 2
D  H  A  B C
C sin q 2  B cos q2  F
2A
 1t
2
cos q 2 
Define:  1  t 2 ; where t  tan q2
 sin q2  2t 2
 1 t2
By substitution, obtain: F  B t 2  2Ct  F  B   0 .
Hence,

B  F 
2 solutions: q2   Atan2    Atan2   ;
C   C 2
 B 2
 F 2


 M cos q1  N sin q1  D
Solving for q1 in the original equations:  ;
 N cos q1  M sin q1  H

M  A  B cos q2  C sin q2

where  .
 N  B sin q 2  C cos q2
DM  HN H  N cos q1
Hence, cos q1  2 2
, and sin q1  ;
M N M
And therefore,

 sin q1 
q1  Atan2 
 cos q1

NOTE: Determining both the sine and cosine functions of the desired joint variable,
and then applying the two-argument arctangent function ensures that we have found all
solutions, and that the solved variable is in the proper quadrant.

NOTE: Before using the algebraic approach, it is always proper to check the
geometrical relations through which the joint variables may be calculated more easily.

4-5
 Chapter 4: Inverse Kinematics AER 525 - Robotics

4.3 Examples
Example 1:
For the 3 d.o.f. planar manipulator shown in the figure, the x-y location ( px and py) and
the orientation (  ) of the end-effector is given. Find the corresponding joint angles.

Solution: By inspection of the geometry, the kinematics equations are immediately

obtained as follows:

 p x  L1 cos  1  L2 cos 1   2 

 p y  L1 sin 1  L2 sin 1   2 

   1   2   3
The first two equations are of type (d) (with C=0), and can be solved for  1 and  2 ,
and  3 is obtained next from the third equation. As shown in the figure, two solutions
exist for the inverse kinematics problem

x̂3 3
ŷ3 
ŷ0
ŷ1 ŷ 2 L2
x̂2 2
Py L1
x̂1
1
x̂0

Px i i-1 ai-1 di i
1 0 0 0 1
2 0 L1 0 2
3 0 L2 0 3

4-6
Chapter 4: Inverse Kinematics AER 525 - Robotics

Example 2: FANUC S-900W Robot

The forward kinematics problem of this robot was solved in Chapter 3, and the position
and orientation of the end-effector were formulated as functions of joint variables.
Having the end-effector position and orientation, the same equations can be used to solve
the inverse kinematics. However, they are highly nonlinear and difficult to solve; hence,
numerical methods must be applied. Nevertheless, having a wrist in the system (three
consecutive coinciding joint axes) necessitates the existence of a closed-form solution for
the inverse kinematics as follows:
a) Wrist-Point Position
The position of the wrist point does not depend on the rotations of the last three joints.
Hence,
0 3
 
w 0 T3 CW  0T3 0 0 d 4 1 .
T

On the other hand,

 px    0   px  d 6 a x   wx 
        
0
 p y   R6  0    p y
0  d6 a y  w y 
ˆ  pˆ  WP 
w w          (Known)
 pz   d 6    p z  d 6 a z   wz 
       
 1   1   1   1 
By equating the above two equations, three scalar equations will be obtained that can be
solved for three unknowns  1 ,  2 , and  3 . However, by pre-multiplying by the inverse

ŵ
W
p̂ P

4-7
Chapter 4: Inverse Kinematics AER 525 - Robotics

0
of T1 simpler equations can be obtained:

T
0
1
1 0
w 1T3 3 w ;
or wx c1  w y s1  a1  a2 c 2  a3 c 23  d 4 s 23
Type (d)
wz  a2 s 2  a3 s 23  d 4 c 23
wx s1  w y c1  0 Type (b)

First, by solving the third equation  1 will be obtained:

 wy 
 11  Atan2 ,
 and  12   11  180 
w
 x
 11 is the front-reach solution and  12 is the back-reach solution, and due to mechanical
constraints the second solution is not feasible for this manipulator.
Next, by solving the first two equations,  2 and  3 will be obtained:
 
 
2
  wx c1  w y s1  a1 2  wz2  a22  a32  d 42  
 d 4  d 4  a3   
2 2

  2a2  
 
 3  2 Atan 2 

a 
 2 2 2
 2
wx c1  w y s1  a1  wz  a2  a3  d 4
2

 3
2 a2 
 
 
The two solutions of  3 are due to the fully-stretched and folded-back configurations. In

case of no real root, the assigned wrist-point position is not reachable. And,

 sin 2 
 2  Atan2 
 cos  2


cos  
 
wx c1  w y s1  a1 a2  a3 c3  d 4 s 3   wz a3 s 3  d 4 c3 

a2  a3c3  d 4 s32  a3 s3  d 4 c32
2

where 
 sin  wz  a3 s 3  d 4 c3 cos 2
 2
a2  a3 c3  d 4 s3

4-8
Chapter 4: Inverse Kinematics AER 525 - Robotics

In conclusion, given the wrist-point location, mathematically there are at most 4 possible
arm configurations, but due to the mechanical constraints, only two of them are
physically possible.
b) End-effector Orientation
0
By solving the first three joints, T3 is known and the forward kinematics equation can

be transformed to:
3
 
T6  0T3
1 0
T6 .
Equating the (3,3) elements of the above equation yields:


cos 5  a x c1s 23  a y s1s 23  a z c 23 , 
and hence,


sin 5   1  a x c1s 23  a y s1s 23  a z c 23 2 .

Therefore, in general for each set of  1 ,  2 ,  3 two solutions exist for  5 :

 s5 
 5   Atan2 
 c5 
Next, equating the elements (1,3) and elements (2,3) results:

 a x c1c 23  a y s1c 23  a z s 23
 cos  4 
s5
 a x s1  a y c1
.
 sin 
 4
s5
Hence, corresponding to each solution set of  1 ,  2 ,  3 , and  5 a unique solution of
 4 can be obtained:
 s4 
 4  Atan2 .
 c4 
Similarly, equating the elements (3,1) and elements (3,2) yields:

4-9
Chapter 4: Inverse Kinematics AER 525 - Robotics

 n x c1s 23  n y s1s 23  n z c 23
 cos  6  
s5
 o x c1s 23  o y s1s 23  o z c 23
;
 sin 
 6
s5
and hence, for each set of  1 ,  2 ,  3 , and  5 , a unique solution if  6 is obtained as:

 s6 
 6  Atan2 .
 c6 
In case  5  0 or  the sixth joint axis ( ẑ 5 ) is in the line with the fourth joint axis
( ẑ 3 ) and hence  4 or  6 are not independent (degeneracy). In this case, one of them can

be arbitrarily set to zero. For example  4  0 and by  6 can be uniquely obtained from
elements (1,1) and (2,1) as:

c1c 23cos 6  s1 sin 6  n x

 .
s1c 23cos 6  c1 sin 6  n y
In conclusion, corresponding to each solution set of the first three joints, there are two
possible wrist configurations. Hence, mathematically a total of eight configurations are
possible. However, due to physical limitations, four of them are feasible. When
 5  0 or  , wrist is in singular configuration and only sum or difference of  4 and
 6 can be computed.

4-10
Chapter 4: Inverse Kinematics AER 525 - Robotics

Example 3: PUMA 560 Robot

Closed-form solutions exist for the inverse kinematics formulation of the PUMA 560
robot. The algebraic solving process is similar to Example 2. As a result, for each desired
end-effector position and orientation inside the workspace, 4 solutions exist as shown in
the figure below.

4-11
Chapter 4: Inverse Kinematics AER 525 - Robotics

4.4 Repeatability and Accuracy

Industrial robots are usually moved to target points that have been already taught. A
taught point is the one that the manipulator is moved to physically, and then the joint
position sensors are read and recorded. When the robot is commanded to return to that
point, each joint is moved to the stored value. In this teach and playback process, the
inverse kinematics problem does not arise, as the target points are not specified in the
task space. The specification of how precisely the robot can return to the taught points, is
the repeatability characteristic of the robot. Manipulators with low joint backlash,
friction, and flexibility usually have high repeatability.
For more complicated tasks, the target points are not taught but defined in the task space
(computed points). For example, the robot is equipped with a vision system for locating a
part that the robot must grasp. In these cases, the inverse kinematics problem must be
solved to obtain the corresponding joint angles. The precision with which the computed
points can be attained illustrates the accuracy characteristic of the robot. The accuracy of
the robot is lower bounded by the repeatability, and depends on how accurately the robot
parameters are estimated and how precisely the inverse kinematics problem is solved.

4-12