Bi-Rotor with Payload

For generalized coordinates $q={\left[\begin{array}{cccc}y& z& \theta & \varphi \end{array}\right]}^{\top }$ the system’s kinetic and potential energy are $$\begin{array}{rl}T& =\frac{1}{2}\left(m_Q({\dot{y}}^2+{\dot{z}}^2)+I_Q{\dot{\theta }}^2+m_P({\dot{y}}_P^2+{\dot{z}}_P^2)\right)\\ V& =g{m}_{Q}z+g{m}_P(z-l\cos (\varphi ))\end{array}$$ where $${\dot{y}}_P=\dot{y}+l\dot{\varphi }\cos (\varphi ),{\dot{z}}_P=\dot{z}+l\dot{\varphi }\sin (\varphi ).$$ The two forces acting on the system and their action arms are $$F_{1,2}=u_{1,2}\left[\begin{array}{c}-\sin (\theta )\\ \cos (\theta )\end{array}\right],r_{1,2}=\left[\begin{array}{c}y\mp a\cos (\theta )\\ z\mp a\sin (\theta )\end{array}\right].$$ The individual terms of the manipulator equations are then $$\begin{array}{rl}M& =\left[\begin{array}{cccc}{m}_Q+m_P& 0& 0& l{m}_P\cos (\varphi )\\ 0& m_Q+m_P& 0& l{m}_P\sin (\varphi )\\ 0& 0& I_Q& 0\\ l{m}_P\cos (\varphi )& l{m}_P\sin (\varphi )& 0& l^2{m}_P\end{array}\right]\\ c& =\left[\begin{array}{c}-l{m}_P\sin (\varphi ){\dot{\varphi }}^2\\ l{m}_P\cos (\varphi ){\dot{\varphi }}^2\\ 0\\ 0\end{array}\right]\\ {\tau }_p& =\left[\begin{array}{c}0\\ -g(m_P+m_Q)\\ 0\\ -gl{m}_P\sin (\varphi )\end{array}\right]\\ B& =\left[\begin{array}{cc}-\sin (\theta )& -\sin (\theta )\\ \cos (\theta )& \cos (\theta )\\ -a& a\\ 0& 0\end{array}\right]\end{array}$$