The Sequential Convex Programming (SCP) Toolbox provides a suite of fast nonlinear optimal control algorithms for solving nonconvex trajectory generation tasks. These algorithms have been applied on problems for organizations such as NASA, SpaceX, Blue Origin, and Masten Space Systems. This post shows you how to get started with the SCP Toolbox by solving a simple obstacle avoidance problem for Dubin’s car model.
The SCP Toolbox is the result of a comprehensive tutorial paper that recently appeared in preprint. The paper covers the complete algorithmic and practical details of the low-level solvers that power the SCP Toolbox. In this post, I will just scratch the surface by providing a simple and concrete implementation of a trajectory generation problem. Keep in mind that this example does not reveal the full generality of the toolbox. If you are interested to learn more about SCP algorithms, I would really recommend that you supplement reading this post with the tutorial paper.
Purpose of the SCP Toolbox
The goal of the SCP Toolbox is to solve nonconvex optimal control problems of the following form:
This is known as a “template” optimal control problem because it defines a whole family of problems that can be solved by the SCP Toolbox. Note that the SCP Toolbox is capable of solving much more general problems as described in the previously mentioned tutorial paper. To keep this introductory post reasonably simple and to-the-point, I’ll only talk explicitly about problems of the form (??)–(??).
Let’s define the elements of this optimal control problem. The vectors , , and represent the state, input, and other “static” (in other words, time-independent) parameters. The function is the running and has to be convex. The vector function defines the path constraints, which can be nonconvex. Finally, the vector functions and denote the initial and terminal boundary conditions, which can also be nonconvex. Importantly, note that the trajectory evolves on a “normalized” time interval . This means that the user has to convert their problem’s “absolute” time to this normalized time convention. We will discuss this nuance explicitly when it comes to implementing the Dubin’s car problem.
Why Another Toolbox?
The SCP Toolbox joins a growing family of tools for solving
nonconvex trajectory problems. These include
GPOPS-II. All of these tools solve some close or
distance relative of the optimal control problem
(??)-(??). Some of the tools target a real-time
implementation (such as
OCS2) while others are oriented towards an accurate
but not real-time solution (such as
GPOPS-II). Perhaps even more importantly,
GPOPS-II implement algorithms that are generally not aimed at
real-time performance, while tools like
algorithms that can perform in real-time using an optimized implemenetation in
a compiled language like C++.
The contribution of the SCP Toolbox is to provide a high-level parser-solver interface for a suite of promising new algorithms that are not accessible using existing tools. These algorithms are:
- Lossless convexification (also known as LCvx, see the paper by Acikmese and Ploen, 2007).
- Successive convexification (also known as SCvx, see the paper by Mao et al., 2019).
- Penalized Trust Region method (also known as PTR, see the paper by Szmuk et al., 2019).
- GuSTO, see the paper by Bonalli et al., 2019.
Under the hood these algorithms use some user-selected convex optimizer, for
example an Interior Point Method-based solver like
ECOS. LCvx is a “vanilla” convex
optimization algorithm in the sense that the solver is called either once or
some pre-determined (small) number of times11The LCvx algorithm is
quite different to the other three algorithms, which are SCP methods. Hence it
sits a little awkwardly in an “SCP Toolbox”, but it is included anyway due to
being part of the tutorial paper from which
the SCP Toolbox originated. Just keep in mind that the descriptions in this
post apply to the SCP algorithms, while LCvx kind of “sits apart” from that
code (for example, the LCvx examples do not use the SCP “parser interface”
discussed in this post). . The latter three algorithms are known as SCP
methods, and they call the optimizer as part of a trust region optimization
method. All four algorithms are capable of real-time performance, as
demonstrated in Scharf et al., 2017,
Dueri et al., 2017, and Reynolds et
al., 2020. While the SCP Toolbox does
solve problems quickly (on the order of seconds), it is first and foremost
aimed at providing a generic, clean, and transparent implementation of the
algorithms. Real-time implementations, unfortunately, tend to be terse and
non-generic because they exploit specific problem structure. Therefore the SCP
Toolbox trades some performance in favor of clarity and generality.
The theoretical guarantees and computational speed offered by convex optimization have made LCvx and SCP algorithms popular in both research and industry circles. LCvx, SCvx, and PTR have all been used in projects for NASA and Masten Space Systems, resulting in Xombie rocket flight tests and a Blue Origin experimental flight. The algorithms were also applied independently to research problems relevant for SpaceX rockets, such as the Starship landing flip maneuver. In robotics, all three SCP methods were used to control quadrotors and microgravity flying assistant robots. To summarize:
The purpose of releasing the SCP Toolbox is to provide public implementations of lossless convexification and sequential convex programming algorithms that have had marked success in modern nonconvex trajectory research and development.
First things first, I will assume the following things about your computer:
- You are running Ubuntu 20.04
- You have Julia 1.6.1 installed
If something doesn’t work, it is likely due to the above criteria not being met. If you cannot resolve an issue, please reach out to me.
Let’s begin by downloading the SCP Toolbox from its GitHub
repository. Note that I
link to the
jgcd branch intentionally, since this branch contains the latest
code from the recent JGCD paper. In the
near future, this branch will be merged to
master, so stay tuned. For
simplicity, I’ll work in the
/tmp/ directory, but you may choose any
directory you like.
$ cd /tmp/ $ git clone https://github.com/dmalyuta/scp_traj_opt $ cd scp_traj_opt $ git checkout jgcd
Congratulations! You now have the SCP Toolbox on your computer. In the next section, we will discuss the file structure of the toolbox.
If you navigate to
/tmp/scp_traj_opt/ and run
tree -L 1 then you will see
the following file structure (I am only showing the folders):
. ├── solvers ├── parser ├── utils ├── examples ├── test └── figures
You should typically not have to touch any of these folders. However, to help you work with the toolbox, it is instructive to describe the purpose of each folder:
solvers/directory contains the code of the low-level algorithms that I mentioned previously: SCvx, PTR, and so on. These solvers are generic in the sense that they expect optimal control problems to be defined using some “obscure” functions and matrices… not fun. A version of this generic problem was given above by (??)–(??). As a user, you want to specify problem in a higher-level code, which is where the next directory comes into play.
parser/directory contains code for the parser interface. The SCP Toolbox makes the reasonable assumption that as a user, your focus is on modeling and solving actual problems, not on developing the underlying algorithm that carries out the solution. Hence, code in
parser/implements a kind of front-end interface that allows you to specify your nonconvex trajectory generation problem. This will be communicated behind the scenes to the underlying solvers in
solvers/, and you won’t have to do any of that “ugly” legwork.
utils/directory contains general code for common functions and objects that are used throughout the rest of the code. Hence you will find here a quaternion object, a bunch of geometric objects such as sets, interpolatable continuous-time trajectories, plotting functions, etc.
examples/directory contains solved examples of nonconvex trajectory generation problems. Each example has its own folder. For instance, the SpaceX Starship flip maneuver is located in
examples/src/starship_flip/. You can think of these examples as unit tests for the SCP Toolbox that confirm that the rest of the code works as intended. They are also there for you to learn from to implement your own problems. However,
examples/is not where you should be implementing your own problems. In fact, this post is all about teaching you where and how to implement your problem correctly!
One other important thing about the examples. The reason that the
jgcdbranch has not yet been merged to
masteris because the examples in
rendezvous_planar/have not yet been updated to the latest versions of the solver and parser code. The latest versions broke some of the backwards compatibility, hence the issue. In all the other working example folders, you will find the following file structure:
. ├── parameters.jl ├── definition.jl ├── plots.jl └── tests.jl
In the same order as above, these files implement: the problem data structures, the problem definition functions (by using the parser code in
parser/), the results plotting functions, and the unit test functions for solving an instance of the problem.
tests/directory contains a single test file that runs all of the unit tests – in other words, all the examples from the
figures/directory should currently be empty except for a
.gitkeepfile. The results plots that are generated by the examples are placed in this directory as PDF files.
Okay, so now you know the lay of the land inside
/tmp/scp_traj_opt/. Like I
said above, you should not be editing this directory22Unless you are
developing your own solver or parser code and want to submit a pull
request… in which case thank you and I’d like to buy you dinner. ! This
leaves a question – where should you implement your own problems, and how are
you to interface with the SCP Toolbox directory? This is covered in the next
Implementing Your Own Problem
At this point we have the SCP Toolbox available in the
directory. To implement your own problem, let’s talk a little bit about how
Julia code is structured. Code is generally divided into
packages. These are
standard file structures for defining code modules (namespaces, roughly
speaking), test scripts, and so on. The SCP Toolbox implements four packages33Technically speaking these are “subpackages”, because the SCP Toolbox
is itself a package. But this technicality doesn’t really matter for us. :
Solverspackage, which is implemented in the toolbox
Parserpackage, which is implemented in the toolbox
Utilspackage, which is implemented in the toolbox
Examplespackage, which is implemented in the toolbox
To implement your own problem, you need to do two things (at least this has been my personal workflow – once you get this to work, feel free to explore workflows that work better for you):
- Create your own (new) package that holds your problem code.
- Import the
Parserpackages at the bare minimum, and usually also the
Utilspackage because it provides helper functions for math, plotting, etc.
Let’s begin with the first step. If you don’t care for the details, I have
actually created a GitHub
repository that you may clone and
that contains all of the code that we are about to write. However, I do
recommend that you follow these steps manually at least once. I’ll create the
package in the
/tmp/scp_new_problem/ directory. Begin by navigating into
/tmp/ and starting Julia:
$ cd /tmp $ julia
You should now see the Julia read-eval-print loop (REPL). Each prompt line
julia>. If you type
] then you should see the package manager
(@v1.6) pkg>), from which you can exit using backspace. Similarly you
; to enter shell mode (Bash in my case, and the prompt looks like
shell>). Look out for these prompts as you follow along. Ready to create a
new package for solving Dubin’s car problem? Let’s go.
(@v1.6) pkg> generate scp_new_problem shell> cd scp_new_problem (@v1.6) pkg> activate . (scp_new_problem) pkg> dev ../scp_traj_opt/solvers/ (scp_new_problem) pkg> dev ../scp_traj_opt/parser/ (scp_new_problem) pkg> dev ../scp_traj_opt/utils/ (scp_new_problem) pkg> precompile
Note above the changes in the left-hand side prompt. Importantly, after the
activate command the prompt changed to
scp_new_problem to indicate that we
are now working inside the new package’s environment. Keep this REPL open, we
will come back to it shortly after editing some files.
To briefly describe the steps taken above, we first generate a new empty
package. Next, we switch to the package’s directory and activate it. This
switches the context for the subsequent commands from the “global” Julia
environment to that of the new package. Next, the sequence of
make the new package dependent on the SCP Toolbox packages that we mentioned
above. Finally, the
precompile step performs ahead-of-time compilation of the
new package and its dependencies. This can save some time later when our code
is executed (Julia tends to compile code just-in-time).
If you run the
tree command line application inside
you should see the following file structure:
. ├── Manifest.toml ├── Project.toml └── src └── scp_new_problem.jl
src/ folder is where you will implement your problem code. Let’s begin by
changing the contents of the auto-generated file
scp_new_problem.jl to the
1 2 3 4 5 6 7 module scp_new_problem include("./my_problem.jl") end # module using .scp_new_problem scp_new_problem.solve()
Let’s also create a new file
my_problem.jl in the
src/ directory, and
populate it with the following initial code:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 using Solvers using Parser using Utils using LinearAlgebra using ECOS using PyPlot using Colors using Printf export solve function solve() @printf("Problem code goes here...") return 0 end
Lines 1-3 import the SCP Toolbox packages that we mentioned earlier. This gives
you access to the gamut of functionality of the SCP Toolbox for solving
nonconvex trajectory generation problems. Lines 5-9 import other standard
packages that we will make use of later (for example, the
provides macros for C-style print statements via
@printf). Line 11 exports
solve() function from the module. This function is where we will define
and solve the Dubin’s car trajectory generation problem using the SCP Toolbox
parser interface. From now on, because this is not a Julia language tutorial, I
will assume that you have a working knowledge of Julia and I’ll stop commenting
too much on the Julia language-specific features.
Let’s now return to the Julia REPL that we opened previously. Remember the
state in which we left the REPL: we activated the
and the REPL was opened inside the
/tmp/scp_new_problem/ directory. Now
execute the following commands:
shell> cd src/ julia> include("scp_new_problem.jl");
If you see “
Problem code goes here...” printed out, then congratulations!
You have successfully loaded the relevant SCP Toolbox packages and executed the
solve() function, which printed the placeholder statement. In the next
section, we will populate the
solve() function with the definition of the
Dubin’s car obstacle avoidance problem, and solve it using the PTR algorithm.
Like I mentioned previously, note that I have made the
directory available for download through
GitHub. You can, for instance,
clone this repository for each new trajectory problem that you want to
solve. If you are more Julia savvy, you can also work on multiple problems at
once by continuing to create more files and complex structures inside
One more thing is worth mentioning about the GitHub repository. Once you
download it and start a new REPL, you will have to use the
in order to switch Julia’s context to the
scp_new_problem package before
you can run any code. For example, suppose that you cloned the repository into
DIR and that you have a terminal shell open inside of
DIR/src/ (the source
file directory). When you start Julia using the shell’s
julia, the REPL will
be automatically opened in the
DIR/src/ directory. You can then activate the
scp_new_problem package using:
(@v1.6) pkg> activate ..
You will see the prompt change to
(scp_new_problem) pkg> after this
command. Note that we are using
.. instead of
. like before. This is
because the REPL is inside the
src/ directory, and package activation
requires pointing to the directory that contains the
Project.toml file which
is located in the parent directory of
Dubin’s Car Problem
I will now walk through an implementation of the
solve() function from the
last section. The goal is to solve an obstacle avoidance trajectory problem
using Dubin’s car model. To begin, let’s write down the mathematical
formulation of the problem. The setup can be visualized as shown in Figure 1.
Dubin’s car is a simple agent that moves in a 2D plane. For trajectory generation we get to decide the forward velocity and the turn rate . Using these control inputs, we want to steer the car from an initial to a final position while avoiding the green circular obstacle. Long story short, this results in the following optimal control problem:
Let’s discuss this problem a bit. Equations (??)–(??) are the equations of motion (also known as the dynamics) for Dubin’s car. The SCP Toolbox computes dynamically feasible trajectories, and the equations of motion define what exactly “dynamic feasibility” means. Equation (??) defines the nonconvex obstacle avoidance constraint. Here, the obstacle is a circle of radius meters that is centered at the position . Finally, equations (??) and (??) define the boundary conditions. The car starts at the origin facing north, and should end up 2 meters along the axis facing the same direction. Note that we have confined the optimization to look for a 3 second trajectory that minimizes the control usage according to the cost function in equation (??). Intuitively, we are looking for a “mild” trajectory that drives the car to its destination “with least effort”.
Now that we have an optimal control problem that specifies the desired trajectory, it is time to plug it into the SCP Toolbox. We do this by using the previously mentioned parser interface to convert equations (??)–(??) into the template form (??)–(??).
The purpose of the SCP Toolbox parser in
/tmp/scp_traj_opt/parser/ is to
provide a set of high-level functions for defining
(??)–(??) given your application-specific
optimal control problem (??)–(??). Going back to
solve() function from the last section, we will now replace the
placeholder with the parser code for our Dubin’s car trajectory problem. Begin
by changing the
solve() function to the following:
1 2 3 4 5 function solve() pbm = TrajectoryProblem(nothing) problem_set_dims!(pbm, 3, 2, 1) # More code... end
On line 2 we initialize the optimal control problem as a
structure, which is defined in the
Parser package of the SCP Toolbox. The
TrajectoryProblem accepts one generic argument that can hold
problem-specific data, such as a custom dictionary or a structure that is
totally up to you to define. Dubin’s car problem is pretty simple so we will
stick to global variables for this tutorial – hence we will pass
here. For examples where a custom structure is passed to the
TrajectoryProblem constructor, see the aforementioned
of the SCP Toolbox.
On line 3, we begin by defining the state, input, and parameter dimensions – in other words, the values of , , and from equations (??)–(??). For Dubin’s car we have three states (), two inputs (), and no static parameters that need to be optimized. However, the SCP Toolbox is currently limited to due to implementation edge cases that need to be handled explicitly when either value becomes zero. Hence we have to set even though mathematically we know that . This means that we will need to stay cognizant of the fact that a one-dimensional “dummy” parameter vector will appear in the underlying optimal control problem, even though none of our constraints nor cost end up using it.
Quick sidenote. To not have to keep writing the complete
definition every time, the code snippets that I provide from now on will be
# More code... comment on line 4 above.
The low-level algorithms of the SCP Toolbox require an initial trajectory guess. As discussed in the associated tutorial paper, SCP algorithms have the nice property that this guess can typically be very simple. In the case of Dubin’s car, we use a straight-line trajectory from start to finish as our guess (even though this trajectory is infeasible with respect to the obstacle constraint). This is done using the code below.
1 2 3 4 5 6 7 8 9 10 _x0 = [0; 0; 0] # Initial state _xf = [0; 2; 0] # Terminal state problem_set_guess!( pbm, (N, pbm) -> begin x = straightline_interpolate(_x0, _xf, N) idle = zeros(pbm.nu) u = straightline_interpolate(idle, idle, N) p = zeros(pbm.np) return x, u, p end)
As you can see, we use the function
problem_set_guess! that is
provided by the parser to set the initial guess. This accepts an anonymous
function whose argument
N corresponds to the number of discrete time nodes in
the trajectory. On line 5 we perform a straight-line interpolation for the
state between the initial and terminal states. We do the same on line 7 for the
input, for which we use a “zero velocity, zero turn rate” initial guess. The
parameter vector is also set to zero on line 8. Remember that although we do
not have any parameters in the mathematical problem, we had to set the
parameter to a dimesion of due to the current limitations of how the
SCP Toolbox works. Therefore, we use
zeros(pbm.np) in order to define a zero
vector of length one for the parameter guess.
Note that in the code above we are using two “global” variables
that define the initial and final conditions according to (??) and
(??). Like I mentioned above, we could have also opted for storing
these variables in a dictionary or a structure that we then pass to the
TrajectoryProblem constructor. Here is an example code sketch:
1 2 3 4 5 6 7 8 9 data = Dict(:x0 => [0; 0; 0], :xf => [0; 2; 0]) pbm = TrajectoryProblem(data) # More code ... problem_set_guess!( pbm, (N, pbm) -> begin data = pbm.mdl x = straightline_interpolate(data[:x0], data[:xf], N) # More code ... end)
1 2 problem_set_running_cost!( pbm, :ptr, (t, k, x, u, p, pbm) -> u'*u)
The parser function
problem_set_running_cost! is responsible for defining the
function from (??). Like before, this is done with a
user-defined anonymous function that computes the running cost value. This
function has to accept a bunch of different arguments, but for this example I’d
like to focus on the only relevant argument
u, which is the input vector
value at the normalized time
t. Previously we defined a two-dimensional input
vector . Comparing this vector with equation
(??), we realize that the running cost is quite simply the dot
product . This is exactly what we enter on line 2 in the above
Our next challenge is to define the equations of motion, also known as the dynamics. This amounts to translating the differential constraints (??)–(??) into the generic form (??). This is done using the parser as follows:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 _tf = 3 problem_set_dynamics!( pbm, # f (t, k, x, u, p, pbm) -> [u*sin(x); u*cos(x); u]*_tf, # df/dx (t, k, x, u, p, pbm) -> [0 0 u*cos(x); 0 0 -u*sin(x); 0 0 0]*_tf, # df/du (t, k, x, u, p, pbm) -> [sin(x) 0; cos(x) 0; 0 1]*_tf, # df/dp (t, k, x, u, p, pbm) -> zeros(pbm.nx, pbm.np))
Evidently this step is a bit more complicated than what we’ve seen thus far. Four anonymous functions are defined: the first function defines in (??) and the remaining three functions define its Jacobians , , and . The Jacobians are needed because sequential convex programming works by linearizing nonconvex parts of the problem, and this linearization (which is a first-order Taylor series expansion) needs the Jacobian values. By defining the three-dimensional state vector as (please excuse the overloaded notation for the -position), we can write the dynamics (??)–(??) as follows:
There is one issue that remains, and this is the conversion between absolute and normalized time. Equation (??) is written in absolute time, which runs like the clock on your wall (if you have one). Recall that (??)–(??) is defined on normalized time, which invariably runs on the time interval. Let’s denote the normalized time by and the absolute time by . Let’s also generalize the Dubin’s car problem statement a bit and say that the trajectory we’re looking for runs on where is the “final time” (in our case, ). We hence have the relationship between absolute and normalized time, and we can use the chain rule to write:
Equation (??) reveals a simple relationship between the equations of motion expressed using the two “times”: to get the dynamics in normalized time, multiply the absolute time dynamics by . You can see that lines 6-8 exactly implement the original equations of motion (??) scaled in this way by . I’ll assume that you are familiar with differential calculus, so the remaining three anonymous functions for the Jacobians of should be self-explanatory.
1 2 3 4 5 6 7 8 9 10 11 12 13 _ro = 0.35 _co = [-0.1; 1] _carw = 0.1 problem_set_s!( pbm, :ptr, # s (t, k, x, u, p, pbm) -> [(_ro+_carw/2)^2-(x-_co)^2-(x-_co)^2], # ds/dx (t, k, x, u, p, pbm) -> collect([-2*(x-_co); -2*(x-_co); 0]'), # ds/du (t, k, x, u, p, pbm) -> zeros(1, pbm.nu), # ds/dp (t, k, x, u, p, pbm) -> zeros(1, pbm.np))
The parser function
problem_set_s! works very similarly to the function
problem_set_dynamics! that we used for the dynamics. Four anonymous functions
are defined, this time for the function in (??) and its
Jacobians , , and . It should be easy to
relate the code snippet to (??). The first anonymous
function simply writes the function as a one-dimensional vector. The other
three anonymous functions compute the Jacobian using the standard differential
calculus definition. Note that given , we have (in other words, a row vector), and similarly for
the other Jacobians.
An interesting bit of trajectory engineering heuristics occurs on line 7, and
it is worth mentioning. The physical car is not a point object, so a collision
will occur if it approaches the obstacle too closely. We define the variable
_carw to store the car body’s width. Then, we “inflate” the physical obstacle
of radius meters by half of the car’s width. This results in an
effective keep-out circular zone of radius meters. The car will no longer
approach the obstacle too closely, and collisions will be avoided as long as
the car’s motion is sufficiently tangential to the obstacle.
1 2 3 4 5 6 7 8 problem_set_bc!( pbm, :ic, # Initial condition (x, p, pbm) -> x-_x0, (x, p, pbm) -> I(pbm.nx)) problem_set_bc!( pbm, :tc, # Terminal condition (x, p, pbm) -> x-_xf, (x, p, pbm) -> I(pbm.nx))
problem_set_bc! allows us to set both the initial conditions
(when its second argument is
:ic) and the terminal conditions (when its
second argument is
:tc). Two anonymous functions are passed: one defining the
boundary condition function itself (either or ) and another
defining its Jacobian with respect to the state (either or
). In the case where the functions also depend on the parameter
vector , a third anonymous function has to be passed that defines the
corresponding Jacobian. This is not the case for this simple problem, but you
may check out the other examples in
/tmp/scp_traj_opt/examples/src/ to see
how that functionality works. Comparing (??) and (??)
with the code snippet, we can see that the code is self-explanatory (the
Jacobians in this case are the identity matrix, since the full state is being
constrained at both endpoints).
Congratulations, at this point we are done using the parser to convert the
Dubin’s car optimal control problem (??)–(??) to the
generic form (??)–(??). The next step is to define
the solver algorithm parameters. This is done by populating the
structure for one of the SCP solvers that is available in the toolbox (PTR,
SCvx, of GuSTO). We will use the PTR solver, which is implemented in the file
1 2 3 4 5 6 7 8 9 10 11 12 N, Nsub = 11, 10 iter_max = 30 disc_method = FOH wvc, wtr = 1e3, 1e0 feas_tol = 5e-3 ε_abs, ε_rel = 1e-5, 1e-3 q_tr = Inf q_exit = Inf solver, options = ECOS, Dict("verbose"=>0) pars = Solvers.PTR.Parameters( N, Nsub, iter_max, disc_method, wvc, wtr, ε_abs, ε_rel, feas_tol, q_tr, q_exit, solver, options)
Let’s go through the options step-by-step (note that some documentation on each
option is provided in the comments of the
N, Nsub: the first parameter defines the discretized trajectory’s number of temporal grid nodes, and the second parameter defines the number of steps used by the Runge-Kutta (RK4) algorithm to integrate “in between” the temporal grid nodes.
iter_max: the maximum number of SCP iterations.
disc_method: the temporal discretization method used. Currently
IMPULSEare implemented. The first method linearly interpolates the input values in between the temporal grid nodes, while the second method assumes that the inputs are “impulsive” – in other words, each input is a scaled Dirac impulse that occurs at the discrete temporal nodes. Roughly speaking, I use
FOHfor control inputs that should be continuous, while
IMPULSEwas implemented specifically for optimizing spacecraft trajectories with pulse-fired reaction control system thrusters. The code is written generically enough to allow for more discretization methods to be implemented.
wvc, wtr: these are the virtual control and trust region penalty weights for the augmented cost function. See the tutorial paper for more information on these. These penalties are fundamental to SCP algorithms, but their discussion will take us too far away from the purposes of this tutorial.
feas_tol: this threshold governs how small the RK4 integration error across a single time interval must be in order to validate that a trajectory is dynamically feasible. Setting this to zero is unrealistic, since there will always be a tiny amount of numerical error.
ε_abs, ε_rel: these are the absolute and relative thresholds for the convergence stopping criterion. Again, I would suggest checking out the tutorial paper for more information.
q_tr: the type of norm used for the trust region constraint in the intermediate optimization subproblems that SCP generates. You have several options:
1: use the L1 “Taxicab” norm, in other words .
2: use the L2 “Eucledian” norm, in other words .
4: use the L2 norm squared, in other words .
Inf: use the L-infinity “Maximum” norm, in other words . This is the norm that I’ve had most consistent success with, since it returns directly the value of the “largest” element in the vector (which I’m using as a placeholder here for whatever quantity).
q_exit: again a norm choice, with the same options, except this one is used for the stopping criterion.
solver: which convex solver do you want to use? Roughly speaking, as long as the solver can be called by the JuMP.jl library, you can put it here44The caveat here is that the solver has to support the kinds of convex constraints that you are using in your optimal control problem. For example, if you use second-order cone constraints in combination with a quadratic problem solver, you’ll get an error during the solution process. .
options: a dictionary of options that the solver accepts. In the above code, I set a
verboseflag to prevent the
ECOSsolver from printing out its solution process every time that it is called.
Congratulations, you’ve now “configured” the PTR solver and, believe it or not, it is ready to be used for Dubin’s car trajectory generation. So, are you ready to solve the problem? Here’s how you do it:
1 2 ptr_pbm = Solvers.PTR.create(pars, pbm) sol, history = Solvers.PTR.solve(ptr_pbm)
The first line creates the problem. The function
defined by the PTR solver, and instantiates a
ptr_pbm object based on two
inputs. The argument
pars specifies the PTR algorithm parameters that we just
pbm specifies the optimal control problem that we defined
using the parser functions (note how the first argument of all the
problem_set_* functions from before was
pbm – these functions were
operating on the internals of the
The second line solves the optimal control problem, and returns the solution
sol) and a history of intermediate iterates of the algorithm (
my four-year-old Dell XPS 13 laptop, the whole solution process takes less than
one second. The output in the Julia REPL should look something like this:
k | status | vd | vs | vbc | J | ΔJ % | Δx | Δu | Δp | δ | dyn | ηx | ηu | ηp ---+----------+-------+-------+-------+-----------+-------+-------+-------+-------+-------+-----+-------+-------+------ 1 | OPTIMAL | 3e-11 | 1e-01 | 1e-13 | 1.66e+01 | | 3e-01 | 2e+00 | 0e+00 | 3e-01 | T | 0.28 | 2.08 | 0.00 2 | OPTIMAL | 3e-12 | 2e-12 | 2e-14 | 8.40e+00 | 49.35 | 1e+00 | 5e+00 | 0e+00 | 1e+00 | F | 1.29 | 5.17 | 0.00 3 | OPTIMAL | 8e-15 | 4e-16 | 5e-15 | 2.69e+00 | 67.91 | 8e-01 | 4e+00 | 0e+00 | 8e-01 | F | 0.85 | 4.12 | 0.00 4 | OPTIMAL | 2e-12 | 3e-13 | 3e-13 | 1.13e+00 | 58.12 | 1e-01 | 3e-01 | 0e+00 | 1e-01 | T | 0.12 | 0.33 | 0.00 5 | OPTIMAL | 5e-12 | 5e-13 | 5e-13 | 9.84e-01 | 12.76 | 1e-02 | 5e-02 | 0e+00 | 1e-02 | T | 0.01 | 0.05 | 0.00 6 | OPTIMAL | 4e-12 | 1e-12 | 4e-13 | 9.54e-01 | 3.06 | 3e-04 | 2e-03 | 0e+00 | 3e-04 | T | 0.00 | 0.00 | 0.00 7 | OPTIMAL | 3e-08 | 9e-09 | 1e-09 | 9.54e-01 | 0.08 | 7e-08 | 6e-08 | 0e+00 | 7e-08 | T | 0.00 | 0.00 | 0.00
If this is roughly what you got on your computer (to within any small numerical
error, perhaps), then great work! PTR just solved the Dubin’s car problem in 7
iterations, and the solution is stored in the
variables. Before poring over some pretty results plots, it’s helpful to
decypher what this progress table is telling us55Note that each SCP
solver has a slightly different progress table, based on the runtime variables
that are relevant for the specific solver. :
k: the current iteration number.
status: the internal status of the convex optimizer (
ECOSin the present case).
vd, vs, vbc: these give the L-infinity norms of the dynamics virtual control (
vd) and the nonconvex constraint virtual buffers (
vsfor the path constraints (??), and
vbcfor the boundary conditions (??)–(??)). If the algorithm is converging, these values should become very small (of a “numerical error” magnitude, such as ).
J, ΔJ: these give the augmented cost function value and the percent change in this value relative to the previous iteration. The “augmented” cost function is a sum of the original cost function (??) and the virtual control and trust region penalty terms (see the tutorial paper for details). As the algorithm converges, these penalty terms become quasi-zero so
Jgradually starts to reflect the original cost function’s value. If you see positive values in the
ΔJcolumn, it means that the cost function decreased relative to the last iteration. Since we seek to minimize the cost function, seeing positive values is good. However, it is normal if there are iterations where the cost function increases – this is simply part of the algorithm’s search for a locally optimal solution.
Δx, Δu, Δp: similar to
vbc, these values give the L-infinity norm deviations of the current solution’s state/input/parameter vectors from the previous solution. If the current iteration’s solution is very close to the last, these values will be small. Note that these values decrease as the iterations progress, which is indicative of the algorithm converging.
δ: this value measures the “distance” by which the current solution has moved away from the previous iteration’s solution. The measurement is done using a norm, and depends on the algorithm parameter
q_exitfrom before. Note that this distance is given in terms of scaled state/input/parameter variables. I leave the details of variable scaling for the tutorial paper, since scaling is an important matter all by itself in the numerical optimization literature.
Tsignifies that the current solution is dynamically feasible, while
Fmeans that the numerically integrated trajectory does not sufficiently match the one output by the optimization, according to the
ηx, ηu, ηp: these give the L-infinity norms of the state/input/parameter trust regions. As the algorithm converges, the trust regions shrink to zero, since the trust region size is penalized in the cost and it upper-bounds the amount by which the current solution is allowed to deviate from the previous iteration’s solution. Note how
ηpis always zero, since we never actually use the “dummy” one-dimensional parameter vector (it is initialized to zero, and stays that way for the whole solution process).
We have now solved the Dubin’s car trajectory generation problem, and we
understand how to read the progress table. It is time to generate some plots of
the computed trajectory. This boils down to extracting the relevant data from
history and plotting it using your library of choice. Many examples
are available already in the SCP Toolbox, and you can check them out in
/tmp/scp_traj_opt/examples/src/. For this tutorial, let’s use
PyPlot.jl to draw the Dubin car’s
trajectory. There are many ways to do this, and in this tutorial I’ll do the
following. First, let’s return the
sol object from the
solve() function by
return 0 with:
1 return sol
Then, in the
scp_new_problem.jl file let’s replace
1 2 solution = scp_new_problem.solve() scp_new_problem.plot(solution)
Basically, we are taking the
sol object, putting it into the
object, and then passing that object to the
plot function that we are going
to dedicate to (you guessed it) plotting the trajectory. Then, back in the
my_problem.jl file, create the
1 2 3 4 5 6 export plot function plot(sol) # Plotting code goes here return nothing end
I won’t discuss the plotting code here, but you can find it in the
GitHub repository. The code has little to do with the SCP Toolbox
except familiarizing yourself with how the solution data is organized in the
sol object. In particular, it is an
SCPSolution structure implemented in
/tmp/scp_traj_opt/solvers/src/scp.jl. Enough talk – here is the computed
We can see that the output is a smooth trajectory and quite predictable for a “minimum effort” cost function. Intuitively, the car “hugs” the obstacle to minimize the effort spent in driving around it. The velocity colormap shows that the car slows down while rounding the obstacle. Because the velocity is always positive, we conclude that the car is always driving forward.
Because we wrote the optimal control problem generically, let’s investigate
what happens to the trajectory for a bigger obstacle. We do this simply by
_ro variable’s value to
0.8. This time, the PTR algorithm
takes 20 iterations to converge, but the total solution time taken by the SCP
Toolbox is still sub-second.
Something very interesting has happened – without giving any new information to the optimization (remember, the initial guess is still just a straight line from start to finish), the car now decides to do a reversing maneuver and drive backwards in order to first back away from the large obstacle, and then to reverse park into the destination. I’d like to say that the optimization algorithm shows “creativity”, because this is a fundamentally different trajectory than the previous one, and certainly it is not close to what we provided as the initial guess. The SCP algorithm used the vehicle’s dynamics (??)–(??) in order to come up with a novel dynamically feasible trajectory. This is pretty exciting, and it is just one example of the kinds of nuanced tricks that optimization-based trajectory generation can teach us.
Hopefully this tutorial has shown you how to use the SCP Toolbox to solve a fairly simple, but nonconvex, trajectory generation problem for Dubin’s car. We also peeked under the hood and saw how the toolbox is organized, and developed a generic framework for how you can use the toolbox in your own personal projects. This amounts to creating a new Julia package that holds your trajectory generation problems, and placing that package alongside the SCP Toolbox. The Julia package that we created in this tutorial is available verbatim on GitHub for your future reference. It can serve as a template to quickly start implementing new and exciting problems.
Finally, I’d like to say a big thank you for taking the time to read this post. Happy problem solving!