# Aerial Tele-Manipulation with Passive Tool via Parallel Position/Force Control +
## 3 Control System
### Force Control
 \tag{10}\]  
where \(f_{d}^{*}=[f_{o}^{*},\ f_{t}^{*},\ f_{n}^{*}]^{\top}\), and \(\sigma(f_{d}^{*})\) is a function projecting the desired commanded force \(f_{d}^{*}\) to the surface of the contact constraints (3) as follows:  
\[\sigma(f_{d}^{*})=\cos(\alpha)f_{d}^{*}+\sin(\alpha)(S(a)f_{d}^{*})+(1-\cos( \alpha))(a^{\top}f_{d}^{*})a+\frac{1}{\sqrt{r_{d}^{2}+\mu_{t}^{2}}}|\tau| \boldsymbol{n}, \tag{11}\]  
where \(\boldsymbol{a}=(S(f_{d}^{*})\boldsymbol{n})/||(S(f_{d}^{*})\boldsymbol{n})||\) is the unit axis of rotation, \(\alpha=\beta-\gamma\) is the required angle to rotate \(f_{d}^{*}\) around \(\boldsymbol{a}\) to project it on the surface of the friction cone, \(\beta\) is the angle between \(f_{d}^{*}\) and \(\boldsymbol{n}\), and \(\gamma=\tan^{-1}(\mu_{s})\) is the translational friction cone angle. This function minimally increases the normal component (by adding \((r_{d}^{2}+\mu_{t}^{2})^{-0.5}|\tau|\boldsymbol{n}\)) to avoid torsional slippage and lifting up the end-effector disk, and applies the minimum rotation to its direction to keep it within the feasible force range. The next step to generate \(\boldsymbol{p}_{f}\) and \(\psi_{f}\) is to feed the force error \(\boldsymbol{\tilde{f}}=\boldsymbol{f}-\boldsymbol{f_{d}}=[\tilde{f}_{o}\, \tilde{f}_{t}\,\tilde{f}_{n}]^{\top}\) to a PI-controller as:  
\[\boldsymbol{u_{f}}=-K_{pf}\boldsymbol{\tilde{f}}-K_{lf}\int_{0}^{t}\boldsymbol {\tilde{f}}dt, \tag{12}\]  
where \(K_{pf},K_{lf}\in\mathbb{R}^{3\times 3}\) are proportional and integral diagonal gain matrices, respectively. The PI-output \(\boldsymbol{u_{f}}=[u_{o}\ u_{t}\ u_{n}]^{\top}\)--after appropriate transformations--generates \(\boldsymbol{p}_{f}\) and \(\psi_{f}\). At the contact moment, in which the springs are in rest position, we define the contact frame \(\mathcal{C}\) which is its orientation w.r.t. \(\mathcal{B}\) represented by rotation matrix \(R_{c}^{b}\). Let \(\boldsymbol{d}=[d_{x},0,d_{z}]^{\top}\); we can always define \(\mathcal{C}\) with a constant \(R_{c}^{b}\) such that \(\boldsymbol{d}^{c}=[d_{o},0,d_{n}]^{\top}\). Expanding (2) we get:  
\[ \begin{cases}f_{o}(\delta o,\delta\eta_{t})=k_{lo}\delta o-\frac{d_{x}k_{l}}{ \|d\|^{2}}\delta\eta_{t}\\ f_{t}(\delta t,\delta\eta_{o},\delta\eta_{n})=\frac{d_{x}k_{n}}{\|d\|^{2}} \delta\eta_{0}+k_{tt}\delta t-\frac{d_{x}k_{m}}{\|d\|^{2}}\delta\eta_{n}\\ f_{n}(\delta n,\delta\eta_{t})=k_{tt}\delta n+\frac{d_{x}k_{t}}{\|d\|^{2}} \delta\eta_{t}.\end{cases}  \tag{13}\]  
Therefore, \(f_{n}\) and \(f_{o}\) can be regulated by commanding the motion along \(\boldsymbol{n}\) and \(\boldsymbol{t}\), respectively. To control \(f_{t}\), we choose \(\delta\eta_{o}\), and the reason is: due to the under-actuation of the quadrotor, changing \(\delta t\) requires changing the roll and pitch angles of the aerial manipulator and this results in applying an undesirable moment around the normal axis of the contact frame. The usage of rotation to generate the desired force also leads to a wider range of feasible forces, as it not only relies on the limited linear friction of the end-effector and object surfaces. \(\delta\eta_{o}\) can be considered as changing the yaw angle \(\psi\) (see Figure 3). Therefore, \(\boldsymbol{p}_{f}\) and \(\psi_{f}\) can be obtained as follows:  
\[ \begin{cases}\boldsymbol{p_{f}}=R^{\top}R_{c}^{b}[u_{0}\ 0\ u_{n}]^{\top}\\ \psi_{f}=u_{t}.\end{cases}  \tag{14}\]