## 3 Integration of Visual Servo Control
### Reaction Force/Torque Feedback Control  
The internal forces measured by the force/torque sensor \(\mathbf{F}_{S}\) is described in the installation coordinate and transformed to the EEF by:  
\[\mathbf{F}_{B}= \begin{bmatrix}{}^{B}R_{S}&0\\ \times{}^{B}\mathbf{P}_{S}^{B}R_{S}&{}^{B}R_{S}\end{bmatrix} \cdot\mathbf{F}_{S} \tag{6}\]  
where \(P\) and \(R\) are the position and rotation matrix expressed in the reference frame sensor to EEF, \(\mathbf{F}_{B}\) represents the interaction force and torque from the manipulator motion. The forward direction is defined from joint to EFF. A special mechanical interface (MI) between CMM and EMM combined the macro and micro manipulator in series. It consists of two different grapple fixtures for each manipulator. Each end of the mechanical interface corresponds to the end-effector of the CMM and EMM, respectively. The coordinate frame defined for the End of CMM (EOC) and Base of EMM (BOE) is shown in Figure 5.  
The relation expressed in the transfer matrix is:  
\[\text{T}_{\text{MI}}= \begin{bmatrix}-1&0&0&d\\ 0&-1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix}  \tag{7}\]  
The Jacobian of the CMM form BOE to the base of the macro-micro manipulator (BMM) is:  
\[{}^{BMM}\mathbf{J}_{\text{CMM\_BOE}}= \begin{bmatrix}I_{3}&-^{BMM}R_{\text{CMM}} \cdot\mathbf{P}_{\text{MI}}\times{}^{BMM}R_{\text{CMM}}^{T}\\ 0_{3}&I_{3}\end{bmatrix} \cdot{}^{BMM}\mathbf{J}_{\text{CMM}} \tag{8}\]  
Figure 4: The joint stiffness of the CMM. Figure 5: The coordinate definition of the mechanical interface. where \({}^{BMM}\mathbf{J}_{\text{CMM}}\) and \(R_{\text{CMM}}\) are expressed in the base coordinate frame of CMM. The calculation method regards the mechanical interface as the tool of the CMM. The deflection torque of the joints due to the reaction force is:  
\[\mathbf{\tau}_{ext}={}^{BOE}\mathbf{J}_{CMM\_BOE}^{T}\cdot\mathbf{F}_{B} \tag{9}\]  
where \({}^{BOE}\mathbf{J}_{CMM\_BOE}\) is the Jacobian of CMM with the mechanical interface and expressed in the coordinate of BOE. \[{}^{BOE}\mathbf{J}_{CMM\_BOE}= \begin{bmatrix}{}^{BOE}R_{BMM}&0\\ 0&{}^{BOE}R_{BMM}\end{bmatrix} \cdot{}^{BMM}\mathbf{J}_{CMM\_BOE} \tag{10}\]  
It is essential to study the manipulator with a stationary initial state. Ignoring the force sensor's elasticity in the model and treating it as an ideal force sensing element, from the dynamic Equation (4), the deflection dynamic can be striped as:  
\[\mathbf{H}_{b}\mathbf{\ddot{q}}_{b}+\mathbf{K}_{b}\Delta\mathbf{q}_{b}+\mathbf{C}_{b}=-\mathbf{\tau}_{ ext} \tag{11}\]  
We can see from the base deflection dynamics that the base forces, which are measured by the FTS, reflect the motion of the base. The deflection forces depend on the robot's state and the control input while performing the visual servo task. The desired closed-loop base deflection dynamic is:  
\[\mathbf{H}_{b}\mathbf{\ddot{q}}_{b}+(\mathbf{D}_{b}+G)\mathbf{\dot{q}}_{b}+\mathbf{K}_{b}\Delta\mathbf{ q}_{b}=0 \tag{12}\]  
The base reaction forces are feedback to achieve the desired closed-loop dynamic, where a positive definite damping term \(G\) is added to damp out the base deflection:  
\[\mathbf{\dot{q}}_{bd}=\mathbf{\tau}_{ext}/G \tag{13}\]  
Due to the slow deflection of the flexible base, the dynamic inertial forces of the macro manipulator can be neglected, the contribution of the joint deflection in the coordinate of BOE denoted by the FTS is:  
\[{}^{BOE}\mathbf{v}_{BOE\_d}={}^{BOE}\mathbf{J}_{CMM\_BOE}\cdot\mathbf{\dot{q}}_{bd}={}^{ BOE}\mathbf{J}_{CMM\_BOE}\cdot G^{-1}\cdot{}^{BOE}\mathbf{J}_{CMM\_BOE}^{T}\cdot\mathbf{F}_{B} \tag{14}\]  
Then, the base deflection can be converted to the EEF by:  
\[{}^{Tool}\mathbf{v}_{f}={}^{Tool}T_{BOE}\cdot{}^{BOE}\mathbf{v}_{BOE\_d} \tag{15}\]  
where \({}^{Tool}T_{BOE}\) is the velocity convert matrix from the tool of the EMM to the BOE. \[{}^{Tool}T_{BOE}= \begin{bmatrix}{}^{BOE}R_{Tool}^{T}&-{}^{BOE}R_{Tool}^{T} \cdot P_{Tool}\times\\ 0&{}^{BOE}R_{Tool}^{T}\end{bmatrix}  \tag{16}\]  
The application process of base FTS is needed.