Two-link Rigid Robot Manipulator

Consider a two-link rigid robot manipulator moving a horizontal plane. The dynamic equations of this MIMO system are

$$\left[\matrix{ \ddot{q}_{1}\cr \ddot{q}_{2} }\right]=\left[\matrix{ M_{11} & M_{12}\cr M_{21} & M_{22} }\right]^{-1} \left\{\left[\matrix{ u_{1}\cr u_{2} }\right]-\left[\matrix{ -h\dot{q}_{2} & -h(\dot{q}_{1}+\dot{q}_{2})\cr h\dot{q}_{1} & 0 }\right]\left[\matrix{ \dot{q}_{1}\cr \dot{q}_{2} }\right]\right\},$$

where

$\begin{align*} M_{11}&=a_{1}+2a_{3}\cos(q_{2})+2a_{4} \sin (q_{2}),\ M_{22}=a_{2} \\ M_{12}&=M_{21}=a_{2}+\alpha_{3}\cos(q_{2})+a_{4}\sin(q_{2}) \\ h&=a_{3}\sin(q_{2})-a_{4}\cos(q_{2}) \end{align*}$

with

$\begin{align*} a_{1}&=I_{1}+m_{1}l_{c1}^{2}+I_{e}+m_{e}l_{ce}^{2}+m_{e}l_{1}^{2} \\ a_{2}&=I_{e}+m_{e}l_{ce}^{2} \\ a_{3}&=m_{e}l_{1}l_{ce}\cos(\delta_{e}) \\ a_{4}&=m_{e}l_{1}l_{ce}\sin(\delta_{e}). \end{align*}$