Dynamic model of simple manipulator structures

Based on Section 7.4 of Robotics: Modelling, Planning and Control.

The two-link planar arm

\(\boldsymbol q = [\theta_1,\;\theta_2]^T\)

\(I_{l_1}, I_{l_2}\) : moments of inertia relative to the centers of mass

\(l_1, l_2\) : distances to the center of mass

\(a_1, a_2\) : link lengths (DH parameters)

The inertia matrix

\[B(\boldsymbol q) = \sum_{i=1}^{2} \Big[ m_{l_i}\,J_{P}^{l_i}(\boldsymbol q)^{T}J_{P}^{l_i}(\boldsymbol q) + J_{\theta}^{l_i}(\boldsymbol q)^{T}\,R_{i}\,I_{l_i}\,R_{i}^{T}\,J_{\theta}^{l_i}(\boldsymbol q) \Big]\]

\[J_{P}^{\,l_i}(\boldsymbol q) = \bigl[\,J_{P_1}^{\,l_i}\;\;J_{P_2}^{\,l_i}\;\dots\;J_{P_i}^{\,l_i}\;\;0\;\dots\;0\,\bigr]\]

\[J_{\theta}^{\,l_i}(\boldsymbol q) = \bigl[\,J_{\theta_1}^{\,l_i}\;\;J_{\theta_2}^{\,l_i}\;\dots\;J_{\theta_i}^{\,l_i}\;\;0\;\dots\;0\,\bigr]\]

\[\begin{split}J_{P_j}^{\,l_i} = \begin{cases} z_{j-1}, & \text{for prismatic joint}\\[6pt] z_{j-1}\times\bigl(\hat P_{l_i}-\hat P_{j-1}\bigr), & \text{for revolute joint} \end{cases}\end{split}\]

\[\begin{split}J_{\theta_j}^{\,l_i} = \begin{cases} 0, & \text{for prismatic joint}\\[3pt] z_{j-1}, & \text{for revolute joint} \end{cases}\end{split}\]

\[J_{P}^{\,l_1}(\boldsymbol q) \;=\;\bigl[J_{P_1}^{\,l_1}\;\;0\bigr], \quad J_{\theta}^{\,l_1}(\boldsymbol q) \;=\;\bigl[J_{\theta}^{\,l_1}\;\;0\bigr]\]

\[J_{P}^{\,l_2}(\boldsymbol q) = \bigl[J_{P_1}^{\,l_2}\;\;J_{P_2}^{\,l_2}\bigr], \quad J_{\theta}^{\,l_2}(\boldsymbol q) = \bigl[J_{\theta_1}^{\,l_2}\;\;J_{\theta_2}^{\,l_2}\bigr]\]

\[\begin{split}P_{1} = \begin{bmatrix} a_1 c_1 \\[3pt] a_1 s_1 \\[3pt] 0 \end{bmatrix}, \quad P_{0} = \begin{bmatrix} 0 \\[3pt] 0 \\[3pt] 0 \end{bmatrix}\end{split}\]

\[\begin{split}P_{l_1} = \begin{bmatrix} l_1 c_1 \\[3pt] l_1 s_1 \\[3pt] 0 \end{bmatrix}, \quad P_{l_2} = \begin{bmatrix} a_1 c_1 + l_2 c_{12} \\[3pt] a_1 s_1 + l_2 s_{12} \\[3pt] 0 \end{bmatrix}\end{split}\]

\[\begin{split}\begin{aligned} J_{P_1}^{\,l_1} &= \boldsymbol z_0 \times (\boldsymbol P_{l_1} - \boldsymbol P_0) \\[6pt] &= \boldsymbol z_0 \times \bigl([\,l_1 c_1\; l_1 s_1\; 0]^T - [\,0\; 0\; 0]^T\bigr) \\[6pt] &= [\,0\; 0\; 1]^T \times [\,l_1 c_1\; l_1 s_1\; 0]^T \\[6pt] &= [\,-l_1 s_1\; l_1 c_1\; 0]^T \end{aligned}\end{split}\]

\[\begin{split}\begin{aligned} J_{P_2}^{\,l_2} &= \boldsymbol z_0 \times (\boldsymbol P_{l_2} - \boldsymbol P_0) \\[6pt] &= [\,0\; 0\; 1]^T \times \Bigl([\,a_1 c_1 + l_2 c_{12}\; a_1 s_1 + l_2 s_{12}\; 0]^T - [\,a_1 c_1\; a_1 s_1\; 0]^T\Bigr) \\[6pt] &= [\,0\; 0\; 1]^T \times [\,l_2 c_{12}\; l_2 s_{12}\; 0]^T \\[6pt] &= [\,-l_2 s_{12}\; l_2 c_{12}\; 0]^T \end{aligned}\end{split}\]

\[\begin{split}J_{P}^{\,l_1}(\boldsymbol q) = \begin{bmatrix} -l_1 s_1 & 0 \\ l_1 c_1 & 0 \\ 0 & 0 \end{bmatrix}, \quad J_{P}^{\,l_2}(\boldsymbol q) = \begin{bmatrix} -a_1 s_1 - l_2 s_{12} & -l_2 s_{12} \\ a_1 c_1 + l_2 c_{12} & l_2 c_{12} \\ 0 & 0 \end{bmatrix}\end{split}\]

\[\begin{split}J_{\theta}^{\,l_1}(\boldsymbol q) = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 0 \end{bmatrix}, \quad J_{\theta}^{\,l_2}(\boldsymbol q) = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{bmatrix}\end{split}\]

\[\begin{split}\begin{aligned} B(\boldsymbol q) &= m_{l_1}\,J_{P}^{\,l_1}(\boldsymbol q)^{T}J_{P}^{\,l_1}(\boldsymbol q) + J_{\theta}^{\,l_1}(\boldsymbol q)^{T}\,R_{1}\,I_{l_1}\,R_{1}^{T}\,J_{\theta}^{\,l_1}(\boldsymbol q) + m_{l_2}\,J_{P}^{\,l_2}(\boldsymbol q)^{T}J_{P}^{\,l_2}(\boldsymbol q) + J_{\theta}^{\,l_2}(\boldsymbol q)^{T}\,R_{2}\,I_{l_2}\,R_{2}^{T}\,J_{\theta}^{\,l_2}(\boldsymbol q) \\ &= B1+B2+B3+B4 \end{aligned}\end{split}\]

a) Linear part \(B1\):

\[\begin{split}\begin{aligned} m_{l_1}\,J_{P}^{l_1}(\boldsymbol q)^{T}J_{P}^{l_1}(\boldsymbol q) &= m_{l_1} \begin{bmatrix} -l_1 s_1 & l_1 c_1 & 0 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} -l_1 s_1 & 0 \\ l_1 c_1 & 0 \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} m_{l_1}\,l_1^2 & 0 \\ 0 & 0 \end{bmatrix} \end{aligned}\end{split}\]

b) Linear part \(B2\):

\[\begin{split}\begin{aligned} m_{l_2}\,J_{P}^{l_2}(\boldsymbol q)^{T}J_{P}^{l_2}(\boldsymbol q) &= m_{l_2} \begin{bmatrix} -a_1 s_1 - l_2 s_{12} & -l_2 s_{12} \\ a_1 c_1 + l_2 c_{12} & l_2 c_{12} \\ 0 & 0 \end{bmatrix}^{T} \begin{bmatrix} -a_1 s_1 - l_2 s_{12} & -l_2 s_{12} \\ a_1 c_1 + l_2 c_{12} & l_2 c_{12} \\ 0 & 0 \end{bmatrix} \\[6pt] &= m_{l_2} \begin{bmatrix} a_1^2 + l_2^2 + 2\,a_1\,l_2\,c_2 & a_1\,l_2\,c_2 + l_2^2 \\[3pt] a_1\,l_2\,c_2 + l_2^2 & l_2^2 \end{bmatrix} \end{aligned}\end{split}\]

c) Angular part \(B3\):

\[\begin{split}\begin{aligned} I_{l_1} &= R_{1}\,I_{l_1}^{\,i}\,R_{1}^{T} \\[6pt] &= \begin{bmatrix} c_1 & -s_1 & 0 \\ s_1 & c_1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & I_{zz_1} \end{bmatrix} \begin{bmatrix} c_1 & s_1 & 0 \\ -s_1 & c_1 & 0 \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & I_{zz_1} \end{bmatrix} \end{aligned}\end{split}\]

\[s_1\,s_{12} + c_1\,c_{12} = s_1\,(s_1\,c_2 + c_1\,s_2) + c_1\,(c_1\,c_2 - s_1\,s_2) = s_1^2\,c_2 + s_1\,c_1\,s_2 + c_1^2\,c_2 - s_1^2\,s_2 = c_2\]

\[\begin{split}J_{\theta}^{\,l_1}(\boldsymbol q)^{T}\,I_{l_1}\,J_{\theta}^{\,l_1}(\boldsymbol q) = \begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & I_{zz_1} \end{bmatrix} \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 0 \end{bmatrix} = \begin{bmatrix} I_{zz_1} & 0 \\ 0 & 0 \end{bmatrix}\end{split}\]

d) Angular part \(B4\):

\[\begin{split}J_{\theta}^{\,l_2}(\boldsymbol q)^{T}\,I_{l_2}\,J_{\theta}^{\,l_2}(\boldsymbol q) = \begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & I_{zz_2} \end{bmatrix} \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{bmatrix} = \begin{bmatrix} I_{zz_2} & I_{zz_2} \\ I_{zz_2} & I_{zz_2} \end{bmatrix}\end{split}\]

Finally:

\[\begin{split}B(\boldsymbol q) = \begin{bmatrix} m_{l_1}\,l_1^2 + m_{l_2}\,\bigl(a_1^2 + l_2^2 + 2\,a_1\,l_2\,c_2\bigr) + I_{zz_1} + I_{zz_2} & m_{l_2}\,\bigl(a_1\,l_2\,c_2 + l_2^2\bigr) + I_{zz_2} \\[6pt] m_{l_2}\,\bigl(a_1\,l_2\,c_2 + l_2^2\bigr) + I_{zz_2} & m_{l_2}\,l_2^2 + I_{zz_2} \end{bmatrix} = \begin{bmatrix} b_{11} & b_{12} \\[3pt] b_{21} & b_{22} \end{bmatrix}, \quad b_{12} = b_{21}\end{split}\]

The Coriolis and centrifugal matrix

\[\begin{split}{\boldsymbol C}\bigl(\boldsymbol q,\dot{\boldsymbol q}\bigr) = \begin{bmatrix} C_{11} & C_{12} \\[3pt] C_{21} & C_{22} \end{bmatrix}\end{split}\]

\[C_{ij} = \sum_{k=1}^{n} C_{ijk}\,\dot q_k, \quad C_{ijk} = \tfrac12\!\Bigl( \frac{\partial b_{ij}}{\partial q_k} + \frac{\partial b_{ik}}{\partial q_j} - \frac{\partial b_{jk}}{\partial q_i} \Bigr)\]

\[C_{11} = C_{111}\,\dot q_1 + C_{112}\,\dot q_2\]

\[C_{12} = C_{121}\,\dot q_1 + C_{122}\,\dot q_2\]

\[C_{21} = C_{211}\,\dot q_1 + C_{212}\,\dot q_2\]

\[C_{22} = C_{221}\,\dot q_1 + C_{222}\,\dot q_2\]

\[C_{111} = \tfrac12\Bigl( \frac{\partial b_{11}}{\partial q_{1}} + \frac{\partial b_{11}}{\partial q_{1}} - \frac{\partial b_{11}}{\partial q_{1}} \Bigr) = 0\]

\[C_{112} = \tfrac12\Bigl( \frac{\partial b_{11}}{\partial q_{2}} + \frac{\partial b_{12}}{\partial q_{1}} - \frac{\partial b_{12}}{\partial q_{1}} \Bigr) = -\,m_{l_{2}}\,a_{1}\,l_{2}\,s_{2} = h\]

\[C_{121} = \tfrac12\Bigl( \frac{\partial b_{12}}{\partial q_{1}} + \frac{\partial b_{11}}{\partial q_{2}} - \frac{\partial b_{21}}{\partial q_{1}} \Bigr) = -\,m_{l_{2}}\,a_{1}\,l_{2}\,s_{2} = h\]

\[C_{112} = C_{121}\]

\[C_{122} = \tfrac12\Bigl( \frac{\partial b_{12}}{\partial q_{2}} + \frac{\partial b_{12}}{\partial q_{2}} - \frac{\partial b_{22}}{\partial q_{2}} \Bigr) = \frac{\partial b_{12}}{\partial q_{2}} - \tfrac12\underbrace{\frac{\partial b_{22}}{\partial q_{2}}}_{0} = 0\]

\[C_{211} = \tfrac12\Bigl( \frac{\partial b_{21}}{\partial q_{1}} + \frac{\partial b_{21}}{\partial q_{1}} - \frac{\partial b_{11}}{\partial q_{2}} \Bigr) = \underbrace{\frac{\partial b_{21}}{\partial q_{1}}}_{0} - \tfrac12\frac{\partial b_{11}}{\partial q_{2}} = -\,h\]

\[C_{212} = C_{221} = \tfrac12\Bigl( \frac{\partial b_{21}}{\partial q_{2}} + \frac{\partial b_{22}}{\partial q_{1}} - \frac{\partial b_{12}}{\partial q_{2}} \Bigr) = \tfrac12\frac{\partial b_{22}}{\partial q_{1}} = 0\]

\[C_{222} = \tfrac12\Bigl( \frac{\partial b_{22}}{\partial q_{2}} + \frac{\partial b_{22}}{\partial q_{2}} - \frac{\partial b_{22}}{\partial q_{2}} \Bigr) = \tfrac12\frac{\partial b_{22}}{\partial q_{2}} = 0\]

\[\begin{split}C(\,\boldsymbol q,\dot{\boldsymbol q}\,) \;=\; \begin{bmatrix} h\,\dot\theta_{2} & h\,\dot\theta_{1} \;+\; h\,\dot\theta_{2} \\[6pt] -\,h\,\dot\theta_{1} & 0 \end{bmatrix}\end{split}\]

We can verify that \({\boldsymbol N} = \dot{\boldsymbol B} \;-\; 2 {\boldsymbol C}\) is skew-symmetric.

\[\begin{split}\dot{\boldsymbol B} \;=\; \begin{bmatrix} -2\,m_{l_2}\,a_{1}\,\ell_{2}\,s_{2}\,\dot\theta_{2} & -\,m_{l_2}\,a_{1}\,\ell_{2}\,s_{2}\,\dot\theta_{2} \\[6pt] -\,m_{l_2}\,a_{1}\,\ell_{2}\,s_{2}\,\dot\theta_{2} & 0 \end{bmatrix} \;=\; \begin{bmatrix} 2\,h\,\dot\theta_{2} & h\,\dot\theta_{2} \\[6pt] h\,\dot\theta_{2} & 0 \end{bmatrix}\end{split}\]

\[\begin{split}{\boldsymbol N} \;=\; \begin{bmatrix} 2\,h\,\dot\theta_{2} & h\,\dot\theta_{2} \\[6pt] h\,\dot\theta_{2} & 0 \end{bmatrix} \;-\;2\, \begin{bmatrix} h\,\dot\theta_{2} & h\,\dot\theta_{1} \;+\; h\,\dot\theta_{2} \\[6pt] -\,h\,\dot\theta_{1} & 0 \end{bmatrix} \;=\; \begin{bmatrix} 0 & -\,2\,h\,\dot\theta_{2} \;-\; h\,\dot\theta_{2} \\[6pt] 2\,h\,\dot\theta_{2} \;+\; h\,\dot\theta_{2} & 0 \end{bmatrix}\end{split}\]

Gravitational term

\[\begin{split}\boldsymbol g_{0} \;=\; \begin{bmatrix} 0 \\[4pt] -\,g \\[4pt] 0 \end{bmatrix} \quad\text{then}\quad \boldsymbol g(\boldsymbol q)\;=\; \begin{bmatrix} g_{1}(\boldsymbol q) \\[4pt] g_{2}(\boldsymbol q) \end{bmatrix}\end{split}\]

\[g_{i}(\boldsymbol q) \;=\; -\sum_{k=1}^{n} m_{l_k}\,\boldsymbol g_{0}^{T}\,J_{P_{i}}^{\,l_{k}}\]

\[\begin{split}g_{1}(\boldsymbol q) \;=\; -\,m_{l_1}\,\boldsymbol g_{0}^{T}\,J_{P_{1}}^{\,l_{1}} \;-\; m_{l_2}\,\boldsymbol g_{0}^{T}\,J_{P_{1}}^{\,l_{2}}\,; \quad J_{P}^{\,l_{1}} \;=\; \begin{bmatrix} -\,\ell_{1}\,s_{1} & 0 \\[6pt] \ell_{1}\,c_{1} & 0 \\[6pt] 0 & 0 \end{bmatrix} \;=\; \bigl[\,J_{P_{1}}^{\,l_{1}}\,,\,J_{P_{2}}^{\,l_{1}}\,\bigr]\end{split}\]

\[\begin{split}g_{2}(\boldsymbol q) \;=\; -\,m_{l_1}\,\boldsymbol g_{0}^{T}\,J_{P_{2}}^{\,l_{1}} \;-\; m_{l_2}\,\boldsymbol g_{0}^{T}\,J_{P_{2}}^{\,l_{2}}\,; \quad J_{P}^{\,l_{2}} \;=\; \begin{bmatrix} -\,a\,s_{1} \;-\; \ell_{2}\,s_{12} & -\,\ell_{2}\,s_{12} \\[6pt] a\,c_{1} \;+\; \ell_{2}\,c_{12} & \ell_{2}\,c_{12} \\[6pt] 0 & 0 \end{bmatrix} \;=\; \bigl[\,J_{P_{1}}^{\,l_{2}}\,,\,J_{P_{2}}^{\,l_{2}}\,\bigr]\end{split}\]

\[g_{1}(\boldsymbol q)=m_{l_1}\,g\,\ell_{1}\,c_{1}+m_{l_2}\,g\,(a_{1}\,c_{1}+\ell_{2}\,c_{12})\]

\[g_{2}(\boldsymbol q)=m_{l_2}\,g\,\ell_{2}\,c_{12}\]

Equations of motion

Finally, the equations of motion (without friction) are:

\[B(\boldsymbol q)\,\ddot{\boldsymbol q} \;+\; C(\boldsymbol q,\dot{\boldsymbol q})\,\dot{\boldsymbol q} \;+\; g(\boldsymbol q) \;=\; \boldsymbol \tau\]

\[b_{11}\,\ddot\theta_{1} + b_{12}\,\ddot\theta_{2} + c_{11}\,\dot\theta_{1} + c_{12}\,\dot\theta_{2} + g_{1} = \tau_{1}\]

\[b_{21}\,\ddot\theta_{1} + b_{22}\,\ddot\theta_{2} + c_{21}\,\dot\theta_{1} + c_{22}\,\dot\theta_{2} + g_{2} = \tau_{2}\]

\[\begin{split}\text{I:}\quad \begin{aligned} & (m_{l_1}\ell_{1}^{2}+m_{2}(a_{1}^{2}+\ell_{2}^{2}+2a_{1}\ell_{2}c_{2}) +I_{l_1}+I_{l_2})\,\ddot\theta_{1}\\ &\quad + (m_{l_2}(a_{1}\ell_{2}c_{2}+\ell_{2}^{2})+I_{2})\,\ddot\theta_{2}\\ &\quad + (-m_{l_2}a_{1}\ell_{2}s_{2}\,\dot\theta_{2})\,\dot\theta_{1}\\ &\quad + (-m_{l_2}a_{1}\ell_{2}s_{2}\,\dot\theta_{1} -m_{l_2}a_{1}\ell_{2}s_{2}\,\dot\theta_{2})\,\dot\theta_{2}\\ &\quad + (m_{1}g\ell_{1}c_{1} +m_{l_2}g(a_{1}c_{1}+\ell_{2}c_{12})) = \tau_{1} \end{aligned}\end{split}\]

\[\begin{split}\text{II:}\quad \begin{aligned} & (m_{l_2}(a_{1}\ell_{2}c_{2}+\ell_{2}^{2})+I_{2})\,\ddot\theta_{1}\\ &\quad + (m_{l_2}\ell_{2}^{2}+I_{2})\,\ddot\theta_{2}\\ &\quad + (m_{l_2}a_{1}\ell_{2}s_{2}\,\dot\theta_{1})\,\dot\theta_{1}\\ &\quad + 0\\ &\quad + (m_{l_2}g\ell_{2}c_{12}) = \tau_{2} \end{aligned}\end{split}\]

Parameterization

To finish with this example, let’s derive the parameterization of the dynamic model i.e.:

\[\boldsymbol \tau = y(\boldsymbol q,\dot{\boldsymbol q},\ddot{\boldsymbol q})\,\boldsymbol \pi\]

\[\begin{split}\boldsymbol \tau = \begin{bmatrix} \tau_{1}\\[4pt] \tau_{2} \end{bmatrix} = \begin{bmatrix} y_{11}^{T} & y_{12}^{T}\\[2pt] 0 & y_{22}^{T} \end{bmatrix} \begin{bmatrix} \boldsymbol \pi_{1}\\[2pt] \boldsymbol \pi_{2} \end{bmatrix} = \begin{bmatrix} y_{111} & y_{112} & y_{121} & y_{122}\\[4pt] 0 & 0 & y_{221} & y_{222} \end{bmatrix} \begin{bmatrix} \boldsymbol \pi_{11}\\[2pt] \boldsymbol \pi_{12}\\[2pt] \boldsymbol \pi_{21}\\[2pt] \boldsymbol \pi_{22} \end{bmatrix}\end{split}\]

\[\begin{split}\boldsymbol \pi_{1} = \begin{bmatrix} m_{l_1}\\ I_{l_1} \end{bmatrix}, \quad \boldsymbol \pi_{2} = \begin{bmatrix} m_{l_2}\\ I_{l_2} \end{bmatrix}\end{split}\]

Expression I and II can be rewritten as:

\[\tau_{1} = y_{11}\,m_{l_1} + y_{12}\,I_{l_1} + y_{21}\,m_{l_2} + y_{22}\,I_{l_2}\]

\[\tau_{2} = y_{221}\,m_{l_2} + y_{222}\,I_{l_2}\]

with:

\[y_{111} = \ell_{1}^{2}\,\ddot\theta_{1} + g\,\ell_{1}\,c_{1}\]

\[y_{112} = \ddot\theta_{1}\]

\[y_{121} = (a_{1}^{2} + \ell_{2}^{2} + 2\,a_{1}\,\ell_{2}\,c_{2})\,\ddot\theta_{1} + (a_{1}\,\ell_{2}\,c_{2} + \ell_{2}^{2})\,\ddot\theta_{2} - 2\,a_{1}\,\ell_{2}\,s_{2}\,\dot\theta_{1}\,\dot\theta_{2} - a_{1}\,\ell_{2}\,s_{2}\,\dot\theta_{2}^{2} + g\,(a_{1}\,c_{1} + \ell_{2}\,c_{12})\]

\[y_{122} = \ddot\theta_{1} + \ddot\theta_{2}\]

\[y_{221} = (a_{1}\,\ell_{2}\,c_{2} + \ell_{2}^{2})\,\ddot\theta_{1} + \ell_{2}^{2}\,\ddot\theta_{2} + a_{1}\,\ell_{2}\,s_{2}\,\dot\theta_{1}^{2} + g\,\ell_{2}\,c_{12}\]

\[y_{222} = \ddot\theta_{1} + \ddot\theta_{2}\]

The two-link Cartesian arm

\[\begin{split}\boldsymbol q = \begin{bmatrix} d_{1} \\ d_{2} \end{bmatrix}, \quad m_{l_1},\,m_{l_2}\text{: masses of the two links}\end{split}\]

\[B(\boldsymbol q) = \sum_{i=1}^{n} \Bigl[ m_{l_i}\,J_{P}^{\,l_{i}T}\,J_{P}^{\,l_{i}} + J_{\theta}^{\,l_{i}T}\,R_{i}^{T}\,I_{i}^{\,l_{i}}\,R_{i}\,J_{\theta}^{\,l_{i}} \Bigr]\]

\[J_{P}^{\,l_{i}} = \bigl[\,J_{P_{1}}^{\,l_{i}}\;J_{P_{2}}^{\,l_{i}}\;\dots\;J_{P_{n}}^{\,l_{i}}\;0\;\dots\;0\bigr]\]

\[J_{\theta}^{\,l_{i}} = \bigl[\,J_{\theta_{1}}^{\,l_{i}}\;J_{\theta_{2}}^{\,l_{i}}\;\dots\;J_{\theta_{i}}^{\,l_{i}}\;0\;\dots\;0\bigr]\]

\[\begin{split}J_{P_{j}}^{\,l_{i}} = \begin{cases} \boldsymbol z_{j-1}, & \text{for prismatic joints},\\[4pt] \boldsymbol z_{j-1}\times\bigl(P_{i}-P_{j-1}\bigr), & \text{for revolute joints}. \end{cases}\end{split}\]

\[\begin{split}J^{\,l}_{\theta_{j}} = \begin{cases} 0 & \text{for prismatic joints}\\[4pt] \boldsymbol z_{j-1} & \text{for revolute joints} \end{cases}\end{split}\]

In the case of the two-link Cartesian arm we have:

\[\begin{split}J_{P}^{\,l_{1}} = \begin{bmatrix} 0 & 0\\ 0 & 0\\ 1 & 0 \end{bmatrix} \quad J_{P}^{\,l_{2}} = \begin{bmatrix} 0 & 1\\ 0 & 0\\ 1 & 0 \end{bmatrix} \quad J_{\theta}^{\,l_{1}} = J_{\theta}^{\,l_{2}} = 0\end{split}\]

\[B(\boldsymbol q) = m_{l_1}\,J_{P}^{\,l_{1}T}\,J_{P}^{\,l_{1}} + m_{l_2}\,J_{P}^{\,l_{2}T}\,J_{P}^{\,l_{2}}\]

\[\begin{split}\quad= m_{l_1} \begin{bmatrix} 0 & 0 & 1\\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & 0\\ 0 & 0\\ 1 & 0 \end{bmatrix} + m_{l_2} \begin{bmatrix} 0 & 0 & 1\\ 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} 0 & 1\\ 0 & 0\\ 1 & 0 \end{bmatrix}\end{split}\]

\[\begin{split}\quad= \begin{bmatrix} m_{l_1} & 0\\ 0 & 0 \end{bmatrix} + \begin{bmatrix} m_{l_2} & 0\\ 0 & m_{l_2} \end{bmatrix} = \begin{bmatrix} m_{l_1}+m_{l_2} & 0\\ 0 & m_{l_2} \end{bmatrix}\end{split}\]

Note

The \(B\) is constant, i.e. it does not depend on the arm configuration. This implies also that \(\boxed{C = 0}\), i.e.there are no contributions of centrifugal or Coriolis forces.

Gravitational term:

\[\boldsymbol g_{0} = \begin{bmatrix} 0 & 0 & -g \end{bmatrix}^{T}\]

\[\begin{split}\boldsymbol g(\boldsymbol q) = \begin{bmatrix} g_{1}(\boldsymbol q)\\ g_{2}(\boldsymbol q) \end{bmatrix}\end{split}\]

\[g_{i}(\boldsymbol q) = -\sum_{j=1}^{n} m_{j}\,\boldsymbol g_{0}^{T}\,J_{P}^{\,l_{j}}\]

\[g_{1} = -\,m_{l_{1}}\;\boldsymbol g_{0}^{T}\;J_{P_{1}}^{\,l_{1}} \;-\;m_{l_{2}}\;\boldsymbol g_{0}^{T}\;J_{P_{1}}^{\,l_{2}}\]

\[\begin{split}g_{1} = -\,m_{l_{1}}\,[0\ 0\ -g] \begin{bmatrix} 0 \\[3pt] 0 \\[3pt] 1 \end{bmatrix} \;-\; m_{l_{2}}\,[0\ 0\ -g] \begin{bmatrix} 0 \\[3pt] 0 \\[3pt] 1 \end{bmatrix} = (m_{l_{1}} + m_{l_{2}})\,g\end{split}\]

\[g_{2} = -\,m_{l_{1}}\;\boldsymbol g_{0}^{T}\;J_{P_{2}}^{\,l_{1}} \;-\;m_{l_{2}}\;\boldsymbol g_{0}^{T}\;J_{P_{2}}^{\,l_{2}}\]

\[\begin{split}g_{2} = -\,m_{l_{1}}\,[0\ 0\ -g] \begin{bmatrix} 0 \\[3pt] 0 \\[3pt] 0 \end{bmatrix} \;-\; m_{l_{2}}\,[0\ 0\ -g] \begin{bmatrix} 0 \\[3pt] 0 \\[3pt] 0 \end{bmatrix} = 0\end{split}\]

\[\boldsymbol g(\boldsymbol q) = \begin{bmatrix} (m_{l_{1}} + m_{l_{2}})\,g & 0 \end{bmatrix}\]

Finally, the dynamic model is:

\[\begin{split}\left\{ \begin{aligned} (m_{l_{1}} + m_{l_{2}})\,\ddot d_{1} + (m_{l_{1}} + m_{l_{2}})\,g &= z_{1},\\[4pt] m_{l_{2}}\;\ddot d_{2} &= z_{2}. \end{aligned} \right.\end{split}\]

A completely decoupled dynamic appears, as a result of the Cartesian structure and the particular choice of the axes (the second axis being perpendicular to the first makes the inertia matrix a diagonal one).