Robotics I – Homework 2

Hand out: December 13, 2010; Return by: January 12, 2011

Consider the 3R anthropomorphic robot manipulator with a trunk/shoulder offset shown in Fig. 1.
The DH parameters are given in Tab. 1, where a1 , a2 , a3 , and d1 are all strictly positive.

y3 x3
P
x3
x2 P
q1 y2 q3 z3

y1 y0 z2
q2 x1

x1
z1 q1

z0 x0
x0
top view
side view

Figure 1: Side and top views of the 3R robot, with DH frames assignment (side view is for q1 = 0)

i αi di ai θi
1 π/2 d1 a1 q1
2 0 0 a2 q2
3 0 0 a3 q3

Table 1: Table of DH parameters

We are interested only in the position p of point P , i.e., the origin of frame 3 attached to the
robot end-effector. The direct kinematics is computed as
   
px cos q1 (a1 + a2 cos q2 + a3 cos(q2 + q3 ))
p =  py  =  sin q1 (a1 + a2 cos q2 + a3 cos(q2 + q3 ))  = f (q). (1)
   

pz d1 + a2 sin q2 + a3 sin(q2 + q3 )

i) Find the solution of the inverse kinematics problem in closed form, as a function of p.
ii) Draw a section of the (primary) workspace in the vertical plane (x1 , y 1 ) in parametric form
w.r.t. a1 , a2 , a3 , and d1 , assuming a2 > a3 and a1 > a2 − a3 for the sake of simplicity.
Indicate the regions in this workspace where the inverse kinematics problem has 4 solutions,
2 solutions, 1 solution, or an infinite number of solutions.

1
iii) Test your solution formulas using the data (in cm)

d1 = 200, a1 = 30, a2 = 100, a3 = 80,

for the following desired positions (in cm) of P :

T
a) p = −20 20 310

T
b) p = 195 0 135
√
T
c) p = 30 + 90 3 0 110 (here, px ' 185.8845)

T
d) p = 0 200 35

T
e) p = 0 0 285
Verify your results, when appropriate, by plugging the obtained configurations in the direct
kinematics (1). In general, allow for some numerical tolerance in the resulting end-effector
position due to truncation of decimals in the joint space solution.

iv) Consider again case a) of iii), and include the presence of the following joint limits (in deg):

q1 ∈ [−150◦ , +150◦ ] , q2 ∈ [−140◦ , +140◦ ] , q3 ∈ [−100◦ , +100◦ ] .

How many feasible inverse kinematic solutions are left?

v) Write a (Matlab) program that takes p as input and provides as output a complete answer
(all solutions in any case), including a warning when there is no solution (p is out of the
workspace).

[estimated time for items i)-ii) : 210 minutes (open books)]

2
 Solution of Homework 2
December 13, 2010

Squaring and summing the first two equations in (1), one obtains a relation which is independent
from q1 :
2
p2x + p2y = (a1 + a2 cos q2 + a3 cos(q2 + q3 )) .
Therefore, q
a1 + a2 cos q2 + a3 cos(q2 + q3 ) = ± p2x + p2y . (2)

Provided that p2x + p2y 6= 0, replacing (2) back into the first two equations of (1) yields

± px ± py
c1 = cos q1 = q , s1 = sin q1 = q ,
p2x + p2y p2x + p2y

and the two solutions for q1 are (choosing, respectively, the positive or the negative sign in ‘±’)

q1I = ATAN2 {py , px } , q1II = ATAN2 {−py , −px } . (3)

Note that the angles q1I and q1II differ by π (both are automatically in the interval (−π, π]). In the
first solution, the vertical trunk of the robot (its first link) is ‘facing’ the desired position of point
P , while in the second it is ‘backing’ point P (the trunk is oriented toward the opposite quadrant).
Moreover, it is
p2x + p2y = 0 ⇐⇒ q1 undefined.
Thus, when the end-effector is placed on the axis of joint 1 the robot is in a singularity.

Consider again eq. (2), and rewrite it as

q
p2x + p2y ∓ a1 = ∓ (a2 cos q2 + a3 cos(q2 + q3 )) . (4)

Also, rewrite the last equation in (1) as

pz − d1 = a2 sin q2 + a3 sin(q2 + q3 ). (5)

The right-hand sides of the above two equations are functions of q2 and q3 only. Squaring and
summing eqs. (4) and (5) leads to
q 2
2
p2x + p2y ∓ a1 + (pz − d1 ) = a22 + a23 + 2a2 a3 (cos q2 cos(q2 + q3 ) + sin q2 sin(q2 + q3 ))
= a22 + a23 + 2a2 a3 cos q3 ,

which is a function of q3 only. From this,

q 2
2

p2x + p2y ∓ a1 + (pz − d1 ) − a22 + a23 q
c3 = cos q3 = , s3 = sin q3 = ± 1 − c23 . (6)
2a2 a3
There are four independent combinations of signs involved in the expressions (6). The first ‘∓’
(in black) should be selected according to the (opposite) choice in the solution for q1 , namely the
upper ‘−’ is associated to the ‘+’ in q1I and the lower ‘+’ is associated to the ‘−’ in q1II . For each

3
of these two choices, one has two alternatives for the second ‘±’ (in red). Thus, there will be four
solutions for q3 , obtained from
q3 = ATAN2 {s3 , c3 } , (7)
and labeled as follows:
q3I,+ , q3I,− , q3II,+ , q3II,− . (8)

When all four solutions exist (see below), the following identities hold:
q3I,+ = −q3I,− , q3II,+ = −q3II,− .
Indeed, in (6) it must be
−1 ≤ c3 ≤ 1.
This condition characterizes the fact that the desired position p belongs to the robot (primary)
workspace. The inequalities are made explicit as
q 2
2 2 2
(a2 − a3 ) = a22 +a23 −2a2 a3 ≤ p2x + p2y ∓ a1 + (pz − d1 ) ≤ a22 +a23 +2a2 a3 = (a2 + a3 ) . (9)

T
Thus, before applying eq. (7), one should check whether the given data p = px py pz
satisfy or not (9) —this test can be done right at the beginning of the solution procedure, so as to
discard requests that are out of the workspace (no solution). If the inequalities in (9) are strictly
satisfied for both choices of signs ‘∓’, then there will be four inverse kinematic solutions. If they are
strictly satisfied only for one of the two signs, then there will be only two solutions. Furthermore,
if one of the two inequalities is active (i.e., is satisfied with the equality sign), then |c3 | = 1 and
a pair of solutions in (8) will collapse into one. This is again a singular situation. In particular,
at the outer boundary of the robot workspace (for c3 = 1, i.e., q3 = 0), the other pair of solutions
in (8) will never be present.

For each numerical solution (q1 , q3 ) found so far, the associated value of q2 is determined as
follows. Adding the first equation in (1) multiplied by c1 to the second one multiplied by s1 yields
c1 px + s1 py − a1 = a2 cos q2 + a3 cos(q2 + q3 ) = (a2 + a3 c3 ) cos q2 − (a3 s3 ) sin q2 . (10)
Equation (5) can be manipulated similarly as
pz − d1 = a2 sin q2 + a3 sin(q2 + q3 ) = (a2 + a3 c3 ) sin q2 + (a2 s3 ) cos q2 . (11)
Equations (10–11) constitute a linear system in the two unknowns cos q2 and sin q2 ,
! ! !
a2 + a3 c3 −a3 s3 cos q2 c1 px + s1 py − a1
= ,
a3 s3 a2 + a3 c3 sin q2 pz − d1
which can be solved in a unique way, provided that the coefficient matrix A on the left-hand side
is non-singular. Since det A = a22 + a33 + 2a2 a3 c3 , this determinant is always positive unless a2 = a3
and c3 = −1. In this case, q2 will be undefined1 . When det A > 0, the unique solution is
! ! ! !
c2 cos q2 1 a2 + a3 c3 a3 s3 c1 px + s1 py − a1
= =
s2 sin q2 det A −a3 s3 a2 + a3 c3 pz − d1
!
1 (a2 + a3 c3 ) (c1 px + s1 py − a1 ) + a3 s3 (pz − d1 )
= .
det A (a2 + a3 c3 ) (pz − d1 ) − a3 s3 (c1 px + s1 py − a1 )
1 In this special situation,
q point P is on joint axis 2, at a distance a1 from joint axis 1. Then, the input data will
necessarily be such that p2x + p2y = a1 and pz = d1 . An infinite number of inverse solutions for q2 results.

4
From this, we obtain as usual2
q2 = ATAN2 {s2 , c2 } , (12)
and label each of the (potential) solutions as

q2I,+ , q2I,− , q2II,+ , q2II,− , (13)

where the same notation has been usedWorkspace section

as in (8). Indeed, some of these values may not need to be
computed, depending on the number of inverse kinematic solutions for q3 . This completes item i).

external radius
= a2+a3

y1 y1
a1>0

x1 x1
internal radius
= a2-a3>0

Robotics 1 – Homework 2 – 2010/11

Figure 2: Two sections of the workspace obtained for a given q1 (left) and5 for q1 + π (right)

For item ii), the previous analysis shows that the workspace of the robot can be divided in
regions where the number of inverse kinematics solutions is the same. With reference to Fig. 2,
consider the case of a given q1 . The workspace section is a circular annulus with external radius
a2 + a3 and internal radius |a2 − a3 | (in the given assumptions, simply a2 − a3 > 0), as depicted
on the left of Fig. 2. Proceeding from the origin of the first frame (x1 = y1 = z1 = 0) and moving
outwards, the number of inverse kinematic solutions in this situation will be, respectively, 0, 1
(on the inner boundary), 2, 1 (on the outer boundary), and 0. However, in the same vertical
plane there is another part of the workspace (obtained by symmetric reflection with respect to
z0 ) corresponding to a value of q1 ± π (right of Fig. 2). These two parts may intersect or not,
depending on the relative size of a1 w.r.t. a2 and a3 . Accordingly, the number of solutions will
change.

Figure 3 shows the total workspace section for a1 < a2 + a3 . The shown picture refers to the
case when the additional condition a1 < 1.5a3 − 0.5a2 holds, in which case the inner empty circle of
the configurations with q1 is fully reached by two configurations obtained for q1 + π (and viceversa,
exchanging the role of q1 and q1 + π). The number of resulting inverse kinematic solutions in the
different regions of the total workspace section is also indicated. In particular, on the vertical axis
z0 there will be an infinite number of inverse kinematic solutions (for any given q1 , there are in
general two inverse solutions). The robot workspace is the 3D volume obtained by rotating the
planar figure around z 0 .

It should be noted that the absence of a shoulder offset (a1 = 0) would largely simplify the
analysis. For instance, the two sections obtained for q1 and q1 + π would coincide, and the number
of inverse kinematic solutions would be either 4 (in the generic case, when P is strictly internal to
the workspace), 1 (on the internal and and external boundaries of the workspace, except for the
two points lying on the z 0 axis), or infinite (when P is on the z 0 axis).
2 Being the common denominator det A to s and c positive, it can be discarded in the evaluation of the
2 2
four-quadrant arctangent function.

5
 3 solutions
(on boundary) 4 solutions infinite solutions
(singularity)

1 solution

2
2 solutions
solutions

1 solution
(singularity)

2 solutions

3 solutions (on boundary)

Figure 3: The total workspace section, as union of the two sections in Fig. 2, and the number of
inverse kinematic solutions in the different regions

With the numerical data given in iii), the inverse kinematic solutions are computed as follows.
They are illustrated in Figs. 4–7.

T
a) For p = −20 20 310 , there are 4 distinct inverse kinematic solutions:

T
T
q I,+ = 2.3562 0.8103 1.8427 [rad] =
[deg] 135.00 46.43 105.58

T

T
q I,− = 2.3562 2.3624 −1.8427 [rad] = 135.00 135.36 −105.58 [deg]

T
T
q II,+ = −0.7854 1.3614 1.6273 [rad] = −45.00 78.00 93.23 [deg]

T
T
q II,− = −0.7854 2.7546 −1.6273 [rad] = −45.00 157.83 −93.23 [deg]

45 0 45 0

40 0 40 0

35 0 35 0

30 0 30 0

25 0 25 0

20 0 20 0

15 0 15 0

10 0 10 0

50 50

0 0

− 50 −50
− 2 50 − 200 − 150 − 100 − 50 0 50 1 00 15 0 20 0 25 0 − 250 − 2 00 − 1 50 − 10 0 − 50 0 50 100 1 50 2 00 2 50

Figure 4: The four inverse kinematic solutions for case a), shown in the vertical plane specified by
θ1 = 135◦ . On the left: q I,+ and q I,− . On the right: q II,+ and q II,−

6

T
b) For p = 195 0 135 , there are only 2 distinct inverse kinematic solutions since point
P is in one of the two ‘far’ parts of the workspace, which can be reached only for one value
of q1 (= q1I ). The two solutions are:

T
T
q I,+ = 0 −0.5290 0.3463 [rad] = 0 −30.31 19.84 [deg]

T
T
q I,− = 0 −0.2215 −0.3463 [rad] = 0 −12.69 −19.84 [deg]

450

400

350

300

250

200

150

100

50

− 50
− 250 − 20 0 − 15 0 − 1 00 − 5 0 0 50 10 0 150 200 250

Figure 5: The two inverse kinematic solutions q I,+ and q I,− for case b), shown in the vertical
plane specified by θ1 = 0

√
T
c) For p = 30 + 90 3 0 110 , there is a unique solution (the second and third link are
stretched, with point P on the outer boundary of the workspace). The solution is:

T
T
q I = 0 −0.5236 0 [rad] = 0 −30 0 [deg]

450

400

350

300

250

200

150

100

50

−50
− 250 − 2 00 − 1 50 − 10 0 − 50 0 50 100 1 50 2 00 2 50

Figure 6: The single inverse kinematic solution q I for case c), shown in the vertical plane specified
by θ1 = 0

T
d) For p = 0 200 35 , there are no solutions (p is out of the workspace).

7

T
e) For p = 0 0 285 , there is an infinite number of solutions since q1 is undefined
(arbitrary). However, all solutions have one of the two following forms:

T
T
q any,+ = any 1.0474 2.1144 [rad] = any 60.01 121.14
[deg]

T
T
q any,− = any 2.7728 −2.1144 [rad] = any 158.87 −121.14 [deg]

450

400

350

300

250

200

150

100

50

−50
− 250 − 2 00 − 1 50 − 100 − 5 0 0 50 100 15 0 20 0 25 0

Figure 7: Two (out of the infinite number of) inverse kinematic solutions for case e), shown in the
vertical plane specified by a generic value of θ1 . All other solutions are obtained from this pair, by
varying θ1 ∈ (−π, π]

Finally, in the presence of the joint limits given at item iv) the only solution that remains feasible

T
T
in case a) is q II,+ = −0.7854 1.3614 1.6273 [rad] = −45.00 78.00 93.23 [deg].

∗∗∗∗∗