Robotics 2

Remote Exam – July 15, 2020

Exercise #1
Consider the 4-dof robot in Fig. 1, made by a 3R planar arm mounted on a rail. The robot has
the last three links of equal length `. The generalized coordinates q ∈ R4 to be used are also
shown. Determine the inertia matrix M (q) of the dynamic model of this robot (if needed, define
symbolically any missing parameters).
•
yw P
m4,I4 ⊕

m2,I2 q3 q4
⊕
⊕ q2
m3,I3

q1 h
⊕ m1
xw

Figure 1: A 4-dof planar robot with generalized coordinates q and relevant parameters.
Exercise #2
For the same robot in Fig. 1, assume ` = h = 1 [m] and consider the following tasks, to be executed
using a kinematic control scheme with joint velocity commands q̇ ∈ R4 .
Task 1. Trace with the end-effector counterclockwise a circle of radius R = 3 [m], centered at
C = (7, 0), starting from point P 0 = (10, 0) and with constant speed v = 1 [m/s].
Task 2. Keep the second link always horizontal (q2 (t) = 0).
Define the augmented Jacobian J A (q) for both tasks 1 and 2. Choose a suitable initial robot
configuration so as to stay at time t = 0 in P 0 and compute there the minimum joint velocity
norm solution that realizes both tasks simultaneously. Determine the first point P s on the circular
path where an algorithmic singularity of J A (q) necessarily occurs. In that situation, compute the
minimum joint velocity norm solution that realizes the first task only. Will the execution of the
second task be relaxed or not?
Exercise #3
A 6-dof robot should hold firmly with its three-fingered gripper a cylindric payload, and move it
along a desired path on a frictionless plane with one of its bases in full contact with the plane, as
shown in Fig. 2. Define an associated task frame where the natural (geometric) constraints and
the artificial (control) constraints of this hybrid task can be defined and realized in a decoupled
way. Where reasonable, provide also values for the control references.

Figure 2: The hybrid task of moving along a path a cylinder in contact with a planar surface.

1
Exercise #4
During the accurate execution of a smooth joint trajectory q d (t) lasting T = 3 [s] with the 2R
planar robot shown in Fig. 3 moving under gravity, the maximum torques of the two joints exceed
at some instants their bounds, as given by |τi | ≤ τmax,i , i = 1, 2. We have in particular

τ1 (t1 ) = max τ1 (t) = 140 > 100 = τmax,1 , τ2 (t2 ) = max τ2 (t) = 25 > 20 = τmax,2 [Nm].
t∈[0,T ] t∈[0,T ]

The robot links have equal length ` = 0.5 [m] and equal, uniformly distributed mass m = 5 [kg].
The robot configurations at the time instants t = t1 and t = t2 are

T
T
q(t1 ) = π/4 0 , q(t2 ) = −π/4 3π/4 [rad].

In order to recover motion feasibility, a uniform trajectory scaling is used. What will be the
minimum feasible motion time T 0 = kT thus obtained? What are the values of the new joint
torques τ1 (kt1 ) and τ2 (kt2 )?
y0

g0
x0

Figure 3: A 2R robot moving under gravity. Joint variables are defined by the D-H convention.

Exercise #5
Consider again the 2R robot in Fig. 3, with the same definition of joint variables and using the
same kinematic and dynamic parameters. The robot is initially at rest at t = 0 in q(0) = q 0 .
Provide the explicit expressions of all terms in the following three feedback control laws, each
achieving its own objective.
a. Global exponential stabilization of the state (q, q̇) = (q d , 0), with decoupled transient evolu-
tions of the position errors ei (t) = qd,i −qi (t), i = 1, 2, of the form e1 (t) = e1 (0) (1 + 5t) exp(−5t)
and e2 (t) = e2 (0) (2 exp(−5t) − exp(−10t)).
b. Global asymptotic stabilization of the state (q, q̇) = (q d , 0), without knowledge of the robot
inertia matrix.
c. Exponential stabilization of the end-effector position p = pd ∈ R2 with zero velocity ṗ = 0, up
to kinematic singularities.

[180 minutes (3 hours); open books]

2
 Solution
July 15, 2020

Exercise #1
Note first that we are using the absolute angles w.r.t. to the x0 -axis for the orientation of the second
to the fourth link (thus, not the Denavit-Hartenberg relative angles). Also, denote by dci > 0 the
distance of the center of mass of link i from joint i, for i = 2, 3, 4. The individual contributions to
the kinetic energy of this 4-dof planar robot are computed as follows.

T1 = 21 m1 q̇12
q̇1 − dc2 sin q2 q̇2
   
q1 + dc2 cos q2
pc2 = ⇒ v c2 =
dc1 sin q2 dc2 cos q2 q̇2
1 1
2 2 2 2


T2 = 2 I2 q̇2 + 2 m2 q̇1 + dc2 q̇2 − 2dc2 sin q2 q̇1 q̇2
q̇1 − ` sin q2 q̇2 − dc3 sin q3 q̇3
   
q1 + ` cos q2 + dc3 cos q3
pc3 = ⇒ v c3 =
` sin q2 + dc3 sin q3 ` cos q2 q̇2 + dc3 cos q3 q̇3
T3 = 12 I3 q̇32 + 12 m3 q̇12 + `2 q̇22 + d2c3 q̇32 − 2` sin q2 q̇1 q̇2 − 2dc3 sin q3 q̇1 q̇3 + 2dc3 ` cos(q3 − q2 ) q̇2 q̇3

q̇1 − `(sin q2 q̇2 + sin q3 q̇3 ) − dc4 sin q4 q̇4

   
q1 + `(cos q2 + cos q3 ) + dc4 cos q4
pc4 = ⇒ v c4 =
`(sin q2 + sin q3 ) + dc4 sin q4 `(cos q2 q̇2 + cos q3 q̇3 ) + dc4 cos q4 q̇4
T4 = 21 I4 q̇42 + 12 m4 q̇12 + `2 (q̇22 + q̇32 + 2 cos(q3 − q2 ) q̇2 q̇3 ) + d2c4 q̇42
−2`(sin q2 q̇2 + sin q3 q̇3 )q̇1 − 2dc4 sin q4 q̇1 q̇4 + 2dc4 `(cos(q4 − q2 ) q̇2 + cos(q4 − q3 ) q̇3 ) q̇4 ) .

From
4 4 4
X 1 T 1 XX
T = Ti = q̇ M (q)q̇ = M ij (q) q̇i q̇j ,
i=1
2 2 i=1 j=1

the elements M ij = M ji of the 4 × 4 symmetric inertia matrix M (q) of this robot are

M 11 = m1 + m2 + m3 + m4
M 12 = −(m2 dc2 + (m3 + m4 )`) sin q2
M 13 = −(m3 dc3 + m4 `) sin q3
M 14 = −m4 dc4 sin q4
M 22 = I2 + m2 d2c2 + (m3 + m4 )`2
M 23 = (m3 dc3 + m4 `)` cos(q3 − q2 )
M 24 = m4 dc4 ` cos(q4 − q2 )
M 33 = I3 + m3 d2c3 + m4 `2
M 34 = m4 dc4 ` cos(q4 − q3 )
M 44 = I4 + m4 d2c4 .

Exercise #2
The kinematics of the first task of dimension m1 = 2 is given by
 
q1 + `(cos q2 + cos q3 + cos q4 )
r1 = p = = f 1 (q),
h + `(sin q2 + sin q3 + sin q4 )

3
with Jacobian
−` sin q2 −` sin q3 −` sin q4
 
∂f 1 (q) 1
J 1 (q) = = ,
∂q 0 ` cos q2 ` cos q3 ` cos q4
while the kinematics of the second task of dimension m2 = 1 is given just by
r2 = q2 = f2 (q),
with Jacobian
∂f2 (q)

J2 = = 0 1 0 0 ,
∂q
The augmented Jacobian is then
 
  1 −` sin q2 −` sin q3 −` sin q4
J 1 (q)
J A (q) = =0 ` cos q2 ` cos q3 ` cos q4  .
 
J2
0 1 0 0
It is easy to verify that J A (q) is singular, i.e., rank (J A (q)) < 3 = mA (= m1 + m2 ), if and only if
cos q3 = cos q4 = 0. This happens when the third and fourth link are aligned (or folded) along the
y w direction.
With reference to Fig. 4, the augmented task requires
 vt   vt 
cos − sin
R  R 
r 1d (t) = C + R   ⇒ ṙ 1d (t) = v 
 
vt vt

sin cos
R R

T
with r 1d (0) = P 0 , ṙ 1d (0) = 0 v and kṙ 1d (t)k = v, as well as
r2d (t) = 0 ⇒ ṙ2d = 0.
Setting now ` = h = 1, in order to be consistent with the augmented task at the initial time
t = 0, there will be multiple robot configurations (q1 , q3 , q4 ) that satisfy the desired end-effector
positioning with q2 = 0 (second link horizontal and pointing to the right):
   
q1 + 1 + cos q3 + cos q4 10
= = P0
sin q3 + sin q4 0

Ps
•
v
yw yw
q4 = p/2
R=3
R=3

q3 = p/2
ℓ = 1 q3 = 0

q2 = 0 q4 = - p/2
q1 = 8 q2 = 0
h=1 q1 = 6
v
xw
•
C P0 xw C P0

Figure 4: The 4-dof planar robot in the initial configuration q 0 [left] and in the singular configu-
ration q s [right] for the two tasks of tracing the circle with its end effector (task 1) while keeping
the second link horizontal (task 2).

4
A simple choice is to pick q1 = 8 [m], q3 = 0, q4 = −π/2 [rad], as in Fig. 4 [left]. The configuration

T
q 0 = q(0) = 8 0 0 −π/2 is not singular. Accordingly, any augmented task velocity
ṙ d ∈ R3 can be instantaneously realized (actually in a infinite number of ways). The minimum
norm joint velocity solution is obtained using pseudoinversion (J # T T −1
A = J A (J A J A ) ):
   
 #   0.5 0 0   0
1 0 0 1 0  0 0 1  0 0
q̇ 0 = J #
A (q 0 )ṙ d (0) =  0 1 1 0   1  =   1  =  .
        
 0 1 −1  1
0 1 0 0 0 0
0.5 0 0 0

As a result, only the third joint moves, rotating counterclockwise. As shown in Fig. 4 [right],
the first point on the circle where the augmented task necessarily encounters a singularity is at
P s = (C x , C y + R) = (7, 3). The robot arrives there at some instant t = ts > 0 and can satisfy the

T
positional/orientation tasks in only one configuration q s = q(ts ) = 6 0 π/2 π/2 , which
is indeed singular. Note that this is a true algorithmic singularity, since both tasks are full rank
(rank (J 1 (q s )) = 2, rank (J 2 ) = 1) but rank (J A (q s )) = 2 < 3 = mA . Indeed, one can still
compute the pseudoinverse solution, which provides
   
 #   0.3333 0 0   −1
1 0 −1 −1 −1 −1
 0 0.5 0.5   1 0 
 
q̇ s = J # (q )ṙ (t ) = 0 1 0 0 0 =  0  =  .
     
A s d s
 −0.3333 0 0  3 1 
    
0 1 0 0 0 0
−0.3333 0 0 1

The prismatic joint retracts, while joints 3 and 4 will rotate counterclockwise. When evaluating
the execution of the augmented task with this joint velocity, we find
 
  −1  
1 0 −1 −1 −1
 1 0  
 
J A (q s )q̇ s =  0 1 0 0 ·   =  0  = ṙ d (ts )!
 
3 1 
0 1 0 0 0
1

Thus, the entire velocity task is still satisfied. In fact, despite the loss of rank of the augmented
Jacobian, it is easy to see that ṙ d (ts ) ∈ R {J A (q s )}. The pseudoinverse joint velocity returns then
the exact solution also in this case. We note finally that the robot will not be able to trace the
entire circle, being the lower part outside its workspace.

Exercise #3
With reference to Fig. 5, we define the task frame with axis z t normal to the plane of motion and
passing through the center of the cylinder base, and axis xt tangential to the path on the plane.
The natural constraints are then

fx = 0, fy = 0, vz = 0, ωx = 0, ωy = 0, µz = 0,

in which we neglected any friction effect at the contact. The complementary artificial constraints
are

vx = vx,d (t) > 0, vy = 0, fz = fz,d 6= 0, µx = 0, µy = 0, ωz = ωz,d .

The value of the velocity vy (normal to the path) is chosen to be zero, signifying that the robot
end-effector/payload should strictly follow the path on the plane. A non-zero fz,d can be chosen

5
 zt
yt
xt

Figure 5: The task frame assignment for the contact motion of a cylinder following a path on a
frictionless plane.

so as to enforce full surface contact with the base, despite of the presence of friction (and other
disturbances) in the real world. The two reaction torques µx and µy are set to zero, in order not
to stress the object while in contact. Finally, ωz,d can be set to zero or not, depending on whether
the cylinder should keep its orientation or rotate around its major axis while the center of its base
is following the path traced on the plane.

Exercise #4
In this exercise, we just need to derive the gravity term in the dynamic model of the 2R planar
robot. No information is required in fact on the inertial terms. Using the Denavit-Hartenberg
coordinates, the q = 0 configuration will correspond to the robot arm being stretched downward
along the x0 -axis, the configuration of minimum potential energy. Therefore, being m1 = m2 = m
and dc1 = dc2 = `/2, the potential energy due to gravity is

U = U1 + U2 = −m1 g0 dc1 cos q1 − m2 g0 (` cos q1 + dc2 cos(q1 + q2 ))

 
3 1
= −mg0 ` cos q1 + cos(q1 + q2 ) ,
2 2

and so  
3 1

!
 mg0 ` sin q1 + sin(q1 + q2 )
T
g1 (q)

∂U (q) 2 2
g(q) = = = . (1)

∂q ` g2 (q)
mg0 sin(q1 + q2 )
2
Setting m = 5 [kg], ` = 0.5 [m] and g0 = 9.81 [m/s2 ], by evaluating (1) at q(t1 ) = (π/4, 0) and
q(t2 ) = (−π/4, 3π/4) we obtain the gravity torques at the two joints

g1 (q(t1 )) = 34.6836 [Nm] and g2 (q(t2 )) = 12.2625 [Nm].

The uniform time scaling factor k > 1 needed to recover feasibility of the entire motion is computed
from s s
τ1 (t1 ) − g1 (q(t1 )) τ2 (t2 ) − g2 (q(t2 ))
k1 = = 1.2698, k2 = = 1.2830,
τmax,1 − g1 (q(t1 )) τmax,2 − g2 (q(t2 ))
as
k = max{k1 , k2 } = 1.2830 (= k2 ).

6
Thus, the second joint is the one with higher relative violation of the torque limit (once gravity is
removed). The motion time is then increased from T = 3 [s] to the new value T 0 = kT = 3.8491 [s],
which is the minimum feasible one under uniform time scaling. The values of the new joint torques
(expressed in [Nm]) at the scaled instants t01 = kt1 and t02 = kt2 are computed as
τ1 (t1 ) − g1 (q(t1 ))
τ1 (t01 ) = + g1 (q(t1 ) = 98.6589 < 100 = τmax,1 ,
k2
τ2 (t2 ) − g2 (q(t2 ))
τ2 (t02 ) = + g2 (q(t2 ) = 20 = τmax,2 .
k2
As expected, the second joint torque will be in saturation at the scaled instant t02 .

Exercise #5
The three requested motion tasks are all regulation problems. The dynamic terms needed for the
various feedback control laws are listed first. We make reference to the 2R robot in Fig. 3, with
the same definition of joint variables and using the same parameters. The inertia matrix is
 
a1 + 2a2 cos q2 a3 + a2 cos q2
M (q) = ,
a3 + a2 cos q2 a3
3 1
with a1 = I1 + I2 + 2 m`2 , a2 = m`2 , a3 = I2 + 14 m`2 . The Coriolis and centrifugal terms are
2

−a2 sin q2 (q̇2 + 2q̇1 )q̇2

 
c(q, q̇) = .
a2 sin q2 q̇12

The gravity vector has been already computed in Exercise #4, and is rewritten here as
 
a4 sin q1 + a5 sin(q1 + q2 )
g(q) =
a5 sin(q1 + q2 )

with a4 = 21 mg0 `, a5 = 21 mg0 `. From the direct kinematics p = f (q), the robot Jacobian that
maps the joint velocity q̇ ∈ R2 to the velocity ṗ ∈ R2 of the end effector is

−`(sin q1 + sin(q1 + q2 )) −` sin(q1 + q2 )

 
∂f (q)
J (q) = = .
∂q `(cos q1 + cos(q1 + q2 )) ` cos(q1 + q2 )

Finally, we shall need also the time derivative of the Jacobian matrix, namely

−` (cos q1 q̇1 + cos(q1 + q2 ) (q̇1 + q̇2 )) −` cos(q1 + q2 ) (q̇1 + q̇2 )

 
J̇ (q) = .
−` (sin q1 q̇1 + sin(q1 + q2 ) (q̇1 + q̇2 )) −` sin(q1 + q2 ) (q̇1 + q̇2 )

a. Global exponential stabilization of the state (q, q̇) = (q d , 0), with decoupled transient evolu-
tions of the position errors ei (t) = qd,i −qi (t), i = 1, 2, of the form e1 (t) = e1 (0) (1 + 5t) exp(−5t)
and e2 (t) = e2 (0) (2 exp(−5t) − exp(−10t)).
This is obtained by feedback linearization control in the joint space:

u = M (q)a + c(q, q̇) + g(q), with a = −K D q̇ + K P (q d − q),

with K P > 0 and K D > 0 and both diagonal. The desired error transients are obtained by
choosing suitable gains in the linear and decoupled second-order dynamics

ëi + KD,i ėi + KP,i ei = 0, i = 1, 2, (2)

7
for the two position errors ei (t) = qd,i −qi (t). For joint 1, substitute e1 (t) = e1 (0) (1 + 5t) exp(−5t)
and its first and second time derivatives in (2):

e1 (0) (−25 + 125t) exp(−5t) − KD,1 e1 (0) 25t exp(−5t) + KP,1 e1 (0) (1 + 5t) exp(−5t) = 0.

Since e1 (0) exp(−5t) 6= 0 for any finite t ≥ 0, this common factor can be eliminated so as to obtain

(−25 + 125t) − 25KD,1 t + (1 + 5t) KP,1 = 0.

By the principle of polynomial identity (w.r.t. the powers of t), this implies

125 − 25 KD,1 + 5 KP,1 = 0, −25 + KP,1 = 0 ⇒ KP,1 = 25, KD,1 = 10.

Moreover, transforming eq. (2) for i = 1 in the Laplace domain and using these values leads to

s2 + 25s + 10 e1 (s) = (s + 5)2 e1 (s) = 0,

namely, the error dynamics at the first joint is characterized by two real and coincident negative
eigenvalues in −5.
We proceed similarly for joint 2. Substitute e2 (t) = e2 (0) (2 exp(−5t) − exp(−10t)) and its first
and second time derivatives in (2):

e2 (0) (50 exp(−5t) − 100 exp(−10t)) + KD,2 e2 (0) (−10 exp(−5t) + 10 exp(−10t))
+ KP,2 e2 (0) (2 exp(−5t) − exp(−10t)) = 0.

Being e2 (0) 6= 0, in order for this expression to vanish identically at all times t ≥ 0, we should zero
the coefficients multiplying the two different exponentials exp(−5t) and exp(−10t). This yields

50 − 10 KD,2 + 2 KP,2 = 0, −100 + 10 KD,2 − KP,2 = 0 ⇒ KP,2 = 50, KD,2 = 15.

Moreover, transforming eq. (2) for i = 2 in the Laplace domain and using these values leads to

s2 + 15s + 50 e2 (s) = (s + 5)(s + 10) e2 (s) = 0,

namely, the error dynamics at the second joint has two real and distinct negative eigenvalues in
−5 and −10.

b. Global asymptotic stabilization of the state (q, q̇) = (q d , 0), without knowledge of the robot
inertia matrix.
This can be obtained by multiple choices, the most common being a PD control with gravity
cancellation
u = K P (q d − q) − K D q̇ + g(q),
with symmetric K P > 0 and K D > 0, typically chosen diagonal. In alternative, one can use
gravity compensation
u = K P (q d − q) − K D q̇ + g(q d )
further requiring that the minimum eigenvalue of K P is larger than a finite upper bound α > 0
on the norm of the Hessian ∂ 2 U (q)/∂q 2 of the gravitational potential energy U .

8
c. Exponential stabilization of the end-effector position p = pd ∈ R2 with zero velocity ṗ = 0, up
to kinematic singularities.

In this case, we require a feedback linearization control in the Cartesian space:

 
u = M (q)J −1 (q) a − J̇ (q)q̇ + c(q, q̇) + g(q),

with
a = −K D ṗ + K P (pd − p) = −K D J (q)q̇ + K P (pd − f (q))
and where K P > 0 and K D > 0 are both chosen diagonal.

∗∗∗∗∗