Vectors  k  e  ~m  and  X e  ~m  are  contact  force  set  points  and  measures  expressed  in  the  contact  frame,  respectively,  m  being  the  number  of  degrees  of  freedom  constrained  by  the  contact  (m  =  3  for  the  RUB  action).  Force  control  is  implemented  as  an  outer  loop  cascaded  to  the  independent  motor  position  loops.  It  adds  a  bias  ~F  ~ 9tn  (n  being  tile  number  of robot  axes)  to  the  nominal motor position  --M  set  points  qm  and  thus  it  integrates  easily  in  a  conunon  functional  architecture  of  an  industrial  controller,  consisting  of  trajectory  planning,  kinematics  inversion  and joint  position  control.  The  bias  is  computed  from  the  force  error  by  means  of  three  blocks:  the  force  regulator,  the  computation of  motor torques,  and  the  torque  conversion  into  motor  displacement. 

The  motor  torques  -F  ~n  required  to  actuate  a  %m force vector  u  ~  ~n  are  computed as: -F  = pTjc(q) TK(q)T  u  T  N  (1) 

Equation  (1)  is  derived  from  the  constrained  dynamic  model  of  the  manipulator  (McClamroch  and  Wang,  1988)  by  imposing  that  Xm  F  controls  the  interaction  forces  without  disturbing  the  programmed  motion in  the  unconstrained  directions  (Ferretti,  et  al.,  1995b).  Matrix  P[nxnl  in  (1)  accounts  for  the  kinematic  relations  among  motor  coordinates  qm  and joint  coordinates  q  (in  general,  P  is  not diagonal,  and Pii  = Ni,  Ni being the  i-th  gear  ratio).  Matrix  Jc(q)[6×n]  is  the  manipulator  Jacobian,  relating  joint  velocities  to  the  linear  and  angular velocities  of the  compliant  frame,  expressed  in  the  compliant  frame  itself.  In  practice,  Jc(q)  is  computed from the  Jacobian  J(q),  relating  joint  and  compliant  frame  velocities  expressed  in  the  base  frame,  and from the  robot  direct kinematics  (rotation  matrix).  Matrix  K(q)lm×61  is  a  mapping  between  vector  7~  and  the  vector  of  contact  forces  and  moments  F  c  ~  9t  6  exerted  in  the  compliant  frame,  and  can  be  directly  derived  by  analysing  the  directions  of  constrained  motion  (Ferretti,  et  al.,  1995b).  For  the  RUB  action,  matrix  K(q)  is  constant,  and is  given by: 

K(10=K=[013×21  I[3×31  013xl]]. 

The  transfer  function  diagonal  matrix  C(s)~n×n!  is  introduced  to  transform  motor  torques  i~n  into  corresponding  motor  rotations  ~F.  It  accounts  for  position  regulator  transfer  functions  and  for  joint  torsional  flexibility.  It  is  shown  in  (Ferretti,  et  al.,  1993),  that  under  reasonable  assumptions,  C(s)  is  a  diagonal  matrix  whose  elements  Cii(s)  are  second order  transfer  functions  whose  singularities  depend  on  Kpi,  Kli,  KDi,  the  proportional,  integral  and  derivative  gain,  respectively,  of  the  i-th  PID  regulator,  and on h),  the  stiffness  constant of  the  i-th  joint. KI can be experimentally  identified  (Ferretti,  et  al.,  1994ab).  In  particular,  it  results  that  C~(0) =  1/ K,.