Robotics 1

June 12, 2024

Exercise 1
Let an initial and a final orientation be specified by the two matrices
   √ √ 
0 0 1 √2/2 0 √2/2
Ri =  0 1 0  Rf =  2/2 0 − 2/2  .
−1 0 0 0 1 0

Using the ZXZ Euler angles φ = (α, β, γ), generate a trajectory φ(t), with t = [0, T ], which
interpolates these two orientations in T = 1.5 s, with initial and final angular velocity given by
   
0 0
ωi =  1  ω f =  0  [rad/s].
0 1

Provide the value of the resulting angular velocity ω(t) at the midtime t = T /2. Does the found
trajectory φ(t) cross any representation singularity? Is it the unique solution to this problem in
the class of interpolating trajectories using ZXZ Euler angles?

Exercise 2
Table 1 contains the Denavit-Hartenberg (D-H) parameters of a robot with three revolute joints.

i αi ai di θi

1 π/2 0 0 θ1

2 0 a2 > 0 0 θ2

3 0 a3 > 0 0 θ3

Table 1: D-H parameters of a 3R robot

• Sketch a skeleton of this robot and of its reachable workspace.

• Assign the frames to the robot links according to the above table.
• Compute the 3 × 3 Jacobian matrix J (θ) associated to the velocity of the origin of the last D-H
frame and determine all its singularities.
• In a singular configuration θ s , let J s = J (θ s ); find a basis for the subspaces N (J s ) and R(J s )
and provide an interpretation in terms of robot motion.

Exercise 3
Consider a digital camera with a number of pixel W × H = 720 × 524 on the image plane and
having a lens with focus f = 8 mm. Each pixel is square with size d = 7µm. Assuming a pinhole
camera model, which are the horizontal and vertical spatial resolutions (expressed in mm) of points
on a plane parallel to the image plane and placed at a distance L = 2 m from the lens center?
What is the angular field of view (in degrees) on the horizontal plane of this camera-lens system?

1
Exercise 4
For a 3R planar robot with unit length links, can you define a circular path of diameter d = 1 m for
its end-effector position p ∈ R2 so that the robot will certainly trace such path without crossing a
singular configuration? If so, provide an example. If not, explain why.

Exercise 5
Consider a point-to-point path planning problem for a 2R planar robot with unit length links.
The robot should move its end-effector between the two Cartesian positions pi = (0.6, −0.4) and
pf = (1, 1) [m]. Moreover, the Cartesian path should have tangent direction at the start and at
the end specified respectively by the vectors p0i = (−2, 0) and p0f = (2, 2), where p0 = dp/ds.

• Define a solution path q(s) directly in the joint space.

• Within the chosen class of interpolating functions, how many solution paths exists? Does any
of these paths cross a kinematic singularity?
• On the chosen solution path, define a rest-to-rest timing law s(t) that completes the motion in
T = 3 s and has continuous acceleration s̈(t) in the (open) time interval (0, T ).
• What is the value of the resulting joint velocity q̇(t) at the midtime t = T /2?
• What is the value of the resulting end-effector velocity v(t) = ṗ(t) at the midtime t = T /2?

[240 minutes (4 hours); open books]

2
 Solution
June 12, 2024

Exercise 1
Using the inverse relationships for the representation of orientation with the ZXZ Euler angles
φ = (α, β, γ), each of the two rotation matrices Ri and Rf can be transformed in a pair of solution
2 2
triples, being the regularity condition R13 + R23 6= 0 on the elements Rij of both matrices satisfied.
Therefore, there are four possible combinations for the boundary conditions of the interpolation
problem when using this minimal representation of orientation.
The regularity condition is equivalent to having sin β 6= 0, or β 6= {0, ±π}, in the triple. Thus,
each pair of solutions is characterized by a positive or a negative value for β, depending on the
choice of the sign in the four-quadrant arctangent function that is used to compute this value.
As a result, a trajectory that interpolates the initial and final values of β, in the usual domain
(−π, π] of definition, will avoid crossing a singularity if and only if both values βi and βf will have
the same sign. Moreover, the amplitude of the needed change for the Euler angles is likely to be
smaller when β(t) does not change sign along the trajectory. These remarks reduce the interesting
combinations of boundary conditions to two; the following solution will present just one of them.
From
 q     
2 2 R31 R32 R13 R23
β = atan2 + R13 + R23 , R33 α = atan2 , γ = atan2 ,− ,
sin β sin β sin β sin β
one obtains        
αi π/2 αf π/4
φi =  βi  =  π/2  φf =  βf  =  π/2  .
γi −π/2 γf 0

The interpolating trajectory has to satisfy also boundary conditions on the initial and final angular
velocity. The transformation between φ̇ and ω is found to be
    
ωx 0 cos α sin α sin β α̇
ω =  ωy  = T (φ) φ̇ =  0 sin α − cos α sin β   β̇  .
ωz 1 0 cos β γ̇

In fact, since the rotation matrix associated to the ZXZ Euler angles is R = Rz (α)Rx (β)Rz (γ),
the three individual contributions of α̇, β̇ and γ̇ to the angular velocity ω are given by

ω α̇ = z α̇ ω β̇ = Rz (α) x β̇ ω γ̇ = Rz (α)Rx (β) z γ̇,

T
T
which build the three columns of T (φ) (above, we used z = 0 0 1 and x = 1 0 0 ).
Evaluating T with the obtained φi and φf , one has
   √ √ 
0 0 1 0 √2/2 √2/2
T (φi ) =  0 1 0  T (φf ) =  0 2/2 − 2/2  ,
1 0 0 1 0 0

and thus   

0 1
φ̇i = T −1(φi ) ω i =  1  φ̇f = T −1(φf ) ω f =  0  .
0 0

3
 ZXZ Euler angles: , (black), - (red), . (blue) ZXZ Euler angles derivatives: d,/dt (black), d -/dt (red), d . /dt (blue)
2
3

1.5
2

1
1

0.5

[rad/s]
[rad]

-1
-0.5

-2
-1

-3
-1.5
0 0.5 1 1.5 0 0.5 1 1.5
time [s] time [s]

Figure 1: The components of the interpolation trajectory φ(t) and of its derivative φ̇(t).

The interpolation problem has the four boundary conditions

φ(0) = φi φ̇(0) = φ̇i φ(T ) = φf φ̇(T ) = φ̇f ,

with T = 1.5 s, and can be solved in a unique way when using a cubic polynomial. In its normalized
form with τ = t/T , the interpolating trajectory for τ ∈ [0, 1] is
     
φ(τ ) = φi + φ̇i T τ + 3 ∆φ − (φ̇f + 2φ̇i )T τ 2 + −2 ∆φ + (φ̇f + φ̇i )T τ 3 ,

where  
−π/4
∆φ = φf − φi =  0  .
π/2
Substituting the numerical data, the trajectories of the Euler angles for τ ∈ [0, 1] are (in [rad]):

α(τ ) = 1.5708 − 3.8562 τ 2 + 3.0708 τ 3

β(τ ) = 1.5708 + 1.5 τ − 3 τ 2 − 1.5 τ 3
γ(τ ) = −1.5708 + 4.7124 τ 2 − 3.1416 τ 3 .

angular velocity components: ! x (black), ! y (red), ! z (blue) norm of angular velocity

1.5 2.2

2
1

1.8

0.5
1.6
[rad/s]

[rad/s]

0 1.4

1.2
-0.5

-1
0.8

-1.5 0.6
0 0.5 1 1.5 0 0.5 1 1.5
time [s] time [s]

Figure 2: The components of the angular velocity vector ω(t) and its norm.

4
The time evolutions of the three components of the trajectory φ(t) and of its derivative φ̇(t) are
shown in Fig. 1. The angular velocity along the trajectory is computed as ω(t) = T (φ(t)) φ̇(t) and
is shown in Fig. 2, together with its norm. At t = T /2 = 0.75 s, it is
 
1.1537
ω(T /2) =  −1.0551  [rad/s].
−1.3282

Exercise 2
A possible structure described by Tab. 1 is a 3R spatial robot, as sketched in Fig. 3 with the
associated D-H frames. Since the size/length of the robot base is irrelevant for the kinematics, the
first joint could also be placed on the ground; this is also the meaning of having a1 = d1 = 0 in
the D-H table.

y2
z0 = y1 x2
y3
𝑎!
z2
𝑎# x3
z1 x1
x0 z3

Figure 3: A 3R spatial robot with D-H frame assignment according to Tab. 1.

The reachable workspace is a sphere centered at the robot shoulder (i.e., at O0 = O1 ) and with
radius R = a2 + a3 > 0. If a2 6= a3 , an inner sphere of radius r = |a2 − a3 | > 0 is removed from
the workspace.
The direct kinematics in position is
 
c1 (a2 c2 + a3 c23 )
p = f (θ) =  s1 (a2 c2 + a3 c23 )  .
a2 s2 + a3 s23

The associated 3 × 3 Jacobian is

 
−s1 (a2 c2 + a3 c23 ) −c1 (a2 s2 + a3 s23 ) −a3 c1 s23
∂f
J (θ) = =  c1 (a2 c2 + a3 c23 ) −s1 (a2 s2 + a3 s23 ) −a3 s1 s23  .
∂θ
0 a2 c2 + a3 c23 a3 c23

To simplify the study of the singularities of J , it is convenient to express the Jacobian in the
rotated frame 1, namely
   
c1 s1 0 0 − (a2 s2 + a3 s23 ) −a3 s23
1
J (θ) = RT1 (θ1 ) J (θ) =  0 0 1  J (θ) =  0 a2 c2 + a3 c23 a3 c23  .
s1 −c1 0 − (a2 c2 + a3 c23 ) 0 0

5
Thus, for the determinant we have

det J (θ) = det 1J (θ) = a2 a3 s3 (a2 c2 + a3 c23 ) ,

and singularities occur when:

a) s3 = 0: the forearm is fully stretched (θ3 = 0) or fully folded (θ3 = ±π);
q
b) a2 c2 + a3 c23 = 0: being this equal to p2x + p2y = 0, the end-effector is located on the z 0 axis;
c) both above situations hold: the robot is stretched or folded vertically along the z 0 axis.
Consider for instance case a) and set θ s = (θ1 , θ2 , 0). Then
 
− (a2 + a3 ) s1 c2 − (a2 + a3 ) c1 s2 −a3 c1 s2
J s = J (θ s ) =  − (a2 + a3 ) c1 c2 − (a2 + a3 ) s1 s2 −a3 s1 s2  .
0 (a2 + a3 ) c2 a3 c2

It is easy to find a basis for each of the two requested subspaces:

     
0  s1 c2 −c1 s2 
N (J s ) =  −a3  R(J s ) = span  c1 c2  ,  −s1 s2  .
a2 + a3 0 c2
 

As a result, in this singular configuration, the robot end effector will remain at rest when the first
joint does not move while the second and third joints move in opposite directions with instantaneous
velocities scaled by the relative factor a3 /(a2 + a3 ) (motion in the null space). If the two links have
unit length (a1 = a2 = 1), then joint 3 will move instantaneously at twice the (opposite) velocity
of joint 2.
On the other hand, to better visualize the range space motion at θ s , consider the rotated Jacobian
in the chosen singular configuration
 
0 − (a2 + a3 ) s2 −a3 s2
1
J s = 1J (θ s ) =  0 (a2 + a3 ) c2 a3 c2  .
− (a2 + a3 ) c2 0 0

Then    
 0 −s2 
R(1J s ) = span  0  ,  c2  .
c2 0
 

Therefore, when observing the end-effector motion in frame (x1 , y 1 , z 1 ) with the robot in this
singular configuration, an instantaneous velocity of the first joint produces only a velocity in the
z 1 (horizontal) direction; the second and third joint can move the end effector only along a single
direction in the plane (x1 , y 1 ). If the two links have unit length and we set θ2 = 0, a unit velocity
of joint 1 gives an end-effector velocity 1 v e = (0, 0, 2) [m/s]; a unit velocity of joint 2 (or of joint
3) gives 1 v e = (0, 2, 0) [m/s] (or 1 v e = (0, 1, 0) [m/s]).

Exercise 3
Let a point c P = (X, Y, Z) be expressed in the camera frame, which is placed on the physical
image plane of the camera, i.e., behind the lens and at a focal distance f . The pinhole model maps
point P into the image plane point p = (u, v) (horizontal and vertical coordinates) as
X Y
u = u0 − f v = v0 − f .
Z Z

6
where (u0 , v0 ) corresponds to the center point of the image on the lens axis (the origin of the
camera coordinates is usually at the top-left of the image plane). Accordingly, a displacement
∆ = (∆X, ∆Y ) of point P on the plane at a distance Z = L + f from the camera frame will result
in variations of the coordinates of the point p on the image plane given by
∆X ∆Y
∆u = −f ∆v = −f .
L+f L+f
Therefore, being the physical size of the square pixels ∆umin = ∆vmin = 7µm = 7 · 10−3 mm, the
spatial resolution of a point P in the plane at Z = L + f (namely, the minimum displacement
detectable by the camera) is
L+f 2008
∆Xmin (= ∆Ymin ) = ∆umin = 7 · 10−3 · = 1.757 mm,
f 8
equal in the horizontal and vertical case.
The physical size of the sensor is W d × Hd = 720 · 7 × 524 · 7 µm = 5.040 × 3.668 mm. The angular
field of view (FOV) on the horizontal plane of this camera-lens system, expressed in degrees, is
W d/2 360◦ W d/2 2.520
FOVH = 2 arctan [rad] = · arctan = 114.59 [◦/rad] · arctan ' 36◦ .
f π f 8

y0

P
d=1 x0

r=1
R=3

Figure 4: A circular path traced without singularities by a 3R planar robot with unit length links.

Exercise 4
The answer is yes. With reference to Fig. 4, a 3R planar robot is in a singular configuration for
a task involving only its end-effector position (with a 2 × 3 Jacobian) when the three links are
stretched or folded along a single radial direction from the robot base, i.e., when θ2 = {0, ±π} and
θ3 = {0, ±π}. When the links have unit length, these singular configurations correspond to the
external boundary of the workspace (a circle of radius R = 3 m that the robot end effector can
reach only in the fully stretched and singular configuration) and to a circle of radius r = 1 m inside
the workspace (where the robot is in singularity only if folded along a radial direction). Therefore,
a circular path of diameter d = 1 m (i.e., of radius d/2 = 0.5 m) for the end-effector can be defined

7
safely (and in many ways!) inside the circle of unit radius, and certainly no singular configurations
will be encountered.

Exercise 5
Being the given two Cartesian positions pi and pf strictly inside the reachable workspace of the
2R planar robot (a circle of radius rmax = 2, whereas kpi k = 0.7211 and pf = 1.4142), they do
not correspond to singular configurations (q2 = 0 or ±π). Thus, there are two regular solutions to
the inverse kinematics problem for these positions. We choose in both cases the elbow-down (viz.
right-arm) solution, namely
   
−1.7899 0
qi = qf = [rad].
2.4039 π/2

By doing so, we choose one out of the four possible combinations of boundary conditions for the
interpolating joint path to be defined (and, correspondingly, four classes of trajectories). Moreover,
choosing the initial and final inverse solutions in the same class will guarantee that interpolating
joint paths that have no wandering (i.e., remain between the initial and final joint values) will
not cross a singular configuration.1 On the other hand, when choosing the two combinations with
values of the second joint of opposite signs, a singular configuration will certainly be encountered.
In order to invert the Cartesian tangent data, we need to compute the 2 × 2 analytical Jacobian
J of the robot. In fact, for the derivatives with respect to the path parameter s, one has
dp dq
p0 = = J (q) = J (q) q 0 ,
ds ds
just like for the transformation between joint velocity q̇ and end-effector velocity ṗ (which are
derivatives of the position p with respect to time t in the two spaces). Being
 
− sin q1 − sin(q1 + q2 ) − sin(q1 + q2 )
J (q) = ,
cos q1 + cos(q1 + q2 ) cos(q1 + q2 )

we have    
0.4000 −0.5761 −1 −1
J (q i ) = J (q f ) =
0.6000 0.8174 1 0
and thus
   
−2.4305 2
q 0i = J −1 (q i ) p0i = q 0f = J −1 (q f ) p0f = [rad].
1.7841 −4

The interpolation problem for a geometric path q(s), with s ∈ [0, 1], has the four boundary
conditions
q(0) = q i q 0 (0) = q 0i q(1) = q f q 0 (1) = q 0f ,
and can be solved using the cubic polynomial

q(s) = q i + q 0i s + 3 ∆q − (q 0f + 2q 0i ) s2 + −2 ∆q + (q 0f + q 0i ) s3 ,

where  
1.7899
∆q = q f − q i = [rad].
−0.8331/2
1 The situation is somewhat similar to Exercise 1.

8
Substituting the numerical data, the paths of the two joints for s ∈ [0, 1] are (in [rad]):

q1 (s) = −1.7899 − 2.4305 s + 8.2308 s2 − 4.0104 s3

q2 (s) = 2.4039 + 1.7841 s − 2.0674 s2 − −0.5498 s3 .

The two components of the joint path q(s) and of its derivative (tangent) q 0 (s) are shown in Fig. 5.
Since q2 (t) never crosses 0 (or ±π), the robot does not encounter a singular configuration. Figure 6
shows the Cartesian path corresponding to q(s).
joint path q(s) - joint 1 (black), joint 2 (blue) joint path tangent q'(s) - joint 1 (black), joint 2 (blue)
3 4

2.5
3

2
2
1.5

1
1

q 1', q 2' [rad]

q 1, q 2 [rad]

0.5 0

0
-1

-0.5
-2
-1

-3
-1.5

-2 -4
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
s s

Figure 5: The components of the joint path q(s) and of its derivative q 0 (s).

geometric path in Cartesian space

1.5

0.5
[m]

-0.5

-1

-1.5
-1.5 -1 -0.5 0 0.5 1 1.5
[m]

Figure 6: The Cartesian path p(s) (in blue) corresponding to q(s). Initial (green) and final (red)
robot postures and initial and final tangent directions to the Cartesian path are also shown.

To obtain a smooth rest-to-rest motion (strictly) inside the motion interval [0, T ], with T = 3 s,
one can use a cubic polynomial as timing law — see Fig. 7:
t 1 2 2 3
s(τ ) = 3 τ 2 − 2 τ 3 τ= ⇒ s(t) = t − t t ∈ [0, 3].
T 3 27
Combining the parametrized path q(s) with the timing law s(t) yields the desired trajectory q(t)
shown in Fig. 8. When comparing this with Fig. 5, one can appreciate the modulation of the joint

9
path motion obtained through the timing law — in particular, the obtained zero initial and final
velocities (horizontal tangents to q1 (t) and q2 (t) at t = 0 and t = 3 s) in face of nonzero tangents
at the path boundaries s = 0 and s = 1.
rest-to-rest timing law rest-to-rest speed
1 0.5

0.9 0.45

0.8 0.4

0.7 0.35

0.6 0.3

ds/dt [s - 1]
s(t)

0.5 0.25

0.4 0.2

0.3 0.15

0.2 0.1

0.1 0.05

0 0
0 0.5 1 1.5 2 2.5 3 0 0.5 1 1.5 2 2.5 3
time [s] time [s]

Figure 7: The rest-to-rest timing law s(t) and its speed ṡ(t).

joint trajectory q(t) - joint 1 (black), joint 2 (blue) joint velocity dq/dt - joint 1 (black), joint 2 (blue)
3 1.5

2.5

2 1

1.5
dq 1/dt, dq 2/dt [rad/s]

1 0.5
q 1, q 2 [rad]

0.5

0 0

-0.5

-1 -0.5

-1.5

-2 -1
0 0.5 1 1.5 2 2.5 3 0 0.5 1 1.5 2 2.5 3
time [s] time [s]

Figure 8: The two components of the resulting joint trajectory q(t) and velocity q̇(t).

The requested values at the trajectory midtime t = T /2 = 1.5 s (or τ = 0.5) are computed as
follows. For the midtime configuration q mid :
 
−1.4488
s(t = 1.5) = s(τ = 0.5) = 0.5 ⇒ q mid = q(t = 1.5) = q(s = 0.5) = [rad];
2.7103
for the midtime joint velocity q̇ mid :
   
0 2.7925 1.3963
q̇ mid = q̇(t = T /2) = q (s = 0.5) ṡ(t = T /2) = · 0.5 = [rad/s];
−0.6956 −0.3478
finally, for the midtime end-effector velocity ṗmid :
    
0.0400 −0.9526 1.3963 0.3872
ṗmid = J (q mid ) q̇ mid = = [m/s].
0.4260 0.3043 −0.3478 0.4890

∗∗∗∗∗

10