# Aerial Physical Interaction in Grabbing Conditions with Lightweight and Compliant Dual Arms
## 3 Modelling  
### Kinematics  
As usual, three reference frames are defined for the aerial manipulation system, as illustrated in Figure 5: the Earth fixed frame \(\{\mathbf{E}\}\) (inertial), the multirotor body frame \(\{\mathbf{B}\}\), and the manipulator frame \(\{\mathbf{i}\}\), with \(i=\{1,2\}\) for the left/right arms. In the following, \({}^{A}\mathbf{r}_{\mathbf{B}}\) denotes the position of a certain point \(\mathbf{B}\) w.r.t. (with respect to) reference frame \(\{\mathbf{A}\}\). In this way, \({}^{E}\mathbf{r}_{\mathbf{B}}=[x,\ y,\ z]^{T}\) and \({}^{E}\mathbf{\eta}_{\mathbf{B}}=[\phi,\ \theta,\ \psi]^{T}\) represent the multirotor position and orientation relative to \(\{\mathbf{E}\}\), and \({}^{i}\mathbf{r}_{TCP,i}\) is the position of the tool center point (TCP) of the \(i\)-th manipulator expressed in its own frame. The origin of \(\{\mathbf{i}\}\) is located at the intersection of the shoulder joints, with the \(x\)-axis pointing forwards, the \(y\)-axis parallel to the baseline of the two arms, and the \(z\)-axis pointing upwards.  
The three reference frames are related through the corresponding transformation matrices:  
\[{}^{E}\mathbf{T}_{\mathbf{B}}=\left[\begin{array}{cc}{}^{E}\mathbf{R}_{\mathbf{B}}(\phi,\ \theta,\ \psi)&{}^{E}\mathbf{r}_{\mathbf{B}}\\ \mathbf{0}_{1\times 3}&1\end{array}\right]\ \ \ ;\ \ \ \ {}^{B}\mathbf{T}_{i}=\left[ \begin{array}{cc}I_{3\times 3}&{}^{B}\mathbf{r}_{i}\\ \mathbf{0}_{1\times 3}&1\end{array}\right]\ \ \ ;\ \ \ {}^{E}\mathbf{T}_{i}={}^{E}\mathbf{T}_{\mathbf{B}}{}^{B}\mathbf{T}_{i} \tag{1}\]  
where \({}^{E}\mathbf{R}_{\mathbf{B}}\) is the multirotor rotation matrix; \({}^{B}\mathbf{r}_{i}=[D_{x},\ \pm D/2,\ D_{z}]^{T}\) is the origin of \(\{\mathbf{i}\}\) relative to \(\{\mathbf{B}\}\), with \(D_{x}\) and \(D_{z}\) being the displacement of the arms with respect to \(\{\mathbf{B}\}\) in the x- and z-axes, respectively; and \(D\) is the separation distance between the arms in the \(y\)-axis.  
The arms implement the 3-DOF (degrees of freedom) configuration considered in our previous work [14; 22], with three joints for TCP positioning: shoulder yaw (base), shoulder pitch, and elbow pitch. The wrist joints are not considered due to the convenience to simplify the mechanical construction and reduce the weight of the arms. The rotation angle of the \(j\)-th joint of the \(i\)-th arm is denoted as \(q_{j}^{i}\), whereas \(\theta_{j}^{i}\) is the corresponding servo shaft position. The difference between these two variables is the deflection angle, \(\Delta\theta_{j}^{i}=\theta_{j}^{i}-q_{j}^{i}\), measured by the encoders [17; 21]. The forward and inverse kinematic models are computed as follows (superscript \(i\) is omitted for clarity reasons):  
\[{}^{i}\mathbf{r}_{TCP,i}=F\mathbf{K}\!\left(q^{i}\right)=\left[\begin{array}{c}r(q _{2},\ q_{3})\!\cdot\!\cos(q_{1})\\ r(q_{2},\ q_{3})\!\cdot\!\sin(q_{1})\\ L_{1}\cos(q_{2})+L_{2}\cos(q_{2}+q_{3})\end{array}\right] \tag{2}\]  
Figure 5: Kinematic model of the dual arm aerial manipulator.  
\[\mathbf{q}^{i}=\mathbf{IK}\mathbf{\left({i}\mathbf{r}_{TCP,i}\right)}=\left[\begin{array}{c}atan 2(y,x)\\ \cos^{-1}\left(\frac{x^{2}+y^{2}+x^{2}-l_{1}^{2}-l_{2}^{2}-l_{2}^{2}}{2l_{1} \sqrt{x^{2}+y^{2}}}\right)\\ \cos^{-1}\left(\frac{x^{2}+y^{2}+x^{2}-l_{1}^{2}-l_{2}^{2}}{2l_{1}L_{2}} \right)\end{array}\right] \tag{3}\]  
where \(L_{1}\) and \(L_{2}\) are the upper arm/forearm link lengths, and \(r(q_{2},q_{3})\) is given by  
\[r(q_{2},\ q_{3})=L_{1}\sin(q_{2})+L_{2}\sin(q_{2}+q_{3}) \tag{4}\]