Dynamic modeling and CPG-based trajectory generation for a masticatory rehab robot

Abstract

Human mastication is a complex and rhythmic biomechanical process which is regulated by a brain stem central pattern generator (CPG). Masticatory patterns, frequency and amplitude of mastication are different from person to person and significantly depend on food properties. The central nervous system controls the activity of muscles to produce smooth transitions between different movements. Therefore, to rehab human mandibular system, there is a real need to use the concept of CPG for development of a new methodology in jaw exercises and to help jaw movements recovery. This paper proposes a novel method for real-time trajectory generation of a mastication rehab robot. The proposed method combines several methods and concepts including kinematics, dynamics, trajectory generation and CPG. The purpose of this article is to provide a methodology to enable physiotherapists to perform the human jaw rehabilitation. In this paper, the robotic setup includes two Gough–Stewart platforms. The first platform is used as the rehab robot, while the second one is used to model the human jaw system. Once the modeling is completed, the second robot will be replaced by an actual patient for the selected physiotherapy. Gibbs–Appell’s formulation is used to obtain the dynamics equations of the rehab robot. Then, a method based on the Fourier series is employed to tune parameters of the CPG. It is shown that changes in leg lengths, due to the online changes of the mastication parameters, occur in a smooth and continuous manner. The key feature of the proposed method, when applied to human mastication, is its ability to adapt to the environment and change the chewing pattern in real-time parameters, such as amplitudes as well as jaw movements velocity during mastication.

Appendix

1.1 Jacobian matrix

Obtaining Jacobian matrix is the critical step in deriving equation of motion in robotics. By taking derivative of the right-hand side of Eq. (1), the velocity of spherical joint, $b_{i}$, can be obtained as:

$$\begin{aligned} {\mathbf{v}}_{bi}={\mathbf{v}}_{M}+\, {{\varvec{\upomega }}}_{M}\times {\mathbf{b}}_{i} \end{aligned}$$

(A.1)

where ${\mathbf{v}}_{M}$ and ${{\varvec{\upomega }}}_{M}$ are linear and angular velocities of mass center of moving platform, respectively. Note that ${\mathbf{v}}_{bi}={}_{~}^{B} {\mathbf{R}}_{i}{}_{~}^{{i}} {\mathbf{v}}_{bi}$. Equation (A.1) can be written as:

$$\begin{aligned} {\mathbf{v}}_{bi}=J_{bi}\dot{\mathbf{x}}_{M} \end{aligned}$$

(A.2)

where $\dot{\mathbf{x}}_{M}$ is a $1\times 6$ vector that consist of linear and angular velocity terms [49, 50]. Therefore, $J_{bi}$ can be defined as:

$$\begin{aligned} {\mathbf{J}}_{bi}=\left[ {\begin{array}{cccccc} {1} &{}\quad \mathrm {0} &{}\quad \mathrm {0} &{}\quad \mathrm {0} &{}\quad b_{i,z} &{}\quad {-b}_{i,y}\\ \mathrm {0} &{}\quad {1} &{}\quad \mathrm {0} &{}\quad {-b}_{i,z} &{}\quad \mathrm {0} &{}\quad b_{i,x}\\ \mathrm {0} &{}\quad \mathrm {0} &{}\quad {1} &{}\quad b_{i,y} &{}\quad {-b}_{i,x} &{}\quad \mathrm {0}\\ \end{array} } \right] \end{aligned}$$

(A.3)

By substituting Eq. (A.2) into ${}_{~}^{{i}}{\mathbf{v}}_{bi}={}_{~}^{{i}} {\mathbf{R}}_{B} {\mathbf{v}}_{bi}$, we have

$$\begin{aligned} {}_{~}^{{i}} {\mathbf{v}}_{bi}={}_{~}^{{i}} {\mathbf{J}}_{bi}\dot{\mathbf{x}}_{M} \end{aligned}$$

(A.4)

where

$$\begin{aligned} {}_{~}^{{i}} {\mathbf{J}}_{bi}={}_{~}^{{i}} {\mathbf{R}}_{B}\, {\mathbf{J}}_{bi}\equiv \left[ {\begin{array}{*{20}c} {}_{~}^{{i}} {\mathbf{J}}_{bi,x}\\ {}_{~}^{{i}} {\mathbf{J}}_{bi,y}\\ {}_{~}^{{i}} {\mathbf{J}}_{bi,z}\\ \end{array} } \right] ,\, {}_{~}^{{i}} {\mathbf{R}}_{B}={}_{~}^{B} {\mathbf{R}}_{i}^\mathrm{T} \end{aligned}$$

(A.5)

where ${}_{~}^{{i}} {\mathbf{J}}_{bi,x}, {}_{~}^{{i}} {\mathbf{J}}_{bi,y}$ and ${}_{~}^{{i}} {\mathbf{J}}_{bi,z}$ are the row of the ${}_{~}^{{i}} {\mathbf{J}}_{bi}$. Based on the above matrices, we have

$$\begin{aligned} \dot{d}_{i}= {}_{~}^{{i}}{\mathbf{J}}_{bi,z}\dot{\mathbf{x}}_{M} \end{aligned}$$

(A.6)

where Eq. (A.6) calculates the linear velocity of piston with respect to cylinder ith link. Since our model has six actuated legs, then the assembled form of Eq. (A.6) can be stated as:

$$\begin{aligned} \dot{\mathbf{q}}={\mathbf{J}}_{p}\, \dot{\mathbf{x}}_{M},\, {\mathbf{J}}_{p}=\left[ {\begin{array}{*{20}c} {}^{1} {\mathbf{J}}_{b\mathrm {1,}z}\\ {}^{2} {\mathbf{J}}_{b\mathrm {2,}z}\\ {\vdots }\\ {}^{\mathrm{6}} {\mathbf{J}}_{b\mathrm {6,}z}\\ \end{array}} \right] \end{aligned}$$

(A.7)

where $\dot{\mathbf{q}}$ is the velocity vector of actuated leg in their actuating direction. Moreover, ${\mathbf{J}}_{p}$ is called the manipulator Jacobian matrix [49]. Subsequently, we have [50]

$$\begin{aligned} {}_{~}^{{i}} {{\varvec{\upomega }}}_{i}= & {} \frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} - {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ {\mathbf{0}}_{\mathrm{1\times 6}}\\ \end{array} } \right] \,\dot{\mathbf{x}}_{M} \end{aligned}$$

(A.8)

$$\begin{aligned} {}_{~}^{{i}} {\mathbf{v}}_{1i}= & {} \frac{e_{1i}}{d_{i}}\left[ {\begin{array}{*{20}c} {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ {\mathbf{0}}_{\mathrm{1\times 6}}\\ \end{array} } \right] \,\dot{\mathbf{x}}_{M} \end{aligned}$$

(A.9)

$$\begin{aligned} {}_{~}^{{i}} {\mathbf{v}}_{2i}= & {} \frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} {\mathrm {(}d_{i}-e_{2i})\, {}_{~}^{{i}}{\mathbf{J}}}_{bi,x}\\ \mathrm {(}d_{i}-e_{2i})\, {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ d_{i} {}_{~}^{{i}}{\mathbf{J}}_{bi,z}\\ \end{array} } \right] \,\dot{\mathbf{x}}_{M} \end{aligned}$$

(A.10)

By combining Eqs. (A.8), (A.9) and (A.10), we have

$$\begin{aligned} {}_{~}^{{i}} \dot{\mathbf{x}}_{1i}= & {} \, {}_{~}^{{i}}{\mathbf{J}}_{1i}\dot{\mathbf{x}}_{M} \end{aligned}$$

(A.11)

$$\begin{aligned} {}_{~}^{{i}} \dot{\mathbf{x}}_{2i}= & {} \, {}_{~}^{{i}}{\mathbf{J}}_{2i}\dot{\mathbf{x}}_{M} \end{aligned}$$

(A.12)

where

$$\begin{aligned} {}_{~}^{{i}} {\mathbf{J}}_{1i}=\frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} e_{1i} {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ e_{1i} {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ {\mathbf{0}}_{{1\times 6}}\\ - {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ {\mathbf{0}}_{{1\times 6}}\\ \end{array} } \right] \end{aligned}$$

(A.13)

and

$$\begin{aligned} {}_{~}^{{i}} {\mathbf{J}}_{2i}=\frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} (d_{i}-e_{2i})\, {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ (d_{i}-e_{2i})\, {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ d_{i} {}_{~}^{{i}}{\mathbf{J}}_{bi,z}\\ - {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ {\mathbf{0}}_{\mathrm{1\times 6}}\\ \end{array} } \right] \end{aligned}$$

(A.14)

where ${}_{~}^{{i}} {\mathbf{J}}_{1i}$ and ${}_{~}^{{i}} {\mathbf{J}}_{2i}$ are known as the link Jacobian matrices. Next, by taking time derivative of Eq. (A.6), the absolute acceleration of point, $\, b_{i}$, can be obtained as

$$\begin{aligned} \dot{\mathbf{v}}_{bi}=\dot{\mathbf{v}}_{M}+\dot{{\varvec{\upomega }} }_{M}\times {\mathbf{b}}_{i}+ {{\varvec{\upomega }}}_{M}\times \left( \, {{\varvec{\upomega }}}_{M}\times \, {\mathbf{b}}_{i} \right) \end{aligned}$$

(A.15)

where $\dot{\mathbf{v}}_{M}$ and $\dot{{\varvec{\upomega }}}_{M}$ are linear and angular acceleration of mass center of moving platform, respectively. Equation (A.15) can be rewritten as:

$$\begin{aligned} \dot{\mathbf{v}}_{bi}= & {} \left[ {\begin{array}{*{20}c} {1} &{}\quad {0} &{}\quad {0} &{}\quad {0} &{}\quad -b_{i,z} &{}\quad -b_{i,y}\\ {0} &{}\quad {1} &{}\quad {0} &{}\quad -b_{i,z} &{}\quad {0} &{}\quad b_{i,x}\\ {0} &{}\quad {0} &{}\quad {1} &{}\quad -b_{i,y} &{}\quad b_{i,x} &{}\quad {0}\\ \end{array} } \right] \left\{ {\begin{array}{*{20}c} \dot{\mathbf{v}}_{p}\\ \dot{{\varvec{\upomega }}}_{p}\\ \end{array} } \right\} \nonumber \\&+\varvec{\upomega }_{p}\times \left( {{\varvec{\upomega }}}_{p}\times {\mathbf{b}}_{i} \right) \nonumber \\= & {} {\mathbf{J}}_{bi}\left\{ {\begin{array}{*{20}c} \dot{\mathbf{v}}_{p}\\ \dot{{\varvec{\upomega }}}_{p}\\ \end{array} } \right\} +{\mathbf{D}}_{1}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) ={\mathbf{J}}_{bi}\ddot{\mathbf{x}}_{M}+{\mathbf{D}}_{1}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \end{aligned}$$

(A.16)

By multiplying both sides of Eq. (A.16) with ${}_{~}^{B} {\mathbf{R}}_{i}$, we have

$$\begin{aligned} {}_{~}^{{i}} \dot{\mathbf{v}}_{bi}= & {} {}_{~}^{{i}}{\mathbf{R}}_{B}\dot{\mathbf{v}}_{bi}= {}_{~}^{{i}}{\mathbf{R}}_{B}{\mathbf{J}}_{bi}\ddot{\mathbf{x}}_{M}+\, {}_{~}^{{i}}{\mathbf{R}}_{B}{\mathbf{D}}_{\mathrm{1i}}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \nonumber \\= & {} \, {}_{~}^{{i}}{\mathbf{J}}_{bi}\ddot{\mathbf{x}}_{M}+\, {}_{~}^{{i}}{\mathbf{R}}_{B}{\mathbf{D}}_{\mathrm{1i}}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \end{aligned}$$

(A.17)

Equation (A.17) can be rewritten as:

$$\begin{aligned} {}_{~}^{{i}} \dot{\mathbf{v}}_{bi}= {}_{~}^{{i}}{\mathbf{J}}_{bi}\ddot{\mathbf{x}}_{M}+{}^{{\varvec{i}}} {\mathbf{D}}_{2i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \end{aligned}$$

(A.18)

where ${}^{{\varvec{i}}} {\mathbf{D}}_{2i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) ={}_{~}^{{i}} {\mathbf{R}}_{B}{\mathbf{D}}_{1i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right)$. By considering the third component of Eq. (A.18), we have the acceleration of link in the $z_{i}$ direction,

$$\begin{aligned} \ddot{\mathbf{q}}={\mathbf{J}}_{p}\ddot{\mathbf{x}}_{M}+\mathbf O \left( {\mathbf{q}},\dot{\mathbf{q}} \right) \end{aligned}$$

(A.19)

where

$$\begin{aligned} {\mathbf{O}}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) = {}_{~}^{{i}} {d}_{i}{}_{~}^{{i}} {\omega }_{i}^{2}=({}_{~}^{{i}} {v}_{bi,x}^{2}+{}_{~}^{{i}} {v}_{bi,y}^{2})/{d}_{i} \end{aligned}$$

(A.20)

On the other hand, based on [49]

$$\begin{aligned} {}_{~}^{{i}} \dot{\varvec{\upomega }}_{i}=\frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} {- {}_{~}^{{i}}\dot{v}}_{bi,y} \\ {}_{~}^{{i}} \dot{v}_{bi,x} \\ \mathrm {0}\\ \end{array} } \right] + \underbrace{{\frac{2}{{{d}_{{i}}^{2} }}\left[ {\begin{array}{*{20}c} {{}_{~}^{{i}} {v}_{{{bi},{y}}}^{~} {}_{{}}^{{i}} {v}_{{{bi},{z}}}^{~} } \\ { - {}_{~}^{{i}} {v}_{{{bi},{x}}}^{~} {}_{{}}^{{i}} {v}_{{{bi},{z}}}^{~} } \\ 0 \\ \end{array} } \right] }}_{{{}_{~}^{{i}} {\mathbf {D}}_{{3{i}}} \left( {{\mathbf {q}},\,\dot{\mathbf{q}}} \right) }} \end{aligned}$$

(A.21)

Substituting Eq. (A.18) in Eq. (A.21), we yield

$$\begin{aligned} {}_{~}^{{i}} \dot{\varvec{\upomega }}_{i}= & {} \frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} - {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\ddot{\mathbf{x}}_{M}-{\mathbf{D}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\ddot{\mathbf{x}}_{M}+{\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {0}\\ \end{array} } \right] +{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{3}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \nonumber \\= & {} \frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} - {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ {0}\\ \end{array} } \right] \,\ddot{\mathbf{x}}_{M}+\frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} {{\mathbf{-D}}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {0}\\ \end{array} } \right] \nonumber \\&+\,{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{3}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \nonumber \\= & {} {}_{~}^{{i}}{\mathbf{J}}_{\omega i}\, \ddot{\mathbf{x}}_{M}+{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{4}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \end{aligned}$$

(A.22)

where

$$\begin{aligned}&{}_{~}^{{i}} {\mathbf{J}}_{\omega i}\, =\frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} - {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ \mathrm {0}\\ \end{array} } \right] \end{aligned}$$

(A.23)

$$\begin{aligned}&{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{4}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) =\frac{{1}}{d_{i}}\left[ {\begin{array}{*{20}c} {-{}^{{\varvec{i}}} {\mathbf{D}}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {}^{{\varvec{i}}} {\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {0}\\ \end{array} } \right] +{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{3}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \end{aligned}$$

(A.24)

Notice that ${} \dot{\varvec{\upomega }}_{i}={}_{~}^{B} {\mathbf{R}}_{i}{}_{~}^{{i}} \dot{\varvec{\upomega }}_{i}$. Based on [49, 50], we have

$$\begin{aligned} {}_{~}^{{i}} \dot{\mathbf{v}}_{1i}=\frac{e_{1i}}{d_{i}}\left[ {\begin{array}{*{20}c} {}_{~}^{{i}}\dot{v}_{bi,x} \\ {}_{~}^{{i}} \dot{v}_{bi,y} \\ \mathrm {0}\\ \end{array} } \right] - \underbrace{{\frac{{ {e}_{1} }}{{ {d}_{ {i}}^{2} }}\left[ {\begin{array}{*{20}c} {2{}_{~}^{ {i}} {v}_{{ {bi}, {x}}}^{~} {}_{{}}^{ {i}} {v}_{{ {bi}, {z}}}^{~} } \\ {2{}_{~}^{ {i}} {v}_{{ {bi}, {y}}}^{~} {}_{{}}^{ {i}} {v}_{{ {bi}, {z}}}^{~} } \\ {{}_{{}}^{ {i}} {v}_{{ {bi}, {x}}}^{2} + {}_{~}^{ {i}} {v}_{{ {bi}, {y}}}^{2} } \\ \end{array}} \right] }}_{{{}_{~}^{ {i}} {\mathbf {D}}_{{5 {i}}} \left( {{\mathbf {q}},\,\dot{\mathbf{q}}} \right) }} \end{aligned}$$

(A.25)

Substituting Eq. (A.18) into Eq. (A.25), we yield

$$\begin{aligned} {}_{~}^{{i}}\dot{\mathbf{v}}_{1i}= & {} {\frac{e_{1i}}{d_{i}}\left[ {\begin{array}{*{20}c} {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\ddot{\mathbf{x}}_{p}+{}^{{\varvec{i}}} {\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\ddot{\mathbf{x}}_{p}+{}^{{\varvec{i}}} {\mathbf{D}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {0}\\ \end{array} } \right] +{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{5}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) } \nonumber \\= & {} {\frac{e_{1i}}{d_{i}}\left[ {\begin{array}{*{20}c} {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ \mathrm {0}\\ \end{array} } \right] \ddot{\mathbf{x}}_{M}+\frac{e_{1i}}{d_{i}}\left[ {\begin{array}{*{20}c} {}^{{\varvec{i}}} {\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {}^{{\varvec{i}}} {\mathbf{D}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ \mathrm {0}\\ \end{array} } \right] +{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{5}i}( {\mathbf{q}},\dot{\mathbf{q}} )}\nonumber \\= & {} {}_{~}^{{i}}{\mathbf{J}}_{V{1}i}\, \ddot{\mathbf{x}}_{M}+{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{6}i}\left( {\mathbf{q}},\dot{\mathbf{q}}\right) \end{aligned}$$

(A.26)

where

$$\begin{aligned}&{}_{~}^{{i}} {\mathbf{J}}_{V{1}i}\, =\frac{e_{1i}}{d_{i}}\left[ {\begin{array}{*{20}c} {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ \mathrm {0}\\ \end{array} } \right] \end{aligned}$$

(A.27)

$$\begin{aligned}&{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{6}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) =\frac{e_{1i}}{d_{i}}\left[ {\begin{array}{*{20}c} {}^{{\varvec{i}}} {\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {}^{{\varvec{i}}} {\mathbf{D}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ {0}\\ \end{array} } \right] +{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{5}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \end{aligned}$$

(A.28)

Note that $\dot{\mathbf{v}}_{1i}={}^{S} {\mathbf{R}}_{i}{}_{~}^{{i}} \dot{\mathbf{v}}_{1i}$. Next, based on [49], we have

$$\begin{aligned} {}_{~}^{{i}} \dot{\mathbf{v}}_{2i}= & {} \frac{{1}}{d_{i}\, }\left[ {\begin{array}{*{20}c} (d_{i}-e_{2i})\, {}_{~}^{{i}}\dot{v}_{bi,x} \\ (d_{i}-e_{2i})\, {}_{~}^{{i}}\dot{v}_{bi,y} \\ d_{i} {}_{~}^{{i}}\dot{v}_{bi,z} \\ \end{array} } \right] \nonumber \\&+ \underbrace{{\frac{{ {e}_{{2 {i}}} }}{{ {d}_{ {i}}^{2} }}\left[ {\begin{array}{*{20}c} {2{}_{~}^{ {i}} {v}_{{ {bi}, {x}}}^{~} {}_{{}}^{ {i}} {v}_{{ {bi}, {z}}}^{~} } \\ {2{}_{~}^{ {i}} {v}_{{ {bi}, {y}}}^{~} {}_{{}}^{ {i}} {v}_{{ {bi}, {z}}}^{~} } \\ {\left( {{}_{~}^{ {i}} {v}_{{ {bi}, {x}}}^{2} + {}_{~}^{ {i}} {v}_{{ {bi}, {y}}}^{2} } \right) } \\ \end{array} } \right] }}_{{{}_{~}^{ {i}} {\mathbf {D}}_{7} \left( {{\mathbf {q}},\,\dot{\mathbf{q}}} \right) }} \end{aligned}$$

(A.29)

Substituting Eq. (A.18) into Eq. (A.29), we yield

$$\begin{aligned} {}_{~}^{{i}} \dot{\mathbf{v}}_{2i}= & {} \frac{{1}}{d_{i}\, }\left[ {\begin{array}{*{20}c} \left( d_{i}-e_{2i} \right) \left\{ \, {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\ddot{\mathbf{x}}_{M}+{}^{{\varvec{i}}} {\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right\} \\ \left( d_{i}-e_{2i} \right) \left\{ \, {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\ddot{\mathbf{x}}_{M}+{}^{{\varvec{i}}} {\mathbf{D}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right\} \\ d_{i}\left\{ {}_{~}^{{i}} {\mathbf{J}}_{bi,z}\ddot{\mathbf{x}}_{M}+{}^{{\varvec{i}}} {\mathbf{D}}_{2i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right\} \\ \end{array} } \right] \nonumber \\&+{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{7}}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \nonumber \\= & {} \frac{{1}}{d_{i} }\left[ {\begin{array}{*{20}c} (d_{i}-e_{2i})\, {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ (d_{i}-e_{2i})\, {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ d_{i} {}_{~}^{{i}}{\mathbf{J}}_{bi,z}\\ \end{array} } \right] \ddot{\mathbf{x}}_{M}\nonumber \\&+{\frac{{1}}{d_{i}\, }\left[ {\begin{array}{*{20}c} (d_{i}-e_{2i})^{{\varvec{i}}} {\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ (d_{i}-e_{2i})^{{\varvec{i}}} {\mathbf{D}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ d_{i}^{{\varvec{i}}} {\mathbf{D}}_{2i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ \end{array} } \right] +{\mathbf{D}}_{\mathrm{7}}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) }\nonumber \\= & {} {}_{~}^{{i}}{\mathbf{J}}_{V{2}i}\, \ddot{\mathbf{x}}_{M}+^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{8}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \end{aligned}$$

(A.30)

where

$$\begin{aligned}&{}_{~}^{{i}} {\mathbf{J}}_{V{2}i}\, =\frac{{1}}{d_{i} }\left[ {\begin{array}{*{20}c} \left( d_{i}-e_{2i} \right) {}_{~}^{{i}}{\mathbf{J}}_{bi,x}\\ \left( d_{i}-e_{2i} \right) {}_{~}^{{i}}{\mathbf{J}}_{bi,y}\\ d_{i} {}_{~}^{{i}}{\mathbf{J}}_{bi,z}\\ \end{array} } \right] \end{aligned}$$

(A.31)

$$\begin{aligned} {}^{{\varvec{i}}} {\mathbf{D}}_{8i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right)= & {} \frac{{1}}{d_{i} }\left[ {\begin{array}{*{20}c} (d_{i}-e_{2i})^{{\varvec{i}}} {\mathbf{D}}_{2i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ (d_{i}-e_{2i})^{{\varvec{i}}} {\mathbf{D}}_{2i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ d_{i}^{{\varvec{i}}} {\mathbf{D}}_{2i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \\ \end{array} } \right] \nonumber \\&+{}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{7}}\left( {\mathbf{q}}, \dot{\mathbf{q}} \right) \end{aligned}$$

(A.32)

Note that $\dot{\mathbf{v}}_{2i}= ^{B} {\mathbf{R}}_{i} {}_{~}^{{i}}\dot{\mathbf{v}}_{2i}$.

.

1.2 Obtain energy of moving platform, ${\varvec{S}}_{\varvec{M}}$

The Gibbs–Appell function is defined as

$$\begin{aligned} S_{M}=\frac{{1}}{{2}}m{\mathbf{a}}_{M}\cdot {\mathbf{a}}_{\mathrm{M}}+\frac{{1}}{{2}}{\varvec{\upalpha }}_{\mathrm{M}}\cdot \frac{{\partial }{\mathbf{H}}_{\mathrm{M}}}{{\partial }t}+{\varvec{\upalpha }}_{\mathrm{M}}\cdot ({{\varvec{\upomega }}}_{M}\times {\mathbf{H}}_{\mathrm{M}})\nonumber \\ \end{aligned}$$

(B.1)

where ${\mathbf{a}}_{\mathrm{M}}, {{\varvec{\upomega }}}_{\mathrm{M}}, {\varvec{\upalpha }}_{\mathrm{M}}$ and ${\mathbf{H}}_{\mathrm{M}}$ are linear acceleration, angular velocity, angular acceleration and angular momentum of mass center of moving platform, respectively. Additionally, $\frac{\delta {\mathbf{H}}_{\mathrm{M}}}{\delta t}$ represents the change of angular momentum. These variables are defined as:

$$\begin{aligned} {\mathbf{a}}_{\mathrm{M}}= & {} \ddot{{x}}\, {\mathbf{i}+}\ddot{y}\, {\mathbf{j}+}\ddot{z} {\mathbf{k}} \end{aligned}$$

(B.2)

$$\begin{aligned} {\varvec{\alpha }}_{\mathrm{M}}= & {} \ddot{\alpha }_{\,}{\mathbf{i}}+\ddot{\beta }_{\,}{\mathbf{j}}+\ddot{\gamma }_{\,}{\mathbf{k}} \end{aligned}$$

(B.3)

$$\begin{aligned} {\mathbf{H}}_{\mathrm{M}}= & {} {\mathbf{I}}_{M}{.} {{\varvec{\upomega }}}_{M}\left[ {\begin{array}{*{20}c} I_{M,xx} &{} {0} &{} {0}\\ {0} &{} I_{M,yy} &{} {0}\\ {0} &{} {0} &{} I_{M,zz}\\ \end{array} } \right] \left[ {\begin{array}{*{20}c} \dot{\alpha }_{\,}\\ \dot{\beta }_{\,}\\ \dot{\gamma }_{\,}\\ \end{array} } \right] \nonumber \\= & {} I_{M,xx}\dot{\alpha }_{{ \, }}{\mathbf{i}+} I_{M,yy}\dot{\beta }_{{ \, }}{\mathbf{j}+} I_{M,zz}\dot{\gamma }_{{ \, }}{\mathbf{k}} \end{aligned}$$

(B.4)

$$\begin{aligned} \frac{{\partial }{\mathbf{H}}_{\mathrm{M}}}{{\partial }t}= & {} I_{M,xx}\ddot{\alpha }{\mathbf{i}}+ I_{M,yy}\ddot{\beta }_{{ \, }}{\mathbf{j}}+ I_{M,zz}\ddot{\gamma }_{{ \, }}{\mathbf{k}} \end{aligned}$$

(B.5)

By substituting Eqs. (B.2)–(B.5) into Eq. (B.1), we have

$$\begin{aligned} S_{M}= & {} \frac{{1}}{{2}}m\left( \ddot{{x}}^{2}+\ddot{y}^{2}+\ddot{z}^{2} \right) \nonumber \\&+\,\frac{{1}}{{2}}\left( \, I_{M,xx}\ddot{\alpha }^{2}+ I_{M,yy}\ddot{\beta }_{{ \, }}^{2}+ I_{M,zz}\ddot{\gamma }^{2} \right) \nonumber \\&+\left( I_{M,zz}- I_{M,yy} \right) \ddot{\alpha }\dot{\beta }_{\,}\dot{\gamma }_{\,}+\left( I_{M,xx}- I_{M,zz} \right) \ddot{\beta }_{{ \, }}\dot{\alpha }_{\,}{\dot{\gamma }}_{{ \, }}\nonumber \\&+\left( I_{M,yy}- I_{M,xx} \right) \ddot{\gamma }\dot{\alpha }_{\,}\dot{\beta }_{\,} \end{aligned}$$

(B.6)

1.3 Obtain energy of cylinder, ${\varvec{S}}_{{\mathbf{1}}{\varvec{i}}}$

The Gibbs–Appell function of cylinder is defined as:

$$\begin{aligned} S_{1i}=\frac{{1}}{{2}}m_{1i}\, \dot{\mathbf{v}}_{1i}\cdot \, \dot{\mathbf{v}}_{1i}+\frac{{1}}{{2}}\, \dot{{\varvec{\upomega }}}_{i}\cdot \frac{{\partial }{\mathbf{H}}_{\mathrm{1i}}}{{\partial }t}+ \dot{{\varvec{\upomega }}}_{i}\cdot ( {{\varvec{\upomega } }}_{i}\times {\mathbf{H}}_{\mathrm{1i}})\nonumber \\ \end{aligned}$$

(C.1)

where ${} \dot{\mathbf{v}}_{1i}, {} {{\varvec{\upomega }}}_{i}, {} \dot{{\varvec{\upomega }}}_{i}$ and ${\mathbf{H}}_{\mathrm{1i}}$ are linear acceleration, angular velocity, angular acceleration and angular momentum of mass center of ith cylinder, respectively. Terms used in Eq. (C.1) can be obtained as:

$$\begin{aligned} \frac{{1}}{{2}}m_{1i} \dot{\mathbf{v}}_{1i}\cdot \dot{\mathbf{v}}_{1i}= & {} \frac{{1}}{{2}}m_{1i}\left[ \left( {\mathbf{J}}_{V{1}i,x}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{6}i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) ^{2}\right. \nonumber \\&+\,\left( {\mathbf{J}}_{V{1}i,y}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{6}i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) ^{2}\nonumber \\&\left. +\,\left( {\mathbf{J}}_{V{1}i,z}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{6}i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) ^{2} \right] \end{aligned}$$

(C.2)

where ${} {\mathbf{J}}_{V{1}i}\, = {}_{~}^{B} {\mathbf{R}}_{i} {}_{~}^{{i}}{\mathbf{J}}_{V{1}i}$ and ${} {\mathbf{D}}_{\mathrm{6}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) = {}_{~}^{B} {\mathbf{R}}_{i}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{6}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right)$.

$$\begin{aligned} \frac{{1}}{{2}} \dot{{\varvec{\upomega }} }_{i}\cdot \frac{{\partial }{\mathbf{H}}_{\mathrm{1i}}}{{\partial }t}= & {} \frac{{1}}{{2}}\left[ {I_{{ 11}}^{2i}\left( {\mathbf{J}}_{\omega i,x}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) }\right. \nonumber \\&+\,{I_{\mathrm{22}}^{2i}\left( \, {\mathbf{J}}_{\omega i,y}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) }^{2}\nonumber \\&\left. +\,{I_{\mathrm{33}}^{2i}\left( {\mathbf{J}}_{\omega i,z}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) }^{2} \right] \end{aligned}$$

(C.3)

where ${} {\mathbf{J}}_{\omega i}\, = {}_{~}^{B} {\mathbf{R}}_{i} {}_{~}^{{i}}{\mathbf{J}}_{\omega i}$ and ${} {\mathbf{D}}_{\mathrm{4}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) = {}_{~}^{B} R_{i}^{{\varvec{i}}} {\mathbf{D}}_{4i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right)$.

$$\begin{aligned} {} \dot{\varvec{\upomega }}_{i}\cdot \left( {{\varvec{\upomega }}}_{i}\times {\mathbf{H}}_{\mathrm{1i}} \right)= & {} \mathrm {K}_{1i,x}\left( {\mathbf{J}}_{\omega i,x}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) \nonumber \\&+\,K_{1i,y}\left( {\mathbf{J}}_{\omega i,y}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) \nonumber \\&+\,K_{1i,z}\left( {\mathbf{J}}_{\omega i,z}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) \end{aligned}$$

(C.4)

where ${\mathbf{K}}_{1i}=\left( {{\varvec{\upomega }}}_{i}\times {\mathbf{H}}_{\mathrm{1ci}} \right)$.

1.4 Obtain energy of piston, ${\varvec{S}}_{{\mathbf{2}}{\varvec{i}}}$

The Gibbs–Appell function of piston is defined as:

$$\begin{aligned} S_{2i}=\frac{{1}}{{2}}m\, \dot{\mathbf{v}}_{2i}\cdot \, \dot{\mathbf{v}}_{2i}+\frac{{1}}{{2}}\, \dot{\varvec{\upomega }}_{i}\cdot \frac{{\partial }{\mathbf{H}}_{\mathrm{2i}}}{{\partial }t}+ \dot{\varvec{\upomega }}_{i}\cdot ( {{\varvec{\upomega }}}_{i}\times {\mathbf{H}}_{\mathrm{2i}}) \end{aligned}$$

(D.1)

where ${} \dot{\mathbf{v}}_{2i}, {} {{\varvec{\upomega }}}_{i}$, ${} \dot{\varvec{\upomega }}_{i}$ and ${\mathbf{H}}_{\mathrm{2i}}$ are linear acceleration, angular velocity, angular acceleration and angular momentum of mass center of ith piston, respectively. Terms of Eq. (D.1) can be obtained as:

$$\begin{aligned} \frac{{1}}{{2}}m_{2i} \dot{\mathbf{v}}_{2i}\cdot \dot{\mathbf{v}}_{2i}= & {} \frac{{1}}{{2}}m_{2i}\left[ \left( {\mathbf{J}}_{V{2}i,x}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{8}i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) ^{2}\right. \nonumber \\&+\left( {\mathbf{J}}_{V{2}i,y}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{8}i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) ^{2}\,\nonumber \\&\left. +\left( {\mathbf{J}}_{V{2}i,z}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{8}i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) ^{2} \right] \end{aligned}$$

(D.2)

where ${} {\mathbf{J}}_{V{2}i}\, = {}_{~}^{B} {\mathbf{R}}_{i} {}_{~}^{{i}}{\mathbf{J}}_{V{2}i}$ and ${} {\mathbf{D}}_{\mathrm{8}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) = ^{B} {\mathbf{R}}_{i}^{i} {\mathbf{D}}_{\mathrm{8}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right)$.

$$\begin{aligned} \frac{{1}}{{2}} \dot{\varvec{\upomega }}_{i}\cdot \frac{{\partial }{\mathbf{H}}_{\mathrm{1i}}}{{\partial }t}= & {} \frac{{1}}{{2}}\left[ {I_{\mathrm{11}}^{1i}\left( {\mathbf{J}}_{\omega i,x}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) }^{2}\right. \nonumber \\&+\,{I_{\mathrm{22}}^{1i}\left( {\mathbf{J}}_{\omega i,y}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) }^{2}\nonumber \\&\left. +\,{I_{\mathrm{33}}^{1i}\left( {\mathbf{J}}_{\omega i,z}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) }^{2} \right] \end{aligned}$$

(D.3)

where ${} {\mathbf{J}}_{\omega i}\, = {}_{~}^{B} {\mathbf{R}}_{i} {}_{~}^{{i}}{\mathbf{J}}_{\omega i}$ and ${} {\mathbf{D}}_{\mathrm{4}i}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) = ^{\,B} {\mathbf{R}}_{i}^{{\varvec{i}}} {\mathbf{D}}_{\mathrm{4}i}\left( {\mathbf{q}},\dot{\mathbf{q}}\right)$.

$$\begin{aligned} {} \dot{\varvec{\upomega }}_{i}\cdot \left( \, {{\varvec{\upomega }}}_{i}\times {\mathbf{H}}_{\mathrm{2i}} \right)= & {} K_{2i,x}\left( {\mathbf{J}}_{\omega i,x}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,x}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) \nonumber \\&+\,K_{2i,y}\left( {\mathbf{J}}_{\omega i,y}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,y}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) \nonumber \\&+\,K_{2i,z}\left( {\mathbf{J}}_{\omega i,z}\, \ddot{\mathbf{x}}_{M}+ {\mathbf{D}}_{\mathrm{4}i,z}\left( {\mathbf{q}},\dot{\mathbf{q}} \right) \right) \end{aligned}$$

(D.4)

where ${\mathbf{K}}_{2i}=\left( {{\varvec{\upomega }}}_{i}\times {\mathbf{H}}_{\mathrm{2\, i}} \right)$.