Robotics I

February 6, 2013

Exercise 1

Figure 1: A 4-DOF finger of a robotic hand (left) and the kinematics of the full hand (right). The
first three DOFs of the index finger are circled in red

Figure 1 shows a picture of the index finger of a prototype robotic hand, as well as the kinematics
scheme of the full hand. The index finger has 4 revolute degrees of freedom (DOFs), two at its
base, allowing abduction/adduction and flexion/extension, and two other for flexion/extension of
the phalanxes. For simplicity, assume that each joint is independently actuated. A reference frame
is placed at the base of the finger, attached to the palm of the hand (see Fig. 2).

X0!
Z0!
Y0!

Figure 2: Kinematics scheme of the 4-DOF index finger

a) Assign a set of Denavit-Hartenberg frames to the robotic finger and define the associated
table of parameters, introducing symbolic quantities as needed. The origin of the last frame
should be located at the tip of the finger.
b) By mimicking the mobility of the index finger of your hand, define reasonable numerical
values for the joint ranges of the robotic finger.
c) Derive the 6 × 4 geometric Jacobian of the robotic finger in symbolic form.

1
Exercise 2
Consider the class of trigonometric functions of time
m     
X π t π t
q(t) = ah sin (2h − 1) + bh cos (2h − 1) , t ∈ [0, T ] (1)
2T 2T
h=1

parametrized by the 2m coefficients ah , bh , for h = 1, . . . , m, and let a trajectory planning problem

be specified by the following boundary conditions to be interpolated

q(0) = q0 , q(T ) = q1 , q̇(0) = v0 , q̇(T ) = v1 , (2)

where T > 0 is the motion time.

a) Address problem (2) using (1), so that a solution trajectory q(t) always exists and is uniquely
specified by the given data. Write a program that computes such solution and plots the
resulting position, velocity, and acceleration profiles.
b) Test your program on the two data sets:

i) q0 = −40◦ , q1 = 40◦ , v0 = v1 = 0, T = 2 s;
ii) q0 = 20◦ , q1 = 60◦ , v0 = v1 = 0, T = 2 s.
Verify that in the second case the resulting trajectory wanders1 , while no under- or over-shoot
occurs to the position profile in the first case. Which is the source of this different behavior?

c) Expand the class of trajectories q(t) in (1) by adding a constant term, namely

q 0 (t) = q̄ + q(t). (3)

The previous method can be modified in a simple way so that, at least in the rest-to-rest
case (v0 = v1 = 0), the solution q 0 (t) will always be a non-wandering trajectory. Show how
to obtain this, and test the modified solution again on case ii) above.

[180 minutes; open books]

1 In general, wandering in case of a rest-to-rest trajectory between two points q and q means that the position
0 1
exceeds the interval [min{q0 , q1 }, max{q0 , q1 }] at some instant during motion.

2
 Solutions
February 6, 2013

Exercise 1

Figure 3 shows a possible assignment of Denavit-Hartenberg frames for the robotic finger. Table 1
contains the associated parameters, where L2 , L3 , and L4 are the lengths of the three phalanxes
of the finger.

X3!
X4!

X2!
Z4!
Z3!
Z2!
Z0! X1=X0!

Z1!

Figure 3: Denavit-Hartenberg frames for the 4-DOF index finger

i αi di ai θi
1 π/2 0 0 q1
2 0 0 L2 q2
3 0 0 L3 q3
4 0 0 L4 q4

Table 1: Denavit-Hartenberg parameters associated to Fig. 3

T
With the above choices, the zero configuration q = q1 q2 q3 q4 = 0 corresponds to the
finger pointing straight upward. Accordingly, possible estimates of the joint ranges that mimic the
mobility of a human index finger are

q1 ∈ [−20◦ , +20◦ ] , q2 ∈ [−10◦ , +80◦ ] , q2 ∈ [0◦ , +95◦ ] , q4 ∈ [0◦ , +45◦ ] .

The desired 6 × 4 Jacobian matrix is most efficiently computed as

∂p04 ∂p04 ∂p04 ∂p04
! !
J L (q) ∂q1 ∂q2 ∂q3 ∂q4
J (q) = = ,
J A (q) z0 z1 z2 z3

namely:

3
 • for the first three rows (linear components), by analytic derivation of the finger tip position
vector p04 ;
• for the last three rows (angular components), by the standard geometric expressions for
revolute joints.

From Table 1, we obtain the homogeneous transformation matrices

 
! cos q1 0 sin q1 0
R1 (q1 ) p01  sin q1 0 − cos q1 0 
A1 (q1 ) = =
 0

0T 1 1 0 0 
0 0 0 1

and
 
! cos qi − sin qi 0 Li cos qi
Ri (qi ) pi−1,i (qi )  sin qi cos qi 0 Li sin qi 
Ai (qi ) = =
 0
, for i = 2, 3, 4.
0T 1 0 1 0 
0 0 0 1

The position of the tip finger is then

    
0
p04,hom (q) = A1 (q1 ) A2 (q2 ) A3 (q3 ) A4 (q4 )
1

cos q1 (L2 cos q2 + L3 cos(q2 + q3 ) + L4 cos(q2 + q3 + q4 ))

 
!
 sin q1 (L2 cos q2 + L3 cos(q2 + q3 ) + L4 cos(q2 + q3 + q4 ))  p04 (q)
=  = .
 
 L2 sin q2 + L3 sin(q2 + q3 ) + L4 sin(q2 + q3 + q4 )  1
1

In the first row above, brackets have been used to indicate the most convenient order of products,
especially for symbolic computations.
Using the usual compact notations, we obtain J L (q) as

∂p04 (q)
J L (q) = =
∂q
 
−(L2 c2 + L3 c23 + L4 c234 )s1 −(L2 s2 + L3 s23 + L4 s234 )c1 −(L3 s23 + L4 s234 )c1 −L4 s234 c1
 (L2 c2 + L3 c23 + L4 c234 )c1 −(L2 s2 + L3 s23 + L4 s234 )s1 −(L3 s23 + L4 s234 )s1 −L4 s234 s1  ,
 
0 L2 c2 + L3 c23 + L4 c234 L3 c23 + L4 c234 L4 c234

while for J A (q) we have

     
0 0 sin q1
z 0 =  0  , z 1 = R1 (q1 )  0  =  − cos q1  ,
1 1 0
       
0 sin q1 0 sin q1
z 2 = R1 (q1 )R2 (q2 )  0  =  − cos q1  , z 3 = R1 (q1 )R2 (q2 )R3 (q3 )  0  =  − cos q1  .
1 0 1 0
Indeed, z 1 = z 2 = z 3 since the three joints axes 2, 3, and 4 are always parallel.

4
Exercise 2
By considering m = 2 in eq. (1), the function q(t) will contain the four coefficients a1 , b1 , a2 ,
and b2 , which are necessary and also sufficient, as we will see, to impose four arbitrary boundary
conditions in (2). We have thus
       
π t 3π t π t 3π t
q(t) = a1 sin + a2 sin + b1 cos + b2 cos ,
2T 2 T 2T 2 T
and
 π   
π t
 
3π t
 
π t
 
3π t

q̇(t) = · a1 cos + 3a2 cos − b1 sin − 3b2 sin .
2T 2T 2 T 2T 2 T
Note that the arguments of the trigonometric terms are properly scaled with respect to the motion
time T , so that all these terms take only the values 0, +1 or −1 at the boundaries t = 0 and t = T .
Therefore,
q(0) = q0 ⇒ b1 + b2 = q0 , q(T ) = q1 ⇒ a1 − a2 = q1 ,
and
2T 2T
q̇(0) = v0 ⇒ ,
a1 + 3a2 = v0 q̇(T ) = v1 ⇒ −b1 + 3b2 = v1 .
π π
The resulting linear system of equations has a simple block diagonal structure
    
1 −1 a1 q1
 1 3   a2   (2T /π) v0 
  = 
 1 1   b1   q0 
−1 3 b2 (2T /π) v1

and is always solvable (the determinant of both 2 × 2 blocks is equal to 4), leading to
1 1
a1 = (3q1 + (2T /π) v0 ) a2 = ((2T /π) v0 − q1 )
4 4
(4)
1 1
b1 = (3q0 − (2T /π) v1 ) b2 = (q0 + (2T /π) v1 ) .
4 4

We note that, by increasing the order m, additional boundary conditions on higher order
derivatives could be handled similarly (e.g., with m = 3, we can match also desired initial and final
accelerations). Just for reference, the expression of the time derivative of (1) for a generic m is
m     
π X π t π t
q̇(t) = (2h − 1) ah cos (2h − 1) − bh cos (2h − 1) , t ∈ [0, T ]
2T 2T 2T
h=1

and the four boundary conditions (2) at t = 0 and t = T are written in general as
m m m m
X X X 2T X 2T
bh = q0 , (−1)h−1 ah = q1 , (2h − 1)ah = v0 , (2h − 1)(−1)h bh = v1 .
π π
h=1 h=1 h=1 h=1

While this flexibility is a nice feature of the considered class of trajectories, there is still an issue
concerning the full predictability of the interpolating motion.
For case i), which is a rest-to-rest motion in T = 2 s from q0 = −40◦ to q1 = 40◦ , Figure 4
shows the position and velocity profiles of the solution trajectory, obtained for a1 = −b1 = 30

5
 position velocity
40 70

30 60

20
50

10
40

[deg / s]
[deg]

0
30

−10

20
−20

10
−30

−40 0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time [s] time [s]

Figure 4: Rest-to-rest motion case i): Balanced data give no wandering motion

and a2 = b2 = −10. The complete motion is satisfactory, with no wandering (neither under- nor
over-shoot in position). We call this a situation with balanced data: the initial and final positions
have opposite values, and motion occurs symmetrically around the zero average position. As a
matter of fact, it can be shown analytically that q0 · q1 ≤ 0 is a sufficient condition for the velocity
not to change sign in the interval [0, T ].

Figure 5 shows the position and velocity profiles obtained for the rest-to-rest case ii), where
q0 = 20◦ and q1 = 60◦ . From (4), we have a1 = 45, b1 = 15, a2 = −15, and b2 = 5. Despite the
total displacement is now halved with respect to the first case, a wandering behavior is present:
the position undershoots the initial value q0 = 20◦ during the first 0.6 s (velocity is negative for
0.4 s from the motion start). The only apparent difference is that the initial and final position
data are now unbalanced with respect to zero.
position velocity
60 45

55 40

35
50

30
45

25
40
[deg / s]
[deg]

20
35
15

30
10

25
5

20
0

15 −5
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time [s] time [s]

Figure 5: Rest-to-rest motion case ii): Unbalanced data give rise to undershoot in position

With reference to the rest-to-rest case (v0 = v1 = 0), the above considerations allows to enforce
a nice behavior in general (i.e., also in he unbalanced case). The addition of a suitable constant
term, as in the modified trajectory (3), overcomes in fact the wandering problem.

6
 position
60 velocity
35

55
30

50
25

45

20
[deg]

[deg / s]
40

15
35

10
30

25 5

20
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time [s]
time [s]

Figure 6: The modified solution for the rest-to-rest motion case ii) eliminates wandering, reduces
accordingly the peak velocity, and provides a symmetric velocity profile

Denote now the given initial and final position as p0 and p1 (this redefinition allows using the
previous formulas, with minimal intervention), and compute
p0 + p1 p1 − p0 p1 − p0
q̄ = , q0 = − , q1 = . (5)
2 2 2
The original data are thus transformed so that motion occurs around the average position q̄, with
opposite q1 = −q0 as desired. Taking into account the boundary conditions (2) for q(t), the
interpolation problem for q 0 (t) is correctly formulated since

at t = 0 ⇒ q 0 (0) = q̄ + q0 = p0 at t = T ⇒ q 0 (T ) = q̄ + q1 = p1 .

As a result, the modified solution q 0 (t) will be defined by q̄ in (5) and by the same expressions (4)
for a1 , a2 , b1 , and b2 , where q0 and q1 are now taken from (5).
The obtained trajectory will have a symmetric velocity profile with respect to the mid-motion
instant t = T /2. We also note that the modified trajectory will provide a natural solution even in
the degenerate case of no displacement (p0 = p1 = p): since we would have then q0 = q1 = 0, the
solution will be a constant trajectory q 0 (t) = q̄ = p (the previous method would generate instead
a trajectory that moves out from the value p, returning to it at the final instant). Figure 6 shows
the trajectory obtained with the modified method for case ii). Wandering is no longer present.
For completeness, Figure 7 reports the acceleration profiles of the various computed trajectories.

A Matlab code follows.

clear all; close all; clc

% boundary data
p0=20; % deg
p1=60;
v0=0; % deg/s
v1=0;
T=2; % total motion time (s)
% balanced solution does not wander (symmetric around average position)
pm=(p0+p1)/2;

7
 acceleration acceleration acceleration
100 80 50

80 60 40

60 40 30

40 20 20

20 0 10

[deg / s2]
[deg / s2]

[deg / s2]
0 −20 0

−20 −40 −10

−40 −60 −20

−60 −80 −30

−80 −100 −40

−100 −120 −50

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time [s] time [s] time [s]

Figure 7: Acceleration profiles for case i) (left), case ii) with original solution (center), and case
ii) with modified solution (right)

pd=p1-p0;
q0=-pd/2;
q1=pd/2;
% unbalanced solution leads to wandering ---uncomment to see this result
% q0=p0;
% q1=p1;
% pm=0;
% solution
a1=0.25*(3*q1+v0*2*T/pi);
a2=0.25*(v0*2*T/pi-q1);
b1=0.25*(3*q0-v1*2*T/pi);
b2=0.25*(q0+v1*2*T/pi);
t=[0:0.01:T];
tau=(pi/2)*t/T;
p=pm+a1*sin(tau)+a2*sin(3*tau)+b1*cos(tau)+b2*cos(3*tau);
pdot=pi/(2*T)*(a1*cos(tau)+3*a2*cos(3*tau)-b1*sin(tau)-3*b2*sin(3*tau));
pddot=-(pi/(2*T))^2*(a1*sin(tau)+9*a2*sin(3*tau)+b1*cos(tau)+9*b2*cos(3*tau));
plot(t,p);grid;title(’position’);xlabel(’time [s]’);ylabel(’[deg]’); pause;
plot(t,pdot);grid;title(’velocity’);xlabel(’time [s]’);ylabel(’[deg / s]’); pause;
plot(t,pddot);grid;title(’acceleration’);xlabel(’time [s]’);ylabel(’[deg / s^2]’)
% end

We conclude with a final remark. An alternative way to avoid trajectory wandering in the
rest-to-rest case (v0 = v1 = 0) would be to replace eqs. (5) with

q̄ = p0 , q0 = 0, q1 = p1 − p0 , (6)

namely resetting the initial position to zero and working with the total displacement p1 −p0 . When
inserted in (4), these choices yield
3 1
a1 = (p1 − p0 ) , a2 = − (p1 − p0 ) , b1 = b2 = 0,
4 4
discarding the presence of cosinusoidal functions in (1). Figure 8 shows the resulting motion for
case ii). Wandering has been eliminated as expected (being q0 · q1 = 0, the sufficient condition is
in fact satisfied), but the velocity profile is no longer symmetric and has a higher peak (compare
with Fig. 6, right). Thus, the modified solution in (5) is to be preferred to (6).

8
 position velocity
60 40

55 35

50 30

45 25

[deg / s]
[deg]

40 20

35 15

30 10

25 5

20 0
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time [s] time [s]

Figure 8: The alternative solution (6) for the rest-to-rest motion case ii) eliminates wandering, but
yields a non-symmetric velocity profile as opposed to the use of (5)

∗∗∗∗∗