Derivation of the Equation of motion of a three-link-manipulator:

This part is necessary for programming Exercise 6 of Introduction to Robotics Mechanics and Control. The following parameters are specified:

$l1 = l2 = 0.5m$

$m1 = 4.6$Kg

$m2 = 2.3$Kg

$m3 = 1.0$Kg

$g = 9.8 m/s^2$

For link 3 the center of mass is located at the origin of frame {3}

Inertia tensor for link 3 is:

The vector that locate each centre of mass relative to the respective link frame are:

${}^1P_{C_1} = l_1 \hat{X}_1$
${}^2P_{C_2} = l_2 \hat{X}_2$
${}^3P_{C_3} = 0$

Other usefull information:

${}^2P_3 = l_2 \hat{X}_2$ is the position of link 3 in the frame of reference {2} ${}^1P_2 = l_1 \hat{X}_1$ is the position of link 2 in the frame of reference {1}

The outward iteration for link 3

$% $ $% $

$= ... + \begin{bmatrix} l_1\ddot{\theta}_1(c_2s_3 + s_2c_3) - l_1\dot{\theta}^2_1(c_2c_3 - s_2s_3) + g(c_3s_{12} + s_3c_{12}) \\ l_1\ddot{\theta}_1(c_2c_3 - s_2s_3) + l_1\dot{\theta}^2_1(c_2s_3 + s_2c_3) + g(c_{12}c_3 - s_{12}s_3) \\ 0 \end{bmatrix}$ $= \begin{bmatrix} -l_2 c_3 (\dot{\theta}_1 + \dot{\theta}_2)^2 + l_2 s_3 (\ddot{\theta}_1 + \ddot{\theta}_2)\\ l_2 s_3 (\dot{\theta}_1 + \dot{\theta}_2)^2 + l_2 c_3 (\ddot{\theta}_1 + \ddot{\theta}_2)\\ 0 \end{bmatrix} + \begin{bmatrix} l_1\ddot{\theta}_1s_{23} - l_1\dot{\theta}^2_1c_{23} + gs_{123} \\ l_1\ddot{\theta}_1c_{23} + l_1\dot{\theta}^2_1s_{23} + gc_{123} \\ 0 \end{bmatrix}$

${}^3\dot{v}_{C_3} = {}^3\dot{v}_3$ because ${}^3P_{C_3} = 0$

${}^3F_3 = m_3\,{}^3\dot{v}_{C_3} = \begin{bmatrix} m_3 l_2 s_3 (\ddot{\theta}_1 + \ddot{\theta}_2) - m_3 l_2 c_3 (\dot{\theta}_1 + \dot{\theta}_2)^2\\ m_3 l_2 c_3 (\ddot{\theta}_1 + \ddot{\theta}_2) + m_3 l_2 s_3 (\dot{\theta}_1 + \dot{\theta}_2)^2\\ 0 \end{bmatrix} + \begin{bmatrix} m_3 l_1\ddot{\theta}_1s_{23} - m_3 l_1\dot{\theta}^2_1c_{23} + m_3 gs_{123} \\ m_3 l_1\ddot{\theta}_1c_{23} + m_3 l_1\dot{\theta}^2_1s_{23} + m_3 gc_{123} \\ 0 \end{bmatrix}$

${}^3N_3 = {}^{C_3}I_3\, {}^3\dot{\omega}_3 + {}^3\omega_3 \times {}^{C_3}I_3\, {}^3\omega_3 = \begin{bmatrix} 0 \\ 0 \\ I_{zz}(\ddot{\theta_1} + \ddot{\theta_2} + \ddot{\theta_3}) \end{bmatrix}$

The inward iteration for link 3

${}^3f_3 = {}^3F_3$

${}^3n_3 = {}^3N_3$

The inward iteration for link 2

${}^2f_2 = {}^2_3R\, {}^3f_3 + {}^2F_2$ $% $ $= \begin{bmatrix} -m_3 l_2 (\dot{\theta}_1 + \dot{\theta}_2)^2 \\ m_3 l_2 (\ddot{\theta}_1 + \ddot{\theta}_2) \\ 0 \end{bmatrix} + \begin{bmatrix} m_3\,l_1\ddot{\theta}_1s_2 - m_3\,l_1\dot{\theta}^2_1c_2 + m_3\,gs_{12} \\ m_3\,l_1\ddot{\theta}_1c_2 + m_3\,l_1\dot{\theta}^2_1s_2 + m_3\,gc_{12} \\ 0 \end{bmatrix} + \begin{bmatrix} m_2l_1\ddot{\theta}_1s_2 - m_2l_1\dot{\theta}^2_1c_2 + m_2 g s_{12} - m_2l_2(\dot{\theta}_1 + \dot{\theta}_2)^2 \\ m_2l_1\ddot{\theta}_1c_2 + m_2l_1\dot{\theta}^2_1s_2 + m_2 g c_{12} + m_2l_2(\ddot{\theta}_1 + \ddot{\theta}_2) \\ 0 \end{bmatrix}$

${}^2n_2 = {}^2N_2 + {}^2_3R\,{}^3n_3 + {}^2P_{C_2} \times {}^2F_2 + {}^2P_3 \times {}^2_3R\, {}^3f_3$

$% $ $= \begin{bmatrix} 0 \\ 0 \\ m_2l_1l_2\ddot{\theta}_1c_2 + m_2l_1l_2\dot{\theta}^2_1s_2 + m_2 l_2 g c_{12} + m_2l^2_2(\ddot{\theta}_1 + \ddot{\theta}_2) + I_{zz}(\ddot{\theta_1} + \ddot{\theta_2} + \ddot{\theta_3}) \end{bmatrix}$ $+ \begin{bmatrix} 0\\ 0\\ m_3 l^2_2 c_3 (\ddot{\theta}_1 + \ddot{\theta}_2) + m_3 l^2_2 s_3 (\dot{\theta}_1 + \dot{\theta}_2)^2 \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ m_3 l_1 l_2\ddot{\theta}_1c_{23} + m_3 l_1 l_2 \dot{\theta}^2_1s_{23} + m_3 l_2 gc_{123} \end{bmatrix}$

The inward iteration for link 1

$% $ $% $ $% $

$= \begin{bmatrix} -m_3 l_2 c_2(\dot{\theta}_1 + \dot{\theta}_2)^2 - m_3 l_2 s_2 (\ddot{\theta}_1 + \ddot{\theta}_2) \\ m_3 l_2 s_2(\dot{\theta}_1 + \dot{\theta}_2)^2 + m_3 l_2 c_2 (\ddot{\theta}_1 + \ddot{\theta}_2) \end{bmatrix}$ $+ \begin{bmatrix} -m_3\,l_1\dot{\theta}^2_1 + m_3 g s_1 \\ m_3 l_1 \ddot{\theta}_1 + m_3 g s_{12} s_2 + m_3 g c_{12} c_2 \\ 0 \end{bmatrix}$ $+ \begin{bmatrix} m_2 g s_1 - m_2 l_2 (\dot{\theta}_1 + \dot{\theta}_2)^2 c_2 - m_2 l_2 (\dot{\theta}_1 + \dot{\theta}_2)^2 s_2 \\ m_2 l_1 \ddot{\theta}_2 + m_2 g s_{12} s_2 + m_2 g c_{12} c_2 - m_2 l_2 s_2 (\ddot{\theta}_1 + \ddot{\theta}_2)^2 + m_2 l_2 c_2 (\ddot{\theta}_1 +\ddot{\theta}_2) \\ 0 \end{bmatrix}$ $+ \begin{bmatrix} -m_1 l_1 \dot{\theta}^2_1 + m_1 g s_1 \\ m_1 l_1 \ddot{\theta}_1 + m_1 g c_1 \\ 0 \end{bmatrix}$

${}^1n_1 = {}^1N_1 + {}^1_2R\,{}^2n_2 + {}^1P_{C_1} \times {}^1F_1 + {}^1P_2 \times {}^1_2R\, {}^2f_2$
$= 0 + {}^{\color{blue}2}\color{blue}{n_2} + \begin{bmatrix} l_1 \\ 0 \\ 0 \end{bmatrix} \color{red}\times {}^{\color{red}1}\color{red}{F_1} + \begin{bmatrix} l_1 \\ 0 \\ 0 \end{bmatrix} \times {}^{\color{green}1}_{\color{green}2}\color{green}R\, {}^{\color{green}2}\color{green}{f_2}$

$= \color{blue}{ m_2l_1l_2c_2\ddot{\theta}_1 + m_2l_1l_2s_2\dot{\theta}^2_1 + m_2 l_2 g c_{12} + m_2l^2_2(\ddot{\theta}_1 + \ddot{\theta}_2) + I_{zz}(\ddot{\theta_1} + \ddot{\theta_2} + \ddot{\theta_3})} \; \hat{Z}$

$\color{blue}{+\; m_3 l^2_2 c_3 (\ddot{\theta}_1 + \ddot{\theta}_2) + m_3 l^2_2 s_3 (\dot{\theta}_1 + \dot{\theta}_2)^2} \; \hat{Z}$

$\color{blue}{+ \; m_3 l_1 l_2 c_{23}\ddot{\theta}_1 + m_3 l_1 l_2 s_{23} \dot{\theta}^2_1 + m_3 l_2 gc_{123}}\;\hat{Z}$

$\color{red}{+\; m_1 l^2_1\ddot{\theta}_1 + m_1 l_1 g c_1} \; \hat{Z}$

$\color{green}{ +\; m_3 l_1 l_2 s_2(\dot{\theta}_1 + \dot{\theta}_2)^2 + m_3 l_1 l_2 c_2 (\ddot{\theta}_1 + \ddot{\theta}_2)}\; \hat{Z}$

$\color{green}{ +\; m_3 l^2_1 \ddot{\theta}_1 + m_3 l_1 g s_{12} s_2 + m_3 l_1 g c_{12} c_2}\; \hat{Z}$

Extracting the torque components

$\tau_i = {}^in^{T}_i\; {}^i\hat{Z}_i$

$\boldsymbol{\tau}_1 = \color{grey}{m_2l^2_2(\ddot{\theta}_1 + \ddot{\theta}_2) + m_2 l_1 l_2 c_2 (2\ddot{\theta}_1 + \ddot{\theta}_2) + (m_1 + m_2)l^2_1 \ddot{\theta}_1 - m_2 l_1 l_2 s_2 \dot{\theta}^2_2 - 2m_2 l_1 l_2 s_2 \dot{\theta}_1 \dot{\theta}_2 }$

$+\; \color{grey}{ m_2 l_2 g c_{12} + (m_1 + m_2)l_1 g c_1}$

$+ \; m_3 l^2_2 c_3 (\ddot{\theta}_1 + \ddot{\theta}_2) + m_3 l_1 l_2 c_2 (\ddot{\theta}_1 + \ddot{\theta}_2) + m_3 l^2_1 \ddot{\theta}_1 + m_3 l_1 l_2 c_{23} \ddot{\theta}_1 + I_{zz}(\ddot{\theta_1} + \ddot{\theta_2} + \ddot{\theta_3})$

$\boldsymbol{\tau}_2 = \color{grey}{m_2l_1l_2c_2 \ddot{\theta}_1 + m_2l_1l_2s_2\dot{\theta}^2_1 + m_2 l_2 g c_{12} + m_2l^2_2(\ddot{\theta}_1 + \ddot{\theta}_2)} + I_{zz}(\ddot{\theta_1} + \ddot{\theta_2} + \ddot{\theta_3})$

$+ \; m_3 l_1 l_2 c_{23}\ddot{\theta}_1 + m_3 l_1 l_2 s_{23} \dot{\theta}^2_1 + m_3 l_2 gc_{123} + m_3 l^2_2 c_3 (\ddot{\theta}_1 + \ddot{\theta}_2) + m_3 l^2_2 s_3 (\dot{\theta}_1 + \dot{\theta}_2)^2$

Dynamic Equation