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,.