\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}