Robotics 1

February 4, 2021
There are 8 questions. Provide answers with short texts, completed with drawings and derivations needed
for the solutions. Students with confirmed midterm grade should do only the second set of 4 questions.

Question #1 [students without midterm]

√ √ √
T
The orientation of a rigid body is defined by the axis-angle pair r = 1/ 3 −1/ 3 1/ 3
and θ = π/6 [rad]. Determine the angles (α, β, γ) of the Roll-Pitch-Yaw sequence XY Z of fixed
axes that provide the same orientation. Check the correctness of the obtained result. Find the
singular cases for this RPY representation and provide an example of an axis-angle pair (r s , θs )
that would fall in this class.

Question #2 [students without midterm]

Figure 1 shows a top view of a planar two-jaw articulated gripper. This robotic gripper has a
revolute joint at its base, followed by two independent revolute joints for each jaw (the first joints
in the two jaws share the same axis). This 5-dof robotic system has a tree structure for which the
usual Denavit-Hartenberg frame assignment can also be applied (to each branch). Define the joint
coordinates accordingly, together with the two DH tables. Provide then the symbolic expression
of some task variables that are relevant for gripping operations, defined as follows:
• position of the midpoint Pc between the tips of the two jaws;
• distance d between the two tips;
• relative angle αrel of the left jaw w.r.t. the right jaw;
• orientation angle β w.r.t. the x0 axis of the jaw pair (from the right jaw tip to the left one).
When the gripper links have all the same length L = 0.05 [m], compute the numerical value of
such task variables in the configuration q = (q1 , qr2 , qr3 , ql2 , ql3 ) = (−π/2, −π/2, 3π/4, π/2, −3π/4).
Subscripts r and l stand respectively for DH variables pertaining to the right or left jaw only.

Ll3

Ll2
y0 axis with double actuation
(one for each branch)
L1
Lr2
x0 Lr3

Figure 1: A planar 5-dof two-jaw gripper.

Question #3 [students without midterm]

A planar 2R robot has incremental encoders at the joints measuring the configuration θ = (θ1 , θ2 )
used in the computation of its direct kinematics. Because of a bad mounting of the encoders, the
two measures are affected by (very) small angular errors δ1 and δ2 . When using these readings,
which of the following statements is correct in terms of Cartesian accuracy of the end-effector
position? A) there is always an error; B) there are configurations at which there may be no error;
C) the error is always negligible (e.g., below the sensor resolution). Provide a detailed explanation
of your answer!

1
Question #4 [students without midterm]
The prismatic joints of the planar PPR robot in Fig. 2 have bounded ranges, qi,min ≤ qi ≤ qi,max ,
for i = 1, 2, while the revolute joint q3 has an unlimited motion range. Draw accurately the primary
workspace W S1 and the secondary workspace W S2 of this robot, under the following assumption
for the third link length: L < min {(q1,max − q1,min ) /2, (q2,max − q2,min ) /2}.

L
q3
y0

q2
q1

x0

Figure 2: A planar PPR robot.

Question #5 [all students]

The direct kinematics and the initial configuration of a planar RP robot are given by
   
q2 cos q1 {0} π/4
p = f (q) = , q = ,
q2 sin q1 ε

where 0 <   1 is a very small number. Given the following desired end-effector positions,
−1
   
1
pd,I = and pd,II = ,
1 1

compute the first iteration (i.e., q {1} ) of a Newton method and of a Gradient method for solving
the two inverse kinematics problems. Discuss what happens in each of the four cases when ε → 0.
Question #6 [all students]
The 3R robotic device in Fig. 3 has joint axes that intersect two by two. The second joint axis
is inclined by an angle δ ≈ 20◦ . This structure is mainly intended for pointing the final axis n
at a moving target in 3D. Provide the explicit expression of the square angular part J A (q) of
the geometric Jacobian of this robot. Find the singularities, if any, of the mapping ω = J A (q)q̇.
Compute the relation between q̇ ∈ R3 and the time derivative ṅ of the pointing axis.
pointing axis n
joint 3
(orthogonal to axis of joint 2)

joint 2
(axis inclined by 𝛿 ≈ 20°)

joint 1
(vertical axis)

Figure 3: A 3-dof robotic pointing device.

2
Question #7 [all students]
The desired initial and final orientation of the end effector of a certain robot are specified at time
t = 0 and t = T , respectively by
 √  √ √ 
0.5 0 − 3/2 2/2 − 2/2 0
 √ √
R(0) = Rin =  − 3/2 0 −0.5  , R(T ) = Rf in =  −0.5 −0.5 − 2/2  .
  
√
0 1 0 0.5 0.5 − 2/2

The end effector should start with zero angular velocity and acceleration (ω in = ω̇ in = 0, at t = 0)
and reach the final orientation with angular velocity and acceleration given by
 
3
ω(T ) = ω f in =  −2  [rad/s], ω̇(T ) = ω̇ f in = 0.
 
1

Plan a smooth and coordinated trajectory for the end-effector orientation that satisfies all the
given boundary conditions for a generic motion time T > 0. Setting next T = 1, compute at the
mid-time instant t = T /2 the numerical values of the resulting orientation R(T /2) and angular
velocity ω(T /2) of the robot end effector.

Question #8 [all students]

The planar 3R robot with unitary link lengths shown in Fig. 4 is initially in the configuration
q in = (−π/9, 11π/18, −π/4). Commanded by a joint velocity q̇(t) that uses feedback from the
current q(t), the robot should perform a self-motion so as to reach asymptotically the final value
q3,f in = −π/2 for the third joint, while keeping the position of its end-effector always at the same
initial point Pin . Verify first that such task is feasible. Design then a control scheme that completes
the task in a robust way, i.e., by rejecting also possible transient errors and without encountering
any singular situation in which the control law is ill conditioned. Hint: Use an approach based on
joint space decomposition.

Pin
y0 q3

q1
x0
q2

Figure 4: A 3R robot that should perform a self-motion task with constant end-effector position.

[240 minutes (4 hours) for the full exam; open books]

[120 minutes (2 hours) for students with midterm; open books]

3
 Solution
February 4, 2021

Question #1 [students without midterm]

√ √ √
T
The orientation of a rigid body is defined by the axis-angle pair r = 1/ 3 −1/ 3 1/ 3 and
θ = π/6 [rad]. Determine the angles (α, β, γ) of the Roll-Pitch-Yaw sequence XY Z of fixed axes
that provide the same orientation. Check the correctness of the obtained result. Find the singular
cases for this RPY representation and provide an example of an axis-angle pair (r s , θs ) that would
fall in this class.
Reply #1
The solution requires back and forth transformations among different representations of orientation.
From the given axis-angle pair (r, θ), we compute the rotation matrix
 
0.9107 −0.3333 −0.2440
R(r, θ) = rr T + I − rr T cos θ + S(r) sin θ =  0.2440


0.9107 −0.3333  = Rd . (1)
 
0.3333 0.2440 0.9107

Three rotations by the angles (α, β, γ) around the sequence of XY Z fixed axes (i.e., of the RPY
type) are associated to the rotation matrix

RXY Z (α, β, γ) = RZ (γ)RY (β)RX (α)

 
cos β cos γ sin α sin β cos γ − cos α sin γ sin α sin γ + cos α sin β cos γ
= cos β sin γ cos α cos γ + sin α sin β sin γ cos α sin β sin γ − sin α cos γ . (2)
 
− sin β sin α cos β cos α cos β

For the nonsingular case, when

q
cos β = ± 2 + R2 6= 0,
R32 33

solving RXY Z (α, β, γ) = Rd = {Rij } for (α, β, γ) yields a pair of solutions (one for each sign
chosen inside cos β):
   
R32 R33 R21 R11
α = ATAN2 , , β = ATAN2 {−R31 , cos β} , γ = ATAN2 , .
cos β cos β cos β cos β
Using the data, we have cos β = ± 0.9428 6= 0 and find the two numerical solutions

α0 = 0.2618, β 0 = −0.3398, γ 0 = 0.2618 [rad]

and
α00 = −2.8798, β 00 = −2.8018, γ 00 = −2.8798 [rad].
Plugging any of these triples into (2) returns indeed Rd . The singularity of this minimal represen-
2 2
tation of orientation occurs for cos β = 0 (i.e., β = ±π/2), namely when R32 + R33 = 0. In this
case, we can further solve only for the sum α + γ (when sin β = −R11 = 1) or the difference α − γ
(for sin β = −1) of the remaining two angles. For instance, a rotation matrix in this class is
 
0 −1 0
Rs =  0 0 −1  , (3)
 
1 0 0

4
for which β = π/2 and α − γ = −π/2. The rotation matrix Rs = {Rij } is in turn nonsingular for
the transformation with an axis-angle representation, being
1p
sin θ = (R12 − R21 )2 + (R13 − R31 )2 + (R23 − R32 )2 = 0.8660 6= 0.
2
Therefore, Rs can be generated by the pair (r 0s , θs0 ) computed as
 
R11 + R22 + R33 − 1
θs0 = ATAN2 sin θ, = 2.0944 [rad]
2
   √ 
R32 − R23 1/ 3
1  √ 
r 0s =  R13 − R31  =  −1/ 3  ,
 
2 sin θ √
R21 − R12 1/ 3

as well as by the pair (r 00s , θs00 ) = (−r 0s , −θs0 ). Note that r 0s = r, the same unit axis given in the
text as a starting point. This coincidence occurs by pure chance. It is indeed θs0 6= θ, otherwise
we would have generated Rs in (3) instead of Rd in (1), obtaining a singular case for the chosen
RPY representation. 
Question #2 [students without midterm]
Figure 1 shows a top view of a planar two-jaw articulated gripper. This robotic gripper has a
revolute joint at its base, followed by two independent revolute joints for each jaw (the first joints
in the two jaws share the same axis). This 5-dof robotic system has a tree structure for which the
usual Denavit-Hartenberg frame assignment can also be applied (to each branch). Define the joint
coordinates accordingly, together with the two DH tables. Provide then the symbolic expression of
some task variables that are relevant for gripping operations, defined as follows:
• position of the midpoint Pc between the tips of the two jaws;
• distance d between the two tips;
• relative angle αrel of the left jaw w.r.t. the right jaw;
• orientation angle β w.r.t. the x0 axis of the jaw pair (from the right jaw tip to the left one).
When the gripper links have all the same length L = 0.05 [m], compute the numerical value of such
task variables in the configuration q = (q1 , qr2 , qr3 , ql2 , ql3 ) = (−π/2, −π/2, 3π/4, π/2, −3π/4).
Subscripts r and l stand respectively for DH variables pertaining to the right or left jaw only.
Reply #2
Figure 5 shows the xi axes of the frames assigned to the articulated gripper according to the
Denavit-Hartenberg convention, together with the associated joint variables and all the relevant
quantities for defining the task variables. The two DH tables, respectively for the right and the
left jaws, are given in Tab. 1.

i αi ai di θi i αi ai di θi

1 0 L1 0 q1 1 0 L1 0 q1

2 0 Lr2 0 qr2 2 0 Ll2 0 ql2

3 0 Lr3 0 qr3 3 0 Ll3 0 ql3

Table 1: Tables of DH parameters for the right and left jaws of the gripper.

5
 xl2
ql3

Pl
Ll3 𝛼l
Ll2 𝛼#$% = 𝛼l − 𝛼 r
ql2 xl3
x1
y0 Pc
(= xr1 = xl1) 𝛽
L1 qr2 d xr3
q1 Lr3
Lr2
Pr 𝛼r
x0 qr3
xr2

Figure 5: DH frames, with definition of coordinates and task variables for the two-jaw gripper.

Based on these definitions, we compute the direct kinematics of the two tip points Pr and Pl and
the final orientations αr and αl of the two jaws:
 
L1 cos q1 + Lr2 cos(q1 + qr2 ) + Lr3 cos(q1 + qr2 + qr3 )
pr =
L1 sin q1 + Lr2 sin(q1 + qr2 ) + Lr3 sin(q1 + qr2 + qr3 )
 
L1 cos q1 + Ll2 cos(q1 + ql2 ) + Ll3 cos(q1 + ql2 + ql3 )
pl =
L1 sin q1 + Ll2 sin(q1 + ql2 ) + Ll3 sin(q1 + ql2 + ql3 )
αr = q1 + qr2 + qr3
αl = q1 + ql2 + ql3 .

The considered task variables are then defined as

pl + pr
pc =
2
d = kpl − pr k

αrel = αl − αr = ql2 + ql3 − qr2 − qr3

β = ATAN2 {ply − pry , plx − prx } .

With the given data for link lengths and current configuration, we obtain the numerical values
 
0 π
pc = [m], d = 0.02929 [m], αrel = − [rad], β = 0. 
−0.08536 2

Question #3 [students without midterm]

A planar 2R robot has incremental encoders at the joints measuring the configuration θ = (θ1 , θ2 )
used in the computation of its direct kinematics. Because of a bad mounting of the encoders, the
two measures are affected by (very) small angular errors δ1 and δ2 . When using these readings,
which of the following statements is correct in terms of Cartesian accuracy of the end-effector
position? A) there is always an error; B) there are configurations at which there may be no error;

6
C) the error is always negligible (e.g., below the sensor resolution). Provide a detailed explanation
of your answer!
Reply #3
The correct answer is B. Statement A will automatically be false (because of the word ‘always’),
once we confirm the correctness of B. Statement C seems ambiguous, since no information is
provided on the sensor resolution nor on the link lengths, which may both be arbitrary small or
large. Therefore, for a given amount of error δ one can consider a robot with sufficiently long links
so that the Cartesian accuracy becomes unacceptably large1 . To show the validity of B, consider
the 2R robot in a singular configuration, say the stretched one θ ∗ = (θ1 , 0) for an arbitrary θ1 .
The robot Jacobian would become
− (l1 sin θ1 + l2 sin(θ1 + θ2 )) −l2 sin(θ1 + θ2 ) −(l1 + l2 ) sin θ1 −l2 sin θ1
   
J (θ ∗ ) = = ,
l1 cos θ1 + l2 cos(θ1 + θ2 ) l2 cos(θ1 + θ2 ) θ=θ ∗
(l1 + l2 ) cos θ1 l2 cos θ1

with rank J (θ ∗ ) = 1. In view of the assumption of small errors δ for the sensor readings in the
joint space, we can use the differential mapping to evaluate their effect on the Cartesian error
displacements, i.e., ∆p = J (θ)δ. Therefore, at θ ∗ , all sensing errors δ ∗ ∈ R2 that are in the null
space of J (θ ∗ ) will produce no the Cartesian errors:

−l1
 
δ∗ =  , ||  1 ⇒ J (θ ∗ )δ ∗ = 0. 
l1 + l2

Question #4 [students without midterm]

The prismatic joints of the planar PPR robot in Fig. 2 have bounded ranges, qi,min ≤ qi ≤ qi,max ,
for i = 1, 2, while the revolute joint q3 has an unlimited motion range. Draw accurately the primary
workspace W S1 and the secondary workspace W S2 of this robot, under the following assumption
for the third link length: L < min {(q1,max − q1,min ) /2, (q2,max − q2,min ) /2}.

y0 WS1

L=3
q2,max = 5

C = (2,0.5)
WS2

q1,min = -2 3
x0
2 q1,max = 6

q2,min = -4

Figure 6: Primary and secondary workspaces for a planar PPR robot with bounded range of the
prismatic joints.
1 Indeed, the definition of a normalized accuracy would scale to the size of the robot. In that case, this argument

would not work. But again, the word ‘always’ largely weakens statement C, just as in case A.

7
Reply #4
The primary and secondary workspaces of the PPR robot are drawn in Fig. 6, using the following
set of representative values:

q1,min = −2, q1,max = 6; q2,min = −4, q2,max = 5; L=3 [m].

The ranges of the prismatic joints need not to be symmetric. The assumption on the length L of
the third link is satisfied here, allowing the presence of a non-vanishing W S2 , where the robot end-
effector can assume any orientation angle in the plane. The outer boundary of W S1 is a rectangle
of side lengths qi,max − qi,min + 2L, i = 1, 2, with corners smoothed by circles of radius L. The
outer boundary of W S2 is a rectangle with sides qi,max − qi,min − 2L > 0, i = 1, 2. 

Question #5 [all students]

The direct kinematics and the initial configuration of a planar RP robot are given by
   
q2 cos q1 π/4
p = f (q) = , q {0} = ,
q2 sin q1 ε

where 0 <   1 is a very small number. Given the following desired end-effector positions,
−1
   
1
pd,I = and pd,II = ,
1 1

compute the first iteration (i.e., q {1} ) of a Newton method and of a Gradient method for solving
the two inverse kinematics problems. Discuss what happens in each of the four cases when ε → 0.
Reply #5
Evaluating the Jacobian of the RP robot at q {0} , we have
 √ √ 
2 2
−q2 sin q1  −ε 2
 
cos q1 2 
J (q {0} ) = =
 √ √  .
q2 cos q1 sin q1 q=q {0} 2 2
ε
2 2
It is det J (q {0} ) = −ε, and thus a singularity is approached when ε → 0. For the desired end-
effector position of case I, with the Newton method we have at the first iteration
{1}
q N ewton,I = q {0} + J −1(q {0} ) pd,I − f (q {0} )

 √ √   √  √ 
 π  2 2 2 
 − 2ε −1
ε π 2
 
2ε  
 
 √2  =  4
  +
=  4 + √ √  1 − ε ,
2 2 2
  
ε 0
ε
2 2 2
whereas with the Gradient method (for a generic step size α > 0) it is
{1}
q Gradient,I = q {0} + α J T(q {0} ) pd,I − f (q {0} )

 √ √   √ 
 π  2 2 2  π √ 
 −ε 2 ε ε + α 2ε
 −1 − 
 
=  4  + α 2   2  
  4
 =
.
 √ √   1 √ 
2 2 2

ε ε ε (1 − α)
2 2 2

8
For ε → 0, it is easy to see that
 π 
∞
 
{1} {1}
q N ewton,I → , q Gradient,I →  4  = q {0} ,
0 0
illustrating how the Newton method diverges while approaching a singularity, while the Gradient
method simply stops. In fact, when ε = 0, the position error e{0} = pd,I − f (q {0} ) = pd,I = (−1, 1)
belongs to the null space of J T(q {0} )|ε=0 . For case II, with the Newton method one has
{1}
q N ewton,II = q {0} + J −1(q {0} ) pd,II − f (q {0} )

 √ √   √ 
 π  2 2 2  π 
 − 2ε ε
 
2ε   1  2   4  = q ∗ .
  
=  4 +  √ √    1 −  √  = √

ε 2 2 2 2
ε
2 2 2
Being f (q ∗ ) = pd,II , we have found a solution q ∗ to the inverse kinematics problem in just one
iteration (thanks to the simple structure of this specific problem). Moreover, this holds true
independently from the value of ε, which in fact cancels out. On the other hand, with the Gradient
method, and for a generic step size α > 0, it is
{1}
q Gradient,II = q {0} + α J T(q {0} ) pd,II − f (q {0} )

 √ √   √ 
 π  2 2 2  π 
 −ε 2 ε ε
 
2  1  2   4
    
=  4  + α  √ √    1 −  √  = √
.
ε 2 2 2 α 2 + ε (1 − α)
ε
2 2 2
When ε → 0, we have  π 
{1} 4 ,
q Gradient.II →  √
α 2
n o
so that the Gradient method will escape the singularity (here, e{0} = pd,II 6∈ N J T(q {0} )|ε=0 ).
One can easily see that also the Gradient method may find the solution q ∗ in just one iteration, but
only if the step size is chosen as α = 1 (and then, once again, this would then occur independently
from the actual value of ε). However, α is usually chosen smaller than unitary in the final iterations
in order to avoid missing a close solution. Therefore, the Gradient method will typically approach
the solution q ∗ at a slower rate than the Newton method. 

Question #6 [all students]

The 3R robotic device in Fig. 3 has joint axes that intersect two by two. The second joint axis
is inclined by an angle δ ≈ 20◦ . This structure is mainly intended for pointing the final axis n
at a moving target in 3D. Provide the explicit expression of the square angular part J A (q) of the
geometric Jacobian of this robot. Find the singularities, if any, of the mapping ω = J A (q)q̇.
Compute the relation between q̇ ∈ R3 and the time derivative ṅ of the pointing axis.
Reply #6
The angular part of the geometric Jacobian for this 3R robotic pointing device is the 3 × 3 matrix
   

J A (q) =  z 0 z1 z2  =  0z0 0
R1 (q1 ) 1 z 1 0
R1 (q1 ) 1 R2 (q2 ) 2 z 2 

9
where z i−1 is a unit vector along the axis of joint i, for i = 1, 2, 3, expressed by default in the

T
robot base frame RF0 , and j z j = 0 0 1 , for any index j. In order to provide the explicit
expression of the elements in this matrix, it is convenient to define a set of frames according to the
DH convention, e.g., as in Fig. 7. From the DH table reported therein2 , one has
   
cos q1 − cos δ sin q1 sin δ sin q1 cos q2 0 − sin q2
0
R1 (q1 ) =  sin q1 cos δ cos q1 − sin δ cos q1  , 1 R2 (q2 ) =  sin q2 0 cos q2  ,
   
0 sin δ cos δ 0 −1 0

and so
 
cos q1 cos q2 − cos δ sin q1 sin q2 − sin δ sin q1 − cos q1 sin q2 − cos δ sin q1 cos q2
0
R2 (q1 , q2 ) =  sin q1 cos q2 + cos δ cos q1 sin q2 sin δ cos q1 cos δ cos q1 cos q2 − sin q1 sin q2  .
 
sin δ sin q2 − cos δ sin δ cos q2

As a result,
 
0 sin δ sin q1 − cos q1 sin q2 − cos δ sin q1 cos q2
J A (q) =  0 − sin δ cos q1 cos δ cos q1 cos q2 − sin q1 sin q2  .
 
1 cos δ sin δ cos q2

pointing axis n
q3 x3
z2 = z3
x2
𝛿
z1 z0
h
x0
q1
q2 x1

i ai ai di 𝜽i
1 𝛿 0 0 q1
2 -𝜋/2 0 h q2
3 0 0 0 q3

Figure 7: DH frames and table used for defining the Jacobian J A (q) of the pointing device.
The singularities of the mapping ω = J A (q)q̇ occur when

det J A (q) = − sin δ sin q2 = 0 ⇐⇒ q2 = {0, π} ,

2 Since z 3 does not enter in the Jacobian J A (q), the choice of a particular final twist α3 is irrelevant here.

10
namely when the axis x1 and the projection of the axis x2 on the plane (x0 , y 0 ) are aligned. In
this situation, the following instantaneous joint velocities
   
−1 −1
q̇ = λ  cos δ , for q2 = 0 or q̇ = λ  cos δ , for q2 = π,
   
sin δ − sin δ

lie in the null space of J and will produce thus ω = 0, ∀λ. Finally, the pointing axis n is given by
 
1
n = 0 R1 (q1 ) 1 R2 (q2 ) 2 R3 (q3 )  0 
 
0
   
(cos q1 cos q2 − cos δ sin q1 sin q2 ) cos q3 − sin δ sin q1 sin q3 n1 (q)
=  (sin q1 cos q2 + cos δ cos q1 sin q2 ) cos q3 + sin δ cos q1 sin q3  =  n2 (q)  .
   
sin δ sin q2 cos q3 − cos δ sin q3 n3 (q)


Let R(q) = 0 R1 (q1 ) 1 R2 (q2 ) 2 R3 (q3 ) = n s a . The time derivative of the unit vector n is
computed then as

Ṙ = S(ω)R ⇒ ṅ = S(ω) n = ω × n = (J A (q)q̇) × n = (z 0 × n) q̇1 + (z 1 × n) q̇2 + (z 3 × n) q̇3 .

The following computations can be conveniently performed with a symbolic code in Matlab:
 
−n2 (q)
z 0 × n =  n1 (q)  ,
 
0
   
sin δ sin q1 − (cos q1 sin q2 + cos δ sin q1 cos q2 ) cos q3
0
R1 1 z 1


z 1 ×n = ×n =  − sin δ cos q1  ×n =  − (sin q1 sin q2 − cos δ cos q1 cos q2 ) cos q3  ,
   
cos δ sin δ cos q2 cos q3
and
 
cos q1 sin q2 − cos δ sin q1 cos q2


z 2 × n = 0 R2 2 z 2 × n =  cos δ cos q1 cos q2 − sin q1 sin q2  × n
 
sin δ cos q2
 
cos δ sin q1 sin q2 sin q3 − sin δ sin q1 cos q3 − cos q1 cos q2 sin q3
=  sin δ cos q1 cos q3 − sin q1 cos q2 sin q3 − cos δ cos q1 sin q2 sin q3  . 
 
− cos δ cos q3 − sin δ sin q2 sin q3

Question #7 [all students]

The desired initial and final orientation of the end effector of a certain robot are specified at time
t = 0 and t = T , respectively by
 √  √ √ 
0.5 0 − 3/2 2/2 − 2/2 0
 √ √
R(0) = Rin =  − 3/2 0 −0.5  , R(T ) = Rf in =  −0.5 −0.5 − 2/2  .
  
√
0 1 0 0.5 0.5 − 2/2

11
The end effector should start with zero angular velocity and acceleration (ω in = ω̇ in = 0, at t = 0)
and reach the final orientation with angular velocity and acceleration given by
 
3
ω(T ) = ω f in =  −2  [rad/s], ω̇(T ) = ω̇ f in = 0.
 
1

Plan a smooth and coordinated trajectory for the end-effector orientation that satisfies all the given
boundary conditions for a generic motion time T > 0. Setting next T = 1, compute at the mid-
time instant t = T /2 the numerical values of the resulting orientation R(T /2) and angular velocity
ω(T /2) of the robot end effector.
Reply #7
This trajectory planning problem for the end-effector orientation has the special feature of requiring
a desired non-zero angular velocity at the final instant of motion. So, it is very unlikely that
addressing the reorientation using the axis-angle method will work (in fact, this has zero probability
to occur!). The reason is that, if we extract a unit axis r and an angle θif from the relative
rotation Rif = RTin Rf in and then perform the rotation around r with any possible timing θ(t),
the associated angular velocity ω r (t) = r θ̇(t) will always be aligned with r. In particular, at
t = T , it will be ω r (T ) 6= ω f in , and we cannot satisfy such equality for an arbitrary ω f in (one can
check that this in fact the case here too). Therefore, we pursue a solution by planning the motion
for the three angles of a minimal representation of orientation, and imposing suitable boundary
conditions. Indeed, many choices are possible and the only precaution is to avoid singularities of
the representation during the entire reorientation. In the following, we shall use the sequence of
XYZ Euler angles φ = (α, β, γ).
For this Euler representation of orientation, we have the rotation matrix

RE,XY Z (φ) = RX (α)RY (β)RZ (γ)

 
cos β cos γ − cos β sin γ sin β
(4)
= cos α sin γ + sin α sin β cos γ cos α cos γ − sin α sin β sin γ − sin α cos β .
 
sin α sin γ − cos α sin β cos γ sin α cos γ + cos α sin β sin γ cos α cos β

At the differential level, we have also the relationship between φ̇ and the angular velocity ω ∈ R3 :
      
α̇ α̇ 0 0
ω = 0 x0 RX (α) 1 y 1 RX (α)RY (β) 2 z 2  β̇  =  0 RX (α) β̇  RX (α)RY (β) 0  

      
γ̇ 0 0 γ̇
   (5)
1 0 sin β α̇
=  0 cos α − sin α cos β   β̇  = T (φ) φ̇.
  
0 sin α cos α cos β γ̇

Matrix T has det T (φ) = cos β. This is indeed the singularity of the chosen Euler representation.
One should make sure that the condition β = ±π/2 is never crossed in the solution of the problem
(otherwise, we should change representation and restart from scratch). With the expression (4) at
hand, one can convert the given initial and final rotation matrices Rin and Rf in into XYZ Euler
angles by using general inversion formulas that hold in the non-singular case only, i.e., when
q
cos β = ± R11 2 + R2 6= 0.
12

12
This happens to be the case for both rotation matrices. Then, from
   
−R23 R33 −R12 R11
α = ATAN2 , , β = ATAN2 {R13 , cos β} , γ = ATAN2 , , (6)
cos β cos β cos β cos β

we obtain the following pairs of solutions (depending on the sign chosen in the expression of cos β)

I π π
 φin = ( 2 , − 3 , 0)

Rin ⇒ cos β = ± 0.5 ⇒ φin = (αin , βin , γin ) =
 φII = (− π , − 2π , π)

in
2 3

I 3π π
 φf in = ( , 0, )


4 4
Rf in ⇒ cos β = ± 1 ⇒ φf in = (αf in , βf in , γf in ) =
π 3π
 φII

f in = (− , π, − ).

4 4
Keeping into account the need to avoid the crossing of a value β = ±π/2, we choose the combination
(out of four possible) φIin (with βin = −π/3) and φIf in (with βf in = 0) as boundary conditions
for the Euler angles trajectories3 . In this way, a smooth interpolation should also guarantee that
β(t) remains in the singularity-free interval [−π/3, 0] for all t ∈ [0, T ]. We use finally eq. (5) to
convert the given angular velocity ω f in into a boundary condition for the first derivative of the
Euler angles at the final time t = T . In view of the non-singularity of T (φf in ), we obtain
 −1    
1 0 0 3 3
√ √
φ̇f in = T −1 (φf in ) ω f in =0 − 2/2 2/2   −2  =  2.1213  [rad/s].
     
√ √ √
0 2/2 − 2/2 1 2/2

The interpolation problem for the Euler angles requires generating a φ(t) for t ∈ [0, T ] such that

φ(0) = φin , φ̇(0) = 0, φ̈(0) = 0, φ(T ) = φf in , φ̇(T ) = φ̇f in , φ̈(T ) = 0.

We use then quintic polynomials in normalized time of the form

t
φ(τ ) = φin + a3 τ 3 + a4 τ 4 + a5 τ 5 , τ= ∈ [0, 1], (7)
T
where the vectors a3 , a4 , a5 ∈ R3 contain the non-vanishing coefficients of the three quintic poly-
nomials for α(τ ), β(τ ), and γ(τ ). The solution is illustrated next for a generic scalar component
φi (τ ) of φ(τ ), with i = 1, 2, 3, dropping also the index. The chosen structure of the polynomial
in (7) already satisfies the three boundary conditions at τ = 0. For the remaining three conditions
at τ = 1, we have:

φ(1) = φin + a3 + a4 + a5 = φf in
dφ dτ 1
φ̇(1) = · = (3a3 + 4a4 + 5a5 ) = φ̇f in
dτ τ =1 dt T
 2
d2 φ dτ 1
φ̈(1) = 2
· = (6a3 + 12a4 + 20a5 ) 2 = 0,
dτ τ =1 dt T
3 In the following, we shall drop for conciseness the index I from φin and φf in .

13
or     
1 1 1 a3 φf in − φin
Ma =  3 4 5   a4  =  φ̇f in T  = b,
    
6 12 20 a5 0
with det M = 2. Solving this linear system yields
    
a3 10 −4 0.5 φf in − φin
a =  a4  = M −1 b =  −15 7 −1   φ̇f in T  ,
    
a5 6 −3 0.5 0
or

a3 = 10(φf in − φin ) − 4φ̇f in T, a4 = −15(φf in − φin ) + 7φ̇f in T, a5 = 6(φf in − φin ) − 3φ̇f in T.

In vector form, the solution can be rewritten as

φ(τ ) = φin + φf in − φin 10τ 3 − 15τ 4 + 6τ 5 + φ̇f in T −4τ 3 + 7τ 4 − 3τ 5 ,

τ ∈ [0, 1].

Moreover, the first and second time derivatives are

φf in − φin
30τ 2 − 60τ 3 + 30τ 4 + φ̇f in −12τ 2 + 28τ 3 − 15τ 4 ,



φ̇(τ ) =
T
φf in − φin
φ̇f in
60τ − 180τ 2 + 120τ 3 + −24τ + 84τ 2 − 60τ 3 .


φ̈(τ ) = 2
T T
By setting now T = 1 [s] as motion time, we can fully evaluate the coefficients of the three quintic
polynomials. The result is
   
α(t) 1.5708 − 4.1460 t3 + 9.2190 t4 − 4.2876 t5
φ(t) =  β(t)  =  −1.0472 + 1.9867 t3 − 0.8587 t4 − 0.0808 t5  , t ∈ [0, 1].
   
γ(t) 5.0256 t3 − 6.8312 t4 + 2.5911 t5

Figure 8 shows the trajectories of these interpolating XYZ Euler angles, together with the evolu-
tions of their first and second time derivatives. At the motion mid-time t = 0.5 [s], we have
   
1.4947 0.6302 −0.1827 −0.7546
φm = φ(0.5) =  −0.8551  [rad] ⇒ RE,XY Z (φm ) =  −0.7015 0.2825 −0.6543  .
   
0.2822 0.3327 0.9417 0.04985

Moreover, from the first derivative of the Euler angles trajectory evaluated at the motion mid-time
t = 0.5 [s],  
0.3202
φ̇m = φ̇(0.5) =  2.0708  [rad/s],
 
2.3265
we also obtain
   
1 0 −0.7546 −1.8511
ω m = ω(0.5) = T (φm )φ̇m =0 0.0760 −0.6543  φ̇m =  2.4771  [rad/s]. 
   
0 0.9971 0.04985 −2.8774

14
 evolution of XYZ Euler angles
2.5

1.5

, (red), - (blue), . (green) [rad]

0.5

-0.5

-1

-1.5
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(normalized) time

evolution of first derivative of XYZ Euler angles

3

2.5
d ,/dt (red), d -/dt (blue), d ./dt (green) [rad/s]

1.5

0.5

-0.5
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(normalized) time

evolution of second derivative of XYZ Euler angles

8

7
d 2 ,/dt 2 (red), d 2 -/dt 2 (blue), d 2 ./dt 2 (green) [rad/s 2 ]

-1

-2
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
(normalized) time

Figure 8: Trajectories of the interpolating XYZ Euler angles (top), with their velocities (center)
and accelerations (bottom).

15
Question #8 [all students]
The planar 3R robot with unitary link lengths shown in Fig. 4 is initially in the configuration
q in = (−π/9, 11π/18, −π/4). Commanded by a joint velocity q̇(t) that uses feedback from the
current q(t), the robot should perform a self-motion so as to reach asymptotically the final value
q3,f in = −π/2 for the third joint, while keeping the position of its end-effector always at the same
initial point Pin . Verify first that such task is feasible. Design then a control scheme that completes
the task in a robust way, i.e., by rejecting also possible transient errors and without encountering
any singular situation in which the control law is ill conditioned. Hint: Use an approach based on
joint space decomposition.
Reply #8
The planar robot has n = 3 joints and is kinematically redundant for the positioning of its end
effector in the plane (m = 2). The requested task requires the use of this redundancy, exploring
the null space motions so as to get to the desired joint configuration while keeping the end-effector
at rest in the initial position. Before proposing a control solution, we have to verify whether or not
the end effector of the 3R robot with the third link at q3 = −π/2 can still reach the point Pin . We
shall call the 3R robot with√ the third √ joint at this angle, a ‘reduced 2R’ robot having its second
equivalent link of length l2 + l3 = 2. Using the initial configuration q in , we compute first the
position of point Pin as
   
cos q1 + cos(q1 + q2 ) + cos(q1 + q2 + q3 ) 1.6468
pin = f (q in ) = = ,
sin q1 + sin(q1 + q2 ) + sin(q1 + q2 + q3 ) q=q
1.3651
in

obtaining kpin k = 2.1390 [m] as distance from the origin. The workspace of √ the reduced 2R
robot is a circular annulus with inner and outer radius given by Rmin = 1 − 2 = 0.4142 and
√
Rmax = 1 + 2 = 2.4142, respectively. Therefore, the point Pin will be inside the workspace of
the reduced 2R robot when at destination. In principle, we should be able in principle to keep the
end effector at Pin during the entire self-motion task —see also Fig. 9.

q3,fin = −𝜋/2
Pin
y0

qin

x0

Figure 9: Configuration reached by the 3R robot at the end of the controlled self-motion task.
A possible approach would be to define the joint velocity q̇ as the projection of a suitable command
ξ ∈ R3 in the null space of the 2 × 3 task Jacobian J (q) = (∂f (q)/∂q), namely4
 
q̇ = I − J # (q)J (q) ξ.
4 See also slide #19 in the lecture block 12 InverseDiffKinStatics.pdf.

16
However, in this way we would have no control on which configuration would be eventually reached
by the robot. Therefore, we opt for a more tailored solution that focuses on the third joint, the only
one that has a special target, leaving to the other two joints the task of keeping the end effector
at the desired position Pin . This is also called a joint space decomposition approach. Indeed, it
could be used in a planning fashion, but here a feedback solution is required. With this in mind,
we set
q̇3 = k3 (q3,f in − q3 ) , k3 > 0. (8)
This guarantees that q3 will converge exponentially from any position to the desired q3,f in . More-
over, the natural motion of q3 will always remain in the interval [q3,f in , q3,in ] = [−π/2, −π/4].
Next, decompose the differential kinematics as follows:

ṗ = J (q)q̇ = J 12 (q)q̇ 12 + J 3 (q) q̇3

− sin q1 − sin(q1 + q2 ) − sin(q1 + q2 + q3 ) − sin(q1 + q2 ) − sin(q1 + q2 + q3 )
  
q̇1
=
cos q1 + cos(q1 + q2 ) + cos(q1 + q2 + q3 ) cos(q1 + q2 ) + cos(q1 + q2 + q3 ) q̇2 (9)
− sin(q1 + q2 + q3 )
 
+ q̇3 .
cos(q1 + q2 + q3 )

The square sub-Jacobian made by the first two columns of J has det J 12 (q) = sin q2 + sin(q2 + q3 ).
As long as this determinant is different from zero, we can set ṗ = 0 in (9) and solve for q̇ 12 so as
to realize our self-motion task by

q̇ 12 = −J −1
12 (q)J 3 (q) q̇3 , (10)

for any motion q̇3 , in particular that given by (8). To introduce more robustness in the task of
keeping the end-effector position at pin , we replace

ṗ = ṗin = 0 ⇒ ṗ = ṗin + K P (pin − f (q)) = K P (pin − f (q)) , (11)

with a (diagonal) 2 × 2 gain matrix K P > 0 weighting the position error. The final control solution
is obtained by using (8) and (11) in eq. (9) and solving again for q̇ 12 :

q̇ 12 = J −1
12 (q) (K P (pin − f (q)) − k3 J 3 (q) (q3,f in − q3 )) . (12)

We can also combine (8) and (12) in a single formula as

!
J −1 −1
12 (q) −J 12 (q) J 3 (q) K P (pin − f (q))
  
q̇ 12
q̇ = = T . (13)
q̇3 0 1 k3 (q3,f in − q3 )

The last thing to check is the absence of singularities for J 12 (q) during the self-motion under the
feedback control law (13). It can be shown that det J 12 (q) = sin q2 + sin(q2 + q3 ) = 0 if and only
if the end-effector of the 3R robot finds itself aligned with the first link of the structure. From the
illustration in Fig. 9, it is rather evident that such condition is not encountered in this task. 

∗∗∗∗∗

17