Lecture
FOUR
Solution to Arm Equation: Inverse Kinematics
4.1 Inverse Kinematics
In the previous chapter we developed a procedure of the arm equation for given joint
variables, i.e. angles. The solution to the arm equation is important, because manipulation
tasks are formulated in terms of the desired position and orientation of the tool. The joint
variables have direct relationship with the tool configuration. For tasks like assembly, welding
and sealing operations, the tool requires following a straight-line path or at least approximated
closely. The movement of the arm is, in general, formulated in terms of position and
orientation of the tool. It is then necessary to determine appropriate values for the joint
variables to properly configure the tool. This problem is the inverse kinematic problem. That
is, finding the joint variables in terms of tool position and orientation is inverse kinematic
problem. In general, the problem of inverse kinematics can be stated as - given a 4 4
homogeneous transformation matrix as equation, i.e., the desired position and orientation of
the robot’s tool (see chapter three equation 3.43), find the joint variables which satisfy the
robot arm equation, i.e. what must be the angles of the robot arm.
i
R
0

Direct
Kinematics
Equations
p
1 
Tool
configuration
space
Joint space
Rn
i
Inverse
Kinematics
Equations
R
0

p
1 
Figure 4.1: Relationship between direct and inverse kinematics.
The general problem of inverse kinematics can be stated as follows. Given a
4 4homogeneous transformation in tool configuration space,
R
H 
0
find (one or all) solutions of the equation
1
p
1 
(4.1)
Where
T0n 1 ,  2 , ,  n   H
(4.2)
T0n 1 ,  2 , ,  n   A01 1 , , Ann1  n 
(4.3)
H represents the desired position and orientation of the end-effector. The task is to find the
values for the joint variables 1 ,  2 ,  ,  n  such that T0n 1 ,  2 , ,  n   H . Equation (4.2)
results in twelve nonlinear equations in n unknown variables, which can be written as
Tij 1,2 ,,n   hij with i  1,2,3 and j  1,2,3,4
(4.4)
where Tij and hij refer to the twelve nontrivial entries of T0n and H , respectively. Since the
4th row of both T0n and H are 0 0 0 1 , four of the sixteen equations represented by (4.2)
are trivial. One can assume that multiple solutions exit if n  12 . For example, Figure 4.2
shows different solutions in joint variable space for the same tool configuration.
(a) Left above arm
(c) Right above arm
(d) Right below arm
(b) Left below arm
Figure 4.2: Different joint variables for the same tool configuration.
This is the case for most robot arms. Therefore, we are looking for efficient and systematic
techniques that exploit the particular kinematic structure of the manipulator. Whereas the
forward kinematics problem always has a unique solution that can be obtained simply by
evaluating the forward equations, the inverse kinematics problem may or may not have a
solution. Even if a solution exists, it may or may not be unique. In solving the inverse
kinematics problem we are interested in finding a closed form solution of the equations, i.e.
looking for an explicit relationship as follows:
k  f k h11, h12 ,, h34 
2
(4.5)
The closed form solutions are preferable for two reasons. First, in certain applications, such as
tracking an object where the location is provided by a vision system, solutions to the inverse
kinematic problem are required at a very fast rate, e.g., every 20 milliseconds. Second, the
kinematic equations in general have multiple solutions, which require a quick decision for
choosing a particular solution among several. The closed form solutions help making such
decisions. The practical question of the existence of solutions to the inverse kinematics
problem depends on engineering as well as mathematical considerations. For example, the
motion of the revolute joints may be restricted to less than 360 degrees of rotation. Therefore,
all mathematical solutions of the kinematic equations will not correspond to physical
configurations of the manipulator. We will assume that the given position and orientation are
such that at least one solution of (4.2) exists. We will also assume that the given
homogeneous matrix H in (4.2) corresponds to a configuration within the manipulator’s
workspace.
4.2 Kinematic Decoupling
Although the general problem of inverse kinematics is quite difficult, it turns out that most of
the commercial robots have either one of the following sufficient conditions which make the
closed form solutions possible:


Three adjacent joint axes intersecting, and
Three adjacent joint axes parallel to one another
The general strategy for solving the inverse kinematics problem simplifies somewhat when
the robot has spherical wrist. To see this, consider the case of n-axis robot, where 4  n  6 .
Suppose the last axis is a tool roll axis, and the robot has spherical wrist, which means that the
n  3 axes at the end of the arm all intersect at a point. For this class of robots, the inverse
kinematics problem can be decoupled into two smaller sub-problems:


Inverse position kinematics, and
Inverse orientation kinematics
Decoupling of the six-joint PUMA robot arm is shown in Figure 4.3, where the last three
joints intersecting at a point. To put it another way, for a six-DOF manipulator with a
spherical wrist, the inverse kinematics problem may be separated into two simpler problems,
namely first finding the position of the intersection of the wrist axes (called the wrist centre),
and then finding the orientation of the wrist. For concreteness let us suppose that there is
exactly six-DOF that the last three joint axes intersect at a point p 0c . We express (4.2) as two
sets of equations representing the rotational and positional equations
R06 1,2 ,,6   R
p06 1 ,  2 , ,  6   p
3
(4.6)
(4.7)
Orientation
kinematics
Position
kinematics
Figure 4.3: PUMA arm split into position and orientation kinematics.
Where p and R are the desired position and orientation of the tool frame, expressed with
respect to the world coordinate system. Thus, we are given p and R , and the inverse
kinematics problem is to solve for 1, 2 ,,6  . The assumption of a spherical wrist means
that the axes z3, z4, and z5 intersect at p 0c and hence the origins o4 and o5 assigned by the DHconvention will always be at the wrist centre p 0c . Often o3 will also be at p 0c , but this is not
necessary for our subsequent development. The important point of this assumption for the
inverse kinematics is that motion of the final three links about these axes will not change the
position of p 0c , and thus, the position of the wrist centre is thus a function of only the first
three joint variables.
Figure 4.4: Six-axis PUMA robot arm.
4
The origin of the tool frame (whose desired coordinates are given by p ) is simply obtained by
a translation of distance d6 along z5 from p 0c (see Table 4.1). In our case, z5 and z6 are the
same axis, and the third column of R expresses the direction of z6 with respect to the base
frame. The idea of kinematic decoupling is illustrated in Figure 4.5. Therefore, we have
p
Link
4
5
6
p 0c
0 
 d 6 R 0
1
(4.8)
Table 4.1: Link parameters
 i 
d i 
ai 
4
5
6
0
0
d6
 i 
 90 
0
0
0
90 
0
Figure 4.5: Kinematic decoupling
Thus in order to have the end-effector of the robot at the point with coordinates given by
p and with the orientation of the end-effector given by R  rij , it is necessary and sufficient
 
that the wrist centre p 0c have coordinates given by
p 0c
0
 p  d 6 R 0
1
(4.9)
and that the orientation of the frame o6x6y6z6 with respect to the base be given by R . If the
components of the end-effector position p are denoted {px, py, pz} and the components of the
wrist centre p 0c are denoted { x c , y c , z c } then (4.9) gives the relationship
5
 x c   p x  d 6 r13 
y   p  d r 
6 23 
 c  y
 z c   p z  d 6 r33 
(4.10)
Using Equation (4.10) we may find the values of the first three joint variables. This
determines the orientation transformation R03 which depends only on these first three joint
variables. We can now determine the orientation of the end-effector relative to the frame
o3x3y3z3 from the expression
R  R03 R36
as
 
R36  R03
1
(4.11)
 
R  R03
T
R
(4.12)
As we will see in a later Section, the final three joint angles can then be found as a set of
Euler angles corresponding to R36 . Note that the right hand side of (4.12) is completely known
since R is given and R03 can be calculated once the first three joint variables are known.
4.3 Inverse Position
For the common kinematic arrangements that we consider, we can use a geometric approach
to find the variables, 1 ,  2 ,  3  corresponding to p 0c given by (4.9). We restrict our
calculation to the geometric approach for two reasons. First, as we have said, most present day
manipulator designs are kinematically simple, usually consisting of one of the basic
configurations described in Chapter 2. Indeed, it is partly due to the difficulty of the general
inverse kinematics problem that manipulator designs have evolved to their present state.
Second, there are few techniques that can handle the general inverse kinematics problem for
arbitrary configurations. In general, the complexity of the inverse kinematics problem
increases with the number of nonzero link parameters. For most manipulators, many of the

a i , d i are zero, the  i are 0 or  , etc. In these cases especially, a geometric approach is the
2
simplest and most suitable. Consider the elbow manipulator shown in Figure 4.6, with the
components of p 0c denoted by {xc, yc, zc} and projected onto the {x0 y0} plane so that the three
joint angles 1 ,  2 ,  3  can be shown graphically.
6
Figure 4.6: The first three-axes of the elbow manipulator.
Now, from the projection in Figure 4.6 we can find
1  A tan( x c , y c )
(4.13)
A tan( x, y) denotes the two argument arctangent function for all x, y   0,0  and equals the
unique angle  such that cos   x  x 2  y 2  and sin   y  x 2  y 2  . For example,





3
. A valid second solution for  1 is
A tan(1,1)   , while A tan(1,1)  
4
4
1    A tan( x c , y c )
(4.14)
It is to be noted carefully here that in the case for any x, y   0,0  , A tan( x, y) in (4.13) and
(4.14) are undefined and the manipulator is in a singular configuration. One such singularity
situation is shown in Figure 4.7(a). A shoulder offset d  0 , as shown in Figure 4.7(b), can
avoid such singularity, i.e., wrist centre does not intersect with z0. This corresponds to leftarm and right-arm configuration, which lead to two solutions for  1 .
To find the angles  2 ,  3  for the elbow manipulator, given  1 , we consider the plane formed
by the second and third links as shown in Figure 4.8. Since the motion of links two and three
is planar, the solution is analogous to that of the two-link manipulator. We can apply the law
of cosines to obtain
r 2  s 2  a 22  a 32 x c2  y c2  d 2  z c2  a 22  a 32
cos  3 

D
(4.15)
2a 2 a 3
2a 2 a 3
7
Z0
d 0
(a) Singular configuration
(b) Shoulder offset
Figure 4.7: Singularity and offset in robot arm configuration.
Since r 2  xc2  y c2  d 2 and s  z c , so we can find  3 using the equation (4.16)
Figure 4.8: Projection onto plane formed by links {2,3}
 3  A tan D, 1  D 2 


(4.16)
Similarly,  2 is given by
 2  A tanr , s   A tana 2  a3 c3 , a3 s3 
(4.17)
 A tan xc2  y c2  d 2 , z c   A tana 2  a3 c3 , a3 s3 


8
The two solutions for  3 correspond to the elbow-up and elbow-down position respectively.
Elbow-up and elbow-down positions are shown in Figure 4.9.
(a) Elbow-up
(b) Elbow-down
Figure 4.9: Elbow-up and elbow-down positions.
4.3 Inverse Orientation
A geometric approach was used to solve the inverse position problem in section 4.2. This
gives the values of the first three joint variables 1 ,  2 ,  3  corresponding to a given position
of the wrist origin. The inverse orientation problem is now one of finding the values of the
final three joint variables corresponding to a given orientation with respect to the frame
(x3y3z3). For a spherical wrist, this can be interpreted as the problem of finding the set of
rotation angles  about x-axis,  about y-axis and  about z-axis corresponding to a given
rotation matrix R , which we discussed in lecture 3. Recall that the matrix R36 in equation
(4.12) shows that the rotation matrix obtained for the spherical wrist has the same form as the
rotation matrix R in (3.37)-(3.39) (See Lecture 3 Section 3.2.8). Therefore, we solve for the
three rotation angles,  ,  ,  using equations (3.6)-(3.9) (see Lecture 3) and then use the
mapping  4   ,  5   , and  6   .
The DH parameters for the frame assignment shown in Figure 4.6 are summarized in Table
4.2. Multiplying the corresponding Ai matrices gives the matrix R03 for the articulated or elbow
manipulator as
R03
C1C 23
  S1C 23
 S 23
 C1 S 23
 S1 S 23
C 23
The matrix R36  A4 A5 A6 is given by
9
S1 
 C1 
0 
(4.18)
R36
C 4 C 5 C 6  S 4 S 6
  S 4 C 5 C 6  C 4 S 6

 S 5C6
 C 4 C5 S 6  S 4 C6
C4 S5 
S 4 S 5 
C 5 
 S 4 C5 S 6  C 4 C6
S5 S6
Table 4.2: Link parameters
 i 
d i 
ai 
Link
1
2
3
1
2
3
(4.19)
 i 
d1
0
0
a2
90 
0
0
a3
0
Using equation (4.12), (4.18) and (4.19), we get
 
R36  R03
T
R
 C1C 23

   S1C 23
 S
  23
C 4 C 5 C 6  S 4 S 6
S C C  C S
4 6
 4 5 6

 S 5C6
 C 4 C5 S 6  S 4 C6
 S 4 C5 S 6  C 4 C6
S5 S6
 C1 S 23
 S1 S 23
C 23
S1  

 C1  
0  
T
C 4 S 5   C1C 23
S 4 S 5    C1 S 23
C 5   S1
 r11
r
 21
r31
r12
r22
r32
S1C 23
 S1 S 23
 C1
r13 
r23 
r33 
S 23   r11
C 23  r21
0  r31
(4.20)
r12
r22
r32
r13 
r23  (4.21)
r33 
The rotation angles can be solved only by equating the three equations given by the third
column in the LHS matrix in (4.21). The equations are given by
C 4 S 5  C1C 23 r13  S1C 23 r23  S 23 r33
S 4 S5  C1S 23r13  S1S 23r23  C23r33
C 5  S1 r13  C1 r23
(4.22)
(4.23)
(4.24)
Hence, if not both of the expressions (4.22), (4.23) are zero, then we obtain  5 from the
relation below.
(4.25)
 5  A tan S1 r13  C1 r23 , 1  ( S1 r13  C1 r23 ) 2 


If the positive square root is chosen in (4.25), then  4 and  6 are given by the equations
(4.26) and (4.27) respectively.
 4  A tan C1C 23 r13  S1C 23 r23  S 23 r33 ,C1 S 23 r13  S1 S 23 r23  C 23 r33 
 6  A tan S1 r11  C1 r21 , S1 r12  C1 r22 
10
(4.26)
(4.27)
The other solutions are obtained analogously. If S 5  0 , then joint axes z3 and z5 are collinear.
This is a singular configuration and only the sum of  4   6 can be determined. One solution
is to choose  4 arbitrarily and then determine  6 from the sum.
Example 4.1: SCARA Manipulator
We consider the SCARA manipulator, shown in Figure 4.10, whose forward kinematics is
defined by T04 as given in (4.28). The parameters of the SCARA arm are given in Table 4.3.
The inverse kinematics is then given as the set of solutions of the equation
C12 C 4  S12 S 4
S C  C S
12 4
 12 4

0

0

S12 C 4  C12 S 4
 C12 C 4  S12 S 4
0
0
a1C1  a 2 C12 
0 a1 S1  a 2 S12   R

1
 d 3  d 4   0

0
1

0
p
(4.28)
1 
Figure 4.10: SCARA manipulator. Joint distance  3  d 3 is variable in prismatic joint.
We first note that, since the SCARA has only four DOF, not every possible H allows a
solution of (4.28). In fact we can easily see that there is no solution of (4.28) unless R is of
the form
C
R   S 
 0
S
 C
0
11
0
0 
 1
(4.29)
Table 4.3: Kinematic parameters of 4-axis SCARA arm
 i 
d i 
ai 
 i 
Link

1
d1=fixed
a1
1
3
2
0
4
4
2
0
a2
0
3  d3
d4=fixed
0
0
0
0
If this is the case, then the sum  1   2   4 is determined by
1   2   4    A tan r11 , r12 
(4.30)
Projecting the manipulator configuration onto the x0−y0 plane immediately yields the situation
in Figure 4.11.
Figure 4.11: Projection onto x0−y0 plane.
We see from this that

 2  A tan C 2 , 1  C 2
Where C 2 
o x2  o 2y  a12  a 22
2a1 a 2

(4.31)
.
1  A tano x , o y   A tana1  a 2 C 2 , a 2 S 2 
(4.32)
We may then determine  4 from (4.30) as
 4  1   2  
 1   2  A tanr11 , r12 
12
(4.33)
Finally d 3 is given as
d3  oz  d 4
(4.34)
References
J.J. Craig (2005). Introduction to Robotics- Mechanics and Control, 3rd Edition, Pearson
Prentice Hall, Upper Saddle, NJ.
K. S. Fu; R. C. Gonzalez; C. S. G. Lee, (1987). Robotics: Control, Sensing, Vision, and
Intelligence, McGraw-Hill Inc., International Edition.
Y. Korem (1985). Robotics for Engineers, McGraw-Hill Book Company, New York.
C. S. G. Lee, R. C. Gonzalez; K. S. Fu, (1983). Tutorial on Robotics, IEEE Computer Society
Press.
D. McCloy and D.M. J. Harris (1986). Robotics: An Introduction, Open University Press,
Milton Keynes.
R. M. Murray, Z. Li and S.S. Sastry (1994). A Mathematical Introduction to Robotic
Manipulator, CRC Press.
R.P. Paul (1982). Robot Manipulators: Mathematics, Programming and Control, The MIT
Press, Cambridge.
R. J. Schilling (1990). Fundamentals of Robotics: Analysis and Control, Prentice Hall,
Englewood Cliffs, New Jersey.
W. E. Snyder (1985). Industrial Robots: Computer Interfacing and Control, Prentice Hall Inc.,
Englewood Cliffs, New Jersey.
M. W. Spong; M. Vidyasagar (1989). Robot Dynamics and Control, John Wiley & Sons; New
York, Chichester, Brisbane, Toronto, Singapore.
F. L. Lewis; C. T. Abdallah; D. M. Dawson (1993). Control of Robot Manipulators,
MacMillan Publishing Company.
M. Xie (2003). Fundamentals of Robotics: Linking Perceptions to Action, World Scientific,
New Jersey, London, Singapore, Hong Kong.
13