sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
11848 Commits
113 Branches
85 Tags
6.2 GiB
Tag: Branch: Tree: c9c2ab9cc8
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'c9c2ab9cc8'
${ noResults }
openpilot_comma/third_party/acados/acados_template/casadi_function_generation.py

709 lines
22 KiB
Raw Normal View History Unescape

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>
2 years ago
#
# 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