**2. Inverse kinematics and dynamics model of the 3-DOF RRR FPPM**

In this section, geometric description, inverse kinematics, Jacobian matrix & Jacobian inversion and inverse dynamics model of the 3-DOF RRR FPPM in explicit form are obtained by applying DH method.

$$\rm{OP}\_{l} + \rm{B}\_{l}M\_{l} = \rm{OP} + \rm{PM}\_{l} \tag{1}$$

$$=\begin{bmatrix} \cos(\mathbf{y}\_1 + \boldsymbol{\Phi}) & -\sin(\mathbf{y}\_1 + \boldsymbol{\Phi}) & 0 & \mathbf{P}\_{\mathbf{X}\_{\mathbf{B}}} + \mathbf{n}\_{\mathbf{l}}\cos\mathbf{y}\_{\mathbf{l}}\cos\boldsymbol{\Phi} - \mathbf{n}\_{\mathbf{l}}\sin\mathbf{y}\_{\mathbf{l}}\sin\boldsymbol{\Phi} \\ \sin(\mathbf{y}\_1 + \boldsymbol{\Phi}) & \cos(\mathbf{y}\_1 + \boldsymbol{\Phi}) & 0 & \mathbf{P}\_{\mathbf{Y}\_{\mathbf{B}}} + \mathbf{n}\_{\mathbf{l}}\cos\mathbf{y}\_{\mathbf{l}}\sin\boldsymbol{\Phi} + \mathbf{n}\_{\mathbf{l}}\sin\mathbf{y}\_{\mathbf{l}}\cos\boldsymbol{\Phi} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{3}$$

$$
\begin{bmatrix}
\mathbf{l\_{2i}}\cos(\theta\_{\mathbf{i}} + \alpha\_{\mathbf{i}}) \\
\mathbf{l\_{2i}}\sin(\theta\_{\mathbf{i}} + \alpha\_{\mathbf{i}})
\end{bmatrix} = \begin{bmatrix}
\mathbf{P\_{X\_{B}}} + \mathbf{b\_{X\_{i}}}\cos\phi - \mathbf{b\_{Y\_{i}}}\sin\phi - \mathbf{o\_{x\_{i}}} - \mathbf{l\_{2i-1}}\cos\theta\_{\mathbf{i}} \\
\mathbf{P\_{Y\_{B}}} + \mathbf{b\_{X\_{i}}}\sin\phi + \mathbf{b\_{Y\_{i}}}\cos\phi - \mathbf{o\_{Y\_{i}}} - \mathbf{l\_{2i-1}}\sin\theta\_{\mathbf{i}}
\end{bmatrix} \tag{4}
$$

$$\mathbf{A}\_{\mathbf{l}}\sin\Theta\_{\mathbf{l}} + \mathbf{B}\_{\mathbf{l}}\cos\Theta\_{\mathbf{l}} = \mathbf{C}\_{\mathbf{l}} \tag{6}$$

$$\Theta\_{\rm l} = \text{Atom2}\{\mathbf{A}\_{\rm l}, \mathbf{B}\_{\rm l}\} \mp \text{Atom2}\left(\sqrt{\mathbf{A}\_{\rm l}^2 + \mathbf{B}\_{\rm l}^2 - \mathbf{C}\_{\rm l}^2}, \mathbf{C}\_{\rm l}\right) \tag{7a}$$

$$\mathbf{a\_{i}} = \text{AtomZ(D\_{l}, E\_{l})} \mp \text{AtomZ}\left(\sqrt{\mathbf{D\_{l}^{2} + E\_{l}^{2} - G\_{l}^{2}}}, \mathbf{G\_{l}}\right) \tag{7b}$$

 $\mathbf{D\_{l}} = -\sin\theta\_{l\nu}$   $\mathbf{E\_{l}} = \cos\theta\_{l}$ 

$$\mathbf{G}\_{\mathbf{l}} = \left(\mathbf{P}\_{\mathbf{X}\_{\mathcal{B}}} + \mathbf{b}\_{\mathbf{x}\_{\mathcal{l}}}\cos\Phi - \mathbf{b}\_{\mathbf{y}\_{\mathcal{l}}}\sin\Phi - \mathbf{o}\_{\mathbf{x}\_{\mathcal{l}}} - \mathbf{l}\_{2\mathbf{l}-1}\cos\Theta\_{\mathbf{l}}\right) / \mathbf{l}\_{2\mathbf{l}}$$

$$\mathbf{B}\dot{\mathbf{q}} = \mathbf{A}\dot{\mathbf{x}}$$

$$
\begin{bmatrix}
\mathbf{d}\_1 & \mathbf{0} & \mathbf{0} \\
\mathbf{0} & \mathbf{d}\_2 & \mathbf{0} \\
\mathbf{0} & \mathbf{0} & \mathbf{d}\_3
\end{bmatrix}
\begin{bmatrix}
\dot{\boldsymbol{\Phi}}\_1 \\
\dot{\boldsymbol{\Phi}}\_2 \\
\dot{\boldsymbol{\Phi}}\_3
\end{bmatrix} = \begin{bmatrix}
\mathbf{a}\_1 & \mathbf{b}\_1 & \mathbf{c}\_1 \\
\mathbf{a}\_2 & \mathbf{b}\_2 & \mathbf{c}\_2 \\
\mathbf{a}\_3 & \mathbf{b}\_3 & \mathbf{c}\_3
\end{bmatrix} \begin{bmatrix}
\dot{\boldsymbol{\Phi}}\_{\mathbf{X}\_{\mathbf{B}}} \\
\dot{\boldsymbol{\Phi}}\_{\mathbf{Y}\_{\mathbf{B}}} \\
\dot{\boldsymbol{\Phi}}\_{\mathbf{\dot{\Phi}}}
\end{bmatrix} \tag{8}
$$

$$\mathbf{a}\_{\mathbf{i}} = -2\{\mathbf{P}\_{\mathbf{X}\_{\mathbf{B}}} - \mathbf{o}\_{\mathbf{x}\_{\mathbf{i}}} + \mathbf{b}\_{\mathbf{x}\_{\mathbf{i}}}\cos\phi - \mathbf{l}\_{2\mathbf{i}-1}\cos\theta\_{\mathbf{i}} - \mathbf{b}\_{\mathbf{y}\_{\mathbf{i}}}\sin\phi\}$$

$$\mathbf{b}\_{\mathbf{i}} = -2\{\mathbf{P}\_{\mathbf{Y}\_{\mathbf{B}}} - \mathbf{o}\_{\mathbf{y}\_{\mathbf{i}}} + \mathbf{b}\_{\mathbf{y}\_{\mathbf{i}}}\cos\phi - \mathbf{l}\_{2\mathbf{i}-1}\sin\theta\_{\mathbf{i}} + \mathbf{b}\_{\mathbf{x}\_{\mathbf{i}}}\sin\phi\}$$

$$\begin{split} \mathbf{c}\_{i} = -2 \Big[ \mathbf{l}\_{2\bar{i}-1} \mathbf{b}\_{\mathbf{y}\_{1}} \cos(\phi - \theta\_{\mathbf{i}}) + \mathbf{l}\_{2\bar{i}-1} \mathbf{b}\_{\mathbf{x}\_{1}} \sin(\phi - \theta\_{\mathbf{i}}) + \cos \phi \Big( \mathbf{P}\_{\mathbf{Y}\_{\mathbf{B}}} \mathbf{b}\_{\mathbf{x}\_{1}} - \mathbf{P}\_{\mathbf{X}\_{\mathbf{B}}} \mathbf{b}\_{\mathbf{y}\_{1}} - \mathbf{b}\_{\mathbf{x}\_{\mathbf{i}}} \mathbf{o}\_{\mathbf{y}\_{1}} + \mathbf{b}\_{\mathbf{y}\_{1}} \mathbf{o}\_{\mathbf{x}\_{\mathbf{i}}} \Big) \Big] \\ + \sin \phi \Big( \mathbf{b}\_{\mathbf{x}\_{1}} \mathbf{o}\_{\mathbf{x}\_{1}} + \mathbf{b}\_{\mathbf{y}\_{1}} \mathbf{o}\_{\mathbf{y}\_{1}} - \mathbf{P}\_{\mathbf{X}\_{\mathbf{B}}} \mathbf{b}\_{\mathbf{x}\_{1}} - \mathbf{P}\_{\mathbf{Y}\_{\mathbf{B}}} \mathbf{b}\_{\mathbf{y}\_{1}} \Big) \Big] \end{split}$$

$$\mathbf{d}\_{\mathbf{i}} = \mathbf{2} \left[ \mathbf{l}\_{2\mathbf{i}-1} \cos \theta\_{\mathbf{i}} \left( \mathbf{o}\_{\mathbf{y}\_{\mathbf{i}}} - \mathbf{P}\_{\mathbf{Y}\_{\mathbf{B}}} \right) + \mathbf{l}\_{2\mathbf{i}-1} \sin \theta\_{\mathbf{i}} \left( \mathbf{P}\_{\mathbf{X}\_{\mathbf{B}}} - \mathbf{o}\_{\mathbf{x}\_{\mathbf{i}}} \right) - \mathbf{l}\_{2\mathbf{i}-1} \mathbf{b}\_{\mathbf{y}\_{\mathbf{i}}} \cos \{ \boldsymbol{\phi} - \boldsymbol{\theta}\_{\mathbf{i}} \} \right]$$

$$-\mathbf{l}\_{2\mathbf{i}-1} \mathbf{b}\_{\mathbf{x}} \sin \{ \boldsymbol{\phi} - \boldsymbol{\theta}\_{\mathbf{i}} \}$$

$$\mathbf{J} = \mathbf{B}^{-1} \mathbf{A} = \begin{bmatrix} \frac{\mathbf{a}\_1}{\mathbf{d}\_1} & \frac{\mathbf{b}\_1}{\mathbf{d}\_1} & \frac{\mathbf{c}\_1}{\mathbf{d}\_1} \\ \frac{\mathbf{a}\_2}{\mathbf{d}\_2} & \frac{\mathbf{b}\_2}{\mathbf{d}\_2} & \frac{\mathbf{c}\_2}{\mathbf{d}\_2} \\ \frac{\mathbf{a}\_3}{\mathbf{d}\_3} & \frac{\mathbf{b}\_3}{\mathbf{d}\_3} & \frac{\mathbf{c}\_3}{\mathbf{d}\_3} \end{bmatrix} \tag{9}$$

$$
\dot{\boldsymbol{\Theta}} = \mathbf{J}\dot{\boldsymbol{\chi}}\tag{10}
$$

$$
\dot{\mathbf{y}} = \mathbf{I}\ddot{\mathbf{x}} + \mathbf{I}\dot{\mathbf{x}} \tag{11}
$$

$$\begin{aligned} \mathbf{j} &= \begin{bmatrix} \mathbf{K\_1} & \mathbf{L\_1} & \mathbf{R\_1} \\ \mathbf{K\_2} & \mathbf{L\_2} & \mathbf{R\_2} \\ \mathbf{K\_3} & \mathbf{L\_3} & \mathbf{R\_3} \end{bmatrix} \end{aligned} \tag{12}$$

$$\mathbf{K}\_{\mathbf{l}} = \frac{\mathbf{a}\_{\mathbf{l}} \mathbf{d}\_{\mathbf{l}} - \mathbf{a}\_{\mathbf{l}} \mathbf{d}\_{\mathbf{l}}}{\mathbf{d}\_{\mathbf{l}}^2} \tag{13}$$

$$\mathbf{L}\_{\mathbf{l}} = \frac{\mathbf{\hat{b}}\_{\mathbf{l}} \mathbf{d}\_{\mathbf{l}} - \mathbf{b}\_{\mathbf{l}} \mathbf{\hat{d}}\_{\mathbf{l}}}{\mathbf{d}\_{\mathbf{l}}^2} \tag{14}$$

$$\mathbf{R}\_{\mathbf{l}} = \frac{\mathbf{c}\_{\mathbf{l}} \mathbf{d}\_{\mathbf{l}} - \mathbf{c}\_{\mathbf{l}} \mathbf{d}\_{\mathbf{l}}}{\mathbf{d}\_{\mathbf{l}}^2} \tag{15}$$

$$
\Theta\_{\mathbf{i}} = \begin{bmatrix}
\frac{\mathbf{a}\_{\mathbf{l}}}{\mathbf{d}\_{\mathbf{l}}} & \frac{\mathbf{b}\_{\mathbf{l}}}{\mathbf{d}\_{\mathbf{l}}} & \frac{\mathbf{c}\_{\mathbf{l}}}{\mathbf{d}\_{\mathbf{l}}}
\end{bmatrix}
\begin{vmatrix}
\dot{\mathbf{P}}\_{\mathbf{X}\_{\mathbf{B}}} \\
\dot{\mathbf{P}}\_{\mathbf{Y}\_{\mathbf{B}}} \\
\dot{\Phi}
\end{vmatrix}, \quad \mathbf{i} = \mathbf{1}, \mathbf{2} \text{ and } \mathbf{3}. \tag{16}
$$

$$
\boldsymbol{\omega}\_{2l-1} = \begin{bmatrix} \frac{\mathbf{a}\_l}{\mathbf{d}\_l} & \frac{\mathbf{b}\_l}{\mathbf{d}\_l} & \frac{\mathbf{c}\_l}{\mathbf{d}\_l} \end{bmatrix}, \quad \mathbf{i} = \mathbf{1}, \mathbf{2} \text{ and } \mathbf{3}. \tag{17}
$$

$$\mathbf{v}\_{2\mathbf{i}-\mathbf{1}} = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}, \quad \mathbf{i} = \mathbf{1}, \mathbf{2} \text{ and } \mathbf{3}. \tag{18}$$

$$\begin{bmatrix} \mathbf{o}\_{\mathbf{x}\_{\mathrm{l}}} + \mathbf{l}\_{2\mathrm{l}} \cos(\theta\_{\mathrm{l}} + \mathbf{a}\_{\mathrm{l}}) + \mathbf{l}\_{2\mathrm{l}-1} \cos \theta\_{\mathrm{l}}\\ \mathbf{o}\_{\mathbf{y}\_{\mathrm{l}}} + \mathbf{l}\_{2\mathrm{l}} \sin(\theta\_{\mathrm{l}} + \mathbf{a}\_{\mathrm{l}}) + \mathbf{l}\_{2\mathrm{l}-1} \sin \theta\_{\mathrm{l}} \end{bmatrix} = \begin{bmatrix} \mathbf{P}\_{\mathbf{X}\_{\mathrm{B}}} + \mathbf{b}\_{\mathrm{x}\_{\mathrm{l}}} \cos \phi - \mathbf{b}\_{\mathrm{y}\_{\mathrm{l}}} \sin \phi\\ \mathbf{P}\_{\mathbf{Y}\_{\mathrm{B}}} + \mathbf{b}\_{\mathrm{x}\_{\mathrm{l}}} \sin \phi + \mathbf{b}\_{\mathrm{y}\_{\mathrm{l}}} \cos \phi \end{bmatrix} \tag{19}$$

$$
\begin{bmatrix}
\mathbf{o\_{x\_l}} + \mathbf{l\_{2i}}\cos\mathbf{\delta\_l} + \mathbf{l\_{2i-1}}\cos\theta\_l\\\mathbf{o\_{y\_l}} + \mathbf{l\_{2i}}\sin\mathbf{\delta\_l} + \mathbf{l\_{2i-1}}\sin\theta\_l
\end{bmatrix} = \begin{bmatrix}
\mathbf{P\_{X\_B}} + \mathbf{b\_{x\_l}}\cos\phi - \mathbf{b\_{y\_l}}\sin\phi\\\mathbf{P\_{Y\_B}} + \mathbf{b\_{x\_l}}\sin\phi + \mathbf{b\_{y\_l}}\cos\phi
\end{bmatrix} \tag{20}
$$

$$
\begin{bmatrix}
\mathbf{l}\_{2\rm l}\dot{\boldsymbol{\Theta}}\_{\rm l}\cos\boldsymbol{\delta}\_{\rm l} + \mathbf{l}\_{2\rm l-1}\dot{\boldsymbol{\Theta}}\_{\rm l}\cos\boldsymbol{\Theta}\_{\rm l}
\end{bmatrix} = \begin{bmatrix}
\dot{\mathbf{P}}\_{\rm X\_{\rm B}} - \dot{\phi}\mathbf{b}\_{\rm x\_{\rm I}}\sin\boldsymbol{\phi} - \dot{\phi}\mathbf{b}\_{\rm y\_{\rm I}}\cos\boldsymbol{\phi} \\
\dot{\mathbf{P}}\_{\rm Y\_{\rm B}} + \dot{\phi}\mathbf{b}\_{\rm x\_{\rm I}}\cos\boldsymbol{\phi} - \dot{\phi}\mathbf{b}\_{\rm y\_{\rm I}}\sin\boldsymbol{\phi}
\end{bmatrix} \tag{21}
$$

$$
\begin{bmatrix}
\begin{bmatrix}
\cos\delta\_{\mathrm{i}}
\end{bmatrix}
\mathbf{l}\_{2\mathrm{i}}\dot{\delta}\_{\mathrm{i}} + \begin{bmatrix}
\mathrm{l}\_{2\mathrm{i}-1}\cos\theta\_{\mathrm{i}}
\end{bmatrix}\dot{\theta}\_{\mathrm{i}} = \begin{bmatrix}
1 & 0 & -\mathrm{b}\_{\mathrm{X}\_{\mathrm{i}}}\sin\phi - \mathrm{b}\_{\mathrm{Y}\_{\mathrm{i}}}\cos\phi \\
0 & 1 & \mathrm{b}\_{\mathrm{X}\_{\mathrm{i}}}\cos\phi - \mathrm{b}\_{\mathrm{Y}\_{\mathrm{i}}}\sin\phi
\end{bmatrix}
\begin{bmatrix}
\mathrm{p}\_{\mathrm{X}\_{\mathrm{B}}} \\
\dot{\mathrm{p}}\_{\mathrm{Y}\_{\mathrm{B}}} \\
\dot{\phi}
\end{bmatrix} \tag{22}
$$

$$
\begin{bmatrix}
\end{bmatrix}\mathbf{l}\_{2l}\dot{\delta}\_1 = \begin{pmatrix}
\begin{bmatrix}
1 & 0 & -\mathbf{b}\_{\mathbf{x}\_l}\sin\phi - \mathbf{b}\_{\mathbf{y}\_l}\cos\phi\\0 & 1 & \mathbf{b}\_{\mathbf{x}\_l}\cos\phi - \mathbf{b}\_{\mathbf{y}\_l}\sin\phi
\end{bmatrix}
\end{bmatrix}
\begin{bmatrix}
\mathbf{a}\_l & \mathbf{b}\_l & \mathbf{c}\_l\\\mathbf{d}\_l & \mathbf{d}\_l & \mathbf{d}\_l
\end{bmatrix}
\end{bmatrix}
\begin{bmatrix}
\mathbf{p}\_{\mathbf{X}\_\mathbf{B}}\\\dot{\mathbf{p}}\_{\mathbf{Y}\_\mathbf{B}}\\\dot{\phi}
\end{bmatrix} \tag{23}
$$

$$\dot{\mathbf{S}}\_{1} = \begin{bmatrix} -\frac{\sin \mathbf{\mathcal{S}}\_{1}}{\mathbf{l}\_{2\mathrm{l}}} & \frac{\cos \mathbf{\mathcal{S}}\_{1}}{\mathbf{l}\_{2\mathrm{l}}} \end{bmatrix} \begin{bmatrix} \begin{bmatrix} 1 & 0 & -\mathbf{b}\_{\mathrm{X}\_{1}} \sin \phi - \mathbf{b}\_{\mathrm{Y}\_{1}} \cos \phi \\ 0 & 1 & \mathbf{b}\_{\mathrm{X}\_{1}} \cos \phi - \mathbf{b}\_{\mathrm{Y}\_{1}} \sin \phi \end{bmatrix} - \begin{bmatrix} -\mathbf{l}\_{2\mathrm{l}-1} \sin \Theta\_{\mathrm{l}} \\ \mathbf{l}\_{2\mathrm{l}-1} \cos \Theta\_{\mathrm{l}} \end{bmatrix} \begin{bmatrix} \begin{matrix} \mathbf{a}\_{\mathrm{l}} & \frac{\mathbf{b}\_{\mathrm{l}}}{\mathbf{d}\_{\mathrm{l}}} & \frac{\mathbf{c}\_{\mathrm{l}}}{\mathbf{d}\_{\mathrm{l}}} \end{bmatrix} \end{bmatrix} \begin{bmatrix} \mathbf{\dot{r}}\_{\mathrm{X}\_{\mathrm{B}}} \\ \mathbf{\dot{r}}\_{\mathrm{B}} \\ \dot{\Phi} \end{bmatrix} \tag{24}$$

$$\boldsymbol{\omega}\_{2l} = \begin{bmatrix} -\frac{\sin \delta\_l}{\mathbf{l}\_{2l}} & \frac{\cos \delta\_l}{\mathbf{l}\_{2l}} \end{bmatrix} \begin{pmatrix} \begin{bmatrix} 1 & 0 & -\mathbf{b}\_{\mathbf{x}\_l} \sin \phi - \mathbf{b}\_{\mathbf{y}\_l} \cos \phi\\ \mathbf{0} & \mathbf{1} & \mathbf{b}\_{\mathbf{x}\_l} \cos \phi - \mathbf{b}\_{\mathbf{y}\_l} \sin \phi \end{bmatrix} - \begin{bmatrix} -\mathbf{l}\_{2l-1} \sin \theta\_l\\ \mathbf{l}\_{2l-1} \cos \theta\_l \end{bmatrix} \begin{bmatrix} \frac{\mathbf{a}\_l}{\mathbf{d}\_l} & \frac{\mathbf{b}\_l}{\mathbf{d}\_l} & \frac{\mathbf{c}\_l}{\mathbf{d}\_l} \end{bmatrix} \tag{25}$$

$$\begin{bmatrix} -\mathbf{l}\_{2i} \{ \ddot{\mathsf{S}}\_{l} \sin \mathsf{\mathsf{S}}\_{l} + \dot{\mathsf{S}}\_{l}^{2} \cos \mathsf{\mathsf{S}}\_{l} \} - \mathbf{l}\_{2i-1} \{ \ddot{\mathsf{S}}\_{l} \sin \mathsf{\mathsf{H}}\_{l} + \dot{\mathsf{\mathsf{H}}}\_{l}^{2} \cos \mathsf{\mathsf{H}}\_{l} \} \\\ \mathbf{l}\_{2i} \{ \ddot{\mathsf{S}}\_{l} \cos \mathsf{\mathsf{S}}\_{l} - \dot{\mathsf{S}}\_{l}^{2} \sin \mathsf{\mathsf{S}}\_{l} \} + \mathbf{l}\_{2i-1} \{ \ddot{\mathsf{S}}\_{l} \cos \mathsf{\mathsf{S}}\_{l} - \dot{\mathsf{\mathsf{H}}}\_{l}^{2} \sin \mathsf{\mathsf{S}}\_{l} \} \end{bmatrix}$$
 
$$\begin{aligned} = \begin{bmatrix} \ddot{\mathsf{P}}\_{\mathbf{X}\_{\mathsf{B}}} - \left( \ddot{\mathsf{\mathsf{S}}} \mathbf{b}\_{\mathbf{x}\_{\mathsf{i}}} \sin \mathsf{\mathsf{s}} + \dot{\mathsf{\mathsf{G}}}^{2} \mathbf{b}\_{\mathbf{x}\_{\mathsf{i}}} \cos \mathsf{\mathsf{s}} \right) - \left( \ddot{\mathsf{\mathsf{S}}} \mathbf{b}\_{\mathbf{y}\_{\mathsf{i}}} \cos \mathsf{\mathsf{s}} - \dot{\mathsf{\mathsf{S}}}^{2} \mathbf{b}\_{\mathbf{y}\_{\mathsf{i}}} \sin \mathsf{\mathsf{s}} \right) \end{bmatrix} \end{aligned} \tag{26}$$

$$\begin{bmatrix} -\sin \delta\_{\mathbf{l}}\\ \cos \delta\_{\mathbf{l}} \end{bmatrix} \mathbf{l}\_{2\mathbf{l}} \ddot{\delta}\_{\mathbf{l}} = \begin{bmatrix} \mathbf{s}\_{11} \\ \mathbf{s}\_{12} \end{bmatrix} \tag{27}$$

$$\mathbf{s}\_{\mathbf{i}1} = \mathfrak{P}\_{\mathbf{k}\_{\mathbf{B}}} - \left\{ \bar{\phi} \mathbf{b}\_{\mathbf{x}\_{\mathbf{i}}} \sin \phi + \dot{\phi}^2 \mathbf{b}\_{\mathbf{x}\_{\mathbf{i}}} \cos \phi \right\} - \left\{ \bar{\phi} \mathbf{b}\_{\mathbf{y}\_1} \cos \phi - \dot{\phi}^2 \mathbf{b}\_{\mathbf{y}\_1} \sin \phi \right\} + \mathbf{l}\_{21} \dot{\mathbf{\delta}}\_1^2 \cos \delta\_1$$

$$+ \mathbf{l}\_{21-1} \left\{ \theta\_1 \sin \theta\_1 + \theta\_1^2 \cos \theta\_1 \right\}$$
 $\mathbf{s}\_{\mathbf{i}2} = \mathfrak{P}\_{\mathbf{i}} + \left\{ \bar{\phi} \mathbf{b}\_{\mathbf{x}\_{\mathbf{i}}} \cos \phi - \dot{\phi}^2 \mathbf{b}\_{\mathbf{x}\_{\mathbf{i}}} \sin \phi \right\} - \left\{ \bar{\phi} \mathbf{b}\_{\mathbf{y}\_1} \sin \phi + \dot{\phi}^2 \mathbf{b}\_{\mathbf{y}\_1} \cos \phi \right\} + \mathbf{l}\_{21} \dot{\mathbf{\delta}}\_1^2 \sin \delta\_1$ 
$$- \mathbf{l}\_{21-1} \left\{ \bar{\theta}\_1 \cos \theta\_1 - \dot{\theta}\_1^2 \sin \theta\_1 \right\}$$

$$\ddot{\boldsymbol{\delta}}\_{1} = \begin{bmatrix} -\frac{\sin \mathfrak{d}\_{1}}{\mathfrak{l}\_{21}} & \frac{\cos \mathfrak{d}\_{1}}{\mathfrak{l}\_{21}} \end{bmatrix} \begin{bmatrix} \mathbf{s}\_{11} \\ \mathbf{s}\_{12} \end{bmatrix} \tag{28}$$

$$\begin{aligned} \mathbf{^0C\_i^1} &= \begin{bmatrix} 1 & 0 & 0 & \mathbf{o\_{x\_i}} \\ 0 & 1 & 0 & \mathbf{o\_{y\_i}} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos \theta\_1 & -\sin \theta\_1 & 0 & 0 \\ \sin \theta\_1 & \cos \theta\_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & \mathbf{l\_{2l-1}} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} \cos \theta\_1 & -\sin \theta\_1 & 0 & \mathbf{o\_{x\_i}} + \mathbf{l\_{2l-1}} \cos \theta\_1 \\ \sin \theta\_1 & \cos \theta\_1 & 0 & \mathbf{o\_{y\_i}} + \mathbf{l\_{2l-1}} \sin \theta\_1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned} \tag{29}$$

$$\mathbf{I}\_{\mathbf{C}\_{\mathbf{I}}}^{\mathbf{O}\_{\mathbf{I}}}\mathbf{T}\_{\mathbf{P}(\mathbf{x},\mathbf{y})}^{\mathbf{1}} = \begin{vmatrix} \mathbf{o}\_{\mathbf{x}\_{\mathbf{i}}} + \mathbf{l}\_{2\mathbf{i}-1}\cos\theta\_{\mathbf{i}}\\ \mathbf{o}\_{\mathbf{y}\_{\mathbf{i}}} + \mathbf{l}\_{2\mathbf{i}-1}\sin\theta\_{\mathbf{i}} \end{vmatrix} \tag{30}$$

$$\boldsymbol{\upsilon}\_{\mathbf{C}\_{l}} = \frac{\mathbf{d}}{\mathrm{d}t} \begin{pmatrix} \mathbf{0}\_{\mathrm{l}} \mathbf{T}\_{\mathrm{P}\left(\mathbf{x},\mathbf{y}\right)}^{\mathrm{d}} \end{pmatrix} = \begin{bmatrix} -\mathrm{l}\_{2\mathrm{l}-1} \sin \Theta\_{\mathrm{l}}\\ \mathrm{l}\_{2\mathrm{l}-1} \cos \Theta\_{\mathrm{l}} \end{bmatrix} \dot{\Theta}\_{\mathrm{l}} \tag{31}$$

$$\boldsymbol{\nu}\_{\boldsymbol{\mathcal{C}}\_{l}} = \begin{bmatrix} -\mathbf{l}\_{2\boldsymbol{1}-\mathbf{1}}\sin\Theta\_{\boldsymbol{\mathcal{I}}\_{l}} \\ \mathbf{l}\_{2\boldsymbol{1}-\mathbf{1}}\cos\Theta\_{\boldsymbol{\mathcal{I}}\_{l}} \end{bmatrix} \begin{bmatrix} \mathbf{a}\_{\boldsymbol{l}} & \mathbf{b}\_{\boldsymbol{l}} & \mathbf{c}\_{\boldsymbol{l}} \\ \mathbf{d}\_{\boldsymbol{l}} & \mathbf{d}\_{\boldsymbol{l}} & \mathbf{d}\_{\boldsymbol{l}} \end{bmatrix} \begin{bmatrix} \mathbf{\dot{P}\_{\mathcal{X}\_{\mathcal{B}}}} \\ \mathbf{\dot{P}\_{\mathcal{Y}\_{\mathcal{B}}}} \\ \boldsymbol{\Phi} \end{bmatrix}$$

$$= \frac{\mathbf{\dot{r}}\_{\boldsymbol{z}\boldsymbol{1}-\mathbf{1}}}{\mathbf{d}\_{\boldsymbol{l}}} \begin{bmatrix} -\mathbf{a}\_{\boldsymbol{l}}\sin\Theta\_{\boldsymbol{l}} & -\mathbf{b}\_{\boldsymbol{l}}\sin\Theta\_{\boldsymbol{l}} & -\mathbf{c}\_{\boldsymbol{l}}\sin\Theta\_{\boldsymbol{l}} \\ \mathbf{a}\_{\boldsymbol{l}}\cos\Theta\_{\boldsymbol{l}} & \mathbf{b}\_{\boldsymbol{l}}\cos\Theta\_{\boldsymbol{l}} & \mathbf{c}\_{\boldsymbol{l}}\cos\Theta\_{\boldsymbol{l}} \end{bmatrix} \begin{bmatrix} \dot{\mathbf{P}\_{\mathcal{X}\_{\mathcal{B}}}} \\ \dot{\mathbf{P}\_{\mathcal{X}\_{\mathcal{B}}}} \\ \dot{\mathbf{P}\_{\mathcal{Y}\_{\mathcal{B}}}} \\ \dot{\Phi} \end{bmatrix} \tag{32}$$

$$\mathbf{u}\_{2\mathbf{l}} = \frac{\mathbf{l}\_{2\mathbf{l}-1}}{\mathbf{d}\_{\mathbf{l}}} \begin{bmatrix} -\mathbf{a}\_{\mathbf{l}}\sin\theta\_{\mathbf{l}} & -\mathbf{b}\_{\mathbf{l}}\sin\theta\_{\mathbf{l}} & -\mathbf{c}\_{\mathbf{l}}\sin\theta\_{\mathbf{l}}\\ \mathbf{a}\_{\mathbf{l}}\cos\theta\_{\mathbf{l}} & \mathbf{b}\_{\mathbf{l}}\cos\theta\_{\mathbf{l}} & \mathbf{c}\_{\mathbf{l}}\cos\theta\_{\mathbf{l}} \end{bmatrix} \tag{33}$$

$$a\_{mp} = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \Phi\_{\mathbf{X}\_{\mathbf{B}}} \\ \Phi\_{\mathbf{Y}\_{\mathbf{B}}} \\ \dot{\Phi} \end{bmatrix} \tag{34}$$

$$
\omega\_{mp} = \begin{bmatrix} 0 & 0 & 1 \end{bmatrix} \tag{35}$$

$$l\_{\nu\_{mp}} = \begin{bmatrix} 1 & 0 & -\mathbf{b}\_{\mathbf{x}\_3}\sin\phi - \mathbf{b}\_{\mathbf{y}\_3}\cos\phi\\ 0 & 1 & \mathbf{b}\_{\mathbf{x}\_3}\cos\phi - \mathbf{b}\_{\mathbf{y}\_3}\sin\phi \end{bmatrix} \begin{vmatrix} \dot{\mathbf{p}}\_{\mathbf{x}\_{\mathbf{B}}}\\ \dot{\mathbf{p}}\_{\mathbf{Y}\_{\mathbf{B}}}\\ \dot{\phi} \end{vmatrix} \tag{36}$$

$$\boldsymbol{\sigma}\_{mp} = \begin{bmatrix} 1 & 0 & -\mathbf{b}\_{\mathbf{x}\_3}\sin\phi - \mathbf{b}\_{\mathbf{y}\_3}\cos\phi\\ 0 & 1 & \mathbf{b}\_{\mathbf{x}\_3}\cos\phi - \mathbf{b}\_{\mathbf{y}\_3}\sin\phi \end{bmatrix} \tag{37}$$

$$\mathbf{I}\_{\mathbf{C}\_{\parallel}}^{\mathbf{O}\_{\parallel}} \mathbf{T}\_{\mathbf{P}\_{\mathbf{C}\_{2l-1}}}^{\mathbf{1}} = \begin{bmatrix} \mathbf{o}\_{\mathbf{x}\_{l}} + \mathbf{r}\_{2l-1} \cos \theta\_{l} \\ \mathbf{o}\_{\mathbf{y}\_{l}} + \mathbf{r}\_{2l-1} \sin \theta\_{l} \end{bmatrix} \tag{38}$$

$$\mathbf{a}\_{\mathbf{c}\_{2l-1}} = \frac{\mathbf{d}}{\mathrm{d}t} \begin{pmatrix} \mathbf{d} \\ \mathrm{d}t \begin{bmatrix} \mathbf{o}\_{\mathbf{x}\_{l}} + \mathbf{r}\_{2l-1} \cos \theta\_{l} \\ \mathbf{o}\_{\mathbf{y}\_{l}} + \mathbf{r}\_{2l-1} \sin \theta\_{l} \end{bmatrix} \end{pmatrix} = \mathbf{r}\_{2l-1} \begin{bmatrix} -\ddot{\theta}\_{l} \sin \theta\_{l} - \dot{\theta}\_{l}^{2} \cos \theta\_{l} \\ \ddot{\theta}\_{l} \cos \theta\_{l} - \dot{\theta}\_{l}^{2} \sin \theta\_{l} \end{bmatrix} \tag{39}$$

$$\mathbf{F}\_{2l-1} = -\mathbf{m}\_{2l-1} (\mathbf{a}\_{c\_{2l-1}} - \mathbf{g})$$

$$= \mathbf{m}\_{2l-1} \mathbf{r}\_{2l-1} \begin{bmatrix} \ddot{\theta}\_l \sin \theta\_l + \dot{\theta}\_l^2 \cos \theta\_l \\ -\ddot{\theta}\_l \cos \theta\_l + \dot{\theta}\_l^2 \sin \theta\_l \end{bmatrix} \tag{40}$$

$$\mathbf{M}\_{2\mathbf{i}-\mathbf{1}} = -\left[\ddot{\boldsymbol{\theta}}\_{\mathrm{l}}\mathbf{I}\_{2\mathbf{i}-\mathbf{1}} + \mathbf{m}\_{2\mathbf{i}-\mathbf{1}} \left(\frac{\mathbf{d}}{\mathbf{d}\boldsymbol{\theta}\_{\mathrm{l}}} \prescript{\mathbf{O}\_{\mathrm{l}}}{\mathbf{C}\_{\mathbf{i}}} \prescript{\mathbf{O}\_{\mathrm{l}}}{\mathbf{T}^{\mathbf{c}}} \prescript{\mathbf{T}}{\mathbf{c}}\_{2\mathbf{i}-\mathbf{1}}\right)^{\mathrm{T}} \mathbf{a}\_{\mathbf{B}\_{\mathrm{l}}}\right]$$

$$= \ddot{\boldsymbol{\theta}}\_{\mathrm{l}} \mathbf{I}\_{2\mathbf{i}-\mathbf{1}} \tag{41}$$

$$\mathbf{I}\_{\mathbf{M}\_{\parallel}}^{\mathbf{O}\_{\parallel}}\mathbf{T}\_{\mathbf{P}\_{2\parallel}}^{\mathbf{1}} = \begin{bmatrix} \mathbf{o}\_{\mathbf{x}\_{\parallel}} + \mathbf{r}\_{2\mathbf{i}}\cos\mathbf{\delta}\_{\mathbf{i}} + \mathbf{l}\_{2\mathbf{i}-\mathbf{1}}\cos\theta\_{\mathbf{i}}\\\mathbf{o}\_{\mathbf{y}\_{\parallel}} + \mathbf{r}\_{2\mathbf{i}}\sin\mathbf{\delta}\_{\mathbf{i}} + \mathbf{l}\_{2\mathbf{i}-\mathbf{1}}\sin\theta\_{\mathbf{i}} \end{bmatrix} \tag{42}$$

$$\mathbf{a}\_{\mathbf{c}\_{2l}} = \frac{\mathbf{d}}{\mathrm{d}t} \left( \frac{\mathbf{d}}{\mathrm{d}t} \begin{bmatrix} \mathbf{o}\_{\mathbf{x}\_{l}} + \mathbf{r}\_{2l}\cos\mathfrak{S}\_{l} + \mathbf{l}\_{2l-1}\cos\mathfrak{S}\_{l} \\ \mathbf{o}\_{\mathbf{y}\_{l}} + \mathbf{r}\_{2l}\sin\mathfrak{S}\_{l} + \mathbf{l}\_{2l-1}\sin\mathfrak{S}\_{l} \end{bmatrix} \right)$$

$$\mathbf{f} = \begin{bmatrix} -\mathbf{r}\_{2l} \left( \ddot{\mathfrak{S}}\_{l}\sin\mathfrak{S}\_{l} + \dot{\mathfrak{S}}\_{l}^{2}\cos\mathfrak{S}\_{l} \right) - \mathbf{l}\_{2l-1} \left( \ddot{\mathfrak{S}}\_{l}\sin\mathfrak{S}\_{l} + \dot{\mathfrak{S}}\_{l}^{2}\cos\mathfrak{S}\_{l} \right) \\ \mathbf{r}\_{2l} \left( \ddot{\mathfrak{S}}\_{l}\cos\mathfrak{S}\_{l} - \dot{\mathfrak{S}}\_{l}^{2}\sin\mathfrak{S}\_{l} \right) + \mathbf{l}\_{2l-1} \left( \ddot{\mathfrak{S}}\_{l}\cos\mathfrak{S}\_{l} - \dot{\mathfrak{S}}\_{l}^{2}\sin\mathfrak{S}\_{l} \right) \end{bmatrix} \tag{43}$$

$$\mathbf{F\_{2l}} = -\mathbf{m\_{2l}} \{ \mathbf{a\_{c\_{2l}}} - \mathbf{g} \}$$

$$\mathbf{f\_{2l}} = -\mathbf{m\_{2l}} \begin{bmatrix} -\mathbf{r\_{2l}} \{ \ddot{\mathbf{\tilde{s}}\_l} \sin \mathbf{\tilde{s}\_l} + \dot{\mathbf{\tilde{s}}\_l^2} \cos \mathbf{\tilde{s}\_l} \} - \mathbf{l\_{2l-1}} \{ \ddot{\mathbf{\tilde{s}}\_l} \sin \mathbf{\theta\_l} + \dot{\mathbf{\theta}\_l^2} \cos \mathbf{\theta\_l} \} \\\ \mathbf{r\_{2l}} \{ \ddot{\mathbf{\tilde{s}}\_l} \cos \mathbf{\tilde{s}\_l} - \dot{\mathbf{\tilde{s}}\_l^2} \sin \mathbf{\tilde{s}\_l} \} + \mathbf{l\_{2l-1}} \{ \ddot{\mathbf{\tilde{s}}\_l} \cos \mathbf{\theta\_l} - \dot{\mathbf{\theta}\_l^2} \sin \mathbf{\theta\_l} \} \end{bmatrix} \tag{44}$$

$$\mathbf{M}\_{2\ell} = -\left[\ddot{\mathbf{\tilde{s}}}\_{l}\mathbf{l}\_{2\mathbf{l}} + \mathbf{m}\_{2\mathbf{l}}\left(\frac{\mathbf{d}}{\mathbf{d}\mathbf{s}\_{\mathbf{l}}}\frac{\mathbf{d}\_{\mathbf{l}}}{\mathbf{d}\_{\mathbf{l}}}\mathbf{T}\_{\mathbf{p}\_{\mathbf{c}\_{2\mathbf{l}}}}^{\mathbf{1}}\right)^{\mathrm{T}}\mathbf{a}\_{\mathbf{c}\_{\mathbf{l}}}\right]$$

$$\mathbf{J} = -\left(\ddot{\mathbf{\tilde{s}}}\_{l}\mathbf{l}\_{2\mathbf{l}} + \mathbf{m}\_{2\mathbf{l}}\mathbf{r}\_{2\mathbf{l}}\mathbf{l}\_{2\mathbf{l}-\mathbf{1}}\left[\sin\mathbf{\tilde{s}}\_{l}\{\ddot{\mathbf{\tilde{s}}}\_{l}\sin\theta\_{\mathbf{l}} + \dot{\theta}\_{l}^{2}\cos\theta\_{\mathbf{l}}\} \quad \cos\mathbf{\tilde{s}}\_{l}\{\ddot{\mathbf{\tilde{s}}}\_{l}\cos\theta\_{\mathbf{l}} - \dot{\theta}\_{l}^{2}\sin\theta\_{\mathbf{l}}\}\right]\right) \tag{45}$$

$$\frac{d}{d\delta\_{\mathbf{l}}} \frac{\mathbf{o}\_{\mathbf{l}}}{\mathbf{M}\_{\mathbf{l}}} \mathbf{T}\_{\mathbf{P} \mathbf{c}\_{2\mathbf{l}}}^{2} = \frac{d}{d\delta\_{\mathbf{l}}} \begin{bmatrix} \mathbf{o}\_{\mathbf{x}\_{\mathbf{l}}} + \mathbf{r}\_{2\mathbf{l}} \cos \delta\_{\mathbf{l}} + \mathbf{l}\_{2\mathbf{l}-1} \cos \theta\_{\mathbf{l}}\\ \mathbf{o}\_{\mathbf{Y}\_{\mathbf{l}}} + \mathbf{r}\_{2\mathbf{l}} \sin \delta\_{\mathbf{l}} + \mathbf{l}\_{2\mathbf{l}-1} \sin \theta\_{\mathbf{l}} \end{bmatrix} = \mathbf{r}\_{2\mathbf{l}} \begin{bmatrix} -\sin \delta\_{\mathbf{l}}\\ \cos \delta\_{\mathbf{l}} \end{bmatrix} \tag{46}$$

$$\mathbf{a}\_{\mathbf{C}\_{\mathrm{l}}} = \frac{\mathbf{d}}{\mathrm{d}t} \left( \frac{\mathbf{d}}{\mathrm{d}t} \begin{bmatrix} \mathbf{o}\_{\mathrm{x}\_{\mathrm{l}}} + \mathbf{l}\_{2\mathrm{l}-1} \cos \theta\_{\mathrm{l}}\\ \mathbf{o}\_{\mathrm{Y}\_{\mathrm{l}}} + \mathbf{l}\_{2\mathrm{l}-1} \sin \theta\_{\mathrm{l}} \end{bmatrix} \right) = -\mathbf{l}\_{2\mathrm{l}-1} \begin{bmatrix} \theta\_{\mathrm{l}} \sin \theta\_{\mathrm{l}} + \theta\_{\mathrm{l}}^{2} \cos \theta\_{\mathrm{l}}\\ -\ddot{\theta}\_{\mathrm{l}} \cos \theta\_{\mathrm{l}} + \dot{\theta}\_{\mathrm{l}}^{2} \sin \theta\_{\mathrm{l}} \end{bmatrix} \tag{47}$$

$$\mathbf{L}\_{\mathbf{M}\_{\parallel}}^{\mathbf{O}\_{\parallel}}\mathbf{T}^{2} = \begin{bmatrix} \mathbf{P}\_{\mathbf{X}\_{\mathbf{B}}} + \mathbf{b}\_{\mathbf{x}\_{\parallel}}\cos\phi - \mathbf{b}\_{\mathbf{y}\_{\parallel}}\sin\phi\\ \mathbf{P}\_{\mathbf{Y}\_{\mathbf{B}}} + \mathbf{b}\_{\mathbf{x}\_{\parallel}}\sin\phi + \mathbf{b}\_{\mathbf{y}\_{\parallel}}\cos\phi \end{bmatrix} \tag{48}$$

$$\mathbf{a}\_{\rm cmp} = \frac{\mathbf{d}}{\mathrm{d}t} \left( \frac{\mathbf{d}}{\mathrm{d}t} \begin{bmatrix} \mathbf{P}\_{\mathbf{X}\_{\rm B}} + \mathbf{b}\_{\mathbf{x}\_{\rm l}} \cos \phi - \mathbf{b}\_{\mathbf{y}\_{\rm l}} \sin \phi \\ \mathbf{P}\_{\mathbf{Y}\_{\rm B}} + \mathbf{b}\_{\mathbf{x}\_{\rm l}} \sin \phi + \mathbf{b}\_{\mathbf{y}\_{\rm l}} \cos \phi \end{bmatrix} \right)$$

$$= \begin{bmatrix} \mathbf{\dot{P}}\_{\rm X\_{\rm B}} - \dot{\phi} \left( \mathbf{b}\_{\mathbf{x}\_{\rm X}} \sin \phi + \mathbf{b}\_{\mathbf{y}\_{\rm X}} \cos \phi \right) - \dot{\phi}^2 \left( \mathbf{b}\_{\mathbf{x}\_{\rm X}} \cos \phi - \mathbf{b}\_{\mathbf{y}\_{\rm X}} \sin \phi \right) \\ \mathbf{\ddot{P}}\_{\rm Y\_{\rm B}} + \dot{\phi} \left( \mathbf{b}\_{\mathbf{x}\_{\rm X}} \cos \phi - \mathbf{b}\_{\mathbf{y}\_{\rm X}} \sin \phi \right) - \dot{\phi}^2 \left( \mathbf{b}\_{\mathbf{x}\_{\rm X}} \sin \phi + \mathbf{b}\_{\mathbf{y}\_{\rm X}} \cos \phi \right) \end{bmatrix} \tag{49}$$

$$\mathbf{F\_{mp}} = -\mathbf{m\_{mp}} \left( \mathbf{a\_{c\_{mp}}} - \mathbf{g} \right)$$

$$= -\mathbf{m\_{mp}} \begin{bmatrix} \ddot{\mathbf{p}}\_{\mathbf{x}\_{B}} - \ddot{\phi} \left( \mathbf{b\_{x\_{3}}} \sin \phi + \mathbf{b\_{y\_{3}}} \cos \phi \right) - \dot{\phi}^{2} \left( \mathbf{b\_{x\_{3}}} \cos \phi - \mathbf{b\_{y\_{3}}} \sin \phi \right) \\ \ddot{\mathbf{p}}\_{\mathbf{y}\_{B}} + \ddot{\phi} \left( \mathbf{b\_{x\_{3}}} \cos \phi - \mathbf{b\_{y\_{3}}} \sin \phi \right) - \dot{\phi}^{2} \left( \mathbf{b\_{x\_{3}}} \sin \phi + \mathbf{b\_{y\_{3}}} \cos \phi \right) \end{bmatrix} \tag{50}$$

$$\mathbf{M}\_{\rm mp} = -\left[\ddot{\boldsymbol{\phi}} \mathbf{I}\_{\rm mp} + \mathbf{m}\_{\rm mp} \left(\frac{\mathbf{d}}{\rm d\phi} \frac{\mathbf{o}\_{\rm I}}{\rm M\_3} \mathbf{T}\_{\rm P(x,y)}^2\right)^{\rm T} \mathbf{a}\_{\rm c\_{mp}}\right]$$

$$= -\left(\ddot{\boldsymbol{\phi}} \mathbf{I}\_{\rm mp} + \mathbf{m}\_{\rm mp} \left[\ddot{\mathbf{P}}\_{\rm X\_{\rm B}} \left(-\mathbf{b}\_{\rm x\_3} \sin\phi - \mathbf{b}\_{\rm Y\_3} \cos\phi\right) + \ddot{\mathbf{P}}\_{\rm Y\_B} \left(\mathbf{b}\_{\rm x\_3} \cos\phi - \mathbf{b}\_{\rm Y\_3} \sin\phi\right)\right]\right) \tag{51}$$

$$\frac{d}{d\phi}\frac{\mathbf{o}\_{\rm I}}{\mathbf{M}\_{3}}\mathbf{T}\_{\rm P(xy)}^{2} = \frac{d}{d\phi} \begin{bmatrix} \mathbf{P}\_{\rm X\_{3}} + \mathbf{b}\_{\rm x\_{3}}\cos\phi - \mathbf{b}\_{\rm Y\_{3}}\sin\phi\\ \mathbf{P}\_{\rm Y\_{3}} + \mathbf{b}\_{\rm x\_{3}}\sin\phi + \mathbf{b}\_{\rm Y\_{3}}\cos\phi \end{bmatrix} = \begin{bmatrix} -\mathbf{b}\_{\rm x\_{3}}\sin\phi - \mathbf{b}\_{\rm Y\_{3}}\cos\phi\\ \mathbf{b}\_{\rm x\_{3}}\cos\phi - \mathbf{b}\_{\rm y\_{3}}\sin\phi \end{bmatrix} \tag{52}$$

$$\mathbf{a}\_{\mathbf{c}\_{\rm mp}} = \begin{bmatrix} \ddot{\mathbf{p}}\_{\mathbf{X}\_{\rm B}} \\ \ddot{\mathbf{p}}\_{\mathbf{Y}\_{\rm B}} \end{bmatrix} \tag{53}$$

$$J^T \mathfrak{r} + F = 0 \tag{54}$$

$$\mathbf{F} = \boldsymbol{\Sigma}\_{l=1}^{3} \left( \begin{bmatrix} \boldsymbol{\upsilon}\_{2l-1}^{T} & \boldsymbol{\alpha}\_{2l-1}^{T} \end{bmatrix} \begin{bmatrix} \boldsymbol{F}\_{2l-1} \\ \boldsymbol{M}\_{2l-1} \end{bmatrix} \right) + \boldsymbol{\Sigma}\_{l=1}^{3} \left( \begin{bmatrix} \boldsymbol{\upsilon}\_{2l}^{T} & \boldsymbol{\alpha}\_{2l}^{T} \end{bmatrix} \begin{bmatrix} \boldsymbol{F}\_{2l} \\ \boldsymbol{M}\_{2l} \end{bmatrix} \right) + \begin{bmatrix} \boldsymbol{\upsilon}\_{mp}^{T} & \boldsymbol{\alpha}\_{mp}^{T} \end{bmatrix} \begin{bmatrix} \boldsymbol{F}\_{mp} \\ \boldsymbol{M}\_{mp} \end{bmatrix} \tag{55}$$

$$
\tau = -(J^T)^{-1}F\tag{56}
$$

$$\mathbf{P\_{X\_B}} = \mathbf{P\_{X\_C+}} \mathbf{a\_m} \cos(\omega\_c \pi \mathbf{t}) \quad 0 \le \mathbf{t} \le 5 \text{ seconds} \tag{57}$$

$$\mathbf{R}\_{\mathbf{Y}\_{\text{B}}} = \mathbf{R}\_{\mathbf{Y}\_{\text{B}} + \mathbf{a}\_{\text{m}} \sin \left(\omega\_{\text{s}} \pi \mathbf{t}\right) \quad \mathbf{0} \le \mathbf{t} \le \mathbf{5} \text{ seconds} \tag{58}$$


Inverse Dynamics of RRR Fully Planar Parallel Manipulator Using DH Method 17

In this chapter, the inverse dynamics problem of 3-DOF RRR FPPM is derived using virtual work principle. Firstly, the inverse kinematics model and Jacobian matrix of 3-DOF RRR FPPM are determined using DH method. Secondly, the partial linear velocity and partial angular velocity matrices are computed. Pivotal points are selected in order to determine the partial linear velocity matrices. Thirdly, the inertial force and moment of each moving part are obtained. Consequently, the inverse dynamic equations of 3-DOF RRR FPPM in explicit form are derived. A butterfly shape Cartesian trajectory is used as a desired end-effector's

<sup>0</sup> 0.5 <sup>1</sup> 1.5 <sup>2</sup> 2.5 <sup>3</sup> 3.5 <sup>4</sup> 4.5 <sup>5</sup> -1500

Time (sec)

Torque1 Torque2 Torque3

Denavit, J. & Hartenberg, R. S., (1955). A kinematic notation for lower-pair mechanisms based on matrices. Journal of Applied Mechanics, Vol., 22, 1955, pp. 215–221 Hubbard, T.; Kujath, M. R. & Fetting, H. (2001). MicroJoints, Actuators, Grippers, and

Kang, B.; Chu, J. & Mills, J. K. (2001). Design of high speed planar parallel manipulator and

Tsai, L. W. (1999). Robot analysis: The mechanics of serial and parallel manipulators, A

Uchiyama, M. (1994). Structures and characteristics of parallel manipulators, Advanced

Wu, J.; Wang J.; You, Z. (2011). A comparison study on the dynamics of planar 3-DOF 4-

Conference on Robotics and Automation, pp. 2723-2728, South Korea Kang, B. & Mills, J. K. (2001). Dynamic modeling and vibration control of high speed planar

Intelligent Robots and Systems, pp. 1287-1292, Hawaii

Merlet, J. P. (2000) Parallel robots, Kluwer Academic Publishers

Wiley-Interscience Publication

robotics, Vol. 8, no. 6. pp. 545-557

manufacturing, Vol.27, pp. 150–156

Mechanisms, CCToMM Symposium on Mechanisms, Machines and Mechatronics,

multiple simultaneous specification control, Proceedings of IEEE International

parallel manipulator, In Proceedings of IEEE/RJS International Conference on

RRR, 3-RRR and 2-RRR parallel manipulators, Robotics and computer-integrated

Fig. 5. The driving torques (߬ଵ ߬ଶ ߬ଷ) of the 3-DOF RRR FPPM

trajectory to demonstrate the active joints torques.



Torques

0

500

1000

Montreal, Canada

**4. Conclusion** 

**5. References** 

Fig. 4. a) Position, b) velocity and c) acceleration of the moving platform

Fig. 5. The driving torques (߬ଵ ߬ଶ ߬ଷ) of the 3-DOF RRR FPPM

### **4. Conclusion**

16 Serial and Parallel Robot Manipulators – Kinematics, Dynamics, Control and Optimization

(a)

VPBX VPBY

14,5 14.6 14.7 14.8 14.9 <sup>15</sup> 15.1 15.2 15.3 15.4 15.5 14.5

PXB

14.6 14.7 14.8 14.9 15 15.1 15.2 15.3 15.4 15.5

> -1 -0.5 0 0.5 1 1.5

Acceleration

APBX APBY

Velocity

PYB

(b)

<sup>0</sup> 0.5 <sup>1</sup> 1.5 <sup>2</sup> 2.5 <sup>3</sup> 3.5 <sup>4</sup> 4.5 <sup>5</sup> -1.5

Time (sec)

(c)

<sup>0</sup> 0.5 <sup>1</sup> 1.5 <sup>2</sup> 2.5 <sup>3</sup> 3.5 <sup>4</sup> 4.5 <sup>5</sup> -4

Time(sec)

Fig. 4. a) Position, b) velocity and c) acceleration of the moving platform

In this chapter, the inverse dynamics problem of 3-DOF RRR FPPM is derived using virtual work principle. Firstly, the inverse kinematics model and Jacobian matrix of 3-DOF RRR FPPM are determined using DH method. Secondly, the partial linear velocity and partial angular velocity matrices are computed. Pivotal points are selected in order to determine the partial linear velocity matrices. Thirdly, the inertial force and moment of each moving part are obtained. Consequently, the inverse dynamic equations of 3-DOF RRR FPPM in explicit form are derived. A butterfly shape Cartesian trajectory is used as a desired end-effector's trajectory to demonstrate the active joints torques.

### **5. References**


**2** 

*Turkey* 

**Dynamic Modeling and** 

Zafer Bingul and Oguzhan Karahan *Mechatronics Engineering, Kocaeli University* 

**Simulation of Stewart Platform** 

Since a parallel structure is a closed kinematics chain, all legs are connected from the origin of the tool point by a parallel connection. This connection allows a higher precision and a higher velocity. Parallel kinematic manipulators have better performance compared to serial kinematic manipulators in terms of a high degree of accuracy, high speeds or accelerations and high stiffness. Therefore, they seem perfectly suitable for industrial high-speed applications, such as pick-and-place or micro and high-speed machining. They are used in many fields such as flight simulation systems, manufacturing and medical applications. One of the most popular parallel manipulators is the general purpose 6 degree of freedom (DOF) Stewart Platform (SP) proposed by Stewart in 1965 as a flight simulator (Stewart, 1965). It consists of a top plate (moving platform), a base plate (fixed base), and six extensible legs connecting the top plate to the bottom plate. SP employing the same architecture of the Gough mechanism (Merlet, 1999) is the most studied type of parallel

Complex kinematics and dynamics often lead to model simplifications decreasing the accuracy. In order to overcome this problem, accurate kinematic and dynamic identification is needed. The kinematic and dynamic modeling of SP is extremely complicated in comparison with serial robots. Typically, the robot kinematics can be divided into forward kinematics and inverse kinematics. For a parallel manipulator, inverse kinematics is straight forward and there is no complexity deriving the equations. However, forward kinematics of SP is very complicated and difficult to solve since it requires the solution of many non-linear equations. Moreover, the forward kinematic problem generally has more than one solution. As a result, most research papers concentrated on the forward kinematics of the parallel manipulators (Bonev and Ryu, 2000; Merlet, 2004; Harib and Srinivasan, 2003; Wang, 2007). For the design and the control of the SP manipulators, the accurate dynamic model is very essential. The dynamic modeling of parallel manipulators is quite complicated because of their closed-loop structure, coupled relationship between system parameters, high nonlinearity in system dynamics and kinematic constraints. Robot dynamic modeling can be also divided into two topics: inverse and forward dynamic model. The inverse dynamic model is important for system control while the forward model is used for system simulation. To obtain the dynamic model of parallel manipulators, there are many valuable studies published by many researches in the literature. The dynamic analysis of parallel manipulators has been traditionally performed through several different methods such as

manipulators. This is also known as Gough–Stewart platforms in literature.

**1. Introduction** 

