ラグランジュ/ニュートン-オイラー:
$$\mathbf{M}(\boldsymbol{\theta})\ddot{\boldsymbol{\theta}}+ \mathbf{C}(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})\dot{\boldsymbol{\theta}}+ \mathbf{G}(\boldsymbol{\theta}) = \boldsymbol{\tau}$$慣性行列:$M_{11}= (m_1+m_2)L_1^2 + m_2 L_2^2 + 2m_2 L_1 L_2 c_2 + I_{z1}$
重力ベクトル:$G_1 = (m_1+m_2)g L_1 c_1 + m_2 g L_2 c_{12}$
三次スプライン:$\theta(t)=\theta_0+(\theta_f-\theta_0)\left[3(t/T)^2-2(t/T)^3\right]$