## 2 Optimal Trajectory Formulation  
### General Problem Formulation  
In order to exploit the benefits of a smooth, i.e., minimum jerk, trajectory, and limit the possible overshoots in the path, a trade-off method between the two objectives is investigated and proposed. To obtain a uniform description of the motion regardless of the space in which the motion is planned, e.g., the joint space of a specific task, we describe it as a curve in an open subset \(\Omega\) of an \(n\)-dimensional manifold \(\mathcal{M}\) such that there exists a chart \((\psi,\Omega)\) such that:  
\[\mathbf{\psi}:\Omega\longrightarrow\mathbb{R}^{n}. \tag{1}\]  
This allows identifying the motion with a curve in \(\mathbb{R}^{n}\) without considering the specific details of the space where the planning is done. Let us consider a partial description of a desired robot motion given as the execution time \(t_{f}\) and an ordered sequence of \(N+1\) via-points in the manifold. Utilizing the previously introduced local coordinate map \(\mathbf{\psi}\), Equation (1), the problem can be rewritten in \(\mathbb{R}^{n}\), and the ordered sequence of desired via-points can be described as \(\mathit{VP}=\{\mathbf{w}_{0},\mathbf{w}_{1},\dots,\mathbf{w}_{N}\}\subset \mathbb{R}^{n}\). This operation translates our problem into that of finding a reasonable curve joining the via-points \(\mathit{VP}\) in such a way that the via-point \(\mathbf{w}_{i}\) and the velocity \(\dot{\mathbf{w}}_{i}\) are attained at the time \(t_{i}\) with \(t_{i}<t_{i+1}\) in which every instant \(t_{i}\) is within the execution time interval \(T=[0,t_{f}]\). Then, the position, velocity, and acceleration constraints can be written as:  
\[ \begin{split}\mathbf{q}(t_{i})&=\mathbf{w}_{i}\\ \dot{\mathbf{q}}(t_{0})&=\dot{\mathbf{w}}_{0}\\ \ddot{\mathbf{q}}(t_{0})&=\ddot{\mathbf{w}}_{0}\\ \end{split} \quad\dot{\mathbf{q}}(t_{N})=\ddot{ \mathbf{w}}_{N}. \tag{2}\]  
with \(t_{0}=0\) and \(t_{N}=t_{f}\). We define the term reasonable curve as a curve that optimizes a desired cost function and is \"physically\" feasible. As the cost functions of this work include the integrals of the derivatives of \(\mathbf{q}\), it is required that the \(k\)-th first derivatives are square integrable. Such a set of functions constitutes a Hilbert space, denoted by \(\mathbb{W}^{2k}(T,\mathbb{R}^{n})\). The second requirement demands that \(\mathbf{q}\) connects the via-points \(\mathit{VP}\) with a trajectory that is continuous to the \(k\)-th order. Furthermore, such variations must be attainable by the dynamics of the systems. Thus, we introduce the set of admissible motions as:  
\[\mathcal{Q}=\Big{\{}\mathbf{q}\in\mathbb{W}^{2,k}(T,\mathbb{R}^{n})\cap C^{k} (T,\mathbb{R}^{n})\Big{|}\exists\{t_{i}\}_{i=0}^{N}\subset T\text{s.t.} \mathbf{q}(t_{i})=\mathbf{w}_{i}\text{ and }t_{i}<t_{i+1}\Big{\}}. \tag{3}\]  
The problem can then be seen as the one of finding the curve among the admissible motions \(\mathcal{Q}\) such that a given performance index is minimized and additional possible constraints are satisfied. We define the performance index as a map \(I:\mathcal{Q}\longrightarrow\mathbb{R}\) that describes a desired quality of the motion under analysis by associating it with a real number. A simple way to construct such a performance index is to take the average value of a given scalar function \(F\), which depends on the desired \(i\)-th derivative. The cost function \(I\) can then be expressed as:  
\[I(\mathbf{q})=\int_{0}^{t_{f}}F(\mathbf{q},\mathbf{\dot{q}},\ddot{\mathbf{q}}, \dddot{\mathbf{q}})\dd t \tag{4}\]  
We can then express the optimal trajectory planning problem in the following form:  
\[ \begin{split}&\min_{\mathbf{q}}I(\mathbf{q})\\ &\text{s.t.}\ \mathbf{q}\in\mathcal{Q}.\end{split}  \tag{5}\]  
to which further constraints can be added. The design problem is therefore often formulated in terms of minimum velocity \(F=\|\mathbf{\dot{q}}\|\), minimum acceleration \(F=\|\ddot{\mathbf{q}}\|\), and/or minimum jerk \(F=\|\dddot{\mathbf{q}}\|\).