## 2 Methods
### Motion Control Strategy
#### 2.3.2 Nonlinear Tracking Control Strategy  
In order to perform a stable and smooth motion, a nonlinear controller based on a feedback linearization strategy is designed [45]. Using the Euler-Lagrange Equation (1), the nonlinear model is presented in an input affine state space representation as follows:  
\[\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x})+\mathbf{g}(\mathbf{x})\mathbf{u}, \tag{8}\]  
\[\text{where }\mathbf{x}:=\left[\begin{array}{c}\mathbf{q}\\ \dot{\mathbf{q}}\end{array}\right]\text{ is the state vector, }\mathbf{u}:=\left[\begin{array}{c}\tau_{1}\\ \tau_{2}\\ \tau_{3}\end{array}\right]\text{ is the vector of control inputs, }\]  
\[\mathbf{f}(\mathbf{x})=\left[\begin{array}{c}\dot{\mathbf{q}}\\ D^{-1}(\mathbf{q})[-C(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}-G(\mathbf{q}) ]\end{array}\right], \tag{9}\]  
and  
\[\mathbf{g}(\mathbf{x})=\left[\begin{array}{c}0\\ D^{-1}(\mathbf{q})\end{array}\right]. \tag{10}\]  
Based on the model (8), the control output is defined as follows:  
\[\mathbf{y}:=\mathbf{h}(\mathbf{x})=\mathbf{q}-\mathbf{q}^{*}, \tag{11}\]  
where \(\mathbf{q}^{*}\) is the reference trajectory computed in Section 2.3.1.  
To find a mathematical form that expresses the output vector as a function of the control-input vector, successive time differentiations of (11) were performed until the vector of control-input signals is explicit. Since the trajectory references are designed to be smooth, their time derivatives are negligible. Then, the time differentiation result is  
\[\frac{d\mathbf{y}}{dt} = \frac{\partial\mathbf{h}(\mathbf{x})}{\partial\mathbf{x}}\dot{ \mathbf{x}}, \tag{12}\] \[= \left[\begin{array}{c}\frac{\partial\mathbf{h}(\mathbf{x})}{ \partial\mathbf{q}}\end{array}\right.\begin{array}{c}\frac{\partial\mathbf{h }(\mathbf{x})}{\partial\mathbf{q}}\end{array}\right][\mathbf{f}(\mathbf{x})+ \mathbf{g}(\mathbf{x})\mathbf{u}],\] (13) \[= \
abla\mathbf{h}(\mathbf{x})\mathbf{f}(\mathbf{x})+\
abla\mathbf{ h}(\mathbf{x})\mathbf{g}(\mathbf{x})\mathbf{u},\] (14) \[= L_{f}\mathbf{h}+L_{g}\mathbf{h}\mathbf{u}, \tag{15}\]  
where \(\
abla\mathbf{h}(\mathbf{x})\) is the gradient of \(\mathbf{h}(\mathbf{x})\), and \(L_{f}\mathbf{h}\) and \(L_{g}\mathbf{h}\) are the Lie derivatives of \(\mathbf{h}(\mathbf{x})\) along \(\mathbf{f}(\mathbf{x})\) and \(\mathbf{g}(\mathbf{x})\), respectively. Given that \(L_{g}\mathbf{h}\mathbf{u}\) is equal to zero, the first time-derivative of the output is independent of the control input; therefore, the second derivative is necessary. This is,  
\[\frac{d^{2}\mathbf{y}}{dt^{2}} = \left[\begin{array}{c}\frac{\partial}{\partial\mathbf{q}}\left( \frac{\partial\mathbf{h}(\mathbf{x})}{\partial\mathbf{q}}\dot{\mathbf{q}} \right)\end{array}\right]\frac{\partial\mathbf{h}}{\partial\mathbf{q}}\;\left] \mathbf{f}(\mathbf{x})+\mathbf{g}(\mathbf{x})\mathbf{u}\right], \tag{16}\] \[= L_{f}^{2}\mathbf{h}+L_{g}L_{f}\mathbf{h}\mathbf{u}, \tag{17}\]  
where \(L_{f}^{2}\mathbf{h}\) is the second Lie derivative of \(\mathbf{h}(\mathbf{x})\) along \(\mathbf{f}(\mathbf{x})\) and \(L_{g}L_{f}\mathbf{h}\) is a decoupling term that is locally invertible around the operation region [46].  
Considering the local transformation in (17), a feedback linearization control law is defined as follows:  
\[\mathbf{u}=L_{g}L_{f}\mathbf{h}^{-1}\left(-L_{f}^{2}\mathbf{h}+\mu\right), \tag{18}\]  
with \(\mu\) being an auxiliary control signal to be specified. Applying the control law (18) to (17), the resulting dynamics  
\[\frac{d^{2}\mathbf{y}}{dt^{2}}=\mu, \tag{19}\]  
are shown to be linear. However, the stability of (19) is unknown. To guaranty a stable behavior, \(\mu\) is defined as  
\[\mu:=-K_{d}\frac{d\mathbf{y}}{dt}-K_{p}\mathbf{y}, \tag{20}\]where \(K_{d}\) and \(K_{p}\) are diagonal matrices used as control gains, which are selected to be strictly positive. This results in a Hurwitz system with the form:  
\[\frac{d^{2}\mathbf{y}}{dt^{2}}+K_{d}\frac{d\mathbf{y}}{dt}+K_{p}\mathbf{y}=0, \tag{21}\]  
which is asymptotically stable.