Link manipulator dynamics

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:

${}^{C_s}I = \begin{bmatrix} 0.05 & 0 & 0 \\ 0 & 0.1 & 0 \\ 0 & 0 & 0.1 \end{bmatrix}$

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

${}^3\omega_3 = \begin{bmatrix} 0 \\ 0 \\ \dot{\theta_1} + \dot{\theta_2} + \dot{\theta_3} \end{bmatrix}$ ${}^3\dot{\omega}_3 = \begin{bmatrix} 0 \\ 0 \\ \ddot{\theta_1} + \ddot{\theta_2} + \ddot{\theta_3} \end{bmatrix}$ ${}^3\dot{v}_3 = {}^3_2R \Bigg( {}^2\dot{\omega}_2 \times {}^2P_3 + {}^2\omega_2 \times ({}^2\omega_2 \times {}^2P_3) + {}^2\dot{v}_2 \Bigg)$ $= {}^3_2R \Bigg( \begin{bmatrix} 0 \\ l_2(\ddot{\theta}_1 + \ddot{\theta}_2) \\ 0 \end{bmatrix} + {}^2\omega_2 \times \begin{bmatrix} 0 \\ l_2(\dot{\theta}_1 + \dot{\theta}_2) \\ 0 \end{bmatrix} + {}^2\dot{v}_2 \Bigg)$ $= {}^3_2R \left( \begin{bmatrix} 0 \\ l_2(\ddot{\theta}_1 + \ddot{\theta}_2) \\ 0 \end{bmatrix} + \begin{bmatrix} -l_2(\dot{\theta}_1 + \dot{\theta}_2)^2 \\ 0 \\ 0 \end{bmatrix} + {}^2\dot{v}_2 \right)$

$= \begin{bmatrix} c_3 & s_3 & 0 \\ -s_3 & c_3 & 0 \\ 0 & 0 & 1 \end{bmatrix} \left( \begin{bmatrix} -l_2(\dot{\theta}_1 + \dot{\theta}_2)^2 \\ l_2(\ddot{\theta}_1 + \ddot{\theta}_2) \\ 0 \end{bmatrix} + \begin{bmatrix} l_1\ddot{\theta}_1s_2 - l_1\dot{\theta}^2_1c_2 + gs_{12} \\ l_1\ddot{\theta}_1c_2 + l_1\dot{\theta}^2_1 s_2 + gc_{12} \\ 0 \end{bmatrix}\right)$ $= \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} c_3 & s_3 & 0 \\ -s_3 & c_3 & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} l_1\ddot{\theta}_1s_2 - l_1\dot{\theta}^2_1c_2 + gs_{12} \\ l_1\ddot{\theta}_1c_2 + l_1\dot{\theta}^2_1 s_2 + gc_{12} \\ 0 \end{bmatrix}$

$= ... + \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

force link 2

${}^2f_2 = {}^2_3R\, {}^3f_3 + {}^2F_2$ $= \begin{bmatrix} c_3 & -s_3 & 0 \\ s_3 & c_3 & 0 \\ 0 & 0 & 1 \end{bmatrix}\left( \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}\right) + {}^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}$

moment link 2

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

$= \begin{bmatrix} c_3 & -s_3 & 0 \\ s_3 & c_3 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ 0 \\ I_{zz}(\ddot{\theta_1} + \ddot{\theta_2} + \ddot{\theta_3}) \end{bmatrix} + \begin{bmatrix} l_2 \\ 0 \\ 0 \end{bmatrix} \times {}^2F_2 + \begin{bmatrix} l_2 \\ 0 \\ 0 \end{bmatrix} \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

force link 1

${}^1f_1 = {}^{\color{green}1}_{\color{green}2}\color{green}R\, {}^{\color{green}2}\color{green}{f_2} + {}^1F_1$

$= \begin{bmatrix} c_2 & -s_2 & 0 \\ s_2 & c_2 & 0 \\ 0 & 0 & 1 \end{bmatrix}\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} c_2 & -s_2 & 0 \\ s_2 & c_2 & 0 \\ 0 & 0 & 1 \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} c_2 & -s_2 & 0 \\ s_2 & c_2 & 0 \\ 0 & 0 & 1 \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} + {}^1F_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}$

moment link 1

${}^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}$

$\color{green}{ +\; m_2 l^2_1 \ddot{\theta}_1 + m_2 l_1 g s_{12} s_2 + m_2 l_1 g c_{12} c_2 - m_2 l_1 l_2 s_2 (\dot{\theta}_1 + \dot{\theta}_2)^2 + m_2 l_1 l_2 c_2 (\ddot{\theta}_1 + \ddot{\theta}_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})$

$+\; m_3 l^2_2 (\dot{\theta}_1 + \dot{\theta}_2)^2 + m_3 l_1 l_2 s_2 (\dot{\theta}_1 + \dot{\theta}_2) + m_3 l_1 l_2 s_{23} \dot{\theta}^2_1 + m_3 l_2 g c_{123} + m_3 l_1 g (s_{12}s_2 + c_{12} c_2)$

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

$\boldsymbol{\tau}_3 = I_{zz}(\ddot{\theta_1} + \ddot{\theta_2} + \ddot{\theta_3})$

Dynamic Equation

$\tau = M(\theta)\ddot{\theta} + V(\theta, \dot{\theta}) + G(\theta)$ $M(\theta)\ddot{\theta} = \begin{bmatrix} I_{zz} + m_3 l^2_1 + m_3 l^2_2 c_3 + m_3 l_1 l_2 c_2 + m_3 l_1 l_2 c_{23} + \color{grey}{l^2_2m_2 + 2l_1l_2m_2c_2 +l^2_1(m_1 + m_2)} & I_{zz} + m_3 l^2_2 c_3 + m_3 l_1 l_2 c_2 + \color{grey}{l^2_2 m_2 + l_1l_2m_2c_2} & I_{zz} \\ I_{zz} + m_3 l^2_2 c_3 + m_3 l_1 l_2 c_{23} + \color{grey}{l^2_2m_2 + l_1l_2m_2c_2} & I_{zz} + m_3 l^2_2 c_3 + \color{grey}{l^2_2 m_2} & I_{zz} \\ I_{zz} & I_{zz} & I_{zz} \end{bmatrix} \begin{bmatrix} \ddot{\theta}_1 \\ \ddot{\theta}_2 \\ \ddot{\theta}_3 \end{bmatrix}$ $V(\theta, \dot{\theta}) = \begin{bmatrix} m_3 l^2_2 (\dot{\theta}_1 + \dot{\theta}_2)^2 + m_3 l_1 l_2 s_2 (\dot{\theta}_1 + \dot{\theta}_2) + m_3 l_1 l_2 s_{23} \dot{\theta}^2_1 \color{grey}{-m_2 l_1 l_2 s_2 \dot{\theta}^2_2 - 2m_2l_1l_2s_2\dot{\theta}_1\dot{\theta}_2} \\ m_3 l^2_2 s_3 (\dot{\theta}_1 + \dot{\theta}_2)^2 + m_3 l_1 l_2 s_{23} \dot{\theta}^2_1 \color{grey}{+ m_2 l_1 l_2 s_2 \dot{\theta}^2_1}\\ 0 \end{bmatrix}$ $G(\theta) = \begin{bmatrix} m_3 l_2 g c_{123} + m_3 l_1 g c_1 + \color{grey}{m_2 l_2 g c_{12} + (m_1 + m_2)l_1 g c_1} \\ m_3 l_2 g c_{123} + \color{grey}{m_2 l_2 g c_{12}} \\ 0 \end{bmatrix}$