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 ), 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 where is a submersion and
is the state vector). As a pedagogical example, consider a two-joint manipulator. Its motion is forced to evolve on the torus(see Figure 2), which represents a two-dimensional 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 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 , and methods exploiting properties of specific types of manifolds (in particular, Lie groups), such as invariant metrics  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 . 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 of a Cartesian product of Lie groups of matrices. Then, the first-order equation that governs the dynamical evolution of the system is
where is the geodesic spray induced by the kinetic energy and is the vertical lift of the generalized force . This can be reinterpreted as a first-order, control-affine equation with a drift term on the space via the identification , 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 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 , we provide convergence guarantees for embedded SCP in the sense of the geometric Pontryagin Maximum Principle (PMP) , 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 , we harness the insights gained through our theoretical analysis to develop a convergence acceleration scheme for trajectory optimization on manifolds based on shooting methods .
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
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 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 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  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 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 point-set distance evaluated w.r.t. some Riemannian metric on . For times , we model the dynamical evolution of the system by the following drift control-affine system in
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
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 positive-definite 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 state-dependant cost and state constraint penalty function (e.g., stemming from collision-avoidance 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 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, provides state constraints):
where is the position of the vehicle, its tangential velocity, its orientation (expressed via quaternions), its angular velocity (
is the usual skew-symmetric 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 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 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 Whitney-type theorems , 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 ), 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 control-affine system in :
Therefore, (OCP) can be embedded in by considering the following Embedded Optimal Control Problem (EOCP), which consists of minimizing the integral cost
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).
The proof makes use of standard tools in differential geometry, whose definitions can be found in  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). ∎
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
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 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 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
where, consistent with the notation of Section II-B, and is any smooth approximation of [24, Chapter 10]. Function provides trust-region guarantees on the updates in the state trajectories and constraints via the bounds and weights . The dynamical constraint for (LEOCP) is
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 .
A convexified formulation similar to (7)-(8) has already been introduced in . 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 well-posed 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 Filippov-type 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 ; in the present contribution, - gather the assumptions in  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 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 , 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). 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) 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 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) 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  when the sequence of problems (LEOCP) 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 as a time-varying function, i.e., , and fix some measurable control time-series . Thanks to Assumptions , , for every , the trajectories arising for the augmented system
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
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 so-called Hamiltonian vector field by the rule , being the canonical symplectic form of (see, e.g., ). Combining the classical geometric Pontryagin Maximum Principle  with the reduction scheme for pointwise state constraints developed in  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:
For each , the adjoint vector satisfies
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).
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:
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 II-C. 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 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 E-SCP are stated next:
Corollary 1 (E-SCP Termination).
Assume (i) - hold, (ii) problems (LEOCP) 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) 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 E-SCP only three mutually exclusive situations arise:
There exists an iteration for which . Then, E-SCP terminates, providing a solution for (LEOCP) satisfying only soft state constraints.
There exists an iteration for which . Then, E-SCP terminates, providing a stationary point, in the sense of the Pontryagin Maximum Principle, for the original problem (OCP).
We have , for every iteration . Then, E-SCP 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).
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
because it clearly converges to . ∎
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 . 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) 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  to the manifold and pointwise state constraint case, i.e., warm-starting shooting methods  with E-SCP.
The key idea is that since the convergence of the adjoint vectors is provided in , one can leverage them to re-state a shooting method on the manifold within . However, unlike the framework proposed in , 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 continuous-time (LEOCP) (see  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 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, (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:
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  using the GuSTO.jl package located at https://github.com/StanfordASL/GuSTO.jl, with optimization problems solved using Gurobi . We chose GuSTO  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 , where 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 . The achieved results are shown in Table I. Here, comparisons are given between E-SCP with GuSTO as the internal solver, TrajOpt  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 I).
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 I 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 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.
|[4pt]Dynamical constraint error||[4pt]||[4pt]||[4pt]||[4pt]|
|[2pt] manifold constraint error||[4pt]||[4pt]||[4pt]||[4pt]|
V-B Convergence Acceleration via Shooting Methods
In this section, we provide numerical simulations that highlight convergence benefits that are obtained when E-SCP 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 50-second 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 in-collision straight-line 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]E-SCP Only||[4pt]E-SCP + Shooting|
In this paper we provided an SCP-based method for trajectory optimization with manifold constraints. Our key insight was to leverage geometric embeddings to lift a manifold-constrained 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 implicitly-defined 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 multi-shooting methods and free final time settings. Third, we would like to leverage techniques from Lie group theory to improve performance when applying E-SCP to such specific class of submanifolds. Finally, we plan to evaluate our method on hardware platforms, such as robotic manipulators and test beds for free-flying robotic spacecraft.
- Agrachev and Sachkov  A. A. Agrachev and Y. Sachkov. Control Theory from the Geometric Viewpoint. Springer, 2004.
- Augugliaro et al.  F. Augugliaro, A. P. Schoellig, and R. D’Andrea. Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach. In IEEE/RSJ Int. Conf. on Intelligent Robots & Systems, 2012.
- Betts  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.  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  R. Bonalli. Optimal control of aerospace systems with control-state constraints and delays. PhD thesis, Sorbonne Université & ONERA - The French Aerospace Lab, 2018.
- Bonalli et al.  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  F. Bullo and A. D Lewis. Geometric Control of Mechanical Systems. Springer-Verlag, 1 edition, 2004.
- Bullo and Lewis  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.
-  E. Coumans. Bullet physics. Available at http://bulletphysics.org/.
- Dmitruk and Kaganovich  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  C. Ericson. Real-Time Collision Detection. CRC Press, 2004.
- Filippov  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.  L. Göllmann, D. Kern, and H. Maurer. Optimal control problems with delays in state and control variables subject to mixed control-state constraints. Optimal Control Applications and Methods, 30(4):341–365, 2008.
- Gurobi Optimization, LLC  Gurobi Optimization, LLC. Gurobi Optimizer Reference Manual, 2018. Available at http://www.gurobi.com.
- Jaillet and Porta  L. Jaillet and J. M. Porta. Path planning with loop closure constraints using an atlas-based RRT. In Int. Symp. on Robotics Research, 2016.
- Janson et al.  L. Janson, E. Schmerling, A. Clark, and M. Pavone. Fast Marching Tree: a fast marching sampling-based method for optimal motion planning in many dimensions. Int. Journal of Robotics Research, 34(7):883–921, 2015.
- Kalakrishnan et al.  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  S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. Int. Journal of Robotics Research, 30(7):846–894, 2011.
- Kavraki et al.  L. E. Kavraki, P. Švestka, J.-C. Latombe, and M. H. Overmars. Probabilistic roadmaps for path planning in high-dimensional spaces. IEEE Transactions on Robotics and Automation, 12(4):566–580, 1996.
- Khorasani and Kokotovic  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  S. M. LaValle. Planning Algorithms. Cambridge Univ. Press, 2006.
- LaValle and Kuffner  S. M. LaValle and J. J. Kuffner. Rapidly-exploring random trees: Progress and prospects. In Workshop on Algorithmic Foundations of Robotics, 2000.
- Lee and Markus  E. B. Lee and L. Markus. Foundations of Optimal Control Theory. John Wiley & Sons, 1967.
- Lee  J. M. Lee. Introduction to Smooth Manifolds. Springer, 1 edition, 2003.
- Lewis and Tyner  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  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  A. Majumdar and R. Tedrake. Funnel libraries for real-time robust feedback motion planning. Int. Journal of Robotics Research, 36(8):947–982, 2017.
- Mao et al.  Y. Mao, M. Szmuk, and B. Açikmeşe. Successive convexification of non-convex optimal control problems and its convergence properties. In Proc. IEEE Conf. on Decision and Control, 2016.
- Mellinger et al.  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  G. Misra and X. Bai. Task-constrained trajectory planning of free-floating space-robotic systems using convex optimization. AIAA Journal of Guidance, Control, and Dynamics, 40(11):2857 – 2870, 2017.
- Murray et al.  R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, first edition, 1994.
- Pontryagin  L. S. Pontryagin. Mathematical Theory of Optimal Processes. Taylor & Francis, 1987.
- Ratliff et al.  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.  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.  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.  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. 
S. Shen, Y. Mulgaonkar, N. Michael, and V. Kumar.
Vision-based state estimation and trajectory control towards high-speed flight with a quadrotor.In Robotics: Science and Systems, 2013.
- Singh et al.  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.  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  H. J. Sussman. An introduction to the coordinate-free maximum principle. In Geometry of Feedback and Optimal Control. 1998.
- Virgili-llop et al.  J. Virgili-llop, C. Zagaris, R. Zappulla II, A. Bradstreet, and M. Romano. A convex-programming-based 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.  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.  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.
Vi-a Proof of Theorem 3
The proof consists of two main steps. First, we apply the reduction scheme in  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).
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.
Vi-B 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 ,
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  to each problem (RLEOCP) provides the existence of nontrivial tuples , where are nonpositive constant and , are absolutely continuous functions in , satisfying, a.e. in ,