Optimal Control Problems#

Problems are usually formulated using cyqlone::LinearOCPStorage. To pass them to the Cyqlone solver, they need to be converted to cyqlone::CyqloneStorage format, where the initial state is eliminated. For testing, it can be helpful to convert the problem to a sparse QP format, which is supported by cyqlone::LinearOCPSparseQP.

struct LinearOCPStorage

Storage for a linear-quadratic OCP of the form.

           ₙ₋₁
minimize    ∑ [½ uᵢᵀ Rᵢ uᵢ + uᵢᵀ S xᵢ + ½ xᵢᵀ Qᵢ xᵢ + rᵢᵀuᵢ + qᵢᵀxᵢ] + ½ xₙᵀ Qₙ xₙ + qₙᵀ xₙ
           ⁱ⁼⁰
s.t.        x₀   = b₀
            xᵢ₊₁ = Aᵢ xᵢ + Bᵢ uᵢ + bᵢ₊₁
            lᵢ   ≤ Cᵢ xᵢ + Dᵢ uᵢ ≤ uᵢ
            lₙ   ≤ Cₙ xₙ ≤ uₙ

Public Functions

inline guanaqo::MatrixView<real_t, index_t> H(index_t i)

inline guanaqo::MatrixView<real_t, index_t> Q(index_t i)

inline guanaqo::MatrixView<real_t, index_t> R(index_t i)

inline guanaqo::MatrixView<real_t, index_t> S(index_t i)

inline guanaqo::MatrixView<real_t, index_t> S_trans(index_t i)

inline guanaqo::MatrixView<real_t, index_t> CD(index_t i)

inline guanaqo::MatrixView<real_t, index_t> C(index_t i)

inline guanaqo::MatrixView<real_t, index_t> D(index_t i)

inline guanaqo::MatrixView<real_t, index_t> AB(index_t i)

inline guanaqo::MatrixView<real_t, index_t> A(index_t i)

inline guanaqo::MatrixView<real_t, index_t> B(index_t i)

inline guanaqo::MatrixView<const real_t, index_t> H(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> Q(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> R(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> S(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> S_trans(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> CD(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> C(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> D(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> AB(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> A(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> B(index_t i) const

inline index_t num_variables() const

inline index_t num_constraints() const

inline index_t num_dynamics_constraints() const

inline guanaqo::MatrixView<real_t, index_t> qr()

inline guanaqo::MatrixView<real_t, index_t> qr(index_t i)

inline guanaqo::MatrixView<real_t, index_t> q(index_t i)

inline guanaqo::MatrixView<real_t, index_t> r(index_t i)

inline guanaqo::MatrixView<real_t, index_t> b()

inline guanaqo::MatrixView<real_t, index_t> b(index_t i)

inline guanaqo::MatrixView<real_t, index_t> b_min()

inline guanaqo::MatrixView<real_t, index_t> b_min(index_t i)

inline guanaqo::MatrixView<real_t, index_t> b_max()

inline guanaqo::MatrixView<real_t, index_t> b_max(index_t i)

inline guanaqo::MatrixView<const real_t, index_t> qr() const

inline guanaqo::MatrixView<const real_t, index_t> qr(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> q(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> r(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> b() const

inline guanaqo::MatrixView<const real_t, index_t> b(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> b_min() const

inline guanaqo::MatrixView<const real_t, index_t> b_min(index_t i) const

inline guanaqo::MatrixView<const real_t, index_t> b_max() const

inline guanaqo::MatrixView<const real_t, index_t> b_max(index_t i) const

KKTError compute_kkt_error(const Solution &sol) const

Public Members

OCPDim dim = {}

std::vector<real_t> storage = create_storage(dim)

Create a single contiguous storage for all problem data.

Storage layout:     size        offset
N × [ Q Sᵀ] = H     (nx+nu)²    0
    [ S R ]
1 × [ Q ]           nx²         N (nx+nu)²
N × [ C D ]         ny(nx+nu)   N (nx+nu)² + nx²
1 × [ C ]           ny_N nx     N (nx+nu+ny)(nx+nu) + nx²
N × [ A B ]         nx(nx+nu)   N (nx+nu+ny)(nx+nu) + (nx+ny_N)nx
N × [ q r ]         nx+nu       N (2nx+nu+ny)(nx+nu) + (nx+ny_N)nx
1 × [ q ]           nx          N (2nx+nu+ny+1)(nx+nu) + (nx+ny_N)nx
N+1 × [ b ]         nx          N (2nx+nu+ny+1)(nx+nu) + (nx+ny_N+1)nx
N × [ l ]           ny          N (2nx+nu+ny+1)(nx+nu) + (nx+ny_N+2+N)nx
1 × [ l ]           ny_N        N (2nx+nu+ny+1)(nx+nu) + (nx+ny_N+2+N)nx + N ny
N × [ u ]           ny          N (2nx+nu+ny+1)(nx+nu) + (nx+ny_N+2+N)nx + N ny + ny_N
1 × [ u ]           ny_N        N (2nx+nu+ny+1)(nx+nu) + (nx+ny_N+2+N)nx + N (2ny) + ny_N
                                N (2nx+nu+ny+1)(nx+nu) + (nx+ny_N+2+N)nx + N (2ny) + 2ny_N

Public Static Functions

static inline std::vector<real_t> create_storage(OCPDim dim)

struct Solution

Public Functions

inline guanaqo::MatrixView<real_t, index_t> x(OCPDim dim)

inline guanaqo::MatrixView<const real_t, index_t> x(OCPDim dim) const

inline guanaqo::MatrixView<real_t, index_t> x(OCPDim dim, index_t i)

inline guanaqo::MatrixView<const real_t, index_t> x(OCPDim dim, index_t i) const

inline guanaqo::MatrixView<real_t, index_t> u(OCPDim dim)

inline guanaqo::MatrixView<const real_t, index_t> u(OCPDim dim) const

inline guanaqo::MatrixView<real_t, index_t> u(OCPDim dim, index_t i)

inline guanaqo::MatrixView<const real_t, index_t> u(OCPDim dim, index_t i) const

inline guanaqo::MatrixView< real_t, index_t > λ (OCPDim dim)

inline guanaqo::MatrixView< const real_t, index_t > λ (OCPDim dim) const

inline guanaqo::MatrixView< real_t, index_t > λ (OCPDim dim, index_t i)

inline guanaqo::MatrixView< const real_t, index_t > λ (OCPDim dim, index_t i) const

inline guanaqo::MatrixView<real_t, index_t> y(OCPDim dim, index_t i)

inline guanaqo::MatrixView<const real_t, index_t> y(OCPDim dim, index_t i) const

Public Members

std::vector<real_t> solution

std::vector<real_t> inequality_multipliers

std::vector<real_t> equality_multipliers

struct KKTError

Public Members

real_t stationarity

real_t inequality_residual

real_t equality_residual

real_t complementarity

template<class T = real_t> struct CyqloneStorage

Storage for a linear-quadratic OCP with the initial states x₀ eliminated.

           ₙ₋₁
minimize    ∑ [½ uᵢᵀ Rᵢ uᵢ + uᵢᵀ Sᵢ xᵢ + ½ xᵢᵀ Qᵢ xᵢ + rᵢᵀuᵢ + qᵢᵀxᵢ]
           ⁱ⁼¹
           + ½ u₀ᵀ R₀ u₀ + (r₀ + S₀ xᵢₙᵢₜ)ᵀ u₀
           + ½ xₙᵀ Qₙ xₙ + qₙᵀ xₙ
s.t.        xᵢ₊₁ = Aᵢ xᵢ + Bᵢ uᵢ + cᵢ
            lᵢ   ≤ Cᵢ xᵢ + Dᵢ uᵢ ≤ uᵢ
            l₀ - C₀ xᵢₙᵢₜ ≤ D₀ U₀ ≤ u₀ - C₀ xᵢₙᵢₜ
            lₙ   ≤ Cₙ xₙ ≤ uₙ

The matrices are combined per stage, with inputs ordered first. The first and last stage are special because of the lack of x₀ and uₙ.

Hᵢ = [ Rᵢ  Sᵢ ],    H₀ = [ R₀  0  ],    Fᵢ = [ Bᵢ  Aᵢ ],    Gᵢ = [ Dᵢ  Cᵢ ],    G₀ = [ D₀ Cₙ ]
     [ Sᵢᵀ Qᵢ ]          [ 0   Qₙ ]

Due to the elimination of x₀, there may be fewer constraints for the first stage, which is tracked by the Ju0 mask. When reconstructing the solution, the multipliers for eliminated constraints are set to zero (we assume that the initial state is feasible w.r.t. the state constraints).

Public Types

using value_type = T

using matrix = batmat::matrix::Matrix<value_type, index_t>

using Solution = LinearOCPStorage::Solution

using KKTError = LinearOCPStorage::KKTError

Public Functions

void update_impl(const LinearOCPStorage &ocp)

void update(const LinearOCPStorage &ocp)

void reconstruct_ineq_multipliers(std::span<const value_type> y_compressed, std::span<value_type> y) const

std::vector<value_type> reconstruct_ineq_multipliers(std::span<const value_type> y_compressed) const

Solution reconstruct_solution (const LinearOCPStorage &ocp, std::span< const value_type > ux_compressed, std::span< const value_type > y_compressed, std::span< const value_type > λ_compressed) const

KKTError compute_kkt_error (const LinearOCPStorage &ocp, std::span< const value_type > ux_compressed, std::span< const value_type > y_compressed, std::span< const value_type > λ_compressed) const

Public Members

index_t N_horiz

index_t nx

index_t nu

index_t ny

index_t ny_0

index_t ny_N

std::vector<bool> Ju0

matrix data_H = [this] {returnmatrix{{.depth = N_horiz, .rows = nu + nx, .cols = nu + nx}};}()

matrix data_F = [this] { return matrix{{.depth = N_horiz, .rows = nx, .cols = nu + nx}}; }()

matrix data_G = [this] {returnmatrix{{.depth = N_horiz - 1, .rows = ny, .cols = nu + nx}};}()

matrix data_G0N = [this] {returnmatrix{{.depth = 1, .rows = ny_0 + ny_N, .cols = nu + nx}};}()

matrix data_rq = [this] { return matrix{{.depth = N_horiz, .rows = nu + nx, .cols = 1}}; }()

matrix data_c = [this] { return matrix{{.depth = N_horiz, .rows = nx, .cols = 1}}; }()

matrix data_lb = [this] { return matrix{{.depth = N_horiz - 1, .rows = ny, .cols = 1}}; }()

matrix data_lb0N = [this] { return matrix{{.depth = 1, .rows = ny_0 + ny_N, .cols = 1}}; }()

matrix data_ub = [this] { return matrix{{.depth = N_horiz - 1, .rows = ny, .cols = 1}}; }()

matrix data_ub0N = [this] { return matrix{{.depth = 1, .rows = ny_0 + ny_N, .cols = 1}}; }()

std::vector<index_t> indices_G0 = std::vector<index_t>(ny_0)

Public Static Functions

static CyqloneStorage build(const LinearOCPStorage &ocp, index_t ny_0 = -1)

static index_t count_constr_0(const LinearOCPStorage &ocp, std::vector<bool> &Ju0)

struct LinearOCPSparseQP

Represents a sparse multiple shooting formulation of the standard optimal control problem.

Public Functions

KKTMatrix build_kkt (real_t S, std::span< const real_t > Σ, std::span< const bool > J) const: Returns the lower part of the symmetric indefinite KKT matrix for the active set J and penalty factors Σ.

Public Members

std::vector<index_t> Q_outer_ptr

std::vector<index_t> Q_inner_idx

std::vector<real_t> Q_values

SparseCSC<index_t, index_t> Q_sparsity

std::vector<index_t> A_outer_ptr

std::vector<index_t> A_inner_idx

std::vector<real_t> A_values

SparseCSC<index_t, index_t> A_sparsity

index_t n

index_t m_eq

index_t m_ineq

Public Static Functions

static LinearOCPSparseQP build(const LinearOCPStorage &ocp)

struct KKTMatrix

Public Members

std::vector<index_t> outer_ptr

std::vector<index_t> inner_idx

std::vector<real_t> values

SparseCSC<index_t, index_t> sparsity