The homogeneous transformation matrices \(T_{i}^{i-1}\) are found by direct inspection.\begin {align*} T_{1}^{0} & =\begin {pmatrix}[1.3] \cos \theta _{1} & 0 & \sin \theta _{1} & L\cos \theta _{1}\\ \sin \theta _{1} & 0 & -\cos \theta _{1} & L\sin \theta _{1}\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ T_{2}^{1} & =\begin {pmatrix}[1.3] \cos \theta _{2} & -\sin \theta _{2} & 0 & L\cos \theta _{2}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & L\sin \theta _{2}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
Hence\begin {align*} T_{2}^{0} & =T_{1}^{0}T_{2}^{1}\\ & =\begin {pmatrix}[1.3] \cos \theta _{1} & 0 & \sin \theta _{1} & L\cos \theta _{1}\\ \sin \theta _{1} & 0 & -\cos \theta _{1} & L\sin \theta _{1}\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix}[1.3] \cos \theta _{2} & -\sin \theta _{2} & 0 & L\cos \theta _{2}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & L\sin \theta _{2}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ & =\begin {pmatrix}[1.3] \cos \theta _{1}\cos \theta _{2} & -\cos \theta _{1}\sin \theta _{2} & \sin \theta _{1} & L\cos \theta _{1}+L\cos \theta _{2}\cos \theta _{1}\\ \cos \theta _{2}\sin \theta _{1} & -\sin \theta _{1}\sin \theta _{2} & -\cos \theta _{1} & L\sin \theta _{1}+L\cos \theta _{2}\sin \theta _{1}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & L\sin \theta _{2}\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
\(T_{c_{i}}^{i-1}\) are now found. These are the same as \(T_{i}^{i-1}\) but with the last column adjusted for the center of mass position which is given as being in the middle of the link, therefore\begin {align*} T_{c_{1}}^{0} & =\begin {pmatrix}[1.3] \cos \theta _{1} & 0 & \sin \theta _{1} & \frac {L}{2}\cos \theta _{1}\\ \sin \theta _{1} & 0 & -\cos \theta _{1} & \frac {L}{2}\sin \theta _{1}\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ T_{c_{2}}^{1} & =\begin {pmatrix}[1.3] \cos \theta _{2} & -\sin \theta _{2} & 0 & \frac {L}{2}\cos \theta _{2}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & \frac {L}{2}\sin \theta _{2}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
Hence\begin {align*} T_{c_{2}}^{0} & =T_{1}^{0}T_{c_{2}}^{1}\\ & =\begin {pmatrix}[1.3] \cos \theta _{1} & 0 & \sin \theta _{1} & L\cos \theta _{1}\\ \sin \theta _{1} & 0 & -\cos \theta _{1} & L\sin \theta _{1}\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix}[1.3] \cos \theta _{2} & -\sin \theta _{2} & 0 & \frac {L}{2}\cos \theta _{2}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & \frac {L}{2}\sin \theta _{2}\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix} \\ & =\begin {pmatrix}[1.3] \cos \theta _{1}\cos \theta _{2} & -\cos \theta _{1}\sin \theta _{2} & \sin \theta _{1} & L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2}\\ \cos \theta _{2}\sin \theta _{1} & -\sin \theta _{1}\sin \theta _{2} & -\cos \theta _{1} & L\sin \theta _{1}+\frac {1}{2}L\cos \theta _{2}\sin \theta _{1}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & \frac {1}{2}L\sin \theta _{2}\\ 0 & 0 & 0 & 1 \end {pmatrix} \end {align*}
VERIFICATION:
The end effector position (origin of frame (2)), relative to the inertial frame is given by\begin {align} r_{2}^{0} & =T_{2}^{0}r_{2}^{2}\nonumber \\ & =\begin {pmatrix}[1.3] \cos \theta _{1}\cos \theta _{2} & -\cos \theta _{1}\sin \theta _{2} & \sin \theta _{1} & L\cos \theta _{1}+L\cos \theta _{2}\cos \theta _{1}\\ \cos \theta _{2}\sin \theta _{1} & -\sin \theta _{1}\sin \theta _{2} & -\cos \theta _{1} & L\sin \theta _{1}+L\cos \theta _{2}\sin \theta _{1}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & L\sin \theta _{2}\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix}[1.3] 0\\ 0\\ 0\\ 1 \end {pmatrix} \nonumber \\ & =\begin {pmatrix}[1.3] L\cos \theta _{1}+L\cos \theta _{1}\cos \theta _{2}\\ L\sin \theta _{1}+L\cos \theta _{2}\sin \theta _{1}\\ L\sin \theta _{2}\\ 1 \end {pmatrix} \tag {1} \end {align}
When \(\theta _{1}=\theta _{2}=0\) the above becomes\[ r_{2}^{0}=\begin {pmatrix}[1.3] 2L\\ 0\\ 0\\ 1 \end {pmatrix} \] Which is what expected. Now it is verified for \(\theta _{1}=90^{0},\theta _{2}=0\), Substituting these angle values in (1) gives \[\begin {pmatrix}[1.3] 0\\ 2L\\ 0\\ 1 \end {pmatrix} \] as expected. At \(\theta _{1}=0,\theta _{2}=90^{0}\) and substituting these angle values in (1) gives \[\begin {pmatrix}[1.3] L\\ 0\\ L\\ 1 \end {pmatrix} \] as expected. Finally, at \(\theta _{1}=\theta _{2}=90^{0}\) and substituting these angle values in (1) gives \[\begin {pmatrix}[1.3] 0\\ L\\ L\\ 1 \end {pmatrix} \] as expected. The result of part (1) have been verified.
To determine the linear Jacobian for the center of the mass, the following quantities needs to be determined \[ z_{0}^{0},z_{1}^{0},o_{0}^{0},o_{1}^{0},o_{c_{1}}^{0},o_{c_{2}}^{0} \] These are obtained from the result of part (1)
\begin {align*} z_{0}^{0} & =\begin {pmatrix}[1.3] 0\\ 0\\ 1 \end {pmatrix} \\ z_{1}^{0} & =\begin {pmatrix}[1.3] \sin \theta _{1}\\ -\cos \theta _{1}\\ 0 \end {pmatrix} \\ o_{0}^{0} & =\begin {pmatrix}[1.3] 0\\ 0\\ 0 \end {pmatrix} \\ o_{1}^{0} & =\begin {pmatrix}[1.3] L\cos \theta _{1}\\ L\sin \theta _{1}\\ 0 \end {pmatrix} \\ o_{c_{1}}^{0} & =\begin {pmatrix}[1.3] \frac {L}{2}\cos \theta _{1}\\ \frac {L}{2}\sin \theta _{1}\\ 0 \end {pmatrix} \\ o_{c_{2}}^{0} & =\begin {pmatrix}[1.3] L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2}\\ L\sin \theta _{1}+\frac {1}{2}L\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}L\sin \theta _{2}\end {pmatrix} \end {align*}
Therefore \begin {align*} J_{v_{c_{1}}} & =\begin {pmatrix}[1.3] z_{0}^{0}\times \left ( o_{c_{1}}^{0}-o_{0}^{0}\right ) & \mathbf {0}\end {pmatrix} \\ & =\begin {pmatrix}[1.3]\begin {pmatrix}[1.3] 0\\ 0\\ 1 \end {pmatrix} \times \left ( \begin {pmatrix}[1.3] \frac {L}{2}\cos \theta _{1}\\ \frac {L}{2}\sin \theta _{1}\\ 0 \end {pmatrix} -\begin {pmatrix}[1.3] 0\\ 0\\ 0 \end {pmatrix} \right ) & \mathbf {0}\end {pmatrix} \\ & =\boxed { \begin {pmatrix}[1.3] -\frac {1}{2}L\sin \theta _{1} & 0\\ \frac {1}{2}L\cos \theta _{1} & 0\\ 0 & 0 \end {pmatrix} } \end {align*}
\begin {align*} J_{v_{c_{2}}} & =\begin {pmatrix}[1.3] z_{0}^{0}\times o_{c_{2}}^{0} & z_{1}^{0}\times \left ( o_{c_{2}}^{0}-o_{1}^{0}\right ) \end {pmatrix} \\ & =\begin {pmatrix}[1.3]\begin {pmatrix}[1.3] 0\\ 0\\ 1 \end {pmatrix} \times \begin {pmatrix}[1.3] L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2}\\ L\sin \theta _{1}+\frac {1}{2}L\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}L\sin \theta _{2}\end {pmatrix} & \begin {pmatrix}[1.3] \sin \theta _{1}\\ -\cos \theta _{1}\\ 0 \end {pmatrix} \times \left ( \begin {pmatrix}[1.3] L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2}\\ L\sin \theta _{1}+\frac {1}{2}L\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}L\sin \theta _{2}\end {pmatrix} -\begin {pmatrix}[1.3] L\cos \theta _{1}\\ L\sin \theta _{1}\\ 0 \end {pmatrix} \right ) \end {pmatrix} \\ & =\boxed { \begin {pmatrix}[1.3] -L\sin \theta _{1}-\frac {1}{2}L\cos \theta _{2}\sin \theta _{1} & -\frac {1}{2}L\cos \theta _{1}\sin \theta _{2}\\ L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2} & -\frac {1}{2}L\sin \theta _{1}\sin \theta _{2}\\ 0 & L\cos \theta _{2}\end {pmatrix} } \end {align*}
Now the angular velocity Jacobians are found\[ J_{\omega _{1}}=\begin {pmatrix}[1.3] \bar {\varepsilon }_{1}z_{0}^{0} & 0 \end {pmatrix} =\boxed { \begin {pmatrix}[1.3] 0 & 0\\ 0 & 0\\ 1 & 0 \end {pmatrix} } \]
\[ J_{\omega _{2}}=\begin {pmatrix}[1.3] \bar {\varepsilon }_{1}z_{0}^{0} & \bar {\varepsilon }_{2}z_{1}^{0}\end {pmatrix} =\boxed { \begin {pmatrix}[1.3] 0 & \sin \theta _{1}\\ 0 & -\cos \theta _{1}\\ 1 & 0 \end {pmatrix} } \]
VERIFICATION:
To verify the linear velocity Jacobians the following relations are used \begin {align} J_{v_{c_{1}}} & =\begin {pmatrix}[1.3] \frac {\partial r_{c_{1}}}{\partial \theta _{1}} & \frac {\partial r_{c_{1}}}{\partial \theta _{2}}\end {pmatrix} \tag {2}\\ J_{v_{c_{2}}} & =\begin {pmatrix}[1.3] \frac {\partial r_{c_{2}}}{\partial \theta _{1}} & \frac {\partial r_{c_{2}}}{\partial \theta _{2}}\end {pmatrix} \tag {3} \end {align}
Result obtained from the above is compared to part (2) result. In the above, \(r_{c_{i}}\) is the position of the center of mass of link \(i\) measured from the origin of the inertial frame. Therefore5\begin {align*} r_{c_{1}}^{0} & =T_{1}^{0}r_{c_{1}}^{1}\\ & =\begin {pmatrix}[1.3] \cos \theta _{1} & 0 & \sin \theta _{1} & L\cos \theta _{1}\\ \sin \theta _{1} & 0 & -\cos \theta _{1} & L\sin \theta _{1}\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix}[1.3] -\frac {L}{2}\\ 0\\ 0\\ 1 \end {pmatrix} \\ & =\begin {pmatrix}[1.3] \frac {1}{2}L\cos \theta _{1}\\ \frac {1}{2}L\sin \theta _{1}\\ 0\\ 1 \end {pmatrix} \end {align*}
Therefore\[ r_{c_{1}}^{0}=\begin {pmatrix}[1.3] \frac {1}{2}L\cos \theta _{1}\\ \frac {1}{2}L\sin \theta _{1}\\ 0 \end {pmatrix} \] Using the above in (2) gives\begin {align*} J_{v_{c_{1}}} & =\begin {pmatrix}[1.3] \frac {\partial }{\partial \theta _{1}}\begin {pmatrix}[1.3] \frac {1}{2}L\cos \theta _{1}\\ \frac {1}{2}L\sin \theta _{1}\\ 0 \end {pmatrix} & \frac {\partial }{\partial \theta _{2}}\begin {pmatrix}[1.3] \frac {1}{2}L\cos \theta _{1}\\ \frac {1}{2}L\sin \theta _{1}\\ 0 \end {pmatrix} \end {pmatrix} \\ & =\boxed { \begin {pmatrix}[1.3] -\frac {1}{2}L\sin \theta _{1} & 0\\ \frac {1}{2}L\cos \theta _{1} & 0\\ 0 & 0 \end {pmatrix} } \end {align*}
The above is the same result obtained in part (2). Similarly \(J_{v_{c_{2}}}\) is verified \begin {align*} r_{c_{2}}^{0} & =T_{2}^{0}r_{c_{2}}^{2}\\ & =\begin {pmatrix}[1.3] \cos \theta _{1}\cos \theta _{2} & -\cos \theta _{1}\sin \theta _{2} & \sin \theta _{1} & L\cos \theta _{1}+L\cos \theta _{2}\cos \theta _{1}\\ \cos \theta _{2}\sin \theta _{1} & -\sin \theta _{1}\sin \theta _{2} & -\cos \theta _{1} & L\sin \theta _{1}+L\cos \theta _{2}\sin \theta _{1}\\ \sin \theta _{2} & \cos \theta _{2} & 0 & L\sin \theta _{2}\\ 0 & 0 & 0 & 1 \end {pmatrix}\begin {pmatrix}[1.3] -\frac {L}{2}\\ 0\\ 0\\ 1 \end {pmatrix} \\ & =\begin {pmatrix}[1.3] L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2}\\ L\sin \theta _{1}+\frac {1}{2}L\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}L\sin \theta _{2}\\ 1 \end {pmatrix} \end {align*}
Hence\[ r_{c_{2}}^{0}=\begin {pmatrix}[1.3] L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2}\\ L\sin \theta _{1}+\frac {1}{2}L\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}L\sin \theta _{2}\end {pmatrix} \] Using the above in (3) gives\begin {align*} J_{v_{c_{2}}} & =\begin {pmatrix}[1.3] \frac {\partial }{\partial \theta _{1}}\begin {pmatrix}[1.3] L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2}\\ L\sin \theta _{1}+\frac {1}{2}L\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}L\sin \theta _{2}\end {pmatrix} & \frac {\partial }{\partial \theta _{2}}\begin {pmatrix}[1.3] L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{1}\cos \theta _{2}\\ L\sin \theta _{1}+\frac {1}{2}L\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}L\sin \theta _{2}\end {pmatrix} \end {pmatrix} \\ & =\boxed { \begin {pmatrix}[1.3] -L\sin \theta _{1}-\frac {1}{2}L\sin \theta _{1}\cos \theta _{2} & -\frac {1}{2}L\cos \theta _{1}\sin \theta _{2}\\ L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{2}\cos \theta _{1} & -\frac {1}{2}L\sin \theta _{2}\sin \theta _{1}\\ 0 & \frac {1}{2}L\cos \theta _{2}\end {pmatrix} } \end {align*}
The above is the same as found in part(2).
The mass matrix \(D(q)\) is evaluated. By definition\begin {equation} D\left ( q\right ) ={\displaystyle \sum \limits _{i=1}^{2}} m_{i}J_{v_{c_{i}}}^{T}J_{v_{c_{i}}}+J_{\omega _{i}}^{T}R_{i}^{0}I_{c_{i}}^{i}\left ( R_{i}^{0}\right ) ^{T}J_{\omega _{i}} \tag {4} \end {equation} The following is found in part (1) \begin {align*} R_{1}^{0} & =\begin {pmatrix}[1.3] \cos \theta _{1} & 0 & \sin \theta _{1}\\ \sin \theta _{1} & 0 & -\cos \theta _{1}\\ 0 & 1 & 0 \end {pmatrix} \\ R_{2}^{0} & =\begin {pmatrix}[1.3] \cos \theta _{1}\cos \theta _{2} & -\cos \theta _{1}\sin \theta _{2} & \sin \theta _{1}\\ \cos \theta _{2}\sin \theta _{1} & -\sin \theta _{1}\sin \theta _{2} & -\cos \theta _{1}\\ \sin \theta _{2} & \cos \theta _{2} & 0 \end {pmatrix} \end {align*}
Therefore (4) becomes\begin {align*} D_{1}\left ( q\right ) & =m_{1}\begin {pmatrix}[1.3] -\frac {1}{2}L\sin \theta _{1} & 0\\ \frac {1}{2}L\cos \theta _{1} & 0\\ 0 & 0 \end {pmatrix} ^{T}\begin {pmatrix}[1.3] -\frac {1}{2}L\sin \theta _{1} & 0\\ \frac {1}{2}L\cos \theta _{1} & 0\\ 0 & 0 \end {pmatrix} \\ & +\begin {pmatrix}[1.3] 0 & 0\\ 0 & 0\\ 1 & 0 \end {pmatrix} ^{T}\begin {pmatrix}[1.3] \cos \theta _{1} & 0 & \sin \theta _{1}\\ \sin \theta _{1} & 0 & -\cos \theta _{1}\\ 0 & 1 & 0 \end {pmatrix}\begin {pmatrix}[1.3] \frac {1}{2}I_a & 0 & 0\\ 0 & I_a & 0\\ 0 & 0 & I_a\end {pmatrix} \\ & \begin {pmatrix}[1.3] \cos \theta _{1} & 0 & \sin \theta _{1}\\ \sin \theta _{1} & 0 & -\cos \theta _{1}\\ 0 & 1 & 0 \end {pmatrix} ^{T}\begin {pmatrix}[1.3] 0 & 0\\ 0 & 0\\ 1 & 0 \end {pmatrix} \\ & =m_{1}\begin {pmatrix}[1.3] \frac {1}{4}L^{2}+I_a & 0\\ 0 & 0 \end {pmatrix} \end {align*}
\begin {align*} D_{2}\left ( q\right ) & =m_{2}\begin {pmatrix}[1.3] -L\sin \theta _{1}-\frac {1}{2}L\sin \theta _{1}\cos \theta _{2} & -\frac {1}{2}L\cos \theta _{1}\sin \theta _{2}\\ L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{2}\cos \theta _{1} & -\frac {1}{2}L\sin \theta _{2}\sin \theta _{1}\\ 0 & \frac {1}{2}L\cos \theta _{2}\end {pmatrix} ^{T}\begin {pmatrix}[1.3] -L\sin \theta _{1}-\frac {1}{2}L\sin \theta _{1}\cos \theta _{2} & -\frac {1}{2}L\cos \theta _{1}\sin \theta _{2}\\ L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{2}\cos \theta _{1} & -\frac {1}{2}L\sin \theta _{2}\sin \theta _{1}\\ 0 & \frac {1}{2}L\cos \theta _{2}\end {pmatrix} \\ & +\begin {pmatrix}[1.3] 0 & \sin \theta _{1}\\ 0 & -\cos \theta _{1}\\ 1 & 0 \end {pmatrix} ^{T}\begin {pmatrix}[1.3] \cos \theta _{1}\cos \theta _{2} & -\cos \theta _{1}\sin \theta _{2} & \sin \theta _{1}\\ \cos \theta _{2}\sin \theta _{1} & -\sin \theta _{1}\sin \theta _{2} & -\cos \theta _{1}\\ \sin \theta _{2} & \cos \theta _{2} & 0 \end {pmatrix}\begin {pmatrix}[1.3] \frac {1}{2}I_{a} & 0 & 0\\ 0 & I_{a} & 0\\ 0 & 0 & I_{a}\end {pmatrix} \\ & \begin {pmatrix}[1.3] \cos \theta _{1}\cos \theta _{2} & -\cos \theta _{1}\sin \theta _{2} & \sin \theta _{1}\\ \cos \theta _{2}\sin \theta _{1} & -\sin \theta _{1}\sin \theta _{2} & -\cos \theta _{1}\\ \sin \theta _{2} & \cos \theta _{2} & 0 \end {pmatrix} ^{T}\begin {pmatrix}[1.3] 0 & \sin \theta _{1}\\ 0 & -\cos \theta _{1}\\ 1 & 0 \end {pmatrix} \\ & =m_{2}\begin {pmatrix}[1.3] \frac {1}{8}\left ( 6I_{a}+9L^{2}+8L^{2}\cos \theta _{2}+\left ( 2I_{a}+L^{2}\right ) \cos \left ( 2\theta _{2}\right ) \right ) & 0\\ 0 & I_{a}+\frac {L^{2}}{4}\end {pmatrix} \end {align*}
Hence the \(D\left ( q\right ) \) becomes\begin {align*} D\left ( q\right ) & =\begin {pmatrix}[1.3] m_{1}\left ( \frac {1}{4}L^{2}+I_a\right ) & 0\\ 0 & 0 \end {pmatrix} +m_{2}\begin {pmatrix}[1.3] \frac {1}{8}\left ( 6I_{a}+9L^{2}+8L^{2}\cos \theta _{2}+\left ( 2I_{a}+L^{2}\right ) \cos \left ( 2\theta _{2}\right ) \right ) & 0\\ 0 & I_{a}+\frac {L^{2}}{4}\end {pmatrix} \\ & =\boxed { \begin {pmatrix}[1.3] m_{1}\left ( I_a+\frac {1}{4}L^{2}\right ) +\frac {1}{8}m_{2}\left ( 6I_{a}+9L^{2}+8L^{2}\cos \theta _{2}+\left ( 2I_{a}+L^{2}\right ) \cos \left ( 2\theta _{2}\right ) \right ) & 0\\ 0 & m_{2}\left ( I_{a}+\frac {L^{2}}{4}\right ) \end {pmatrix} } \end {align*}
The Coriolis term \(B(q) \left [\dot {q}\dot {q}\right ]\) is now evaluated \begin {equation} B\left ( q\right ) \left [ \dot {q}\dot {q}\right ] =\begin {bmatrix} 2b_{1,12}\\ 2b_{2,12}\end {bmatrix} \left [ \dot {\theta }_{1}\dot {\theta }_{2}\right ] \tag {1} \end {equation} Where \(b_{i,jk}\) is the Christoffel symbol of first kind defined as\[ b_{i,jk}=\frac {1}{2}\left ( d_{ijk}+d_{ikj}-d_{jki}\right ) \] Where \(d_{ijk}=\frac {\partial d_{ij}}{\partial q_{k}}\). Using these in (1) gives\begin {align*} B\left ( q\right ) \left [ \dot {q}\dot {q}\right ] & =\begin {bmatrix} d_{112}+d_{121}-d_{121}\\ d_{212}+d_{221}-d_{122}\end {bmatrix} \left [ \dot {\theta }_{1}\dot {\theta }_{2}\right ] \\ & =\begin {bmatrix} d_{112}\\ d_{212}+d_{221}-d_{122}\end {bmatrix} \left [ \dot {\theta }_{1}\dot {\theta }_{2}\right ] \\ & =\begin {bmatrix} \frac {\partial d_{11}}{\partial q_{2}}\\ \frac {\partial d_{21}}{\partial q_{2}}+\frac {\partial d_{22}}{\partial q_{1}}-\frac {\partial d_{12}}{\partial q_{2}}\end {bmatrix} \left [ \dot {\theta }_{1}\dot {\theta }_{2}\right ] \end {align*}
The mass matrix was found in part (3) as\[ D\left ( q\right ) =\begin {bmatrix} m_{1}\left ( I_a+\frac {1}{4}L^{2}\right ) +\frac {1}{8}m_{2}\left ( 6I_{a}+9L^{2}+8L^{2}\cos \theta _{2}+\left ( 2I_{a}+L^{2}\right ) \cos \left ( 2\theta _{2}\right ) \right ) & 0\\ 0 & m_{2}\left ( I_{a}+\frac {L^{2}}{4}\right ) \end {bmatrix} \] Hence\begin {align*} \frac {\partial d_{11}}{\partial q_{2}} & =\frac {\partial d_{11}}{\partial \theta _{2}}\\ & =\frac {\partial }{\partial \theta _{2}}\left [ m_{1}\left ( I_a+\frac {1}{4}L^{2}\right ) +\frac {1}{8}m_{2}\left ( 6I_{a}+9L^{2}+8L^{2}\cos \theta _{2}+\left ( 2I_{a}+L^{2}\right ) \cos \left ( 2\theta _{2}\right ) \right ) \right ] \\ & =\frac {1}{8}m_{2}\left ( -8L^{2}\sin \theta _{2}-2\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \right ) \\ & =-m_{2}L^{2}\sin \theta _{2}-\frac {1}{4}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \end {align*}
And\begin {align*} \frac {\partial d_{21}}{\partial q_{2}} & =\frac {\partial d_{21}}{\partial \theta _{2}}\\ & =0 \end {align*}
And\begin {align*} \frac {\partial d_{22}}{\partial q_{1}} & =\frac {\partial d_{22}}{\partial \theta _{1}}\\ & =0 \end {align*}
And\begin {align*} \frac {\partial d_{12}}{\partial q_{2}} & =\frac {\partial d_{12}}{\partial \theta _{2}}\\ & =0 \end {align*}
Hence\begin {align*} B\left ( q\right ) \left [ \dot {q}\dot {q}\right ] & =\begin {bmatrix} -m_{2}L^{2}\sin \theta _{2}-\frac {1}{4}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \\ 0 \end {bmatrix} \left [ \dot {\theta }_{1}\dot {\theta }_{2}\right ] \\ & =\boxed { \begin {bmatrix} -m_{2}\dot {\theta }_{1}\dot {\theta }_{2}L^{2}\sin \theta _{2}-\frac {1}{4}\dot {\theta }_{1}\dot {\theta }_{2}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \\ 0 \end {bmatrix} } \end {align*}
The centrifugal term is now evaluated\[ C\left ( q\right ) \left [ \dot {q}^{2}\right ] =\begin {bmatrix} b_{1,11} & b_{1,22}\\ b_{2,11} & b_{2,22}\end {bmatrix}\begin {bmatrix} \dot {\theta }_{1}^{2}\\ \dot {\theta }_{2}^{2}\end {bmatrix} \] Where \begin {align*} b_{1,11} & =\frac {1}{2}\left ( d_{111}+d_{111}-d_{111}\right ) \\ & =\frac {1}{2}d_{111}\\ & =\frac {1}{2}\frac {\partial d_{11}}{\partial q_{1}}\\ & =0 \end {align*}
And\begin {align*} b_{1,22} & =\frac {1}{2}\left ( d_{122}+d_{122}-d_{221}\right ) \\ & =d_{122}-\frac {1}{2}d_{221}\\ & =\frac {\partial d_{12}}{\partial \theta _{2}}-\frac {1}{2}\frac {\partial d_{22}}{\partial \theta _{1}}\\ & =0 \end {align*}
And\begin {align*} b_{2,11} & =\frac {1}{2}\left ( d_{211}+d_{211}-d_{112}\right ) \\ & =d_{211}-\frac {1}{2}d_{112}\\ & =\frac {\partial d_{21}}{\partial \theta _{1}}-\frac {1}{2}\frac {\partial d_{11}}{\partial \theta _{2}}\\ & =0-\frac {1}{2}\left [ -m_{2}L^{2}\sin \theta _{2}-\frac {1}{4}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \right ] \\ & =\frac {1}{2}m_{2}L^{2}\sin \theta _{2}+\frac {1}{8}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \end {align*}
And\begin {align*} b_{2,22} & =\frac {1}{2}\left ( d_{222}+d_{222}-d_{222}\right ) \\ & =\frac {1}{2}d_{222}\\ & =\frac {1}{2}\frac {\partial d_{21}}{\partial \theta _{2}}\\ & =0 \end {align*}
Therefore\begin {align*} C\left ( q\right ) \left [ \dot {q}^{2}\right ] & =\begin {bmatrix} 0 & 0\\ \frac {1}{2}m_{2}L^{2}\sin \theta _{2}+\frac {1}{8}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) & 0 \end {bmatrix}\begin {bmatrix} \dot {\theta }_{1}^{2}\\ \dot {\theta }_{2}^{2}\end {bmatrix} \\ & =\boxed { \begin {bmatrix} 0\\ \frac {1}{2}m_{2}\dot {\theta }_{1}^{2}L^{2}\sin \theta _{2}+\frac {1}{8}m_{2}\dot {\theta }_{1}^{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \end {bmatrix} } \end {align*}
The gravity vector \(G(q)\) is now evaluated\begin {equation} G\left ( q\right ) =-\left [ J_{v_{c_{1}}}^{T}m_{1}\mathbf {g}+J_{v_{c_{2}}}^{T}m_{2}\mathbf {g}\right ] \tag {1} \end {equation} From part(2) we found\begin {align*} J_{v_{c_{1}}} & =\begin {bmatrix} -\frac {1}{2}L\sin \theta _{1} & 0\\ \frac {1}{2}L\cos \theta _{1} & 0\\ 0 & 0 \end {bmatrix} \\ J_{v_{c_{2}}} & =\begin {bmatrix} -L\sin \theta _{1}-\frac {1}{2}L\sin \theta _{1}\cos \theta _{2} & -\frac {1}{2}L\cos \theta _{1}\sin \theta _{2}\\ L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{2}\cos \theta _{1} & -\frac {1}{2}L\sin \theta _{2}\sin \theta _{1}\\ 0 & \frac {1}{2}L\cos \theta _{2}\end {bmatrix} \end {align*}
Hence (1) becomes\begin {align*} G\left ( q\right ) & =-m_{1}g\begin {bmatrix} -\frac {1}{2}L\sin \theta _{1} & 0\\ \frac {1}{2}L\cos \theta _{1} & 0\\ 0 & 0 \end {bmatrix} ^{T}\begin {bmatrix} 1\\ 0\\ 0 \end {bmatrix} -m_{2}g\begin {bmatrix} -L\sin \theta _{1}-\frac {1}{2}L\sin \theta _{1}\cos \theta _{2} & -\frac {1}{2}L\cos \theta _{1}\sin \theta _{2}\\ L\cos \theta _{1}+\frac {1}{2}L\cos \theta _{2}\cos \theta _{1} & -\frac {1}{2}L\sin \theta _{2}\sin \theta _{1}\\ 0 & \frac {1}{2}L\cos \theta _{2}\end {bmatrix} ^{T}\begin {bmatrix} 1\\ 0\\ 0 \end {bmatrix} \\ & =\begin {bmatrix} \frac {1}{2}gm_{1}L\sin \theta _{1}\\ 0 \end {bmatrix} -\begin {bmatrix} -Lgm_{2}\sin \theta _{1}-\frac {1}{2}Lgm_{2}\cos \theta _{2}\sin \theta _{1}\\ -\frac {1}{2}gm_{2}L\cos \theta _{1}\sin \theta _{2}\end {bmatrix} \\ & =\boxed { \begin {bmatrix} \frac {1}{2}Lgm_{1}\sin \theta _{1}+Lgm_{2}\sin \theta _{1}+\frac {1}{2}Lgm_{2}\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}Lgm_{2}\cos \theta _{1}\sin \theta _{2}\end {bmatrix} } \end {align*}
The equation of motion is now found using above results. Using the notation \([\quad ]\) for a matrix and \(\{ \quad \}\) for a vector, it is written as
\[ \boxed { \overbrace {[D] \left \{\ddot {q}\right \} }^{\text {mass matrix}} + \overbrace { [B] \{\dot {q}\dot {q}\}}^{\text {Coriolis}} + \overbrace { [C] \left \{\dot {q}^{2}\right \}}^{\text {centrifugal}} + \overbrace { [G] }^{\text {gravity}} = \overbrace { \{\tau \} } ^{\text {torques}} } \]
Since there is no applied external torques or forces, the right hand side is zero. The equation of motion becomes
\begin {equation*} \boxed { \begin {split} & \begin {bmatrix} m_{1}\left ( I_a+\frac {1}{4}L^{2}\right ) +\frac {1}{8}m_{2}\left ( 6I_{a}+9L^{2}+8L^{2}\cos \theta _{2}+\left ( 2I_{a}+L^{2}\right ) \cos \left ( 2\theta _{2}\right ) \right ) & 0\\ 0 & m_{2}\left ( I_{a}+\frac {L^{2}}{4}\right ) \end {bmatrix}\begin {bmatrix} \ddot {\theta }_{1}\\ \ddot {\theta }_{2}\end {bmatrix} \\ &+\begin {bmatrix} -m_{2}L^{2}\sin \theta _{2}-\frac {1}{4}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \\ 0 \end {bmatrix} \left [ \dot {\theta }_{1}\dot {\theta }_{2}\right ] +\begin {bmatrix} 0 & 0\\ \frac {1}{2}m_{2}L^{2}\sin \theta _{2}+\frac {1}{8}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) & 0 \end {bmatrix}\begin {bmatrix} \dot {\theta }_{1}^{2}\\ \dot {\theta }_{2}^{2}\end {bmatrix}\\ &+\begin {bmatrix} \frac {1}{2}Lgm_{1}\sin \theta _{1}+Lgm_{2}\sin \theta _{1}+\frac {1}{2}Lgm_{2}\cos \theta _{2}\sin \theta _{1}\\ \frac {1}{2}Lgm_{2}\cos \theta _{1}\sin \theta _{2}\end {bmatrix} =\begin {bmatrix} \tau _{1}\\ \tau _{1}\end {bmatrix} \end {split} } \end {equation*} Looking at each equation of motion on its own, for \(\theta _{1}\) the equation of motion is\begin {equation*} \boxed { \begin {split} &\ddot {\theta }_1\left [ m_1 \left (I_a+\frac {1}{4}L^{2}\right ) +\frac {1}{8}m_2\left ( 6I_a+ \left ( \cos (2\theta _2)\right ) \left ( L^{2}+2I_a\right ) +8L^2 \cos \theta _2+9L^2 \right ) \right ]\\ & -\left ( m_2 L^{2}\sin \theta _2 +\frac {1}{4}m_2\left ( 2I_a+L^{2}\right ) \sin \left ( 2\theta _2\right ) \right ) \dot {\theta }_1\dot {\theta }_{2} +\frac {1}{2}Lgm_{1}\sin \theta _{1}+Lgm_{2}\sin \theta _1+\frac {1}{2}L gm_2 \cos \theta _2\sin \theta _1=0 \end {split} } \end {equation*} And the equation of motion for \(\theta _{2}\) is \[ \boxed { \ddot {\theta }_{2}m_{2}\left ( \frac {1}{4}L^{2}+I_{a}\right ) +\left ( \frac {1}{2}m_{2}L^{2}\sin \theta _{2}+\frac {1}{8}m_{2}\left ( 2I_{a}+L^{2}\right ) \sin \left ( 2\theta _{2}\right ) \right ) \dot {\theta }_{1}^{2}+\frac {1}{2}Lgm_{2}\cos \theta _{1}\sin \theta _{2}=0 } \] These are coupled differential equations since \(\theta _{2},\theta _{1}\) appears in both equations. They are also nonlinear due to the \(\dot {\theta }_{2}^{2},\dot {\theta }_{1}^{2}\,\) terms.
The equations of motions for the 2 link serial manipulator were simulated by modifying the learn UW Matlab code using results of problem 1.
A small amount of damping \(c=1\,\) N-m/r/s was added to both joints in the file zDot2dof.m and the simulation was run for 20 seconds.
When adding damping, the equation of motion becomes \[ \boxed { \overbrace {[D] \left \{\ddot {q}\right \} }^{\text {mass matrix}} + \overbrace {c \{\dot {q}\} }^{\text {damping}} + \overbrace { [B] \{\dot {q}\dot {q}\}}^{\text {Coriolis}} + \overbrace { [C] \left \{\dot {q}^{2}\right \}}^{\text {centrifugal}} + \overbrace { [G] }^{\text {gravity}} = \overbrace { \{\tau \} } ^{\text {torques}} } \]
Where \(c\) above is the damping constant used.
The diagram below shows that total energy decreased as would be expected.
The damping \(c\) was now increased to 5 N-m/r/s and the simulation time increased to 50 seconds. Now the total energy decreased to almost zero by the end of the simulation and the robot arm came close to being at rest. This was done as an additional verification yo verify the equations of motion.
The damping constant was removed and the simulation run again for 20 seconds. The total energy remained constant as would be expected
The following is a movie of the first 10 seconds of the simulation for the zero damping case.
The following are the Matlab source code listings of all the files used used for the implementation of this problem.
%nma_HW3_problem.m %This is modified version of the UW learn script used to solve %probvlem 2, HW 3. ME 739, Univ. Wisconsin Madison %Nasser M. Abbasi %----------------------------------------------------------------------- % NUMERICAL INTEGRATION OF DYNAMIC EQUATIONS %----------------------------------------------------------------------- clear all; close all; clc; DO_MOVIE = false; %make true to generate movie frames % model parameters modelParameters.g = 9.81; % gravitational constant [m/s^2] modelParameters.m = 10; % link mass [kg] modelParameters.L = 2; % link length [m] modelParameters.Ia = 5; % inertia (_|_ to link's CL) [kg/m^2] % initialize integration variables dT = .01; %integration step size tend = 20; %simulation run time numPts = floor(tend/dT); q = zeros(2,numPts); %pre-allocate array dq = zeros(2,numPts); t = zeros(1,numPts); q(:,1) = [pi; 0]; %initial position qd(:,1) = [0.1; 0.1]; %initial velocity z = [q(:,1); qd(:,1)]; %initialize the state variables % integrate equations of motion for i = 1:numPts-1 % Runge-Kutta 4th order k1 = zDot2dof(z,modelParameters); k2 = zDot2dof(z + 0.5*k1*dT,modelParameters); k3 = zDot2dof(z + 0.5*k2*dT,modelParameters); k4 = zDot2dof(z + k3*dT,modelParameters); z = z + (1/6)*(k1 + 2*k2 + 2*k3 + k4)*dT; % store joint position and velocity for post processing q(:,i+1) = z(1:2); qd(:,i+1) = z(3:4); t(1,i+1) = t(1,i) + dT; end %----------------------------------------------------------------------- % RENDERING INITIALIZATION %----------------------------------------------------------------------- %----set rendering window view parameters L = modelParameters.L; L1 = L; L2 = L; f_handle = 1; axis_limits = L*[-2 2 -2 2 -1 1]; render_view = [-.4 -.8 .5]; view_up = [-1 0 0]; SetRenderingViewParameters(axis_limits,render_view,view_up,f_handle); camproj perspective % turns on perspective %----initialize rendering % link 00 rendering initialization, this is the base on the wall % fixed, used for illustration r00 = L; sides00 = 4; axis00 = 3; norm_L00 = 1.0; linkColor1 = [.6 0.75 0]; plotFrame00 = 0; d00 = CreateLinkRendering(0.01*L,r00,sides00,axis00,norm_L00,... linkColor1,plotFrame00,f_handle); % link 0 this is the rod holding the arm, fixed does not move r0 = L/10; sides0 = 10; axis0 = 3; norm_L = 1.0; linkColor0 = [0.75 1 1]; plotFrame0 = 0; d0 = CreateLinkRendering(L,r0,sides0,axis0,norm_L,linkColor0,... plotFrame0,f_handle); % link 1 rendering initialization r1 = L1/5; sides1 = 10; axis1 = 1; norm_L1 = 1.0; linkColor1 = [0 0.75 0]; plotFrame1 = 0; d1 = CreateLinkRendering(L1,r1,sides1,axis1,norm_L1,linkColor1,... plotFrame1,f_handle); % link 2 rendering initialization r2 = L2/6; sides2 = 10; axis2 = 1; norm_L2 = 1.0; linkColor2 = [0.75 0 0]; plotFrame2 = 0; d2 = CreateLinkRendering(L2,r2,sides2,axis2,norm_L2,linkColor2,... plotFrame2,f_handle); %------------------------------------------------------------------- % DISPLAY INTERATION RESULTS %------------------------------------------------------------------- %since these links are fixed, they are set outside the loop T00 = [1 0 0 0 0 1 0 0 0 0 1 -L 0 0 0 1]; T0 = [1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1]; UpdateLink(d00,T00); UpdateLink(d0,T0); k = 0; for i = 1:numPts % Update frame {1} c = cos(q(1,i)); s = sin(q(1,i)); L = L1; T10 = [c 0 s L*c s 0 -c L*s 0 1 0 0 0 0 0 1]; % Update frame {2} c = cos(q(2,i)); s = sin(q(2,i)); L = L2; T21 = [c -s 0 L*c s c 0 L*s 0 0 1 0 0 0 0 1]; T20 = T10*T21; UpdateLink(d1,T10); UpdateLink(d2,T20); title(sprintf('time = %f',i*dT)); if i == 1; %pause at start of simulation rendering pause; end if DO_MOVIE k = k+1; I = getframe(gcf); imwrite(I.cdata, sprintf('frame%d.png',k)); end pause(dT); end %------------------------------------------------------------------- % ENERGY BALANCE - TO CHECK SIMULATION RESULTS %------------------------------------------------------------------- V = zeros(numPts,1); T = V; E = V; for i = 1:numPts % kinetic energy Q = q(:,i); Qd = qd(:,i); D = Dmatrix2dof(Q,modelParameters); T(i) = (1/2)*Qd'*D*Qd; % potential energy % evaluate position of links center of mass % Link 1: c = cos(q(1,i)); s = sin(q(1,i)); L = L1; T10 = [c 0 s L*c s 0 -c L*s 0 1 0 0 0 0 0 1]; L = L1/2; T10c = [c 0 s L*c s 0 -c L*s 0 1 0 0 0 0 0 1]; % Link 2: c = cos(q(2,i)); s = sin(q(2,i)); L = L2; T21 = [c -s 0 L*c s c 0 L*s 0 0 1 0 0 0 0 1]; L = L2/2; T21c = [c -s 0 L*c s c 0 L*s 0 0 1 0 0 0 0 1]; T20 = T10*T21; T20c = T10*T21c; % assign center of mass position vectors rc1 = T10c(1:3,4); rc2 = T20c(1:3,4); g = modelParameters.g*[1; 0; 0]; m = modelParameters.m; % calculate the gravitational potential energy V(i) = -(m*g'*rc1 + m*g'*rc2 ); % calculate the total system energy E(i) = V(i) + T(i); end % plot the energy terms as a function of time figure; plot(t,E,'b',t, V,'g',t,T,'r'); legend('total energy', 'potential energy', 'kinetic energy') xlabel('Time [seconds]');ylabel('Energy [joules]'); title({'System energy as a function of time','HW 3, problem 2'}); axis([0 max(t) -1.5*max(E) 2*max(E)])
%zDot2dof.m %problem 2, HW 3. ME 739, Univ. Wisconsin Madison %called from RK-4 numerical integration method %Nasser M. Abbasi function [zDot] = zDot2dof(z,modelParameters); % assign joint displacements / velocities from state variables q = [z(1); z(2)]; qd = [z(3); z(4)]; % calculate D, B, D, and G matrices D = Dmatrix2dof(q,modelParameters); B = Bmatrix2dof(q,modelParameters); C = Cmatrix2dof(q,modelParameters); G = Gvector2dof(q,modelParameters); %viscous friction in joints tau_friction = 0; %5*qd; %5*qd; %acceleration qdqd = [qd(1)*qd(2)]; qd2 = [qd(1)*qd(1); qd(2)*qd(2)]; qdd = D\(-B*qdqd - C*qd2 - G - tau_friction); % assign state variable derivatives zDot = [qd; qdd]; end
function G = Gvector2dof(q,modelParameters) % assign model parameters to local variables g = modelParameters.g; m = modelParameters.m; L = modelParameters.L; G=zeros(2,1); G(1,1) = 1/2*L*m*g*sin(q(1))+L*g*m*sin(q(1))+1/2*L*g*m*cos(q(2))*sin(q(1)); G(2,1) = 1/2*L*g*m*cos(q(1))*sin(q(2)); end
%Dmatrix2dof.m %probvlem 2, HW 3. ME 739, Univ. Wisconsin Madison %build the mass matrix D %Nasser M. Abbasi function D = Dmatrix2dof(q,modelParameters) % assign model parameters to local variables m = modelParameters.m; Ia = modelParameters.Ia; L = modelParameters.L; D=zeros(2,2); D(1,1) = m*(Ia+1/4*L^2)+1/8*m*(6*Ia+9*L^2+8*L^2*cos(q(2))... +(2*Ia+L^2)*cos(2*q(2))); D(1,2) = 0; D(2,1) = D(1,2); D(2,2) = m*(Ia+L^2/4); end
%Cmatrix2dof.m %probvlem 2, HW 3. ME 739, Univ. Wisconsin Madison %build the C matrix %Nasser M. Abbasi function C = Cmatrix2dof(q,modelParameters) % assign model parameters to local variables m = modelParameters.m; Ia = modelParameters.Ia; L = modelParameters.L; C=zeros(2,2); C(1,1) = 0; C(1,2) = 0; C(2,1) = 1/2*m*L^2*sin(q(2))+1/8*m*(2*Ia+L^2)*sin(2*q(2)); C(2,2) = 0; end
%Bmatrix2dof.m %probvlem 2, HW 3. ME 739, Univ. Wisconsin Madison %build the B matrix %Nasser M. Abbasi function B = Bmatrix2dof(q,modelParameters) % assign model parameters to local variables m = modelParameters.m; Ia = modelParameters.Ia; L = modelParameters.L; B=zeros(2,1); B(1,1) = -m*L^2*sin(q(2))-1/4*m*(2*Ia+L^2)*sin(2*q(2)); B(2,1) = 0; end
5we can also use \(r_{c_{1}}^{0}=T_{c_{1}}^{0}r_{c_{1}}^{c_{1}}\) where in this case \(r_{c_{1}}^{c_{1}}=\left [ 0,0,0,1\right ] ^{T}\)