# Aerial Tele-Manipulation with Passive Tool via Parallel Position/Force Control +
## 2 Dynamic Model
The wrench applied to the end-effector is modeled as spring wrench \((\mathbf{f_{t}},\mathbf{\tau_{r}})\), expressed in \(\mathcal{W}\) as \(\mathbf{f_{t}}=-R_{\mathrm{s}}^{\omega}K_{t}\mathbf{\delta}\mathbf{p}^{\mathbf{c}}\) and \(\mathbf{\tau_{r}}=-R_{\mathrm{c}}^{\omega}K_{r}\mathbf{\delta}\mathbf{p}^{\mathbf{c}}\), where \(R_{\mathrm{c}}^{\omega}\) is the rotation matrix representing the orientation of \(\mathcal{C}\) w.r.t. \(\mathcal{W}\), and \(K_{t}=diag\{k_{to},k_{tt},k_{m}\},K_{r}=diag\{k_{to},k_{rt},k_{rn}\}\) are the diagonal stiffness matrices, \(\mathbf{\delta}\mathbf{p}^{\mathbf{c}}=[\delta\ \delta\ \delta t\ \delta n]^{\top}\) is the compression of the linear spring, and \(\mathbf{\delta}\mathbf{p}^{\mathbf{c}}=[\delta\eta_{o}\ \delta\eta_{t}\ \delta\eta_{n}]^{\top}\) is the compression of the angular spring, expressed in \(\mathcal{C}\), respectively. We consider the _soft finger model_ for the contact in which all three components of force and the normal component of the torque are transmitted in the contact independently [27]. Considering this model and the force torque balance for the end-effector, the wrench transmitted by the end-effector, _that is,_ applied force \(\mathbf{f}=[f_{o}\ f_{t}\ f_{n}]^{\top}\) and torque \(\mathbf{\tau}=[0\ 0\ \tau_{n}]^{\top}\), in \(\mathcal{C}\), can be obtained as:  
\[ \begin{cases}\mathbf{f}=-f_{t}^{\top}-\frac{1}{\|\mathbf{d}\|^{2}}S(\mathbf{d}^{\mathbf{c}})\mathbf{ \tau}_{r}^{\mathbf{c}}=K_{t}\mathbf{\delta}\mathbf{p}^{\mathbf{c}}+\frac{1}{\|\mathbf{d}\|^{2}}S( \mathbf{d}^{\mathbf{c}})K_{r}\mathbf{\delta}\mathbf{\eta}^{\mathbf{c}}\\ \tau_{n}=k_{rn}\delta\eta_{n}.\end{cases}  \tag{2}\]Equation (2) is used in the next section to define the control inputs of the force controller. The constraints of the force and torque to keep the contact are as follows:  
\[ \begin{cases}0\leq f_{n}\\ \sqrt{f_{o}^{2}+f_{t}^{2}}/\mu_{s}\leq f_{n}\\ |\tau_{n}|/\mu_{t}\leq f_{n}\\ \sqrt{\tau_{o}^{2}+\tau_{t}^{2}}/r_{d}\leq f_{n},\end{cases}  \tag{3}\]  
where \(\mu_{s}\) and \(\mu_{t}\) are the linear and angular friction coefficients of the end-effector surface with the object surface, and \(r_{d}\) is the end-effector surface disk radius. The first constraint is the unilateral condition, the second one is to avoid translation slippage, the third one is to avoid rotational slippage, and the last one is to prevent the disk lifting up. The constraint (3) is used in the next section to modify the desired force vector in order to keep the contact. The dynamic model of the haptic interface, that is, the master robot, considering an inverse dynamic controller with gravity and nonlinear compensation [28], in the operational workspace, can be described as  
\[M_{m}\tilde{\mathbf{x}}_{m}+K_{mD}\tilde{\mathbf{x}}_{m}=K_{m}\tilde{\mathbf{x}}_{m}+\mathbf{f }_{h}-\mathbf{f}_{c}, \tag{4}\]  
where \(\mathbf{x}_{m}\in\mathbb{R}^{n_{m}}\) (\(n_{m}\) is the number of master robot's actuated DOFs is the master device pose, \(\tilde{\mathbf{x}}_{m}\) is the pose error, \(M_{m}\) is the diagonalized inertia matrix, \(K_{mP},K_{mD}\) are the PD-controller gains regulating the desired master robot pose, \(\mathbf{f}_{h}\) is the force applied by the human operator and \(\mathbf{f}_{c}\) is the reflected teleoperation force.