brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Actions 4 Packages Projects Releases Wiki Activity

clearpilot: initial commit of full source

Browse Source
This commit is contained in:
Brian Hanson
2026-04-11 06:25:25 +00:00
commit e2a0c1894a
3383 changed files with 834683 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

446
third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h vendored Executable file
View File

@@ -0,0 +1,446 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \defgroup ocp_nlp ocp_nlp
/// @{
/// @}
/// \defgroup ocp_nlp_solver ocp_nlp_solver
/// @{
/// @}
/// \ingroup ocp_nlp
/// @{
/// \ingroup ocp_nlp_solver
/// @{
/// \defgroup ocp_nlp_common ocp_nlp_common
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/ocp_qp/ocp_qp_xcond_solver.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* config
************************************************/
typedef struct ocp_nlp_config
{
int N; // number of stages
// solver-specific implementations of memory management functions
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts_);
void (*opts_update)(void *config, void *dims, void *opts_);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts_);
void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory);
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts_);
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
void (*opts_set_at_stage)(void *config_, void *opts_, size_t stage, const char *field, void* value);
// evaluate solver // TODO rename into solve
int (*evaluate)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
void (*eval_param_sens)(void *config, void *dims, void *opts_, void *mem, void *work,
char *field, int stage, int index, void *sens_nlp_out);
// prepare memory
int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
void (*memory_reset_qp_solver)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
// initalize this struct with default values
void (*config_initialize_default)(void *config);
// general getter
void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_);
void (*opts_get)(void *config_, void *dims, void *opts_, const char *field, void *return_value_);
void (*work_get)(void *config_, void *dims, void *work_, const char *field, void *return_value_);
// config structs of submodules
ocp_qp_xcond_solver_config *qp_solver; // TODO rename xcond_solver
ocp_nlp_dynamics_config **dynamics;
ocp_nlp_cost_config **cost;
ocp_nlp_constraints_config **constraints;
ocp_nlp_reg_config *regularize;
} ocp_nlp_config;
//
acados_size_t ocp_nlp_config_calculate_size(int N);
//
ocp_nlp_config *ocp_nlp_config_assign(int N, void *raw_memory);
/************************************************
* dims
************************************************/
/// Structure to store dimensions/number of variables.
typedef struct ocp_nlp_dims
{
void **cost;
void **dynamics;
void **constraints;
ocp_qp_xcond_solver_dims *qp_solver; // xcond solver instead ??
ocp_nlp_reg_dims *regularize;
int *nv; // number of primal variables (states+controls+slacks)
int *nx; // number of differential states
int *nu; // number of inputs
int *ni; // number of two-sided inequality constraints: nb+ng+nh+ns
int *nz; // number of algebraic variables
int *ns; // number of slack variables
int N; // number of shooting nodes
void *raw_memory; // Pointer to allocated memory, to be used for freeing
} ocp_nlp_dims;
//
acados_size_t ocp_nlp_dims_calculate_size(void *config);
//
ocp_nlp_dims *ocp_nlp_dims_assign(void *config, void *raw_memory);
/// Sets the dimension of optimization variables
/// (states, constrols, algebraic variables, slack variables).
///
/// \param config_ The configuration struct.
/// \param dims_ The dimension struct.
/// \param field The type of optimization variables, either nx, nu, nz, or ns.
/// \param value_array Number of variables for each stage.
void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_,
const char *field, const void* value_array);
/// Sets the dimensions of constraints functions for a stage
/// (bounds on states, bounds on controls, equality constraints,
/// inequality constraints).
///
/// \param config_ The configuration struct.
/// \param dims_ The dimension struct.
/// \param stage Stage number.
/// \param field The type of constraint/bound, either nbx, nbu, ng, or nh.
/// \param value_field Number of constraints/bounds for the given stage.
void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage,
const char *field, const void* value_field);
/// Sets the dimensions of the cost terms for a stage.
///
/// \param config_ The configuration struct.
/// \param dims_ The dimension struct.
/// \param stage Stage number.
/// \param field Type of cost term, can be eiter ny.
/// \param value_field Number of cost terms/residuals for the given stage.
void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field,
const void* value_field);
/// Sets the dimensions of the dynamics for a stage.
///
/// \param config_ The configuration struct.
/// \param dims_ The dimension struct.
/// \param stage Stage number.
/// \param field TBD
/// \param value TBD
void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char *field,
const void* value);
/************************************************
* Inputs
************************************************/
/// Struct for storing the inputs of an OCP NLP solver
typedef struct ocp_nlp_in
{
/// Length of sampling intervals/timesteps.
double *Ts;
/// Pointers to cost functions (TBC).
void **cost;
/// Pointers to dynamics functions (TBC).
void **dynamics;
/// Pointers to constraints functions (TBC).
void **constraints;
/// Pointer to allocated memory, to be used for freeing.
void *raw_memory;
} ocp_nlp_in;
//
acados_size_t ocp_nlp_in_calculate_size_self(int N);
//
acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
//
ocp_nlp_in *ocp_nlp_in_assign_self(int N, void *raw_memory);
//
ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory);
/************************************************
* out
************************************************/
typedef struct ocp_nlp_out
{
struct blasfeo_dvec *ux; // NOTE: this contains [u; x; s_l; s_u]! - rename to uxs?
struct blasfeo_dvec *z; // algebraic vairables
struct blasfeo_dvec *pi; // multipliers for dynamics
struct blasfeo_dvec *lam; // inequality mulitpliers
struct blasfeo_dvec *t; // slack variables corresponding to evaluation of all inequalities (at the solution)
// NOTE: the inequalities are internally organized in the following order:
// [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
double inf_norm_res;
void *raw_memory; // Pointer to allocated memory, to be used for freeing
} ocp_nlp_out;
//
acados_size_t ocp_nlp_out_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
//
ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
void *raw_memory);
/************************************************
* options
************************************************/
/// Globalization types
typedef enum
{
FIXED_STEP,
MERIT_BACKTRACKING,
} ocp_nlp_globalization_t;
typedef struct ocp_nlp_opts
{
ocp_qp_xcond_solver_opts *qp_solver_opts; // xcond solver opts instead ???
void *regularize;
void **dynamics; // dynamics_opts
void **cost; // cost_opts
void **constraints; // constraints_opts
double step_length; // step length in case of FIXED_STEP
double levenberg_marquardt; // LM factor to be added to the hessian before regularization
int reuse_workspace;
int num_threads;
int print_level;
// TODO: move to separate struct?
ocp_nlp_globalization_t globalization;
int full_step_dual;
int line_search_use_sufficient_descent;
int globalization_use_SOC;
double alpha_min;
double alpha_reduction;
double eps_sufficient_descent;
} ocp_nlp_opts;
//
acados_size_t ocp_nlp_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value);
//
void ocp_nlp_opts_set_at_stage(void *config, void *opts, int stage, const char *field, void *value);
/************************************************
* residuals
************************************************/
typedef struct ocp_nlp_res
{
struct blasfeo_dvec *res_stat; // stationarity
struct blasfeo_dvec *res_eq; // dynamics
struct blasfeo_dvec *res_ineq; // inequality constraints
struct blasfeo_dvec *res_comp; // complementarity
struct blasfeo_dvec tmp; // tmp
double inf_norm_res_stat;
double inf_norm_res_eq;
double inf_norm_res_ineq;
double inf_norm_res_comp;
acados_size_t memsize;
} ocp_nlp_res;
//
acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
//
ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
//
void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
/************************************************
* memory
************************************************/
typedef struct ocp_nlp_memory
{
// void *qp_solver_mem; // xcond solver mem instead ???
ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ???
void *regularize_mem;
void **dynamics; // dynamics memory
void **cost; // cost memory
void **constraints; // constraints memory
// residuals
ocp_nlp_res *nlp_res;
// qp in & out
ocp_qp_in *qp_in;
ocp_qp_out *qp_out;
// QP stuff not entering the qp_in struct
struct blasfeo_dmat *dzduxt; // dzdux transposed
struct blasfeo_dvec *z_alg; // z_alg, output algebraic variables
struct blasfeo_dvec *cost_grad;
struct blasfeo_dvec *ineq_fun;
struct blasfeo_dvec *ineq_adj;
struct blasfeo_dvec *dyn_fun;
struct blasfeo_dvec *dyn_adj;
double cost_value;
bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
struct blasfeo_dvec *sim_guess;
int *sqp_iter; // pointer to iteration number
} ocp_nlp_memory;
//
acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts);
//
ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
ocp_nlp_opts *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
typedef struct ocp_nlp_workspace
{
void *qp_work;
void **dynamics; // dynamics_workspace
void **cost; // cost_workspace
void **constraints; // constraints_workspace
// for globalization: -> move to module?!
ocp_nlp_out *tmp_nlp_out;
ocp_nlp_out *weight_merit_fun;
struct blasfeo_dvec tmp_nxu;
struct blasfeo_dvec tmp_ni;
struct blasfeo_dvec dxnext_dy;
} ocp_nlp_workspace;
//
acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts);
//
ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
ocp_nlp_opts *opts, ocp_nlp_memory *mem, void *raw_memory);
/************************************************
* function
************************************************/
void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_initialize_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha);
//
int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
int check_early_termination);
//
double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_initialize_t_slacks(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out,
ocp_nlp_res *res, ocp_nlp_memory *mem);
//
void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
/// @}
/// @}
/// @}

238
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h vendored Executable file
View File

@@ -0,0 +1,238 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_constraints
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx;
int nu;
int nz;
int nb; // nbx + nbu
int nbu; // number of input box constraints
int nbx; // number of state box constraints
int ng; // number of general linear constraints
int nh; // number of nonlinear path constraints
int ns; // nsbu + nsbx + nsg + nsh
int nsbu; // number of softened input bounds
int nsbx; // number of softened state bounds
int nsg; // number of softened general linear constraints
int nsh; // number of softened nonlinear constraints
int nbue; // number of input box constraints which are equality
int nbxe; // number of state box constraints which are equality
int nge; // number of general linear constraints which are equality
int nhe; // number of nonlinear path constraints which are equality
} ocp_nlp_constraints_bgh_dims;
//
acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config);
//
void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_,
const char *field, const int* value);
/************************************************
* model
************************************************/
typedef struct
{
int *idxb;
int *idxs;
int *idxe;
struct blasfeo_dvec d; // gathers bounds
struct blasfeo_dmat DCt; // general linear constraint matrix
// lg <= [D, C] * [u; x] <= ug
external_function_generic *nl_constr_h_fun; // nonlinear: lh <= h(x,u) <= uh
external_function_generic *nl_constr_h_fun_jac; // nonlinear: lh <= h(x,u) <= uh
external_function_generic *nl_constr_h_fun_jac_hess; // nonlinear: lh <= h(x,u) <= uh
} ocp_nlp_constraints_bgh_model;
//
acados_size_t ocp_nlp_constraints_bgh_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_,
void *model_, const char *field, void *value);
//
void ocp_nlp_constraints_bgh_model_get(void *config_, void *dims_,
void *model_, const char *field, void *value);
/************************************************
* options
************************************************/
typedef struct
{
int compute_adj;
int compute_hess;
} ocp_nlp_constraints_bgh_opts;
//
acados_size_t ocp_nlp_constraints_bgh_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_constraints_bgh_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_constraints_bgh_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_constraints_bgh_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_constraints_bgh_opts_set(void *config, void *opts, char *field, void *value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec fun;
struct blasfeo_dvec adj;
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
struct blasfeo_dvec *lam; // pointer to lam in nlp_out
struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out
struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory
struct blasfeo_dmat *DCt; // pointer to DCt in qp_in
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
int *idxb; // pointer to idxb[ii] in qp_in
int *idxs_rev; // pointer to idxs_rev[ii] in qp_in
int *idxe; // pointer to idxe[ii] in qp_in
} ocp_nlp_constraints_bgh_memory;
//
acados_size_t ocp_nlp_constraints_bgh_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_constraints_bgh_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_adj_ptr(void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
//
void ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_idxb_ptr(int *idxb, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat tmp_nv_nv;
struct blasfeo_dmat tmp_nz_nh;
struct blasfeo_dmat tmp_nv_nh;
struct blasfeo_dmat tmp_nz_nv;
struct blasfeo_dmat hess_z;
struct blasfeo_dvec tmp_ni;
struct blasfeo_dvec tmp_nh;
} ocp_nlp_constraints_bgh_workspace;
//
acados_size_t ocp_nlp_constraints_bgh_workspace_calculate_size(void *config, void *dims, void *opts);
/* functions */
//
void ocp_nlp_constraints_bgh_config_initialize_default(void *config);
//
void ocp_nlp_constraints_bgh_initialize(void *config, void *dims, void *model, void *opts,
void *mem, void *work);
//
void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
//
void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
//
void ocp_nlp_constraints_bgh_bounds_update(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
/// @}
/// @}

218
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h vendored Executable file
View File

@@ -0,0 +1,218 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_constraints
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/* dims */
typedef struct
{
int nx;
int nu;
int nz;
int nb; // nbx + nbu
int nbu;
int nbx;
int ng; // number of general linear constraints
int nphi; // dimension of convex outer part
int ns; // nsbu + nsbx + nsg + nsphi
int nsbu; // number of softened input bounds
int nsbx; // number of softened state bounds
int nsg; // number of softened general linear constraints
int nsphi; // number of softened nonlinear constraints
int nr; // dimension of nonlinear function in convex_over_nonlinear constraint
int nbue; // number of input box constraints which are equality
int nbxe; // number of state box constraints which are equality
int nge; // number of general linear constraints which are equality
int nphie; // number of nonlinear path constraints which are equality
} ocp_nlp_constraints_bgp_dims;
//
acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config);
//
void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value);
/* model */
typedef struct
{
// ocp_nlp_constraints_bgp_dims *dims;
int *idxb;
int *idxs;
int *idxe;
struct blasfeo_dvec d;
struct blasfeo_dmat DCt;
external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux;
external_function_generic *nl_constr_phi_o_r_fun;
external_function_generic *nl_constr_r_fun_jac;
} ocp_nlp_constraints_bgp_model;
//
acados_size_t ocp_nlp_constraints_bgp_calculate_size(void *config, void *dims);
//
void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_,
void *model_, const char *field, void *value);
//
void ocp_nlp_constraints_bgp_model_get(void *config_, void *dims_,
void *model_, const char *field, void *value);
/* options */
typedef struct
{
int compute_adj;
int compute_hess;
} ocp_nlp_constraints_bgp_opts;
//
acados_size_t ocp_nlp_constraints_bgp_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_constraints_bgp_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_constraints_bgp_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_constraints_bgp_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_constraints_bgp_opts_set(void *config, void *opts, char *field, void *value);
/* memory */
typedef struct
{
struct blasfeo_dvec fun;
struct blasfeo_dvec adj;
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
struct blasfeo_dvec *lam; // pointer to lam in nlp_out
struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out
struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory
struct blasfeo_dmat *DCt; // pointer to DCt in qp_in
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
int *idxb; // pointer to idxb[ii] in qp_in
int *idxs_rev; // pointer to idxs_rev[ii] in qp_in
int *idxe; // pointer to idxe[ii] in qp_in
} ocp_nlp_constraints_bgp_memory;
//
acados_size_t ocp_nlp_constraints_bgp_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_constraints_bgp_memory_assign(void *config, void *dims, void *opts,
void *raw_memory);
//
struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_adj_ptr(void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
//
void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
/* workspace */
typedef struct
{
struct blasfeo_dvec tmp_ni;
struct blasfeo_dmat jac_r_ux_tran;
struct blasfeo_dmat tmp_nr_nphi_nr;
struct blasfeo_dmat tmp_nv_nr;
struct blasfeo_dmat tmp_nv_nphi;
struct blasfeo_dmat tmp_nz_nphi;
} ocp_nlp_constraints_bgp_workspace;
//
acados_size_t ocp_nlp_constraints_bgp_workspace_calculate_size(void *config, void *dims, void *opts);
/* functions */
//
void ocp_nlp_constraints_bgp_config_initialize_default(void *config);
//
void ocp_nlp_constraints_bgp_initialize(void *config, void *dims, void *model,
void *opts, void *mem, void *work);
//
void ocp_nlp_constraints_bgp_update_qp_matrices(void *config_, void *dims,
void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims,
void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_constraints_bgp_bounds_update(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
/// @}
/// @}

108
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h vendored Executable file
View File

@@ -0,0 +1,108 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \ingroup ocp_nlp
/// @{
/// \defgroup ocp_nlp_constraints ocp_nlp_constraints
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* config
************************************************/
typedef struct
{
acados_size_t (*dims_calculate_size)(void *config);
void *(*dims_assign)(void *config, void *raw_memory);
acados_size_t (*model_calculate_size)(void *config, void *dims);
void *(*model_assign)(void *config, void *dims, void *raw_memory);
int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value);
void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value);
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts);
void (*opts_update)(void *config, void *dims, void *opts);
void (*opts_set)(void *config, void *opts, char *field, void *value);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory);
struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory);
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
void (*memory_set_lam_ptr)(struct blasfeo_dvec *lam, void *memory);
void (*memory_set_tmp_lam_ptr)(struct blasfeo_dvec *tmp_lam, void *memory);
void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory);
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory);
void (*memory_set_idxb_ptr)(int *idxb, void *memory);
void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory);
void (*memory_set_idxe_ptr)(int *idxe, void *memory);
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
void (*initialize)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
void (*update_qp_matrices)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
void (*compute_fun)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
void (*bounds_update)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
void (*config_initialize_default)(void *config);
// dimension setters
void (*dims_set)(void *config_, void *dims_, const char *field, const int *value);
void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
} ocp_nlp_constraints_config;
//
acados_size_t ocp_nlp_constraints_config_calculate_size();
//
ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
/// @}
/// @}

107
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h vendored Executable file
View File

@@ -0,0 +1,107 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
///
/// \defgroup ocp_nlp_cost ocp_nlp_cost
///
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
/// @{
/// \addtogroup ocp_nlp_cost_common ocp_nlp_cost_common
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* config
************************************************/
typedef struct
{
acados_size_t (*dims_calculate_size)(void *config);
void *(*dims_assign)(void *config, void *raw_memory);
void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
void (*dims_get)(void *config_, void *dims_, const char *field, int *value);
acados_size_t (*model_calculate_size)(void *config, void *dims);
void *(*model_assign)(void *config, void *dims, void *raw_memory);
int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts);
void (*opts_update)(void *config, void *dims, void *opts);
void (*opts_set)(void *config, void *opts, const char *field, void *value);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
double *(*memory_get_fun_ptr)(void *memory);
struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory);
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory);
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
void (*memory_set_Z_ptr)(struct blasfeo_dvec *Z, void *memory);
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
// computes the function value, gradient and hessian (approximation) of the cost function
void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
// computes the cost function value (intended for globalization)
void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
void (*config_initialize_default)(void *config);
void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
} ocp_nlp_cost_config;
//
acados_size_t ocp_nlp_cost_config_calculate_size();
//
ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
/// @}
/// @}

207
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h vendored Executable file
View File

@@ -0,0 +1,207 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
/// @{
/// \addtogroup ocp_nlp_cost_conl ocp_nlp_cost_conl
/// \brief This module implements convex-over-nonlinear costs of the form
/// \f$\min_{x,u,z} \psi(y(x,u,z,p) - y_{\text{ref}}, p)\f$,
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx; // number of states
int nz; // number of algebraic variables
int nu; // number of inputs
int ny; // number of outputs
int ns; // number of slacks
} ocp_nlp_cost_conl_dims;
//
acados_size_t ocp_nlp_cost_conl_dims_calculate_size(void *config);
//
void *ocp_nlp_cost_conl_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_cost_conl_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
//
void ocp_nlp_cost_conl_dims_set(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_cost_conl_dims_get(void *config_, void *dims_, const char *field, int* value);
/************************************************
* model
************************************************/
typedef struct
{
// slack penalty has the form z^T * s + .5 * s^T * Z * s
external_function_generic *conl_cost_fun;
external_function_generic *conl_cost_fun_jac_hess;
struct blasfeo_dvec y_ref;
struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector
struct blasfeo_dvec z; // gradient of slacks as vector
double scaling;
} ocp_nlp_cost_conl_model;
//
acados_size_t ocp_nlp_cost_conl_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_conl_model_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_cost_conl_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
/************************************************
* options
************************************************/
typedef struct
{
bool gauss_newton_hess; // dummy options, we always use a gauss-newton hessian
} ocp_nlp_cost_conl_opts;
//
acados_size_t ocp_nlp_cost_conl_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_conl_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_cost_conl_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_conl_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_conl_opts_set(void *config, void *opts, const char *field, void *value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec grad; // gradient of cost function
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; // pointer to Z in qp_in
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
double fun; ///< value of the cost function
} ocp_nlp_cost_conl_memory;
//
acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_cost_conl_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
double *ocp_nlp_cost_conl_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_);
//
void ocp_nlp_cost_conl_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
//
void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
//
void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat W; // hessian of outer loss function
struct blasfeo_dmat W_chol; // cholesky factor of hessian of outer loss function
struct blasfeo_dmat Jt_ux; // jacobian of inner residual function
struct blasfeo_dmat Jt_ux_tilde; // jacobian of inner residual function plus gradient contribution of algebraic variables
struct blasfeo_dmat Jt_z; // jacobian of inner residual function wrt algebraic variables
struct blasfeo_dmat tmp_nv_ny;
struct blasfeo_dvec tmp_ny;
struct blasfeo_dvec tmp_2ns;
} ocp_nlp_cost_conl_workspace;
//
acados_size_t ocp_nlp_cost_conl_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* functions
************************************************/
//
void ocp_nlp_cost_conl_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_conl_config_initialize_default(void *config);
//
void ocp_nlp_cost_conl_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
//
void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
/// @}
/// @}
/// @}

187
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h vendored Executable file
View File

@@ -0,0 +1,187 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx; // number of states
int nz; // number of algebraic variables
int nu; // number of inputs
int ns; // number of slacks
} ocp_nlp_cost_external_dims;
//
acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config);
//
void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value);
/************************************************
* model
************************************************/
typedef struct
{
external_function_generic *ext_cost_fun; // function
external_function_generic *ext_cost_fun_jac_hess; // function, gradient and hessian
external_function_generic *ext_cost_fun_jac; // function, gradient
struct blasfeo_dvec Z;
struct blasfeo_dvec z;
struct blasfeo_dmat numerical_hessian; // custom hessian approximation
double scaling;
} ocp_nlp_cost_external_model;
//
acados_size_t ocp_nlp_cost_external_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_external_model_assign(void *config, void *dims, void *raw_memory);
/************************************************
* options
************************************************/
typedef struct
{
int use_numerical_hessian; // > 0 indicating custom hessian is used instead of CasADi evaluation
} ocp_nlp_cost_external_opts;
//
acados_size_t ocp_nlp_cost_external_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_external_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_cost_external_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_external_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_external_opts_set(void *config, void *opts, const char *field, void *value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec grad; // gradient of cost function
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; // pointer to Z in qp_in
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
double fun; ///< value of the cost function
} ocp_nlp_cost_external_memory;
//
acados_size_t ocp_nlp_cost_external_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_cost_external_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
double *ocp_nlp_cost_external_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_cost_external_memory_get_grad_ptr(void *memory_);
//
void ocp_nlp_cost_external_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
//
void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
//
void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_cost_external_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat tmp_nunx_nunx;
struct blasfeo_dmat tmp_nz_nz;
struct blasfeo_dmat tmp_nz_nunx;
struct blasfeo_dvec tmp_nunxnz;
struct blasfeo_dvec tmp_2ns; // temporary vector of dimension 2*ns
} ocp_nlp_cost_external_workspace;
//
acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* functions
************************************************/
//
void ocp_nlp_cost_external_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_external_config_initialize_default(void *config);
//
void ocp_nlp_cost_external_initialize(void *config_, void *dims, void *model_,
void *opts_, void *mem_, void *work_);
//
void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_external_compute_fun(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_

243
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h vendored Executable file
View File

@@ -0,0 +1,243 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
/// @{
/// \addtogroup ocp_nlp_cost_ls ocp_nlp_cost_ls
/// \brief This module implements linear-least squares costs of the form
/// \f$\min_{x,u,z} \| V_x x + V_u u + V_z z - y_{\text{ref}}\|_W^2\f$.
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
////////////////////////////////////////////////////////////////////////////////
// dims //
////////////////////////////////////////////////////////////////////////////////
typedef struct
{
int nx; // number of states
int nz; // number of algebraic variables
int nu; // number of inputs
int ny; // number of outputs
int ns; // number of slacks
} ocp_nlp_cost_ls_dims;
/// Calculate the size of the ocp_nlp_cost_ls_dims struct
///
/// \param[in] config_ structure containing configuration of ocp_nlp_cost
/// module
/// \param[out] []
/// \return \c size of ocp_nlp_dims struct
acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config);
/// Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct
///
/// \param[in] config structure containing configuration of ocp_nlp_cost
/// module
/// \param[in] raw_memory pointer to memory location
/// \param[out] []
/// \return dims
void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int* value);
////////////////////////////////////////////////////////////////////////////////
// model //
////////////////////////////////////////////////////////////////////////////////
/// structure containing the data describing the linear least-square cost
typedef struct
{
// slack penalty has the form z^T * s + .5 * s^T * Z * s
struct blasfeo_dmat Cyt; ///< output matrix: Cy * [u,x] = y; in transposed form
struct blasfeo_dmat Vz; ///< Vz in ls cost Vx*x + Vu*u + Vz*z
struct blasfeo_dmat W; ///< ls norm corresponding to this matrix
struct blasfeo_dvec y_ref; ///< yref
struct blasfeo_dvec Z; ///< diagonal Hessian of slacks as vector (lower and upper)
struct blasfeo_dvec z; ///< gradient of slacks as vector (lower and upper)
double scaling;
int W_changed; ///< flag indicating whether W has changed and needs to be refactorized
int Cyt_or_scaling_changed; ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed
} ocp_nlp_cost_ls_model;
//
acados_size_t ocp_nlp_cost_ls_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_ls_model_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_cost_ls_model_set(void *config_, void *dims_, void *model_,
const char *field, void *value_);
////////////////////////////////////////////////////////////////////////////////
// options //
////////////////////////////////////////////////////////////////////////////////
typedef struct
{
int dummy; // struct can't be void
} ocp_nlp_cost_ls_opts;
//
acados_size_t ocp_nlp_cost_ls_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_ls_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_cost_ls_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_ls_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void *value);
////////////////////////////////////////////////////////////////////////////////
// memory //
////////////////////////////////////////////////////////////////////////////////
/// structure containing the memory associated with cost_ls component
/// of the ocp_nlp module
typedef struct
{
struct blasfeo_dmat hess; ///< hessian of cost function
struct blasfeo_dmat W_chol; ///< cholesky factor of weight matrix
struct blasfeo_dvec res; ///< ls residual r(x)
struct blasfeo_dvec grad; ///< gradient of cost function
struct blasfeo_dvec *ux; ///< pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; ///< pointer to ux in tmp_nlp_out
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
struct blasfeo_dmat *RSQrq; ///< pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; ///< pointer to Z in qp_in
double fun; ///< value of the cost function
} ocp_nlp_cost_ls_memory;
//
acados_size_t ocp_nlp_cost_ls_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_cost_ls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
double *ocp_nlp_cost_ls_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_cost_ls_memory_get_grad_ptr(void *memory_);
//
void ocp_nlp_cost_ls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
//
void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
//
void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_cost_ls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
////////////////////////////////////////////////////////////////////////////////
// workspace //
////////////////////////////////////////////////////////////////////////////////
typedef struct
{
struct blasfeo_dmat tmp_nv_ny; // temporary matrix of dimensions nv, ny
struct blasfeo_dmat Cyt_tilde; // updated Cyt (after z elimination)
struct blasfeo_dmat dzdux_tran; // derivatives of z wrt u and x (tran)
struct blasfeo_dvec tmp_ny; // temporary vector of dimension ny
struct blasfeo_dvec tmp_2ns; // temporary vector of dimension ny
struct blasfeo_dvec tmp_nz; // temporary vector of dimension nz
struct blasfeo_dvec y_ref_tilde; // updated y_ref (after z elimination)
} ocp_nlp_cost_ls_workspace;
//
acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, void *opts);
////////////////////////////////////////////////////////////////////////////////
// functions //
////////////////////////////////////////////////////////////////////////////////
// computations that are done once when solver is created
void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_ls_config_initialize_default(void *config);
//
void ocp_nlp_cost_ls_initialize(void *config_, void *dims, void *model_, void *opts_,
void *mem_, void *work_);
//
void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
/// @}
/// @}
/// @}

211
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h vendored Executable file
View File

@@ -0,0 +1,211 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
/// @{
/// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls
/// \brief This module implements nonlinear-least squares costs of the form
/// \f$\min_{x,u,z} \| y(x,u,z,p) - y_{\text{ref}} \|_W^2\f$,
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx; // number of states
int nz; // number of algebraic variables
int nu; // number of inputs
int ny; // number of outputs
int ns; // number of slacks
} ocp_nlp_cost_nls_dims;
//
acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config);
//
void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value);
/************************************************
* model
************************************************/
typedef struct
{
// nonliner function nls_y(x,u) replaces Cy * [x,u] in ls_cost
// slack penalty has the form z^T * s + .5 * s^T * Z * s
external_function_generic *nls_y_fun; // evaluation of nls function
external_function_generic *nls_y_fun_jac; // evaluation nls function and jacobian
external_function_generic *nls_y_hess; // hessian*seeds of nls residuals
struct blasfeo_dmat W; //
struct blasfeo_dvec y_ref;
struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector
struct blasfeo_dvec z; // gradient of slacks as vector
double scaling;
int W_changed; ///< flag indicating whether W has changed and needs to be refactorized
} ocp_nlp_cost_nls_model;
//
acados_size_t ocp_nlp_cost_nls_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_nls_model_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
/************************************************
* options
************************************************/
typedef struct
{
bool gauss_newton_hess; // gauss-newton hessian approximation
} ocp_nlp_cost_nls_opts;
//
acados_size_t ocp_nlp_cost_nls_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_nls_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_cost_nls_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_nls_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_nls_opts_set(void *config, void *opts, const char *field, void *value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dmat W_chol; // cholesky factor of weight matrix
struct blasfeo_dmat Jt; // jacobian of nls fun
struct blasfeo_dvec res; // nls residual r(x)
struct blasfeo_dvec grad; // gradient of cost function
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; // pointer to Z in qp_in
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
double fun; ///< value of the cost function
} ocp_nlp_cost_nls_memory;
//
acados_size_t ocp_nlp_cost_nls_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_cost_nls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
double *ocp_nlp_cost_nls_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_grad_ptr(void *memory_);
//
void ocp_nlp_cost_nls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
//
void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
//
void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_cost_nls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat tmp_nv_ny;
struct blasfeo_dmat tmp_nv_nv;
struct blasfeo_dmat Vz;
struct blasfeo_dmat Cyt_tilde;
struct blasfeo_dvec tmp_ny;
struct blasfeo_dvec tmp_2ns;
struct blasfeo_dvec tmp_nz;
} ocp_nlp_cost_nls_workspace;
//
acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* functions
************************************************/
//
void ocp_nlp_cost_nls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_nls_config_initialize_default(void *config);
//
void ocp_nlp_cost_nls_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
//
void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
/// @}
/// @}
/// @}

119
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h vendored Executable file
View File

@@ -0,0 +1,119 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \ingroup ocp_nlp
/// @{
/// \defgroup ocp_nlp_dynamics ocp_nlp_dynamics
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* config
************************************************/
typedef struct
{
void (*config_initialize_default)(void *config);
sim_config *sim_solver;
/* dims */
acados_size_t (*dims_calculate_size)(void *config);
void *(*dims_assign)(void *config, void *raw_memory);
void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
/* model */
acados_size_t (*model_calculate_size)(void *config, void *dims);
void *(*model_assign)(void *config, void *dims, void *raw_memory);
void (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
/* opts */
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts);
void (*opts_set)(void *config_, void *opts_, const char *field, void *value);
void (*opts_update)(void *config, void *dims, void *opts);
/* memory */
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
// get shooting node gap x_next(x_n, u_n) - x_{n+1}
struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory_);
void (*memory_set_ux1_ptr)(struct blasfeo_dvec *ux1, void *memory_);
void (*memory_set_tmp_ux1_ptr)(struct blasfeo_dvec *tmp_ux1, void *memory_);
void (*memory_set_pi_ptr)(struct blasfeo_dvec *pi, void *memory_);
void (*memory_set_tmp_pi_ptr)(struct blasfeo_dvec *tmp_pi, void *memory_);
void (*memory_set_BAbt_ptr)(struct blasfeo_dmat *BAbt, void *memory_);
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory_);
void (*memory_set_dzduxt_ptr)(struct blasfeo_dmat *mat, void *memory_);
void (*memory_set_sim_guess_ptr)(struct blasfeo_dvec *vec, bool *bool_ptr, void *memory_);
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *vec, void *memory_);
void (*memory_get)(void *config, void *dims, void *mem, const char *field, void* value);
/* workspace */
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
int (*precompute)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
} ocp_nlp_dynamics_config;
//
acados_size_t ocp_nlp_dynamics_config_calculate_size();
//
ocp_nlp_dynamics_config *ocp_nlp_dynamics_config_assign(void *raw_memory);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
/// @}
/// @}

209
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h vendored Executable file
View File

@@ -0,0 +1,209 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_dynamics
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
#include "acados_c/sim_interface.h"
/************************************************
* dims
************************************************/
typedef struct
{
void *sim;
int nx; // number of states at the current stage
int nz; // number of algebraic states at the current stage
int nu; // number of inputs at the current stage
int nx1; // number of states at the next stage
int nu1; // number of inputes at the next stage
} ocp_nlp_dynamics_cont_dims;
//
acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config);
//
void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value);
/************************************************
* options
************************************************/
typedef struct
{
void *sim_solver;
int compute_adj;
int compute_hess;
} ocp_nlp_dynamics_cont_opts;
//
acados_size_t ocp_nlp_dynamics_cont_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_dynamics_cont_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_dynamics_cont_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_dynamics_cont_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_dynamics_cont_opts_set(void *config, void *opts, const char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec fun;
struct blasfeo_dvec adj;
struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage
struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage
struct blasfeo_dvec *tmp_ux1; // pointer to ux in tmp_nlp_out at next stage
struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage
struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage
struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec *z_alg; // pointer to output z at t = 0
bool *set_sim_guess; // indicate if initialization for integrator is set from outside
struct blasfeo_dvec *sim_guess; // initializations for integrator
// struct blasfeo_dvec *z; // pointer to (input) z in nlp_out at current stage
struct blasfeo_dmat *dzduxt; // pointer to dzdux transposed
void *sim_solver; // sim solver memory
} ocp_nlp_dynamics_cont_memory;
//
acados_size_t ocp_nlp_dynamics_cont_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_dynamics_cont_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_fun_ptr(void *memory);
//
struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_adj_ptr(void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat hess;
sim_in *sim_in;
sim_out *sim_out;
void *sim_solver; // sim solver workspace
} ocp_nlp_dynamics_cont_workspace;
acados_size_t ocp_nlp_dynamics_cont_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* model
************************************************/
typedef struct
{
void *sim_model;
// double *state_transition; // TODO
double T; // simulation time
} ocp_nlp_dynamics_cont_model;
//
acados_size_t ocp_nlp_dynamics_cont_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_dynamics_cont_model_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_dynamics_cont_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
/************************************************
* functions
************************************************/
//
void ocp_nlp_dynamics_cont_config_initialize_default(void *config);
//
void ocp_nlp_dynamics_cont_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
/// @}
/// @}

192
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h vendored Executable file
View File

@@ -0,0 +1,192 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_dynamics
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx; // number of states at the current stage
int nu; // number of inputs at the current stage
int nx1; // number of states at the next stage
int nu1; // number of inputes at the next stage
} ocp_nlp_dynamics_disc_dims;
//
acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config);
//
void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value);
/************************************************
* options
************************************************/
typedef struct
{
int compute_adj;
int compute_hess;
} ocp_nlp_dynamics_disc_opts;
//
acados_size_t ocp_nlp_dynamics_disc_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_dynamics_disc_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_dynamics_disc_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_dynamics_disc_opts_update(void *config, void *dims, void *opts);
//
int ocp_nlp_dynamics_disc_precompute(void *config_, void *dims, void *model_, void *opts_,
void *mem_, void *work_);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec fun;
struct blasfeo_dvec adj;
struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage
struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage
struct blasfeo_dvec *tmp_ux1;// pointer to ux in tmp_nlp_out at next stage
struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage
struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage
struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
} ocp_nlp_dynamics_disc_memory;
//
acados_size_t ocp_nlp_dynamics_disc_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_dynamics_disc_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_fun_ptr(void *memory);
//
struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_adj_ptr(void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat tmp_nv_nv;
} ocp_nlp_dynamics_disc_workspace;
acados_size_t ocp_nlp_dynamics_disc_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* model
************************************************/
typedef struct
{
external_function_generic *disc_dyn_fun;
external_function_generic *disc_dyn_fun_jac;
external_function_generic *disc_dyn_fun_jac_hess;
} ocp_nlp_dynamics_disc_model;
//
acados_size_t ocp_nlp_dynamics_disc_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_dynamics_disc_model_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_dynamics_disc_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
/************************************************
* functions
************************************************/
//
void ocp_nlp_dynamics_disc_config_initialize_default(void *config);
//
void ocp_nlp_dynamics_disc_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
/// @}
/// @}

122
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h vendored Executable file
View File

@@ -0,0 +1,122 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \ingroup ocp_nlp
/// @{
/// \defgroup ocp_nlp_reg ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/ocp_qp/ocp_qp_common.h"
/* dims */
//typedef ocp_qp_dims ocp_nlp_reg_dims;
typedef struct
{
int *nx;
int *nu;
int *nbu;
int *nbx;
int *ng;
int N;
} ocp_nlp_reg_dims;
//
acados_size_t ocp_nlp_reg_dims_calculate_size(int N);
//
ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory);
//
void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char *field, int* value);
/* config */
typedef struct
{
/* dims */
acados_size_t (*dims_calculate_size)(int N);
ocp_nlp_reg_dims *(*dims_assign)(int N, void *raw_memory);
void (*dims_set)(void *config, ocp_nlp_reg_dims *dims, int stage, char *field, int *value);
/* opts */
acados_size_t (*opts_calculate_size)(void);
void *(*opts_assign)(void *raw_memory);
void (*opts_initialize_default)(void *config, ocp_nlp_reg_dims *dims, void *opts);
void (*opts_set)(void *config, ocp_nlp_reg_dims *dims, void *opts, char *field, void* value);
/* memory */
acados_size_t (*memory_calculate_size)(void *config, ocp_nlp_reg_dims *dims, void *opts);
void *(*memory_assign)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
void (*memory_set)(void *config, ocp_nlp_reg_dims *dims, void *memory, char *field, void* value);
void (*memory_set_RSQrq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
void (*memory_set_rq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
void (*memory_set_BAbt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
void (*memory_set_b_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
void (*memory_set_idxb_ptr)(ocp_nlp_reg_dims *dims, int **idxb, void *memory);
void (*memory_set_DCt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
void (*memory_set_ux_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
void (*memory_set_pi_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
void (*memory_set_lam_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
/* functions */
void (*regularize_hessian)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
void (*correct_dual_sol)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
} ocp_nlp_reg_config;
//
acados_size_t ocp_nlp_reg_config_calculate_size(void);
//
void *ocp_nlp_reg_config_assign(void *raw_memory);
/* regularization help functions */
void acados_reconstruct_A(int dim, double *A, double *V, double *d);
void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon);
void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
/// @}
/// @}

146
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h vendored Executable file
View File

@@ -0,0 +1,146 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
typedef struct
{
double delta;
double epsilon;
// double gamma; // 0.0
} ocp_nlp_reg_convexify_opts;
//
acados_size_t ocp_nlp_reg_convexify_opts_calculate_size(void);
//
void *ocp_nlp_reg_convexify_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_convexify_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_convexify_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct {
double *R;
double *V; // TODO move to workspace
double *d; // TODO move to workspace
double *e; // TODO move to workspace
double *reg_hess; // TODO move to workspace
struct blasfeo_dmat Q_tilde;
struct blasfeo_dmat Q_bar;
struct blasfeo_dmat BAQ;
struct blasfeo_dmat L;
struct blasfeo_dmat delta_eye;
struct blasfeo_dmat St_copy;
struct blasfeo_dmat *original_RSQrq;
struct blasfeo_dmat tmp_RSQ;
struct blasfeo_dvec tmp_nuxM;
struct blasfeo_dvec tmp_nbgM;
// struct blasfeo_dvec grad;
// struct blasfeo_dvec b2;
// giaf's
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec **rq; // pointer to rq in qp_in
struct blasfeo_dmat **BAbt; // pointer to BAbt in qp_in
struct blasfeo_dvec **b; // pointer to b in qp_in
struct blasfeo_dmat **DCt; // pointer to DCt in qp_in
struct blasfeo_dvec **ux; // pointer to ux in qp_out
struct blasfeo_dvec **pi; // pointer to pi in qp_out
struct blasfeo_dvec **lam; // pointer to lam in qp_out
int **idxb; // pointer to idxb in qp_in
} ocp_nlp_reg_convexify_memory;
//
acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_convexify_assign_memory(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// TODO
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
/// @}
/// @}

121
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h vendored Executable file
View File

@@ -0,0 +1,121 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
typedef struct
{
double epsilon;
} ocp_nlp_reg_mirror_opts;
//
acados_size_t ocp_nlp_reg_mirror_opts_calculate_size(void);
//
void *ocp_nlp_reg_mirror_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_mirror_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
double *reg_hess; // TODO move to workspace
double *V; // TODO move to workspace
double *d; // TODO move to workspace
double *e; // TODO move to workspace
// giaf's
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
} ocp_nlp_reg_mirror_memory;
//
acados_size_t ocp_nlp_reg_mirror_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_mirror_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// TODO
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
/// @}
/// @}

106
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h vendored Executable file
View File

@@ -0,0 +1,106 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
//
acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
//
void *ocp_nlp_reg_noreg_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_noreg_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
//
acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_noreg_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// not needed
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
/// @}
/// @}

121
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h vendored Executable file
View File

@@ -0,0 +1,121 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
typedef struct
{
double epsilon;
} ocp_nlp_reg_project_opts;
//
acados_size_t ocp_nlp_reg_project_opts_calculate_size(void);
//
void *ocp_nlp_reg_project_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_project_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
double *reg_hess; // TODO move to workspace
double *V; // TODO move to workspace
double *d; // TODO move to workspace
double *e; // TODO move to workspace
// giaf's
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
} ocp_nlp_reg_project_memory;
//
acados_size_t ocp_nlp_reg_project_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_project_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// TODO
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
/// @}
/// @}

132
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h vendored Executable file
View File

@@ -0,0 +1,132 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
typedef struct
{
double thr_eig;
double min_eig;
double min_pivot;
int pivoting;
} ocp_nlp_reg_project_reduc_hess_opts;
//
acados_size_t ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void);
//
void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
double *reg_hess; // TODO move to workspace
double *V; // TODO move to workspace
double *d; // TODO move to workspace
double *e; // TODO move to workspace
// giaf's
struct blasfeo_dmat L; // TODO move to workspace
struct blasfeo_dmat L2; // TODO move to workspace
struct blasfeo_dmat L3; // TODO move to workspace
struct blasfeo_dmat Ls; // TODO move to workspace
struct blasfeo_dmat P; // TODO move to workspace
struct blasfeo_dmat AL; // TODO move to workspace
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dmat **BAbt; // pointer to RSQrq in qp_in
} ocp_nlp_reg_project_reduc_hess_memory;
//
acados_size_t ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// TODO
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
/// @}
/// @}

170
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h vendored Executable file
View File

@@ -0,0 +1,170 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_solver
/// @{
/// \addtogroup ocp_nlp_sqp ocp_nlp_sqp
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_H_
#define ACADOS_OCP_NLP_OCP_NLP_SQP_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_nlp/ocp_nlp_common.h"
#include "acados/utils/types.h"
/************************************************
* options
************************************************/
typedef struct
{
ocp_nlp_opts *nlp_opts;
double tol_stat; // exit tolerance on stationarity condition
double tol_eq; // exit tolerance on equality constraints
double tol_ineq; // exit tolerance on inequality constraints
double tol_comp; // exit tolerance on complementarity condition
int max_iter;
int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
int qp_warm_start; // qp_warm_start in all but the first sqp iterations
bool warm_start_first_qp; // to set qp_warm_start in first iteration
int rti_phase; // only phase 0 at the moment
int initialize_t_slacks; // 0-false or 1-true
} ocp_nlp_sqp_opts;
//
acados_size_t ocp_nlp_sqp_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_sqp_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_sqp_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_sqp_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* value);
//
void ocp_nlp_sqp_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
// nlp memory
ocp_nlp_memory *nlp_mem;
double time_qp_sol;
double time_qp_solver_call;
double time_qp_xcond;
double time_lin;
double time_reg;
double time_tot;
double time_glob;
double time_sim;
double time_sim_la;
double time_sim_ad;
double time_solution_sensitivities;
// statistics
double *stat;
int stat_m;
int stat_n;
int status;
int sqp_iter;
} ocp_nlp_sqp_memory;
//
acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config, void *dims, void *opts_);
//
void *ocp_nlp_sqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
//
void ocp_nlp_sqp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
void *opts_, void *mem_, void *work_);
/************************************************
* workspace
************************************************/
typedef struct
{
ocp_nlp_workspace *nlp_work;
// temp QP in & out (to be used as workspace in param sens)
ocp_qp_in *tmp_qp_in;
ocp_qp_out *tmp_qp_out;
// qp residuals
ocp_qp_res *qp_res;
ocp_qp_res_ws *qp_res_ws;
} ocp_nlp_sqp_workspace;
//
acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config, void *dims, void *opts_);
/************************************************
* functions
************************************************/
//
int ocp_nlp_sqp(void *config, void *dims, void *nlp_in, void *nlp_out,
void *args, void *mem, void *work_);
//
void ocp_nlp_sqp_config_initialize_default(void *config_);
//
int ocp_nlp_sqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
void *opts_, void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_H_
/// @}
/// @}
/// @}

163
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h vendored Executable file
View File

@@ -0,0 +1,163 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_solver
/// @{
/// \addtogroup ocp_nlp_sqp_rti ocp_nlp_sqp_rti
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
#define ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_nlp/ocp_nlp_common.h"
#include "acados/utils/types.h"
/************************************************
* options
************************************************/
typedef struct
{
ocp_nlp_opts *nlp_opts;
int compute_dual_sol;
int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
int qp_warm_start; // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
bool warm_start_first_qp; // to set qp_warm_start in first iteration
int rti_phase; // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both)
} ocp_nlp_sqp_rti_opts;
//
acados_size_t ocp_nlp_sqp_rti_opts_calculate_size(void *config_, void *dims_);
//
void *ocp_nlp_sqp_rti_opts_assign(void *config_, void *dims_, void *raw_memory);
//
void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *opts_);
//
void ocp_nlp_sqp_rti_opts_update(void *config_, void *dims_, void *opts_);
//
void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, void* value);
//
void ocp_nlp_sqp_rti_opts_set_at_stage(void *config_, void *opts_, size_t stage,
const char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
// nlp memory
ocp_nlp_memory *nlp_mem;
double time_qp_sol;
double time_qp_solver_call;
double time_qp_xcond;
double time_lin;
double time_reg;
double time_tot;
double time_glob;
double time_solution_sensitivities;
// statistics
double *stat;
int stat_m;
int stat_n;
int status;
} ocp_nlp_sqp_rti_memory;
//
acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts_);
//
void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_,
void *raw_memory);
/************************************************
* workspace
************************************************/
typedef struct
{
ocp_nlp_workspace *nlp_work;
// temp QP in & out (to be used as workspace in param sens)
ocp_qp_in *tmp_qp_in;
ocp_qp_out *tmp_qp_out;
// qp residuals
ocp_qp_res *qp_res;
ocp_qp_res_ws *qp_res_ws;
} ocp_nlp_sqp_rti_workspace;
//
acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims_, void *opts_);
/************************************************
* functions
************************************************/
//
int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
void *opts_, void *mem_, void *work_);
//
void ocp_nlp_sqp_rti_config_initialize_default(void *config_);
//
int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_,
void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
/// @}
/// @}
/// @}