## 2 Optimal Trajectory Formulation
### The Variational Formalism  
The variational formalism of the optimization of (4) requires the formal definition of a small variation of a curve \(\mathbf{q}\in\mathcal{Q}\). Following a commonly adopted approach [27; 28], we construct a small variation of a curve \(\mathbf{q}\in\mathcal{Q}\):  
\[\mathbf{p}(t)=\mathbf{q}(t)+\varepsilon\boldsymbol{\eta}(t). \tag{9}\]  
by using an arbitrarily smooth map \(\boldsymbol{\eta}\in C^{\infty}(T,\mathbb{R}^{n})\) called variation and with an arbitrarily small number \(\varepsilon>0\). Both curves \(\mathbf{p}\) and \(\mathbf{q}\) satisfy the constraints (2) at their respective set of times \(t_{0},\dots,t_{N}\) and \(r_{0},\dots,r_{N}\) such that \(\mathbf{q}(t_{i})=\mathbf{q}_{i}\) and \(\mathbf{p}(r_{i})=q_{i}\). By definition, the difference between \(t_{i}\) and \(r_{i}\) is arbitrarily small and depends on \(\varepsilon\) through the relation [28]\(r_{i}=t_{i}+\varepsilon\delta t_{i}\), where the scalar \(\delta t_{i}\) is an arbitrary variation of \(t_{i}\). Furthermore, due to the small differences in the values of velocities and acceleration at the prescribed points, consequently, we have to consider the arbitrary variations \(\delta\dot{\mathbf{q}}_{i}=\dot{\mathbf{p}}(r_{i})-\dot{\mathbf{q}}(t_{i})\) and \(\delta\ddot{\mathbf{q}}_{i}=\dot{\mathbf{p}}(r_{i})-\ddot{\mathbf{q}}(t_{i})\). Such variations have a concrete relation with the variations \(\boldsymbol{\eta}(t)\) given by [28],  
\[\boldsymbol{\eta}(t_{i}) =-\dot{\mathbf{q}}(t_{i})\delta t_{i} \tag{10}\] \[\dot{\boldsymbol{\eta}}(t_{i}) =\delta\dot{\mathbf{q}}_{i}-\ddot{\mathbf{q}}(t_{i})\delta t_{i}\] \[\ddot{\boldsymbol{\eta}}(t_{i}) =\delta\ddot{\mathbf{q}}_{i}-\dddot{\mathbf{q}}(t_{i})\delta t_{i}.\]  
The variations \(\delta t_{i}\), \(\boldsymbol{\eta}(t_{i})\), \(\delta\dot{\mathbf{q}}_{i}\), and \(\delta\ddot{\mathbf{q}}_{i}\) vanish for \(i=0\) and \(i=N\) due to the constraints in (2). Thanks to the formal definition of small variations stated above, we can disclose the necessary conditions that make an element of \(\mathcal{Q}\) a stationary point of (4). Such elements are called extremals of the functional (4) and are characterized by the relation:  
\[I(\mathbf{p})-I(\mathbf{q})=\mathcal{O}(\varepsilon^{2}), \tag{11}\]  
where \(\mathcal{O}(\varepsilon^{2})\) represents a negligible quantity proportional to \(\varepsilon^{2}\) called big-O notation. Such a relation may be expanded at each interval as:  
\[I(\mathbf{p})-I(\mathbf{q})=\sum_{i=0}^{N-1}\int_{r_{i}}^{r_{i+1}}F(\dot{ \mathbf{p}},\dddot{\mathbf{p}})\mathrm{d}t-\int_{t_{i}}^{t_{i+1}}F(\dot{ \mathbf{q}},\dddot{\mathbf{q}})\mathrm{d}t\ \ =\sum_{i=0}^{N-1}I_{i}. \tag{12}\]  
By considering the relations between \(t_{i}\) and \(r_{i}\), we have:  
\[ \begin{split} I_{i}=&\int_{t_{i}}^{t_{i+1}}F(\dot{ \mathbf{p}},\dddot{\mathbf{p}})-F(\dot{\mathbf{q}},\dddot{\mathbf{q}})\mathrm{ d}t\\ &+\int_{t_{i}+\varepsilon\delta t_{i}}^{t_{i}}F(\dot{\mathbf{p}}, \dddot{\mathbf{p}})\mathrm{d}t+\int_{t_{i+1}}^{t_{i+1}+\varepsilon\delta t_{i+ 1}}F(\dot{\mathbf{p}},\dddot{\mathbf{p}})\mathrm{d}t.\end{split}  \tag{13}\]  
By integrating by parts and neglecting the terms proportional to \(\varepsilon^{2}\), this relation can be expanded to:  
\[