Trajectory Optimization on Manifolds: A Theoretically-Guaranteed Embedded Sequential Convex Programming Approach

TL;DR

This paper extends Sequential Convex Programming (SCP) to manifold-constrained trajectory optimization problems by leveraging geometric embeddings, providing theoretical guarantees and demonstrating practical effectiveness.

Contribution

It introduces a novel SCP algorithm for manifold problems using geometric embeddings, bridging the gap between Euclidean and manifold trajectory optimization.

Findings

The proposed method achieves theoretical guarantees similar to Euclidean SCP.

Numerical experiments demonstrate the practical effectiveness of the manifold SCP approach.

The approach enables trajectory optimization with manifold constraints like loop closure.

Abstract

Sequential Convex Programming (SCP) has recently gained popularity as a tool for trajectory optimization due to its sound theoretical properties and practical performance. Yet, most SCP-based methods for trajectory optimization are restricted to Euclidean settings, which precludes their application to problem instances where one must reason about manifold-type constraints (that is, constraints, such as loop closure, which restrict the motion of a system to a subset of the ambient space). The aim of this paper is to fill this gap by extending SCP-based trajectory optimization methods to a manifold setting. The key insight is to leverage geometric embeddings to lift a manifold-constrained trajectory optimization problem into an equivalent problem defined over a space enjoying a Euclidean structure. This insight allows one to extend existing SCP methods to a manifold setting in a fairly natural way. In particular, we present a SCP algorithm for manifold problems with refined theoretical guarantees that resemble those derived for the Euclidean setting, and demonstrate its practical performance via numerical experiments.

Tables

Table 1. TABLE I: Averaged results of manipulator arm experiments, with normalized cost, computation times and non-normalized values for constraint errors.

	\addstackgap[4pt]\CenterstackE-SCP	\addstackgap[4pt]\CenterstackSCP	\addstackgap[4pt]\CenterstackTrajOpt	\addstackgap[4pt]\CenterstackTrajOpt-P
\addstackgap[4pt]\CenterstackDynamical constraint error	\addstackgap[4pt]\Centerstack$1.9$$\times {10}^{-3}$	\addstackgap[4pt]\Centerstack$5.8$$\times {10}^{-3}$	\addstackgap[4pt]\Centerstack$8.5$$\times {10}^{-3}$	\addstackgap[4pt]\Centerstack$4.8$$\times {10}^{-3}$
\addstackgap[2pt]\Centerstack${𝕋}^7$ manifold constraint error	\addstackgap[4pt]\Centerstack$3.9$$\times {10}^{-2}$	\addstackgap[4pt]\Centerstack$3.6$$\times {10}^{-6}$	\addstackgap[4pt]\Centerstack$1.6$$\times {10}^{-2}$	\addstackgap[4pt]\Centerstack$3.1$$\times {10}^{-6}$
\addstackgap[2pt]\CenterstackTrue cost	0.105	1.0	0.215	0.792
\addstackgap[2pt]\CenterstackComputation time	0.466	0.129	0.321	1.0

Table 2. TABLE II: Averaged results of experiments using a shooting method to accelerate the convergence of E-SCP, while resulting in lower trajectory cost than using E-SCP alone.

	\addstackgap[4pt]\CenterstackE-SCP Only	\addstackgap[4pt]\CenterstackE-SCP + Shooting
\addstackgap[2pt]SCP Iterations	32	13
\addstackgap[2pt]Reported Cost	0.221	0.176

Equations

$$\dot{v}(t)=\Gamma (v(t))+\sum _{i=1}^m{u}_i(t)\text{vlft}(F_i)(v(t)),v\in TG,$$

$$\dot{x}=f(x,u),x\in {\mathbb{R}}^N\text{with the additional constraint}x\in M$$

$$\{\begin{array}{l}\dot{x}(t)=f(x(t),u(t))=f_0(x(t))+\sum _{j=1}^m{u}^j(t)f_j(x(t))\text{vskip}6.0ptplus2.0ptminus2.0pt\\ x(0)={\bar{x}}_0,g^i(x(t_i))=0,i=1,\dots ,\ell ,t\in [0,t_{\ell }],\end{array}$$

$$\begin{array}{rl}J(t_{\ell },& x,u)={\int }_0^{t_{\ell }}{f}^0(x(t),u(t))\mathrm{d}t=\\ & {\int }_0^{t_{\ell }}\left(\Vert u(t){\Vert }_R^2+u(t)\cdot f_u^0(x(t))+g(x(t))\right)\mathrm{d}t\end{array}$$

$$\displaystyle\textnormal{({Ex})}\ \begin{cases}\displaystyle\quad\min\ \int^{t_{\ell}}_{0}\Big{(}\|u_{1}(t)\|^{2}+\|u_{2}(t)\|^{2}+g(r(t),q(t))\Big{)}\;\mathrm{d}t\vskip 12.0pt plus 4.0pt minus 4.0pt\\ \displaystyle\ \dot{r}=v\ ,\ \dot{v}=\frac{u_{1}}{m}\ ,\ \dot{q}=\frac{1}{2}\Omega(w)q\ ,\ \dot{w}=J^{-1}(u_{2}-w\times Jw)\vskip 6.0pt plus 2.0pt minus 2.0pt\\ \ (r,v,q,w)(0)=\bar{x}_{0}\ ,\ (r,v,q,w)(t_{\ell})=\bar{x}_{\ell}\ ,\ (u_{1},u_{2})\in U\vskip 6.0pt plus 2.0pt minus 2.0pt\\ \ (r,v,q,w)\in\mathbb{R}^{6}\times S^{3}\times\mathbb{R}^{3}\ ,\ S^{3}=\{q\in\mathbb{R}^{4}:\|q\|=1\}\end{cases}$$

$$\{\begin{array}{l}\dot{x}(t)=F(x(t),u(t))=F_0(x(t))+\sum _{j=1}^m{u}^j(t)F_j(x(t))\text{vskip}6.0ptplus2.0ptminus2.0pt\\ x(0)={\bar{x}}_0\in M\subseteq {\mathbb{R}}^N,G^i(x(t_i))=0,i=1,\dots ,\ell .\end{array}$$

$$\begin{array}{rl}J(t_{\ell },& x,u)={\int }_0^{t_{\ell }}{F}^0(x(t),u(t))\mathrm{d}t=\\ & {\int }_0^{t_{\ell }}\left(\Vert u(t){\Vert }_R^2+u(t)\cdot F_u^0(x(t))+G(x(t))\right)\mathrm{d}t\end{array}$$

$$\displaystyle S^{3}\times\mathbb{R}^{3}\subseteq\mathbb{R}^{7}\rightarrow T(S^{3}\times\mathbb{R}^{3}):(q,w)\mapsto\Big{(}\frac{1}{2}\Omega(w)q,J^{-1}(u_{2}-w\times Jw)\Big{)},$$

$$J_{k+1}(t_{\ell},x,u)=\int_{0}^{t_{\ell}}F^{0}_{k+1}(t,x(t),u(t))\;\mathrm{d}t=\vskip 6.0pt plus 2.0pt minus 2.0pt\\ \int_{0}^{t_{\ell}}\Big{(}\|u(t)\|^{2}_{R}+h_{k}(\|x(t)-x_{k}(t)\|^{2}-\Delta_{k})\Big{)}\;\mathrm{d}t\\ +\int_{0}^{t_{\ell}}u(t)\cdot\left(F^{0}_{u}(x_{k}(t))+\frac{\partial F^{0}_{u}}{\partial x}(x_{k}(t))\cdot(x(t)-x_{k}(t))\right)\;\mathrm{d}t\\ +\int_{0}^{t_{\ell}}\left(G_{k}(x_{k}(t))+\frac{\partial G_{k}}{\partial x}(x_{k}(t))\cdot(x(t)-x_{k}(t))\right)\;\mathrm{d}t$$

$$\{\begin{array}{l}\dot{x}(t)=F_{k+1}(t,x(t),u(t))=\text{vskip}6.0ptplus2.0ptminus2.0pt\\ \left(F_0(x_k(t))+\sum _{j=1}^m{u}^j(t)F_j(x_k(t))\right)\text{vskip}6.0ptplus2.0ptminus2.0pt\\ +\left(\frac{\partial F_0}{\partial x}(x_k(t))+\sum _{j=1}^m{u}_k^j(t)\frac{\partial F_j}{\partial x}(x_k(t))\right)\cdot (x(t)-x_k(t))\text{vskip}6.0ptplus2.0ptminus2.0pt\\ x(0)={\bar{x}}_0\in M,G_{k+1}^i(x(t_i))=G^i(x_k(t_i))+\text{vskip}6.0ptplus2.0ptminus2.0pt\\ \frac{\partial G^i}{\partial x}(x_k(t_i))\cdot (x(t_i)-x_k(t_i))=0,i=1,\dots ,\ell ,\end{array}$$

$$\{\begin{array}{l}{\dot{x}}^0(t)=f^0(t,x(t),u(t)),\dot{x}(t)=f(t,x(t),u(t))\text{vskip}6.0ptplus2.0ptminus2.0pt\\ (x^0,x)(t_0)=(0,y_0),(x^0,x)(t)\in \mathbb{R}\times M\end{array}$$

$$\begin{gathered} H:\mathbb{R}\times T^{\ast }(\mathbb{R}\times M)\times {\mathbb{R}}^m\to \mathbb{R} \\ (t,p^0,p,u)\mapsto ⟨p,f(t,\pi (p),u)⟩+p^0{f}^0(t,\pi (p),u) \end{gathered}$$

$$\frac{d(p^0,p)}{dt}(t)=\overrightarrow{H}(t,p^0,p(t),u(t))$$

$$H(t,p^0,p(t),u(t))=\underset{u\in U}{\max }H(t,p^0,p(t),u)$$

$$p(t_i^{-})-p(t_i^{+})\perp \text{ker}\frac{\partial g^i}{\partial x}(x(t_i)),p(t_{\ell })\perp \text{ker}\frac{\partial g^{\ell }}{\partial x}(x(t_{\ell })).$$

$$\Big{(}(x_{0},u_{0}),(x_{1},u_{1}),\dots,(x_{k-1},u_{k-1}),(x_{k},u_{k}),(x_{k},u_{k}),(x_{k},u_{k}),\dots\Big{)}$$

$$f_i:{\mathbb{T}}^1\times \mathbb{R}\to T{\mathbb{T}}^1:(x_i,y_i,u_i)\mapsto (-y_i{u}_i,x_i{u}_i)$$

$$\{\begin{array}{l}\dot{y}(t)=\frac{\partial H}{\partial \xi }(t,p^0,y(t),\xi (t),u(t))\text{vskip}6.0ptplus2.0ptminus2.0pt\\ \dot{\xi }(t)=-\frac{\partial H}{\partial y}(t,p^0,y(t),\xi (t),u(t)).\end{array}$$

$$\begin{array}{rl}& a_{k+1}(s)=x_{k+1}(t_{1}s),b_{k+1}(s)=x_{k+1}((t_2-t_1)s+t_1)\\ & v_{k+1}(s)=u_{k+1}(t_{1}s),w_{k+1}(s)=u_{k+1}((t_2-t_1)s+t_1).\end{array}$$

$$\displaystyle\begin{cases}\displaystyle\min\ \int^{1}_{0}\Big{(}t_{1}F^{0}_{k+1}(t_{1}s,a(s),v(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\\ \hskip 45.0pt(t_{2}-t_{1})F^{0}_{k+1}((t_{2}-t_{1})s+t_{1},b(s),w(s))\Big{)}\;\mathrm{d}s\vskip 12.0pt plus 4.0pt minus 4.0pt\\ a^{\prime}(s)=t_{1}F_{k+1}(t_{1}s,a(s),v(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\ ,\ s\in[0,1]\\ b^{\prime}(s)=(t_{2}-t_{1})F_{k+1}((t_{2}-t_{1})s+t_{1},b(s),w(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\\ a(0)=\bar{x}_{0}\ ,\ G^{1}_{k+1}(a(1))=0\ ,\ G^{2}_{k+1}(b(1))=0\vskip 6.0pt plus 2.0pt minus 2.0pt\\ a(1)-b(0)=0\quad,\quad v(s)\ ,\ w(s)\in U\ ,\ s\in[0,1]\end{cases}$$

$$\zeta^{\prime}_{k+1}(s)=-t_{1}\bigg{(}\zeta^{\top}_{k+1}(s)\frac{\partial F_{k+1}}{\partial x}(t_{1}s,a_{k+1}(s),v_{k+1}(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +p^{0}_{k+1}\frac{\partial F^{0}_{k+1}}{\partial x}(t_{1}s,a_{k+1}(s),v_{k+1}(s))\bigg{)}$$

$$\eta^{\prime}_{k+1}(s)=-(t_{2}-t_{1})\bigg{(}\vskip 6.0pt plus 2.0pt minus 2.0pt\\ \eta^{\top}_{k+1}(s)\frac{\partial F_{k+1}}{\partial x}((t_{2}-t_{1})s+t_{1},b_{k+1}(s),w_{k+1}(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +p^{0}_{k+1}\frac{\partial F^{0}_{k+1}}{\partial x}((t_{2}-t_{1})s+t_{1},b_{k+1}(s),w_{k+1}(s))\bigg{)}$$

$$t_{1}\Big{(}\zeta_{k+1}(s)\cdot F_{k+1}(t_{1}s,a_{k+1}(s),v_{k+1}(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +p^{0}_{k+1}F^{0}_{k+1}(t_{1}s,a_{k+1}(s),v_{k+1}(s))\Big{)}\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +(t_{2}-t_{1})\Big{(}\eta_{k+1}(s)\cdot F_{k+1}((t_{2}-t_{1})s+t_{1},b_{k+1}(s),w_{k+1}(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +p^{0}_{k+1}F^{0}_{k+1}((t_{2}-t_{1})s+t_{1},b_{k+1}(s),w_{k+1}(s))\Big{)}\vskip 6.0pt plus 2.0pt minus 2.0pt\\ =\underset{(v,w)\in U^{2}}{\max}\ \bigg{(}t_{1}\Big{(}\zeta_{k+1}(s)\cdot F_{k+1}(t_{1}s,a_{k+1}(s),v)\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +p^{0}_{k+1}F^{0}_{k+1}(t_{1}s,a_{k+1}(s),v)\Big{)}\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +(t_{2}-t_{1})\Big{(}\eta_{k+1}(s)\cdot F_{k+1}((t_{2}-t_{1})s+t_{1},b_{k+1}(s),w)\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +p^{0}_{k+1}F^{0}_{k+1}((t_{2}-t_{1})s+t_{1},b_{k+1}(s),w)\Big{)}\bigg{)}$$

$$\begin{array}{rl}{\zeta }_{k+1}(1)-& {\eta }_{k+1}(0)\perp \text{ker}\frac{\partial G_{k+1}^1}{\partial x}(a_{k+1}(1))\text{vskip}6.0ptplus2.0ptminus2.0pt\\ & {\eta }_{k+1}(1)\perp \text{ker}\frac{\partial G_{k+1}^2}{\partial x}(b_{k+1}(1))\end{array}$$

$$\displaystyle\begin{cases}\displaystyle\min\ \int^{1}_{0}\Big{(}t_{1}F^{0}(a(s),v(s))+(t_{2}-t_{1})F^{0}(b(s),w(s))\Big{)}\;\mathrm{d}s\vskip 12.0pt plus 4.0pt minus 4.0pt\\ a^{\prime}(s)=t_{1}F(a(s),v(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\ ,\ s\in[0,1]\\ b^{\prime}(s)=(t_{2}-t_{1})F(b(s),w(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\\ a(0)=\bar{x}_{0}\ ,\ G^{1}(a(1))=0\ ,\ G^{2}(b(1))=0\vskip 6.0pt plus 2.0pt minus 2.0pt\\ a(1)-b(0)=0\quad,\quad v(s)\ ,\ w(s)\in U\ ,\ s\in[0,1]\end{cases}$$

$$\tilde{\zeta}^{\prime}(s)=-t_{1}\bigg{(}\tilde{\zeta}^{\top}(s)\frac{\partial F}{\partial x}(\tilde{a}(s),\tilde{v}(s))+\tilde{p}^{0}\frac{\partial F^{0}}{\partial x}(\tilde{a}(s),\tilde{v}(s))\bigg{)}$$

$$\tilde{\eta}^{\prime}(s)=-(t_{2}-t_{1})\bigg{(}\tilde{\eta}^{\top}(s)\frac{\partial F}{\partial x}(\tilde{b}(s),\tilde{w}(s))\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +\tilde{p}^{0}\frac{\partial F^{0}}{\partial x}(\tilde{b}(s),\tilde{w}(s))\bigg{)}$$

$$t_{1}\Big{(}\tilde{\zeta}(s)\cdot F(\tilde{a}(s),\tilde{v}(s))+\tilde{p}^{0}F^{0}(\tilde{a}(s),\tilde{v}(s))\Big{)}\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +(t_{2}-t_{1})\Big{(}\tilde{\eta}(s)\cdot F(\tilde{b}(s),\tilde{w}(s))+\tilde{p}^{0}F^{0}(\tilde{b}(s),\tilde{w}(s))\Big{)}\vskip 6.0pt plus 2.0pt minus 2.0pt\\ =\underset{(v,w)\in U^{2}}{\max}\ \bigg{(}t_{1}\Big{(}\tilde{\zeta}(s)\cdot F(\tilde{a}(s),v)+\tilde{p}^{0}F^{0}(\tilde{a}(s),v)\Big{)}\vskip 6.0pt plus 2.0pt minus 2.0pt\\ +(t_{2}-t_{1})\Big{(}\tilde{\eta}(s)\cdot F(\tilde{b}(s),w)+\tilde{p}^{0}F^{0}(\tilde{b}(s),w)\Big{)}\bigg{)}$$

$$\tilde{\zeta }(1)-\tilde{\eta }(0)\perp \text{ker}\frac{\partial G^1}{\partial x}(\tilde{a}(1)),\tilde{\eta }(1)\perp \text{ker}\frac{\partial G^2}{\partial x}(\tilde{b}(1)).$$

$$\{\begin{array}{l}\bullet p_k^0\to {\tilde{p}}^0\\ \bullet (a_k,b_k)\to (\tilde{a},\tilde{b})\text{, for the strong topology of}{C}^0\\ \bullet ({\zeta }_k,{\eta }_k)\to (\tilde{\zeta },\tilde{\eta })\text{, for the strong topology of}{C}^0\\ \bullet (v_k,w_k)\to (\tilde{v},\tilde{w})\text{, for the weak topology of}{L}^2.\end{array}$$

Full text

I Introduction

Trajectory optimization is a key problem in robotics, and it has thus been studied extensively through a variety of mathematical frameworks. Examples include sampling-based motion planning techniques [16, 18, 19, 21, 22], variational approaches such as CHOMP and STOMP [33, 17], sum-of-squares methods [27, 38], and sequential convex programming (SCP) techniques such as TrajOpt and GuSTO [6, 26, 28, 36]. Most of these methods, however, are restricted to Euclidean settings, which precludes their application (at least directly) to problem instances where one needs to reason about manifold-type constraints. For example, such constraints arise when the motion of a robotic system is forced to evolve on subsets of the ambient space (e.g., due to the presence of closed kinematic chains giving rise to loop closure constraints [15]), which are mathematically modeled as manifolds. Systems having such constraints include quadrotors [2, 29], robots with camera orientation constraints [37, 43], manipulator systems [20, 31] and robotic spacecraft [30, 41], to name a few. For such systems, trajectory optimization methods must ensure that the computed trajectories lie on the relevant manifolds, preventing the planning of infeasible motions. However, this is in general challenging, as manifold-type constraints are often defined only locally (i.e., through local charts) or as implicit constraints (i.e., constraints of the type $E(x)=0$ where $E$ is a submersion and $x$ is the state vector). As a pedagogical example, consider a two-joint manipulator. Its motion is forced to evolve on the torus $\mathbb{T}^{2}$ (see Figure 2), which represents a two-dimensional submanifold of $\mathbb{R}^{3}$. Specifically, each joint variable ($\theta_{i}$, $i=1,2$) evolves on the unitary circle $S^{1}$, and thus the combined evolution is on the Cartesian product $S^{1}\times S^{1}$, which is diffeomorphic to $\mathbb{T}^{2}$ (as characterized by implicit and nonlinear equality constraints).

Despite the ubiquitous presence of manifold constraints in robotic applications, the set of trajectory optimization tools that handle such constraints is relatively limited. The most naïve technique consists of simply removing, without principled justification, all manifold-type constraints, and then solving a relaxed version of the original problem in the resulting Euclidean space. Since this approach cannot guarantee trajectory feasibility, one needs to resort to post-processing before trajectory execution, often using a heuristic correction step which may be unsuccessful. This has prompted the design of optimization approaches that explicitly account for the presence of manifold constraints, including sampling-based techniques leveraging local chart analysis, [39, 15], methods employing global chart-gluing procedures [43], and methods exploiting properties of specific types of manifolds (in particular, Lie groups), such as invariant metrics [42] and projection operators [34, 35]. These methods, while directly accounting for the presence of manifold constraints, do not in general enjoy theoretical guarantees, and they only consider a subset of the typical constraints arising in robotic applications (e.g., control or goal region constraints are generally not addressed).

Building on the recent success of SCP-based techniques for trajectory optimization, the aim of this paper is to provide an SCP-based framework for trajectory optimization on manifolds that (1) enjoys theoretical guarantees in terms of convergence, (2) is general, in that it accounts for a vast class of constraints arising in robotic applications (possibly implicitly defined), and (3) provides effective and reliable practical performance. Specifically, SCP entails successively convexifying the cost function and constraints of a nonconvex optimal control problem, seeking a solution to the original problem through a sequence of convex problems. Its attractiveness is due to high computational speed [2, 36], broad applicability [26, 41], and (continuous-time) theoretical guarantees [6, 28]. Extending SCP-based methods, primarily developed for Euclidean settings, to manifold-constrained problems is, however, challenging. In particular, when dealing with manifolds, it is challenging to make linearizations (required by SCP schemes that operate on dynamics) well-posed [8]. The key technical idea of this paper is to leverage geometric embeddings – that is, mappings that allow one to recover manifolds as subsets of Euclidean spaces. Leveraging embeddings provides four main advantages. First, it allows one to lift a manifold-constrained problem into an equivalent problem defined over a space enjoying Euclidean structure, where linearizations can be easily computed. Second, embedded problems are often easier to address than their counterparts in local coordinates since, for example, linearity can be partially maintained. Third, embeddings provide a pathway to address implicitly-defined manifolds, as the equality constraints defining them are automatically satisfied in the lifted Euclidean space without the need for explicit enforcement. Fourth, and crucially, for dynamical systems evolving on Lie groups (as it is the case for virtually all robotic systems), there is always a “natural” embedding that can, at least in principle, be leveraged. Indeed, any mechanical system can always be identified with a subgroup $G\subseteq\mathbb{R}^{n\times n}$ of a Cartesian product of Lie groups of matrices. Then, the first-order equation that governs the dynamical evolution of the system is

[TABLE]

where $\Gamma$ is the geodesic spray induced by the kinetic energy and $\textnormal{vlft}(F_{i})$ is the vertical lift of the generalized force $F_{i}$ [7]. This can be reinterpreted as a first-order, control-affine equation with a drift term on the space $G\times\mathfrak{g}$ via the identification $TG\cong G\times\mathfrak{g}$ [7], where $\mathfrak{g}$ denotes the Lie algebra of $G$. A natural embedding is then the inclusion $G\times\mathfrak{g}\subseteq\mathbb{R}^{2n^{2}}$ (we provide explicit examples in the rest of the paper).

Statement of Contributions: In this paper, we leverage geometric embeddings to extend SCP-based methods for trajectory optimization to manifold-constrained problems. Specifically, the contribution of this paper is fourfold. First, we introduce the notion of embedded SCP, a trajectory optimization method that exploits geometric embeddings to recast optimization on manifolds as a sequence of convex optimal control problems within Euclidean spaces. Importantly, a large number of trajectory optimization problems can be “naturally” (in the sense above) embedded in Euclidean spaces, which makes this step generally straightforward. Second, leveraging such a reformulation and extending recent results on theoretical guarantees for SCP-based methods in Euclidean spaces [6], we provide convergence guarantees for embedded SCP in the sense of the geometric Pontryagin Maximum Principle (PMP) [32], i.e., in the sense of convergence of both the solution and corresponding Lagrange multipliers to stationary points satisfying necessary conditions for optimality and complying with the structure of the manifold characterizing the problem. In particular, a key aspect of our theoretical analysis entails showing how one can avoid manifold-type constraints in the optimization process, and yet can still guarantee that the computed solution lies on the manifold – thus providing a computationally efficient pathway to deal with implicit manifold constraints. Third, by merging techniques from indirect optimal control and differential geometry, we extend the theoretical results to a large variety of settings, e.g., goal region constraints and pointwise state constraints arising in multitask scenarios. Fourth, again inspired by analogue results in the Euclidean setting [6], we harness the insights gained through our theoretical analysis to develop a convergence acceleration scheme for trajectory optimization on manifolds based on shooting methods [3].

One must note that while geometric embeddings allow a principled and systematic development of SCP methods for manifold-constrained problems, they come with two key drawbacks. First, embedding the manifold into an Euclidean space requires solving an optimization problem on a space having a higher dimension than the original manifold (albeit of simpler structure). Nevertheless, SCP-based algorithms scale rather well with problem dimensionality, and thus this drawback is offset by the simplification in the problem structure. Second, a straightforward and simple leveraging of embeddings is possible only if globally defined dynamical equations are at our disposal, i.e., it is easy to write

[TABLE]

for some $N\in\mathbb{N}$, where $M$ is a $n$-dimensional manifold. Indeed, though recovering the expression above is always theoretically possible thanks to (1), it could be hard to practically describe complex dynamics as in (2) (for example, for second-order dynamics that are defined only by local coordinates). Still, most of the systems commonly used in robotics are in the form provided by (2), which makes possible to efficiently put in practice the method developed in this paper.

Structure of the Paper: In Section II, we define the problem of trajectory optimization on manifolds and introduce the tool of embeddings that allows us to reformulate the problem in a standard Euclidean space on which we can proceed by linearization. In Section III, we introduce the embedding trajectory optimization algorithm and the primary theoretical contributions that are geometrically consistent with the structure of the manifold. Finally, Sections V and VI present experiments, conclusions and future extensions for this work.

Notation: We denote by $M$ an $n$-dimensional manifold with tangent bundle $TM$ and cotangent bundle $T^{*}M$ (both manifolds of dimension $2n$). The tangent space of $M$ at $x\in M$ will be denoted by $T_{x}M$. Moreover, we recall that a smooth vector field on $M$ is a mapping $f:M\rightarrow TM$ such that $f(x)\in T_{x}M$, for every $x\in M$. The interested reader is referred to [24] for related concepts in differential geometry.

II Problem Formulation and

Sequential Convex Programming on Manifolds

We begin in Section II-A by formulating trajectory optimization for dynamical systems as an optimal control problem on manifolds. Then, in Section II-B, we introduce a procedure for appropriately embedding the original problem on manifolds into an Euclidean space. This allows us to exploit classical SCP frameworks for trajectory optimization in Euclidean spaces to solve the problem in Section II-C.

II-A Trajectory Optimization on Generic Manifolds

In this paper we consider a continuous-time formulation to ensure that the theoretical guarantees we derive are independent of the discretization scheme that is employed. A discussion about the impact of discretization schemes on the proposed methodology will be discussed in Section III-D.

Specifically, consider an initial point $\bar{x}_{0}\in M$ and smooth mappings $g^{i}:M\rightarrow\mathbb{R}^{r_{i}}$, $i=1,\dots,\ell$, which are submersions at 0. Here, $g^{i}$ represent pointwise state constraints that are used to mathematically model multitask scenarios; in particular, $g^{\ell}$ represents goal region constraints. Without loss of generality, we require $\textnormal{dist}(\bar{x}_{0},(g^{\ell})^{-1}(0))>0$, where dist is a point-set distance evaluated w.r.t. some Riemannian metric on $M$. For times $0<t_{1}<\dots<t_{\ell}$, we model the dynamical evolution of the system by the following drift control-affine system in $M$

[TABLE]

where $f_{j}:M\rightarrow TM$, $j=0,\dots,m$ are $C^{1}$ vector fields. The pointwise state constraints $g^{i}(x(t_{i}))=0$ are useful in multitask scenarios where one seeks to jointly optimize subtrajectories connecting different waypoints. We emphasize that, as previously mentioned, the dynamics of every mechanical system can be written as in Eq. (3) by substituting the manifold $M$ with its tangent bundle $TM$.

We pose trajectory optimization as an optimal control problem with penalized state constraints. Specifically, we define the Optimal Control Problem (OCP) as minimizing the integral cost

[TABLE]

under dynamics and pointwise constraints (3), among all control trajectories $u\in L^{\infty}([0,t_{\ell}],\,\mathbb{R}^{m})$ satisfying $u(t)\in U$ almost everywhere in $[0,t_{\ell}]$, where the measurable set $U\subseteq\mathbb{R}^{m}$ represents control constraints. Here, $f^{0}_{u}:M\rightarrow\mathbb{R}^{m}$, $g:M\rightarrow\mathbb{R}$ are $C^{1}$, $\|\cdot\|_{R}$ is the weighted norm defined by a constant positive-definite matrix $R\in\mathbb{R}^{m\times m}$, and times $t_{i}$, $i=1,\dots,{\ell}$ are fixed. We remark that hard enforcement of dynamical and intermediate/final goal set constraints is naturally imposed by (3). The function $g=g_{a}+\omega g_{b}$ accumulates contributions from a purely state-dependant cost $g_{a}$ and state constraint penalty function $g_{b}$ (e.g., stemming from collision-avoidance constraints), weighted by $\omega\geq 1$. Penalizing state constraints (e.g., collision avoidance) provides both theoretical and numerical benefits: it allows us to obtain theoretical guarantees in the sense of the Pontryagin Maximum Principle [32, 1, 10], necessary conditions for optimality that are stronger than standard Lagrange multiplier rules (see also Theorem 3 below), and it provides numerical flexibility by often allowing simple trajectories that violate constraints such as obstacle avoidance to be exploited for initialization. Indeed, given correct design of an SCP algorithm, we can still guarantee that returned solutions satisfy state constraints up to a user-defined tolerance.

A representative example (that will serve as running example) of a trajectory optimization problem evolving on manifolds is the minimum-energy optimal control of a spacecraft avoiding collisions in a microgravity environment, which can be stated as (under the previous formalism, $g$ provides state constraints):

[TABLE]

where $r$ is the position of the vehicle, $v$ its tangential velocity, $q$ its orientation (expressed via quaternions), $w$ its angular velocity ($\Omega(w)$ is the usual skew-symmetric matrix depending on $w$) and the manifold $S^{3}\subseteq\mathbb{R}^{4}$ characterizes quaternions. Controls are represented by the thrust $u_{1}$ and the torque $u_{2}$. A naïve way to approach our running example (Ex) would entail removing, without principled justification, the constraint $q\in S^{3}$, and then solving the relaxed problem in the resulting Euclidean space – this could result, however, in computation of infeasible trajectories. Better justified approaches could exploit local charts or Lie group properties, as mentioned in Section I. However, in what follows, we demonstrate another method to tackle the implicit manifold constraint $q\in S^{3}$, that hinges on embeddings and provides a way to lift SCP methods to manifold-constrained problems.

II-B Embedding the Problem into the Euclidean Space

We would like to solve (OCP) via SCP, i.e., by an iterative procedure based on the linearization of all nonlinear mappings around the solution at the previous iteration. This requires us to compose a notion of linearized vector fields of $M$ around curves. In [40, 25, 8], the authors adapt such a definition on manifolds by recasting differential equations as algebraic equations of operators in $TM$. However, in many applications concerning dynamical systems, $M$ naturally appears as subset of the Euclidean space, in which case the most intuitive linearization is the one operating in the ambient Euclidean space. This insight motivates our approach: to recast (OCP) into an appropriate Euclidean space via geometric embeddings, i.e., mappings $e:M\rightarrow\mathbb{R}^{N}$ for $N\in\mathbb{N}$, and then to linearize in the ambient space.

Following the previous discussion, we assume that $M$ is a closed submanifold of $\mathbb{R}^{N}$, for some $N\in\mathbb{N}$. This means that we fix a particular embedding, which is given by the canonical inclusion $e:M\rightarrow\mathbb{R}^{N}:x\mapsto x$. This choice is made without loss of generality because, due to Whitney-type theorems [24], such a mapping always exists. Moreover, consistent with the previous motivating discussion, we assume that the mappings defining (OCP) naturally extend to $\mathbb{R}^{N}$, i.e., there exist $C^{1}$ vector fields $F_{j}:\mathbb{R}^{N}\rightarrow T\mathbb{R}^{N}\cong\mathbb{R}^{N}$, $j=1,\dots,m$ (with an abuse of notation, see [24]), and $C^{1}$ functions $F^{0}_{u}:\mathbb{R}^{N}\rightarrow\mathbb{R}$, $G:\mathbb{R}^{N}\rightarrow\mathbb{R}$ and $G^{i}:\mathbb{R}^{N}\rightarrow\mathbb{R}$, $i=1,\dots,{\ell}$, that are smooth submersions at 0, such that $F_{j}|_{M}=f_{j}$, $G^{i}|_{M}=g^{i}$, $F^{0}_{u}|_{M}=f^{0}_{u}$ and $G|_{M}=g$. This setup allows us to transform the dynamics (3) into the following drift control-affine system in $\mathbb{R}^{N}$:

[TABLE]

Therefore, (OCP) can be embedded in $\mathbb{R}^{N}$ by considering the following Embedded Optimal Control Problem (EOCP), which consists of minimizing the integral cost

[TABLE]

under dynamics (5), among all control trajectories $u\in L^{\infty}([0,t_{\ell}],\,\mathbb{R}^{m})$ satisfying $u(t)\in U$ almost everywhere in $[0,t_{\ell}]$. At this step, it is worth noting that this embedding approach is justified only if solving (EOCP) is equivalent to solve (OCP). Fortunately, this is actually the case: every couple $(x,u)$ is optimal for (OCP) if and only if it is optimal for the embedded problem (EOCP). The validity of the whole scheme hinges on this crucial remark, which is summarized in the statement below,

Lemma 1** (Embedding Lemma).**

A tuple $(x,u)$ is optimal for (EOCP) if and only if it is optimal for (OCP).

Proof.

The proof makes use of standard tools in differential geometry, whose definitions can be found in [24] and are omitted here due to space limitations. We retrace the main steps of the proof. Denote by $e:M\rightarrow\mathbb{R}^{N}$ the canonical inclusion. It follows that $F$ is $e$-correlated to $f$, which implies their flows satisfy $\Phi_{F}(t,x)=\Phi_{F}(t,e(x))=e(\Phi_{f}(t,x))=\Phi_{f}(t,x)$ for every $(t,x)\in\mathbb{R}\times M$ for which the flow $\Phi_{f}$ is defined. Since $\bar{x}_{0}\in M$, we obtain that $(x,u)$ satisfies the dynamics in (5) if and only if it satisfies the embedded dynamics in (3). From this, the optimality of $(x,u)$ follows from the similarity between (4) and (6). ∎

Remark 1**.**

Crucially, from Lemma 1, we see that the satisfaction of implicit manifold-type constraints for (EOCP) is induced by hard enforcement of dynamical constraints. Therefore, any numerical strategy used to solve (EOCP) must provide hard enforcement of dynamics – otherwise, the solution trajectory is not guaranteed to lie on the manifold!

Let us show how this embedding framework applies to our running example (Ex). It is sufficient to note that the only components of the dynamics evolving on a manifold are given by the mapping

[TABLE]

and that this mapping is also defined when $(q,w)\in\mathbb{R}^{7}$. In other words, the original dynamics is equivalent to \Big{(}\frac{1}{2}\Omega(w)q,J^{-1}(u_{2}-w\times Jw)\Big{)} restricted to the subset $S^{3}\times\mathbb{R}^{3}$. Therefore, the embedded dynamics related to this mapping are exactly the same but extended on $\mathbb{R}^{7}$, which shows that the embedded version of our example problem coincides with the original (OCP). Luckily, for many robotics applications which include trajectory optimization on manifolds, (EOCP) is equivalent to (OCP), which is also the case when formulation (1) is met. This is among the main motivations for developing such an embedded framework (see also our discussion at the end of Section II-A).

II-C Reformulating Problem (OCP) via SCP in Euclidean Space

Given that (EOCP) evolves in the Euclidean space, we may solve it using SCP. Below, we describe a particular SCP formulation that enjoys geometrically consistent theoretical convergence guarantees.

Under the assumption that $U$ is convex, we iteratively linearize the nonlinear contributions of (EOCP) around local solutions, thus recursively defining a sequence of simplified problems. Specifically, at the end of iteration $k$, assume we have some continuous curves $x_{k}:[0,t_{\ell}]\rightarrow\mathbb{R}^{N}$ and $u_{k}:[0,t_{\ell}]\rightarrow\mathbb{R}^{m}$, continuously extended in the interval $(0,+\infty)$. Then, at iteration $k+1$, the Linearized Embedded Optimal Control Problem (LEOCP)k+1 consists of minimizing the new cost

[TABLE]

where, consistent with the notation of Section II-B, $G_{k}=G_{a}+\omega_{k}G_{b}$ and $h_{k}(s)$ is any smooth approximation of $\max\{0,s\}$ [24, Chapter 10]. Function $h_{k}$ provides trust-region guarantees on the updates in the state trajectories and constraints via the bounds $0\leq\Delta_{k}\leq\Delta_{0}$ and weights $1\leq\omega_{0}\leq\omega_{k}\leq\omega_{\max}$. The dynamical constraint for (LEOCP)k+1 is

[TABLE]

obtained from the linearized expansion of all nonlinear mappings. We minimize among all controls $u\in L^{\infty}([0,t_{\ell}],\,\mathbb{R}^{m})$ satisfying $u(t)\in U$ almost everywhere in $[0,t_{\ell}]$. Inductively, the curves $x_{k+1}:[0,t_{\ell}]\rightarrow\mathbb{R}^{N}$ and $u_{k+1}:[0,t_{\ell}]\rightarrow\mathbb{R}^{m}$ are defined as the optimal solution for problem (LEOCP)k+1, continuously extended in the interval $(0,+\infty)$. Ideally, SCP algorithms may vary $\Delta_{k}$ and $\omega_{k}$ at each iteration to smoothen the process towards convergence, for example as in [6].

A convexified formulation similar to (7)-(8) has already been introduced in [6]. However, we stress the fact that this new formulation deals with the presence of the manifold $M$ and of pointwise state constraints. The introduction of these two new features necessitates a considerable revision of the proof of theoretical guarantees (see the Appendix).

The sequence of problems (LEOCP)k is well-posed if, for each iteration $k\geq 1$, an optimal solution for (LEOCP)k exists. For this, we consider the following assumptions:

• $(A_{1})$

The set $U$ is compact and convex. Moreover, the differentials of mappings $G^{i}$, $i=1,\dots,{\ell}$, are of full rank.

• $(A_{2})$

Mappings $f^{0}$, $g$, vector fields $f_{j}$, $j=0,\dots,m$ and their differentials have compact supports (and do $F^{0}$, $G$, $F_{j}$).

• $(A_{3})$

At every iteration $k\geq 1$, problem (LEOCP)k is feasible.

Under these assumptions, classical existence Filippov-type arguments [12, 23] (applied to the reduced form of (LEOCP)k, see also the Appendix) show that, at each iteration $k\geq 1$, the problem (LEOCP)k has at least one optimal solution. We remark that similar assumptions have been considered in [6]; in the present contribution, $(A_{1})$-$(A_{3})$ gather the assumptions in [6] and appropriately adapt them to the context of manifolds and pointwise state constraints. Comments on their validity for very general trajectory optimization problems are easily adapted from [6, Section II.B].

Coming back to our running example problem (Ex), since we have already proved that the embedded problem coincides with (Ex), the linearization technique above applies directly to (Ex) without any additional step. This is particularly useful and happens every time the embedded problem is equivalent to the original one, which is common in trajectory optimization as highlighted previously. We remark that in nearly all scenarios having natural control constraints $U$, Assumptions $(A_{1})$-$(A_{3})$ are easily satisfied by (Ex).

III Algorithm Overview and Theoretical Guarantees

In Section III-A, we detail a general algorithm for the solution of (OCP) which combines SCP-based procedures with the embedded framework defined previously. Its convergence guarantees, in the sense of the Pontryagin Maximum Principle [32], are studied in Section III-B to III-D, where we show that these respect the original structure of the manifold in (OCP), despite solving a sequence of linearized versions of the embedded problem. Notably, this procedure allows one to solve (OCP) on manifolds defined implicitly via nonlinear equalities without explicit representation.

III-A SCP-based Trajectory Optimization on Manifolds

The first two steps in the algorithm above consist of transforming (OCP) into the optimal control problem (EOCP) on the Euclidean space via embedding procedures and successively linearizing it as detailed in Section II-B. In the third step, one finally applies some SCP scheme on Euclidean spaces. It is important to remark that E-SCP provides the user with the freedom to choose any sequential convex procedure to solve the sequence of problems (LEOCP)k. However, we show in Section III-B that specific choices of SCP (e.g. using hard enforcement of dynamical constraints) provide theoretical guarantees for E-SCP which are also consistent with the presence of the manifold within the original problem (OCP).

Problem (LEOCP)1 is linearized around an initial curve tuple $(x_{0},u_{0})$, where these initialization curves should be as close as possible to a feasible or even optimal curve for (LEOCP)1, although we do not require that $(x_{0},u_{0})$ is feasible for the embedded problem (EOCP). This allows one to initialize E-SCP with simple, even infeasible, guesses for solutions of (EOCP), such as a straight line in the manifold, as detailed in [6, Section III.A].

III-B Necessary Conditions for Optimality

Although the strategy for solving problems (LEOCP)k in E-SCP is up to the user, we show that specific choices of solvers allow one to recover important theoretical guarantees for the convergence of E-SCP to critical points for the original problem (OCP) on manifolds. Specifically, we can show the convergence of E-SCP towards a trajectory satisfying first-order necessary conditions for optimality under the Pontryagin Maximum Principle [32] when the sequence of problems (LEOCP)k is chosen as in Section II-C. However, we must adapt the proof for the classical Euclidean setting to take into account the presence of both manifold and pointwise state constraints. This will be done by leveraging fundamental results from differential geometry and optimal control, i.e., Hamiltonian systems and the Pontryagin Maximum Principle with pointwise state constraints. For self-containtment, we summarize some of these results in the following discussion (see, e.g., [24, 1, 10] for an extended treatment).

For consistency with the existing presentation of these results, denote $f^{0}$ as a time-varying function, i.e., $f^{0}=f^{0}(t,x,u)$, and fix some measurable control time-series $u:\mathbb{R}\rightarrow\mathbb{R}^{m}$. Thanks to Assumptions $(A_{1})$, $(A_{2})$, for every $t_{0}\in\mathbb{R}$, the trajectories arising for the augmented system

[TABLE]

exist in $\mathbb{R}$ for every $y_{0}\in M$. Notice that $x^{0}(t)$ is simply the accumulated cost at time $t$. Therefore, the flow of system (9) is defined for every $(t_{0},y_{0})\in\mathbb{R}\times M$ and we denote it by $\exp_{(f^{0}_{u},f_{u})}(\cdot;\cdot,\cdot):\mathbb{R}^{2}\times M\rightarrow\mathbb{R}\times M$, such that $\exp_{(f^{0}_{u},f_{u})}(\cdot;t_{0},y_{0})$ represents the trajectory of (9) starting from $(0,y_{0})$ at $t_{0}$.

By standard identifications, we denote $(p^{0},p)\in T^{*}(\mathbb{R}\times M)$ and we define the Hamiltonian function related to (9) as

[TABLE]

where $\pi:T^{*}M\rightarrow M$ is the canonical projection and $\langle\cdot,\cdot\rangle$ denotes the duality in $T^{*}M$. As a classical result on Hamiltonian systems [1, 24], for every $(t,u)\in\mathbb{R}^{m+1}$, one can uniquely associate to (10) the so-called Hamiltonian vector field $\overset{\rightarrow}{H}(t,\cdot,\cdot,u):T^{*}(\mathbb{R}\times M)\rightarrow T(T^{*}(\mathbb{R}\times M))$ by the rule $\sigma_{(p^{0},p)}(\cdot,\overset{\rightarrow}{H}(t,\cdot,\cdot,u))=d_{(p^{0},p)}H(t,\cdot,\cdot,u)$, $\sigma$ being the canonical symplectic form of $T^{*}(\mathbb{R}\times M)$ (see, e.g., [1]). Combining the classical geometric Pontryagin Maximum Principle [1] with the reduction scheme for pointwise state constraints developed in [10] yields the following extended geometric Pontryagin Maximum Principle (see Appendix for a proof sketch).

Theorem 2** (Geometric Pontryagin Maximum Principle with Pointwise State Constraints).**

Let $x$ be an optimal trajectory for (OCP), associated with the control $u$ in $[0,t_{\ell}]$ and with fixed interior and final times $t_{i}$, $i=1,\dots,{\ell}$. There exists a nonpositive constant scalar $p^{0}$ and a piecewise absolutely continuous function $p:[0,t_{\ell}]\rightarrow T^{*}M$, called the adjoint vector, satisfying $p(t)\in T^{*}_{x(t)}M$, with $(p^{0},p)\neq 0$, such that, almost everywhere in $[0,t_{\ell}]$, the following relations hold:

• •

Adjoint Equations**

[TABLE]

• •

Maximality Condition**

[TABLE]

• •

Transversality Conditions**

For each $i=1,\dots,{\ell}-1$, the adjoint vector satisfies

[TABLE]

The tuple $(p^{0},p,u)$ is a (Pontryagin) extremal. We say that $(p^{0},p,u)$ is normal if $p^{0}\neq 0$, and abnormal otherwise.

It is important to recall that the statement of optimality in Theorem 2 supersedes the classical Euclidean case as it additionally addresses pointwise state constraints (which consequently imply discontinuity of the adjoint vector), in context of nonlinear manifolds. These features considerably complexify the proof (cf. Appendix).

III-C Convergence with Geometric Consistency

The convergence of E-SCP can be inferred by leveraging one further commonly adopted regularity assumption concerning optimal controls:

• $(A_{4})$

At every iteration $k\geq 1$, the optimal control $u_{k}$ of (LEOCP)k is piecewise continuous in every subinterval $[0,t_{1}]$ and $[t_{i-1},t_{i}]$ for $i=2,\dots,{\ell}$.

The key convergence result is stated next:

Theorem 3** (Convergence Theorem).**

Suppose that $(A_{1})$-$(A_{4})$ hold. Given any sequence of trust region radii and weights $((\Delta_{k},\omega_{k}))_{k\in\mathbb{N}}\subseteq[0,\Delta_{0}]\times[\omega_{0},\omega_{\max}]$, let $((x_{k},u_{k}))_{k\in\mathbb{N}}$ be any sequence such that, for every $k\geq 1$, $(x_{k},u_{k})$ is optimal for (LEOCP)k in $[0,t_{\ell}]$, where problems (LEOCP)k are built as detailed in Section II-C. Up to some subsequence:

• •

$x_{k}\rightarrow\tilde{x}\in C^{0}([0,t_{\ell}],M)$, for the strong topology of $C^{0}$, and

• •

$u_{k}\rightarrow\tilde{u}\in L^{\infty}([0,t_{\ell}],U)$, for the weak topology of $L^{2}$,

where $(\tilde{x},\tilde{u})$ is feasible for the original problem (OCP). Moreover, there exists a nonpositive constant scalar $\tilde{p}^{0}$ and a piecewise absolutely continuous function $\tilde{\gamma}:[0,t_{\ell}]\rightarrow\mathbb{R}^{N}$, with $(\tilde{p}^{0},\tilde{\gamma})\neq 0$, such that the tuple $((\tilde{p}^{0},\tilde{p})=\textnormal{Pr}(\tilde{p}^{0},\tilde{x},\tilde{\gamma}),\tilde{u})$, where Pr is the orthogonal projection of $T^{*}\mathbb{R}^{N+1}|_{\mathbb{R}\times M}$ onto $T^{*}(\mathbb{R}\times M)$ (cf. Appendix), represents a geometric Pontryagin extremal (in the sense of Theorem 2) for the original problem (OCP) on $M$. In particular, as $k$ tends to infinity, up to some subsequence:

• •

$p^{0}_{k}\rightarrow\tilde{p}^{0}$, and

• •

$\gamma_{k}|_{[t_{i},t_{i+1})}\rightarrow\tilde{\gamma}|_{[t_{i},t_{i+1})}\in C^{0}([t_{i},t_{i+1}),\mathbb{R}^{N})$, $i=1,\dots,{\ell}-1$, and $\gamma_{k}|_{[t_{{\ell}-1},t_{\ell}]}\rightarrow\tilde{\gamma}|_{[t_{{\ell}-1},t_{\ell}]}\in C^{0}([t_{{\ell}-1},t_{\ell}],\mathbb{R}^{N})$ for the strong topology of $C^{0}$,

where $(p^{0}_{k},x_{k},\gamma_{k},u_{k})$ is a Pontryagin extremal of (LEOCP)k.

Due to space limitations, we present the proof of Theorem 3 in the Appendix. In short, Theorem 3 asserts that there exists a sequence of solutions for problems (LEOCP)k that converges (under appropriate topologies) to a critical point for (OCP), in the (strong) sense of Theorem 2. Importantly, the limiting trajectory lies on $M$, despite solving the linearized, embedded versions (LEOCP)k (see Figure 2). The termination properties of Algorithm E-SCP are stated next:

Corollary 1** (E-SCP Termination).**

Assume (i) $(A_{1})$-$(A_{4})$ hold, (ii) problems (LEOCP)k are built as detailed in Section II-C, (iii) the SCP procedure adopted for E-SCP (line 6 of Algorithm 1) enforces hard dynamical constraints (as opposed to a penalized implementation – see Remark 1), (iv) $\Delta_{k}\in[0,\Delta_{0}]$ for all $k$ and $\omega_{k}<\omega_{\max}$ are chosen such that for every iteration $k\geq 1$, an output $(x_{k},u_{k})$ is always provided, and (v) the algorithm is terminated if $\omega_{k}>\omega_{\max}$. Then, in solving (OCP) by E-SCP only three mutually exclusive situations arise:

There exists an iteration $k\geq 1$ for which $\omega_{k}>\omega_{\max}$. Then, E-SCP terminates, providing a solution for (LEOCP)k satisfying only soft state constraints. 2. 2.

There exists an iteration $k\geq 0$ for which $(x_{k+1},u_{k+1})=(x_{k},u_{k})$. Then, E-SCP terminates, providing a stationary point, in the sense of the Pontryagin Maximum Principle, for the original problem (OCP). 3. 3.

We have $(x_{k+1},u_{k+1})\neq(x_{k},u_{k})$, for every iteration $k\geq 0$. Then, E-SCP builds a sequence of optimal solutions for (LEOCP)k that has a subsequence converging (with respect to appropriate topologies) to a stationary point, in the sense of the Pontryagin Maximum Principle, for the original problem (OCP).

Proof.

The assumptions on the internal SCP procedure for E-SCP imply that only these three cases may happen and that they are mutually exclusive. Moreover, case 3) is a direct consequence of Theorem 3 and, for case 2), it is sufficient to apply Theorem 3 to the sequence of solutions

[TABLE]

because it clearly converges to $(\tilde{x},\tilde{u})=(x_{k},u_{k})$. ∎

In case 1), SCP fails because no feasible strategies can be computed, which also occurs in other state-of-the-art trajectory optimization solvers such as TrajOpt [36]. Since the convergence of numerical methods usually leverages a termination threshold, case 3) is the most common outcome. Fortunately, Theorem 3 ensures that we are converging to a stationary point that satisfies strong necessary conditions for optimality in the sense of Theorem 2 and manifold-type constraints.

III-D Geometric Consistency and Discrete-Time Convergence

Despite the fact that E-SCP entails solving a sequence of linearized problems for the embedded reformulation (EOCP), i.e., without explicit representation of the manifold, Lemma 1 and Corollary 1 ensure that the numerical solution converges to trajectories that satisfy the manifold constraints and also ensure that the limiting solution satisfies strong first-order necessary conditions for optimality that respect the geometric structure of the original manifold.

The additional advantage of working within a continuous-time setting is that the validity of the theoretical guarantees is independent of the time-discretization scheme used to solve the ODEs. For instance, choosing schemes such as Euler or Simpson’s rule lead to well well-posed convex optimization problems when the dynamics are linearized within each SCP step. With a sufficiently small time-step, one can ensure that the discrete solution provided by SCP stays close to the solution of the continuous-time problem (EOCP). Finally, Corollary 1 further implies that this discretized solution stays close to the solution for the original continuous-time (OCP).

IV Convergence Acceleration via

Differential Shooting Method

An important result provided by Theorem 3 is the convergence of Pontryagin extremals related to the sequence of solutions for problem (LEOCP)k in the Euclidean space towards a Pontryagin extremal related to the solution for (OCP) in the manifold, found by E-SCP. As a consequence, we can extend the acceleration procedure proposed in [6] to the manifold and pointwise state constraint case, i.e., warm-starting shooting methods [3] with E-SCP.

The key idea is that since the convergence of the adjoint vectors is provided in $\mathbb{R}^{N}$, one can leverage them to re-state a shooting method on the manifold within $\mathbb{R}^{N}$. However, unlike the framework proposed in [6], the main difficulty concerns the pointwise state constraints that introduce discontinuities for the multipliers. Fortunately, we can still use this hybrid method by leveraging the knowledge of adjoint vectors at intermediate times. Assuming SCP is converging, the Lagrange multipliers $\lambda^{i}_{k}$ related to the pointwise condition $G^{i}_{k}(x(t_{i}))=0$ for the finite dimensional discretization of problems (LEOCP)k approximate the values $\gamma_{k}(t_{i})$ of the adjoint vectors related to the continuous-time (LEOCP)k (see [13] and the problem reduction provided in the Appendix). Then, up to some subsequence, for every small $\delta>0$, there exists an iteration $k_{\delta}\geq 1$ for which, for every iteration $k\geq k_{\delta}$, one has $\|\tilde{\gamma}(t_{i})-\lambda^{i}_{k}\|<\delta$, $i=1,\dots,{\ell}$, where $\tilde{\gamma}$ is an adjoint vector related to the solution of (OCP) found by SCP (see Theorem 3). This means that, starting from some iteration $k\geq k_{\delta}$, we can run a shooting method to solve (OCP), initializing using $\lambda^{i}_{k}$, $i=1,\dots,{\ell}$. At each iteration of SCP, we use the values $\lambda^{i}_{k}$ provided by the solver to initialize the shooting method until convergence is achieved. This provides a theoretically guaranteed method to accelerate convergence for SCP towards a more accurate solution.

V Numerical Experiments and Discussion

In this section, we provide implementation details and examples to demonstrate various facets of our approach. We focus on three important aspects: (1) providing comparisons between E-SCP and standard SCP on Euclidean spaces, (2) providing comparisons between E-SCP and state-of-the-art algorithms for trajectory optimization, and (3) analyzing convergence acceleration for E-SCP via shooting methods.

Simulations are provided by considering two problems: our running example (Ex), defined in Section II-A, and a trajectory optimization problem for a 7 degree-of-freedom manipulator in a cluttered environment. Consistent with our embedding framework, rather than describing the manipulator via joint angle variables (i.e., local variables), the states are characterized by tuples $(x_{i},y_{i})\in\mathbb{S}^{1}$, $i=1,\dots,7$ (one for each joint), so that the system evolves in the $7$-dimensional torus, i.e., $(x_{1},y_{1},\dots,x_{7},y_{7})\in\mathbb{T}^{7}$, which is naturally embedded in $\mathbb{R}^{14}$. The dynamics are given by the kinematic equations of a manipulator, that is, each joint $i$ satisfies:

[TABLE]

where each $u_{i}\in\mathbb{R}$ is a control variable. As for our running example (Ex), (14) naturally represents a dynamical system in $\mathbb{R}^{2}$, so that, problem (EOCP) coincides with (OCP).

The examples and algorithms presented in this work were implemented in the Julia programming language [4] using the GuSTO.jl package located at https://github.com/StanfordASL/GuSTO.jl, with optimization problems solved using Gurobi [14]. We chose GuSTO [6] as the SCP procedure for E-SCP (line 4 of Algorithm 1), so that Corollary 1 held. For each compared SCP method, the continuous-time optimal control problem was discretized using a trapezoidal approximation of the dynamics, assuming a zero-order hold for the control, and the discrete-time cost considered for each problem was the energy $\sum_{k=1}^{d-1}||u_{k}||_{2}^{2}\Delta t$, where $d$ is the number of discretization points for the trajectory. Additionally, obstacle avoidance constraints served as our non-convex state constraints. For each set of simulations presented, we report results for 50 experiments with different start and goal configurations. A SCP trial is marked as successful if the algorithm converged and the resulting solution was collision-free. We used the Bullet Physics engine to calculate signed distances for obstacle avoidance constraints or penalties [9, 11]. For each experiment, the variables were initialized using straight-line initializations on the manifold.

V-A Comparison with State-of-the-Art

In this section, we demonstrate the benefits obtained when using E-SCP to solve trajectory optimization problems on manifolds, rather than standard SCP approaches. More specifically, the main advantage of E-SCP is that, when considering hard enforcement of dynamical constraints, the limiting numerical solution is guaranteed to lie on the manifold, even if such a constraint is not explicitly enforced. This approach is in contrast to enforcing the nonlinear manifold-type equality constraints that appear when using standard Euclidean-based SCP approaches. Since added equality constraints increase the complexity of the problem and may adversely affect efficiency, removing manifold-type constraints in SCP provides greater flexibility in solving the sequential problems, given that these constraints are implicitly satisfied.

For this comparison, we considered the 7-DoF manipulator using 120 discretization points over a trajectory time of 30 seconds [22]. The achieved results are shown in Table V-A. Here, comparisons are given between E-SCP with GuSTO as the internal solver, TrajOpt [36] without any enforced manifold-type constraints, and versions of GuSTO and TrajOpt where the manifold-type constraints are penalized (denoted by SCP and TrajOpt-P, respectively, in Table V-A).

E-SCP has the best performance in reduction of dynamical constraint error (even if negligible). As can be expected, the methods that explicitly penalize the presence of the manifold achieve the highest precision for manifold-constraint satisfaction (even if also negligible). However, a keen analysis of Table V-A shows that this comes with the tradeoff that the additional state-constraint penalties produce a greater tendency to fall into high-cost local minima, since the penalization affects the way the internal convex optimization algorithm reduces the cost. Consequently, the resulting true cost of the non-penalizing algorithms is on average lower than their penalizing counterparts. In other words, E-SCP provides better optimal solutions than state-of-the-art penalization approaches.

In addition, we note that although unit-norm constraints like $x\in\mathbb{T}^{n}$ can be easily formulated as penalty expressions and satisfied through penalization, more complex manifold constraints (e.g., those associated with SO(3), closed-kinematic chains, etc.) are more difficult to formulate and satisfy in this way, and would be better handled using E-SCP. Indeed, from Theorem 3 we see that manifold constraint error in E-SCP scales with the dynamical constraint error, which in turn depends only on the discretization scheme. Thus, we expect the negligible manifold constraint error for E-SCP in the previous example to carry over to more complex manifolds.