Automatica 36 (2000) 879}887

Brief Paper
Task-space regulation of cooperative manipulators夽
F. Caccavale *, P. Chiacchio , S. Chiaverini

Dipartimento di Informatica e Sistemistica, Universita% degli Studi di Napoli, Federico II via Claudio 21, 80125 Napoli, Italy

Dipartimento di Automazione, Elettromagnetismo, Ingegneria dell 'Informazione e Matematica Industriale, Universita% degli Studi di Cassino,
via G. Di Biasio 43, 03043 Cassino, Italy
Received 18 May 1998; revised 6 September 1999; received in "nal form 4 October 1999

Abstract

In this paper a new task-space regulation scheme for a system of two cooperative manipulators tightly grasping a rigid object is
proposed. The control architecture is based on individual task-space regulators for the two manipulators. In order to overcome
problems arising from representation singularities, the unit quaternion is used to describe the orientation of relevant frames. To avoid
steady-state internal stresses at the held object, kinetostatic "ltering of the control action is introduced together with internal force
feedback. Also, the performance of the control scheme is analyzed in the presence of a class of modeling uncertainties. The equilibria of
the closed-loop system are then explicitly computed and asymptotic stability is proven via Lyapunov-like analysis.  2000 Elsevier
Science Ltd. All rights reserved.

Keywords: Manipulators; Robot control; Regulation; Equilibrium; Stability

1. Introduction A major problem of the approaches recalled above is

that the task-space variables deriving from kinetostatic
When a multi-arm system is employed for the manip- mappings are often unsuitable for the description of the
ulation of a common object, it is important to control cooperative task. Moreover, minimal representations of
both the variables describing the absolute motion of the the orientation (e.g., three Euler angles) are usually ad-
system and the internal stresses at the held object. To this opted in the control laws; however, this class of repres-
aim, the mappings between forces and velocities at the entations su!ers from representation singularities and
end e!ectors of the single manipulators and their does not provide a geometrically meaningful tool to
counterparts at the manipulated object are to be con- describe the orientation.
sidered (Uchiyama & Dauchez, 1993). Control schemes As for the de"nition of the task-space variables, in this
designed in this framework have been proposed for the paper the task-oriented formulation devised by (Chiac-
control of absolute motion and internal forces (e.g. chio, Chiaverini & Siciliano, 1996) is adopted, which fully
Li, Hsu & Sastry, 1989; Chiacchio, Chiaverini, Sciavicco characterizes a coordinated motion task and allows the
& Siciliano, 1990; Wen & Kreutz, 1992; Hsu, 1993; user to specify the task in terms of meaningful variables.
Khatib, 1995). More recently, joint-space control Based on this formulation, a kinematic control strategy
schemes (Luecke & Lai, 1997), adaptive approaches to has been recently proposed (Chiacchio & Chiaverini,
force/motion control (Jean & Fu, 1993; Liu & Arimoto, 1996; Caccavale, Chiacchio & Chiaverini, 1999). The
1998) as well as impedance control schemes (Bonitz desired cooperative task-space trajectory is transformed
& Hsia, 1996) have been proposed. into the corresponding joint motion for each manipula-
tor via an inverse kinematics algorithm; then, an indi-
vidual joint-space controller is designed for each
manipulator. An alternative approach is represented by
夽
This paper was not presented at any IFAC meeting. This paper task-space control, in which a centralized control loop is
was recommended for publication in revised form by Associate Editor closed directly on the cooperative task-space variables
L-C. Fu under the direction of Editor K. Furuta.
* Corresponding author. Tel.: #39-(081)-768-3635; fax: #39-(081)- (Caccavale, Chiacchio & Chiaverini, 1997).
768-3186. In this paper a novel force/position regulation scheme
E-mail address: [email protected] (F. Caccavale) based on the same task-oriented formulation is proposed.

0005-1098/00/$ - see front matter  2000 Elsevier Science Ltd. All rights reserved.
PII: S 0 0 0 5 - 1 0 9 8 ( 9 9 ) 0 0 2 1 5 - 0
880 F. Caccavale et al. / Automatica 36 (2000) 879}887

Namely, the cooperative task-space desired trajectory is end-e!ector orientation, and Q the unit quaternion
G
transformed into the corresponding motion at the two corresponding to R (see the appendix). Let also
G
end e!ectors; then, a regulation loop is designed in the v "[p 2 x2]2 be the (6;1) end-e!ector (linear and
G G G
task space of each manipulator. Remarkably, the pro- angular) velocity vector which is related to the joint
posed strategy still allows the speci"cation of the motion velocity vector through the manipulator Jacobian matrix
in the cooperative task space, while each control loop is J (q ). All quantities are expressed in a common base
G G
designed in the task space of the single robot. Therefore, frame.
the proposed approach does not need the adoption of The absolute position of the system is given by the
an inverse kinematics procedure, as required for the vector
kinematic control strategy. Null internal forces at steady
p "(p #p ), (1)
state are achieved by resorting to kinetostatic "ltering of   
the task-space control actions, while force regulation is which de"nes the origin of the absolute frame. The rota-
performed via internal force feedback. tion matrix de"ning the orientation of the absolute frame
A remarkable feature of the proposed scheme is the (i.e., the absolute orientation) is given by
adoption the unit quaternion to represent the orientation
of relevant frames; this choice allows avoiding the rep- R "R R(k ,0 /2), (2)
 I  
resentation singularities and provides a geometrically
where k and 0 are the unit vector and the angle,
meaningful way to describe the orientation as well as a  
respectively, that realize the rotation described by
powerful analysis tool.
R "R2 R , and the expression RH (kH,0 ) denotes the
The equilibrium of the closed-loop system is analyzed    I
rotation matrix corresponding to a rotation of 0 about
by resorting to the quaternion algebra. It is worth re-
the axis kH. Let
marking that the use of the unit quaternion has made

 
possible to compute and discuss the existence of the 0 0
Q"+g , e," cos  , k sin  (3)
equilibria in terms of geometrically meaningful quantities I I I 4  4
(i.e., rotation matrices and equivalent rotation axes and
angles). The stability analysis of the cooperative system denote the unit quaternion extracted from R(k ,0 /2);
I  
under the proposed control law is developed and asymp- then, the absolute orientation can be expressed in terms
totic convergence to the equilibria is proven via a of quaternion product (see the appendix) as follows:
Lyapunov-like argument.
Q "+g , e ,"Q * Q. (4)
Also, the behavior of the proposed scheme in the  I
presence of imperfect gravity compensation is discussed In order to fully describe a coordinated motion, the
in terms of position and internal force regulation accu- position and orientation of one manipulator relative to
racy. The performance of the regulation scheme has been the other is also of concern. The relative position between
experimentally veri"ed on a setup composed by two the two end e!ectors is de"ned as the vector
six-dof industrial manipulators (Caccavale, Chiacchio
& Chiaverini, 1998). The results * not reported here for p "p !p . (5)
  
brevity * have con"rmed that good performance can be The relative orientation between the two end e!ectors is
achieved in the presence of modeling errors and noisy de"ned with reference to the "rst end-e!ector frame in
measurements. terms of the rotation matrix
Finally, it is worth remarking that, although the pro-
posed scheme has been conceived to solve regulation R"R "R2 R , (6)
   
problems, the validity of the underlying ideas is more
general, and can be extended to solve, e.g., tracking representing the mutual orientation of the two end
control problems. e!ectors. The relative orientation can be also expressed
in terms of quaternion product through Eq. (A.2):
Q"+g , e,"Q\ * Q . (7)
2. Cooperative task-space formulation     

In this section the task-oriented formulation developed 3. Modeling

in Chiacchio et al. (1996) is reformulated by using the unit
quaternion for the description of the orientation. 3.1. Dynamics
Consider a system of two manipulators tightly grasp-
ing a rigid object. For each manipulator (i"1,2) let n be
G The dynamics of the two manipulators can be written
the number of joints, q be the (n ;1) vector of joint
G G in compact form as
variables, p the (3;1) vector denoting the end-e!ector
G
position, R the (3;3) rotation matrix expressing the M(q)qK #C(q, q )q #Fq #g(q)"s!J2(q)h, (8)
G
 F. Caccavale et al. / Automatica 36 (2000) 879}887 881

where the matrices are block diagonal, e.g. M" When each virtual stick r is null, then the grasp matrix
G
blockdiag(M , M ), and the vectors are stacked, e.g. reduces to W"[I I ]; this is the case when the kin-
   
g"[g2 g2 ]2. For each manipulator, M is the (n ;n ) ematics of the ith manipulator is expressed in terms of an
  G G G
symmetric positive-de"nite inertia matrix, C q is the end-e!ector frame located at the tip of the corresponding
G G
(n ;1) vector of Coriolis and centrifugal forces, F is virtual stick; without loss of generality, it is assumed also
G G
the (n ;n ) diagonal matrix of friction coe$cients, g is that the orientation of the two end-e!ector frames
G G G
the (n ;1) vector of gravitational forces, the vector coincides (i.e., R2 R "I ). It can be recognized that in
G   
s represents the joint torques and h is the (6;1) vector such a case the task-oriented formulation reviewed in
G G
of generalized forces acting at the end e!ector of the ith Section 2 collapses into the so-called symmetric formula-
manipulator. The dynamics of the object can be de- tion proposed in Uchiyama and Dauchez (1993).
scribed by the equation
3.3. Closed-chain constraints
M v #C v #g "h , (9)
     
Tight grasp of a rigid object results in the di!erential
where v is the vector expressing the (linear and angular)
 constraint
velocity of a frame attached to a "xed point on the object
(e.g., the center of mass), M is the object inertia matrix,
 V2v"V2Jq "0, (12)
C v is the vector of velocity-dependent terms, g is
   which represents the closed-chain constraint expressed in
the vector of gravitational forces, and h is the vector of
 terms of velocities. When the direct kinematics of each
external forces acting on the object.
manipulator is referred to the tip of the corresponding
virtual stick (i.e., r "0), the closed-chain constraint can
3.2. Grasp geometry G
be expressed at position and orientation level as follows:
Since the grasp is tight, each end e!ector can exert p !p "0,
both a force and a moment on the object at the contact   (13)
point. The mapping of the contact force vector h into the Q\ * Q "Q * Q\"Q ,
    
(6;1) external force vector h is
 where Q "+1, 0, is the unitary quaternion (see the ap-

pendix).
 
I O I O
h "     h"Wh, (10) It is worth remarking that the above conditions rep-
 L I L I
    resent a set of mechanical constraints, and thus they are
always ful"lled during system's motion. On the other
where W is the grasp matrix, O denotes the (l;l) null
J hand, the desired trajectories may violate the constraints
matrix and I denotes the (l;l) identity matrix. The
J if the trajectory generation module computes the set-
matrix L "S(r ) (i"1,2) transforms the applied contact
G G points based on an inaccurate geometrical model of the
forces in moments at the object frame, S( ) ) is the skew-
grasp as well as if even small algorithmic errors arise
symmetric matrix operator performing the cross product
when the desired con"guration is computed on line. This
and r is the (3;1) vector from the ith end e!ector to the
G may cause building up of internal stresses at the held
point "xed on the object (i.e., r is the so-called virtual
G object at steady state. Finally, it is worth remarking that
stick in Uchiyama and Dauchez (1993)).
during fast transients large internal forces may be experi-
The matrix W is full row rank; then, for a given h the
 enced as well, e.g., due to imperfectly and/or partially
inverse solution to (10) is given by
compensated dynamics in the system or large tracking
h"W-h #Vh , (11) errors.
  In the following, a regulation strategy is proposed
where WR denotes a pseudoinverse of W, V is a full which is capable of counteracting the above-mentioned
column rank matrix spanning the null space of W, and e!ects at steady state.
h represents the vector of internal forces, i.e., the forces

which do not contribute to the motion of the object, but
represent mechanical stresses applied to the object. It has 4. Task-space regulation
been recognized in Walker, Freeman and Marcus (1989)
that the use in (11) of a generic pseudoinverse of W, e.g. 4.1. Generation of reference trajectories
the Moore}Penrose pseudoinverse, may lead to internal
stresses even if h "0; to avoid this, WR must be proper- The task-oriented formulation described in Section 2

ly chosen. One possible choice for the matrix V is pro- fully characterizes a coordinated motion task and, at the
posed in Chiacchio, Chiaverini, Sciavicco and Siciliano same time, allows the user to naturally specify the task in
(1991) and is not reported here for brevity. terms of meaningful variables. Therefore, it is convenient
882 F. Caccavale et al. / Automatica 36 (2000) 879}887

to assign the task for the cooperative system in terms of region of their workspace (i.e., the Jacobian matrix J is
absolute and relative variables. square and nonsingular).
At the motion control level, the adoption of a kin- If a regulation problem is considered (i.e., constant
ematic control strategy (Chiacchio & Chiaverini, 1996; desired trajectories), a possible control law designed in
Caccavale et al., 1999) requires the presence of an inverse the task space of the single manipulators is the PD
kinematics generating the reference variables for the regulator with gravity compensation:
joint-space individual controllers. On the other hand,
s"J2K e!K q #g , (20)
a control law designed directly in the cooperative task   
space (Caccavale et al., 1997) does not allow the adoption
where K is a positive-de"nite matrix, K "diag+K ,
of two individual controllers as building blocks of the   .
k I , K , k I , is a block-diagonal positive-de"nite
control architecture. The goal here is to retain the ad- -  . - 
matrix, and g "g#J2WRg .
vantage of the kinematic control (i.e., the adoption of  
The "rst term on the right-hand side of (20) is a pro-
individual controllers for the manipulators), and, at the
portional action on the task-space error e"[e2 e2 ]2,
same time, to avoid the use of an inverse kinematics  
with
procedure to generate the joint motion references. To the

  
purpose, a little extra computational e!ort has to be p p !p
spent at the trajectory generation level. e " G " G G , i"1,2 (21)
G e R e G
Once the desired motion for the system is assigned in G G G
terms of cooperative task-space variables (i.e., absolute where e G is the vector part of the quaternion
and relative), these can be transformed into the corre- G
QI G "Q\ * Q and e corresponds to QI "Q * Q\ (see
sponding ones at the end e!ectors of the two arms. Let G G G G G G G
(A.2) and (A.3) in the appendix).
p and p be the vectors of desired absolute and relative The equilibrium of system (8), (9), (11) under the con-
 
positions, respectively; then, the corresponding desired trol law (20) satis"es the conditions (the subscript ss
end-e!ector position vectors p and p can be easily denotes steady-state quantities)
 
derived from (1) and (5) as
WK e "0, (22)
p "p !p , (14)  
   
h "VRK e , (23)
p "p #p . (15)    
   
which reveal that internal forces arise at steady state if
The desired rotation matrices R and R can be de- a task-space error is present. Such an error may be due
 
rived via (2) and (6) as to even small inaccuracies of the trajectory generation
module, leading to reference trajectories not consistent
R "R R2(k,0 /2), (16)
  I   with the geometry of the grasp (i.e., with the closed-
R "R R, (17) chain constraints).
   To avoid building of internal forces at steady state,
where R and R are the desired absolute and rela- it is proposed to modify the control law (20) to include
 
tive rotation matrices, and k and 0 are the desired kinetostatic "ltering of the cooperative task-space
 
unit vector and rotation angle used to assign control action
R"R(k, 0 ). Of course, the above two relations
 I   s"J2(WRW#VRVR)K e!K q #g . (24)
can be expressed in terms of quaternions as follows:   
In (24) the (6;6) diagonal matrix R"diag+p , weights
 
0 0 G
Q "Q * cos  ,!k sin  , (18) the components of K e in each direction of the subspace
  4  4 
of the internal forces via the constant values 04p 41.
G

 
0 0 In detail, if R"O then all these components are com-
Q "Q * Q"Q * cos  , k sin  , (19) 
    2  2 pletely cancelled from the control action, while the choice
R"I leads to the control law (20) without kinetostatic

where the unit quaternion Q , representing the desired "ltering. The equilibrium of system (8), (9), (11) under the

absolute orientation, can be keenly computed in terms of control law (24), satis"es the conditions
an equivalent angle/axis description, i.e. Q "+cos (0 /2),
 
k sin (0 /2),. WK e "0,
 
(25)
 
h "RVRK e . (26)
4.2. Control law    
Remarkably, the former condition implies that the com-
Consider a system of two spatial nonredundant ponent of the task-space error term in the external-force
manipulators (n "n "6) moving in a singularity-free space vanishes, the latter ensures that the ith component
 
 F. Caccavale et al. / Automatica 36 (2000) 879}887 883

of the internal force at steady state is reduced according which reveals that the position set-points are reached at
to p . Therefore, if p "0 the control law (24) cancels the steady state.
G G
ith component of the internal force at steady state, even if Regarding the orientation errors, Eqs. (28) and (29)
the corresponding task-space set point cannot be reached must be separately combined with the second equation in
due to the closed-chain constraints. (32). Let us "rst consider condition (28) which, combined
with the second equation in (32), yields
4.3. Equilibrium analysis QI QI "QM \ "+g ,!e ,. (35)
  *     
Eqs. (25) and (26) can be interpreted as a set of con- By applying Property A.1 in the appendix, it can be
straints on e . Nonlinearity of the equations does not recognized that Eq. (35) provides two solutions for

allow drawing general conclusions about their solution. QI (if g 50)
Nevertheless, when the direct kinematics of each manipu-   
lator is expressed in terms a virtual end e!ector coinci- QI "+g , e ,, QI "+!g ,!e ,, (36)
       
dent with the tip of the virtual stick, the equilibria can
be explicitly computed. Therefore, all the results in this where
section are derived under this assumption.

 
1#g (2
In the case of kinematics referred to the tip of virtual +g , e ,"  ,! e . (37)
sticks, Eq. (25) can be rewritten as   2 2(1#g 



p #p "0, The corresponding value of QI for each solution is
    (27)  
computed from (28)
e #e "0.
   
QI "QI \ . (38)
It can be recognized that the second equation in (27) is    
implied either by the condition Note that Eq. (36) provides two equilibria corresponding
to the same relative orientation between the actual and
QI "QI \ , (28) desired frames. In the case QM "Q (i.e., set points
     
compatible with the constraints), the equilibria in (36)
or by the condition become

QI "QI  "+!g ,!e ,. (29) QI "QI "Q , QI "QI "Q , (39)

                 
The constant reference trajectories satisfy the following i.e., the desired orientation is reached at steady state. On
constraints: the other hand, it can be easily recognized that condition
(29) holds if and only if QM "+!1, 0,, which is in
p !p "p 
  
, (30) contradiction with the assumption g 50; hence, the

only equilibrium solutions for the orienation errors are
Q * Q\"QM "+g , e ,, (31) given by (36) or (39).
    
Finally, it is worth investigating the steady-state be-
where it is assumed g 50. According to (13), it is
 havior of the system in terms of absolute and relative
p "0 and QM "Q if the set-points are consistent
   variables. Obviously, the desired absolute and relative
with the closed-chain constraints. From (30), (31) and the con"gurations are reached if the set-points do not violate
constraint equations (13), it follows the constraints. On the other hand, when the set-points
are not compatible with the closed-chain constraints it is


p !p "p ,
     (32) not trivial to predict the behavior of the absolute and
QI QI \ "QM . relative displacements. In the following, it is shown that
  *   
the desired absolute con"guration is always reached at
As for the position errors, Eqs. (27) and (32) provide the steady state, even if the set-points are not compatible
equilibrium solution with the geometry of the grasp.
It can be easily recognized that the "rst condition in
p "!p "!p . (33) (27) implies
     
If the reference trajectories are compatible with the p !p "0 (40)
 
constraints (i.e., p "0), the equilibrium solution for

the position is represented by and thus the absolute position set-point is reached. As
for the orientation error, Eqs. (19) and (A.3) imply
p "p "0, (34) g "cos (0 /2) and e "R k sin (0 /2). Hence,
         
884 F. Caccavale et al. / Automatica 36 (2000) 879}887

from (37) and (A.3) it follows that where the identity v2(M Q !2C )v"0 and the closed-
 
chain constraint (12) have been used, and F "J\2FJ\.

 
0 0
Q\ * Q " cos  ,!k sin  , (41) From (21) and (A.4) it follows
   4  4
<Q #<Q "!p 2K e !x2K e "!v2K e, (48)
. - . . - - 
 
0 0
Q\ * Q " !cos  , k sin  , (42) where K "diag+k I , k I ,. Thus, Eq. (47) becomes
   4  4 - -  - 
which correspond to the same mutual orientation <Q (x)"!v2(F #J\2K J\)v
 
between the desired and the actual frame. Hence, the
!v2(I !WRW)K e. (49)
expression of the absolute orientation error can be  
derived via (18) and, e.g., (41) It can be recognized that the matrix (I !WRW) is

a projector onto the null space of the grasp matrix W; in
 
0 0
Q * Q\ "Q * cos  , k sin  * Q\ view of constraint (12), this implies that the second term
   4  4 
on the right-hand side of the above equality is null, and
"Q (Q\ Q )\ * Q\ "Q , (43) thus Eq. (49) reduces to
 *   *    
where the equality Q "Q , coming from the closed- <Q (x)"!v2(F #J\2K J\)v, (50)
    
chain constraint (13) and the de"nition of the absolute
which shows that <Q is negative semi-de"nite all over ).
orientation (4), has been exploited. Hence, the absolute
The set R of all points x3) where <Q (x)"0 is given by
orientation at steady state coincides with the desired one.
In conclusion, the proposed control law ensures null R"+x3): v"0,. (51)
internal forces at the expense of the sole relative vari-
ables, while the desired absolute con"guration is reached. From the equilibrium analysis in the previous subsection
This ensures always a precise positioning of the held it follows that the largest invariant set in R is
object while avoiding object's internal stresses.
M"+x3R: e satis"es (25),. (52)

4.4. Stability analysis Therefore, the global invariant set theorem ensures
asymptotic convergence to the set M. In the cases ana-
Assume the matrix J square and nonsingular, i.e., the lyzed in the previous subsection the set M reduces to the
two manipulators are nonredundant and moving in elements de"ned either by (33), (36), (38) or by (34), (39).
a singularity free region of the workspace. Under this In the last case, the equilibrium is unique for the position
assumption a stability analysis can be devised based on error, while Eq. (39) de"nes two di!erent equilibria
the invariant set theorem (e.g., see Khalil, 1996). (although they represent the same mutual orientation).
Consider the scalar function with continuous "rst However, it can be recognized that the equilibrium repre-
partial derivatives sented by Q "Q "+!1,0, is unstable. In fact, the
   
value of function (44) at this equilibrium is
<(x)"v2M (q)v#< #< , (44)
  . -
< "8k . (53)
where x"[e2 v2]2 belongs to the set )L1 of  -
task-space errors and end-e!ector velocities satisfying Consider a small perturbation around the equilibrium
the closed-chain constraints, and M "J\2MJ\# characterized by p "0, v "0, g "!1#p (p'0),
 G G G
WRM WR2. In (44) < is de"ned as follows:
 . and consequently ""e G """p(2!p) (i"1,2). The pertur-
G
bed Lyapunov function is
< "e2 K e , (45)
.  . . .


where e "[p 2 p 2 ]2 and K "diag+K , K ,. The p
.   . . . < "8k 1! (< . (54)
function < is chosen as N - 2 
-
< "k ((1!g )#""e  ""#(1!g )#""e  ""). (46) Since <(t) has been proven to be decreasing, it will never
- -     return to < , implying that this equilibrium is unstable.
Note that < and < are both quadratic, thus <(x) is 
. - Therefore, the only asymptotically stable equilibrium is
positive de"nite and radially unbounded in ). x"0, with g "1 (i"1,2).
The time derivative of (44) along the trajectories of G
system (8), (9), (11) under the control law (24) is given by 4.5. Addition of internal force feedback
<Q (x)"!v2(F #J\2K J\)v#v2WRWK e
   If it is desired to impose a given internal force set point,
#<Q #<Q , (47) force feedback should be added. Also, addition of force
. -
 F. Caccavale et al. / Automatica 36 (2000) 879}887 885

feedback might improve the performance in the presence noticing that the object's gravity force does not contrib-
of modeling errors and disturbances. To the purpose, ute to this additional term, and thus VR(g !g( )"0 if
 
control law (24) can be modi"ed as follows: the gravity terms in the manipulators model are perfectly
compensated.
s"J2(WRW#VRVR)K e!K q #g #J2Vh . (55)
     The convergence to the new set of equilibria can be
The new control input h is de"ned as shown via the same Lyapunov argument as above, where
  the candidate function is to be modi"ed as follows:
h "h #K (h !h ), (56)
    D    < (x)"v2M (q)v#< #< #; (q)!;K (q), (63)
   . -  
where K is a positive-de"nite matrix and h is the
D   where ; and ;K are two scalar energy functions repres-
vector of desired internal forces. The internal force vector  
enting the potential associated with the gravity terms (see
h can be computed from the vector of measured end-
 Tomei, 1991)
e!ector forces h via the inverse of mapping (11).
For given set points, the equilibrium of system (8), (9), *; (q) *;K (q)
 "J2g ,  "J2g( . (64)
(11) under the control law (55), (56) is de"ned by the two *q  *q 
equations
In view of the boundedness of the two energy functions, it
WK e "0, (57) can be recognized that < is radially unbounded and its
  
time derivative is the same as <Q in (50). Hence, the
h "h #(I #K )\RVRK e , (58)
     D   invariant set theorem ensures global asymptotic conver-
which reveal that the adoption of the force loop ensures gence to the set
h "h when R"O ; if ROO the desired value
      M "+x3R: e satis"es (61),. (65)
of internal force is reached along the ith component if  
p "0.
G
The equilibrium analysis for e is the same as that
 5. Conclusions
developed in the previous section. To analyze the stabil-
ity of the equilibria de"ned by (57) and (58) the same
In this paper a novel task-space regulation scheme
scalar function <(x) as in (44) can be used. It can be
for a two-manipulator system has been proposed. The
recognized that its time derivative along the trajectories
regulator is based on kinetostatic "ltering of the control
of the system is the same as in (50); indeed, by substituting
action and on internal force feedback. The equilibrium of
control law (55) into (47), the force feedback terms do not
the system under the control law has been studied lead-
contribute to <Q (x) in view of the closed-chain constraint
ing to a set of equilibria corresponding to null internal
(12). Therefore, the same argument leads to establishing
forces. Application of the invariant-set theorem has led
asymptotic convergence to the set
to proving asymptotic stability of the equilibria. The
M "+x3R: e satis"es (57),. (59) closed-loop behavior of the proposed scheme is also
D  analyzed in the case of imperfect gravity compensation.
4.6. Imperfect gravity compensation The performance of the regulation scheme in the presence
of noise and modeling inaccuracies has been experi-
In many practical cases, the object's dynamic model is mentally veri"ed on an industrial cooperative setup in
not accurately known and only nominal estimates of the Caccavale et al. (1998).
dynamic parameters are available. Thus, if an estimate of Future research e!orts will be devoted to extend the
the gravity term g( is used, the control law (55) becomes approach to force/position tracking control problems as
 well as to cooperative systems for the manipulation of
s"J2(WRW#VRVR)K e!K q #g( #J2Vh , (60) elastic objects.
    
where g( "g( #J2WRg( .
 
In this case the equilibria satisfy the conditions
Acknowledgements
WK e "W(g !g( ), (61)
    This research is supported partly by CNR (National
h "h #(I #K )\ Research Council) and partly by MURST (Ministero
     D
;(RVRK e !VR(g !g( )). (62) dell' Università e della Ricerca Scienti"ca e Tecnologica).
   
It can be easily recognized that zero steady-state errors Appendix. The unit quaternion
cannot be obtained, even if the set-points are compatible
with the geometry of grasp; moreover, an additional term For the reader's convenience, a few basic concepts
a!ects the internal forces at steady state. It is worth regarding the unit quaternion are summarized hereafter
886 F. Caccavale et al. / Automatica 36 (2000) 879}887

(see e.g. Roberson & Schwertassek, 1988; Chou, 1992 for frame j relative to frame i, which has been referred
further details). to frame i.
The orientation of a rigid body in space is typically
described in terms of a (3;3) rotation matrix R express- Property A.1. Given a unit quaternion QOQ "+!1,0,,
ing the orientation of a frame attached to the body with 
then the equation Q"Q * Q admits only two solutions
respect to a "xed base frame. An alternative description V V
which represent the same orientation.
can be obtained by resorting to a four-parameter repres-
entation in terms of a unit quaternion (viz., Euler para-
meters) Proof. The thesis follows from the explicit expression of
Q"Q * Q :
V V

 
h h
Q"+g, e," cos , k sin , (A.1) g!""e """g, 2g e "e.
2 2 V V V V
If gO!1, the above system of equations admits only
where h and k are, respectively, the rotation and the two real solutions Q "+g , e , and Q "+!g ,!e ,
(3;1) unit vector of an equivalent angle/axis description V V V V V V
with
of orientation. Note that the scalar part and the vector


part of the unit quaternion are constrained by the equa- g#1 (2
g " , e " e. 䊐
tion g#e2e"1. It is worth remarking that Q"+g, e, V 2 V 2(g#1
and Q"+!g,!e, represent the same orientation.
Consider now two frames, conventionally labeled i and
j. Let R and R respectively denote the rotation matrices
G H References
expressing the orientation of the two frames with respect
to the base frame. Then, the mutual orientation between Bonitz, R. G., & Hsia, T. C. (1996). Internal force-based impedance
the two frames can be described by the rotation matrix control for cooperating manipulators. IEEE Transactions on
RG "R2R . The unit quaternion QI G describing the Robotics and Automation, 12, 78}89.
H G H HG Caccavale, F., Chiacchio, P., & Chiaverini, S. (1997). A cooperative
mutual orientation can be computed by composition
task-space control law for a two-manipulator system. Preprints of
(quaternion product) of the corresponding unit
xfth IFAC symposium on robot control, Nantes, France (pp. 367}372).
quaternions Q\"+g ,!e , (i.e., the conjugate of Q )
G G G G Caccavale, F., Chiacchio, P., & Chiaverini, S. (1998). A quaternion-
and Q "+g , e ,, i.e. based regulator for cooperative manipulators. Proceedings of the
H H H
1998 IEEE international conference on control applications, Trieste,
QI G "Q\ * Q "+g , e G ,, (A.2) Italy (pp. 557}561).
HG G H HG HG Caccavale, F., Chiacchio, P., & Chiaverini, S. (1999). Stability analysis
of a joint space control law for a two-manipulator system. IEEE
where * denotes the quaternion product (composition). Transactions on Automatic Control, 44, 85}88.
If the two frames coincide (i.e., R2R "I ) then Chiacchio, P., & Chiaverini, S. (1996). PD-type control schemes for
G H 
QI G "Q "+1,0, or QI G "Q "+!1,0,; given a unit cooperative manipulator systems. International Journal of Intelligent
HG  HG  Automation and Soft Computing, 2, 65}72.
quaternion Q, the unitary quaternion Q is such that
 Chiacchio, P., Chiaverini, S., Sciavicco, L., & Siciliano, B. (1990).
Q * Q"Q * Q "Q, while Q * Q"Q * Q "Q.
    Dynamic force/motion control of cooperative robot systems.
The vector part of QI G can be represented in a com-
HG Proceedings of the ASME winter annual meeting, vol. DSC-26,
mon base frame through the rotation matrix R , i.e., Dallas, TX (pp. 121}126).
G
e "R e G ; it can be shown that the following equality Chiacchio, P., Chiaverini, S., Sciavicco, L., & Siciliano, B. (1991). Global
HG G HG task space manipulability ellipsoids for multiple arm systems. IEEE
holds (Chou, 1992):
Transactions on Robotics and Automation, 7, 678}685.
Chiacchio, P., Chiaverini, S., & Siciliano, B. (1996). Direct and inverse
QI "Q * Q\"Q * QI G * Q\"+g , R e G ,, (A.3) kinematics for coordinated motion tasks of a two-manipulator
HG H G G HG G HG G HG
system. ASME Journal of Dynamic Systems, Measurement, and
which corresponds to the rotation represented by R R2. Control, 118, 691}697.
H G Hsu, P. (1993). Coordinated control of multiple manipulator systems.
The relationship between the time derivative of the IEEE Transactions on Robotics and Automation, 9, 400}410.
quaternion components and the relative angular velocity Jean, J.-H., & Fu, L.-C. (1993). An adaptive control scheme for coor-
is established by the so-called quaternion propagation dinated multimanipulator systems. IEEE Transactions on
rule Robotics and Automation, 9, 226}231.
Khalil, H. K. (1996). Nonlinear systems. Upper Saddle River, NJ:
g "!e G2x G , e G "E(g , e G )x G ,
Prentice-Hall.
(A.4)
HG  HG HG HG  HG HG HG Khatib, O. (1995). Inertial properties in robotic manipulation: An
object level framework. International Journal of Robotics Research,
13, 19}36.
where E( ) )"gI !S( ) ), S( ) ) is the skew-symmetric
 Li, Z., Hsu, P., & Sastry, S. (1989). Grasping and coordinated manip-
matrix operator performing the cross product and ulation by a multi"ngered robot hand. International Journal of
x G "xG !xG "R2(x !x ) is the angular velocity of Robotics Research, 8, 33}50.
HG H G G H G
 F. Caccavale et al. / Automatica 36 (2000) 879}887 887

Liu, Y.-H., & Arimoto, S. (1998). Decentralized adaptive and non- Pasquale Chiacchio was born in Naples,
adaptive position/force controllers for redundant manipulators Italy, on September 7, 1963. He
in cooperation. International Journal of Robotics Research, 17, received the Laurea degreee and the Re-
232}247. search Doctorate degree in Electronic En-
Luecke, G. R., & Lai, K. W. (1997). A joint error-feedback approach gineering from the University of Naples in
to internal force regulation in cooperating manipulator systems. 1987 and 1992, respectively. From 1988 to
Journal of Robotic Systems, 14, 631}648. date he has been working at the Depart-
ment of Computer and Systems Engineer-
Roberson, R. E., & Schwertassek, R. (1988). Dynamics of multibody
ing of the University of Naples where he is
systems. Berlin: Springer. currently Associate Professor of Auto-
Tomei, P. (1991). Adaptive PD controller for robot manipulators. IEEE matic Control. He is also memeber of the
Transactions on Robotics and Automation, 7, 565}570. scienti"c committee of the research con-
Uchiyama, M., & Dauchez, P. (1993). Symmetric kinematic formulation sortium CREATE. From June to November 1991 and from June to
and non-master/slave coordinated control of two-arm robots. September 1995 he was a visiting scientist at the Laboratoire d'Auto-
Advanced Robotics, 7, 361}383. matique et de Microelectronique de Montpellier in France. His current
Walker, I. D., Freeman, R. A., & Marcus, S. I. (1989). Analysis of motion research interests include control of redundant and cooperative
and internal loading of objects grasped by multiple cooperating manipulators, fault detection and recovery in dynamic systems, control
of discrete-event systems. He has published three books and more than
manipulators. International Journal of Robotics Research, 10,
60 journal and conference papers.
396}409.
Wen, J. T, & Kreutz, K. (1992). Motion and force control of multiple
robotic manipulators. Automatica, 28, 729}743.
Stefano Chiaverini was born in Naples,
Italy, on December 5, 1961. He received
the Laurea degreee and the Research Doc-
Fabrizio Caccavale was born in Naples, torate degree in Electronic Engineering
Italy, on November 14, 1965. He received from the University of Naples in 1986 and
the Laurea degree and the Research Doc- 1990, respectively. From 1990 to 1998 he
torate degree in Electronic Engineering has been working at the Department of
from the University of Naples in 1993 and Computer and Systems Engineering of the
1997, respectively. Since 1997 he has been University of Naples. He is currently Asso-
working at the Faculty of Engineering of ciate Professor of Automatic Control in
the University of Naples, where he is cur- the Faculty of Engineering of the Univer-
rently Post-Doctorate Fellow of Robotics sity of Cassino. From January to June
in the Department of Computer and Sys- 1989 he was a visiting scientist at the Robotics Laboratory of the
tems Engineering. From April to October German Aerospace Research Establishment (DLR). His research inter-
1996 he was a visiting scholar at the De- ests include manipulator inverse kinematics techniques, redundant
partment of Electrical and Computer Engineering of Rice University. manipulator control, force/motion control of manipulators,
His research interests include manipulator inverse kinematics tech- cooperative robot manipulation, and underwater robotics. He
niques, cooperative robot manipulation, and nonlinear control of has published more than 80 journal and conference papers. He is
mechanical systems. He has published more than 30 journal and confer- co-editor of the book Complex Robotic Systems (London: Springer-
ence papers. Verlag, 1998).