Robotics I

September 12, 2011

The direct kinematics of a 4-dof robot of the SCARA type (2R-1P-1R) is

x = l1 cos q1 + l2 cos(q1 + q2 )
y = l1 sin q1 + l2 sin(q1 + q2 )
z = q3
(1)
α = 0
β = 0
γ = q1 + q2 + q4 ,

T
T
where p = x y z is the position of the end-effector and φ = α β γ is its orienta-
tion, expressed in XY Z angles (roll-pitch-yaw) with respect to the fixed base frame. The length
of links 1 and 2 is l1 = l2 = 1 [m].

T
In the configuration q A = 0 π/2 1 −π/2 , check if the following end-effector generalized
(linear/angular) velocity vector
   
vx 1
 

 vy  
  −1 

v  vz   1 
= =  (2)
ω 
 ωx  
  0 

 ωy   0 
ωz −1

is feasible or not. Provide then the expression and the numerical value of the joint velocity vector
q̇ ∈ R4 that guarantees the exact execution of the desired end-effector velocity (if the task (2) is
feasible), or at least the minimization of the Cartesian/task velocity error norm in a least squares
sense (if (2) is unfeasible).

Repeat the above analysis and the synthesis of a joint velocity vector with the same properties

T
when the robot is in the configuration q B = π/4 0 1 0 .

[120 minutes; open books]

1
 Solution
September 12, 2011

Denote the 6-dimensional task vector associated to the end-effector pose of the robot as
 
x
  y 
 

p  z 
r= =  = f (q),
φ  α 
 
 β 
γ
where f (q) is the direct kinematics expressed by the right-hand side of (1). The 6 × 4 analytic
Jacobian matrix J a relating q̇ to ṙ
 
ẋ
 ẏ 
 
 ż  ∂f (q)
 α̇  = ∂q q̇ = J a (q)q̇
ṙ =  
 
 β̇ 
γ̇
and the 6 × 4 geometric Jacobian matrix J g relating q̇ to the linear and angular velocity of the
end-effector  
vx
  vy 
 

v  vz 
=  = J g (q)q̇
ω  ωx 
 
 ωy 
ωz
will coincide in this case (J a = J g = J ), since the only admissible rotations of the end-effector
frame of this robot occur around a single constant axis (the Z axis). In particular, it will always
be ωx = α̇ = 0, ωy = β̇ = 0, and ωz = γ̇.

Therefore, we compute (for convenience, analytically from (1))

 
− (l1 sin q1 + l2 sin(q1 + q2 )) −l2 sin(q1 + q2 ) 0 0
 l1 cos q1 + l2 cos(q1 + q2 ) l2 cos(q1 + q2 ) 0 0 
 
 0 0 1 0 
J (q) = 
 .
 0 0 0 0 

 0 0 0 0 
1 1 0 1

T
Evaluating this matrix at the configuration q A = 0 π/2 1 −π/2 , and with the given
link lengths, provides  
−1 −1 0 0
 1 0 0 0 
 
 0 0 1 0 
J A = J (q A ) = 
 0
,
 0 0 0 

 0 0 0 0 
1 1 0 1

2
which has full row rank equal to 4 (det J TA J A = 1 6= 0). The desired linear and angular velocity (2),
due to its structure with zeros in the fourth and fifth components, will certainly be feasible and a
joint velocity solution is computed as
   −1  
v v
q̇ A = J #
A = J T
J
A A J T
A
ω ω
 
  1  
0 1 0 0 0 0  −1  −1
  (3)
 −1 −1 0 0 0 0   1   0 
= 
  
 =  1  [rad/s].
  
0 0 1 0 0 0   0 
1 0 0 0 0 1  0  0
−1
We note that in this case q̇ A is also the unique joint velocity realizing the desired task velocity.

T
At the configuration q B = π/4 0 1 0 , we have
 √ √ 
−√ 2 −√ 2/2 0 0

 2 2/2 0 0 

 0 0 1 0 
JB = J (q B ) = 
 0
,
 0 0 0 

 0 0 0 0 
1 1 0 1
which has clearly rank equal to 3. Still the desired linear and angular velocity lies in the range
of matrix J B and thus it is feasible. The only difference with respect to the previous case is that
a joint velocity solution should be computed using the numerical algorithm for the pseudoinverse
of J B (i.e., using the SVD of J B , or by simply calling the pinv function in Matlab). We obtain,
with some numerical rounding,
 
# v
q̇ B = J B
ω
 
  1  
−0.3536 0.3536 0 0 0 −0.1667  −1 
  −0.5404 (4)
0 0 0 0 0 0.3333    1  =  −0.3333  [rad/s].
    
=  0 0 1 0 0 0  0  
  1 
0.3536 −0.3536 0 0 0 0.8333  0  −0.1262
−1
The solution q̇ B is not unique in this case, but it has automatically the minimum norm property
among all joint velocities realizing the task. For illustration, another joint velocity satisfying the
task is  
−0.1322
 −1.1498 
q̇ 0B =   [rad/s],
 1 
0.2820
which is obtained by adding to q̇ B a vector nB in the one-dimensional null space of J B :
 
0.4082
 −0.8165 
q̇ 0B = q̇ B + nB , nB =  , being J B nB = 0.
 0 
0.4082

3
Checking the norm of these two joint velocity solutions gives

kq̇ B k = 1.1913 < 1.554 = kq̇ 0B k,

as expected.

In both cases A and B (as well as in B 0 ), the task velocity error is zero, since the desired linear
and angular velocity (2) is exactly realized. Indeed, one can verify that
 
1
 −1 
 
0
 1 
J A q̇ A = J B q̇ B = J B q̇ B = 
 .
 0 

 0 
−1

In case B, there exist other task velocity vectors (still with structural zeros in the fourth and fifth
components) that are not feasible. The joint velocity obtained by using the pseudoinverse of J B
will still provide a task velocity error of minimum norm, although this error would then be different
from zero. The reader is invited to provide an example.

We finally note that, due to the presence of structural zeros in the fourth and fifth rows of J ,
all previous results could have been obtained by reducing the problem to the 4-dimensional task

T
space r̄ = x y z γ and working with the square 4 × 4 matrix J̄ obtained by deleting
rows 4 and 5 from J . For case A, we would obtain the same solution q̇ A given in (3) by computing
−1 #
q̇ A = J̄ ¯ṙ, while for case B the same solution q̇ B given in (4) would be obtained as q̇ B = J̄ ¯ṙ.

In fact we can state in general that, given a m × n matrix J (with m > n) of the form
 
J̄
J =T ,
O

where T is a row permutation (and thus, an orthonormal) matrix and J̄ is a square m × m matrix,
the following expression holds for the pseudoinverse:
#
J # = J̄ TT.


I m×m O
# −1
Morever, when det J̄ 6= 0, it is J̄ = J̄ . In our application, this relation means that we can
simply work with the reduced task space and the associated reduced Jacobian, discarding the zero
rows.

∗∗∗∗∗