\begin{aligned} M(q) =&
\begin{bmatrix} (m_1 + m_2)l_1^2 + m_2 l_2^2 + 2 m_2 l_1l_2 c_2 & m_2 l_2^2
+ m_2 l_1 l_2 c_2 \\ m_2 l_2^2 + m_2 l_1 l_2 c_2 & m_2 l_2^2 \end{bmatrix}
\\ C(q,\dot{q}) =& \begin{bmatrix} 0 & -m_2 l_1 l_2 (2\dot{q}_1 +
\dot{q}_2)s_2 \\ m_2 l_1 l_2 \dot{q}_1 s_2 & 0 \end{bmatrix} \\
\tau_g(q) =& -g \begin{bmatrix} (m_1 + m_2) l_1 s_1 + m_2 l_2 s_{1+2} \\
m_2 l_2 s_{1+2} \end{bmatrix} , \quad B = \begin{bmatrix} 1 & 0 \\ 0 & 1
\end{bmatrix}
\end{aligned}