uw-acl / scptoolbox.jl

Sequential Convex Programming Toolbox for nonconvex trajectory optimization.

ℹ️ The repository name has changed to SCPToolbox.jl in order to reflect the project's direction: to develop a general-purpose trajectory optimization toolkit using sequential convex programming algorithms.

The SCP Toolbox provides the tools necessary to define and solve nonconvex trajectory optimization problems. The user-facing part of the toolbox provides a trajectory problem parser that allows one to define the system dynamics, state and input constraints, and boundary conditions. Under the hood, the problem is solved using any one of several Sequential Convex Programming (SCP) algorithms. These algorithms have been successfully demonstrated on a number of difficult aerospace, autonomous driving, robotics, and other applications. A major goal of the SCP Toolbox is to provide working reference implementations of the SCP algorithms. By placing the algorithms behind a parser that transforms trajectory problems into their abstract mathematical definitions, the algorithms can be generically tested on a suite of examples without having to re-implement the underlying algorithms each time.

Getting Started

Click on the button to spin up a remote Jupyter environment. Follow the included notebooks to get a feel for the toolbox, and finish by solving a self-guided tutorial to land a rocket on the Moon! Check out the SCPToolbox_tutorial repository for more details.

Implemented SCP algorithms

The following algorithms are implemented, and can be found in the src/solvers/ directory:

• Penalized trust region (PTR)
• Successive convexification (SCvx)
• Guaranteed Sequential Trajectory Optimization (GuSTO)
• Lossless convexification (LCvx)

Implemented examples

Several example applications show how the algorithms can be used. These can all be found in the test/examples/ directory, and include:

1. Double integrator with friction
2. Mars rocket landing
3. SpaceX Starship landing "flip" maneuver
4. Mass-spring-damper with an actuator deadband or "sticking"
5. Quadrotor flight around obstacles
6. Space station freeflyer robot
7. Planar spacecraft rendezvous with discrete logic
8. Apollo transposition and docking maneuver with discrete logic

Citing

If you use the SCP Toolbox, kindly cite the following associated publication.

@article{SCPToolboxCSM2022,
  doi = {10.1109/mcs.2022.3187542},
  url = {https://doi.org/10.1109/mcs.2022.3187542},
  year = {2022},
  month = oct,
  publisher = {Institute of Electrical and Electronics Engineers ({IEEE})},
  volume = {42},
  number = {5},
  pages = {40--113},
  author = {Danylo Malyuta and Taylor P. Reynolds and Michael Szmuk and Thomas Lew
            and Riccardo Bonalli and Marco Pavone and Behçet Açıkmeşe},
  title = {Convex Optimization for Trajectory Generation: A Tutorial on Generating
           Dynamically Feasible Trajectories Reliably and Efficiently},
  journal = {{IEEE} Control Systems},
  note = {Free preprint available at https://arxiv.org/abs/2106.09125}
}

scptoolbox.jl's Issues

Julia Version and dependencies

Hi,

which julia version and related dependencies should be used?
Is it possible to add those to the Readme?

Best,

Carlo

Add variable scaling advice

Is a state or input is unconstrained, there is no information to go by for auto-computing the appropriate scaling. In this situation, we currently use no scaling (i.e., unit scaling with zero offset). Add the possibility for user to "advise" on the variable scaling by providing manual bounds on the variable, which are used for scaling in this situation. If this advice is provided for a constrained variable, then we should probably ignore the advice since the auto-computed value is more accurate.

Make SCvx and GuSTO work in the jgcd branch

Currently the SCvx and GuSTO algorithms are working only in the master and csm branches. As #3 mentioned, the jgcd branch does not have these solvers yet. This is due to using a more interesting back-end parser code to keep track of SCP subproblems as convex programming programs in conic form. The PTR algorithm was ported to use this back-end, while SCvx and GuSTO have not yet been.

Timeline to completion: the next 1-2 months.

Implement inverse-free discretization

Replace the existing discretization routines with their matrix-inverse-free counterparts, which are faster and more stable.

What is dynamic feasibility?

To decrease cost, I lowered wtr but that has led to me getting 'dyn: F' indefinitely (using ptr). I am unsure of what that actually means, is it that the discretisation is in inaccurate? I increased N tenfold and it didn't seem to improve much.

I would like to know where in the code it is determined whether a trajectory is dynamically feasibly, I could only find it here https://github.com/dmalyuta/scp_traj_opt/blob/b7dd4b9d5437c98559f73128983045915ba22423/solvers/src/discretization.jl#L207-L212 but that doesn't seem to be used other than for the initial guess/ warm start.

Also, I got scvx working in a hacky way in my fork https://github.com/Zentrik/scp_traj_opt if you want to copy it.

use toolbox in c++ programe

First of all, thanks for your great work, it helps me learning trajectory optimization.

Now I want to use this in a c++ program. I wonder where should I start.
I noticed the underdying solver of scp_traj_opt is ECOS solver which is a c library. But scp_tool_box also use JuMP. So if I want to solve a c++ trajectory optimization problem. Where should I start? Is it better to just rewrite SCP_TOOL_BOX in c++ or should I look for other trajectory optimization toolbox in C++?

Improve the SCP exit status

Currently, if no convex optimizer issue came up, then the SCP solution will have in its status field "SCP_SOLVED". However, it may be the case that the dynamics are not satisfied, or the constraints are also infeasible (all due to relaxations via virtual controls/soft penalties in GuSTO).

Task: improve the exit status so that it tells the user if the converged solution is dynamically infeasible and/or does not satisfy the nonconvex constraints.

No method matching ConvexCone

Hi,

I am trying to add a second order cone constraint to my problem but I get the following error

ERROR: MethodError: no method matching SCPToolbox.Parser.ConicLinearProgram.ConvexCone(::Matrix{JuMP.AffExpr}, ::SCPToolbox.Parser.ConicLinearProgram.SupportedCone)
Closest candidates are:
  SCPToolbox.Parser.ConicLinearProgram.ConvexCone(::Union{Float64, Int64, Vector{Float64}, Vector{Int64}, Vector{JuMP.AffExpr}, Vector{JuMP.QuadExpr}, Vector{JuMP.VariableRef}, JuMP.VariableRef, JuMP.AffExpr, JuMP.QuadExpr}, ::Union{SCPToolbox.Parser.ConicLinearProgram.SupportedCone, SCPToolbox.Parser.ConicLinearProgram.SupportedDualCone}) at D:\fdietric\Documents\Julia\SCPToolbox.jl\src\parser\cone.jl:121
Stacktrace:
 [1] SCPToolbox.Parser.ConicLinearProgram.ConicConstraint(f::SCPToolbox.Parser.ConicLinearProgram.ProgramFunction, kind::SCPToolbox.Parser.ConicLinearProgram.SupportedCone, prog::ConicProgram; name::String)
   @ SCPToolbox.Parser.ConicLinearProgram D:\fdietric\Documents\Julia\SCPToolbox.jl\src\parser\constraint.jl:129
 [2] constraint!(prog::ConicProgram, kind::SCPToolbox.Parser.ConicLinearProgram.SupportedCone, f::Function, x::Tuple{SCPToolbox.Parser.ConicLinearProgram.VariableArgumentBlock{1}}, p::Tuple{}; refname::String)
   @ SCPToolbox.Parser.ConicLinearProgram D:\fdietric\Documents\Julia\SCPToolbox.jl\src\parser\program.jl:240
 [3] top-level scope
   @ D:\fdietric\Documents\Julia\SCPToolbox.jl\src\parser\program.jl:754

My code is the following.

My variable T is the concatenation of 3 vectors Ti of dimension 3.
I am trying to constrain the norm of each (scaled) Ti to be less than 1.

import Pkg
Pkg.activate(".")

Pkg.develop(path="../SCPToolbox.jl/")
Pkg.precompile()

Pkg.add("JuMP")
Pkg.build("JuMP")
 
## using

using SCPToolbox 
using ECOS
using LinearAlgebra

## Engine saturations

Tₘᵢₙ = 300e3
Tₘₐₓ = 1000e3
αₘₐₓ = 5.0 # deg

## Cluster parameters

n = 3

## Construct problem parameters

params = Dict()
params["solver"] = ECOS
params["opts"] = Dict("verbose"=>0,"abstol"=>1e-8)

pbm = ConicProgram(params; 
        solver=params["solver"], 
        solver_options=params["opts"])
    # Conic linear program

    # Feasibility problem
    # 0 variables (0 blocks)
    # 0 parameters (0 blocks)
    # 0 constraints

    # Variable argument
    #     0 elements
    #     0 blocks


    # Parameter argument
    #     0 elements
    #     0 blocks

## Variable

T = @new_variable(pbm,3*n,"T")
    # Vector variable
    #   9 elements
    #   (9,) shape
    #   Name: T
    #   Block index in stack: 1
    #   Indices in stack: 1:9
    #   Type: Vector{JuMP.AffExpr}
    #   No scaling (x=xh)
    #   Any perturbation allowed
    #   Value =
    #      T[1]
    #      T[2]
    #      T[3]
    #      ⋮
    #      T[8]
    #      T[9]

## Scaling 

Tx_max = Tₘₐₓ
Tx_min = Tₘᵢₙ * cosd(αₘₐₓ)

Tx_scale = Tx_max - Tx_min
Tx_offset = Tx_min

Tyz_max = Tₘₐₓ * sind(αₘₐₓ)
Tyz_min = 0.0

Tyz_scale = Tyz_max - Tyz_min
Tyz_offset = Tyz_min

@scale(T,[Tx_scale;Tyz_scale;Tyz_scale],[Tx_offset;Tyz_offset;Tyz_offset])

value(T)
    # 9-element Vector{JuMP.AffExpr}:
    #  701141.5905724764 T[1] + 298858.4094275237
    #  87155.74274765818 T[2]
    #  87155.74274765818 T[3]
    #  701141.5905724764 T[4] + 298858.4094275237
    #  87155.74274765818 T[5]
    #  87155.74274765818 T[6]
    #  701141.5905724764 T[7] + 298858.4094275237
    #  87155.74274765818 T[8]
    #  87155.74274765818 T[9]


## Constraints

# for i=1:n
    i = 2
    
    name = "max_norm_"*string(i)
    
    A = zeros(1+3,3*n)
    A[2:4,3*(i-1)+1:3*i] = Matrix(I(3))
        # 4×9 Matrix{Float64}:
        # 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
        # 0.0  0.0  0.0  1.0  0.0  0.0  0.0  0.0  0.0
        # 0.0  0.0  0.0  0.0  1.0  0.0  0.0  0.0  0.0
        # 0.0  0.0  0.0  0.0  0.0  1.0  0.0  0.0  0.0

    b = zeros(4,1)
    b[1] = 1.0
        # 4×1 Matrix{Float64}:
        # 1.0
        # 0.0
        # 0.0
        # 0.0

    @add_constraint(pbm,SOC,name,T -> A*T .+ b)
    #     ERROR: MethodError: no method matching SCPToolbox.Parser.ConicLinearProgram.ConvexCone(::Matrix{JuMP.AffExpr}, ::SCPToolbox.Parser.ConicLinearProgram.SupportedCone)
    # Closest candidates are:
    #   SCPToolbox.Parser.ConicLinearProgram.ConvexCone(::Union{Float64, Int64, Vector{Float64}, Vector{Int64}, Vector{JuMP.AffExpr}, Vector{JuMP.QuadExpr}, Vector{JuMP.VariableRef}, JuMP.VariableRef, JuMP.AffExpr, JuMP.QuadExpr}, ::Union{SCPToolbox.Parser.ConicLinearProgram.SupportedCone, SCPToolbox.Parser.ConicLinearProgram.SupportedDualCone}) at D:\fdietric\Documents\Julia\SCPToolbox.jl\src\parser\cone.jl:121
    # Stacktrace:
    #  [1] SCPToolbox.Parser.ConicLinearProgram.ConicConstraint(f::SCPToolbox.Parser.ConicLinearProgram.ProgramFunction, kind::SCPToolbox.Parser.ConicLinearProgram.SupportedCone, prog::ConicProgram; name::String)
    #    @ SCPToolbox.Parser.ConicLinearProgram D:\fdietric\Documents\Julia\SCPToolbox.jl\src\parser\constraint.jl:129
    #  [2] constraint!(prog::ConicProgram, kind::SCPToolbox.Parser.ConicLinearProgram.SupportedCone, f::Function, x::Tuple{SCPToolbox.Parser.ConicLinearProgram.VariableArgumentBlock{1}}, p::Tuple{}; refname::String)
    #    @ SCPToolbox.Parser.ConicLinearProgram D:\fdietric\Documents\Julia\SCPToolbox.jl\src\parser\program.jl:240
    #  [3] top-level scope
    #    @ D:\fdietric\Documents\Julia\SCPToolbox.jl\src\parser\program.jl:754
# end

If needed, here is the status of my packages

(SCPToolbox) pkg> status
     Project SCPToolbox v1.0.0
      Status `D:\fdietric\Documents\Julia\SCPToolbox.jl\Project.toml`
  [5ae59095] Colors v0.12.8
  [e2685f51] ECOS v1.1.0
  [b6b21f68] Ipopt v1.0.3
  [4076af6c] JuMP v1.1.1
  [98e50ef6] JuliaFormatter v1.0.7
  [d330b81b] PyPlot v2.10.0
  [37e2e46d] LinearAlgebra
  [de0858da] Printf
  [10745b16] Statistics
  [8dfed614] Test

Thanks in advance for your help

Add CI and CodeCov

Would be nice to have CI run automatically using Github Actions.