acados: update to 0.2.2 (#28821)
* use 0.2.2 * Add mac binaries * Replace libqpOASES with symlink on larch64 * Add comment to build script --------- Co-authored-by: Kacper Rączy <gfw.kra@gmail.com>pull/29420/head
Maxime Desroches
2 years ago
committed by
GitHub
parent
7ec52da33a
commit
4c1b8b71d0
144 changed files with 7235 additions and 3087 deletions
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,64 @@ |
||||
# -*- coding: future_fstrings -*- |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
|
||||
cdef extern from "acados/sim/sim_common.h": |
||||
ctypedef struct sim_config: |
||||
pass |
||||
|
||||
ctypedef struct sim_opts: |
||||
pass |
||||
|
||||
ctypedef struct sim_in: |
||||
pass |
||||
|
||||
ctypedef struct sim_out: |
||||
pass |
||||
|
||||
|
||||
cdef extern from "acados_c/sim_interface.h": |
||||
|
||||
ctypedef struct sim_plan: |
||||
pass |
||||
|
||||
ctypedef struct sim_solver: |
||||
pass |
||||
|
||||
# out |
||||
void sim_out_get(sim_config *config, void *dims, sim_out *out, const char *field, void *value) |
||||
int sim_dims_get_from_attr(sim_config *config, void *dims, const char *field, void *dims_data) |
||||
|
||||
# opts |
||||
void sim_opts_set(sim_config *config, void *opts_, const char *field, void *value) |
||||
|
||||
# get/set |
||||
void sim_in_set(sim_config *config, void *dims, sim_in *sim_in, const char *field, void *value) |
||||
void sim_solver_set(sim_solver *solver, const char *field, void *value) |
||||
@ -0,0 +1,256 @@ |
||||
# -*- coding: future_fstrings -*- |
||||
# |
||||
# 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.; |
||||
# |
||||
# cython: language_level=3 |
||||
# cython: profile=False |
||||
# distutils: language=c |
||||
|
||||
cimport cython |
||||
from libc cimport string |
||||
# from libc cimport bool as bool_t |
||||
|
||||
cimport acados_sim_solver_common |
||||
cimport acados_sim_solver |
||||
|
||||
cimport numpy as cnp |
||||
|
||||
import os |
||||
from datetime import datetime |
||||
import numpy as np |
||||
|
||||
|
||||
cdef class AcadosSimSolverCython: |
||||
""" |
||||
Class to interact with the acados sim solver C object. |
||||
""" |
||||
|
||||
cdef acados_sim_solver.sim_solver_capsule *capsule |
||||
cdef void *sim_dims |
||||
cdef acados_sim_solver_common.sim_opts *sim_opts |
||||
cdef acados_sim_solver_common.sim_config *sim_config |
||||
cdef acados_sim_solver_common.sim_out *sim_out |
||||
cdef acados_sim_solver_common.sim_in *sim_in |
||||
cdef acados_sim_solver_common.sim_solver *sim_solver |
||||
|
||||
cdef bint solver_created |
||||
|
||||
cdef str model_name |
||||
|
||||
cdef str sim_solver_type |
||||
|
||||
cdef list gettable_vectors |
||||
cdef list gettable_matrices |
||||
cdef list gettable_scalars |
||||
|
||||
def __cinit__(self, model_name): |
||||
|
||||
self.solver_created = False |
||||
|
||||
self.model_name = model_name |
||||
|
||||
# create capsule |
||||
self.capsule = acados_sim_solver.acados_sim_solver_create_capsule() |
||||
|
||||
# create solver |
||||
assert acados_sim_solver.acados_sim_create(self.capsule) == 0 |
||||
self.solver_created = True |
||||
|
||||
# get pointers solver |
||||
self.__get_pointers_solver() |
||||
|
||||
self.gettable_vectors = ['x', 'u', 'z', 'S_adj'] |
||||
self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic'] |
||||
self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la'] |
||||
|
||||
def __get_pointers_solver(self): |
||||
""" |
||||
Private function to get the pointers for solver |
||||
""" |
||||
# get pointers solver |
||||
self.sim_opts = acados_sim_solver.acados_get_sim_opts(self.capsule) |
||||
self.sim_dims = acados_sim_solver.acados_get_sim_dims(self.capsule) |
||||
self.sim_config = acados_sim_solver.acados_get_sim_config(self.capsule) |
||||
self.sim_out = acados_sim_solver.acados_get_sim_out(self.capsule) |
||||
self.sim_in = acados_sim_solver.acados_get_sim_in(self.capsule) |
||||
self.sim_solver = acados_sim_solver.acados_get_sim_solver(self.capsule) |
||||
|
||||
|
||||
def simulate(self, x=None, u=None, z=None, p=None): |
||||
""" |
||||
Simulate the system forward for the given x, u, z, p and return x_next. |
||||
Wrapper around `solve()` taking care of setting/getting inputs/outputs. |
||||
""" |
||||
if x is not None: |
||||
self.set('x', x) |
||||
if u is not None: |
||||
self.set('u', u) |
||||
if z is not None: |
||||
self.set('z', z) |
||||
if p is not None: |
||||
self.set('p', p) |
||||
|
||||
status = self.solve() |
||||
|
||||
if status == 2: |
||||
print("Warning: acados_sim_solver reached maximum iterations.") |
||||
elif status != 0: |
||||
raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.') |
||||
|
||||
x_next = self.get('x') |
||||
return x_next |
||||
|
||||
|
||||
def solve(self): |
||||
""" |
||||
Solve the sim with current input. |
||||
""" |
||||
return acados_sim_solver.acados_sim_solve(self.capsule) |
||||
|
||||
|
||||
def get(self, field_): |
||||
""" |
||||
Get the last solution of the solver. |
||||
|
||||
:param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la'] |
||||
""" |
||||
field = field_.encode('utf-8') |
||||
|
||||
if field_ in self.gettable_vectors: |
||||
return self.__get_vector(field) |
||||
elif field_ in self.gettable_matrices: |
||||
return self.__get_matrix(field) |
||||
elif field_ in self.gettable_scalars: |
||||
return self.__get_scalar(field) |
||||
else: |
||||
raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \ |
||||
f' available fields are {", ".join(self.gettable.keys())}') |
||||
|
||||
|
||||
def __get_scalar(self, field): |
||||
cdef double scalar |
||||
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> &scalar) |
||||
return scalar |
||||
|
||||
|
||||
def __get_vector(self, field): |
||||
cdef int[2] dims |
||||
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0]) |
||||
# cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((dims[0],), dtype=np.float64)) |
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims[0]),) |
||||
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data) |
||||
return out |
||||
|
||||
|
||||
def __get_matrix(self, field): |
||||
cdef int[2] dims |
||||
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0]) |
||||
cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64) |
||||
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data) |
||||
return out |
||||
|
||||
|
||||
def set(self, field_: str, value_): |
||||
""" |
||||
Set numerical data inside the solver. |
||||
|
||||
:param field: string in ['p', 'seed_adj', 'T', 'x', 'u', 'xdot', 'z'] |
||||
:param value: the value with appropriate size. |
||||
""" |
||||
settable = ['seed_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw |
||||
|
||||
# cast value_ to avoid conversion issues |
||||
if isinstance(value_, (float, int)): |
||||
value_ = np.array([value_]) |
||||
# if len(value_.shape) > 1: |
||||
# raise RuntimeError('AcadosSimSolverCython.set(): value_ should be 1 dimensional') |
||||
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64).flatten() |
||||
|
||||
field = field_.encode('utf-8') |
||||
cdef int[2] dims |
||||
|
||||
# treat parameters separately |
||||
if field_ == 'p': |
||||
assert acados_sim_solver.acados_sim_update_params(self.capsule, <double *> value.data, value.shape[0]) == 0 |
||||
return |
||||
else: |
||||
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0]) |
||||
|
||||
value_ = np.ravel(value_, order='F') |
||||
|
||||
value_shape = value_.shape |
||||
if len(value_shape) == 1: |
||||
value_shape = (value_shape[0], 0) |
||||
|
||||
if value_shape != tuple(dims): |
||||
raise Exception(f'AcadosSimSolverCython.set(): mismatching dimension' \ |
||||
f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).') |
||||
|
||||
# set |
||||
if field_ in ['xdot', 'z']: |
||||
acados_sim_solver_common.sim_solver_set(self.sim_solver, field, <void *> value.data) |
||||
elif field_ in settable: |
||||
acados_sim_solver_common.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, <void *> value.data) |
||||
else: |
||||
raise Exception(f'AcadosSimSolverCython.set(): Unknown field {field_},' \ |
||||
f' available fields are {", ".join(settable)}') |
||||
|
||||
|
||||
def options_set(self, field_: str, value_: bool): |
||||
""" |
||||
Set solver options |
||||
|
||||
:param field: string in ['sens_forw', 'sens_adj', 'sens_hess'] |
||||
:param value: Boolean |
||||
""" |
||||
fields = ['sens_forw', 'sens_adj', 'sens_hess'] |
||||
if field_ not in fields: |
||||
raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n") |
||||
|
||||
field = field_.encode('utf-8') |
||||
|
||||
if not isinstance(value_, bool): |
||||
raise TypeError("options_set: expected boolean for value") |
||||
|
||||
cdef bint bool_value = value_ |
||||
acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value) |
||||
# TODO: only allow setting |
||||
# if getattr(self.acados_sim.solver_options, field_) or value_ == False: |
||||
# acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value) |
||||
# else: |
||||
# raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n") |
||||
|
||||
return |
||||
|
||||
|
||||
def __del__(self): |
||||
if self.solver_created: |
||||
acados_sim_solver.acados_sim_free(self.capsule) |
||||
acados_sim_solver.acados_sim_solver_free_capsule(self.capsule) |
||||
@ -1 +0,0 @@ |
||||
exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.? |
||||
@ -0,0 +1,51 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
cimport acados_sim_solver_common |
||||
|
||||
cdef extern from "acados_sim_solver_{{ model.name }}.h": |
||||
ctypedef struct sim_solver_capsule "sim_solver_capsule": |
||||
pass |
||||
|
||||
sim_solver_capsule * acados_sim_solver_create_capsule "{{ model.name }}_acados_sim_solver_create_capsule"() |
||||
int acados_sim_solver_free_capsule "{{ model.name }}_acados_sim_solver_free_capsule"(sim_solver_capsule *capsule) |
||||
|
||||
int acados_sim_create "{{ model.name }}_acados_sim_create"(sim_solver_capsule * capsule) |
||||
int acados_sim_solve "{{ model.name }}_acados_sim_solve"(sim_solver_capsule * capsule) |
||||
int acados_sim_free "{{ model.name }}_acados_sim_free"(sim_solver_capsule * capsule) |
||||
int acados_sim_update_params "{{ model.name }}_acados_sim_update_params"(sim_solver_capsule * capsule, double *value, int np_) |
||||
# int acados_sim_update_params_sparse "{{ model.name }}_acados_sim_update_params_sparse"(sim_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) |
||||
|
||||
acados_sim_solver_common.sim_in *acados_get_sim_in "{{ model.name }}_acados_get_sim_in"(sim_solver_capsule * capsule) |
||||
acados_sim_solver_common.sim_out *acados_get_sim_out "{{ model.name }}_acados_get_sim_out"(sim_solver_capsule * capsule) |
||||
acados_sim_solver_common.sim_solver *acados_get_sim_solver "{{ model.name }}_acados_get_sim_solver"(sim_solver_capsule * capsule) |
||||
acados_sim_solver_common.sim_config *acados_get_sim_config "{{ model.name }}_acados_get_sim_config"(sim_solver_capsule * capsule) |
||||
acados_sim_solver_common.sim_opts *acados_get_sim_opts "{{ model.name }}_acados_get_sim_opts"(sim_solver_capsule * capsule) |
||||
void *acados_get_sim_dims "{{ model.name }}_acados_get_sim_dims"(sim_solver_capsule * capsule) |
||||
@ -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.; |
||||
*/ |
||||
|
||||
|
||||
#ifndef {{ model.name }}_COST |
||||
#define {{ model.name }}_COST |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
|
||||
// Cost at initial shooting node
|
||||
{% if cost.cost_type_0 == "NONLINEAR_LS" %} |
||||
int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_0_fun_n_in(void); |
||||
int {{ model.name }}_cost_y_0_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void); |
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_0_hess_n_in(void); |
||||
int {{ model.name }}_cost_y_0_hess_n_out(void); |
||||
{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} |
||||
|
||||
int {{ model.name }}_conl_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_conl_cost_0_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_conl_cost_0_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_conl_cost_0_fun_sparsity_out(int); |
||||
int {{ model.name }}_conl_cost_0_fun_n_in(void); |
||||
int {{ model.name }}_conl_cost_0_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_conl_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_conl_cost_0_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_conl_cost_0_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_conl_cost_0_fun_jac_hess_n_out(void); |
||||
|
||||
{% elif cost.cost_type_0 == "EXTERNAL" %} |
||||
{%- if cost.cost_ext_fun_type_0 == "casadi" %} |
||||
int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void); |
||||
{%- else %} |
||||
int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *); |
||||
{%- endif %} |
||||
{% endif %} |
||||
|
||||
|
||||
// Cost at path shooting node
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %} |
||||
int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_fun_n_in(void); |
||||
int {{ model.name }}_cost_y_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void); |
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_hess_n_in(void); |
||||
int {{ model.name }}_cost_y_hess_n_out(void); |
||||
|
||||
{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} |
||||
int {{ model.name }}_conl_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_conl_cost_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_conl_cost_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_conl_cost_fun_sparsity_out(int); |
||||
int {{ model.name }}_conl_cost_fun_n_in(void); |
||||
int {{ model.name }}_conl_cost_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_conl_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_conl_cost_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_conl_cost_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_conl_cost_fun_jac_hess_n_out(void); |
||||
{% elif cost.cost_type == "EXTERNAL" %} |
||||
{%- if cost.cost_ext_fun_type == "casadi" %} |
||||
int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_fun_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void); |
||||
{%- else %} |
||||
int {{ cost.cost_function_ext_cost }}(void **, void **, void *); |
||||
{%- endif %} |
||||
{% endif %} |
||||
|
||||
// Cost at terminal shooting node
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %} |
||||
int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_e_fun_n_in(void); |
||||
int {{ model.name }}_cost_y_e_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void); |
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_e_hess_n_in(void); |
||||
int {{ model.name }}_cost_y_e_hess_n_out(void); |
||||
{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} |
||||
int {{ model.name }}_conl_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_conl_cost_e_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_conl_cost_e_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_conl_cost_e_fun_sparsity_out(int); |
||||
int {{ model.name }}_conl_cost_e_fun_n_in(void); |
||||
int {{ model.name }}_conl_cost_e_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_conl_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_conl_cost_e_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_conl_cost_e_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_conl_cost_e_fun_jac_hess_n_out(void); |
||||
{% elif cost.cost_type_e == "EXTERNAL" %} |
||||
{%- if cost.cost_ext_fun_type_e == "casadi" %} |
||||
int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void); |
||||
{%- else %} |
||||
int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *); |
||||
{%- endif %} |
||||
{% endif %} |
||||
|
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_COST
|
||||
@ -1,69 +0,0 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* |
||||
* 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 {{ model.name }}_Y_0_COST |
||||
#define {{ model.name }}_Y_0_COST |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
{% if cost.cost_type_0 == "NONLINEAR_LS" %} |
||||
int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_0_fun_n_in(void); |
||||
int {{ model.name }}_cost_y_0_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void); |
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_0_hess_n_in(void); |
||||
int {{ model.name }}_cost_y_0_hess_n_out(void); |
||||
{% endif %} |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_Y_0_COST
|
||||
@ -1,69 +0,0 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* |
||||
* 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 {{ model.name }}_Y_E_COST |
||||
#define {{ model.name }}_Y_E_COST |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %} |
||||
int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_e_fun_n_in(void); |
||||
int {{ model.name }}_cost_y_e_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void); |
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_e_hess_n_in(void); |
||||
int {{ model.name }}_cost_y_e_hess_n_out(void); |
||||
{% endif %} |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_Y_E_COST
|
||||
@ -1,69 +0,0 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* |
||||
* 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 {{ model.name }}_Y_COST |
||||
#define {{ model.name }}_Y_COST |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %} |
||||
int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_fun_n_in(void); |
||||
int {{ model.name }}_cost_y_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void); |
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_y_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_y_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_y_hess_n_in(void); |
||||
int {{ model.name }}_cost_y_hess_n_out(void); |
||||
{% endif %} |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_Y_COST
|
||||
@ -1,74 +0,0 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* |
||||
* 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 {{ model.name }}_EXT_COST |
||||
#define {{ model.name }}_EXT_COST |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
{% if cost.cost_ext_fun_type == "casadi" %} |
||||
{% if cost.cost_type == "EXTERNAL" %} |
||||
int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_fun_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void); |
||||
{% endif %} |
||||
|
||||
{% else %} |
||||
int {{ cost.cost_function_ext_cost }}(void **, void **, void *); |
||||
{% endif %} |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_EXT_COST
|
||||
@ -1,75 +0,0 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* |
||||
* 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 {{ model.name }}_EXT_COST_0 |
||||
#define {{ model.name }}_EXT_COST_0 |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
{% if cost.cost_ext_fun_type_0 == "casadi" %} |
||||
|
||||
{% if cost.cost_type_0 == "EXTERNAL" %} |
||||
int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void); |
||||
{% endif %} |
||||
|
||||
{% else %} |
||||
int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *); |
||||
{% endif %} |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_EXT_COST_0
|
||||
@ -1,74 +0,0 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* |
||||
* 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 {{ model.name }}_EXT_COST_E |
||||
#define {{ model.name }}_EXT_COST_E |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
{% if cost.cost_ext_fun_type_e == "casadi" %} |
||||
{% if cost.cost_type_e == "EXTERNAL" %} |
||||
int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void); |
||||
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); |
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void); |
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void); |
||||
{% endif %} |
||||
|
||||
{% else %} |
||||
int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *); |
||||
{% endif %} |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_EXT_COST_E
|
||||
@ -1,70 +0,0 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* |
||||
* 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 {{ model.name }}_H_CONSTRAINT |
||||
#define {{ model.name }}_H_CONSTRAINT |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
{% if dims.nh > 0 %} |
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int); |
||||
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int); |
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void); |
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void); |
||||
|
||||
int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_constr_h_fun_sparsity_in(int); |
||||
const int *{{ model.name }}_constr_h_fun_sparsity_out(int); |
||||
int {{ model.name }}_constr_h_fun_n_in(void); |
||||
int {{ model.name }}_constr_h_fun_n_out(void); |
||||
|
||||
{% if solver_options.hessian_approx == "EXACT" -%} |
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int); |
||||
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int); |
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void); |
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void); |
||||
{% endif %} |
||||
{% endif %} |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_H_CONSTRAINT
|
||||
@ -1,8 +1,5 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* Copyright (c) The acados authors. |
||||
* |
||||
* This file is part of acados. |
||||
* |
||||
@ -1,8 +1,5 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* Copyright (c) The acados authors. |
||||
* |
||||
* This file is part of acados. |
||||
* |
||||
@ -1,8 +1,5 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* Copyright (c) The acados authors. |
||||
* |
||||
* This file is part of acados. |
||||
* |
||||
@ -1,8 +1,5 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* Copyright (c) The acados authors. |
||||
* |
||||
* This file is part of acados. |
||||
* |
||||
@ -1,8 +1,5 @@ |
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
* Copyright (c) The acados authors. |
||||
* |
||||
* This file is part of acados. |
||||
* |
||||
@ -1,21 +0,0 @@ |
||||
#ifndef {{ model.name }}_PHI_E_CONSTRAINT |
||||
#define {{ model.name }}_PHI_E_CONSTRAINT |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
{% if dims.nphi_e > 0 %} |
||||
int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); |
||||
int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *); |
||||
const int *{{ model.name }}_phi_e_constraint_sparsity_in(int); |
||||
const int *{{ model.name }}_phi_e_constraint_sparsity_out(int); |
||||
int {{ model.name }}_phi_e_constraint_n_in(void); |
||||
int {{ model.name }}_phi_e_constraint_n_out(void); |
||||
{% endif %} |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // {{ model.name }}_PHI_E_CONSTRAINT
|
||||
@ -0,0 +1,708 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
import os |
||||
import casadi as ca |
||||
from .utils import is_empty, casadi_length |
||||
|
||||
|
||||
def get_casadi_symbol(x): |
||||
if isinstance(x, ca.MX): |
||||
return ca.MX.sym |
||||
elif isinstance(x, ca.SX): |
||||
return ca.SX.sym |
||||
else: |
||||
raise TypeError("Expected casadi SX or MX.") |
||||
|
||||
################ |
||||
# Dynamics |
||||
################ |
||||
|
||||
|
||||
def generate_c_code_discrete_dynamics( model, opts ): |
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
# load model |
||||
x = model.x |
||||
u = model.u |
||||
p = model.p |
||||
phi = model.disc_dyn_expr |
||||
model_name = model.name |
||||
nx = casadi_length(x) |
||||
|
||||
symbol = get_casadi_symbol(x) |
||||
# assume nx1 = nx !!! |
||||
lam = symbol('lam', nx, 1) |
||||
|
||||
# generate jacobians |
||||
ux = ca.vertcat(u,x) |
||||
jac_ux = ca.jacobian(phi, ux) |
||||
# generate adjoint |
||||
adj_ux = ca.jtimes(phi, ux, lam, True) |
||||
# generate hessian |
||||
hess_ux = ca.jacobian(adj_ux, ux) |
||||
|
||||
# change directory |
||||
cwd = os.getcwd() |
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model')) |
||||
if not os.path.exists(model_dir): |
||||
os.makedirs(model_dir) |
||||
os.chdir(model_dir) |
||||
|
||||
# set up & generate ca.Functions |
||||
fun_name = model_name + '_dyn_disc_phi_fun' |
||||
phi_fun = ca.Function(fun_name, [x, u, p], [phi]) |
||||
phi_fun.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_dyn_disc_phi_fun_jac' |
||||
phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T]) |
||||
phi_fun_jac_ut_xt.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_dyn_disc_phi_fun_jac_hess' |
||||
phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux]) |
||||
phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
os.chdir(cwd) |
||||
return |
||||
|
||||
|
||||
|
||||
def generate_c_code_explicit_ode( model, opts ): |
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
generate_hess = opts["generate_hess"] |
||||
|
||||
# load model |
||||
x = model.x |
||||
u = model.u |
||||
p = model.p |
||||
f_expl = model.f_expl_expr |
||||
model_name = model.name |
||||
|
||||
## get model dimensions |
||||
nx = x.size()[0] |
||||
nu = u.size()[0] |
||||
|
||||
symbol = get_casadi_symbol(x) |
||||
|
||||
## set up functions to be exported |
||||
Sx = symbol('Sx', nx, nx) |
||||
Sp = symbol('Sp', nx, nu) |
||||
lambdaX = symbol('lambdaX', nx, 1) |
||||
|
||||
fun_name = model_name + '_expl_ode_fun' |
||||
|
||||
## Set up functions |
||||
expl_ode_fun = ca.Function(fun_name, [x, u, p], [f_expl]) |
||||
|
||||
vdeX = ca.jtimes(f_expl,x,Sx) |
||||
vdeP = ca.jacobian(f_expl,u) + ca.jtimes(f_expl,x,Sp) |
||||
|
||||
fun_name = model_name + '_expl_vde_forw' |
||||
|
||||
expl_vde_forw = ca.Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP]) |
||||
|
||||
adj = ca.jtimes(f_expl, ca.vertcat(x, u), lambdaX, True) |
||||
|
||||
fun_name = model_name + '_expl_vde_adj' |
||||
expl_vde_adj = ca.Function(fun_name, [x, lambdaX, u, p], [adj]) |
||||
|
||||
if generate_hess: |
||||
S_forw = ca.vertcat(ca.horzcat(Sx, Sp), ca.horzcat(ca.DM.zeros(nu,nx), ca.DM.eye(nu))) |
||||
hess = ca.mtimes(ca.transpose(S_forw),ca.jtimes(adj, ca.vertcat(x,u), S_forw)) |
||||
hess2 = [] |
||||
for j in range(nx+nu): |
||||
for i in range(j,nx+nu): |
||||
hess2 = ca.vertcat(hess2, hess[i,j]) |
||||
|
||||
fun_name = model_name + '_expl_ode_hess' |
||||
expl_ode_hess = ca.Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2]) |
||||
|
||||
# change directory |
||||
cwd = os.getcwd() |
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model')) |
||||
if not os.path.exists(model_dir): |
||||
os.makedirs(model_dir) |
||||
os.chdir(model_dir) |
||||
|
||||
# generate C code |
||||
fun_name = model_name + '_expl_ode_fun' |
||||
expl_ode_fun.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_expl_vde_forw' |
||||
expl_vde_forw.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_expl_vde_adj' |
||||
expl_vde_adj.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
if generate_hess: |
||||
fun_name = model_name + '_expl_ode_hess' |
||||
expl_ode_hess.generate(fun_name, casadi_codegen_opts) |
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
|
||||
|
||||
def generate_c_code_implicit_ode( model, opts ): |
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
# load model |
||||
x = model.x |
||||
xdot = model.xdot |
||||
u = model.u |
||||
z = model.z |
||||
p = model.p |
||||
f_impl = model.f_impl_expr |
||||
model_name = model.name |
||||
|
||||
# get model dimensions |
||||
nx = casadi_length(x) |
||||
nz = casadi_length(z) |
||||
|
||||
# generate jacobians |
||||
jac_x = ca.jacobian(f_impl, x) |
||||
jac_xdot = ca.jacobian(f_impl, xdot) |
||||
jac_u = ca.jacobian(f_impl, u) |
||||
jac_z = ca.jacobian(f_impl, z) |
||||
|
||||
# Set up functions |
||||
p = model.p |
||||
fun_name = model_name + '_impl_dae_fun' |
||||
impl_dae_fun = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl]) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' |
||||
impl_dae_fun_jac_x_xdot_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' |
||||
impl_dae_fun_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z]) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' |
||||
impl_dae_fun_jac_x_xdot_u = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u]) |
||||
|
||||
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' |
||||
impl_dae_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) |
||||
|
||||
if opts["generate_hess"]: |
||||
x_xdot_z_u = ca.vertcat(x, xdot, z, u) |
||||
symbol = get_casadi_symbol(x) |
||||
multiplier = symbol('multiplier', nx + nz) |
||||
ADJ = ca.jtimes(f_impl, x_xdot_z_u, multiplier, True) |
||||
HESS = ca.jacobian(ADJ, x_xdot_z_u) |
||||
fun_name = model_name + '_impl_dae_hess' |
||||
impl_dae_hess = ca.Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS]) |
||||
|
||||
# change directory |
||||
cwd = os.getcwd() |
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model')) |
||||
if not os.path.exists(model_dir): |
||||
os.makedirs(model_dir) |
||||
os.chdir(model_dir) |
||||
|
||||
# generate C code |
||||
fun_name = model_name + '_impl_dae_fun' |
||||
impl_dae_fun.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' |
||||
impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' |
||||
impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' |
||||
impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' |
||||
impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
if opts["generate_hess"]: |
||||
fun_name = model_name + '_impl_dae_hess' |
||||
impl_dae_hess.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
os.chdir(cwd) |
||||
return |
||||
|
||||
|
||||
def generate_c_code_gnsf( model, opts ): |
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
model_name = model.name |
||||
|
||||
# set up directory |
||||
cwd = os.getcwd() |
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model')) |
||||
if not os.path.exists(model_dir): |
||||
os.makedirs(model_dir) |
||||
os.chdir(model_dir) |
||||
|
||||
# obtain gnsf dimensions |
||||
get_matrices_fun = model.get_matrices_fun |
||||
phi_fun = model.phi_fun |
||||
|
||||
size_gnsf_A = get_matrices_fun.size_out(0) |
||||
gnsf_nx1 = size_gnsf_A[1] |
||||
gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] |
||||
gnsf_nuhat = max(phi_fun.size_in(1)) |
||||
gnsf_ny = max(phi_fun.size_in(0)) |
||||
gnsf_nout = max(phi_fun.size_out(0)) |
||||
|
||||
# set up expressions |
||||
# if the model uses ca.MX because of cost/constraints |
||||
# the DAE can be exported as ca.SX -> detect GNSF in Matlab |
||||
# -> evaluated ca.SX GNSF functions with ca.MX. |
||||
u = model.u |
||||
symbol = get_casadi_symbol(u) |
||||
|
||||
y = symbol("y", gnsf_ny, 1) |
||||
uhat = symbol("uhat", gnsf_nuhat, 1) |
||||
p = model.p |
||||
x1 = symbol("gnsf_x1", gnsf_nx1, 1) |
||||
x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) |
||||
z1 = symbol("gnsf_z1", gnsf_nz1, 1) |
||||
dummy = symbol("gnsf_dummy", 1, 1) |
||||
empty_var = symbol("gnsf_empty_var", 0, 0) |
||||
|
||||
## generate C code |
||||
fun_name = model_name + '_gnsf_phi_fun' |
||||
phi_fun_ = ca.Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)]) |
||||
phi_fun_.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_gnsf_phi_fun_jac_y' |
||||
phi_fun_jac_y = model.phi_fun_jac_y |
||||
phi_fun_jac_y_ = ca.Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p)) |
||||
phi_fun_jac_y_.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_gnsf_phi_jac_y_uhat' |
||||
phi_jac_y_uhat = model.phi_jac_y_uhat |
||||
phi_jac_y_uhat_ = ca.Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p)) |
||||
phi_jac_y_uhat_.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz' |
||||
f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz |
||||
f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p) |
||||
|
||||
# avoid codegeneration issue |
||||
if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval): |
||||
f_lo_fun_jac_x1k1uz_eval = [empty_var] |
||||
|
||||
f_lo_fun_jac_x1k1uz_ = ca.Function(fun_name, [x1, x1dot, z1, u, p], |
||||
f_lo_fun_jac_x1k1uz_eval) |
||||
f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
fun_name = model_name + '_gnsf_get_matrices_fun' |
||||
get_matrices_fun_ = ca.Function(fun_name, [dummy], get_matrices_fun(1)) |
||||
get_matrices_fun_.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
# remove fields for json dump |
||||
del model.phi_fun |
||||
del model.phi_fun_jac_y |
||||
del model.phi_jac_y_uhat |
||||
del model.f_lo_fun_jac_x1k1uz |
||||
del model.get_matrices_fun |
||||
|
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
|
||||
|
||||
################ |
||||
# Cost |
||||
################ |
||||
|
||||
def generate_c_code_external_cost(model, stage_type, opts): |
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
x = model.x |
||||
p = model.p |
||||
u = model.u |
||||
z = model.z |
||||
symbol = get_casadi_symbol(x) |
||||
|
||||
if stage_type == 'terminal': |
||||
suffix_name = "_cost_ext_cost_e_fun" |
||||
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" |
||||
suffix_name_jac = "_cost_ext_cost_e_fun_jac" |
||||
ext_cost = model.cost_expr_ext_cost_e |
||||
custom_hess = model.cost_expr_ext_cost_custom_hess_e |
||||
# Last stage cannot depend on u and z |
||||
u = symbol("u", 0, 0) |
||||
z = symbol("z", 0, 0) |
||||
|
||||
elif stage_type == 'path': |
||||
suffix_name = "_cost_ext_cost_fun" |
||||
suffix_name_hess = "_cost_ext_cost_fun_jac_hess" |
||||
suffix_name_jac = "_cost_ext_cost_fun_jac" |
||||
ext_cost = model.cost_expr_ext_cost |
||||
custom_hess = model.cost_expr_ext_cost_custom_hess |
||||
|
||||
elif stage_type == 'initial': |
||||
suffix_name = "_cost_ext_cost_0_fun" |
||||
suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" |
||||
suffix_name_jac = "_cost_ext_cost_0_fun_jac" |
||||
ext_cost = model.cost_expr_ext_cost_0 |
||||
custom_hess = model.cost_expr_ext_cost_custom_hess_0 |
||||
|
||||
nunx = x.shape[0] + u.shape[0] |
||||
|
||||
# set up functions to be exported |
||||
fun_name = model.name + suffix_name |
||||
fun_name_hess = model.name + suffix_name_hess |
||||
fun_name_jac = model.name + suffix_name_jac |
||||
|
||||
# generate expression for full gradient and Hessian |
||||
hess_uxz, grad_uxz = ca.hessian(ext_cost, ca.vertcat(u, x, z)) |
||||
|
||||
hess_ux = hess_uxz[:nunx, :nunx] |
||||
hess_z = hess_uxz[nunx:, nunx:] |
||||
hess_z_ux = hess_uxz[nunx:, :nunx] |
||||
|
||||
if custom_hess is not None: |
||||
hess_ux = custom_hess |
||||
|
||||
ext_cost_fun = ca.Function(fun_name, [x, u, z, p], [ext_cost]) |
||||
|
||||
ext_cost_fun_jac_hess = ca.Function( |
||||
fun_name_hess, [x, u, z, p], [ext_cost, grad_uxz, hess_ux, hess_z, hess_z_ux] |
||||
) |
||||
ext_cost_fun_jac = ca.Function( |
||||
fun_name_jac, [x, u, z, p], [ext_cost, grad_uxz] |
||||
) |
||||
|
||||
# change directory |
||||
cwd = os.getcwd() |
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) |
||||
if not os.path.exists(cost_dir): |
||||
os.makedirs(cost_dir) |
||||
os.chdir(cost_dir) |
||||
|
||||
ext_cost_fun.generate(fun_name, casadi_codegen_opts) |
||||
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_codegen_opts) |
||||
ext_cost_fun_jac.generate(fun_name_jac, casadi_codegen_opts) |
||||
|
||||
os.chdir(cwd) |
||||
return |
||||
|
||||
|
||||
def generate_c_code_nls_cost( model, cost_name, stage_type, opts ): |
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
x = model.x |
||||
z = model.z |
||||
p = model.p |
||||
u = model.u |
||||
|
||||
symbol = get_casadi_symbol(x) |
||||
|
||||
if stage_type == 'terminal': |
||||
middle_name = '_cost_y_e' |
||||
u = symbol('u', 0, 0) |
||||
y_expr = model.cost_y_expr_e |
||||
|
||||
elif stage_type == 'initial': |
||||
middle_name = '_cost_y_0' |
||||
y_expr = model.cost_y_expr_0 |
||||
|
||||
elif stage_type == 'path': |
||||
middle_name = '_cost_y' |
||||
y_expr = model.cost_y_expr |
||||
|
||||
# change directory |
||||
cwd = os.getcwd() |
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) |
||||
if not os.path.exists(cost_dir): |
||||
os.makedirs(cost_dir) |
||||
os.chdir(cost_dir) |
||||
|
||||
# set up expressions |
||||
cost_jac_expr = ca.transpose(ca.jacobian(y_expr, ca.vertcat(u, x))) |
||||
dy_dz = ca.jacobian(y_expr, z) |
||||
ny = casadi_length(y_expr) |
||||
|
||||
y = symbol('y', ny, 1) |
||||
|
||||
y_adj = ca.jtimes(y_expr, ca.vertcat(u, x), y, True) |
||||
y_hess = ca.jacobian(y_adj, ca.vertcat(u, x)) |
||||
|
||||
## generate C code |
||||
suffix_name = '_fun' |
||||
fun_name = cost_name + middle_name + suffix_name |
||||
y_fun = ca.Function( fun_name, [x, u, z, p], [ y_expr ]) |
||||
y_fun.generate( fun_name, casadi_codegen_opts ) |
||||
|
||||
suffix_name = '_fun_jac_ut_xt' |
||||
fun_name = cost_name + middle_name + suffix_name |
||||
y_fun_jac_ut_xt = ca.Function(fun_name, [x, u, z, p], [ y_expr, cost_jac_expr, dy_dz ]) |
||||
y_fun_jac_ut_xt.generate( fun_name, casadi_codegen_opts ) |
||||
|
||||
suffix_name = '_hess' |
||||
fun_name = cost_name + middle_name + suffix_name |
||||
y_hess = ca.Function(fun_name, [x, u, z, y, p], [ y_hess ]) |
||||
y_hess.generate( fun_name, casadi_codegen_opts ) |
||||
|
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
|
||||
|
||||
|
||||
def generate_c_code_conl_cost(model, cost_name, stage_type, opts): |
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
x = model.x |
||||
z = model.z |
||||
p = model.p |
||||
|
||||
symbol = get_casadi_symbol(x) |
||||
|
||||
if stage_type == 'terminal': |
||||
u = symbol('u', 0, 0) |
||||
|
||||
yref = model.cost_r_in_psi_expr_e |
||||
inner_expr = model.cost_y_expr_e - yref |
||||
outer_expr = model.cost_psi_expr_e |
||||
res_expr = model.cost_r_in_psi_expr_e |
||||
|
||||
suffix_name_fun = '_conl_cost_e_fun' |
||||
suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess' |
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess_e |
||||
|
||||
elif stage_type == 'initial': |
||||
u = model.u |
||||
|
||||
yref = model.cost_r_in_psi_expr_0 |
||||
inner_expr = model.cost_y_expr_0 - yref |
||||
outer_expr = model.cost_psi_expr_0 |
||||
res_expr = model.cost_r_in_psi_expr_0 |
||||
|
||||
suffix_name_fun = '_conl_cost_0_fun' |
||||
suffix_name_fun_jac_hess = '_conl_cost_0_fun_jac_hess' |
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess_0 |
||||
|
||||
elif stage_type == 'path': |
||||
u = model.u |
||||
|
||||
yref = model.cost_r_in_psi_expr |
||||
inner_expr = model.cost_y_expr - yref |
||||
outer_expr = model.cost_psi_expr |
||||
res_expr = model.cost_r_in_psi_expr |
||||
|
||||
suffix_name_fun = '_conl_cost_fun' |
||||
suffix_name_fun_jac_hess = '_conl_cost_fun_jac_hess' |
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess |
||||
|
||||
# set up function names |
||||
fun_name_cost_fun = model.name + suffix_name_fun |
||||
fun_name_cost_fun_jac_hess = model.name + suffix_name_fun_jac_hess |
||||
|
||||
# set up functions to be exported |
||||
outer_loss_fun = ca.Function('psi', [res_expr, p], [outer_expr]) |
||||
cost_expr = outer_loss_fun(inner_expr, p) |
||||
|
||||
outer_loss_grad_fun = ca.Function('outer_loss_grad', [res_expr, p], [ca.jacobian(outer_expr, res_expr).T]) |
||||
|
||||
if custom_hess is None: |
||||
outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [ca.hessian(outer_loss_fun(res_expr, p), res_expr)[0]]) |
||||
else: |
||||
outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [custom_hess]) |
||||
|
||||
Jt_ux_expr = ca.jacobian(inner_expr, ca.vertcat(u, x)).T |
||||
Jt_z_expr = ca.jacobian(inner_expr, z).T |
||||
|
||||
cost_fun = ca.Function( |
||||
fun_name_cost_fun, |
||||
[x, u, z, yref, p], |
||||
[cost_expr]) |
||||
|
||||
cost_fun_jac_hess = ca.Function( |
||||
fun_name_cost_fun_jac_hess, |
||||
[x, u, z, yref, p], |
||||
[cost_expr, outer_loss_grad_fun(inner_expr, p), Jt_ux_expr, Jt_z_expr, outer_hess_fun(inner_expr, p)] |
||||
) |
||||
# change directory |
||||
cwd = os.getcwd() |
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost')) |
||||
if not os.path.exists(cost_dir): |
||||
os.makedirs(cost_dir) |
||||
os.chdir(cost_dir) |
||||
|
||||
# generate C code |
||||
cost_fun.generate(fun_name_cost_fun, casadi_codegen_opts) |
||||
cost_fun_jac_hess.generate(fun_name_cost_fun_jac_hess, casadi_codegen_opts) |
||||
|
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
|
||||
|
||||
################ |
||||
# Constraints |
||||
################ |
||||
def generate_c_code_constraint( model, con_name, is_terminal, opts ): |
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
# load constraint variables and expression |
||||
x = model.x |
||||
p = model.p |
||||
|
||||
symbol = get_casadi_symbol(x) |
||||
|
||||
if is_terminal: |
||||
con_h_expr = model.con_h_expr_e |
||||
con_phi_expr = model.con_phi_expr_e |
||||
# create dummy u, z |
||||
u = symbol('u', 0, 0) |
||||
z = symbol('z', 0, 0) |
||||
else: |
||||
con_h_expr = model.con_h_expr |
||||
con_phi_expr = model.con_phi_expr |
||||
u = model.u |
||||
z = model.z |
||||
|
||||
if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)): |
||||
raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.") |
||||
|
||||
if (is_empty(con_h_expr) and is_empty(con_phi_expr)): |
||||
# both empty -> nothing to generate |
||||
return |
||||
|
||||
if is_empty(con_h_expr): |
||||
constr_type = 'BGP' |
||||
else: |
||||
constr_type = 'BGH' |
||||
|
||||
if is_empty(p): |
||||
p = symbol('p', 0, 0) |
||||
|
||||
if is_empty(z): |
||||
z = symbol('z', 0, 0) |
||||
|
||||
if not (is_empty(con_h_expr)) and opts['generate_hess']: |
||||
# multipliers for hessian |
||||
nh = casadi_length(con_h_expr) |
||||
lam_h = symbol('lam_h', nh, 1) |
||||
|
||||
# set up & change directory |
||||
cwd = os.getcwd() |
||||
constraints_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_constraints')) |
||||
if not os.path.exists(constraints_dir): |
||||
os.makedirs(constraints_dir) |
||||
os.chdir(constraints_dir) |
||||
|
||||
# export casadi functions |
||||
if constr_type == 'BGH': |
||||
if is_terminal: |
||||
fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt' |
||||
else: |
||||
fun_name = con_name + '_constr_h_fun_jac_uxt_zt' |
||||
|
||||
jac_ux_t = ca.transpose(ca.jacobian(con_h_expr, ca.vertcat(u,x))) |
||||
jac_z_t = ca.jacobian(con_h_expr, z) |
||||
constraint_fun_jac_tran = ca.Function(fun_name, [x, u, z, p], \ |
||||
[con_h_expr, jac_ux_t, jac_z_t]) |
||||
|
||||
constraint_fun_jac_tran.generate(fun_name, casadi_codegen_opts) |
||||
if opts['generate_hess']: |
||||
|
||||
if is_terminal: |
||||
fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess' |
||||
else: |
||||
fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess' |
||||
|
||||
# adjoint |
||||
adj_ux = ca.jtimes(con_h_expr, ca.vertcat(u, x), lam_h, True) |
||||
# hessian |
||||
hess_ux = ca.jacobian(adj_ux, ca.vertcat(u, x)) |
||||
|
||||
adj_z = ca.jtimes(con_h_expr, z, lam_h, True) |
||||
hess_z = ca.jacobian(adj_z, z) |
||||
|
||||
# set up functions |
||||
constraint_fun_jac_tran_hess = \ |
||||
ca.Function(fun_name, [x, u, lam_h, z, p], \ |
||||
[con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z]) |
||||
|
||||
# generate C code |
||||
constraint_fun_jac_tran_hess.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
if is_terminal: |
||||
fun_name = con_name + '_constr_h_e_fun' |
||||
else: |
||||
fun_name = con_name + '_constr_h_fun' |
||||
h_fun = ca.Function(fun_name, [x, u, z, p], [con_h_expr]) |
||||
h_fun.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
else: # BGP constraint |
||||
if is_terminal: |
||||
fun_name = con_name + '_phi_e_constraint' |
||||
r = model.con_r_in_phi_e |
||||
con_r_expr = model.con_r_expr_e |
||||
else: |
||||
fun_name = con_name + '_phi_constraint' |
||||
r = model.con_r_in_phi |
||||
con_r_expr = model.con_r_expr |
||||
|
||||
nphi = casadi_length(con_phi_expr) |
||||
con_phi_expr_x_u_z = ca.substitute(con_phi_expr, r, con_r_expr) |
||||
phi_jac_u = ca.jacobian(con_phi_expr_x_u_z, u) |
||||
phi_jac_x = ca.jacobian(con_phi_expr_x_u_z, x) |
||||
phi_jac_z = ca.jacobian(con_phi_expr_x_u_z, z) |
||||
|
||||
hess = ca.hessian(con_phi_expr[0], r)[0] |
||||
for i in range(1, nphi): |
||||
hess = ca.vertcat(hess, ca.hessian(con_phi_expr[i], r)[0]) |
||||
|
||||
r_jac_u = ca.jacobian(con_r_expr, u) |
||||
r_jac_x = ca.jacobian(con_r_expr, x) |
||||
|
||||
constraint_phi = \ |
||||
ca.Function(fun_name, [x, u, z, p], \ |
||||
[con_phi_expr_x_u_z, \ |
||||
ca.vertcat(ca.transpose(phi_jac_u), ca.transpose(phi_jac_x)), \ |
||||
ca.transpose(phi_jac_z), \ |
||||
hess, |
||||
ca.vertcat(ca.transpose(r_jac_u), ca.transpose(r_jac_x))]) |
||||
|
||||
constraint_phi.generate(fun_name, casadi_codegen_opts) |
||||
|
||||
# change directory back |
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
|
||||
@ -0,0 +1,819 @@ |
||||
/*
|
||||
* 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.; |
||||
*/ |
||||
|
||||
// This is a template based custom_update function
|
||||
#include <stdlib.h> |
||||
#include <stdio.h> |
||||
#include <assert.h> |
||||
#include <math.h> |
||||
|
||||
#include "custom_update_function.h" |
||||
#include "acados_solver_{{ model.name }}.h" |
||||
#include "acados_c/ocp_nlp_interface.h" |
||||
#include "acados/utils/mem.h" |
||||
|
||||
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" |
||||
#include "blasfeo/include/blasfeo_d_blasfeo_api.h" |
||||
|
||||
|
||||
typedef struct custom_memory |
||||
{ |
||||
// covariance matrics
|
||||
struct blasfeo_dmat *uncertainty_matrix_buffer; // shape = (N+1, nx, nx)
|
||||
// covariance matrix of the additive disturbance
|
||||
struct blasfeo_dmat W_mat; // shape = (nw, nw)
|
||||
struct blasfeo_dmat unc_jac_G_mat; // shape = (nx, nw)
|
||||
struct blasfeo_dmat temp_GW_mat; // shape = (nx, nw)
|
||||
struct blasfeo_dmat GWG_mat; // shape = (nx, nx)
|
||||
// sensitivity matrices
|
||||
struct blasfeo_dmat A_mat; // shape = (nx, nx)
|
||||
struct blasfeo_dmat B_mat; // shape = (nx, nu)
|
||||
// matrix in linear constraints
|
||||
struct blasfeo_dmat Cg_mat; // shape = (ng, nx)
|
||||
struct blasfeo_dmat Dg_mat; // shape = (ng, nu)
|
||||
struct blasfeo_dmat Cg_e_mat; // shape = (ng_e, nx)
|
||||
struct blasfeo_dmat dummy_Dgh_e_mat; // shape = (ngh_e_max, nu)
|
||||
// matrix in nonlinear constraints
|
||||
struct blasfeo_dmat Ch_mat; // shape = (nh, nx)
|
||||
struct blasfeo_dmat Dh_mat; // shape = (nh, nu)
|
||||
struct blasfeo_dmat Ch_e_mat; // shape = (nh_e, nx)
|
||||
// feedback gain matrix
|
||||
struct blasfeo_dmat K_mat; // shape = (nu, nx)
|
||||
// AK = A - B@K
|
||||
struct blasfeo_dmat AK_mat; // shape = (nx, nx)
|
||||
// A@P_k
|
||||
struct blasfeo_dmat temp_AP_mat; // shape = (nx, nx)
|
||||
// K@P_k, K@P_k@K^T
|
||||
struct blasfeo_dmat temp_KP_mat; // shape = (nu, nx)
|
||||
struct blasfeo_dmat temp_KPK_mat; // shape = (nu, nu)
|
||||
// C + D @ K, (C + D @ K) @ P_k
|
||||
struct blasfeo_dmat temp_CaDK_mat; // shape = (ngh_me_max, nx)
|
||||
struct blasfeo_dmat temp_CaDKmP_mat; // shape = (ngh_me_max, nx)
|
||||
struct blasfeo_dmat temp_beta_mat; // shape = (ngh_me_max, ngh_me_max)
|
||||
|
||||
double *d_A_mat; // shape = (nx, nx)
|
||||
double *d_B_mat; // shape = (nx, nu)
|
||||
double *d_Cg_mat; // shape = (ng, nx)
|
||||
double *d_Dg_mat; // shape = (ng, nu)
|
||||
double *d_Cg_e_mat; // shape = (ng_e, nx)
|
||||
double *d_Cgh_mat; // shape = (ng+nh, nx)
|
||||
double *d_Dgh_mat; // shape = (ng+nh, nu)
|
||||
double *d_Cgh_e_mat; // shape = (ng_e+nh_e, nx)
|
||||
double *d_state_vec; |
||||
// upper and lower bounds on state variables
|
||||
double *d_lbx; // shape = (nbx,)
|
||||
double *d_ubx; // shape = (nbx,)
|
||||
double *d_lbx_e; // shape = (nbx_e,)
|
||||
double *d_ubx_e; // shape = (nbx_e,)
|
||||
// tightened upper and lower bounds on state variables
|
||||
double *d_lbx_tightened; // shape = (nbx,)
|
||||
double *d_ubx_tightened; // shape = (nbx,)
|
||||
double *d_lbx_e_tightened; // shape = (nbx_e,)
|
||||
double *d_ubx_e_tightened; // shape = (nbx_e,)
|
||||
// upper and lower bounds on control inputs
|
||||
double *d_lbu; // shape = (nbu,)
|
||||
double *d_ubu; // shape = (nbu,)
|
||||
// tightened upper and lower bounds on control inputs
|
||||
double *d_lbu_tightened; // shape = (nbu,)
|
||||
double *d_ubu_tightened; // shape = (nbu,)
|
||||
// upper and lower bounds on polytopic constraints
|
||||
double *d_lg; // shape = (ng,)
|
||||
double *d_ug; // shape = (ng,)
|
||||
double *d_lg_e; // shape = (ng_e,)
|
||||
double *d_ug_e; // shape = (ng_e,)
|
||||
// tightened lower bounds on polytopic constraints
|
||||
double *d_lg_tightened; // shape = (ng,)
|
||||
double *d_ug_tightened; // shape = (ng,)
|
||||
double *d_lg_e_tightened; // shape = (ng_e,)
|
||||
double *d_ug_e_tightened; // shape = (ng_e,)
|
||||
// upper and lower bounds on nonlinear constraints
|
||||
double *d_lh; // shape = (nh,)
|
||||
double *d_uh; // shape = (nh,)
|
||||
double *d_lh_e; // shape = (nh_e,)
|
||||
double *d_uh_e; // shape = (nh_e,)
|
||||
// tightened upper and lower bounds on nonlinear constraints
|
||||
double *d_lh_tightened; // shape = (nh,)
|
||||
double *d_uh_tightened; // shape = (nh,)
|
||||
double *d_lh_e_tightened; // shape = (nh_e,)
|
||||
double *d_uh_e_tightened; // shape = (nh_e,)
|
||||
|
||||
int *idxbx; // shape = (nbx,)
|
||||
int *idxbu; // shape = (nbu,)
|
||||
int *idxbx_e; // shape = (nbx_e,)
|
||||
|
||||
void *raw_memory; // Pointer to allocated memory, to be used for freeing
|
||||
} custom_memory; |
||||
|
||||
static int int_max(int num1, int num2) |
||||
{ |
||||
return (num1 > num2 ) ? num1 : num2; |
||||
} |
||||
|
||||
|
||||
static int custom_memory_calculate_size(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims) |
||||
{ |
||||
int N = nlp_dims->N; |
||||
int nx = {{ dims.nx }}; |
||||
int nu = {{ dims.nu }}; |
||||
int nw = {{ zoro_description.nw }}; |
||||
|
||||
int ng = {{ dims.ng }}; |
||||
int nh = {{ dims.nh }}; |
||||
int nbx = {{ dims.nbx }}; |
||||
int nbu = {{ dims.nbu }}; |
||||
|
||||
int ng_e = {{ dims.ng_e }}; |
||||
int nh_e = {{ dims.nh_e }}; |
||||
int ngh_e_max = int_max(ng_e, nh_e); |
||||
int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh)); |
||||
int nbx_e = {{ dims.nbx_e }}; |
||||
|
||||
assert({{zoro_description.nlbx_t}} <= nbx); |
||||
assert({{zoro_description.nubx_t}} <= nbx); |
||||
assert({{zoro_description.nlbu_t}} <= nbu); |
||||
assert({{zoro_description.nubu_t}} <= nbu); |
||||
assert({{zoro_description.nlg_t}} <= ng); |
||||
assert({{zoro_description.nug_t}} <= ng); |
||||
assert({{zoro_description.nlh_t}} <= nh); |
||||
assert({{zoro_description.nuh_t}} <= nh); |
||||
assert({{zoro_description.nlbx_e_t}} <= nbx_e); |
||||
assert({{zoro_description.nubx_e_t}} <= nbx_e); |
||||
assert({{zoro_description.nlg_e_t}} <= ng_e); |
||||
assert({{zoro_description.nug_e_t}} <= ng_e); |
||||
assert({{zoro_description.nlh_e_t}} <= nh_e); |
||||
assert({{zoro_description.nuh_e_t}} <= nh_e); |
||||
|
||||
acados_size_t size = sizeof(custom_memory); |
||||
size += nbx * sizeof(int); |
||||
/* blasfeo structs */ |
||||
size += (N + 1) * sizeof(struct blasfeo_dmat); |
||||
/* blasfeo mem: mat */ |
||||
size += (N + 1) * blasfeo_memsize_dmat(nx, nx); // uncertainty_matrix_buffer
|
||||
size += blasfeo_memsize_dmat(nw, nw); // W_mat
|
||||
size += 2 * blasfeo_memsize_dmat(nx, nw); // unc_jac_G_mat, temp_GW_mat
|
||||
size += 4 * blasfeo_memsize_dmat(nx, nx); // GWG_mat, A_mat, AK_mat, temp_AP_mat
|
||||
size += blasfeo_memsize_dmat(nx, nu); // B_mat
|
||||
size += 2 * blasfeo_memsize_dmat(nu, nx); // K_mat, temp_KP_mat
|
||||
size += blasfeo_memsize_dmat(nu, nu); // temp_KPK_mat
|
||||
size += blasfeo_memsize_dmat(ng, nx); // Cg_mat
|
||||
size += blasfeo_memsize_dmat(ng, nu); // Dg_mat
|
||||
size += blasfeo_memsize_dmat(ng_e, nx); // Cg_e_mat
|
||||
size += blasfeo_memsize_dmat(ngh_e_max, nu); // dummy_Dgh_e_mat
|
||||
size += blasfeo_memsize_dmat(nh, nx); // Ch_mat
|
||||
size += blasfeo_memsize_dmat(nh, nu); // Dh_mat
|
||||
size += blasfeo_memsize_dmat(nh_e, nx); // Ch_e_mat
|
||||
size += 2 * blasfeo_memsize_dmat(ngh_me_max, nx); // temp_CaDK_mat, temp_CaDKmP_mat
|
||||
size += blasfeo_memsize_dmat(ngh_me_max, ngh_me_max); // temp_beta_mat
|
||||
/* blasfeo mem: vec */ |
||||
/* Arrays */ |
||||
size += nx*nx *sizeof(double); // d_A_mat
|
||||
size += nx*nu *sizeof(double); // d_B_mat
|
||||
size += (ng + ng_e) * nx * sizeof(double); // d_Cg_mat, d_Cg_e_mat
|
||||
size += (ng) * nu * sizeof(double); // d_Dg_mat
|
||||
size += (nh + nh_e + ng + ng_e) * nx * sizeof(double); // d_Cgh_mat, d_Cgh_e_mat
|
||||
size += (nh + ng) * nu * sizeof(double); // d_Dgh_mat
|
||||
// d_state_vec
|
||||
size += nx *sizeof(double); |
||||
// constraints and tightened constraints
|
||||
size += 4 * (nbx + nbu + ng + nh)*sizeof(double); |
||||
size += 4 * (nbx_e + ng_e + nh_e)*sizeof(double); |
||||
size += (nbx + nbu + nbx_e)*sizeof(int); // idxbx, idxbu, idxbx_e
|
||||
|
||||
size += 1 * 8; // initial alignment
|
||||
make_int_multiple_of(64, &size); |
||||
size += 1 * 64; |
||||
|
||||
return size; |
||||
} |
||||
|
||||
|
||||
static custom_memory *custom_memory_assign(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims, void *raw_memory) |
||||
{ |
||||
int N = nlp_dims->N; |
||||
int nx = {{ dims.nx }}; |
||||
int nu = {{ dims.nu }}; |
||||
int nw = {{ zoro_description.nw }}; |
||||
|
||||
int ng = {{ dims.ng }}; |
||||
int nh = {{ dims.nh }}; |
||||
int nbx = {{ dims.nbx }}; |
||||
int nbu = {{ dims.nbu }}; |
||||
|
||||
int ng_e = {{ dims.ng_e }}; |
||||
int nh_e = {{ dims.nh_e }}; |
||||
int ngh_e_max = int_max(ng_e, nh_e); |
||||
int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh)); |
||||
int nbx_e = {{ dims.nbx_e }}; |
||||
|
||||
char *c_ptr = (char *) raw_memory; |
||||
custom_memory *mem = (custom_memory *) c_ptr; |
||||
c_ptr += sizeof(custom_memory); |
||||
|
||||
align_char_to(8, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_structs(N+1, &mem->uncertainty_matrix_buffer, &c_ptr); |
||||
|
||||
align_char_to(64, &c_ptr); |
||||
|
||||
for (int ii = 0; ii <= N; ii++) |
||||
{ |
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->uncertainty_matrix_buffer[ii], &c_ptr); |
||||
} |
||||
// Disturbance Dynamics
|
||||
assign_and_advance_blasfeo_dmat_mem(nw, nw, &mem->W_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->unc_jac_G_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->temp_GW_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->GWG_mat, &c_ptr); |
||||
// System Dynamics
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->A_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nx, nu, &mem->B_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(ng, nx, &mem->Cg_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(ng, nu, &mem->Dg_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(ng_e, nx, &mem->Cg_e_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(ngh_e_max, nu, &mem->dummy_Dgh_e_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nh, nx, &mem->Ch_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nh, nu, &mem->Dh_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nh_e, nx, &mem->Ch_e_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->K_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->AK_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->temp_AP_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->temp_KP_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(nu, nu, &mem->temp_KPK_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDK_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDKmP_mat, &c_ptr); |
||||
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, ngh_me_max, &mem->temp_beta_mat, &c_ptr); |
||||
|
||||
assign_and_advance_double(nx*nx, &mem->d_A_mat, &c_ptr); |
||||
assign_and_advance_double(nx*nu, &mem->d_B_mat, &c_ptr); |
||||
assign_and_advance_double(ng*nx, &mem->d_Cg_mat, &c_ptr); |
||||
assign_and_advance_double(ng*nu, &mem->d_Dg_mat, &c_ptr); |
||||
assign_and_advance_double(ng_e*nx, &mem->d_Cg_e_mat, &c_ptr); |
||||
assign_and_advance_double((ng + nh)*nx, &mem->d_Cgh_mat, &c_ptr); |
||||
assign_and_advance_double((ng + nh)*nu, &mem->d_Dgh_mat, &c_ptr); |
||||
assign_and_advance_double((ng_e + nh_e)*nx, &mem->d_Cgh_e_mat, &c_ptr); |
||||
assign_and_advance_double(nx, &mem->d_state_vec, &c_ptr); |
||||
assign_and_advance_double(nbx, &mem->d_lbx, &c_ptr); |
||||
assign_and_advance_double(nbx, &mem->d_ubx, &c_ptr); |
||||
assign_and_advance_double(nbx_e, &mem->d_lbx_e, &c_ptr); |
||||
assign_and_advance_double(nbx_e, &mem->d_ubx_e, &c_ptr); |
||||
assign_and_advance_double(nbx, &mem->d_lbx_tightened, &c_ptr); |
||||
assign_and_advance_double(nbx, &mem->d_ubx_tightened, &c_ptr); |
||||
assign_and_advance_double(nbx_e, &mem->d_lbx_e_tightened, &c_ptr); |
||||
assign_and_advance_double(nbx_e, &mem->d_ubx_e_tightened, &c_ptr); |
||||
assign_and_advance_double(nbu, &mem->d_lbu, &c_ptr); |
||||
assign_and_advance_double(nbu, &mem->d_ubu, &c_ptr); |
||||
assign_and_advance_double(nbu, &mem->d_lbu_tightened, &c_ptr); |
||||
assign_and_advance_double(nbu, &mem->d_ubu_tightened, &c_ptr); |
||||
assign_and_advance_double(ng, &mem->d_lg, &c_ptr); |
||||
assign_and_advance_double(ng, &mem->d_ug, &c_ptr); |
||||
assign_and_advance_double(ng_e, &mem->d_lg_e, &c_ptr); |
||||
assign_and_advance_double(ng_e, &mem->d_ug_e, &c_ptr); |
||||
assign_and_advance_double(ng, &mem->d_lg_tightened, &c_ptr); |
||||
assign_and_advance_double(ng, &mem->d_ug_tightened, &c_ptr); |
||||
assign_and_advance_double(ng_e, &mem->d_lg_e_tightened, &c_ptr); |
||||
assign_and_advance_double(ng_e, &mem->d_ug_e_tightened, &c_ptr); |
||||
assign_and_advance_double(nh, &mem->d_lh, &c_ptr); |
||||
assign_and_advance_double(nh, &mem->d_uh, &c_ptr); |
||||
assign_and_advance_double(nh_e, &mem->d_lh_e, &c_ptr); |
||||
assign_and_advance_double(nh_e, &mem->d_uh_e, &c_ptr); |
||||
assign_and_advance_double(nh, &mem->d_lh_tightened, &c_ptr); |
||||
assign_and_advance_double(nh, &mem->d_uh_tightened, &c_ptr); |
||||
assign_and_advance_double(nh_e, &mem->d_lh_e_tightened, &c_ptr); |
||||
assign_and_advance_double(nh_e, &mem->d_uh_e_tightened, &c_ptr); |
||||
|
||||
assign_and_advance_int(nbx, &mem->idxbx, &c_ptr); |
||||
assign_and_advance_int(nbu, &mem->idxbu, &c_ptr); |
||||
assign_and_advance_int(nbx_e, &mem->idxbx_e, &c_ptr); |
||||
|
||||
assert((char *) raw_memory + custom_memory_calculate_size(nlp_config, nlp_dims) >= c_ptr); |
||||
mem->raw_memory = raw_memory; |
||||
|
||||
return mem; |
||||
} |
||||
|
||||
|
||||
|
||||
static void *custom_memory_create({{ model.name }}_solver_capsule* capsule) |
||||
{ |
||||
printf("\nin custom_memory_create_function\n"); |
||||
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule); |
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule); |
||||
acados_size_t bytes = custom_memory_calculate_size(nlp_config, nlp_dims); |
||||
|
||||
void *ptr = acados_calloc(1, bytes); |
||||
|
||||
custom_memory *custom_mem = custom_memory_assign(nlp_config, nlp_dims, ptr); |
||||
custom_mem->raw_memory = ptr; |
||||
|
||||
return custom_mem; |
||||
} |
||||
|
||||
|
||||
static void custom_val_init_function(ocp_nlp_dims *nlp_dims, ocp_nlp_in *nlp_in, ocp_nlp_solver *nlp_solver, custom_memory *custom_mem) |
||||
{ |
||||
int N = nlp_dims->N; |
||||
int nx = {{ dims.nx }}; |
||||
int nu = {{ dims.nu }}; |
||||
int nw = {{ zoro_description.nw }}; |
||||
|
||||
int ng = {{ dims.ng }}; |
||||
int nh = {{ dims.nh }}; |
||||
int nbx = {{ dims.nbx }}; |
||||
int nbu = {{ dims.nbu }}; |
||||
|
||||
int ng_e = {{ dims.ng_e }}; |
||||
int nh_e = {{ dims.nh_e }}; |
||||
int ngh_e_max = int_max(ng_e, nh_e); |
||||
int nbx_e = {{ dims.nbx_e }}; |
||||
|
||||
/* Get the state constraint bounds */ |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbx", custom_mem->idxbx); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "idxbx", custom_mem->idxbx_e); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbu", custom_mem->idxbu); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu); |
||||
// Get the Jacobians and the bounds of the linear constraints
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "C", custom_mem->d_Cg_mat); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "D", custom_mem->d_Dg_mat); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "C", custom_mem->d_Cg_e_mat); |
||||
blasfeo_pack_dmat(ng, nx, custom_mem->d_Cg_mat, ng, &custom_mem->Cg_mat, 0, 0); |
||||
blasfeo_pack_dmat(ng, nu, custom_mem->d_Dg_mat, ng, &custom_mem->Dg_mat, 0, 0); |
||||
blasfeo_pack_dmat(ng_e, nx, custom_mem->d_Cg_e_mat, ng_e, &custom_mem->Cg_e_mat, 0, 0); |
||||
blasfeo_dgese(ngh_e_max, nu, 0., &custom_mem->dummy_Dgh_e_mat, 0, 0); //fill with zeros
|
||||
// NOTE: fixed lower and upper bounds of nonlinear constraints
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e); |
||||
|
||||
/* Initilize tightened constraints*/ |
||||
// NOTE: tightened constraints are only initialized once
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened); |
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened); |
||||
|
||||
/* Initialize the W matrix */ |
||||
// blasfeo_dgese(nw, nw, 0., &custom_mem->W_mat, 0, 0);
|
||||
{%- for ir in range(end=zoro_description.nw) %} |
||||
{%- for ic in range(end=zoro_description.nw) %} |
||||
blasfeo_dgein1({{zoro_description.W_mat[ir][ic]}}, &custom_mem->W_mat, {{ir}}, {{ic}}); |
||||
{%- endfor %} |
||||
{%- endfor %} |
||||
|
||||
{%- for ir in range(end=dims.nx) %} |
||||
{%- for ic in range(end=zoro_description.nw) %} |
||||
blasfeo_dgein1({{zoro_description.unc_jac_G_mat[ir][ic]}}, &custom_mem->unc_jac_G_mat, {{ir}}, {{ic}}); |
||||
{%- endfor %} |
||||
{%- endfor %} |
||||
|
||||
// NOTE: if G is changing this is not in init!
|
||||
// temp_GW_mat = unc_jac_G_mat * W_mat
|
||||
blasfeo_dgemm_nn(nx, nw, nw, 1.0, &custom_mem->unc_jac_G_mat, 0, 0, |
||||
&custom_mem->W_mat, 0, 0, 0.0, |
||||
&custom_mem->temp_GW_mat, 0, 0, &custom_mem->temp_GW_mat, 0, 0); |
||||
// GWG_mat = temp_GW_mat * unc_jac_G_mat^T
|
||||
blasfeo_dgemm_nt(nx, nx, nw, 1.0, &custom_mem->temp_GW_mat, 0, 0, |
||||
&custom_mem->unc_jac_G_mat, 0, 0, 0.0, |
||||
&custom_mem->GWG_mat, 0, 0, &custom_mem->GWG_mat, 0, 0); |
||||
|
||||
/* Initialize the uncertainty_matrix_buffer[0] */ |
||||
{%- for ir in range(end=dims.nx) %} |
||||
{%- for ic in range(end=dims.nx) %} |
||||
blasfeo_dgein1({{zoro_description.P0_mat[ir][ic]}}, &custom_mem->uncertainty_matrix_buffer[0], {{ir}}, {{ic}}); |
||||
{%- endfor %} |
||||
{%- endfor %} |
||||
|
||||
/* Initialize the feedback gain matrix */ |
||||
{%- for ir in range(end=dims.nu) %} |
||||
{%- for ic in range(end=dims.nx) %} |
||||
blasfeo_dgein1({{zoro_description.fdbk_K_mat[ir][ic]}}, &custom_mem->K_mat, {{ir}}, {{ic}}); |
||||
{%- endfor %} |
||||
{%- endfor %} |
||||
} |
||||
|
||||
|
||||
int custom_update_init_function({{ model.name }}_solver_capsule* capsule) |
||||
{ |
||||
capsule->custom_update_memory = custom_memory_create(capsule); |
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule); |
||||
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule); |
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule); |
||||
custom_val_init_function(nlp_dims, nlp_in, nlp_solver, capsule->custom_update_memory); |
||||
return 1; |
||||
} |
||||
|
||||
static void compute_gh_beta(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* C_mat, |
||||
struct blasfeo_dmat* D_mat, struct blasfeo_dmat* CaDK_mat, |
||||
struct blasfeo_dmat* CaDKmP_mat, struct blasfeo_dmat* beta_mat, |
||||
struct blasfeo_dmat* P_mat, |
||||
int n_cstr, int nx, int nu) |
||||
{ |
||||
// (C+DK)@P@(C^T+K^TD^T)
|
||||
// CaDK_mat = C_mat + D_mat @ K_mat
|
||||
blasfeo_dgemm_nn(n_cstr, nx, nu, 1.0, D_mat, 0, 0, |
||||
K_mat, 0, 0, 1.0, |
||||
C_mat, 0, 0, CaDK_mat, 0, 0); |
||||
// CaDKmP_mat = CaDK_mat @ P_mat
|
||||
blasfeo_dgemm_nn(n_cstr, nx, nx, 1.0, CaDK_mat, 0, 0, |
||||
P_mat, 0, 0, 0.0, |
||||
CaDKmP_mat, 0, 0, CaDKmP_mat, 0, 0); |
||||
// beta_mat = CaDKmP_mat @ CaDK_mat^T
|
||||
blasfeo_dgemm_nt(n_cstr, n_cstr, nx, 1.0, CaDKmP_mat, 0, 0, |
||||
CaDK_mat, 0, 0, 0.0, |
||||
beta_mat, 0, 0, beta_mat, 0, 0); |
||||
} |
||||
|
||||
static void compute_KPK(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* temp_KP_mat, |
||||
struct blasfeo_dmat* temp_KPK_mat, struct blasfeo_dmat* P_mat, |
||||
int nx, int nu) |
||||
{ |
||||
// K @ P_k @ K^T
|
||||
// temp_KP_mat = K_mat @ P_mat
|
||||
blasfeo_dgemm_nn(nu, nx, nx, 1.0, K_mat, 0, 0, |
||||
P_mat, 0, 0, 0.0, |
||||
temp_KP_mat, 0, 0, temp_KP_mat, 0, 0); |
||||
// temp_KPK_mat = temp_KP_mat @ K_mat^T
|
||||
blasfeo_dgemm_nt(nu, nu, nx, 1.0, temp_KP_mat, 0, 0, |
||||
K_mat, 0, 0, 0.0, |
||||
temp_KPK_mat, 0, 0, temp_KPK_mat, 0, 0); |
||||
} |
||||
|
||||
static void compute_next_P_matrix(struct blasfeo_dmat* P_mat, struct blasfeo_dmat* P_next_mat, |
||||
struct blasfeo_dmat* A_mat, struct blasfeo_dmat* B_mat, |
||||
struct blasfeo_dmat* K_mat, struct blasfeo_dmat* W_mat, |
||||
struct blasfeo_dmat* AK_mat, struct blasfeo_dmat* temp_AP_mat, int nx, int nu) |
||||
{ |
||||
// AK_mat = -B@K + A
|
||||
blasfeo_dgemm_nn(nx, nx, nu, -1.0, B_mat, 0, 0, K_mat, 0, 0, |
||||
1.0, A_mat, 0, 0, AK_mat, 0, 0); |
||||
// temp_AP_mat = AK_mat @ P_k
|
||||
blasfeo_dgemm_nn(nx, nx, nx, 1.0, AK_mat, 0, 0, |
||||
P_mat, 0, 0, 0.0, |
||||
temp_AP_mat, 0, 0, temp_AP_mat, 0, 0); |
||||
// P_{k+1} = temp_AP_mat @ AK_mat^T + GWG_mat
|
||||
blasfeo_dgemm_nt(nx, nx, nx, 1.0, temp_AP_mat, 0, 0, |
||||
AK_mat, 0, 0, 1.0, |
||||
W_mat, 0, 0, P_next_mat, 0, 0); |
||||
} |
||||
|
||||
static void reset_P0_matrix(ocp_nlp_dims *nlp_dims, struct blasfeo_dmat* P_mat, double* data) |
||||
{ |
||||
int nx = nlp_dims->nx[0]; |
||||
blasfeo_pack_dmat(nx, nx, data, nx, P_mat, 0, 0); |
||||
} |
||||
|
||||
static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out, custom_memory *custom_mem) |
||||
{ |
||||
ocp_nlp_config *nlp_config = solver->config; |
||||
ocp_nlp_dims *nlp_dims = solver->dims; |
||||
|
||||
int N = nlp_dims->N; |
||||
int nx = nlp_dims->nx[0]; |
||||
int nu = nlp_dims->nu[0]; |
||||
int nx_sqr = nx*nx; |
||||
int nbx = {{ dims.nbx }}; |
||||
int nbu = {{ dims.nbu }}; |
||||
int ng = {{ dims.ng }}; |
||||
int nh = {{ dims.nh }}; |
||||
int ng_e = {{ dims.ng_e }}; |
||||
int nh_e = {{ dims.nh_e }}; |
||||
int nbx_e = {{ dims.nbx_e }}; |
||||
double backoff_scaling_gamma = {{ zoro_description.backoff_scaling_gamma }}; |
||||
|
||||
// First Stage
|
||||
// NOTE: lbx_0 and ubx_0 should not be tightened.
|
||||
// NOTE: lg_0 and ug_0 are not tightened.
|
||||
// NOTE: lh_0 and uh_0 are not tightened.
|
||||
{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %} |
||||
compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat, |
||||
&custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[0]), nx, nu); |
||||
|
||||
{%- if zoro_description.nlbu_t > 0 %} |
||||
// backoff lbu
|
||||
{%- for it in zoro_description.idx_lbu_t %} |
||||
custom_mem->d_lbu_tightened[{{it}}] |
||||
= custom_mem->d_lbu[{{it}}] |
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, |
||||
custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}])); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbu", custom_mem->d_lbu_tightened); |
||||
{%- endif %} |
||||
{%- if zoro_description.nubu_t > 0 %} |
||||
// backoff ubu
|
||||
{%- for it in zoro_description.idx_ubu_t %} |
||||
custom_mem->d_ubu_tightened[{{it}}] |
||||
= custom_mem->d_ubu[{{it}}] |
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, |
||||
custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}])); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubu", custom_mem->d_ubu_tightened); |
||||
{%- endif %} |
||||
{%- endif %} |
||||
// Middle Stages
|
||||
// constraint tightening: for next stage based on dynamics of ii stage
|
||||
// P[ii+1] = (A-B@K) @ P[ii] @ (A-B@K).T + G@W@G.T
|
||||
for (int ii = 0; ii < N-1; ii++) |
||||
{ |
||||
// get and pack: A, B
|
||||
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "A", custom_mem->d_A_mat); |
||||
blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0); |
||||
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "B", custom_mem->d_B_mat); |
||||
blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0); |
||||
|
||||
compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[ii]), |
||||
&(custom_mem->uncertainty_matrix_buffer[ii+1]), |
||||
&custom_mem->A_mat, &custom_mem->B_mat, |
||||
&custom_mem->K_mat, &custom_mem->GWG_mat, |
||||
&custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu); |
||||
|
||||
// state constraints
|
||||
{%- if zoro_description.nlbx_t + zoro_description.nubx_t> 0 %} |
||||
{%- if zoro_description.nlbx_t > 0 %} |
||||
// lbx
|
||||
{%- for it in zoro_description.idx_lbx_t %} |
||||
custom_mem->d_lbx_tightened[{{it}}] |
||||
= custom_mem->d_lbx[{{it}}] |
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1], |
||||
custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}])); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbx", custom_mem->d_lbx_tightened); |
||||
{%- endif %} |
||||
{% if zoro_description.nubx_t > 0 %} |
||||
// ubx
|
||||
{%- for it in zoro_description.idx_ubx_t %} |
||||
custom_mem->d_ubx_tightened[{{it}}] = custom_mem->d_ubx[{{it}}] |
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1], |
||||
custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}])); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubx", custom_mem->d_ubx_tightened); |
||||
{%- endif %} |
||||
{%- endif %} |
||||
|
||||
{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %} |
||||
// input constraints
|
||||
compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat, |
||||
&custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[ii+1]), nx, nu); |
||||
|
||||
{%- if zoro_description.nlbu_t > 0 %} |
||||
{%- for it in zoro_description.idx_lbu_t %} |
||||
custom_mem->d_lbu_tightened[{{it}}] = custom_mem->d_lbu[{{it}}] |
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, |
||||
custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}])); |
||||
{%- endfor %} |
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbu", custom_mem->d_lbu_tightened); |
||||
{%- endif %} |
||||
{%- if zoro_description.nubu_t > 0 %} |
||||
{%- for it in zoro_description.idx_ubu_t %} |
||||
custom_mem->d_ubu_tightened[{{it}}] = custom_mem->d_ubu[{{it}}] |
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, |
||||
custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}])); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubu", custom_mem->d_ubu_tightened); |
||||
{%- endif %} |
||||
{%- endif %} |
||||
|
||||
{%- if zoro_description.nlg_t + zoro_description.nug_t > 0 %} |
||||
// Linear constraints: g
|
||||
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat, |
||||
&custom_mem->Dg_mat, &custom_mem->temp_CaDK_mat, |
||||
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat, |
||||
&custom_mem->uncertainty_matrix_buffer[ii+1], ng, nx, nu); |
||||
|
||||
{%- if zoro_description.nlg_t > 0 %} |
||||
{%- for it in zoro_description.idx_lg_t %} |
||||
custom_mem->d_lg_tightened[{{it}}] |
||||
= custom_mem->d_lg[{{it}}] |
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lg", custom_mem->d_lg_tightened); |
||||
{%- endif %} |
||||
{%- if zoro_description.nug_t > 0 %} |
||||
{%- for it in zoro_description.idx_ug_t %} |
||||
custom_mem->d_ug_tightened[{{it}}] |
||||
= custom_mem->d_ug[{{it}}] |
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ug", custom_mem->d_ug_tightened); |
||||
{%- endif %} |
||||
{%- endif %} |
||||
|
||||
|
||||
{%- if zoro_description.nlh_t + zoro_description.nuh_t > 0 %} |
||||
// nonlinear constraints: h
|
||||
// Get C_{k+1} and D_{k+1}
|
||||
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "C", custom_mem->d_Cgh_mat); |
||||
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "D", custom_mem->d_Dgh_mat); |
||||
// NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints
|
||||
blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0); |
||||
blasfeo_pack_dmat(nh, nu, custom_mem->d_Dgh_mat+ng, ng+nh, &custom_mem->Dh_mat, 0, 0); |
||||
|
||||
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat, |
||||
&custom_mem->Dh_mat, &custom_mem->temp_CaDK_mat, |
||||
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat, |
||||
&custom_mem->uncertainty_matrix_buffer[ii+1], nh, nx, nu); |
||||
|
||||
{%- if zoro_description.nlh_t > 0 %} |
||||
{%- for it in zoro_description.idx_lh_t %} |
||||
custom_mem->d_lh_tightened[{{it}}] |
||||
= custom_mem->d_lh[{{it}}] |
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lh", custom_mem->d_lh_tightened); |
||||
{%- endif %} |
||||
{%- if zoro_description.nuh_t > 0 %} |
||||
{%- for it in zoro_description.idx_uh_t %} |
||||
custom_mem->d_uh_tightened[{{it}}] = custom_mem->d_uh[{{it}}] |
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "uh", custom_mem->d_uh_tightened); |
||||
{%- endif %} |
||||
{%- endif %} |
||||
} |
||||
|
||||
// Last stage
|
||||
// get and pack: A, B
|
||||
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "A", custom_mem->d_A_mat); |
||||
blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0); |
||||
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "B", custom_mem->d_B_mat); |
||||
blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0); |
||||
// AK_mat = -B*K + A
|
||||
compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[N-1]), |
||||
&(custom_mem->uncertainty_matrix_buffer[N]), |
||||
&custom_mem->A_mat, &custom_mem->B_mat, |
||||
&custom_mem->K_mat, &custom_mem->GWG_mat, |
||||
&custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu); |
||||
|
||||
// state constraints nlbx_e_t
|
||||
{%- if zoro_description.nlbx_e_t + zoro_description.nubx_e_t> 0 %} |
||||
{%- if zoro_description.nlbx_e_t > 0 %} |
||||
// lbx_e
|
||||
{%- for it in zoro_description.idx_lbx_e_t %} |
||||
custom_mem->d_lbx_e_tightened[{{it}}] |
||||
= custom_mem->d_lbx_e[{{it}}] |
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N], |
||||
custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}])); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened); |
||||
{%- endif %} |
||||
{% if zoro_description.nubx_e_t > 0 %} |
||||
// ubx_e
|
||||
{%- for it in zoro_description.idx_ubx_e_t %} |
||||
custom_mem->d_ubx_e_tightened[{{it}}] = custom_mem->d_ubx_e[{{it}}] |
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N], |
||||
custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}])); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened); |
||||
{%- endif %} |
||||
{%- endif %} |
||||
|
||||
{%- if zoro_description.nlg_e_t + zoro_description.nug_e_t > 0 %} |
||||
// Linear constraints: g
|
||||
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat, |
||||
&custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat, |
||||
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat, |
||||
&custom_mem->uncertainty_matrix_buffer[N], ng, nx, nu); |
||||
|
||||
{%- if zoro_description.nlg_e_t > 0 %} |
||||
{%- for it in zoro_description.idx_lg_e_t %} |
||||
custom_mem->d_lg_e_tightened[{{it}}] |
||||
= custom_mem->d_lg_e[{{it}}] |
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened); |
||||
{%- endif %} |
||||
{%- if zoro_description.nug_e_t > 0 %} |
||||
{%- for it in zoro_description.idx_ug_e_t %} |
||||
custom_mem->d_ug_e_tightened[{{it}}] |
||||
= custom_mem->d_ug_e[{{it}}] |
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened); |
||||
{%- endif %} |
||||
{%- endif %} |
||||
|
||||
|
||||
{%- if zoro_description.nlh_e_t + zoro_description.nuh_e_t > 0 %} |
||||
// nonlinear constraints: h
|
||||
// Get C_{k+1} and D_{k+1}
|
||||
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, N, "C", custom_mem->d_Cgh_mat); |
||||
// NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints
|
||||
blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0); |
||||
|
||||
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat, |
||||
&custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat, |
||||
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat, |
||||
&custom_mem->uncertainty_matrix_buffer[N], nh, nx, nu); |
||||
|
||||
{%- if zoro_description.nlh_e_t > 0 %} |
||||
{%- for it in zoro_description.idx_lh_e_t %} |
||||
custom_mem->d_lh_e_tightened[{{it}}] |
||||
= custom_mem->d_lh_e[{{it}}] |
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened); |
||||
{%- endif %} |
||||
{%- if zoro_description.nuh_e_t > 0 %} |
||||
{%- for it in zoro_description.idx_uh_e_t %} |
||||
custom_mem->d_uh_e_tightened[{{it}}] = custom_mem->d_uh_e[{{it}}] |
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); |
||||
{%- endfor %} |
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened); |
||||
{%- endif %} |
||||
{%- endif %} |
||||
|
||||
} |
||||
|
||||
|
||||
int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len) |
||||
{ |
||||
custom_memory *custom_mem = (custom_memory *) capsule->custom_update_memory; |
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule); |
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule); |
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule); |
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule); |
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule); |
||||
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(capsule); |
||||
|
||||
if (data_len > 0) |
||||
{ |
||||
reset_P0_matrix(nlp_dims, &custom_mem->uncertainty_matrix_buffer[0], data); |
||||
} |
||||
uncertainty_propagate_and_update(nlp_solver, nlp_in, nlp_out, custom_mem); |
||||
|
||||
return 1; |
||||
} |
||||
|
||||
|
||||
int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule) |
||||
{ |
||||
custom_memory *mem = capsule->custom_update_memory; |
||||
|
||||
free(mem->raw_memory); |
||||
return 1; |
||||
} |
||||
|
||||
// useful prints for debugging
|
||||
|
||||
/*
|
||||
printf("A_mat:\n"); |
||||
blasfeo_print_exp_dmat(nx, nx, &custom_mem->A_mat, 0, 0); |
||||
printf("B_mat:\n"); |
||||
blasfeo_print_exp_dmat(nx, nu, &custom_mem->B_mat, 0, 0); |
||||
printf("K_mat:\n"); |
||||
blasfeo_print_exp_dmat(nu, nx, &custom_mem->K_mat, 0, 0); |
||||
printf("AK_mat:\n"); |
||||
blasfeo_print_exp_dmat(nx, nx, &custom_mem->AK_mat, 0, 0); |
||||
printf("temp_AP_mat:\n"); |
||||
blasfeo_print_exp_dmat(nx, nx, &custom_mem->temp_AP_mat, 0, 0); |
||||
printf("W_mat:\n"); |
||||
blasfeo_print_exp_dmat(nx, nx, &custom_mem->W_mat, 0, 0); |
||||
printf("P_k+1:\n"); |
||||
blasfeo_print_exp_dmat(nx, nx, &(custom_mem->uncertainty_matrix_buffer[ii+1]), 0, 0);*/ |
||||
@ -1,180 +0,0 @@ |
||||
# |
||||
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
import os |
||||
from casadi import * |
||||
from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning |
||||
|
||||
def generate_c_code_constraint( model, con_name, is_terminal, opts ): |
||||
|
||||
casadi_version = CasadiMeta.version() |
||||
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
if casadi_version not in (ALLOWED_CASADI_VERSIONS): |
||||
casadi_version_warning(casadi_version) |
||||
|
||||
# load constraint variables and expression |
||||
x = model.x |
||||
p = model.p |
||||
|
||||
if isinstance(x, casadi.MX): |
||||
symbol = MX.sym |
||||
else: |
||||
symbol = SX.sym |
||||
|
||||
if is_terminal: |
||||
con_h_expr = model.con_h_expr_e |
||||
con_phi_expr = model.con_phi_expr_e |
||||
# create dummy u, z |
||||
u = symbol('u', 0, 0) |
||||
z = symbol('z', 0, 0) |
||||
else: |
||||
con_h_expr = model.con_h_expr |
||||
con_phi_expr = model.con_phi_expr |
||||
u = model.u |
||||
z = model.z |
||||
|
||||
if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)): |
||||
raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.") |
||||
|
||||
if not (is_empty(con_h_expr) and is_empty(con_phi_expr)): |
||||
if is_empty(con_h_expr): |
||||
constr_type = 'BGP' |
||||
else: |
||||
constr_type = 'BGH' |
||||
|
||||
if is_empty(p): |
||||
p = symbol('p', 0, 0) |
||||
|
||||
if is_empty(z): |
||||
z = symbol('z', 0, 0) |
||||
|
||||
if not (is_empty(con_h_expr)) and opts['generate_hess']: |
||||
# multipliers for hessian |
||||
nh = casadi_length(con_h_expr) |
||||
lam_h = symbol('lam_h', nh, 1) |
||||
|
||||
# set up & change directory |
||||
code_export_dir = opts["code_export_directory"] |
||||
if not os.path.exists(code_export_dir): |
||||
os.makedirs(code_export_dir) |
||||
|
||||
cwd = os.getcwd() |
||||
os.chdir(code_export_dir) |
||||
gen_dir = con_name + '_constraints' |
||||
if not os.path.exists(gen_dir): |
||||
os.mkdir(gen_dir) |
||||
gen_dir_location = os.path.join('.', gen_dir) |
||||
os.chdir(gen_dir_location) |
||||
|
||||
# export casadi functions |
||||
if constr_type == 'BGH': |
||||
if is_terminal: |
||||
fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt' |
||||
else: |
||||
fun_name = con_name + '_constr_h_fun_jac_uxt_zt' |
||||
|
||||
jac_ux_t = transpose(jacobian(con_h_expr, vertcat(u,x))) |
||||
jac_z_t = jacobian(con_h_expr, z) |
||||
constraint_fun_jac_tran = Function(fun_name, [x, u, z, p], \ |
||||
[con_h_expr, jac_ux_t, jac_z_t]) |
||||
|
||||
constraint_fun_jac_tran.generate(fun_name, casadi_opts) |
||||
if opts['generate_hess']: |
||||
|
||||
if is_terminal: |
||||
fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess' |
||||
else: |
||||
fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess' |
||||
|
||||
# adjoint |
||||
adj_ux = jtimes(con_h_expr, vertcat(u, x), lam_h, True) |
||||
# hessian |
||||
hess_ux = jacobian(adj_ux, vertcat(u, x)) |
||||
|
||||
adj_z = jtimes(con_h_expr, z, lam_h, True) |
||||
hess_z = jacobian(adj_z, z) |
||||
|
||||
# set up functions |
||||
constraint_fun_jac_tran_hess = \ |
||||
Function(fun_name, [x, u, lam_h, z, p], \ |
||||
[con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z]) |
||||
|
||||
# generate C code |
||||
constraint_fun_jac_tran_hess.generate(fun_name, casadi_opts) |
||||
|
||||
if is_terminal: |
||||
fun_name = con_name + '_constr_h_e_fun' |
||||
else: |
||||
fun_name = con_name + '_constr_h_fun' |
||||
h_fun = Function(fun_name, [x, u, z, p], [con_h_expr]) |
||||
h_fun.generate(fun_name, casadi_opts) |
||||
|
||||
else: # BGP constraint |
||||
if is_terminal: |
||||
fun_name = con_name + '_phi_e_constraint' |
||||
r = model.con_r_in_phi_e |
||||
con_r_expr = model.con_r_expr_e |
||||
else: |
||||
fun_name = con_name + '_phi_constraint' |
||||
r = model.con_r_in_phi |
||||
con_r_expr = model.con_r_expr |
||||
|
||||
nphi = casadi_length(con_phi_expr) |
||||
con_phi_expr_x_u_z = substitute(con_phi_expr, r, con_r_expr) |
||||
phi_jac_u = jacobian(con_phi_expr_x_u_z, u) |
||||
phi_jac_x = jacobian(con_phi_expr_x_u_z, x) |
||||
phi_jac_z = jacobian(con_phi_expr_x_u_z, z) |
||||
|
||||
hess = hessian(con_phi_expr[0], r)[0] |
||||
for i in range(1, nphi): |
||||
hess = vertcat(hess, hessian(con_phi_expr[i], r)[0]) |
||||
|
||||
r_jac_u = jacobian(con_r_expr, u) |
||||
r_jac_x = jacobian(con_r_expr, x) |
||||
|
||||
constraint_phi = \ |
||||
Function(fun_name, [x, u, z, p], \ |
||||
[con_phi_expr_x_u_z, \ |
||||
vertcat(transpose(phi_jac_u), \ |
||||
transpose(phi_jac_x)), \ |
||||
transpose(phi_jac_z), \ |
||||
hess, vertcat(transpose(r_jac_u), \ |
||||
transpose(r_jac_x))]) |
||||
|
||||
constraint_phi.generate(fun_name, casadi_opts) |
||||
|
||||
# change directory back |
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
@ -1,98 +0,0 @@ |
||||
# |
||||
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
# |
||||
# 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. |
||||
# |
||||
|
||||
import os |
||||
import casadi as ca |
||||
from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning |
||||
|
||||
def generate_c_code_discrete_dynamics( model, opts ): |
||||
|
||||
casadi_version = ca.CasadiMeta.version() |
||||
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
if casadi_version not in (ALLOWED_CASADI_VERSIONS): |
||||
casadi_version_warning(casadi_version) |
||||
|
||||
# load model |
||||
x = model.x |
||||
u = model.u |
||||
p = model.p |
||||
phi = model.disc_dyn_expr |
||||
model_name = model.name |
||||
nx = casadi_length(x) |
||||
|
||||
if isinstance(phi, ca.MX): |
||||
symbol = ca.MX.sym |
||||
elif isinstance(phi, ca.SX): |
||||
symbol = ca.SX.sym |
||||
else: |
||||
Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi))) |
||||
|
||||
# assume nx1 = nx !!! |
||||
lam = symbol('lam', nx, 1) |
||||
|
||||
# generate jacobians |
||||
ux = ca.vertcat(u,x) |
||||
jac_ux = ca.jacobian(phi, ux) |
||||
# generate adjoint |
||||
adj_ux = ca.jtimes(phi, ux, lam, True) |
||||
# generate hessian |
||||
hess_ux = ca.jacobian(adj_ux, ux) |
||||
|
||||
## change directory |
||||
code_export_dir = opts["code_export_directory"] |
||||
if not os.path.exists(code_export_dir): |
||||
os.makedirs(code_export_dir) |
||||
|
||||
cwd = os.getcwd() |
||||
os.chdir(code_export_dir) |
||||
model_dir = model_name + '_model' |
||||
if not os.path.exists(model_dir): |
||||
os.mkdir(model_dir) |
||||
model_dir_location = os.path.join('.', model_dir) |
||||
os.chdir(model_dir_location) |
||||
|
||||
# set up & generate Functions |
||||
fun_name = model_name + '_dyn_disc_phi_fun' |
||||
phi_fun = ca.Function(fun_name, [x, u, p], [phi]) |
||||
phi_fun.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_dyn_disc_phi_fun_jac' |
||||
phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T]) |
||||
phi_fun_jac_ut_xt.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_dyn_disc_phi_fun_jac_hess' |
||||
phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux]) |
||||
phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_opts) |
||||
|
||||
os.chdir(cwd) |
||||
@ -1,124 +0,0 @@ |
||||
# |
||||
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
import os |
||||
from casadi import * |
||||
from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning |
||||
|
||||
def generate_c_code_explicit_ode( model, opts ): |
||||
|
||||
casadi_version = CasadiMeta.version() |
||||
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
if casadi_version not in (ALLOWED_CASADI_VERSIONS): |
||||
casadi_version_warning(casadi_version) |
||||
|
||||
|
||||
generate_hess = opts["generate_hess"] |
||||
code_export_dir = opts["code_export_directory"] |
||||
|
||||
# load model |
||||
x = model.x |
||||
u = model.u |
||||
p = model.p |
||||
f_expl = model.f_expl_expr |
||||
model_name = model.name |
||||
|
||||
## get model dimensions |
||||
nx = x.size()[0] |
||||
nu = u.size()[0] |
||||
|
||||
if isinstance(f_expl, casadi.MX): |
||||
symbol = MX.sym |
||||
elif isinstance(f_expl, casadi.SX): |
||||
symbol = SX.sym |
||||
else: |
||||
raise Exception("Invalid type for f_expl! Possible types are 'SX' and 'MX'. Exiting.") |
||||
## set up functions to be exported |
||||
Sx = symbol('Sx', nx, nx) |
||||
Sp = symbol('Sp', nx, nu) |
||||
lambdaX = symbol('lambdaX', nx, 1) |
||||
|
||||
fun_name = model_name + '_expl_ode_fun' |
||||
|
||||
## Set up functions |
||||
expl_ode_fun = Function(fun_name, [x, u, p], [f_expl]) |
||||
|
||||
vdeX = jtimes(f_expl,x,Sx) |
||||
vdeP = jacobian(f_expl,u) + jtimes(f_expl,x,Sp) |
||||
|
||||
fun_name = model_name + '_expl_vde_forw' |
||||
|
||||
expl_vde_forw = Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP]) |
||||
|
||||
adj = jtimes(f_expl, vertcat(x, u), lambdaX, True) |
||||
|
||||
fun_name = model_name + '_expl_vde_adj' |
||||
expl_vde_adj = Function(fun_name, [x, lambdaX, u, p], [adj]) |
||||
|
||||
if generate_hess: |
||||
S_forw = vertcat(horzcat(Sx, Sp), horzcat(DM.zeros(nu,nx), DM.eye(nu))) |
||||
hess = mtimes(transpose(S_forw),jtimes(adj, vertcat(x,u), S_forw)) |
||||
hess2 = [] |
||||
for j in range(nx+nu): |
||||
for i in range(j,nx+nu): |
||||
hess2 = vertcat(hess2, hess[i,j]) |
||||
|
||||
fun_name = model_name + '_expl_ode_hess' |
||||
expl_ode_hess = Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2]) |
||||
|
||||
## generate C code |
||||
if not os.path.exists(code_export_dir): |
||||
os.makedirs(code_export_dir) |
||||
|
||||
cwd = os.getcwd() |
||||
os.chdir(code_export_dir) |
||||
model_dir = model_name + '_model' |
||||
if not os.path.exists(model_dir): |
||||
os.mkdir(model_dir) |
||||
model_dir_location = os.path.join('.', model_dir) |
||||
os.chdir(model_dir_location) |
||||
fun_name = model_name + '_expl_ode_fun' |
||||
expl_ode_fun.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_expl_vde_forw' |
||||
expl_vde_forw.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_expl_vde_adj' |
||||
expl_vde_adj.generate(fun_name, casadi_opts) |
||||
|
||||
if generate_hess: |
||||
fun_name = model_name + '_expl_ode_hess' |
||||
expl_ode_hess.generate(fun_name, casadi_opts) |
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
@ -1,116 +0,0 @@ |
||||
# |
||||
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
import os |
||||
from casadi import SX, MX, Function, transpose, vertcat, horzcat, hessian, CasadiMeta |
||||
from .utils import ALLOWED_CASADI_VERSIONS, casadi_version_warning |
||||
|
||||
|
||||
def generate_c_code_external_cost(model, stage_type, opts): |
||||
|
||||
casadi_version = CasadiMeta.version() |
||||
casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double") |
||||
|
||||
if casadi_version not in (ALLOWED_CASADI_VERSIONS): |
||||
casadi_version_warning(casadi_version) |
||||
|
||||
x = model.x |
||||
p = model.p |
||||
|
||||
if isinstance(x, MX): |
||||
symbol = MX.sym |
||||
else: |
||||
symbol = SX.sym |
||||
|
||||
if stage_type == 'terminal': |
||||
suffix_name = "_cost_ext_cost_e_fun" |
||||
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" |
||||
suffix_name_jac = "_cost_ext_cost_e_fun_jac" |
||||
u = symbol("u", 0, 0) |
||||
ext_cost = model.cost_expr_ext_cost_e |
||||
custom_hess = model.cost_expr_ext_cost_custom_hess_e |
||||
|
||||
elif stage_type == 'path': |
||||
suffix_name = "_cost_ext_cost_fun" |
||||
suffix_name_hess = "_cost_ext_cost_fun_jac_hess" |
||||
suffix_name_jac = "_cost_ext_cost_fun_jac" |
||||
u = model.u |
||||
ext_cost = model.cost_expr_ext_cost |
||||
custom_hess = model.cost_expr_ext_cost_custom_hess |
||||
|
||||
elif stage_type == 'initial': |
||||
suffix_name = "_cost_ext_cost_0_fun" |
||||
suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" |
||||
suffix_name_jac = "_cost_ext_cost_0_fun_jac" |
||||
u = model.u |
||||
ext_cost = model.cost_expr_ext_cost_0 |
||||
custom_hess = model.cost_expr_ext_cost_custom_hess_0 |
||||
|
||||
# set up functions to be exported |
||||
fun_name = model.name + suffix_name |
||||
fun_name_hess = model.name + suffix_name_hess |
||||
fun_name_jac = model.name + suffix_name_jac |
||||
|
||||
# generate expression for full gradient and Hessian |
||||
full_hess, grad = hessian(ext_cost, vertcat(u, x)) |
||||
|
||||
if custom_hess is not None: |
||||
full_hess = custom_hess |
||||
|
||||
ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost]) |
||||
ext_cost_fun_jac_hess = Function( |
||||
fun_name_hess, [x, u, p], [ext_cost, grad, full_hess] |
||||
) |
||||
ext_cost_fun_jac = Function( |
||||
fun_name_jac, [x, u, p], [ext_cost, grad] |
||||
) |
||||
|
||||
# generate C code |
||||
code_export_dir = opts["code_export_directory"] |
||||
if not os.path.exists(code_export_dir): |
||||
os.makedirs(code_export_dir) |
||||
|
||||
cwd = os.getcwd() |
||||
os.chdir(code_export_dir) |
||||
gen_dir = model.name + '_cost' |
||||
if not os.path.exists(gen_dir): |
||||
os.mkdir(gen_dir) |
||||
gen_dir_location = "./" + gen_dir |
||||
os.chdir(gen_dir_location) |
||||
|
||||
ext_cost_fun.generate(fun_name, casadi_opts) |
||||
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts) |
||||
ext_cost_fun_jac.generate(fun_name_jac, casadi_opts) |
||||
|
||||
os.chdir(cwd) |
||||
return |
||||
@ -1,131 +0,0 @@ |
||||
# |
||||
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
import os |
||||
from casadi import * |
||||
from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning |
||||
|
||||
def generate_c_code_gnsf( model, opts ): |
||||
|
||||
casadi_version = CasadiMeta.version() |
||||
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
if casadi_version not in (ALLOWED_CASADI_VERSIONS): |
||||
casadi_version_warning(casadi_version) |
||||
|
||||
model_name = model.name |
||||
code_export_dir = opts["code_export_directory"] |
||||
|
||||
# set up directory |
||||
if not os.path.exists(code_export_dir): |
||||
os.makedirs(code_export_dir) |
||||
|
||||
cwd = os.getcwd() |
||||
os.chdir(code_export_dir) |
||||
model_dir = model_name + '_model' |
||||
if not os.path.exists(model_dir): |
||||
os.mkdir(model_dir) |
||||
model_dir_location = os.path.join('.', model_dir) |
||||
os.chdir(model_dir_location) |
||||
|
||||
# obtain gnsf dimensions |
||||
get_matrices_fun = model.get_matrices_fun |
||||
phi_fun = model.phi_fun |
||||
|
||||
size_gnsf_A = get_matrices_fun.size_out(0) |
||||
gnsf_nx1 = size_gnsf_A[1] |
||||
gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] |
||||
gnsf_nuhat = max(phi_fun.size_in(1)) |
||||
gnsf_ny = max(phi_fun.size_in(0)) |
||||
gnsf_nout = max(phi_fun.size_out(0)) |
||||
|
||||
# set up expressions |
||||
# if the model uses MX because of cost/constraints |
||||
# the DAE can be exported as SX -> detect GNSF in Matlab |
||||
# -> evaluated SX GNSF functions with MX. |
||||
u = model.u |
||||
|
||||
if isinstance(u, casadi.MX): |
||||
symbol = MX.sym |
||||
else: |
||||
symbol = SX.sym |
||||
|
||||
y = symbol("y", gnsf_ny, 1) |
||||
uhat = symbol("uhat", gnsf_nuhat, 1) |
||||
p = model.p |
||||
x1 = symbol("gnsf_x1", gnsf_nx1, 1) |
||||
x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) |
||||
z1 = symbol("gnsf_z1", gnsf_nz1, 1) |
||||
dummy = symbol("gnsf_dummy", 1, 1) |
||||
empty_var = symbol("gnsf_empty_var", 0, 0) |
||||
|
||||
## generate C code |
||||
fun_name = model_name + '_gnsf_phi_fun' |
||||
phi_fun_ = Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)]) |
||||
phi_fun_.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_gnsf_phi_fun_jac_y' |
||||
phi_fun_jac_y = model.phi_fun_jac_y |
||||
phi_fun_jac_y_ = Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p)) |
||||
phi_fun_jac_y_.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_gnsf_phi_jac_y_uhat' |
||||
phi_jac_y_uhat = model.phi_jac_y_uhat |
||||
phi_jac_y_uhat_ = Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p)) |
||||
phi_jac_y_uhat_.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz' |
||||
f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz |
||||
f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p) |
||||
|
||||
# avoid codegeneration issue |
||||
if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval): |
||||
f_lo_fun_jac_x1k1uz_eval = [empty_var] |
||||
|
||||
f_lo_fun_jac_x1k1uz_ = Function(fun_name, [x1, x1dot, z1, u, p], |
||||
f_lo_fun_jac_x1k1uz_eval) |
||||
f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_gnsf_get_matrices_fun' |
||||
get_matrices_fun_ = Function(fun_name, [dummy], get_matrices_fun(1)) |
||||
get_matrices_fun_.generate(fun_name, casadi_opts) |
||||
|
||||
# remove fields for json dump |
||||
del model.phi_fun |
||||
del model.phi_fun_jac_y |
||||
del model.phi_jac_y_uhat |
||||
del model.f_lo_fun_jac_x1k1uz |
||||
del model.get_matrices_fun |
||||
|
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
@ -1,139 +0,0 @@ |
||||
# |
||||
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
import os |
||||
from casadi import * |
||||
from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning |
||||
|
||||
def generate_c_code_implicit_ode( model, opts ): |
||||
|
||||
casadi_version = CasadiMeta.version() |
||||
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
if casadi_version not in (ALLOWED_CASADI_VERSIONS): |
||||
casadi_version_warning(casadi_version) |
||||
|
||||
generate_hess = opts["generate_hess"] |
||||
code_export_dir = opts["code_export_directory"] |
||||
|
||||
## load model |
||||
x = model.x |
||||
xdot = model.xdot |
||||
u = model.u |
||||
z = model.z |
||||
p = model.p |
||||
f_impl = model.f_impl_expr |
||||
model_name = model.name |
||||
|
||||
## get model dimensions |
||||
nx = casadi_length(x) |
||||
nu = casadi_length(u) |
||||
nz = casadi_length(z) |
||||
|
||||
## generate jacobians |
||||
jac_x = jacobian(f_impl, x) |
||||
jac_xdot = jacobian(f_impl, xdot) |
||||
jac_u = jacobian(f_impl, u) |
||||
jac_z = jacobian(f_impl, z) |
||||
|
||||
## generate hessian |
||||
x_xdot_z_u = vertcat(x, xdot, z, u) |
||||
|
||||
if isinstance(x, casadi.MX): |
||||
symbol = MX.sym |
||||
else: |
||||
symbol = SX.sym |
||||
|
||||
multiplier = symbol('multiplier', nx + nz) |
||||
|
||||
ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True) |
||||
HESS = jacobian(ADJ, x_xdot_z_u) |
||||
|
||||
## Set up functions |
||||
p = model.p |
||||
fun_name = model_name + '_impl_dae_fun' |
||||
impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' |
||||
impl_dae_fun_jac_x_xdot_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) |
||||
|
||||
# fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' |
||||
# impl_dae_fun_jac_x_xdot = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) |
||||
|
||||
# fun_name = model_name + '_impl_dae_jac_x_xdot_u' |
||||
# impl_dae_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' |
||||
impl_dae_fun_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z]) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' |
||||
impl_dae_fun_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u]) |
||||
|
||||
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' |
||||
impl_dae_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) |
||||
|
||||
|
||||
fun_name = model_name + '_impl_dae_hess' |
||||
impl_dae_hess = Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS]) |
||||
|
||||
# generate C code |
||||
if not os.path.exists(code_export_dir): |
||||
os.makedirs(code_export_dir) |
||||
|
||||
cwd = os.getcwd() |
||||
os.chdir(code_export_dir) |
||||
model_dir = model_name + '_model' |
||||
if not os.path.exists(model_dir): |
||||
os.mkdir(model_dir) |
||||
model_dir_location = os.path.join('.', model_dir) |
||||
os.chdir(model_dir_location) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun' |
||||
impl_dae_fun.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' |
||||
impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' |
||||
impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' |
||||
impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_opts) |
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' |
||||
impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_opts) |
||||
|
||||
if generate_hess: |
||||
fun_name = model_name + '_impl_dae_hess' |
||||
impl_dae_hess.generate(fun_name, casadi_opts) |
||||
|
||||
os.chdir(cwd) |
||||
@ -1,113 +0,0 @@ |
||||
# |
||||
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, |
||||
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, |
||||
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, |
||||
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
import os |
||||
from casadi import * |
||||
from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning |
||||
|
||||
def generate_c_code_nls_cost( model, cost_name, stage_type, opts ): |
||||
|
||||
casadi_version = CasadiMeta.version() |
||||
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') |
||||
|
||||
if casadi_version not in (ALLOWED_CASADI_VERSIONS): |
||||
casadi_version_warning(casadi_version) |
||||
|
||||
x = model.x |
||||
p = model.p |
||||
|
||||
if isinstance(x, casadi.MX): |
||||
symbol = MX.sym |
||||
else: |
||||
symbol = SX.sym |
||||
|
||||
if stage_type == 'terminal': |
||||
middle_name = '_cost_y_e' |
||||
u = symbol('u', 0, 0) |
||||
cost_expr = model.cost_y_expr_e |
||||
|
||||
elif stage_type == 'initial': |
||||
middle_name = '_cost_y_0' |
||||
u = model.u |
||||
cost_expr = model.cost_y_expr_0 |
||||
|
||||
elif stage_type == 'path': |
||||
middle_name = '_cost_y' |
||||
u = model.u |
||||
cost_expr = model.cost_y_expr |
||||
|
||||
# set up directory |
||||
code_export_dir = opts["code_export_directory"] |
||||
if not os.path.exists(code_export_dir): |
||||
os.makedirs(code_export_dir) |
||||
|
||||
cwd = os.getcwd() |
||||
os.chdir(code_export_dir) |
||||
gen_dir = cost_name + '_cost' |
||||
if not os.path.exists(gen_dir): |
||||
os.mkdir(gen_dir) |
||||
gen_dir_location = os.path.join('.', gen_dir) |
||||
os.chdir(gen_dir_location) |
||||
|
||||
# set up expressions |
||||
cost_jac_expr = transpose(jacobian(cost_expr, vertcat(u, x))) |
||||
|
||||
ny = casadi_length(cost_expr) |
||||
|
||||
y = symbol('y', ny, 1) |
||||
|
||||
y_adj = jtimes(cost_expr, vertcat(u, x), y, True) |
||||
y_hess = jacobian(y_adj, vertcat(u, x)) |
||||
|
||||
## generate C code |
||||
suffix_name = '_fun' |
||||
fun_name = cost_name + middle_name + suffix_name |
||||
y_fun = Function( fun_name, [x, u, p], \ |
||||
[ cost_expr ]) |
||||
y_fun.generate( fun_name, casadi_opts ) |
||||
|
||||
suffix_name = '_fun_jac_ut_xt' |
||||
fun_name = cost_name + middle_name + suffix_name |
||||
y_fun_jac_ut_xt = Function(fun_name, [x, u, p], \ |
||||
[ cost_expr, cost_jac_expr ]) |
||||
y_fun_jac_ut_xt.generate( fun_name, casadi_opts ) |
||||
|
||||
suffix_name = '_hess' |
||||
fun_name = cost_name + middle_name + suffix_name |
||||
y_hess = Function(fun_name, [x, u, y, p], [ y_hess ]) |
||||
y_hess.generate( fun_name, casadi_opts ) |
||||
|
||||
os.chdir(cwd) |
||||
|
||||
return |
||||
|
||||
@ -0,0 +1 @@ |
||||
|
||||
@ -0,0 +1,216 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
from acados_template.utils import casadi_length |
||||
from casadi import * |
||||
import numpy as np |
||||
|
||||
|
||||
def check_reformulation(model, gnsf, print_info): |
||||
|
||||
## Description: |
||||
# this function takes the implicit ODE/ index-1 DAE and a gnsf structure |
||||
# to evaluate both models at num_eval random points x0, x0dot, z0, u0 |
||||
# if for all points the relative error is <= TOL, the function will return:: |
||||
# 1, otherwise it will give an error. |
||||
|
||||
TOL = 1e-14 |
||||
num_eval = 10 |
||||
|
||||
# get dimensions |
||||
nx = gnsf["nx"] |
||||
nu = gnsf["nu"] |
||||
nz = gnsf["nz"] |
||||
nx1 = gnsf["nx1"] |
||||
nx2 = gnsf["nx2"] |
||||
nz1 = gnsf["nz1"] |
||||
nz2 = gnsf["nz2"] |
||||
n_out = gnsf["n_out"] |
||||
|
||||
# get model matrices |
||||
A = gnsf["A"] |
||||
B = gnsf["B"] |
||||
C = gnsf["C"] |
||||
E = gnsf["E"] |
||||
c = gnsf["c"] |
||||
|
||||
L_x = gnsf["L_x"] |
||||
L_xdot = gnsf["L_xdot"] |
||||
L_z = gnsf["L_z"] |
||||
L_u = gnsf["L_u"] |
||||
|
||||
A_LO = gnsf["A_LO"] |
||||
E_LO = gnsf["E_LO"] |
||||
B_LO = gnsf["B_LO"] |
||||
c_LO = gnsf["c_LO"] |
||||
|
||||
I_x1 = range(nx1) |
||||
I_x2 = range(nx1, nx) |
||||
|
||||
I_z1 = range(nz1) |
||||
I_z2 = range(nz1, nz) |
||||
|
||||
idx_perm_f = gnsf["idx_perm_f"] |
||||
|
||||
# get casadi variables |
||||
x = gnsf["x"] |
||||
xdot = gnsf["xdot"] |
||||
z = gnsf["z"] |
||||
u = gnsf["u"] |
||||
y = gnsf["y"] |
||||
uhat = gnsf["uhat"] |
||||
p = gnsf["p"] |
||||
|
||||
# create functions |
||||
impl_dae_fun = Function("impl_dae_fun", [x, xdot, u, z, p], [model.f_impl_expr]) |
||||
phi_fun = Function("phi_fun", [y, uhat, p], [gnsf["phi_expr"]]) |
||||
f_lo_fun = Function( |
||||
"f_lo_fun", [x[range(nx1)], xdot[range(nx1)], z, u, p], [gnsf["f_lo_expr"]] |
||||
) |
||||
|
||||
# print(gnsf) |
||||
# print(gnsf["n_out"]) |
||||
|
||||
for i_check in range(num_eval): |
||||
# generate random values |
||||
x0 = np.random.rand(nx, 1) |
||||
x0dot = np.random.rand(nx, 1) |
||||
z0 = np.random.rand(nz, 1) |
||||
u0 = np.random.rand(nu, 1) |
||||
|
||||
if gnsf["ny"] > 0: |
||||
y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0[I_z1] |
||||
else: |
||||
y0 = [] |
||||
if gnsf["nuhat"] > 0: |
||||
uhat0 = L_u @ u0 |
||||
else: |
||||
uhat0 = [] |
||||
|
||||
# eval functions |
||||
p0 = np.random.rand(gnsf["np"], 1) |
||||
f_impl_val = impl_dae_fun(x0, x0dot, u0, z0, p0).full() |
||||
phi_val = phi_fun(y0, uhat0, p0) |
||||
f_lo_val = f_lo_fun(x0[I_x1], x0dot[I_x1], z0[I_z1], u0, p0) |
||||
|
||||
f_impl_val = f_impl_val[idx_perm_f] |
||||
# eval gnsf |
||||
if n_out > 0: |
||||
C_phi = C @ phi_val |
||||
else: |
||||
C_phi = np.zeros((nx1 + nz1, 1)) |
||||
try: |
||||
gnsf_val1 = ( |
||||
A @ x0[I_x1] + B @ u0 + C_phi + c - E @ vertcat(x0dot[I_x1], z0[I_z1]) |
||||
) |
||||
# gnsf_1 = (A @ x[I_x1] + B @ u + C_phi + c - E @ vertcat(xdot[I_x1], z[I_z1])) |
||||
except: |
||||
import pdb |
||||
|
||||
pdb.set_trace() |
||||
|
||||
if nx2 > 0: # eval LOS: |
||||
gnsf_val2 = ( |
||||
A_LO @ x0[I_x2] |
||||
+ B_LO @ u0 |
||||
+ c_LO |
||||
+ f_lo_val |
||||
- E_LO @ vertcat(x0dot[I_x2], z0[I_z2]) |
||||
) |
||||
gnsf_val = vertcat(gnsf_val1, gnsf_val2).full() |
||||
else: |
||||
gnsf_val = gnsf_val1.full() |
||||
# compute error and check |
||||
rel_error = np.linalg.norm(f_impl_val - gnsf_val) / np.linalg.norm(f_impl_val) |
||||
|
||||
if rel_error > TOL: |
||||
print("transcription failed rel_error > TOL") |
||||
print("you are in debug mode now: import pdb; pdb.set_trace()") |
||||
abs_error = gnsf_val - f_impl_val |
||||
# T = table(f_impl_val, gnsf_val, abs_error) |
||||
# print(T) |
||||
print("abs_error:", abs_error) |
||||
# error('transcription failed rel_error > TOL') |
||||
# check = 0 |
||||
import pdb |
||||
|
||||
pdb.set_trace() |
||||
if print_info: |
||||
print(" ") |
||||
print("model reformulation checked: relative error <= TOL = ", str(TOL)) |
||||
print(" ") |
||||
check = 1 |
||||
## helpful for debugging: |
||||
# # use in calling function and compare |
||||
# # compare f_impl(i) with gnsf_val1(i) |
||||
# |
||||
|
||||
# nx = gnsf['nx'] |
||||
# nu = gnsf['nu'] |
||||
# nz = gnsf['nz'] |
||||
# nx1 = gnsf['nx1'] |
||||
# nx2 = gnsf['nx2'] |
||||
# |
||||
# A = gnsf['A'] |
||||
# B = gnsf['B'] |
||||
# C = gnsf['C'] |
||||
# E = gnsf['E'] |
||||
# c = gnsf['c'] |
||||
# |
||||
# L_x = gnsf['L_x'] |
||||
# L_z = gnsf['L_z'] |
||||
# L_xdot = gnsf['L_xdot'] |
||||
# L_u = gnsf['L_u'] |
||||
# |
||||
# A_LO = gnsf['A_LO'] |
||||
# |
||||
# x0 = rand(nx, 1) |
||||
# x0dot = rand(nx, 1) |
||||
# z0 = rand(nz, 1) |
||||
# u0 = rand(nu, 1) |
||||
# I_x1 = range(nx1) |
||||
# I_x2 = nx1+range(nx) |
||||
# |
||||
# y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0 |
||||
# uhat0 = L_u @ u0 |
||||
# |
||||
# gnsf_val1 = (A @ x[I_x1] + B @ u + # C @ phi_current + c) - E @ [xdot[I_x1] z] |
||||
# gnsf_val1 = gnsf_val1.simplify() |
||||
# |
||||
# # gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2] |
||||
# gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2] |
||||
# |
||||
# |
||||
# gnsf_val = [gnsf_val1 gnsf_val2] |
||||
# gnsf_val = gnsf_val.simplify() |
||||
# dyn_expr_f = dyn_expr_f.simplify() |
||||
# import pdb; pdb.set_trace() |
||||
|
||||
return check |
||||
@ -0,0 +1,278 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
from casadi import * |
||||
from .check_reformulation import check_reformulation |
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function |
||||
from ..utils import casadi_length, print_casadi_expression |
||||
|
||||
|
||||
def detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info): |
||||
|
||||
## Description |
||||
# this function takes a gnsf structure with trivial model matrices (A, B, |
||||
# E, c are zeros, and C is eye). |
||||
# It detects all affine linear terms and sets up an equivalent model in the |
||||
# GNSF structure, where all affine linear terms are modeled through the |
||||
# matrices A, B, E, c and the linear output system (LOS) is empty. |
||||
# NOTE: model is just taken as an argument to check equivalence of the |
||||
# models within the function. |
||||
|
||||
model = acados_ocp.model |
||||
if print_info: |
||||
print(" ") |
||||
print("====================================================================") |
||||
print(" ") |
||||
print("============ Detect affine-linear dependencies ==================") |
||||
print(" ") |
||||
print("====================================================================") |
||||
print(" ") |
||||
# symbolics |
||||
x = gnsf["x"] |
||||
xdot = gnsf["xdot"] |
||||
u = gnsf["u"] |
||||
z = gnsf["z"] |
||||
|
||||
# dimensions |
||||
nx = gnsf["nx"] |
||||
nu = gnsf["nu"] |
||||
nz = gnsf["nz"] |
||||
|
||||
ny_old = gnsf["ny"] |
||||
nuhat_old = gnsf["nuhat"] |
||||
|
||||
## Represent all affine dependencies through the model matrices A, B, E, c |
||||
## determine A |
||||
n_nodes_current = n_nodes(gnsf["phi_expr"]) |
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])): |
||||
fii = gnsf["phi_expr"][ii] |
||||
for ix in range(nx): |
||||
var = x[ix] |
||||
varname = var.name |
||||
# symbolic jacobian of fii w.r.t. xi |
||||
jac_fii_xi = jacobian(fii, var) |
||||
if jac_fii_xi.is_constant(): |
||||
# jacobian value |
||||
jac_fii_xi_fun = Function("jac_fii_xi_fun", [x[1]], [jac_fii_xi]) |
||||
# x[1] as input just to have a scalar input and call the function as follows: |
||||
gnsf["A"][ii, ix] = jac_fii_xi_fun(0).full() |
||||
else: |
||||
gnsf["A"][ii, ix] = 0 |
||||
if print_info: |
||||
print( |
||||
"phi(", |
||||
str(ii), |
||||
") is nonlinear in x(", |
||||
str(ix), |
||||
") = ", |
||||
varname, |
||||
) |
||||
print(fii) |
||||
print("-----------------------------------------------------") |
||||
f_next = gnsf["phi_expr"] - gnsf["A"] @ x |
||||
f_next = simplify(f_next) |
||||
n_nodes_next = n_nodes(f_next) |
||||
|
||||
if print_info: |
||||
print("\n") |
||||
print(f"determined matrix A:") |
||||
print(gnsf["A"]) |
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") |
||||
# assert(n_nodes_current >= n_nodes_next,'n_nodes_current >= n_nodes_next FAILED') |
||||
gnsf["phi_expr"] = f_next |
||||
|
||||
check_reformulation(model, gnsf, print_info) |
||||
|
||||
## determine B |
||||
n_nodes_current = n_nodes(gnsf["phi_expr"]) |
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])): |
||||
fii = gnsf["phi_expr"][ii] |
||||
for iu in range(nu): |
||||
var = u[iu] |
||||
varname = var.name |
||||
# symbolic jacobian of fii w.r.t. ui |
||||
jac_fii_ui = jacobian(fii, var) |
||||
if jac_fii_ui.is_constant(): # i.e. hessian is structural zero: |
||||
# jacobian value |
||||
jac_fii_ui_fun = Function("jac_fii_ui_fun", [x[1]], [jac_fii_ui]) |
||||
gnsf["B"][ii, iu] = jac_fii_ui_fun(0).full() |
||||
else: |
||||
gnsf["B"][ii, iu] = 0 |
||||
if print_info: |
||||
print(f"phi({ii}) is nonlinear in u(", str(iu), ") = ", varname) |
||||
print(fii) |
||||
print("-----------------------------------------------------") |
||||
f_next = gnsf["phi_expr"] - gnsf["B"] @ u |
||||
f_next = simplify(f_next) |
||||
n_nodes_next = n_nodes(f_next) |
||||
|
||||
if print_info: |
||||
print("\n") |
||||
print(f"determined matrix B:") |
||||
print(gnsf["B"]) |
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") |
||||
|
||||
gnsf["phi_expr"] = f_next |
||||
|
||||
check_reformulation(model, gnsf, print_info) |
||||
|
||||
## determine E |
||||
n_nodes_current = n_nodes(gnsf["phi_expr"]) |
||||
k = vertcat(xdot, z) |
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])): |
||||
fii = gnsf["phi_expr"][ii] |
||||
for ik in range(casadi_length(k)): |
||||
# symbolic jacobian of fii w.r.t. ui |
||||
var = k[ik] |
||||
varname = var.name |
||||
jac_fii_ki = jacobian(fii, var) |
||||
if jac_fii_ki.is_constant(): |
||||
# jacobian value |
||||
jac_fii_ki_fun = Function("jac_fii_ki_fun", [x[1]], [jac_fii_ki]) |
||||
gnsf["E"][ii, ik] = -jac_fii_ki_fun(0).full() |
||||
else: |
||||
gnsf["E"][ii, ik] = 0 |
||||
if print_info: |
||||
print(f"phi( {ii}) is nonlinear in xdot_z({ik}) = ", varname) |
||||
print(fii) |
||||
print("-----------------------------------------------------") |
||||
f_next = gnsf["phi_expr"] + gnsf["E"] @ k |
||||
f_next = simplify(f_next) |
||||
n_nodes_next = n_nodes(f_next) |
||||
|
||||
if print_info: |
||||
print("\n") |
||||
print(f"determined matrix E:") |
||||
print(gnsf["E"]) |
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") |
||||
|
||||
gnsf["phi_expr"] = f_next |
||||
check_reformulation(model, gnsf, print_info) |
||||
|
||||
## determine constant term c |
||||
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"]) |
||||
for ii in range(casadi_length(gnsf["phi_expr"])): |
||||
fii = gnsf["phi_expr"][ii] |
||||
if fii.is_constant(): |
||||
# function value goes into c |
||||
fii_fun = Function("fii_fun", [x[1]], [fii]) |
||||
gnsf["c"][ii] = fii_fun(0).full() |
||||
else: |
||||
gnsf["c"][ii] = 0 |
||||
if print_info: |
||||
print(f"phi(", str(ii), ") is NOT constant") |
||||
print(fii) |
||||
print("-----------------------------------------------------") |
||||
gnsf["phi_expr"] = gnsf["phi_expr"] - gnsf["c"] |
||||
gnsf["phi_expr"] = simplify(gnsf["phi_expr"]) |
||||
n_nodes_next = n_nodes(gnsf["phi_expr"]) |
||||
|
||||
if print_info: |
||||
print("\n") |
||||
print(f"determined vector c:") |
||||
print(gnsf["c"]) |
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") |
||||
|
||||
check_reformulation(model, gnsf, print_info) |
||||
|
||||
## determine nonlinearity & corresponding matrix C |
||||
## Reduce dimension of phi |
||||
n_nodes_current = n_nodes(gnsf["phi_expr"]) |
||||
ind_non_zero = [] |
||||
for ii in range(casadi_length(gnsf["phi_expr"])): |
||||
fii = gnsf["phi_expr"][ii] |
||||
fii = simplify(fii) |
||||
if not fii.is_zero(): |
||||
ind_non_zero = list(set.union(set(ind_non_zero), set([ii]))) |
||||
gnsf["phi_expr"] = gnsf["phi_expr"][ind_non_zero] |
||||
|
||||
# C |
||||
gnsf["C"] = np.zeros((nx + nz, len(ind_non_zero))) |
||||
for ii in range(len(ind_non_zero)): |
||||
gnsf["C"][ind_non_zero[ii], ii] = 1 |
||||
gnsf = determine_input_nonlinearity_function(gnsf) |
||||
n_nodes_next = n_nodes(gnsf["phi_expr"]) |
||||
|
||||
if print_info: |
||||
print(" ") |
||||
print("determined matrix C:") |
||||
print(gnsf["C"]) |
||||
print( |
||||
"---------------------------------------------------------------------------------" |
||||
) |
||||
print( |
||||
"------------- Success: Affine linear terms detected -----------------------------" |
||||
) |
||||
print( |
||||
"---------------------------------------------------------------------------------" |
||||
) |
||||
print( |
||||
f'reduced nonlinearity dimension n_out from {nx+nz} to {gnsf["n_out"]}' |
||||
) |
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes") |
||||
print(" ") |
||||
print("phi now reads as:") |
||||
print_casadi_expression(gnsf["phi_expr"]) |
||||
|
||||
## determine input of nonlinearity function |
||||
check_reformulation(model, gnsf, print_info) |
||||
|
||||
gnsf["ny"] = casadi_length(gnsf["y"]) |
||||
gnsf["nuhat"] = casadi_length(gnsf["uhat"]) |
||||
|
||||
if print_info: |
||||
print( |
||||
"-----------------------------------------------------------------------------------" |
||||
) |
||||
print(" ") |
||||
print( |
||||
f"reduced input ny of phi from ", |
||||
str(ny_old), |
||||
" to ", |
||||
str(gnsf["ny"]), |
||||
) |
||||
print( |
||||
f"reduced input nuhat of phi from ", |
||||
str(nuhat_old), |
||||
" to ", |
||||
str(gnsf["nuhat"]), |
||||
) |
||||
print( |
||||
"-----------------------------------------------------------------------------------" |
||||
) |
||||
|
||||
# if print_info: |
||||
# print(f"gnsf: {gnsf}") |
||||
|
||||
return gnsf |
||||
@ -0,0 +1,240 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com |
||||
|
||||
from casadi import Function, jacobian, SX, vertcat, horzcat |
||||
|
||||
from .determine_trivial_gnsf_transcription import determine_trivial_gnsf_transcription |
||||
from .detect_affine_terms_reduce_nonlinearity import ( |
||||
detect_affine_terms_reduce_nonlinearity, |
||||
) |
||||
from .reformulate_with_LOS import reformulate_with_LOS |
||||
from .reformulate_with_invertible_E_mat import reformulate_with_invertible_E_mat |
||||
from .structure_detection_print_summary import structure_detection_print_summary |
||||
from .check_reformulation import check_reformulation |
||||
|
||||
|
||||
def detect_gnsf_structure(acados_ocp, transcribe_opts=None): |
||||
|
||||
## Description |
||||
# This function takes a CasADi implicit ODE or index-1 DAE model "model" |
||||
# consisting of a CasADi expression f_impl in the symbolic CasADi |
||||
# variables x, xdot, u, z, (and possibly parameters p), which are also part |
||||
# of the model, as well as a model name. |
||||
# It will create a struct "gnsf" containing all information needed to use |
||||
# it with the gnsf integrator in acados. |
||||
# Additionally it will create the struct "reordered_model" which contains |
||||
# the permuted state vector and permuted f_impl, in which additionally some |
||||
# functions, which were made part of the linear output system of the gnsf, |
||||
# have changed signs. |
||||
|
||||
# Options: transcribe_opts is a Matlab struct consisting of booleans: |
||||
# print_info: if extensive information on how the model is processed |
||||
# is printed to the console. |
||||
# generate_gnsf_model: if the neccessary C functions to simulate the gnsf |
||||
# model with the acados implementation of the GNSF exploiting |
||||
# integrator should be generated. |
||||
# generate_gnsf_model: if the neccessary C functions to simulate the |
||||
# reordered model with the acados implementation of the IRK |
||||
# integrator should be generated. |
||||
# check_E_invertibility: if the transcription method should check if the |
||||
# assumption that the main blocks of the matrix gnsf.E are invertible |
||||
# holds. If not, the method will try to reformulate the gnsf model |
||||
# with a different model, such that the assumption holds. |
||||
|
||||
# acados_root_dir = getenv('ACADOS_INSTALL_DIR') |
||||
|
||||
## load transcribe_opts |
||||
if transcribe_opts is None: |
||||
print("WARNING: GNSF structure detection called without transcribe_opts") |
||||
print(" using default settings") |
||||
print("") |
||||
transcribe_opts = dict() |
||||
|
||||
if "print_info" in transcribe_opts: |
||||
print_info = transcribe_opts["print_info"] |
||||
else: |
||||
print_info = 1 |
||||
print("print_info option was not set - default is true") |
||||
|
||||
if "detect_LOS" in transcribe_opts: |
||||
detect_LOS = transcribe_opts["detect_LOS"] |
||||
else: |
||||
detect_LOS = 1 |
||||
if print_info: |
||||
print("detect_LOS option was not set - default is true") |
||||
|
||||
if "check_E_invertibility" in transcribe_opts: |
||||
check_E_invertibility = transcribe_opts["check_E_invertibility"] |
||||
else: |
||||
check_E_invertibility = 1 |
||||
if print_info: |
||||
print("check_E_invertibility option was not set - default is true") |
||||
|
||||
## Reformulate implicit index-1 DAE into GNSF form |
||||
# (Generalized nonlinear static feedback) |
||||
gnsf = determine_trivial_gnsf_transcription(acados_ocp, print_info) |
||||
gnsf = detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info) |
||||
|
||||
if detect_LOS: |
||||
gnsf = reformulate_with_LOS(acados_ocp, gnsf, print_info) |
||||
|
||||
if check_E_invertibility: |
||||
gnsf = reformulate_with_invertible_E_mat(gnsf, acados_ocp, print_info) |
||||
|
||||
# detect purely linear model |
||||
if gnsf["nx1"] == 0 and gnsf["nz1"] == 0 and gnsf["nontrivial_f_LO"] == 0: |
||||
gnsf["purely_linear"] = 1 |
||||
else: |
||||
gnsf["purely_linear"] = 0 |
||||
|
||||
structure_detection_print_summary(gnsf, acados_ocp) |
||||
check_reformulation(acados_ocp.model, gnsf, print_info) |
||||
|
||||
## copy relevant fields from gnsf to model |
||||
acados_ocp.model.get_matrices_fun = Function() |
||||
dummy = acados_ocp.model.x[0] |
||||
model_name = acados_ocp.model.name |
||||
|
||||
get_matrices_fun = Function( |
||||
f"{model_name}_gnsf_get_matrices_fun", |
||||
[dummy], |
||||
[ |
||||
gnsf["A"], |
||||
gnsf["B"], |
||||
gnsf["C"], |
||||
gnsf["E"], |
||||
gnsf["L_x"], |
||||
gnsf["L_xdot"], |
||||
gnsf["L_z"], |
||||
gnsf["L_u"], |
||||
gnsf["A_LO"], |
||||
gnsf["c"], |
||||
gnsf["E_LO"], |
||||
gnsf["B_LO"], |
||||
gnsf["nontrivial_f_LO"], |
||||
gnsf["purely_linear"], |
||||
gnsf["ipiv_x"] + 1, |
||||
gnsf["ipiv_z"] + 1, |
||||
gnsf["c_LO"], |
||||
], |
||||
) |
||||
|
||||
phi = gnsf["phi_expr"] |
||||
y = gnsf["y"] |
||||
uhat = gnsf["uhat"] |
||||
p = gnsf["p"] |
||||
|
||||
jac_phi_y = jacobian(phi, y) |
||||
jac_phi_uhat = jacobian(phi, uhat) |
||||
|
||||
phi_fun = Function(f"{model_name}_gnsf_phi_fun", [y, uhat, p], [phi]) |
||||
acados_ocp.model.phi_fun = phi_fun |
||||
acados_ocp.model.phi_fun_jac_y = Function( |
||||
f"{model_name}_gnsf_phi_fun_jac_y", [y, uhat, p], [phi, jac_phi_y] |
||||
) |
||||
acados_ocp.model.phi_jac_y_uhat = Function( |
||||
f"{model_name}_gnsf_phi_jac_y_uhat", [y, uhat, p], [jac_phi_y, jac_phi_uhat] |
||||
) |
||||
|
||||
x1 = acados_ocp.model.x[gnsf["idx_perm_x"][: gnsf["nx1"]]] |
||||
x1dot = acados_ocp.model.xdot[gnsf["idx_perm_x"][: gnsf["nx1"]]] |
||||
if gnsf["nz1"] > 0: |
||||
z1 = acados_ocp.model.z[gnsf["idx_perm_z"][: gnsf["nz1"]]] |
||||
else: |
||||
z1 = SX.sym("z1", 0, 0) |
||||
f_lo = gnsf["f_lo_expr"] |
||||
u = acados_ocp.model.u |
||||
acados_ocp.model.f_lo_fun_jac_x1k1uz = Function( |
||||
f"{model_name}_gnsf_f_lo_fun_jac_x1k1uz", |
||||
[x1, x1dot, z1, u, p], |
||||
[ |
||||
f_lo, |
||||
horzcat( |
||||
jacobian(f_lo, x1), |
||||
jacobian(f_lo, x1dot), |
||||
jacobian(f_lo, u), |
||||
jacobian(f_lo, z1), |
||||
), |
||||
], |
||||
) |
||||
|
||||
acados_ocp.model.get_matrices_fun = get_matrices_fun |
||||
|
||||
size_gnsf_A = gnsf["A"].shape |
||||
acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1] |
||||
acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] |
||||
acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1)) |
||||
acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0)) |
||||
acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0)) |
||||
|
||||
# # dim |
||||
# model['dim_gnsf_nx1'] = gnsf['nx1'] |
||||
# model['dim_gnsf_nx2'] = gnsf['nx2'] |
||||
# model['dim_gnsf_nz1'] = gnsf['nz1'] |
||||
# model['dim_gnsf_nz2'] = gnsf['nz2'] |
||||
# model['dim_gnsf_nuhat'] = gnsf['nuhat'] |
||||
# model['dim_gnsf_ny'] = gnsf['ny'] |
||||
# model['dim_gnsf_nout'] = gnsf['n_out'] |
||||
|
||||
# # sym |
||||
# model['sym_gnsf_y'] = gnsf['y'] |
||||
# model['sym_gnsf_uhat'] = gnsf['uhat'] |
||||
|
||||
# # data |
||||
# model['dyn_gnsf_A'] = gnsf['A'] |
||||
# model['dyn_gnsf_A_LO'] = gnsf['A_LO'] |
||||
# model['dyn_gnsf_B'] = gnsf['B'] |
||||
# model['dyn_gnsf_B_LO'] = gnsf['B_LO'] |
||||
# model['dyn_gnsf_E'] = gnsf['E'] |
||||
# model['dyn_gnsf_E_LO'] = gnsf['E_LO'] |
||||
# model['dyn_gnsf_C'] = gnsf['C'] |
||||
# model['dyn_gnsf_c'] = gnsf['c'] |
||||
# model['dyn_gnsf_c_LO'] = gnsf['c_LO'] |
||||
# model['dyn_gnsf_L_x'] = gnsf['L_x'] |
||||
# model['dyn_gnsf_L_xdot'] = gnsf['L_xdot'] |
||||
# model['dyn_gnsf_L_z'] = gnsf['L_z'] |
||||
# model['dyn_gnsf_L_u'] = gnsf['L_u'] |
||||
# model['dyn_gnsf_idx_perm_x'] = gnsf['idx_perm_x'] |
||||
# model['dyn_gnsf_ipiv_x'] = gnsf['ipiv_x'] |
||||
# model['dyn_gnsf_idx_perm_z'] = gnsf['idx_perm_z'] |
||||
# model['dyn_gnsf_ipiv_z'] = gnsf['ipiv_z'] |
||||
# model['dyn_gnsf_idx_perm_f'] = gnsf['idx_perm_f'] |
||||
# model['dyn_gnsf_ipiv_f'] = gnsf['ipiv_f'] |
||||
|
||||
# # flags |
||||
# model['dyn_gnsf_nontrivial_f_LO'] = gnsf['nontrivial_f_LO'] |
||||
# model['dyn_gnsf_purely_linear'] = gnsf['purely_linear'] |
||||
|
||||
# # casadi expr |
||||
# model['dyn_gnsf_expr_phi'] = gnsf['phi_expr'] |
||||
# model['dyn_gnsf_expr_f_lo'] = gnsf['f_lo_expr'] |
||||
|
||||
return acados_ocp |
||||
@ -0,0 +1,110 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com |
||||
|
||||
from casadi import * |
||||
from ..utils import casadi_length, is_empty |
||||
|
||||
|
||||
def determine_input_nonlinearity_function(gnsf): |
||||
|
||||
## Description |
||||
# this function takes a structure gnsf and updates the matrices L_x, |
||||
# L_xdot, L_z, L_u and CasADi vectors y, uhat of this structure as follows: |
||||
|
||||
# given a CasADi expression phi_expr, which may depend on the variables |
||||
# (x1, x1dot, z, u), this function determines a vector y (uhat) consisting |
||||
# of all components of (x1, x1dot, z) (respectively u) that enter phi_expr. |
||||
# Additionally matrices L_x, L_xdot, L_z, L_u are determined such that |
||||
# y = L_x * x + L_xdot * xdot + L_z * z |
||||
# uhat = L_u * u |
||||
# Furthermore the dimensions ny, nuhat, n_out are updated |
||||
|
||||
## y |
||||
y = SX.sym('y', 0, 0) |
||||
# components of x1 |
||||
for ii in range(gnsf["nx1"]): |
||||
if which_depends(gnsf["phi_expr"], gnsf["x"][ii])[0]: |
||||
y = vertcat(y, gnsf["x"][ii]) |
||||
# else: |
||||
# x[ii] is not part of y |
||||
# components of x1dot |
||||
for ii in range(gnsf["nx1"]): |
||||
if which_depends(gnsf["phi_expr"], gnsf["xdot"][ii])[0]: |
||||
print(gnsf["phi_expr"], "depends on", gnsf["xdot"][ii]) |
||||
y = vertcat(y, gnsf["xdot"][ii]) |
||||
# else: |
||||
# xdot[ii] is not part of y |
||||
# components of z |
||||
for ii in range(gnsf["nz1"]): |
||||
if which_depends(gnsf["phi_expr"], gnsf["z"][ii])[0]: |
||||
y = vertcat(y, gnsf["z"][ii]) |
||||
# else: |
||||
# z[ii] is not part of y |
||||
## uhat |
||||
uhat = SX.sym('uhat', 0, 0) |
||||
# components of u |
||||
for ii in range(gnsf["nu"]): |
||||
if which_depends(gnsf["phi_expr"], gnsf["u"][ii])[0]: |
||||
uhat = vertcat(uhat, gnsf["u"][ii]) |
||||
# else: |
||||
# u[ii] is not part of uhat |
||||
## generate gnsf['phi_expr_fun'] |
||||
# linear input matrices |
||||
if is_empty(y): |
||||
gnsf["L_x"] = [] |
||||
gnsf["L_xdot"] = [] |
||||
gnsf["L_u"] = [] |
||||
gnsf["L_z"] = [] |
||||
else: |
||||
dummy = SX.sym("dummy_input", 0) |
||||
L_x_fun = Function( |
||||
"L_x_fun", [dummy], [jacobian(y, gnsf["x"][range(gnsf["nx1"])])] |
||||
) |
||||
L_xdot_fun = Function( |
||||
"L_xdot_fun", [dummy], [jacobian(y, gnsf["xdot"][range(gnsf["nx1"])])] |
||||
) |
||||
L_z_fun = Function( |
||||
"L_z_fun", [dummy], [jacobian(y, gnsf["z"][range(gnsf["nz1"])])] |
||||
) |
||||
L_u_fun = Function("L_u_fun", [dummy], [jacobian(uhat, gnsf["u"])]) |
||||
|
||||
gnsf["L_x"] = L_x_fun(0).full() |
||||
gnsf["L_xdot"] = L_xdot_fun(0).full() |
||||
gnsf["L_u"] = L_u_fun(0).full() |
||||
gnsf["L_z"] = L_z_fun(0).full() |
||||
gnsf["y"] = y |
||||
gnsf["uhat"] = uhat |
||||
|
||||
gnsf["ny"] = casadi_length(y) |
||||
gnsf["nuhat"] = casadi_length(uhat) |
||||
gnsf["n_out"] = casadi_length(gnsf["phi_expr"]) |
||||
|
||||
return gnsf |
||||
@ -0,0 +1,155 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
|
||||
from casadi import * |
||||
import numpy as np |
||||
from ..utils import casadi_length, idx_perm_to_ipiv |
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function |
||||
from .check_reformulation import check_reformulation |
||||
|
||||
|
||||
def determine_trivial_gnsf_transcription(acados_ocp, print_info): |
||||
## Description |
||||
# this function takes a model of an implicit ODE/ index-1 DAE and sets up |
||||
# an equivalent model in the GNSF structure, with empty linear output |
||||
# system and trivial model matrices, i.e. A, B, E, c are zeros, and C is |
||||
# eye. - no structure is exploited |
||||
|
||||
model = acados_ocp.model |
||||
# initial print |
||||
print("*****************************************************************") |
||||
print(" ") |
||||
print(f"****** Restructuring {model.name} model ***********") |
||||
print(" ") |
||||
print("*****************************************************************") |
||||
|
||||
# load model |
||||
f_impl_expr = model.f_impl_expr |
||||
|
||||
model_name_prefix = model.name |
||||
|
||||
# x |
||||
x = model.x |
||||
nx = acados_ocp.dims.nx |
||||
# check type |
||||
if isinstance(x[0], SX): |
||||
isSX = True |
||||
else: |
||||
print("GNSF detection only works for SX CasADi type!!!") |
||||
import pdb |
||||
|
||||
pdb.set_trace() |
||||
# xdot |
||||
xdot = model.xdot |
||||
# u |
||||
nu = acados_ocp.dims.nu |
||||
if nu == 0: |
||||
u = SX.sym("u", 0, 0) |
||||
else: |
||||
u = model.u |
||||
|
||||
nz = acados_ocp.dims.nz |
||||
if nz == 0: |
||||
z = SX.sym("z", 0, 0) |
||||
else: |
||||
z = model.z |
||||
|
||||
p = model.p |
||||
nparam = acados_ocp.dims.np |
||||
|
||||
# avoid SX of size 0x1 |
||||
if casadi_length(u) == 0: |
||||
u = SX.sym("u", 0, 0) |
||||
nu = 0 |
||||
## initialize gnsf struct |
||||
# dimensions |
||||
gnsf = {"nx": nx, "nu": nu, "nz": nz, "np": nparam} |
||||
gnsf["nx1"] = nx |
||||
gnsf["nx2"] = 0 |
||||
gnsf["nz1"] = nz |
||||
gnsf["nz2"] = 0 |
||||
gnsf["nuhat"] = nu |
||||
gnsf["ny"] = 2 * nx + nz |
||||
|
||||
gnsf["phi_expr"] = f_impl_expr |
||||
gnsf["A"] = np.zeros((nx + nz, nx)) |
||||
gnsf["B"] = np.zeros((nx + nz, nu)) |
||||
gnsf["E"] = np.zeros((nx + nz, nx + nz)) |
||||
gnsf["c"] = np.zeros((nx + nz, 1)) |
||||
gnsf["C"] = np.eye(nx + nz) |
||||
gnsf["name"] = model_name_prefix |
||||
|
||||
gnsf["x"] = x |
||||
gnsf["xdot"] = xdot |
||||
gnsf["z"] = z |
||||
gnsf["u"] = u |
||||
gnsf["p"] = p |
||||
|
||||
gnsf = determine_input_nonlinearity_function(gnsf) |
||||
|
||||
gnsf["A_LO"] = [] |
||||
gnsf["E_LO"] = [] |
||||
gnsf["B_LO"] = [] |
||||
gnsf["c_LO"] = [] |
||||
gnsf["f_lo_expr"] = [] |
||||
|
||||
# permutation |
||||
gnsf["idx_perm_x"] = range(nx) # matlab-style) |
||||
gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"]) # blasfeo-style |
||||
gnsf["idx_perm_z"] = range(nz) |
||||
gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"]) |
||||
gnsf["idx_perm_f"] = range((nx + nz)) |
||||
gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"]) |
||||
|
||||
gnsf["nontrivial_f_LO"] = 0 |
||||
|
||||
check_reformulation(model, gnsf, print_info) |
||||
if print_info: |
||||
print(f"Success: Set up equivalent GNSF model with trivial matrices") |
||||
print(" ") |
||||
if print_info: |
||||
print( |
||||
"-----------------------------------------------------------------------------------" |
||||
) |
||||
print(" ") |
||||
print( |
||||
"reduced input ny of phi from ", |
||||
str(2 * nx + nz), |
||||
" to ", |
||||
str(gnsf["ny"]), |
||||
) |
||||
print( |
||||
"reduced input nuhat of phi from ", str(nu), " to ", str(gnsf["nuhat"]) |
||||
) |
||||
print(" ") |
||||
print( |
||||
"-----------------------------------------------------------------------------------" |
||||
) |
||||
return gnsf |
||||
@ -0,0 +1,43 @@ |
||||
# matlab to python |
||||
|
||||
% -> # |
||||
|
||||
; -> |
||||
|
||||
from casadi import * |
||||
-> |
||||
from casadi import * |
||||
|
||||
|
||||
print\('(.*)'\) |
||||
print('$1') |
||||
|
||||
print\(\['(.*)'\]\) |
||||
print(f'$1') |
||||
|
||||
keyboard |
||||
import pdb; pdb.set_trace() |
||||
|
||||
|
||||
range((([^))]*)) |
||||
range($1) |
||||
|
||||
\s*end |
||||
-> |
||||
nothing |
||||
|
||||
|
||||
if (.*) |
||||
if $1: |
||||
|
||||
else |
||||
else: |
||||
|
||||
num2str |
||||
str |
||||
|
||||
for ([a-z_]*) = |
||||
for $1 in |
||||
|
||||
length\( |
||||
len( |
||||
@ -0,0 +1,394 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com |
||||
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function |
||||
from .check_reformulation import check_reformulation |
||||
from casadi import * |
||||
from ..utils import casadi_length, idx_perm_to_ipiv, is_empty |
||||
|
||||
|
||||
def reformulate_with_LOS(acados_ocp, gnsf, print_info): |
||||
|
||||
## Description: |
||||
# This function takes an intitial transcription of the implicit ODE model |
||||
# "model" into "gnsf" and reformulates "gnsf" with a linear output system |
||||
# (LOS), containing as many states of the model as possible. |
||||
# Therefore it might be that the state vector and the implicit function |
||||
# vector have to be reordered. This reordered model is part of the output, |
||||
# namely reordered_model. |
||||
|
||||
## import CasADi and load models |
||||
model = acados_ocp.model |
||||
|
||||
# symbolics |
||||
x = gnsf["x"] |
||||
xdot = gnsf["xdot"] |
||||
u = gnsf["u"] |
||||
z = gnsf["z"] |
||||
|
||||
# dimensions |
||||
nx = gnsf["nx"] |
||||
nz = gnsf["nz"] |
||||
|
||||
# get model matrices |
||||
A = gnsf["A"] |
||||
B = gnsf["B"] |
||||
C = gnsf["C"] |
||||
E = gnsf["E"] |
||||
c = gnsf["c"] |
||||
|
||||
A_LO = gnsf["A_LO"] |
||||
|
||||
y = gnsf["y"] |
||||
|
||||
phi_old = gnsf["phi_expr"] |
||||
|
||||
if print_info: |
||||
print(" ") |
||||
print("=================================================================") |
||||
print(" ") |
||||
print("================ Detect Linear Output System ===============") |
||||
print(" ") |
||||
print("=================================================================") |
||||
print(" ") |
||||
## build initial I_x1 and I_x2_candidates |
||||
# I_xrange( all components of x for which either xii or xdot_ii enters y): |
||||
# I_LOS_candidates: the remaining components |
||||
|
||||
I_nsf_components = set() |
||||
I_LOS_candidates = set() |
||||
|
||||
if gnsf["ny"] > 0: |
||||
for ii in range(nx): |
||||
if which_depends(y, x[ii])[0] or which_depends(y, xdot[ii])[0]: |
||||
# i.e. xii or xiidot are part of y, and enter phi_expr |
||||
if print_info: |
||||
print(f"x_{ii} is part of x1") |
||||
I_nsf_components = set.union(I_nsf_components, set([ii])) |
||||
else: |
||||
# i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr |
||||
I_LOS_candidates = set.union(I_LOS_candidates, set([ii])) |
||||
if print_info: |
||||
print(" ") |
||||
for ii in range(nz): |
||||
if which_depends(y, z[ii])[0]: |
||||
# i.e. xii or xiidot are part of y, and enter phi_expr |
||||
if print_info: |
||||
print(f"z_{ii} is part of x1") |
||||
I_nsf_components = set.union(I_nsf_components, set([ii + nx])) |
||||
else: |
||||
# i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr |
||||
I_LOS_candidates = set.union(I_LOS_candidates, set([ii + nx])) |
||||
else: |
||||
I_LOS_candidates = set(range((nx + nz))) |
||||
if print_info: |
||||
print(" ") |
||||
print(f"I_LOS_candidates {I_LOS_candidates}") |
||||
new_nsf_components = I_nsf_components |
||||
I_nsf_eq = set([]) |
||||
unsorted_dyn = set(range(nx + nz)) |
||||
xdot_z = vertcat(xdot, z) |
||||
|
||||
## determine components of Linear Output System |
||||
# determine maximal index set I_x2 |
||||
# such that the components x(I_x2) can be written as a LOS |
||||
Eq_map = [] |
||||
while True: |
||||
## find equations corresponding to new_nsf_components |
||||
for ii in new_nsf_components: |
||||
current_var = xdot_z[ii] |
||||
var_name = current_var.name |
||||
|
||||
# print( unsorted_dyn) |
||||
# print("np.nonzero(E[:,ii])[0]",np.nonzero(E[:,ii])[0]) |
||||
I_eq = set.intersection(set(np.nonzero(E[:, ii])[0]), unsorted_dyn) |
||||
if len(I_eq) == 1: |
||||
i_eq = I_eq.pop() |
||||
if print_info: |
||||
print(f"component {i_eq} is associated with state {ii}") |
||||
elif len(I_eq) > 1: # x_ii_dot occurs in more than 1 eq linearly |
||||
# find the equation with least linear dependencies on |
||||
# I_LOS_cancidates |
||||
number_of_eq = 0 |
||||
candidate_dependencies = np.zeros(len(I_eq), 1) |
||||
I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx))) |
||||
for eq in I_eq: |
||||
depending_candidates = set.union( |
||||
np.nonzero(E[eq, I_LOS_candidates])[0], |
||||
np.nonzero(A[eq, I_x2_candidates])[0], |
||||
) |
||||
candidate_dependencies[number_of_eq] = +len(depending_candidates) |
||||
number_of_eq += 1 |
||||
number_of_eq = np.argmin(candidate_dependencies) |
||||
i_eq = I_eq[number_of_eq] |
||||
else: ## x_ii_dot does not occur linearly in any of the unsorted dynamics |
||||
for j in unsorted_dyn: |
||||
phi_eq_j = gnsf["phi_expr"][np.nonzero(C[j, :])[0]] |
||||
if which_depends(phi_eq_j, xdot_z(ii))[0]: |
||||
I_eq = set.union(I_eq, j) |
||||
if is_empty(I_eq): |
||||
I_eq = unsorted_dyn |
||||
# find the equation with least linear dependencies on I_LOS_cancidates |
||||
number_of_eq = 0 |
||||
candidate_dependencies = np.zeros(len(I_eq), 1) |
||||
I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx))) |
||||
for eq in I_eq: |
||||
depending_candidates = set.union( |
||||
np.nonzero(E[eq, I_LOS_candidates])[0], |
||||
np.nonzero(A[eq, I_x2_candidates])[0], |
||||
) |
||||
candidate_dependencies[number_of_eq] = +len(depending_candidates) |
||||
number_of_eq += 1 |
||||
number_of_eq = np.argmin(candidate_dependencies) |
||||
i_eq = I_eq[number_of_eq] |
||||
## add 1 * [xdot,z](ii) to both sides of i_eq |
||||
if print_info: |
||||
print( |
||||
"adding 1 * ", |
||||
var_name, |
||||
" to both sides of equation ", |
||||
i_eq, |
||||
".", |
||||
) |
||||
gnsf["E"][i_eq, ii] = 1 |
||||
i_phi = np.nonzero(gnsf["C"][i_eq, :]) |
||||
if is_empty(i_phi): |
||||
i_phi = len(gnsf["phi_expr"]) + 1 |
||||
gnsf["C"][i_eq, i_phi] = 1 # add column to C with 1 entry |
||||
gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], 0) |
||||
gnsf["phi_expr"][i_phi] = ( |
||||
gnsf["phi_expr"](i_phi) |
||||
+ gnsf["E"][i_eq, ii] / gnsf["C"][i_eq, i_phi] * xdot_z[ii] |
||||
) |
||||
if print_info: |
||||
print( |
||||
"detected equation ", |
||||
i_eq, |
||||
" to correspond to variable ", |
||||
var_name, |
||||
) |
||||
I_nsf_eq = set.union(I_nsf_eq, {i_eq}) |
||||
# remove i_eq from unsorted_dyn |
||||
unsorted_dyn.remove(i_eq) |
||||
Eq_map.append([ii, i_eq]) |
||||
|
||||
## add components to I_x1 |
||||
for eq in I_nsf_eq: |
||||
I_linear_dependence = set.union( |
||||
set(np.nonzero(A[eq, :])[0]), set(np.nonzero(E[eq, :])[0]) |
||||
) |
||||
I_nsf_components = set.union(I_linear_dependence, I_nsf_components) |
||||
# I_nsf_components = I_nsf_components[:] |
||||
|
||||
new_nsf_components = set.intersection(I_LOS_candidates, I_nsf_components) |
||||
if is_empty(new_nsf_components): |
||||
if print_info: |
||||
print("new_nsf_components is empty") |
||||
break |
||||
# remove new_nsf_components from candidates |
||||
I_LOS_candidates = set.difference(I_LOS_candidates, new_nsf_components) |
||||
if not is_empty(Eq_map): |
||||
# [~, new_eq_order] = sort(Eq_map(1,:)) |
||||
# I_nsf_eq = Eq_map(2, new_eq_order ) |
||||
for count, m in enumerate(Eq_map): |
||||
m.append(count) |
||||
sorted(Eq_map, key=lambda x: x[1]) |
||||
new_eq_order = [m[2] for m in Eq_map] |
||||
Eq_map = [Eq_map[i] for i in new_eq_order] |
||||
I_nsf_eq = [m[1] for m in Eq_map] |
||||
|
||||
else: |
||||
I_nsf_eq = [] |
||||
|
||||
I_LOS_components = I_LOS_candidates |
||||
I_LOS_eq = sorted(set.difference(set(range(nx + nz)), I_nsf_eq)) |
||||
I_nsf_eq = sorted(I_nsf_eq) |
||||
|
||||
I_x1 = set.intersection(I_nsf_components, set(range(nx))) |
||||
I_z1 = set.intersection(I_nsf_components, set(range(nx, nx + nz))) |
||||
I_z1 = set([i - nx for i in I_z1]) |
||||
|
||||
I_x2 = set.intersection(I_LOS_components, set(range(nx))) |
||||
I_z2 = set.intersection(I_LOS_components, set(range(nx, nx + nz))) |
||||
I_z2 = set([i - nx for i in I_z2]) |
||||
|
||||
if print_info: |
||||
print(f"I_x1 {I_x1}, I_x2 {I_x2}") |
||||
|
||||
## permute x, xdot |
||||
if is_empty(I_x1): |
||||
x1 = [] |
||||
x1dot = [] |
||||
else: |
||||
x1 = x[list(I_x1)] |
||||
x1dot = xdot[list(I_x1)] |
||||
if is_empty(I_x2): |
||||
x2 = [] |
||||
x2dot = [] |
||||
else: |
||||
x2 = x[list(I_x2)] |
||||
x2dot = xdot[list(I_x2)] |
||||
if is_empty(I_z1): |
||||
z1 = [] |
||||
else: |
||||
z1 = z(I_z1) |
||||
if is_empty(I_z2): |
||||
z2 = [] |
||||
else: |
||||
z2 = z[list(I_z2)] |
||||
|
||||
I_x1 = sorted(I_x1) |
||||
I_x2 = sorted(I_x2) |
||||
I_z1 = sorted(I_z1) |
||||
I_z2 = sorted(I_z2) |
||||
gnsf["xdot"] = vertcat(x1dot, x2dot) |
||||
gnsf["x"] = vertcat(x1, x2) |
||||
gnsf["z"] = vertcat(z1, z2) |
||||
gnsf["nx1"] = len(I_x1) |
||||
gnsf["nx2"] = len(I_x2) |
||||
gnsf["nz1"] = len(I_z1) |
||||
gnsf["nz2"] = len(I_z2) |
||||
|
||||
# store permutations |
||||
gnsf["idx_perm_x"] = I_x1 + I_x2 |
||||
gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"]) |
||||
gnsf["idx_perm_z"] = I_z1 + I_z2 |
||||
gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"]) |
||||
gnsf["idx_perm_f"] = I_nsf_eq + I_LOS_eq |
||||
gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"]) |
||||
|
||||
f_LO = SX.sym("f_LO", 0, 0) |
||||
|
||||
## rewrite I_LOS_eq as LOS |
||||
if gnsf["n_out"] == 0: |
||||
C_phi = np.zeros(gnsf["nx"] + gnsf["nz"], 1) |
||||
else: |
||||
C_phi = C @ phi_old |
||||
if gnsf["nx1"] == 0: |
||||
Ax1 = np.zeros(gnsf["nx"] + gnsf["nz"], 1) |
||||
else: |
||||
Ax1 = A[:, sorted(I_x1)] @ x1 |
||||
if gnsf["nx1"] + gnsf["nz1"] == 0: |
||||
lhs_nsf = np.zeros(gnsf["nx"] + gnsf["nz"], 1) |
||||
else: |
||||
lhs_nsf = E[:, sorted(I_nsf_components)] @ vertcat(x1, z1) |
||||
n_LO = len(I_LOS_eq) |
||||
B_LO = np.zeros((n_LO, gnsf["nu"])) |
||||
A_LO = np.zeros((gnsf["nx2"] + gnsf["nz2"], gnsf["nx2"])) |
||||
E_LO = np.zeros((n_LO, n_LO)) |
||||
c_LO = np.zeros((n_LO, 1)) |
||||
|
||||
I_LOS_eq = list(I_LOS_eq) |
||||
for eq in I_LOS_eq: |
||||
i_LO = I_LOS_eq.index(eq) |
||||
f_LO = vertcat(f_LO, Ax1[eq] + C_phi[eq] - lhs_nsf[eq]) |
||||
print(f"eq {eq} I_LOS_components {I_LOS_components}, i_LO {i_LO}, f_LO {f_LO}") |
||||
E_LO[i_LO, :] = E[eq, sorted(I_LOS_components)] |
||||
A_LO[i_LO, :] = A[eq, I_x2] |
||||
c_LO[i_LO, :] = c[eq] |
||||
B_LO[i_LO, :] = B[eq, :] |
||||
if casadi_length(f_LO) == 0: |
||||
f_LO = SX.zeros((gnsf["nx2"] + gnsf["nz2"], 1)) |
||||
f_LO = simplify(f_LO) |
||||
gnsf["A_LO"] = A_LO |
||||
gnsf["E_LO"] = E_LO |
||||
gnsf["B_LO"] = B_LO |
||||
gnsf["c_LO"] = c_LO |
||||
gnsf["f_lo_expr"] = f_LO |
||||
|
||||
## remove I_LOS_eq from NSF type system |
||||
gnsf["A"] = gnsf["A"][np.ix_(sorted(I_nsf_eq), sorted(I_x1))] |
||||
gnsf["B"] = gnsf["B"][sorted(I_nsf_eq), :] |
||||
gnsf["C"] = gnsf["C"][sorted(I_nsf_eq), :] |
||||
gnsf["E"] = gnsf["E"][np.ix_(sorted(I_nsf_eq), sorted(I_nsf_components))] |
||||
gnsf["c"] = gnsf["c"][sorted(I_nsf_eq), :] |
||||
|
||||
## reduce phi, C |
||||
I_nonzero = [] |
||||
for ii in range(gnsf["C"].shape[1]): # n_colums of C: |
||||
print(f"ii {ii}") |
||||
if not all(gnsf["C"][:, ii] == 0): # if column ~= 0 |
||||
I_nonzero.append(ii) |
||||
gnsf["C"] = gnsf["C"][:, I_nonzero] |
||||
gnsf["phi_expr"] = gnsf["phi_expr"][I_nonzero] |
||||
|
||||
gnsf = determine_input_nonlinearity_function(gnsf) |
||||
|
||||
check_reformulation(model, gnsf, print_info) |
||||
|
||||
gnsf["nontrivial_f_LO"] = 0 |
||||
if not is_empty(gnsf["f_lo_expr"]): |
||||
for ii in range(casadi_length(gnsf["f_lo_expr"])): |
||||
fii = gnsf["f_lo_expr"][ii] |
||||
if not fii.is_zero(): |
||||
gnsf["nontrivial_f_LO"] = 1 |
||||
if not gnsf["nontrivial_f_LO"] and print_info: |
||||
print("f_LO is fully trivial (== 0)") |
||||
check_reformulation(model, gnsf, print_info) |
||||
|
||||
if print_info: |
||||
print("") |
||||
print( |
||||
"---------------------------------------------------------------------------------" |
||||
) |
||||
print( |
||||
"------------- Success: Linear Output System (LOS) detected ----------------------" |
||||
) |
||||
print( |
||||
"---------------------------------------------------------------------------------" |
||||
) |
||||
print("") |
||||
print( |
||||
"==>> moved ", |
||||
gnsf["nx2"], |
||||
"differential states and ", |
||||
gnsf["nz2"], |
||||
" algebraic variables to the Linear Output System", |
||||
) |
||||
print( |
||||
"==>> recuced output dimension of phi from ", |
||||
casadi_length(phi_old), |
||||
" to ", |
||||
casadi_length(gnsf["phi_expr"]), |
||||
) |
||||
print(" ") |
||||
print("Matrices defining the LOS read as") |
||||
print(" ") |
||||
print("E_LO =") |
||||
print(gnsf["E_LO"]) |
||||
print("A_LO =") |
||||
print(gnsf["A_LO"]) |
||||
print("B_LO =") |
||||
print(gnsf["B_LO"]) |
||||
print("c_LO =") |
||||
print(gnsf["c_LO"]) |
||||
|
||||
return gnsf |
||||
@ -0,0 +1,167 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com |
||||
|
||||
from casadi import * |
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function |
||||
from .check_reformulation import check_reformulation |
||||
|
||||
|
||||
def reformulate_with_invertible_E_mat(gnsf, model, print_info): |
||||
## Description |
||||
# this function checks that the necessary condition to apply the gnsf |
||||
# structure exploiting integrator to a model, namely that the matrices E11, |
||||
# E22 are invertible holds. |
||||
# if this is not the case, it will make these matrices invertible and add: |
||||
# corresponding terms, to the term C * phi, such that the obtained model is |
||||
# still equivalent |
||||
|
||||
# check invertibility of E11, E22 and reformulate if needed: |
||||
ind_11 = range(gnsf["nx1"]) |
||||
ind_22 = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"]) |
||||
|
||||
if print_info: |
||||
print(" ") |
||||
print("----------------------------------------------------") |
||||
print("checking rank of E11 and E22") |
||||
print("----------------------------------------------------") |
||||
## check if E11, E22 are invertible: |
||||
z_check = False |
||||
if gnsf["nz1"] > 0: |
||||
z_check = ( |
||||
np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_22, ind_22)]) != gnsf["nz1"] |
||||
) |
||||
|
||||
if ( |
||||
np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_11, ind_11)]) != gnsf["nx1"] |
||||
or z_check |
||||
): |
||||
# print warning (always) |
||||
print(f"the rank of E11 or E22 is not full after the reformulation") |
||||
print("") |
||||
print( |
||||
f"the script will try to reformulate the model with an invertible matrix instead" |
||||
) |
||||
print( |
||||
f"NOTE: this feature is based on a heuristic, it should be used with care!!!" |
||||
) |
||||
|
||||
## load models |
||||
xdot = gnsf["xdot"] |
||||
z = gnsf["z"] |
||||
|
||||
# # GNSF |
||||
# get dimensions |
||||
nx1 = gnsf["nx1"] |
||||
x1dot = xdot[range(nx1)] |
||||
|
||||
k = vertcat(x1dot, z) |
||||
for i in [1, 2]: |
||||
if i == 1: |
||||
ind = range(gnsf["nx1"]) |
||||
else: |
||||
ind = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"]) |
||||
mat = gnsf["E"][np.ix_(ind, ind)] |
||||
import pdb |
||||
|
||||
pdb.set_trace() |
||||
while np.linalg.matrix_rank(mat) < len(ind): |
||||
# import pdb; pdb.set_trace() |
||||
if print_info: |
||||
print(" ") |
||||
print(f"the rank of E", str(i), str(i), " is not full") |
||||
print( |
||||
f"the algorithm will try to reformulate the model with an invertible matrix instead" |
||||
) |
||||
print( |
||||
f"NOTE: this feature is not super stable and might need more testing!!!!!!" |
||||
) |
||||
for sub_max in ind: |
||||
sub_ind = range(min(ind), sub_max) |
||||
# regard the submatrix mat(sub_ind, sub_ind) |
||||
sub_mat = gnsf["E"][sub_ind, sub_ind] |
||||
if np.linalg.matrix_rank(sub_mat) < len(sub_ind): |
||||
# reformulate the model by adding a 1 to last diagonal |
||||
# element and changing rhs respectively. |
||||
gnsf["E"][sub_max, sub_max] = gnsf["E"][sub_max, sub_max] + 1 |
||||
# this means adding the term 1 * k(sub_max) to the sub_max |
||||
# row of the l.h.s |
||||
if len(np.nonzero(gnsf["C"][sub_max, :])[0]) == 0: |
||||
# if isempty(find(gnsf['C'](sub_max,:), 1)): |
||||
# add new nonlinearity entry |
||||
gnsf["C"][sub_max, gnsf["n_out"] + 1] = 1 |
||||
gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], k[sub_max]) |
||||
else: |
||||
ind_f = np.nonzero(gnsf["C"][sub_max, :])[0] |
||||
if len(ind_f) != 1: |
||||
raise Exception("C is assumed to be a selection matrix") |
||||
else: |
||||
ind_f = ind_f[0] |
||||
# add term to corresponding nonlinearity entry |
||||
# note: herbey we assume that C is a selection matrix, |
||||
# i.e. gnsf['phi_expr'](ind_f) is only entering one equation |
||||
|
||||
gnsf["phi_expr"][ind_f] = ( |
||||
gnsf["phi_expr"][ind_f] |
||||
+ k[sub_max] / gnsf["C"][sub_max, ind_f] |
||||
) |
||||
gnsf = determine_input_nonlinearity_function(gnsf) |
||||
check_reformulation(model, gnsf, print_info) |
||||
print("successfully reformulated the model with invertible matrices E11, E22") |
||||
else: |
||||
if print_info: |
||||
print(" ") |
||||
print( |
||||
"the rank of both E11 and E22 is naturally full after the reformulation " |
||||
) |
||||
print("==> model reformulation finished") |
||||
print(" ") |
||||
if (gnsf['nx2'] > 0 or gnsf['nz2'] > 0) and det(gnsf["E_LO"]) == 0: |
||||
print( |
||||
"_______________________________________________________________________________________________________" |
||||
) |
||||
print(" ") |
||||
print("TAKE CARE ") |
||||
print("E_LO matrix is NOT regular after automatic transcription!") |
||||
print("->> this means the model CANNOT be used with the gnsf integrator") |
||||
print( |
||||
"->> it probably means that one entry (of xdot or z) that was moved to the linear output type system" |
||||
) |
||||
print(" does not appear in the model at all (zero column in E_LO)") |
||||
print(" OR: the columns of E_LO are linearly dependent ") |
||||
print(" ") |
||||
print( |
||||
" SOLUTIONs: a) go through your model & check equations the method wanted to move to LOS" |
||||
) |
||||
print(" b) deactivate the detect_LOS option") |
||||
print( |
||||
"_______________________________________________________________________________________________________" |
||||
) |
||||
return gnsf |
||||
@ -0,0 +1,174 @@ |
||||
# |
||||
# 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.; |
||||
# |
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com |
||||
|
||||
from casadi import n_nodes |
||||
import numpy as np |
||||
|
||||
|
||||
def structure_detection_print_summary(gnsf, acados_ocp): |
||||
|
||||
## Description |
||||
# this function prints the most important info after determining a GNSF |
||||
# reformulation of the implicit model "initial_model" into "gnsf", which is |
||||
# equivalent to the "reordered_model". |
||||
model = acados_ocp.model |
||||
# # GNSF |
||||
# get dimensions |
||||
nx = gnsf["nx"] |
||||
nu = gnsf["nu"] |
||||
nz = gnsf["nz"] |
||||
|
||||
nx1 = gnsf["nx1"] |
||||
nx2 = gnsf["nx2"] |
||||
|
||||
nz1 = gnsf["nz1"] |
||||
nz2 = gnsf["nz2"] |
||||
|
||||
# np = gnsf['np'] |
||||
n_out = gnsf["n_out"] |
||||
ny = gnsf["ny"] |
||||
nuhat = gnsf["nuhat"] |
||||
|
||||
# |
||||
f_impl_expr = model.f_impl_expr |
||||
n_nodes_initial = n_nodes(model.f_impl_expr) |
||||
# x_old = model.x |
||||
# f_impl_old = model.f_impl_expr |
||||
|
||||
x = gnsf["x"] |
||||
z = gnsf["z"] |
||||
|
||||
phi_current = gnsf["phi_expr"] |
||||
|
||||
## PRINT SUMMARY -- STRUCHTRE DETECTION |
||||
print(" ") |
||||
print( |
||||
"*********************************************************************************************" |
||||
) |
||||
print(" ") |
||||
print( |
||||
"****************** SUCCESS: GNSF STRUCTURE DETECTION COMPLETE !!! ***************" |
||||
) |
||||
print(" ") |
||||
print( |
||||
"*********************************************************************************************" |
||||
) |
||||
print(" ") |
||||
print( |
||||
f"========================= STRUCTURE DETECTION SUMMARY ====================================" |
||||
) |
||||
print(" ") |
||||
print("-------- Nonlinear Static Feedback type system --------") |
||||
print(" ") |
||||
print(" successfully transcribed dynamic system model into GNSF structure ") |
||||
print(" ") |
||||
print( |
||||
"reduced dimension of nonlinearity phi from ", |
||||
str(nx + nz), |
||||
" to ", |
||||
str(gnsf["n_out"]), |
||||
) |
||||
print(" ") |
||||
print( |
||||
"reduced input dimension of nonlinearity phi from ", |
||||
2 * nx + nz + nu, |
||||
" to ", |
||||
gnsf["ny"] + gnsf["nuhat"], |
||||
) |
||||
print(" ") |
||||
print(f"reduced number of nodes in CasADi expression of nonlinearity phi from {n_nodes_initial} to {n_nodes(phi_current)}\n") |
||||
print("----------- Linear Output System (LOS) ---------------") |
||||
if nx2 + nz2 > 0: |
||||
print(" ") |
||||
print(f"introduced Linear Output System of size ", str(nx2 + nz2)) |
||||
print(" ") |
||||
if nx2 > 0: |
||||
print("consisting of the states:") |
||||
print(" ") |
||||
print(x[range(nx1, nx)]) |
||||
print(" ") |
||||
if nz2 > 0: |
||||
print("and algebraic variables:") |
||||
print(" ") |
||||
print(z[range(nz1, nz)]) |
||||
print(" ") |
||||
if gnsf["purely_linear"] == 1: |
||||
print(" ") |
||||
print("Model is fully linear!") |
||||
print(" ") |
||||
if not all(gnsf["idx_perm_x"] == np.array(range(nx))): |
||||
print(" ") |
||||
print( |
||||
"--------------------------------------------------------------------------------------------------" |
||||
) |
||||
print( |
||||
"NOTE: permuted differential state vector x, such that x_gnsf = x(idx_perm_x) with idx_perm_x =" |
||||
) |
||||
print(" ") |
||||
print(gnsf["idx_perm_x"]) |
||||
if nz != 0 and not all(gnsf["idx_perm_z"] == np.array(range(nz))): |
||||
print(" ") |
||||
print( |
||||
"--------------------------------------------------------------------------------------------------" |
||||
) |
||||
print( |
||||
"NOTE: permuted algebraic state vector z, such that z_gnsf = z(idx_perm_z) with idx_perm_z =" |
||||
) |
||||
print(" ") |
||||
print(gnsf["idx_perm_z"]) |
||||
if not all(gnsf["idx_perm_f"] == np.array(range(nx + nz))): |
||||
print(" ") |
||||
print( |
||||
"--------------------------------------------------------------------------------------------------" |
||||
) |
||||
print( |
||||
"NOTE: permuted rhs expression vector f, such that f_gnsf = f(idx_perm_f) with idx_perm_f =" |
||||
) |
||||
print(" ") |
||||
print(gnsf["idx_perm_f"]) |
||||
## print GNSF dimensions |
||||
print( |
||||
"--------------------------------------------------------------------------------------------------------" |
||||
) |
||||
print(" ") |
||||
print("The dimensions of the GNSF reformulated model read as:") |
||||
print(" ") |
||||
# T_dim = table(nx, nu, nz, np, nx1, nz1, n_out, ny, nuhat) |
||||
# print( T_dim ) |
||||
print(f"nx ", {nx}) |
||||
print(f"nu ", {nu}) |
||||
print(f"nz ", {nz}) |
||||
# print(f"np ", {np}) |
||||
print(f"nx1 ", {nx1}) |
||||
print(f"nz1 ", {nz1}) |
||||
print(f"n_out ", {n_out}) |
||||
print(f"ny ", {ny}) |
||||
print(f"nuhat ", {nuhat}) |
||||
@ -0,0 +1,78 @@ |
||||
# 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.; |
||||
|
||||
from dataclasses import dataclass, field |
||||
import numpy as np |
||||
|
||||
|
||||
@dataclass |
||||
class ZoroDescription: |
||||
""" |
||||
Zero-Order Robust Optimization scheme. |
||||
|
||||
For advanced users. |
||||
""" |
||||
backoff_scaling_gamma: float = 1.0 |
||||
fdbk_K_mat: np.ndarray = None |
||||
unc_jac_G_mat: np.ndarray = None # default: an identity matrix |
||||
P0_mat: np.ndarray = None |
||||
W_mat: np.ndarray = None |
||||
idx_lbx_t: list = field(default_factory=list) |
||||
idx_ubx_t: list = field(default_factory=list) |
||||
idx_lbx_e_t: list = field(default_factory=list) |
||||
idx_ubx_e_t: list = field(default_factory=list) |
||||
idx_lbu_t: list = field(default_factory=list) |
||||
idx_ubu_t: list = field(default_factory=list) |
||||
idx_lg_t: list = field(default_factory=list) |
||||
idx_ug_t: list = field(default_factory=list) |
||||
idx_lg_e_t: list = field(default_factory=list) |
||||
idx_ug_e_t: list = field(default_factory=list) |
||||
idx_lh_t: list = field(default_factory=list) |
||||
idx_uh_t: list = field(default_factory=list) |
||||
idx_lh_e_t: list = field(default_factory=list) |
||||
idx_uh_e_t: list = field(default_factory=list) |
||||
|
||||
def process_zoro_description(zoro_description: ZoroDescription): |
||||
zoro_description.nw, _ = zoro_description.W_mat.shape |
||||
if zoro_description.unc_jac_G_mat is None: |
||||
zoro_description.unc_jac_G_mat = np.eye(zoro_description.nw) |
||||
zoro_description.nlbx_t = len(zoro_description.idx_lbx_t) |
||||
zoro_description.nubx_t = len(zoro_description.idx_ubx_t) |
||||
zoro_description.nlbx_e_t = len(zoro_description.idx_lbx_e_t) |
||||
zoro_description.nubx_e_t = len(zoro_description.idx_ubx_e_t) |
||||
zoro_description.nlbu_t = len(zoro_description.idx_lbu_t) |
||||
zoro_description.nubu_t = len(zoro_description.idx_ubu_t) |
||||
zoro_description.nlg_t = len(zoro_description.idx_lg_t) |
||||
zoro_description.nug_t = len(zoro_description.idx_ug_t) |
||||
zoro_description.nlg_e_t = len(zoro_description.idx_lg_e_t) |
||||
zoro_description.nug_e_t = len(zoro_description.idx_ug_e_t) |
||||
zoro_description.nlh_t = len(zoro_description.idx_lh_t) |
||||
zoro_description.nuh_t = len(zoro_description.idx_uh_t) |
||||
zoro_description.nlh_e_t = len(zoro_description.idx_lh_e_t) |
||||
zoro_description.nuh_e_t = len(zoro_description.idx_uh_e_t) |
||||
return zoro_description.__dict__ |
||||
@ -0,0 +1,110 @@ |
||||
/*
|
||||
* 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_DENSE_QP_DENSE_QP_DAQP_H_ |
||||
#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h" |
||||
|
||||
// daqp
|
||||
#include "daqp/include/types.h" |
||||
|
||||
// acados
|
||||
#include "acados/dense_qp/dense_qp_common.h" |
||||
#include "acados/utils/types.h" |
||||
|
||||
|
||||
typedef struct dense_qp_daqp_opts_ |
||||
{ |
||||
DAQPSettings* daqp_opts; |
||||
int warm_start; |
||||
} dense_qp_daqp_opts; |
||||
|
||||
|
||||
typedef struct dense_qp_daqp_memory_ |
||||
{ |
||||
double* lb_tmp; |
||||
double* ub_tmp; |
||||
int* idxb; |
||||
int* idxv_to_idxb; |
||||
int* idxs; |
||||
int* idxdaqp_to_idxs; |
||||
|
||||
double* Zl; |
||||
double* Zu; |
||||
double* zl; |
||||
double* zu; |
||||
double* d_ls; |
||||
double* d_us; |
||||
|
||||
double time_qp_solver_call; |
||||
int iter; |
||||
DAQPWorkspace * daqp_work; |
||||
|
||||
} dense_qp_daqp_memory; |
||||
|
||||
// opts
|
||||
acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims); |
||||
//
|
||||
void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory); |
||||
//
|
||||
void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_); |
||||
//
|
||||
void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_); |
||||
//
|
||||
// memory
|
||||
acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_); |
||||
//
|
||||
void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory); |
||||
//
|
||||
acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_); |
||||
//
|
||||
void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory); |
||||
//
|
||||
// functions
|
||||
int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); |
||||
//
|
||||
void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); |
||||
//
|
||||
void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); |
||||
//
|
||||
void dense_qp_daqp_config_initialize_default(void *config_); |
||||
|
||||
#ifdef __cplusplus |
||||
} /* extern "C" */ |
||||
#endif |
||||
|
||||
#endif // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
|
||||
@ -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_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue