Chapter 5

Inverse Kinematics for Position

The problem of inverse kinematics is to nd the joint angles, given the end effector position and orientation.
In general, inverse kinematics is much harder than forward kinematics. Sometimes no analytical solution
is possible, and an iterative search is required. Even with analytical solutions possible, multiple solutions
arise from which one must pick. In the case of redundant manipulators, there are innitely many solutions
from which to choose. Another complication is that workspace limits may be violated (the point is outside
the reach of the manipulator, or joint limits are exceeded).
5.1 Two-link Planar Manipulator
The simplest non-trivial manipulator for which to study inverse kinematics is the planar two-link manipu-
lator (Figure 5.1). Moreover, parts of some spatial manipulators have this structure as a subchain, and the
following solution applies to the analysis of those manipulators inverse kinematics as we shall see later.
First solve for
2
using Figure 5.1(A). From the cosine rule,
x
2
+y
2
= a
2
1
+a
2
2
2a
1
a
2
cos(
2
)
cos
2
=
x
2
+y
2
a
2
1
a
2
2
2a
1
a
2
(5.1)
As usual, we should try to avoid using the arccos function because of inaccuracy. Instead, the following
half-angle formula is employed:
tan
2

2
2
=
1 cos
2
1 + cos
2
=
2a
1
a
2
x
2
y
2
+a
2
1
+a
2
2
2a
1
a
2
+x
2
+y
2
a
2
1
a
2
2
=
(a
1
+a
2
)
2
(x
2
+y
2
)
(x
2
+y
2
) (a
1
a
2
)
2
(5.2)
The second joint angle
2
is then found accurately as:

2
= 2 tan
1

(a
1
+a
2
)
2
(x
2
+y
2
)
(x
2
+y
2
) (a
1
a
2
)
2
(5.3)
Although there are two solutions from the use of tan
1
, these solutions are separated by , and subsequent
multiplication by 2 means that the two solutions are the same. Two solutions do however result because of
the square root. They are called elbow up and elbow down (Figure 5.1). We pick one of the two solutions to
proceed.
c John M. Hollerbach, March 27, 2008
1
2 CHAPTER 5. INVERSE KINEMATICS FOR POSITION
2

(x,y)
.
0
O
a

1
a
1

2
0
0
y
x
2
a
(x,y)
.
0
a
2

1
O
0
0
y
x
(A) (B)
Figure 5.1: Inverse kinematics for two-link planar manipulator. Elbow down (A) and elbow up (B) solutions.
Next we will nd
1
, which is determined uniquely given
2
. From Figure 5.1 we get the relation:

1
= (5.4)
where is the angle between the radial line to the endpoint and the x axis, and is the angle between link
1 and the radial line. These two angles are found uniquely from
= atan2(y, x) (5.5)
= atan2(a
2
sin
2
, a
1
+a
2
cos
2
) (5.6)
Note the two different solutions for
1
corresponding to the elbow-down versus elbow-up congurations
(Figure 5.1).
5.1.1 Alternate Solution
Two arctangent functions are involved to nd
1
in (5.4). There is an alternate solution based on Gaussian
elimination that requires only one arctangent evaluation, which may be more efcient on a computer. The
forward kinematics can be written by inspection or by referring to Chapter 2:
x = a
1
c
1
+a
2
c(
1
+
2
) (5.7)
y = a
1
s
1
+a
2
s(
1
+
2
) (5.8)
Take two different linear combinations of these equations to produce after simplication:
xc
1
+ys
1
= a
1
+a
2
c
2
(5.9)
xs
1
yc
1
= a
2
s
2
(5.10)
There are now two equations to solve for s
1
and c
1
separately using Gaussian elimination.
c
1
=
x(a
1
+a
2
c
2
) +y(a
2
s
2
)
x
2
+y
2
(5.11)
s
1
=
y(a
1
+a
2
c
2
) x(a
2
s
2
)
x
2
+y
2
(5.12)
The 4-quadrant arctangent is then applied only once to nd
1
.
5.2. SPATIAL 3-DOF MANIPULATORS 3
5.2 Spatial 3-DOF Manipulators
Manipulators can often be considered to be composed of two parts. The rst 3 joints forma regional structure
whose primary purpose is to position the wrist in space. The last 3 joints form the orienting structure whose
purpose is to orient the hand or grasped object. In this section, we will solve the inverse kinematics of the
rst three degrees of freedom of the elbow robot of the previous chapter, assuming that the wrist position is
given. A planar two-link manipulator often makes up the last two links of the regional structure, and it will
be seen that the solution of the previous section can be directly applied.
3
a
2
z
2
O
0
O
0
O
2
x
3
4
4
1
x
4
z
O
z
d
4
z
2
x
3 1
O x
z
x
d
1
0
1
i a
i
d
i

i
1 0 d
1
/2
2 a
2
0 0
3 0 0 /2
4 0 d
4
/2
Figure 5.2: First four joints of the elbow robot.
5.2.1 The Elbow Robot Regional Structure
The DH parameters for the rst 4 links are shown in Figure 5.2. The link 4 parameters have been included
in the regional structure because they locate the wrist point O
4
, which we assume is known relative to O
0
,
i.e.,
0
d
04
= O
4
O
0
. Although z
3
and z
4
are rotation axes, they have no inuence on the wrist position O
4
because they pass through that point. For the purposes of this section, they can be considered inactive.
Project the position of the wrist point into the x
0
, y
0
plane, i.e., just select the x, y coordinates of
0
d
04
(Figure 5.3(A)). The radial line from O
0
to this projected point is parallel to x
1
. Hence the angle between
this radial line and x
0
is
1
:

1
= tan
1
_
0
d
04y
0
d
04x
_
(5.13)
Because of the arctangent function, there are two possible solutions for
1
. These correspond to lefty and
righty congurations: starting from one of these congurations, the base rotates 180 degrees while the
upper arm rotates over the top to achieve the same wrist position. In Figure 5.3(A), this corresponds to the
x
1
axis pointing in the other direction; the two
1
solutions differ by . For manipulators such as the PUMA
which have an offset in the shoulder, the result is the left versus right arm look; this case is treated below.
Next, nd the vector
1
d
14
from coordinate origin 1 to the wrist. We can express the result with respect
to the orientation of frame 1 because we now know
1
.
1
d
14
=
1
d
04

1
d
01
=
0
R
T
1
(
0
d
04

0
d
01
) (5.14)
4 CHAPTER 5. INVERSE KINEMATICS FOR POSITION
O
1
1
04
z
d
0

1
x
0
04x 04y
y
1
(d ,d ,0)
O
4
4
2
a
O
d
0
0
1
x
y
z
4
14
1
4
d
d
x
2
z
3
2
z
3

O
2
O
O
a
2
3
2
x
1
x

1
y
1
z
(A) (B)
Figure 5.3: (A) Solution for
1
. (B) Solution for
2
and
3
.
where
0
R
1
=
_

_
c
1
s
1
c
1
s
1
s
1
s
1
c
1
c
1
c
1
s
1
0 s
1
c
1
_

_ =
_

_
c
1
0 s
1
s
1
0 c
1
0 1 0
_

_ (5.15)
since
1
= /2. Hence
1
d
14
=
_

_
c
1
s
1
0
0 0 1
s
1
c
1
0
_

_
_
_
_
_

_
0
d
04x
0
d
04y
0
d
04z
_

_ d
1
_

_
0
0
1
_

_
_
_
_ =
_

_
0
d
04x
c
1
+
0
d
04y
s
1
0
d
04z
d
1
0
d
04x
s
1

0
d
04y
c
1
_

_
(5.16)
The upper arm and forearm lie in the x
1
, y
1
plane. Hence we may apply the results for the inverse
kinematics of a planar 2-link manipulator, taking into account the difference in zero-angle position. Call

3
the solution for the elbow angle of the planar three-link manipulator:

3
= 2 tan
1

(a
2
+d
4
)
2
(x
2
1
+y
2
1
)
(x
2
1
+y
2
1
) (a
2
d
4
)
2
(5.17)
where in (5.3) we have substituted the upper arm length a
2
and the forearm length d
4
, and the abbreviation
1
d
14
= (x
1
, y
1
, z
1
) is used. When

3
= 0, the planar manipulators elbow is straight, but when
3
= 0 of
the present manipulator, the elbow is bent at 90
o
. This means that

3
=

3
/2 (5.18)
To nd
2
, we use formulas similar to (5.4)-(5.6) after appropriate substitutions:
= atan2(y
1
, x
1
) (5.19)
5.2. SPATIAL 3-DOF MANIPULATORS 5
= atan2(d
4
sin

3
, a
2
+d
4
cos

3
) (5.20)

2
= (5.21)
Note the use of

3
in order to apply to previous formulas.
2 3
x
4
a
O
1
2
z
x
0
O
2
4
d
4
z
1
O
2
d
z
4
z
3
d
O
x x
2 3
1
x
1
O
0
z
0
i a
i
d
i

i
1 0 d
1
/2
2 a
2
d
2
0
3 0 0 /2
4 0 d
4
/2
Figure 5.4: First four joints of the shoulder-elbow robot.
5.2.2 An Elbow Robot with a Shoulder
As mentioned above, some robots appear to have a shoulder, in that the plane of the upper arm and forearm
does not intersect the rst rotation axis. The robot of Figure 5.4 is derived from the previous robot by adding
an offset d
2
, which makes it look like a left arm. From a workspace standpoint, the advantage is that the
upper arm will not hit the base when it is straight down.
The shoulder offset makes the calculation of
1
more complex, because the x
1
axis is no longer in the
vertical plane of the upper arm and forearm (Figure 5.5(A)). When x
1
and the wrist point are projected onto
the x
0
, y
0
plane, then the angle
1
from x
0
to x
1
is no longer the same as the angle from x
0
to the projected
wrist point. The projection on the x
0
, y
0
plane is redrawn in Figure 5.5(B) for clarity. Suppose the wrist
point is (x, y), is the angle to the radial line of length r =
_
x
2
+y
2
to the wrist point, and is as shown.
Then

1
= + (/2 ) (5.22)
where
= atan2(y, x) (5.23)
cos = d
2
/r (5.24)
Similar to (5.2), we compute from
tan
2

2
=
1 cos
1 + cos
=
r d
2
r +d
2
(5.25)
There are two solutions for and hence for
1
, which more transparently correspond to lefty and righty.
6 CHAPTER 5. INVERSE KINEMATICS FOR POSITION
x
1
0
y
1
1
x
0
0
O
0

z
04
d
d
4
O
1
4
1
d
2
a
2
O
z
(d ,d ,0)
04x 04y
y
0
x
r
2
O
d

/2

1
0
.
(x,y)
0
y
(A) (B)
Figure 5.5: (A) A shoulder causes the upper arm/forearm vertical plane not to intersect the origin O
0
. (B)
Projection of the arm and shoulder onto the x
0
, y
0
plane.
To nd
2
and
3
, we again reduce the problem to that of the planar two-link manipulator by nding the
wrist position relative to the shoulder:
1
d
14
=
0
R
T
1
_
0
d
04
d
1
0
z
0
_
(5.26)
Choose the (x, y) components of
1
d
14
to solve for
2
,
3
as in the previous section. The shoulder offset d
2
,
which is along the z
1
axis, does not affect these components.
5.2.3 Projection Approach
For diverse 3-DOF spatial manipulators, other approaches than shadow lines or boxes might be necessary.
One approach is to discern if there are particular directions x
0
, y
0
, or z
0
affected by only one of the joints.
Consider the polar robot with attached elbow in of Figure 5.6. The y
0
axis is in the same direction as z
1
in
the zero-angle position. The table below indicates by an X the inuence of each of the variables
1
, d
2
,
3
on the (x,y,z) coordinates of the endpoint position
0
d
03
.
Variable x y z

1
X X
d
2
X X

3
X X X
In deciding on the inuence, the manipulator has to be moved out of its zero-angle position. Thus when
z
1
is slanted, its displacement d
2
affects both the x, y coordinates of
0
d
03
. Joint angle
3
affects all three
5.2. SPATIAL 3-DOF MANIPULATORS 7
x
z
O
0
0 1
x
0
O
1
O
2
2
z
2
x
O
3
x
3
z
d
z
1
2
a
3
3
i a
i
d
i

i

i
1 0 0

2

2 0

2

2
3 a
3
0 0
Figure 5.6: A polar robot with an elbow.
coordinates in general position. The table clearly shows that
3
is the only joint variable affecting the z
position. Therefore it can be found in isolation rst.
To do so, consider the general expression for the endpoint position
0
d
0n
of an n-link robot derived from
Chapter 4.
0
d
0n
=
n

i=1
_
d
i
0
z
i1
+a
i
0
x
i
_
(5.27)
In this case, n = 3 and some of the DH parameters are zero. The result is:
0
d
03
= d
2
0
z
1
+a
3
0
x
3
(5.28)
Project this equation onto
0
z
0
to solve for
3
:
0
d
03

0
z
0
= d
2
0
z
1

0
z
0
+a
3
0
x
3

0
z
0
= a
3
0
x
3

0
z
0
Axes z
0
and x
3
are separated by two coordinate systems, but by noting that z
0
= x
2
the dot product can be
evaluated.
0
d
03

0
z
0
= a
3
c
3
(5.29)

3
is found using the half-angle tangent formula 5.2.
There are different ways of nding the remain two joint variables. Perhaps the simplest way is to draw
the triangle formed by d
1
, a
3
, and the radial line r =
_
x
2
+y
2
+z
2
in the y
1
, z
1
plane. Figure 5.7(A)
shows the robot in the zero-angle position (obtained by rotating Figure 5.6 about the z
1
axis by /2).
When
3
changes, the triangle is altered as in Figure 5.7(B). The Pythagorean theorem can be used to nd
d
2
.
r
2
= (a
3
c
3
)
2
+ (d
2
+a
3
s
3
)
2
8 CHAPTER 5. INVERSE KINEMATICS FOR POSITION
z
1
z
2
d
2
x
2
2
O
O
3
x
3
z
3
3
a
y
1
r
O
1
z
1
d
2
y
1
O
1
O
3
r
a
3

3
O
2
(A) (B)
Figure 5.7: (A) The polar elbow robot in zero angle position seen in coordinate system 1. (B) The triangle
perturbed with non-zero angle
3
.
d
2
= a
3
s
3
+
_
r
2
a
3
c
3
= a
3
s
3
+
_
x
2
+y
2
(5.30)
where the positive square root has been taken and the term inside the square root has been simplied to
reect just the x, y coordinates of the end point.
There is no simple way to nd
1
. Either one has to project (5.28) onto the x
0
axis or the y
0
axis, then
do some rotations to evaluate the dot products. Suppose we project with y
0
.
0
d
03

0
y
0
= d
2
0
z
1

0
y
0
+a
3
0
x
3

0
y
0
= d
2
c
1
+a
3
0
x
3

0
y
0
where y
0
and z
1
are aligned when
1
= 0. There is no simple relation between x
3
and y
0
, and doing some
form of rotation is inevitable. One approach that avoids using rotation matrices is to express x
3
in terms of
axes 2 and y
0
in terms of axes 1.
0
x
3
=
0
x
2
c
3
+
0
y
3
s
3
0
y
0
=
0
x
1
s
1
+
0
z
1
c
1
Whether a particular axis is multiplied by the sine or the cosine can be determined by two values of the
rotation angle, 0 and /2. Next, because axis 2 is a prismatic joint, there is a simple relation between axes
1 and 2: z
1
= y
2
and x
1
= z
2
. Substituting and taking the dot product:
0
x
3

0
y
0
= (
0
x
2
c
3
+
0
y
2
s
3
) (
0
z
2
s
1
+
0
y
2
c
1
)
= (
0
x
2
c
3
+
0
y
2
s
3
) (
0
z
2
s
1
+
0
y
2
c
1
)
= s
3
c
1
5.3. SPHERICAL WRIST 9
d
3
O
5
z
O O
3
z
4
6
O
6
5 4
x
6
4
x
4
5
3
x
z
z
x

4

4
z
x
5
z

6
x
6

6
5
5

4
5
z
4
x
3
z
3
x
i a
i
d
i

i

i
4 0 d
4
/2
4
5 0 0 /2
5
6 0 0 0
6
(A) (B)
Figure 5.8: (A) Spherical joint in zero position. (B) Spherical joint after displacements in all angles.
Once again, nd
1
using the half-angle tangent formula (5.2).
5.3 Spherical Wrist
The spherical wrist on manipulators primarily serves to orient the last link. When considered by itself, a
spherical wrist is called a spherical manipulator or mechanism. Consider again the spherical joint from the
previous chapter (Figure 5.8). The numbering of the rst joint begins with 4, because as will be seen later
the inverse kinematics of the 6 degree-of-freedom elbow robot can be obtained merely by juxtaposing the
solutions for the spatial 3R manipulator and the spherical manipulator.
Suppose that the orientation
3
R
6
of frame 6 relative to frame 3 is given, and we have to nd the three
joint angles
4
,
5
,
6
to generate
3
R
6
. The offset d
4
of the wrist relative to O
3
does not inuence this
orientation problem. The solution is more elaborate and less intuitive than the positioning problem for
the spatial 3R manipulator, and proceeds through matrix manipulation and simultaneous equations. For
simplicity, let r
ij
represent an element of matrix
3
R
6
. From the denition of
3
R
6
,
3
R
6
=
3
R
4
4
R
5
5
R
6
(5.31)
Rather than multiply out the right side, a trick is to solve a simpler problem by solving for
4
from
3
R
6
5
R
T
6
=
3
R
4
4
R
5
(5.32)
This trick separates out
6
, as can be seen when each side is expanded below.
3
R
4
4
R
5
=
_

_
c
4
0 s
4
s
4
0 c
4
0 1 0
_

_
_

_
c
5
0 s
5
s
5
0 c
5
0 1 0
_

_ =
_

_
.
.
.
.
.
. c
4
s
5
.
.
.
.
.
. s
4
s
5
.
.
.
.
.
. c
5
_

_
(5.33)
10 CHAPTER 5. INVERSE KINEMATICS FOR POSITION
where we only care about the last column. Then
3
R
6
5
R
T
6
=
_

_
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
_

_
_

_
c
6
s
6
0
s
6
c
6
0
0 0 1
_

_ =
_

_
.
.
.
.
.
. r
13
.
.
.
.
.
. r
23
.
.
.
.
.
. r
33
_

_
(5.34)
where again we only care about the last column. Equating corresponding elements,

4
= tan
1
_
r
23
r
13
_
= tan
1
_
s
4
s
5
c
4
s
5
_
(5.35)
There are two possible solutions for
4
.
However,
5
and
6
are determined uniquely as evidenced by the use of the 4-quadrant artangent function
below. Again from the denition of
3
R
6
,
(
3
R
4
)
T 3
R
6
=
4
R
5
5
R
6
(5.36)
where
4
R
5
5
R
6
=
_

_
c
5
0 s
5
s
5
0 c
5
0 1 0
_

_
_

_
c
6
s
6
0
s
6
c
6
0
0 0 1
_

_ =
_

_
.
.
.
.
.
. s
5
.
.
.
.
.
. c
5
s
6
c
6
0
_

_
(5.37)
(
3
R
4
)
T 3
R
6
=
_

_
c
4
s
4
0
0 0 1
s
4
c
4
0
_

_
_

_
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
_

_
=
_

_
.
.
.
.
.
. r
13
c
4
+r
23
s
4
.
.
.
.
.
. r
33
r
11
s
4
r
21
c
4
r
12
s
4
r
22
c
4
r
13
s
4
r
23
c
4
_

_
(5.38)
Hence equating corresponding elements,
s
5
= r
13
c
4
r
23
s
4
c
5
= r
33
(5.39)

5
= atan2(s
5
, c
5
)
s
6
= r
11
s
4
+r
21
c
4
c
6
= r
12
s
4
+r
22
c
4
(5.40)

6
= atan2(s
6
, c
6
)
When sin
5
= 0 in (5.35), there is a degeneracy called the wrist singularity. At this singularity, the
z
3
and z
5
rotation axes align (Figure 5.8(A)), resulting in a loss of one degree of freedom. Only the linear
combination of
4
and
6
can be found, a situation analogous to the Euler angle degeneracy.
1. If
5
= 0, then
5.4. INVERSE KINEMATICS OF 6-DOF MANIPULATORS 11
3
R
4
4
R
5
5
R
6
=
_

_
c
4
0 s
4
s
4
0 c
4
0 1 0
_

_
_

_
1 0 0
0 0 1
0 1 0
_

_
_

_
c
6
s
6
0
s
6
c
6
0
0 0 1
_

_
=
_

_
c(
4
+
6
) s(
4
+
6
) 0
s(
4
+
6
) c(
4
+
6
) 0
0 0 1
_

_
Then
4
+
6
= atan2(r
21
, r
11
).
2. If
5
= , then
3
R
4
4
R
5
5
R
6
=
_

_
c
4
0 s
4
s
4
0 c
4
0 1 0
_

_
_

_
1 0 0
0 0 1
0 1 0
_

_
_

_
c
6
s
6
0
s
6
c
6
0
0 0 1
_

_
=
_

_
c(
4

6
) s(
4

6
) 0
s(
4

6
) c(
4

6
) 0
0 0 1
_

_
Then
4

6
= atan2(r
21
, r
11
).
One possibility is to arbitrarily set
6
= 0 in these linear combinations.
5.4 Inverse Kinematics of 6-DOF Manipulators
In general, the inverse kinematics of arbitrary 6-DOF rotary manipulators cannot be solved analytically.
Under two special cases, however, an analytic solution is possible.
1. There is a spherical joint anywhere in the chain.
2. There is a planar pair anywhere in the chain.
These joints allow the manipulator to be broken into subsystems which are more easily solved.
The elbowmanipulator has a spherical wrist, and hence its inverse kinematics may be solved analytically.
The DH parameters of the elbow manipulator are repeated in Figure 5.9, where the zero position is also
shown. The last frame 6 has been chosen to be coincident with frame 5 when
6
= 0.
Suppose the robot is grasping an object, which has a tool frame
6
T
tool
relative to frame 6 (Figure
5.10(A)). Then the endpoint location is:
0
T
tool
=
_
_
0
R
tool
0
d
tool
0
T
1
_
_
=
0
T
1
1
T
2

5
T
6
6
T
tool
(5.41)
The position
0
d
tool
and orientation
0
R
tool
of the tool is somehow given to us, for example from a trajectory
planner. Our job is to nd the joint angles that realize this tool pose. The solution to the inverse kinematics
proceeds by 3 steps.
12 CHAPTER 5. INVERSE KINEMATICS FOR POSITION
x
z
z
O O
z
x
O
3
a
2
z
2
x
d
5 6
5 6
z
1
x
4
4
5
4
3
4
O
6
2
z
x
1
O
z
0
O
0
O
3
d
1
0
1
x
2
x
i a
i
d
i

i
1 0 d
1
/2
2 a
2
0 0
3 0 0 /2
4 0 d
4
/2
5 0 0 /2
6 0 0 0
Figure 5.9: Zero position of elbow robot, and associated DH parameterization.
5.4.1 Step 1: Decompose the Manipulator at the Wrist
This is the singly most important step of the procedure. The spherical wrist makes this possible. We nd the
position of the wrist point, which subsequently is used to solve for the rst 3 joint angles. The wrist joint
angles are then solved to give the correct orientation. Let the tool transform be given:
6
T
tool
=
_
_
6
R
tool
6
d
6,tool
0
T
1
_
_
(5.42)
Since
6
d
6,tool
is the vector from the wrist to the tool frame origin, it also locates origins O
4
and O
5
which
are coincident with O
6
. Hence we express the distance from base to wrist as the vector from O
0
to O
4
:
0
d
04
=
0
d
0,tool

0
d
6,tool
=
0
d
0,tool

0
R
tool
(
6
R
tool
)
T 6
d
6,tool
(5.43)
The various inter-frame translation vectors d
ij
are shown in Figure 5.10(B).
5.4.2 Step 2: Find the First 3 Joint Angles
We can now simply apply the solution of Section 5.2.1 to nd
1
,
2
,
3
given
0
d
04
.
5.4.3 Step 3: Find the Last 3 Joint Angles
The rst 3 joint angles impart a certain ortientation in space of the forearm. The wrist angles correct for the
difference between this orientation and the desired orientation of the end link.
0
R
tool
= (
0
R
1
1
R
2
2
R
3
)(
3
R
4
4
R
5
5
R
6
)
6
R
tool
3
R
6
= (
0
R
3
)
T 0
R
tool
6
R
T
tool
(5.44)
5.5. WORKSPACE 13
O
6
x
6
O
tool
tool
6
tool
R
z
6
6,tool
d
tool
tool
6
z
x
y
y
Base
d
01
24
=d
34
d
Shoulder
4,tool
=d
5,tool
=d
d
13
6,tool
d
12
d
d
=d
Wrist
Tool
04
Elbow
0,tool
14
d
(A) (B)
Figure 5.10: (A) Tool frame location relative to frame 6. (B) Inter-frame vectors.
where the elements of
3
R
6
= {r
ij
} are now known. Simply apply the solution for the spherical manipulator
of Section 5.3 to nd
4
,
5
,
6
.
In total, there are 8 = 2 2 2 solutions for the inverse kinematics for the 6R arm: 2 for the shoulder
(leftie/rightie), 2 for the elbow (up and down), and 2 for the wrist.
5.5 Workspace
Workspace is the set of congurations that a manipulator can assume. Workspace depends on the joint limits
and on the presence of obstacles. For the planar 2-link manipulator without joint limits, the workspace is
a hollow disk, regardless of the relative length of a
1
versus a
2
(Figure 5.11(A)). The outer boundary of the
disk has radius a
1
+ a
2
, while the inner boundary has radius |a
1
a
2
|. For a given absolute difference
|a
1
a
2
|, the inner boundary radius holds whether a
1
< a
2
or a
1
> a
2
.
With joint limits (e.g., 0 <
1
< 90
o
and 0 <
2
< 180
o
), only a portion of this hollow disk can be
reached (Figure 5.11(B)).
Consider now a spatial 2-link manipulator, where z
1
z
0
and a
1
> a
2
(Figure 5.12). Rotation of the
endpoint about z
1
generates a circle of radius a
2
. Rotation about axis z
0
sweeps the circle to form a torus
(Figure 5.12(B)); only half of the torus is shown in the gure. The workspace of the endpoint lies on the
surface of the torus.
5.5.1 Holes and Voids
Consider a 3-link spatial manipulator formed by adding a link to the 2-link spatial manipulator above, so
that z
2
z
3
and a
3
= 0 (Figure 5.13). Suppose a
1
> a
2
+a
3
and a
2
> a
3
. Joints 2 and 3 are like the planar
2-link manipulator, and sweep out a hollow disk. Joint 1 sweeps the hollow disk to form a torus (Figure
5.13(B)).
The donut hole is called a hole. The empty space in the interior of the donut is called a void. Voids
are considered much worse than holes, because they are interior to the workspace and pose a difcult for
trajectory planning. Manipulator designers attempt to avoid voids by setting a
2
= a
3
. The hole on the other
hand represents the workspace boundary, and there has to be a workspace boundary someplace.
14 CHAPTER 5. INVERSE KINEMATICS FOR POSITION
| |
2
a
a >a
2
2 1
a -a
1
1
.
a +a
1 2
.
2
a
1
.
a <a
2
a +a
1 2
a a
1
.
180
180
.
90
(A) (B)
Figure 5.11: Workspace of two-link planar manipulator: (A) no joint limits. (B) Joint limits of 0
1
/2
and 0
2
.
The donut hole can be minimized by setting a
1
= a
2
+ a
3
. Alternatively, if a
1
= 0 and a
2
= a
3
, then
the workspace is a perfect sphere. The manipulator in Figure 5.13(A) is the same as the rst 3 joints of the
elbow robot. Commercial elbow robots are typically designed with these criteria to avoid holes and voids.
5.5.2 Total, Primary, and Secondary Workspace
So far, we have only considered position. Consider now the elbow manipulator, where we also have to
consider orientation of the end link as well as position of the endpoint. In Figure 5.14, we have depicted the
last three joints as a ball joint to suggest the spherical joint equivalent (Figure 5.14(A)). The length of the
end link from wrist to endpoint is d
6
.
The total or reachable workspace is the positioning workspace, without regard for orientation. Suppose
a
2
> d
4
> d
6
and a
2
> d
4
+ d
6
. Then the reachable workspace is a hollow sphere with outer radius
a
2
+d
4
+d
6
and inner radius a
2
d
4
d
6
, Figure 5.14(B) shows a cross-section of the workspace sphere
through its center; the total of all shaded areas represents the reachable workspace. The endpoint can achieve
all positions in these shaded areas.
The primary workspace comprises points reachable in all orientations. For the elbow manipulator, the
primary workspace has an outer radius a
2
+ d
4
d
6
and an inner radius a
2
d
4
+ d
6
; in Figure 5.14(B)
the dark-shaded area represents the primary workspace. When the wrist point is located in the primary
workspace, any orientation of the end link may be achieved. Notice how small the primary workspace is.
The secondary workspace is the total workspace minus the primary workspace. The secondary workspace
comprises points that can be reached in only partial orientations. For the elbow manipulator, the secondary
workspace is a hollow sphere with outer radius a
2
+ d
4
+ d
6
and inner radius a
2
+ d
4
d
6
plus another
hollow sphere with outer radius a
2
d
4
+d
6
and inner radius a
2
d
4
d
6
. The light-shaded areas in Figure
5.14(B) depict the secondary workspace.
5.5. WORKSPACE 15
z
1
1
a
0
z
Endpoint
.
a
2
a
2
a
1
(A) (B)
Figure 5.12: A spatial 2R manipulator (A) and its workspace (B).
2
a
1
z
1
Endpoint
2
z
a
3
.
a
0
z
2
a -a
a +a
3 2
a
1
| |
3
(A) (B)
Figure 5.13: A spatial 3R manipulator (A) and its workspace (B).
16 CHAPTER 5. INVERSE KINEMATICS FOR POSITION
d
a
4
d
2
6
6 2
a -d
2 4
a
2 d
6
a -d -d
4
2
.
a -d +d
2 4 6
.
.
a +d -d
4 6
.
.
4
.
.
.
.
d
(A) (B)
Figure 5.14: Elbow robot with last three joints depicted as a ball joint.