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 samplingbased motion planning techniques [16, 18, 19, 21, 22], variational approaches such as CHOMP and STOMP [33, 17], sumofsquares 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 manifoldtype 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 manifoldtype constraints are often defined only locally (i.e., through local charts) or as implicit constraints (i.e., constraints of the type where is a submersion and
is the state vector). As a pedagogical example, consider a twojoint manipulator. Its motion is forced to evolve on the torus
(see Figure 2), which represents a twodimensional submanifold of . Specifically, each joint variable (, ) evolves on the unitary circle , and thus the combined evolution is on the Cartesian product , which is diffeomorphic to (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 manifoldtype 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 postprocessing 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 samplingbased techniques leveraging local chart analysis,
[39, 15], methods employing global chartgluing 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 SCPbased techniques for trajectory optimization, the aim of this paper is to provide an SCPbased 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 (continuoustime) theoretical guarantees [6, 28]. Extending SCPbased methods, primarily developed for Euclidean settings, to manifoldconstrained problems is, however, challenging. In particular, when dealing with manifolds, it is challenging to make linearizations (required by SCP schemes that operate on dynamics) wellposed [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 manifoldconstrained 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 implicitlydefined 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 of a Cartesian product of Lie groups of matrices. Then, the firstorder equation that governs the dynamical evolution of the system is
(1) 
where is the geodesic spray induced by the kinetic energy and is the vertical lift of the generalized force [7]. This can be reinterpreted as a firstorder, controlaffine equation with a drift term on the space via the identification [7], where denotes the Lie algebra of . A natural embedding is then the inclusion (we provide explicit examples in the rest of the paper).
Statement of Contributions: In this paper, we leverage geometric embeddings to extend SCPbased methods for trajectory optimization to manifoldconstrained 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 SCPbased 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 manifoldtype 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 manifoldconstrained 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, SCPbased 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
(2) 
for some , where is a 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 secondorder 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 an dimensional manifold with tangent bundle and cotangent bundle (both manifolds of dimension ). The tangent space of at will be denoted by . Moreover, we recall that a smooth vector field on is a mapping such that , for every . 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 IIA by formulating trajectory optimization for dynamical systems as an optimal control problem on manifolds. Then, in Section IIB, 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 IIC.
Iia Trajectory Optimization on Generic Manifolds
In this paper we consider a continuoustime 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 IIID.
Specifically, consider an initial point and smooth mappings , , which are submersions at 0. Here, represent pointwise state constraints that are used to mathematically model multitask scenarios; in particular, represents goal region constraints. Without loss of generality, we require , where dist is a pointset distance evaluated w.r.t. some Riemannian metric on . For times , we model the dynamical evolution of the system by the following drift controlaffine system in
(3) 
where , are vector fields. The pointwise state constraints 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 with its tangent bundle .
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
(4) 
under dynamics and pointwise constraints (3), among all control trajectories satisfying almost everywhere in , where the measurable set represents control constraints. Here, , are , is the weighted norm defined by a constant positivedefinite matrix , and times , are fixed. We remark that hard enforcement of dynamical and intermediate/final goal set constraints is naturally imposed by (3). The function accumulates contributions from a purely statedependant cost and state constraint penalty function (e.g., stemming from collisionavoidance constraints), weighted by . 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 userdefined tolerance.
A representative example (that will serve as running example) of a trajectory optimization problem evolving on manifolds is the minimumenergy optimal control of a spacecraft avoiding collisions in a microgravity environment, which can be stated as (under the previous formalism, provides state constraints):
where is the position of the vehicle, its tangential velocity, its orientation (expressed via quaternions), its angular velocity (
is the usual skewsymmetric matrix depending on
) and the manifold characterizes quaternions. Controls are represented by the thrust and the torque . A naïve way to approach our running example (Ex) would entail removing, without principled justification, the constraint , 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 , that hinges on embeddings and provides a way to lift SCP methods to manifoldconstrained problems.IiB 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 around curves. In [40, 25, 8], the authors adapt such a definition on manifolds by recasting differential equations as algebraic equations of operators in . However, in many applications concerning dynamical systems, 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 for , and then to linearize in the ambient space.
Following the previous discussion, we assume that is a closed submanifold of , for some . This means that we fix a particular embedding, which is given by the canonical inclusion . This choice is made without loss of generality because, due to Whitneytype theorems [24], such a mapping always exists. Moreover, consistent with the previous motivating discussion, we assume that the mappings defining (OCP) naturally extend to , i.e., there exist vector fields , (with an abuse of notation, see [24]), and functions , and , , that are smooth submersions at 0, such that , , and . This setup allows us to transform the dynamics (3) into the following drift controlaffine system in :
(5) 
Therefore, (OCP) can be embedded in by considering the following Embedded Optimal Control Problem (EOCP), which consists of minimizing the integral cost
(6) 
under dynamics (5), among all control trajectories satisfying almost everywhere in . 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 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 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 the canonical inclusion. It follows that is correlated to , which implies their flows satisfy for every for which the flow is defined. Since , we obtain that satisfies the dynamics in (5) if and only if it satisfies the embedded dynamics in (3). From this, the optimality of follows from the similarity between (4) and (6). ∎
Remark 1.
Crucially, from Lemma 1, we see that the satisfaction of implicit manifoldtype 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
and that this mapping is also defined when . In other words, the original dynamics is equivalent to restricted to the subset . Therefore, the embedded dynamics related to this mapping are exactly the same but extended on , 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 IIA).
IiC 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 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 , assume we have some continuous curves and , continuously extended in the interval . Then, at iteration , the Linearized Embedded Optimal Control Problem (LEOCP) consists of minimizing the new cost
(7) 
where, consistent with the notation of Section IIB, and is any smooth approximation of [24, Chapter 10]. Function provides trustregion guarantees on the updates in the state trajectories and constraints via the bounds and weights . The dynamical constraint for (LEOCP) is
(8) 
obtained from the linearized expansion of all nonlinear mappings. We minimize among all controls satisfying almost everywhere in . Inductively, the curves and are defined as the optimal solution for problem (LEOCP), continuously extended in the interval . Ideally, SCP algorithms may vary and 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 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) is wellposed if, for each iteration , an optimal solution for (LEOCP) exists. For this, we consider the following assumptions:

The set is compact and convex. Moreover, the differentials of mappings , , are of full rank.

Mappings , , vector fields , and their differentials have compact supports (and do , , ).

At every iteration , problem (LEOCP) is feasible.
Under these assumptions, classical existence Filippovtype arguments [12, 23] (applied to the reduced form of (LEOCP), see also the Appendix) show that, at each iteration , the problem (LEOCP) has at least one optimal solution. We remark that similar assumptions have been considered in [6]; in the present contribution,  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 , Assumptions  are easily satisfied by (Ex).
Iii Algorithm Overview and Theoretical Guarantees
In Section IIIA, we detail a general algorithm for the solution of (OCP) which combines SCPbased procedures with the embedded framework defined previously. Its convergence guarantees, in the sense of the Pontryagin Maximum Principle [32], are studied in Section IIIB to IIID, 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.
Iiia SCPbased 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 IIB. In the third step, one finally applies some SCP scheme on Euclidean spaces. It is important to remark that ESCP provides the user with the freedom to choose any sequential convex procedure to solve the sequence of problems (LEOCP). However, we show in Section IIIB that specific choices of SCP (e.g. using hard enforcement of dynamical constraints) provide theoretical guarantees for ESCP which are also consistent with the presence of the manifold within the original problem (OCP).
Problem (LEOCP) is linearized around an initial curve tuple , where these initialization curves should be as close as possible to a feasible or even optimal curve for (LEOCP), although we do not require that is feasible for the embedded problem (EOCP). This allows one to initialize ESCP with simple, even infeasible, guesses for solutions of (EOCP), such as a straight line in the manifold, as detailed in [6, Section III.A].
IiiB Necessary Conditions for Optimality
Although the strategy for solving problems (LEOCP) in ESCP is up to the user, we show that specific choices of solvers allow one to recover important theoretical guarantees for the convergence of ESCP to critical points for the original problem (OCP) on manifolds. Specifically, we can show the convergence of ESCP towards a trajectory satisfying firstorder necessary conditions for optimality under the Pontryagin Maximum Principle [32] when the sequence of problems (LEOCP) is chosen as in Section IIC. 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 selfcontaintment, 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 as a timevarying function, i.e., , and fix some measurable control timeseries . Thanks to Assumptions , , for every , the trajectories arising for the augmented system
(9) 
exist in for every . Notice that is simply the accumulated cost at time . Therefore, the flow of system (9) is defined for every and we denote it by , such that represents the trajectory of (9) starting from at .
By standard identifications, we denote and we define the Hamiltonian function related to (9) as
(10) 
where is the canonical projection and denotes the duality in . As a classical result on Hamiltonian systems [1, 24], for every , one can uniquely associate to (10) the socalled Hamiltonian vector field by the rule , being the canonical symplectic form of (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 be an optimal trajectory for (OCP), associated with the control in and with fixed interior and final times , . There exists a nonpositive constant scalar and a piecewise absolutely continuous function , called the adjoint vector, satisfying , with , such that, almost everywhere in , the following relations hold:

Adjoint Equations
(11) 
Maximality Condition
(12) 
Transversality Conditions
For each , the adjoint vector satisfies
(13)
The tuple is a (Pontryagin) extremal. We say that is normal if , 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).
IiiC Convergence with Geometric Consistency
The convergence of ESCP can be inferred by leveraging one further commonly adopted regularity assumption concerning optimal controls:

At every iteration , the optimal control of (LEOCP) is piecewise continuous in every subinterval and for .
The key convergence result is stated next:
Theorem 3 (Convergence Theorem).
Suppose that  hold. Given any sequence of trust region radii and weights , let be any sequence such that, for every , is optimal for (LEOCP) in , where problems (LEOCP) are built as detailed in Section IIC. Up to some subsequence:

, for the strong topology of , and

, for the weak topology of ,
where is feasible for the original problem (OCP). Moreover, there exists a nonpositive constant scalar and a piecewise absolutely continuous function , with , such that the tuple , where Pr is the orthogonal projection of onto (cf. Appendix), represents a geometric Pontryagin extremal (in the sense of Theorem 2) for the original problem (OCP) on . In particular, as tends to infinity, up to some subsequence:

, and

, , and for the strong topology of ,
where is a Pontryagin extremal of (LEOCP).
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) that converges (under appropriate topologies) to a critical point for (OCP), in the (strong) sense of Theorem 2. Importantly, the limiting trajectory lies on , despite solving the linearized, embedded versions (LEOCP) (see Figure 2). The termination properties of Algorithm ESCP are stated next:
Corollary 1 (ESCP Termination).
Assume (i)  hold, (ii) problems (LEOCP) are built as detailed in Section IIC, (iii) the SCP procedure adopted for ESCP (line 6 of Algorithm 1) enforces hard dynamical constraints (as opposed to a penalized implementation – see Remark 1), (iv) for all and are chosen such that for every iteration , an output is always provided, and (v) the algorithm is terminated if . Then, in solving (OCP) by ESCP only three mutually exclusive situations arise:

There exists an iteration for which . Then, ESCP terminates, providing a solution for (LEOCP) satisfying only soft state constraints.

There exists an iteration for which . Then, ESCP terminates, providing a stationary point, in the sense of the Pontryagin Maximum Principle, for the original problem (OCP).

We have , for every iteration . Then, ESCP builds a sequence of optimal solutions for (LEOCP) 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 ESCP 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
because it clearly converges to . ∎
In case 1), SCP fails because no feasible strategies can be computed, which also occurs in other stateoftheart 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 manifoldtype constraints.
IiiD Geometric Consistency and DiscreteTime Convergence
Despite the fact that ESCP 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 firstorder necessary conditions for optimality that respect the geometric structure of the original manifold.
The additional advantage of working within a continuoustime setting is that the validity of the theoretical guarantees is independent of the timediscretization scheme used to solve the ODEs. For instance, choosing schemes such as Euler or Simpson’s rule lead to well wellposed convex optimization problems when the dynamics are linearized within each SCP step. With a sufficiently small timestep, one can ensure that the discrete solution provided by SCP stays close to the solution of the continuoustime problem (EOCP). Finally, Corollary 1 further implies that this discretized solution stays close to the solution for the original continuoustime (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) in the Euclidean space towards a Pontryagin extremal related to the solution for (OCP) in the manifold, found by ESCP. As a consequence, we can extend the acceleration procedure proposed in [6] to the manifold and pointwise state constraint case, i.e., warmstarting shooting methods [3] with ESCP.
The key idea is that since the convergence of the adjoint vectors is provided in , one can leverage them to restate a shooting method on the manifold within . 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 related to the pointwise condition for the finite dimensional discretization of problems (LEOCP) approximate the values of the adjoint vectors related to the continuoustime (LEOCP) (see [13] and the problem reduction provided in the Appendix). Then, up to some subsequence, for every small , there exists an iteration for which, for every iteration , one has , , where is an adjoint vector related to the solution of (OCP) found by SCP (see Theorem 3). This means that, starting from some iteration , we can run a shooting method to solve (OCP), initializing using , . At each iteration of SCP, we use the values 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 ESCP and standard SCP on Euclidean spaces, (2) providing comparisons between ESCP and stateoftheart algorithms for trajectory optimization, and (3) analyzing convergence acceleration for ESCP via shooting methods.
Simulations are provided by considering two problems: our running example (Ex), defined in Section IIA
, and a trajectory optimization problem for a 7 degreeoffreedom 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
, (one for each joint), so that the system evolves in the dimensional torus, i.e., , which is naturally embedded in . The dynamics are given by the kinematic equations of a manipulator, that is, each joint satisfies:(14) 
where each is a control variable. As for our running example (Ex), (14) naturally represents a dynamical system in , 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 ESCP (line 4 of Algorithm 1), so that Corollary 1 held. For each compared SCP method, the continuoustime optimal control problem was discretized using a trapezoidal approximation of the dynamics, assuming a zeroorder hold for the control, and the discretetime cost considered for each problem was the energy , where is the number of discretization points for the trajectory. Additionally, obstacle avoidance constraints served as our nonconvex 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 collisionfree. 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 straightline initializations on the manifold.
Va Comparison with StateoftheArt
In this section, we demonstrate the benefits obtained when using ESCP to solve trajectory optimization problems on manifolds, rather than standard SCP approaches. More specifically, the main advantage of ESCP 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 manifoldtype equality constraints that appear when using standard Euclideanbased SCP approaches. Since added equality constraints increase the complexity of the problem and may adversely affect efficiency, removing manifoldtype constraints in SCP provides greater flexibility in solving the sequential problems, given that these constraints are implicitly satisfied.
For this comparison, we considered the 7DoF manipulator using 120 discretization points over a trajectory time of 30 seconds [22]. The achieved results are shown in Table I. Here, comparisons are given between ESCP with GuSTO as the internal solver, TrajOpt [36] without any enforced manifoldtype constraints, and versions of GuSTO and TrajOpt where the manifoldtype constraints are penalized (denoted by SCP and TrajOptP, respectively, in Table I).
ESCP 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 manifoldconstraint satisfaction (even if also negligible). However, a keen analysis of Table I shows that this comes with the tradeoff that the additional stateconstraint penalties produce a greater tendency to fall into highcost local minima, since the penalization affects the way the internal convex optimization algorithm reduces the cost. Consequently, the resulting true cost of the nonpenalizing algorithms is on average lower than their penalizing counterparts. In other words, ESCP provides better optimal solutions than stateoftheart penalization approaches.
In addition, we note that although unitnorm constraints like can be easily formulated as penalty expressions and satisfied through penalization, more complex manifold constraints (e.g., those associated with SO(3), closedkinematic chains, etc.) are more difficult to formulate and satisfy in this way, and would be better handled using ESCP. Indeed, from Theorem 3 we see that manifold constraint error in ESCP scales with the dynamical constraint error, which in turn depends only on the discretization scheme. Thus, we expect the negligible manifold constraint error for ESCP in the previous example to carry over to more complex manifolds.
[4pt]ESCP  [4pt]SCP  [4pt]TrajOpt  [4pt]TrajOptP  
[4pt]Dynamical constraint error  [4pt]  [4pt]  [4pt]  [4pt] 
[2pt] manifold constraint error  [4pt]  [4pt]  [4pt]  [4pt] 
[2pt]True cost  0.105  1.0  0.215  0.792 
[2pt]Computation time  0.466  0.129  0.321  1.0 
VB Convergence Acceleration via Shooting Methods
In this section, we provide numerical simulations that highlight convergence benefits that are obtained when ESCP is combined with shooting methods. In particular, we consider trajectory optimization of a spacecraft having the dynamics and embedding given in (Ex) and navigating through a highly cluttered environment, using 100 discretization points over 50second trajectories. As shown in Table II, the results are very promising: on average, the shooting method cuts significantly the number of SCP iterations required to converge to a trajectory, resulting in an overall 59.4% increase in speed. The difference in performance is made more stark by the fact that the shooting method can occasionally converge in cases where SCP is unable to converge at all due to the use of naive incollision straightline initialization. Indeed, it is also interesting to note that in cases where the shooting method provides the final convergence, the final trajectory cost is always lower (if sometimes only slightly) than the cost returned by SCP alone, as the shooting method relies on the Newton method and thus tends to achieve a much higher proximity to the optimal on convergence than SCP.
[4pt]ESCP Only  [4pt]ESCP + Shooting  

[2pt]SCP Iterations  32  13 
[2pt]Reported Cost  0.221  0.176 
Vi Conclusions
In this paper we provided an SCPbased method for trajectory optimization with manifold constraints. Our key insight was to leverage geometric embeddings to lift a manifoldconstrained trajectory optimization problem into an equivalent problem defined over a space enjoying Euclidean structure, where SCP can be readily applied. We derived sound theoretical guarantees and validated the proposed methodology via numerical experiments. Among other benefits, our method can easily accommodate implicitlydefined manifold constraints.
This work opens the field to many future avenues of research. First, we plan to study the setting with free final times , , both from a theoretical and numerical standpoint. Second, related to the previous direction, it is of interest to design accurate numerical schemes that can handle multishooting methods and free final time settings. Third, we would like to leverage techniques from Lie group theory to improve performance when applying ESCP to such specific class of submanifolds. Finally, we plan to evaluate our method on hardware platforms, such as robotic manipulators and test beds for freeflying robotic spacecraft.
References
 Agrachev and Sachkov [2004] A. A. Agrachev and Y. Sachkov. Control Theory from the Geometric Viewpoint. Springer, 2004.
 Augugliaro et al. [2012] F. Augugliaro, A. P. Schoellig, and R. D’Andrea. Generation of collisionfree trajectories for a quadrocopter fleet: A sequential convex programming approach. In IEEE/RSJ Int. Conf. on Intelligent Robots & Systems, 2012.
 Betts [1998] J. T. Betts. Survey of numerical methods for trajectory optimization. AIAA Journal of Guidance, Control, and Dynamics, 21(2):193–207, 1998.
 Bezanson et al. [2012] J. Bezanson, S. Karpinski, V. B. Shah, and A. Edelman. Julia: A fast dynamic language for technical computing, 2012. Available at http://arxiv.org/abs/1209.5145.
 Bonalli [2018] R. Bonalli. Optimal control of aerospace systems with controlstate constraints and delays. PhD thesis, Sorbonne Université & ONERA  The French Aerospace Lab, 2018.
 Bonalli et al. [2019] R. Bonalli, A. Cauligi, A. Bylard, and M. Pavone. GuSTO: Guaranteed sequential trajectory optimization via sequential convex programming. In Proc. IEEE Conf. on Robotics and Automation, 2019.
 Bullo and Lewis [2004] F. Bullo and A. D Lewis. Geometric Control of Mechanical Systems. SpringerVerlag, 1 edition, 2004.
 Bullo and Lewis [2007] F. Bullo and A. D. Lewis. Reduction, linearization, and stability of relative equilibria for mechanical systems on Riemannian manifolds. Acta Applicandae Mathematicae, 99(1):53–95, 2007.
 [9] E. Coumans. Bullet physics. Available at http://bulletphysics.org/.
 Dmitruk and Kaganovich [2011] A. V. Dmitruk and A. M. Kaganovich. Maximum principle for optimal control problems with intermediate constraints. Computational Mathematics and Modeling, 22(2):2180–215, 2011.
 Ericson [2004] C. Ericson. RealTime Collision Detection. CRC Press, 2004.
 Filippov [1962] A. F. Filippov. On certain questions in the theory of optimal control. SIAM Journal on Control, 1(1):76–84, 1962.
 Göllmann et al. [2008] L. Göllmann, D. Kern, and H. Maurer. Optimal control problems with delays in state and control variables subject to mixed controlstate constraints. Optimal Control Applications and Methods, 30(4):341–365, 2008.
 Gurobi Optimization, LLC [2018] Gurobi Optimization, LLC. Gurobi Optimizer Reference Manual, 2018. Available at http://www.gurobi.com.
 Jaillet and Porta [2016] L. Jaillet and J. M. Porta. Path planning with loop closure constraints using an atlasbased RRT. In Int. Symp. on Robotics Research, 2016.
 Janson et al. [2015] L. Janson, E. Schmerling, A. Clark, and M. Pavone. Fast Marching Tree: a fast marching samplingbased method for optimal motion planning in many dimensions. Int. Journal of Robotics Research, 34(7):883–921, 2015.
 Kalakrishnan et al. [2011] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal. STOMP: Stochastic trajectory optimization for motion planning. In Proc. IEEE Conf. on Robotics and Automation, 2011.
 Karaman and Frazzoli [2011] S. Karaman and E. Frazzoli. Samplingbased algorithms for optimal motion planning. Int. Journal of Robotics Research, 30(7):846–894, 2011.
 Kavraki et al. [1996] L. E. Kavraki, P. Švestka, J.C. Latombe, and M. H. Overmars. Probabilistic roadmaps for path planning in highdimensional spaces. IEEE Transactions on Robotics and Automation, 12(4):566–580, 1996.
 Khorasani and Kokotovic [1985] K. Khorasani and P. V. Kokotovic. Feedback linearization of a flexible manipulator near its rigid body manifold. System and Control Letters, 6(3):187–192, 1985.
 LaValle [2006] S. M. LaValle. Planning Algorithms. Cambridge Univ. Press, 2006.
 LaValle and Kuffner [2000] S. M. LaValle and J. J. Kuffner. Rapidlyexploring random trees: Progress and prospects. In Workshop on Algorithmic Foundations of Robotics, 2000.
 Lee and Markus [1967] E. B. Lee and L. Markus. Foundations of Optimal Control Theory. John Wiley & Sons, 1967.
 Lee [2003] J. M. Lee. Introduction to Smooth Manifolds. Springer, 1 edition, 2003.
 Lewis and Tyner [2003] A. D. Lewis and D. R. Tyner. Jacobian linearisation in a geometric setting. In Proc. IEEE Conf. on Decision and Control, 2003.
 Liu and Lu [2014] X. Liu and P. Lu. Solving nonconvex optimal control problems by convex optimization. AIAA Journal of Guidance, Control, and Dynamics, 37(3):750 – 765, 2014.
 Majumdar and Tedrake [2017] A. Majumdar and R. Tedrake. Funnel libraries for realtime robust feedback motion planning. Int. Journal of Robotics Research, 36(8):947–982, 2017.
 Mao et al. [2016] Y. Mao, M. Szmuk, and B. Açikmeşe. Successive convexification of nonconvex optimal control problems and its convergence properties. In Proc. IEEE Conf. on Decision and Control, 2016.
 Mellinger et al. [2012] D. Mellinger, N. Michael, and V. Kumar. Trajectory generation and control for precise aggressive maneuvers with quadrotors. Int. Journal of Robotics Research, 31(5):664–674, 2012.
 Misra and Bai [2017] G. Misra and X. Bai. Taskconstrained trajectory planning of freefloating spacerobotic systems using convex optimization. AIAA Journal of Guidance, Control, and Dynamics, 40(11):2857 – 2870, 2017.
 Murray et al. [1994] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, first edition, 1994.
 Pontryagin [1987] L. S. Pontryagin. Mathematical Theory of Optimal Processes. Taylor & Francis, 1987.
 Ratliff et al. [2009] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa. CHOMP: Gradient optimization techniques for efficient motion planning. In Proc. IEEE Conf. on Robotics and Automation, 2009.
 Saccon et al. [2012] A. Saccon, A. P. Aguiar, A. J. Häusler, J. Hauser, and A. M. Pascoal. Constrained motion planning for multiple vehicles on SE(3). In Proc. IEEE Conf. on Decision and Control, 2012.
 Saccon et al. [2013] A. Saccon, J. Hauser, and A. P. Aguiar. Optimal control on Lie groups: The projection operator approach. IEEE Transactions on Automatic Control, 58(9):2230–2245, 2013.
 Schulman et al. [2014] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel. Motion planning with sequential convex optimization and convex collision checking. Int. Journal of Robotics Research, 33(9):1251–1270, 2014.

Shen et al. [2013]
S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar.
Visionbased state estimation and trajectory control towards highspeed flight with a quadrotor.
In Robotics: Science and Systems, 2013.  Singh et al. [2018] S. Singh, M. Chen, S. L. Herbert, C. J. Tomlin, and M. Pavone. Robust tracking with model mismatch for fast and safe planning: an SOS optimization approach. In Workshop on Algorithmic Foundations of Robotics, 2018. In Press.
 Suh et al. [2011] C. Suh, T. T. Um, B. Kim, H. Noh, M. Kim, and F. C. Park. Tangent space RRT: A randomized planning algorithm on constraint manifolds. In Proc. IEEE Conf. on Robotics and Automation, 2011.
 Sussman [1998] H. J. Sussman. An introduction to the coordinatefree maximum principle. In Geometry of Feedback and Optimal Control. 1998.
 Virgilillop et al. [2019] J. Virgilillop, C. Zagaris, R. Zappulla II, A. Bradstreet, and M. Romano. A convexprogrammingbased guidance algorithm to capture a tumbling object on orbit using a spacecraft equipped with a robotic manipulator. Int. Journal of Robotics Research, 38(1):40 – 72, 2019.
 Watterson et al. [2016] M. Watterson, T. Smith, and V. Kumar. Smooth trajectory generation on SE(3) for a free flying space robot. In IEEE/RSJ Int. Conf. on Intelligent Robots & Systems, 2016.
 Watterson et al. [2018] M. Watterson, S. Liu, K. Sun, T. Smith, and V. Kumar. Trajectory optimization on manifolds with applications to SO(3) and R3XS2. In Robotics: Science and Systems, 2018.
Appendix
Via Proof of Theorem 3
The proof consists of two main steps. First, we apply the reduction scheme in [10] to each linearized problems (LEOCP): every convexified optimal control problem with pointwise state constraints is converted into an optimal control problems without such constraints but with higher dimension. We apply the SCP convergence result provided by [6, Theorem III.1] to this class of problems, recovering an extremal for the embedded problem (EOCP). Then, we project the obtained adjoint vector getting a geometric extremal for the original optimal control problem (OCP).
Remark 2.
Considering Darboux coordinates for , equations (11) locally take the common form [1]
(15) 
In particular, when , the adjoint equations (15) are globally equivalent to the adjoint equations (11). Moreover, the constancy of arises from (15) and the fact that the Hamiltonian (10) does not depend on the variable . This facts will be used in what follows.
ViB Reduction of (Leocp) to Classical Optimal Control Problems and Analysis for the Convergence of Extremals
We first reduce the linearized problems (LEOCP) to optimal control problems without pointwise state constraints. For sake of concision and without loss of generality, from now on we assume that , i.e. we have two times , . Considering the general case goes similarly (see also [dmitruk2011maximum]).
Let be the sequence of optimal solutions for (LEOCP) in given in Theorem 3. We chop each by defining, for every and every ,
(16) 
For every iteration , consider the Reduced Linearized Embedded Optimal Control Problem (RLEOCP) in
where the condition translates into the continuity of admissible trajectories for (LEOCP) at . Therefore, by relations (16) and the definition of problems (LEOCP), each tuple is an optimal solution for problem (RLEOCP). Applying the classical Pontryagin Maximum Principle with transversality conditions [32] to each problem (RLEOCP) provides the existence of nontrivial tuples , where are nonpositive constant and , are absolutely continuous functions in , satisfying, a.e. in ,
(17) 
(18) 
Comments
There are no comments yet.