# Aerial Tele-Manipulation with Passive Tool via Parallel Position/Force Control +
## 3 Control System
### Force Control  
The force is regulated by providing the internal position control loop with an appropriate reference. The robot's desired position \(\mathbf{p_{d}}\) and desired yaw \(\psi_{d}\) are four commanded states of the system, noting that the system is mechanically under-actuated. As depicted in Figure 5, the outer force regulating the feedback loop generates additional terms \((\mathbf{p_{f}},\psi_{f})\), depending on the force error, that are added to the previously commanded pose \((\mathbf{p_{p}},\psi_{p})\) in the free flight. As we aimed at contact interaction with the environment using the end-effector, we command the end-effector pose; therefore, we convert the end-effector pose to the COM pose by including the term \(-Rd\) in the desired pose. Thus, \(\mathbf{p_{d}}\) and \(\psi_{d}\) are expressed as:  
\[ \begin{cases}\mathbf{p_{d}}=\mathbf{p_{p}}+\mathbf{p_{f}}-R\mathbf{d}\\ \psi_{d}=\psi_{p}+\psi_{f}.\end{cases}  \tag{6}\]  
In the sequel, we see how \(\mathbf{p_{p}}\), \(\psi_{p}\), \(\mathbf{p_{f}}\), and \(\psi_{f}\) are generated from the user command, given by \(\mathbf{x}_{m}\), based on the aerial manipulator contact condition. When the robot is in free flight condition, the master robot position \((\mathbf{x}_{m})\) is interpreted as a position command; while when the end-effector comes in contact \(\mathbf{x}_{m}\) is interpreted as the desired force. In order to distinguish the two conditions, let us introduce the contact function \(u(\mathbf{f},\mathbf{f_{d}})\) as:  
\[u(t_{k})= \begin{cases}1&\text{if }f_{n}(t_{k})>0\ \&\ f_{n}(t_{k-1})>\epsilon\\ 0&\text{if }f_{n}(t_{k})=0\ \&\ f_{d,n}(t_{k})<-\epsilon\\ u(t_{k-1})&\text{otherwise}.\end{cases}  \tag{7}\]  
This hysteresis-like function is intended to prevent the chattering phenomenon in the attachment and detachment phases. In Equation (7), \(f_{n}(t_{k})\) is the normal component of the measured contact force at \(t_{k}\) instance and \(f_{n}(t_{k-1})\) is the previous sample of the same measurement; \(f_{d,n}(t_{k})\) is the normal component of the commanded force, \(u(t_{k-1})\) is the previous output of the function (\(u(0)=0\)), and \(\epsilon\in\mathbb{R}^{+}\) is a small positive value. In the contact condition, if the commanded normal component of the force is negative (\(f_{d,n}(t_{k})<-\epsilon\)) and the normal component of the applied force is zero, the detachment takes place. During free flight \(u(t)=0\) and the component of the desired position that is intended to control the aerial manipulator position in free flight is generated by integrating the master position as:  
\[\mathbf{p_{p}}=\mathbf{p_{d}}(t_{0})+\int_{t_{0}}^{t}(1-u(t))K_{p}^{f}\mathbf{x_{m}}\ dt, \tag{8}\]  
where \(K_{p}^{f}\) is a \(3\times 3\) matrix that rotates and scales the master robot motion appropriately. The aerial manipulator heading \(\psi_{p}\) in free flight is directly commanded by the user, through mapping one of the master robot motions, similar to \(\mathbf{p_{p}}\). When the end-effector establishes a contact \(u(t)=1\), and thus \((1-u(t))=0\), in this case the integral stores the end-effector position at the contact moment, and the motion of the master robot during the contact interaction is used to command the desired force. The force commanded by the human operator, \(\mathbf{f_{d}^{*}}\in\mathbb{R}^{3}\), expressed in \(\mathcal{C}\), is generated as:  
\[\mathbf{f_{d}^{*}}=u(t)K_{f}^{f}\mathbf{x_{m}}, \tag{9}\]  
where \(K_{f}^{f}\) is a \(3\times 3\) matrix that rotates and scales the master robot motion appropriately. To prevent the desired force violating the contact constraint (3), the commanded force is modified as follows:\[f_{d}= \begin{cases}f_{d}^{*}&\text{if }\frac{1}{\mu_{s}}\sqrt{(f_{o}^{*})^{2}+(f_{ t}^{*})^{2}}+\frac{1}{\sqrt{r_{d}^{2}+\mu_{t}^{2}}}|\tau|\leq f_{n}^{*}\\ \sigma(f_{d}^{*})&\text{otherwise,}\end{cases}