Learning Dynamic-Objective Policies from a Class of Optimal Trajectories
Christopher Iliffe Sprague Dario Izzo Petter Ögren

Capturing sets of ordinals by normal ultrapowers
    Radek Honzík
    
================================================

empty
empty


Optimal state-feedback controllers, capable of changing between different objective functions,
are advantageous to systems in which unexpected situations may arise.
However, synthesising such controllers, even for a single objective, is a demanding process.
In this paper, we present a novel and straightforward approach to synthesising these policies through a combination of trajectory optimisation, homotopy continuation, and imitation learning.
We use numerical continuation to efficiently generate optimal demonstrations across several objectives and boundary conditions, and use these to train our policies.
Additionally, we demonstrate the ability of our policies to effectively learn families of optimal state-feedback controllers, which can be used to change objective functions online.
We illustrate this approach across two trajectory optimisation problems, an inverted pendulum swingup and a spacecraft orbit transfer, and show that the synthesised policies, when evaluated in simulation, produce trajectories that are near-optimal.
These results indicate the benefit of trajectory optimisation and homotopy continuation to the synthesis of controllers in dynamic-objective contexts.
















Optimal Control, Deep Learning, Homotopy, Online Planning



  every neuron/.style=
    circle,
    draw,
    minimum size=0.3cm
  ,
  neuron missing/.style=
    draw=none, 
    scale=0.9,
    text height=0.27cm,
    execute at begin node=⋮
  ,





§ INTRODUCTION

An optimal controller that is capable of changing its objective function in the middle of a mission can be very useful in an autonomous system. Imagine a vehicle that started off on a time-optimal trajectory towards a given destination but suddenly finds itself having less energy than expected; perhaps due to a battery failure, a broken solar panel, or a leaking fuel tank. It can then instantly switch to an effort-optimal controller. Conversely, a vehicle that was saving energy for future transitions might suddenly be in a hurry to reach a destination and can then instantly switch to a controller that reaches the goal in the shortest possible time.

In general, applying an optimal control policy is desirable, but often infeasible for many autonomous systems.
The reason for this is that, for many systems characterised by nonlinear dynamics, finding a closed-loop solution to the underlying optimal control is intractable.
A common approach to circumvent this is to precompute an open-loop solution, through trajectory optimisation <cit.>, and apply a simple state-feedback controller to track it.
However, such an approach suffers when unforeseen events require a change of the objective function or lead the system's state out of the controller's region of attraction.

An alternative approach is to apply model predictive control (MPC), where open-loop solutions are incrementally computed and executed on a finite-time horizon. 
Although this approach provides reactivity in response to unforeseen events, it might suffer in performance, as it does not consider the full-time horizon; and in computational complexity, as it requires repeatedly solving an optimisation problem.



In recent years, with the progression of machine learning (ML), data-driven approaches have become popular in the context of control. 
Across these approaches, control policies are represented by neural networks (NN) <cit.>, whose feed-forward architectures express simple executable functions. When given appropriate data, these models can be trained to approximate optimal control policies, and pose a reasonable alternative to MPC in terms of both performance and computational complexity  <cit.>, especially on low-end hardware.

Whilst the problem of synthesising optimal control policies has been well addressed in ML literature, in both single and multi-objective contexts <cit.>, relatively few have tackled the problem of adapting policies online to changing objectives <cit.>. Such a case is particularly relevant to systems in which objective priorities can change unexpectedly, e.g. saving energy vs. saving time.
To the best our knowledge, the use of trajectory optimisation and imitation learning has been unexplored in this regard.

In this work, we show that we can straightforwardly leverage trajectory optimisation and homotopy continuation to synthesise state-feedback controllers capable of adapting online to dynamic objectives. Our contributions are:

    
  * we develop an efficient data-generation pipeline to produce optimal demonstrations across multiple objectives and boundary conditions;
    
  * and we show that our trained policies can continuously represent homotopies between optimal policies.





































The outline of the paper is as follows.
In Section <ref> we describe related work.
Then, in Section <ref>, we explain the homotopy continuation method we use for trajectory optimisation.
In Section <ref>, we explain our method of dataset generation, our machine learning setup, and the results we achieved.
Finally, in Section <ref>, we reflect upon our results and discuss what they entail.



































§ RELATED WORK



Many problems in robotics are naturally formulated in terms of
nonlinear optimal control problems

 <cit.>, but they often lack a closed-form solution.
One can usually, however, solve the trajectory optimisation problem (TOP) using numerical methods <cit.>, to obtain open-loop solutions. But, as described before, naïvely executing or tracking these solutions might have drawbacks in terms of robustness.

MPC addresses this issue by iteratively computing and executing an open-loop solution on a finite-time horizon, allowing for online reactions in response to unforeseen events.
With advances in MPC's efficiency <cit.> and increasing computational capabilities of many robotic platforms, this approach has become popular in a variety of applications <cit.>.
But, although it can be quite effective, the need to employ a finite-time horizon, regardless of computational capabilities, results in suboptimal trajectories, as they do not take the full-time horizon into account.
































A set of approaches that mitigates these issues, by learning the optimal control with respect to the full- (or infinite-) time horizon, uses reinforcement learning (RL) <cit.>.
These approaches operate by constructing a policy through incremental interaction with the environment.
Although such approaches have been quite successful in the continuous control domain <cit.>, particularly for stochastic systems, they sometimes struggle to converge and are sensitive to the choice of rewards <cit.>.







In the case that expert demonstrations are either readily available or producible, it is sometimes more straightforward to rely on the alternative ML approach: imitation learning (IL).
In this approach, expert demonstrations are utilised to synthesise optimal policies in a supervised-learning manner.
This approach benefits from a greater sampling-efficiency and has been shown to rapidly accelerate policy-convergence in RL approaches <cit.>.
It is usually the case that demonstrations are not necessarily optimal, but if, however, the system at hand is well-suited to providing optimal demonstrations, e.g. through trajectory optimisation, this approach proves to be particularly useful in converging to an optimal policy in an uncomplicated way <cit.>.

A commonality among these approaches — IL and RL — is that their policies are represented by NNs and are traditionally orientated towards single-objective scenarios only.
Some pertaining works have indeed addressed the multi-objective[The single- and multi-objective modalities of trajectory optimisation are often referred to as single- and multi-task in ML parlance, respectively. Whilst, in ML, these terms often describe boundary conditions associated with different tasks, we maintain that an objective describes only the cost of a trajectory (𝒥).]
case, but typically have only sought policies lying along the Pareto front <cit.>, and only few have targeted the case where objective priorities may change over time <cit.>. 

In the virtual-first work to address this case <cit.>, a set of policies was learnt in an RL setting, to discretely represent a spectrum of different objectives, which could then be referred to online to adapt to changing objective priorities. 
However, as noted in <cit.>, using a discrete set of policies does not scale to complex problems nor truly represent a continuous mapping between objectives.
Recently, <cit.> showed that policies could be adapted online to dynamic preferences in an RL setting, using a Q-network, conditioned on the objective-priority.
We diverge from these works in that we show how trajectory optimisation and homotopy can be straightforwardly leveraged to synthesise these conditional policies in an IL setting.


In <cit.>, it was shown that Pontryagin's minimum principle (PMP)[PMP is a manifestation of indirect trajectory optimisation; alternatively, direct methods could be used to generate optimal demonstrations in the case of a black-box dynamics model. For an overview of trajectory optimisation methods refer to <cit.>.] <cit.> could be used to generate optimal demonstrations in order to synthesise optimal policies in an IL setting.
It was shown, across a variety of two-dimensional dynamical systems, that the resulting policies, when executed in simulation, produced near-optimal trajectories, even when the systems were initialised in unseen regions of the state-space.
In <cit.>, we showed that this approach could be extended to three dimensions and used to encode manifold boundary conditions in the state-space, using transversality conditions of PMP.
These results were built upon in <cit.>, where an investigation of NN hyperparameters and an improved evaluation metric — policy trajectory optimality — were presented.

In these works, optimal policies were synthesised for a single objective. However, in both <cit.> and <cit.>, homotopy continuation <cit.> was employed between different objectives to ease the burden of generating demonstrations under objectives which are particularly challenging for gradient-based optimisers.
Such a case arises when the topology of the TOP becomes discontinuous due to the underlying optimal control being bang-bang. We build upon these works by taking advantage of the full homotopy path between objectives to straightforwardly synthesise conditional policies, which can react online to changing objective priorities, as we will describe in more detail in the following sections.
































































§ TRAJECTORY OPTIMISATION
 







In this section, we outline our optimal-trajectory dataset generation process, used for learning in Section <ref>.

Throughout this paper, we consider nonlinear autonomous dynamical systems of the form ṡ = f(s, u), with scalar objectives of the form 𝒥 = ∫_t_0^t_fℒ(u(t)) t.
With these, one can solve the TOP to determine an optimal trajectory [s(t), u(t)]^⊺ — an open-loop solution — given an initial and desired state.
In this work, we formulate this problem as a nonlinear programme and solve it using numerical optimisation <cit.>, where we must determine the optimal decision vector z^⋆ to obtain the optimal trajectory.
For brevity, we denote the TOPs in Sections <ref> and <ref> as P_0 and P_1, respectively.




















 §.§ Homotopy continuation

Under certain objectives, computing an optimal trajectory can be rather difficult; an example is when the optimal control profile u^⋆(t) is discontinuous, e.g. in the bang-bang case.
In order to decrease the burden of finding a solution, homotopy continuation, or regularisation techniques, can be employed to achieve convergence. This method is often used to solve singular-control problems <cit.>.

To elaborate further: given two different objectives, one can define a linear homotopy between them

    𝒥 = (1 - α) ∫_t_0^t_fℒ(u(t))  t + α∫_t_0^t_fℒ'(u(t))  t,

where the homotopy parameter α∈ [0,1] characterises the objectives' priorities.
Assuming ℒ characterises the easier objective, we can solve the TOP first with α = 0, then use its solution to resolve the same problem with a slightly increased value.
With this process, we iteratively solve the TOP until α = 1, at which point we will have converged to a solution under the difficult objective.
Through this process, we obtain a convergent series of solutions — a path-homotopy.
If there are several different objectives, one can extend this approach to multiple homotopies, as we do in Section <ref>.
This process is further described in Algorithm <ref>, where the boundary conditions remain constant, 𝚜𝚘𝚕𝚟𝚎(s_0,z, α) minimises (<ref>) from an initial state s_0,
and α^* is the last successful solution parameter.






[ht] 
Criteria homotopy

FMainhomotopy_criteria
FnFunction:
s_0^⋆, z^⋆, α^⋆
T, α←∅, α^⋆ 

α < 1
    z = 𝚜𝚘𝚕𝚟𝚎(s_0^⋆, z^⋆, α) 

    𝚜𝚞𝚌𝚌𝚎𝚜𝚜𝚏𝚞𝚕(z)
        z^⋆, α^⋆←z, α 

        T←T∪{(s_0^⋆, z^⋆, α^⋆) } save traj.
        α←α∈ [α^⋆, 1] increase  α 
    
    
        α←α∈ [α^⋆, α] decrease  α
    
    T



Similarly, to obtain a database that encompasses a diverse set of states, we can also utilise continuation upon the initial state.
To do this, we slightly perturb the initial state and resolve the TOP with the previous solution as the initial guess, again leading to a convergent series of solutions.
We further describe this process in Algorithm <ref>, where the homotopy parameter remains constant, δ defines the perturbation size, and n is the desired number of solutions.
Through these algorithms, we obtain a large set of TOP solutions T, that encompass the entire range of homotopy parameters and a broad range of initial states.












[ht]
    State Homotopy
    
    FMainhomotopy_state
    FnFunction:
    s_0^⋆, z^⋆, α^⋆, n
        T←∅

        |T| < n
            s_0 ←𝚙𝚎𝚛𝚝𝚞𝚛𝚋(s_0^⋆,  δ) 

            z←𝚜𝚘𝚕𝚟𝚎(s_0, z^⋆, α^⋆) 

            𝚜𝚞𝚌𝚌𝚎𝚜𝚜𝚏𝚞𝚕(z)
                s_0^⋆, z^⋆←s_0, z 

                T←T∪{(s_0^⋆, z^⋆, α^⋆) } 

                δ←𝚒𝚗𝚌𝚛𝚎𝚊𝚜𝚎(δ)
            
            
                δ←𝚍𝚎𝚌𝚛𝚎𝚊𝚜𝚎(δ)
            
        
        T
    








 §.§ Inverted pendulum swing up
 
For our first test case, we consider the simple nondimensional inverted pendulum <cit.>,

    ṡ = 
        [ẋ
    v̇
    θ̇
    ω̇]
        =
        [
            v
    
            u
    ω
    sin (θ ) - u cos (θ )] 
        = f(s, u),

where the variables are: cart position x, cart velocity v, pole angle θ, pole angular velocity ω, and control u ∈ [-1, 1].



We begin by defining a homotopic objective

    𝒥 = (1 - α)∫_t_0^t_fu(t)^2 t + α∫_t_0^t_f t ,

defined from quadratically optimal (α = 0) to time-optimal (α = 1) control. 
Using PMP, we define the Hamiltonian

    ℋ = 
        λ^⊺·f(s, u) + ℒ = 
        λ_x v +
        λ_v u +
        λ_θω
     + 
        λ_ω(sin (θ ) - u cos (θ )) + 
        (1 - α)u^2 + α,

where λ is the Lagrange multiplier vector — or costate — having the same dimensionality as the state.
We then minimise ℋ with respect to u to find the optimal control

    u^⋆ = 
        uargmin(ℋ) = 
        λ_v - λ_ωcos (θ )/2 (α - 1),

defined for α∈ [0, 1).





As we drive the homotopy parameter to unity, we find the optimal switching function

    σ =  λ_v - λ_ωcos (θ ),

defining the bounded time-optimal control

    lim_α→ 1(u^⋆) = 
      
        -1    if σ < 0 
    
        1    if σ > 0
      ,

where it should be noted that the edge case σ = 0 is only encountered instantaneously, if at all, so singular control analysis is not necessary here.
Lastly, we compute the costate equations

    λ̇ = -∇_sℋ
    =
    [0
    - λ_x
    - λ_ω(u sin (θ ) + cos (θ ))
    - λ_θ].

To encode the swingup task into the TOP, we enforce the equality constraint s(t_f) = 0. 
With the shooting method <cit.>, the decision vector becomes z = [T, λ(t_0)]^⊺, where T = t_f - t_0 is the trajectory duration and λ(t_0) is the initial costate.












Following Algorithm <ref> and considering the nominal downright state s_0 = [0 0 π 0]^⊺, we compute our initial path-homotopy between quadratically and time-optimal control, as shown in Figure <ref>.












   











 §.§ Spacecraft orbit transfer
 

As an increase in complexity of boundary conditions and dimensionality, we now consider the problem of optimising a spacecraft's trajectory from Earth to the orbit of Mars. 
Its heliocentric dynamics are given by

    ṡ =
        [ṙ
    v̇
    ṁ
    ] =
        [v
    T u/mû - μ/r^3r
    
        - T u/I_sp g_0] =
        f(s, u),

where the state variables are: position r = [x, y, z]^⊺, velocity v = [v_x, v_y, v_z]^⊺, and mass m. 
The controls are the thrust's throttle u ∈ [0, 1] and direction û = [û_x, û_y, û_z]^⊺. The constant parameters governing the dynamics are: maximum thrust capability T = 0.2  [N], specific impulse I_sp = 2500  [s], gravitational acceleration at sea level g_0 = 9.81  [m · s^-1], and standard gravitational parameter of the sun μ = 1.3271× 10^20  [m^3 · s^-2].

We again construct a homotopic objective,

    𝒥 = (1 - β) ∫_t_0^t_f u(t)^2 t
        +
        β∫_t_0^t_f(α + u(t) (1 - α)) t,

where we have added a secondary homotopy parameter β∈ [0, 1] as a means to feasibly arrive to a solution of the problem at α∈ [0, 1], β = 1.
The homotopies between optimal controls are defined as:
quadratic to effort α = 0, β∈ [0, 1],
quadratic to time α = 1, β∈ [0, 1],
and effort to time α∈ [0, 1], β = 1.
Using PMP, we then define the Hamiltonian

    ℋ = 
        
        λ_r ·v +
        λ_v ·(T u/mû - μ/r^3r) + 
        λ_m (- T u/I_sp g_0)  
    
        + 
        β(α + u (1 - α)) + u^2(1 - β)

To  find the optimal thrust direction û^⋆, we isolate the portion of ℋ that depends on it:
T u/m( û·λ_v ).
Considering that T, m, and u are positive, û^⋆ must be directed opposite of λ_v to be a minimiser:

    û^⋆ = - λ_v/λ_v.

Substituting û^⋆ into ℋ and minimising it with respect to u, we obtain 

    u^⋆ = 
        1/2(1-β)(
        T (λ_v ·λ_v)/λ_v  m 
        +
        T λ_m/ I_sp  g_0 +
        β(α - 1)
        ),























defined for α∈ [0, 1], β∈ [0, 1).
Driving β to unity, we find

    σ = 
        λ_v ·λ_v
        /
            m λ_v
         +
        λ_m +  1-α,

defining the bounded control

    lim_β→ 1( u^⋆) = 
      
        0    if σ < 0 
    
        1    if σ > 0
      ,

expressing the homotopy between effort- (α = 0) and time-optimal control (α = 1), which are discontinuous.
Finally, we compute

    λ̇ =
    [μ/r^3λ_v - 3μ/r^5( λ_v ·r) 
    
        - λ_r 
    T u/m^2( λ_v ·û)
    ].

To finish defining the TOP, we enforce two transversality conditions.
The first is to allow the optimiser to choose the best final mass
λ_m(t_f) = 0,
since it is affected by the control.
The second is to allow the optimiser to choose the best terminal state along Mars's orbit

    r^3 ( λ_v ·v)
            - μ( λ_r ·r)
        /√(μ^2 ( r·r) +
                (v·v) (r·r)^3) = 0,

see <cit.> for details.
The decision vector becomes 
z = [T, M(t_f), λ(t_0)]^⊺, where T = t_f - t_0 is the time of flight, M(t_f) is the mean anomaly of the final orbit, and again λ(t_0) is the initial costate.
Following Algorithm <ref> again and considering Earth's position and velocity as the initial state, and m(t_0)=1000 [kg], we compute each homotopy path, as shown in Figures <ref>–<ref>, for an interplanetary transfer to somewhere along Mars's orbit[We pick the starting time to be 0   [MJD2000], using Modified Julian Date notation.].




















































































§ LEARNING
 

In this section we describe the generated datasets in detail, outline the implemented NN architectures, and evaluate the results of the learning process.



 §.§ Datasets


To assemble the datasets, we first solve the TOPs from their nominal initial states, as described in Sections <ref> and <ref>.
From these initial states, we employ Algorithm <ref> to obtain a set of solutions T from various other initial states, under the nominal homotopy parameter configurations: α = 0 and α = β = 0 for P_0 and P_1, respectively.
We then employ Algorithm <ref> to obtain solutions across the homotopy parameters' ranges.
Finally, we numerically integrate[We use an adaptive-stepsize Runge-Kutta method of order 8(5,3) <cit.>.] these systems with the solutions' parameters to obtain datasets of the form D = {(s, α, u)_i ∀ i} — D_0 with 945 trajectories (i=577,480) from P_0, and D_1 with 5,525 trajectories (i=2,124,668) from P_1 (β = 1).


































 §.§ Neural network architectures

We augment the state-feedback controller formalism to synthesise multilayer-perceptron (MLP) policies of the form π(s, α) ↦u^⋆.
We denote π's hidden architecture with m × n, describing n hidden layers, each having m nodes.
For every hidden layer we sequentially apply the standard NN operations: linear transformation, layer normalisation, and softplus — <cit.> indicated that softplus activations result in smoother and more meaningful control strategies.
For brevity, we denote P_0's and P_1's instances of π as π_0 and π_1, respectively.
We apply the hyperbolic tangent in the last layer and use its output to approximate the controls of P_0 and P_1:
π_0(s, α) ∈ [-1,1] = u
and
π_1(s, α) ∈ [-1,1]^3
↦ [u ϕ θ]^⊺∈ [0,1]×[0,π]×[0,2π]
↦ u · [
    sin(θ)cos(ϕ),
    sin(θ)sin(ϕ),
    cos(θ)
]^⊺
= u ·û,
respectively.




























    


    


    


    



    


    



    



    



    









    
















 §.§ Evaluation

For π_0 and π_1, we consider the hidden architectures:
50 × 2, 50 × 4, 100 × 2, 100 × 4.
We train these MLPs on their respective datasets — D_0 and D_1 — for 10,000 episodes with the Adam optimiser <cit.>, with a learning rate of 10^-3, a decay rate of 10^-5, and a mean-squared error (MSE) loss function. At each training epoch, we randomly sample 20,000 datapoints, reserving 10% for validation.
The training history and regression performances are shown in Figure <ref> and Table <ref>, respectively.

Since success in the regression task does not necessarily mean that the policy-controlled systems will behave as expected, we also evaluate the policy trajectory optimality metric, described in <cit.>, indicating the optimality-difference between a policy-controlled and optimal trajectory from an initial state.
From the systems' nominal initial states, we evaluate this metric across the hidden architectures and a range of homotopy parameters — shown in Table <ref>.
















Across all architectures, we find that π_0 and π_1 perform well in the regression task and result in trajectories that are near-optimal with respect to the true optimal, across a range of homotopy parameters.
We observe that increasing node breadth and number of layers generally results in slightly better regression performance as well as trajectory optimality.
































	














	


















































































§ CONCLUSIONS
 
We presented a novel approach to synthesising objective-conditioned near-optimal policies through trajectory optimisation, homotopy, and imitation learning.


We further demonstrated that these state-feedback policies produce trajectories that are near-optimal, and can adapt their behaviours online to changing objectives, due to unexpected events or user-defined preferences.























§ ACKNOWLEDGEMENT

This  work  was  supported  by  Stiftelsen  for StrategiskForskning  
(SSF)  through  the  Swedish  Maritime Robotics Centre (SMaRC) (IRC15-0046).

IEEEtran


