# Aerial Tele-Manipulation with Passive Tool via Parallel Position/Force Control +
## 2 Dynamic Model  
The bilateral teleoperation system consists of: a human operator, a haptic device (master), an aerial manipulator (slave), and the remote environment (cf. Section 3). This section presents the dynamic model of the aerial manipulator (a quadrotor UAV equipped with a passive lightweight tool) in contact with the environment. The tool is rigidly connected to the top of the quadrotor, and a lightweight rigid link ensures enough room between the propellers and the end-effector to allow safe contact with the environment. A compliant spherical joint connects the lightweight rigid link to the end-effector, and an elastic shock absorber damper along with the compliant joint help to establish smoother contacts (Figure 2). In the following we assume that the spherical joint rotation is small, so the springs remain in their linear region, the weight of the tool is negligible, and the tool link is rigid. Let us define the world frame \(\mathcal{W}:\{O_{\mathcal{W}},\mathbf{x}_{w},\mathbf{y}_{w},\mathbf{z}_{w}\}\), the body frame \(\mathcal{B}:\{O_{\mathcal{B}},\mathbf{x}_{b},\mathbf{y}_{b},\mathbf{z}_{b}\}\) for the robot, and the contact frame \(\mathcal{C}:\{O_{\mathcal{C}},\mathbf{o},\mathbf{t},\mathbf{n}\}\) placed at the contact point (Figure 3). The position of \(O_{\mathcal{B}}\) in \(\mathcal{W}\) is indicated with \(\mathbf{p}=[x\ y\ z]^{\top}\in\mathbb{R}^{3}\), \(R\in SO(3)\) is the rotation matrix representing the orientation of \(\mathcal{B}\) in \(\mathcal{W}\). We consider the RPY parameterization of \(R\), that is, \(\mathbf{\eta}:=[\mathbf{\phi}\ \theta\ \psi]^{\top}\in\mathbb{R}^{3}\), where \(\theta,\theta,\mathbf{\eta}\) are the roll, pitch and yaw angles, respectively, and are bounded as \(\frac{\pi}{2}<\phi<\frac{\pi}{2}\), \(\frac{\pi}{2}^{-\frac{\pi}{2}}<\theta<\frac{\pi}{2}\), and \(-\pi<\psi\leq\pi\). The end-effector position expressed in \(\mathcal{W}\) is \(\mathbf{p_{e}}=\mathbf{p}+R\mathbf{d}\), where \(\mathbf{d}\) is the end-effector position vector in \(\mathcal{B}\). The angular velocity of \(\mathcal{B}\), denoted by \(\mathbf{\omega}\in\mathbb{R}^{3}\), is related to the derivative of Euler angles \(\dot{\mathbf{\eta}}\) by \(\mathbf{\omega}=E(\mathbf{\eta})\dot{\mathbf{\eta}}\) where \(E(\mathbf{\eta})\in\mathbb{R}^{3\times 3}\) is defined according to \(R\). The robot dynamics can be expressed in terms of robot pose \(\mathbf{x}=[\mathbf{p}^{\top}\ \mathbf{\eta}^{\top}]^{\top}\in\mathbb{R}^{6}\), in \(\mathcal{W}\), as follows. \[M_{\mathrm{s}}(\mathbf{x})\ddot{\mathbf{x}}+C_{\mathrm{s}}(\mathbf{x},\dot{\mathbf{x}})\dot{ \mathbf{x}}+\mathbf{g}(\mathbf{x})=\mathbf{w}+\mathbf{w_{e}}, \tag{1}\]  
where \(M_{\mathrm{s}}=diag\{mI_{3\times 3},M\}\), with \(M=E^{\top}JE\), is the inertia matrix in which \(m\in\mathbb{R}^{+}\), \(J\in\mathbb{R}^{3\times 3}\) are the robot mass and moment of inertia matrix, \(C_{\mathrm{s}}=diag\{\mathbf{0}_{3\times 3},C\}\), with \(C=E^{\top}(J\dot{E}+S(E\dot{\mathbf{\eta}})JE)\) includes the Coriolis/centripetal dynamics in which \(S(\mathbf{a})\) is the skew-symmetric matrix of a generic vector \(\mathbf{a}\); \(\mathbf{g}=[mg\mathbf{z}_{w}^{\top},\mathbf{0}_{3}]^{\top}\) is the gravity vector with \(g\) being the gravity acceleration constant. \(\mathbf{w}=[(u_{z}Rz_{b}),\mathbf{u}_{\eta}^{\top}]^{\top}\) is the robot control wrench in which the magnitude of the total thrust acting along the \(\mathbf{z}_{b}\) direction is denoted with \(u_{z}\in\mathbb{R}^{+}\), and \(\mathbf{u}_{\eta}\in\mathbb{R}^{3}\) is the rotational control moment. \(\mathbf{w_{e}}=[f_{t}^{\top}\ \ (S(\mathbf{d})\mathbf{f_{t}}+\mathbf{\tau_{r}})^{\top}]^{\top}\) is the external wrench applied to the system.