Upstreamed and update acados (#23927)
* change acados version to finalized PR https://github.com/acados/acados/pull/791 * acados: update include and x86_64 build * acados: update pyextra * adapt mpc files to updated acados * acados buid: clear acados_template and include dir before updating * update acados again * acados MPC: only generate solver, building is done via scons * update acados to latest acados/master * c3 libs * c2 libs * acados scons files: add acados build script as a dependency * acados scons files: add libacados.so and ocp_nlp_interface.h, build script not in files_common * update acados * adapts long & lat MPC file * update acados reference commit to acados/master Co-authored-by: Comma Device <device@comma.ai>android9
parent
107156cd58
commit
d653dd843a
|
@ -5,6 +5,9 @@
|
|||
"acados_include_path": [
|
||||
"str"
|
||||
],
|
||||
"cython_include_dirs": [
|
||||
"str"
|
||||
],
|
||||
"model": {
|
||||
"name" : [
|
||||
"str"
|
||||
|
@ -23,7 +26,15 @@
|
|||
],
|
||||
"dyn_disc_fun" : [
|
||||
"str"
|
||||
]
|
||||
],
|
||||
"gnsf" : {
|
||||
"nontrivial_f_LO": [
|
||||
"int"
|
||||
],
|
||||
"purely_linear": [
|
||||
"int"
|
||||
]
|
||||
}
|
||||
},
|
||||
"parameter_values": [
|
||||
"ndarray",
|
||||
|
@ -693,6 +704,18 @@
|
|||
"alpha_reduction": [
|
||||
"float"
|
||||
],
|
||||
"line_search_use_sufficient_descent": [
|
||||
"int"
|
||||
],
|
||||
"globalization_use_SOC": [
|
||||
"int"
|
||||
],
|
||||
"full_step_dual": [
|
||||
"int"
|
||||
],
|
||||
"eps_sufficient_descent": [
|
||||
"float"
|
||||
],
|
||||
"sim_method_num_stages": [
|
||||
"ndarray",
|
||||
[
|
||||
|
|
|
@ -78,6 +78,13 @@ class AcadosModel():
|
|||
self.dyn_disc_fun_jac = None #: name of function discrete dyanamics + jacobian; Default: :code:`None`
|
||||
self.dyn_disc_fun = None #: name of function discrete dyanamics; Default: :code:`None`
|
||||
|
||||
# for GNSF models
|
||||
self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0}
|
||||
"""
|
||||
dictionary containing information on GNSF structure needed when rendering templates.
|
||||
Contains integers `nontrivial_f_LO`, `purely_linear`.
|
||||
"""
|
||||
|
||||
## for OCP
|
||||
# constraints
|
||||
self.con_h_expr = None #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
|
||||
|
|
|
@ -270,28 +270,28 @@ class AcadosOcpDims:
|
|||
|
||||
@nx.setter
|
||||
def nx(self, nx):
|
||||
if type(nx) == int and nx > 0:
|
||||
if isinstance(nx, int) and nx > 0:
|
||||
self.__nx = nx
|
||||
else:
|
||||
raise Exception('Invalid nx value, expected positive integer. Exiting.')
|
||||
|
||||
@nz.setter
|
||||
def nz(self, nz):
|
||||
if type(nz) == int and nz > -1:
|
||||
if isinstance(nz, int) and nz > -1:
|
||||
self.__nz = nz
|
||||
else:
|
||||
raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nu.setter
|
||||
def nu(self, nu):
|
||||
if type(nu) == int and nu > -1:
|
||||
if isinstance(nu, int) and nu > -1:
|
||||
self.__nu = nu
|
||||
else:
|
||||
raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@np.setter
|
||||
def np(self, np):
|
||||
if type(np) == int and np > -1:
|
||||
if isinstance(np, int) and np > -1:
|
||||
self.__np = np
|
||||
else:
|
||||
raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
|
||||
|
@ -312,49 +312,49 @@ class AcadosOcpDims:
|
|||
|
||||
@ny_e.setter
|
||||
def ny_e(self, ny_e):
|
||||
if type(ny_e) == int and ny_e > -1:
|
||||
if isinstance(ny_e, int) and ny_e > -1:
|
||||
self.__ny_e = ny_e
|
||||
else:
|
||||
raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nr.setter
|
||||
def nr(self, nr):
|
||||
if type(nr) == int and nr > -1:
|
||||
if isinstance(nr, int) and nr > -1:
|
||||
self.__nr = nr
|
||||
else:
|
||||
raise Exception('Invalid nr value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nr_e.setter
|
||||
def nr_e(self, nr_e):
|
||||
if type(nr_e) == int and nr_e > -1:
|
||||
if isinstance(nr_e, int) and nr_e > -1:
|
||||
self.__nr_e = nr_e
|
||||
else:
|
||||
raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nh.setter
|
||||
def nh(self, nh):
|
||||
if type(nh) == int and nh > -1:
|
||||
if isinstance(nh, int) and nh > -1:
|
||||
self.__nh = nh
|
||||
else:
|
||||
raise Exception('Invalid nh value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nh_e.setter
|
||||
def nh_e(self, nh_e):
|
||||
if type(nh_e) == int and nh_e > -1:
|
||||
if isinstance(nh_e, int) and nh_e > -1:
|
||||
self.__nh_e = nh_e
|
||||
else:
|
||||
raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nphi.setter
|
||||
def nphi(self, nphi):
|
||||
if type(nphi) == int and nphi > -1:
|
||||
if isinstance(nphi, int) and nphi > -1:
|
||||
self.__nphi = nphi
|
||||
else:
|
||||
raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nphi_e.setter
|
||||
def nphi_e(self, nphi_e):
|
||||
if type(nphi_e) == int and nphi_e > -1:
|
||||
if isinstance(nphi_e, int) and nphi_e > -1:
|
||||
self.__nphi_e = nphi_e
|
||||
else:
|
||||
raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.')
|
||||
|
@ -375,42 +375,42 @@ class AcadosOcpDims:
|
|||
|
||||
@nbx_0.setter
|
||||
def nbx_0(self, nbx_0):
|
||||
if type(nbx_0) == int and nbx_0 > -1:
|
||||
if isinstance(nbx_0, int) and nbx_0 > -1:
|
||||
self.__nbx_0 = nbx_0
|
||||
else:
|
||||
raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nbx_e.setter
|
||||
def nbx_e(self, nbx_e):
|
||||
if type(nbx_e) == int and nbx_e > -1:
|
||||
if isinstance(nbx_e, int) and nbx_e > -1:
|
||||
self.__nbx_e = nbx_e
|
||||
else:
|
||||
raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nbu.setter
|
||||
def nbu(self, nbu):
|
||||
if type(nbu) == int and nbu > -1:
|
||||
if isinstance(nbu, int) and nbu > -1:
|
||||
self.__nbu = nbu
|
||||
else:
|
||||
raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nsbx.setter
|
||||
def nsbx(self, nsbx):
|
||||
if type(nsbx) == int and nsbx > -1:
|
||||
if isinstance(nsbx, int) and nsbx > -1:
|
||||
self.__nsbx = nsbx
|
||||
else:
|
||||
raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nsbx_e.setter
|
||||
def nsbx_e(self, nsbx_e):
|
||||
if type(nsbx_e) == int and nsbx_e > -1:
|
||||
if isinstance(nsbx_e, int) and nsbx_e > -1:
|
||||
self.__nsbx_e = nsbx_e
|
||||
else:
|
||||
raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nsbu.setter
|
||||
def nsbu(self, nsbu):
|
||||
if type(nsbu) == int and nsbu > -1:
|
||||
if isinstance(nsbu, int) and nsbu > -1:
|
||||
self.__nsbu = nsbu
|
||||
else:
|
||||
raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.')
|
||||
|
@ -1592,14 +1592,14 @@ class AcadosOcpConstraints:
|
|||
# initial x
|
||||
@lbx_0.setter
|
||||
def lbx_0(self, lbx_0):
|
||||
if type(lbx_0) == np.ndarray:
|
||||
if isinstance(lbx_0, np.ndarray):
|
||||
self.__lbx_0 = lbx_0
|
||||
else:
|
||||
raise Exception('Invalid lbx_0 value. Exiting.')
|
||||
|
||||
@ubx_0.setter
|
||||
def ubx_0(self, ubx_0):
|
||||
if type(ubx_0) == np.ndarray:
|
||||
if isinstance(ubx_0, np.ndarray):
|
||||
self.__ubx_0 = ubx_0
|
||||
else:
|
||||
raise Exception('Invalid ubx_0 value. Exiting.')
|
||||
|
@ -1613,7 +1613,7 @@ class AcadosOcpConstraints:
|
|||
|
||||
@Jbx_0.setter
|
||||
def Jbx_0(self, Jbx_0):
|
||||
if type(Jbx_0) == np.ndarray:
|
||||
if isinstance(Jbx_0, np.ndarray):
|
||||
self.__idxbx_0 = J_to_idx(Jbx_0)
|
||||
else:
|
||||
raise Exception('Invalid Jbx_0 value. Exiting.')
|
||||
|
@ -1639,28 +1639,28 @@ class AcadosOcpConstraints:
|
|||
# bounds on x
|
||||
@lbx.setter
|
||||
def lbx(self, lbx):
|
||||
if type(lbx) == np.ndarray:
|
||||
if isinstance(lbx, np.ndarray):
|
||||
self.__lbx = lbx
|
||||
else:
|
||||
raise Exception('Invalid lbx value. Exiting.')
|
||||
|
||||
@ubx.setter
|
||||
def ubx(self, ubx):
|
||||
if type(ubx) == np.ndarray:
|
||||
if isinstance(ubx, np.ndarray):
|
||||
self.__ubx = ubx
|
||||
else:
|
||||
raise Exception('Invalid ubx value. Exiting.')
|
||||
|
||||
@idxbx.setter
|
||||
def idxbx(self, idxbx):
|
||||
if type(idxbx) == np.ndarray:
|
||||
if isinstance(idxbx, np.ndarray):
|
||||
self.__idxbx = idxbx
|
||||
else:
|
||||
raise Exception('Invalid idxbx value. Exiting.')
|
||||
|
||||
@Jbx.setter
|
||||
def Jbx(self, Jbx):
|
||||
if type(Jbx) == np.ndarray:
|
||||
if isinstance(Jbx, np.ndarray):
|
||||
self.__idxbx = J_to_idx(Jbx)
|
||||
else:
|
||||
raise Exception('Invalid Jbx value. Exiting.')
|
||||
|
@ -1668,28 +1668,28 @@ class AcadosOcpConstraints:
|
|||
# bounds on u
|
||||
@lbu.setter
|
||||
def lbu(self, lbu):
|
||||
if type(lbu) == np.ndarray:
|
||||
if isinstance(lbu, np.ndarray):
|
||||
self.__lbu = lbu
|
||||
else:
|
||||
raise Exception('Invalid lbu value. Exiting.')
|
||||
|
||||
@ubu.setter
|
||||
def ubu(self, ubu):
|
||||
if type(ubu) == np.ndarray:
|
||||
if isinstance(ubu, np.ndarray):
|
||||
self.__ubu = ubu
|
||||
else:
|
||||
raise Exception('Invalid ubu value. Exiting.')
|
||||
|
||||
@idxbu.setter
|
||||
def idxbu(self, idxbu):
|
||||
if type(idxbu) == np.ndarray:
|
||||
if isinstance(idxbu, np.ndarray):
|
||||
self.__idxbu = idxbu
|
||||
else:
|
||||
raise Exception('Invalid idxbu value. Exiting.')
|
||||
|
||||
@Jbu.setter
|
||||
def Jbu(self, Jbu):
|
||||
if type(Jbu) == np.ndarray:
|
||||
if isinstance(Jbu, np.ndarray):
|
||||
self.__idxbu = J_to_idx(Jbu)
|
||||
else:
|
||||
raise Exception('Invalid Jbu value. Exiting.')
|
||||
|
@ -1697,28 +1697,28 @@ class AcadosOcpConstraints:
|
|||
# bounds on x at shooting node N
|
||||
@lbx_e.setter
|
||||
def lbx_e(self, lbx_e):
|
||||
if type(lbx_e) == np.ndarray:
|
||||
if isinstance(lbx_e, np.ndarray):
|
||||
self.__lbx_e = lbx_e
|
||||
else:
|
||||
raise Exception('Invalid lbx_e value. Exiting.')
|
||||
|
||||
@ubx_e.setter
|
||||
def ubx_e(self, ubx_e):
|
||||
if type(ubx_e) == np.ndarray:
|
||||
if isinstance(ubx_e, np.ndarray):
|
||||
self.__ubx_e = ubx_e
|
||||
else:
|
||||
raise Exception('Invalid ubx_e value. Exiting.')
|
||||
|
||||
@idxbx_e.setter
|
||||
def idxbx_e(self, idxbx_e):
|
||||
if type(idxbx_e) == np.ndarray:
|
||||
if isinstance(idxbx_e, np.ndarray):
|
||||
self.__idxbx_e = idxbx_e
|
||||
else:
|
||||
raise Exception('Invalid idxbx_e value. Exiting.')
|
||||
|
||||
@Jbx_e.setter
|
||||
def Jbx_e(self, Jbx_e):
|
||||
if type(Jbx_e) == np.ndarray:
|
||||
if isinstance(Jbx_e, np.ndarray):
|
||||
self.__idxbx_e = J_to_idx(Jbx_e)
|
||||
else:
|
||||
raise Exception('Invalid Jbx_e value. Exiting.')
|
||||
|
@ -1742,14 +1742,14 @@ class AcadosOcpConstraints:
|
|||
|
||||
@lg.setter
|
||||
def lg(self, lg):
|
||||
if type(lg) == np.ndarray:
|
||||
if isinstance(lg, np.ndarray):
|
||||
self.__lg = lg
|
||||
else:
|
||||
raise Exception('Invalid lg value. Exiting.')
|
||||
|
||||
@ug.setter
|
||||
def ug(self, ug):
|
||||
if type(ug) == np.ndarray:
|
||||
if isinstance(ug, np.ndarray):
|
||||
self.__ug = ug
|
||||
else:
|
||||
raise Exception('Invalid ug value. Exiting.')
|
||||
|
@ -1765,14 +1765,14 @@ class AcadosOcpConstraints:
|
|||
|
||||
@lg_e.setter
|
||||
def lg_e(self, lg_e):
|
||||
if type(lg_e) == np.ndarray:
|
||||
if isinstance(lg_e, np.ndarray):
|
||||
self.__lg_e = lg_e
|
||||
else:
|
||||
raise Exception('Invalid lg_e value. Exiting.')
|
||||
|
||||
@ug_e.setter
|
||||
def ug_e(self, ug_e):
|
||||
if type(ug_e) == np.ndarray:
|
||||
if isinstance(ug_e, np.ndarray):
|
||||
self.__ug_e = ug_e
|
||||
else:
|
||||
raise Exception('Invalid ug_e value. Exiting.')
|
||||
|
@ -1780,14 +1780,14 @@ class AcadosOcpConstraints:
|
|||
# nonlinear constraints
|
||||
@lh.setter
|
||||
def lh(self, lh):
|
||||
if type(lh) == np.ndarray:
|
||||
if isinstance(lh, np.ndarray):
|
||||
self.__lh = lh
|
||||
else:
|
||||
raise Exception('Invalid lh value. Exiting.')
|
||||
|
||||
@uh.setter
|
||||
def uh(self, uh):
|
||||
if type(uh) == np.ndarray:
|
||||
if isinstance(uh, np.ndarray):
|
||||
self.__uh = uh
|
||||
else:
|
||||
raise Exception('Invalid uh value. Exiting.')
|
||||
|
@ -1795,14 +1795,14 @@ class AcadosOcpConstraints:
|
|||
# convex-over-nonlinear constraints
|
||||
@lphi.setter
|
||||
def lphi(self, lphi):
|
||||
if type(lphi) == np.ndarray:
|
||||
if isinstance(lphi, np.ndarray):
|
||||
self.__lphi = lphi
|
||||
else:
|
||||
raise Exception('Invalid lphi value. Exiting.')
|
||||
|
||||
@uphi.setter
|
||||
def uphi(self, uphi):
|
||||
if type(uphi) == np.ndarray:
|
||||
if isinstance(uphi, np.ndarray):
|
||||
self.__uphi = uphi
|
||||
else:
|
||||
raise Exception('Invalid uphi value. Exiting.')
|
||||
|
@ -1810,14 +1810,14 @@ class AcadosOcpConstraints:
|
|||
# nonlinear constraints at shooting node N
|
||||
@lh_e.setter
|
||||
def lh_e(self, lh_e):
|
||||
if type(lh_e) == np.ndarray:
|
||||
if isinstance(lh_e, np.ndarray):
|
||||
self.__lh_e = lh_e
|
||||
else:
|
||||
raise Exception('Invalid lh_e value. Exiting.')
|
||||
|
||||
@uh_e.setter
|
||||
def uh_e(self, uh_e):
|
||||
if type(uh_e) == np.ndarray:
|
||||
if isinstance(uh_e, np.ndarray):
|
||||
self.__uh_e = uh_e
|
||||
else:
|
||||
raise Exception('Invalid uh_e value. Exiting.')
|
||||
|
@ -1825,14 +1825,14 @@ class AcadosOcpConstraints:
|
|||
# convex-over-nonlinear constraints at shooting node N
|
||||
@lphi_e.setter
|
||||
def lphi_e(self, lphi_e):
|
||||
if type(lphi_e) == np.ndarray:
|
||||
if isinstance(lphi_e, np.ndarray):
|
||||
self.__lphi_e = lphi_e
|
||||
else:
|
||||
raise Exception('Invalid lphi_e value. Exiting.')
|
||||
|
||||
@uphi_e.setter
|
||||
def uphi_e(self, uphi_e):
|
||||
if type(uphi_e) == np.ndarray:
|
||||
if isinstance(uphi_e, np.ndarray):
|
||||
self.__uphi_e = uphi_e
|
||||
else:
|
||||
raise Exception('Invalid uphi_e value. Exiting.')
|
||||
|
@ -1841,21 +1841,21 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on x
|
||||
@lsbx.setter
|
||||
def lsbx(self, lsbx):
|
||||
if type(lsbx) == np.ndarray:
|
||||
if isinstance(lsbx, np.ndarray):
|
||||
self.__lsbx = lsbx
|
||||
else:
|
||||
raise Exception('Invalid lsbx value. Exiting.')
|
||||
|
||||
@usbx.setter
|
||||
def usbx(self, usbx):
|
||||
if type(usbx) == np.ndarray:
|
||||
if isinstance(usbx, np.ndarray):
|
||||
self.__usbx = usbx
|
||||
else:
|
||||
raise Exception('Invalid usbx value. Exiting.')
|
||||
|
||||
@idxsbx.setter
|
||||
def idxsbx(self, idxsbx):
|
||||
if type(idxsbx) == np.ndarray:
|
||||
if isinstance(idxsbx, np.ndarray):
|
||||
self.__idxsbx = idxsbx
|
||||
else:
|
||||
raise Exception('Invalid idxsbx value. Exiting.')
|
||||
|
@ -1870,28 +1870,28 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on u
|
||||
@lsbu.setter
|
||||
def lsbu(self, lsbu):
|
||||
if type(lsbu) == np.ndarray:
|
||||
if isinstance(lsbu, np.ndarray):
|
||||
self.__lsbu = lsbu
|
||||
else:
|
||||
raise Exception('Invalid lsbu value. Exiting.')
|
||||
|
||||
@usbu.setter
|
||||
def usbu(self, usbu):
|
||||
if type(usbu) == np.ndarray:
|
||||
if isinstance(usbu, np.ndarray):
|
||||
self.__usbu = usbu
|
||||
else:
|
||||
raise Exception('Invalid usbu value. Exiting.')
|
||||
|
||||
@idxsbu.setter
|
||||
def idxsbu(self, idxsbu):
|
||||
if type(idxsbu) == np.ndarray:
|
||||
if isinstance(idxsbu, np.ndarray):
|
||||
self.__idxsbu = idxsbu
|
||||
else:
|
||||
raise Exception('Invalid idxsbu value. Exiting.')
|
||||
|
||||
@Jsbu.setter
|
||||
def Jsbu(self, Jsbu):
|
||||
if type(Jsbu) == np.ndarray:
|
||||
if isinstance(Jsbu, np.ndarray):
|
||||
self.__idxsbu = J_to_idx_slack(Jsbu)
|
||||
else:
|
||||
raise Exception('Invalid Jsbu value. Exiting.')
|
||||
|
@ -1899,28 +1899,28 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on x at shooting node N
|
||||
@lsbx_e.setter
|
||||
def lsbx_e(self, lsbx_e):
|
||||
if type(lsbx_e) == np.ndarray:
|
||||
if isinstance(lsbx_e, np.ndarray):
|
||||
self.__lsbx_e = lsbx_e
|
||||
else:
|
||||
raise Exception('Invalid lsbx_e value. Exiting.')
|
||||
|
||||
@usbx_e.setter
|
||||
def usbx_e(self, usbx_e):
|
||||
if type(usbx_e) == np.ndarray:
|
||||
if isinstance(usbx_e, np.ndarray):
|
||||
self.__usbx_e = usbx_e
|
||||
else:
|
||||
raise Exception('Invalid usbx_e value. Exiting.')
|
||||
|
||||
@idxsbx_e.setter
|
||||
def idxsbx_e(self, idxsbx_e):
|
||||
if type(idxsbx_e) == np.ndarray:
|
||||
if isinstance(idxsbx_e, np.ndarray):
|
||||
self.__idxsbx_e = idxsbx_e
|
||||
else:
|
||||
raise Exception('Invalid idxsbx_e value. Exiting.')
|
||||
|
||||
@Jsbx_e.setter
|
||||
def Jsbx_e(self, Jsbx_e):
|
||||
if type(Jsbx_e) == np.ndarray:
|
||||
if isinstance(Jsbx_e, np.ndarray):
|
||||
self.__idxsbx_e = J_to_idx_slack(Jsbx_e)
|
||||
else:
|
||||
raise Exception('Invalid Jsbx_e value. Exiting.')
|
||||
|
@ -1959,21 +1959,21 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on nonlinear constraints
|
||||
@lsh.setter
|
||||
def lsh(self, lsh):
|
||||
if type(lsh) == np.ndarray:
|
||||
if isinstance(lsh, np.ndarray):
|
||||
self.__lsh = lsh
|
||||
else:
|
||||
raise Exception('Invalid lsh value. Exiting.')
|
||||
|
||||
@ush.setter
|
||||
def ush(self, ush):
|
||||
if type(ush) == np.ndarray:
|
||||
if isinstance(ush, np.ndarray):
|
||||
self.__ush = ush
|
||||
else:
|
||||
raise Exception('Invalid ush value. Exiting.')
|
||||
|
||||
@idxsh.setter
|
||||
def idxsh(self, idxsh):
|
||||
if type(idxsh) == np.ndarray:
|
||||
if isinstance(idxsh, np.ndarray):
|
||||
self.__idxsh = idxsh
|
||||
else:
|
||||
raise Exception('Invalid idxsh value. Exiting.')
|
||||
|
@ -1989,21 +1989,21 @@ class AcadosOcpConstraints:
|
|||
# soft bounds on convex-over-nonlinear constraints
|
||||
@lsphi.setter
|
||||
def lsphi(self, lsphi):
|
||||
if type(lsphi) == np.ndarray:
|
||||
if isinstance(lsphi, np.ndarray):
|
||||
self.__lsphi = lsphi
|
||||
else:
|
||||
raise Exception('Invalid lsphi value. Exiting.')
|
||||
|
||||
@usphi.setter
|
||||
def usphi(self, usphi):
|
||||
if type(usphi) == np.ndarray:
|
||||
if isinstance(usphi, np.ndarray):
|
||||
self.__usphi = usphi
|
||||
else:
|
||||
raise Exception('Invalid usphi value. Exiting.')
|
||||
|
||||
@idxsphi.setter
|
||||
def idxsphi(self, idxsphi):
|
||||
if type(idxsphi) == np.ndarray:
|
||||
if isinstance(idxsphi, np.ndarray):
|
||||
self.__idxsphi = idxsphi
|
||||
else:
|
||||
raise Exception('Invalid idxsphi value. Exiting.')
|
||||
|
@ -2151,6 +2151,10 @@ class AcadosOcpOptions:
|
|||
self.__ext_cost_num_hess = 0
|
||||
self.__alpha_min = 0.05
|
||||
self.__alpha_reduction = 0.7
|
||||
self.__line_search_use_sufficient_descent = 0
|
||||
self.__globalization_use_SOC = 0
|
||||
self.__full_step_dual = 0
|
||||
self.__eps_sufficient_descent = 1e-4
|
||||
|
||||
|
||||
@property
|
||||
|
@ -2367,6 +2371,43 @@ class AcadosOcpOptions:
|
|||
"""Step size reduction factor for globalization MERIT_BACKTRACKING, default: 0.7."""
|
||||
return self.__alpha_reduction
|
||||
|
||||
@property
|
||||
def line_search_use_sufficient_descent(self):
|
||||
"""
|
||||
Determines if sufficient descent (Armijo) condition is used in line search.
|
||||
Type: int; 0 or 1;
|
||||
default: 0.
|
||||
"""
|
||||
return self.__line_search_use_sufficient_descent
|
||||
|
||||
@property
|
||||
def eps_sufficient_descent(self):
|
||||
"""
|
||||
Factor for sufficient descent (Armijo) conditon, see line_search_use_sufficient_descent.
|
||||
Type: float,
|
||||
default: 1e-4.
|
||||
"""
|
||||
return self.__eps_sufficient_descent
|
||||
|
||||
@property
|
||||
def globalization_use_SOC(self):
|
||||
"""
|
||||
Determines if second order correction (SOC) is done when using MERIT_BACKTRACKING.
|
||||
SOC is done if preliminary line search does not return full step.
|
||||
Type: int; 0 or 1;
|
||||
default: 0.
|
||||
"""
|
||||
return self.__globalization_use_SOC
|
||||
|
||||
@property
|
||||
def full_step_dual(self):
|
||||
"""
|
||||
Determines if dual variables are updated with full steps (alpha=1.0) when primal variables are updated with smaller step.
|
||||
Type: int; 0 or 1;
|
||||
default: 0.
|
||||
"""
|
||||
return self.__full_step_dual
|
||||
|
||||
@property
|
||||
def nlp_solver_tol_ineq(self):
|
||||
"""NLP solver inequality tolerance"""
|
||||
|
@ -2524,12 +2565,23 @@ class AcadosOcpOptions:
|
|||
|
||||
@time_steps.setter
|
||||
def time_steps(self, time_steps):
|
||||
self.__time_steps = time_steps
|
||||
if isinstance(time_steps, np.ndarray):
|
||||
if len(time_steps.shape) == 1:
|
||||
self.__time_steps = time_steps
|
||||
else:
|
||||
raise Exception('Invalid time_steps, expected np.ndarray of shape (N,).')
|
||||
else:
|
||||
raise Exception('Invalid time_steps, expected np.ndarray.')
|
||||
|
||||
@shooting_nodes.setter
|
||||
def shooting_nodes(self, shooting_nodes):
|
||||
self.__shooting_nodes = shooting_nodes
|
||||
|
||||
if isinstance(shooting_nodes, np.ndarray):
|
||||
if len(shooting_nodes.shape) == 1:
|
||||
self.__shooting_nodes = shooting_nodes
|
||||
else:
|
||||
raise Exception('Invalid shooting_nodes, expected np.ndarray of shape (N+1,).')
|
||||
else:
|
||||
raise Exception('Invalid shooting_nodes, expected np.ndarray.')
|
||||
|
||||
@Tsim.setter
|
||||
def Tsim(self, Tsim):
|
||||
|
@ -2537,7 +2589,12 @@ class AcadosOcpOptions:
|
|||
|
||||
@globalization.setter
|
||||
def globalization(self, globalization):
|
||||
self.__globalization = globalization
|
||||
globalization_types = ('MERIT_BACKTRACKING', 'FIXED_STEP')
|
||||
if globalization in globalization_types:
|
||||
self.__globalization = globalization
|
||||
else:
|
||||
raise Exception('Invalid globalization value. Possible values are:\n\n' \
|
||||
+ ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.')
|
||||
|
||||
@alpha_min.setter
|
||||
def alpha_min(self, alpha_min):
|
||||
|
@ -2547,10 +2604,38 @@ class AcadosOcpOptions:
|
|||
def alpha_reduction(self, alpha_reduction):
|
||||
self.__alpha_reduction = alpha_reduction
|
||||
|
||||
@line_search_use_sufficient_descent.setter
|
||||
def line_search_use_sufficient_descent(self, line_search_use_sufficient_descent):
|
||||
if line_search_use_sufficient_descent in [0, 1]:
|
||||
self.__line_search_use_sufficient_descent = line_search_use_sufficient_descent
|
||||
else:
|
||||
raise Exception(f'Invalid value for line_search_use_sufficient_descent. Possible values are 0, 1, got {line_search_use_sufficient_descent}')
|
||||
|
||||
@globalization_use_SOC.setter
|
||||
def globalization_use_SOC(self, globalization_use_SOC):
|
||||
if globalization_use_SOC in [0, 1]:
|
||||
self.__globalization_use_SOC = globalization_use_SOC
|
||||
else:
|
||||
raise Exception(f'Invalid value for globalization_use_SOC. Possible values are 0, 1, got {globalization_use_SOC}')
|
||||
|
||||
@full_step_dual.setter
|
||||
def full_step_dual(self, full_step_dual):
|
||||
if full_step_dual in [0, 1]:
|
||||
self.__full_step_dual = full_step_dual
|
||||
else:
|
||||
raise Exception(f'Invalid value for full_step_dual. Possible values are 0, 1, got {full_step_dual}')
|
||||
|
||||
@eps_sufficient_descent.setter
|
||||
def eps_sufficient_descent(self, eps_sufficient_descent):
|
||||
if isinstance(eps_sufficient_descent, float) and eps_sufficient_descent > 0:
|
||||
self.__eps_sufficient_descent = eps_sufficient_descent
|
||||
else:
|
||||
raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float. Exiting')
|
||||
|
||||
@sim_method_num_stages.setter
|
||||
def sim_method_num_stages(self, sim_method_num_stages):
|
||||
|
||||
# if type(sim_method_num_stages) == int:
|
||||
# if isinstance(sim_method_num_stages, int):
|
||||
# self.__sim_method_num_stages = sim_method_num_stages
|
||||
# else:
|
||||
# raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.')
|
||||
|
@ -2560,7 +2645,7 @@ class AcadosOcpOptions:
|
|||
@sim_method_num_steps.setter
|
||||
def sim_method_num_steps(self, sim_method_num_steps):
|
||||
|
||||
# if type(sim_method_num_steps) == int:
|
||||
# if isinstance(sim_method_num_steps, int):
|
||||
# self.__sim_method_num_steps = sim_method_num_steps
|
||||
# else:
|
||||
# raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.')
|
||||
|
@ -2570,7 +2655,7 @@ class AcadosOcpOptions:
|
|||
@sim_method_newton_iter.setter
|
||||
def sim_method_newton_iter(self, sim_method_newton_iter):
|
||||
|
||||
if type(sim_method_newton_iter) == int:
|
||||
if isinstance(sim_method_newton_iter, int):
|
||||
self.__sim_method_newton_iter = sim_method_newton_iter
|
||||
else:
|
||||
raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.')
|
||||
|
@ -2593,7 +2678,7 @@ class AcadosOcpOptions:
|
|||
|
||||
@nlp_solver_step_length.setter
|
||||
def nlp_solver_step_length(self, nlp_solver_step_length):
|
||||
if type(nlp_solver_step_length) == float and nlp_solver_step_length > 0:
|
||||
if isinstance(nlp_solver_step_length, float) and nlp_solver_step_length > 0:
|
||||
self.__nlp_solver_step_length = nlp_solver_step_length
|
||||
else:
|
||||
raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting')
|
||||
|
@ -2614,7 +2699,7 @@ class AcadosOcpOptions:
|
|||
|
||||
@qp_solver_cond_N.setter
|
||||
def qp_solver_cond_N(self, qp_solver_cond_N):
|
||||
if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N > 0:
|
||||
if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0:
|
||||
self.__qp_solver_cond_N = qp_solver_cond_N
|
||||
else:
|
||||
raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting')
|
||||
|
@ -2705,21 +2790,21 @@ class AcadosOcpOptions:
|
|||
@nlp_solver_max_iter.setter
|
||||
def nlp_solver_max_iter(self, nlp_solver_max_iter):
|
||||
|
||||
if type(nlp_solver_max_iter) == int and nlp_solver_max_iter > 0:
|
||||
if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter > 0:
|
||||
self.__nlp_solver_max_iter = nlp_solver_max_iter
|
||||
else:
|
||||
raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting')
|
||||
|
||||
@print_level.setter
|
||||
def print_level(self, print_level):
|
||||
if type(print_level) == int and print_level >= 0:
|
||||
if isinstance(print_level, int) and print_level >= 0:
|
||||
self.__print_level = print_level
|
||||
else:
|
||||
raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting')
|
||||
|
||||
@model_external_shared_lib_dir.setter
|
||||
def model_external_shared_lib_dir(self, model_external_shared_lib_dir):
|
||||
if type(model_external_shared_lib_dir) == str :
|
||||
if isinstance(model_external_shared_lib_dir, str) :
|
||||
self.__model_external_shared_lib_dir = model_external_shared_lib_dir
|
||||
else:
|
||||
raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \
|
||||
|
@ -2727,7 +2812,7 @@ class AcadosOcpOptions:
|
|||
|
||||
@model_external_shared_lib_name.setter
|
||||
def model_external_shared_lib_name(self, model_external_shared_lib_name):
|
||||
if type(model_external_shared_lib_name) == str :
|
||||
if isinstance(model_external_shared_lib_name, str) :
|
||||
if model_external_shared_lib_name[-3:] == '.so' :
|
||||
raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \
|
||||
+ '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.')
|
||||
|
@ -2810,6 +2895,9 @@ class AcadosOcp:
|
|||
self.acados_lib_path = f'{acados_path}/lib'
|
||||
"""Path to where acados library is located, type: string"""
|
||||
|
||||
import numpy
|
||||
self.cython_include_dirs = numpy.get_include()
|
||||
|
||||
self.__parameter_values = np.array([])
|
||||
self.__problem_class = 'OCP'
|
||||
|
||||
|
|
|
@ -37,7 +37,7 @@ import os
|
|||
import json
|
||||
import numpy as np
|
||||
from datetime import datetime
|
||||
import ctypes
|
||||
import importlib
|
||||
from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref
|
||||
|
||||
from copy import deepcopy
|
||||
|
@ -51,9 +51,9 @@ from .generate_c_code_nls_cost import generate_c_code_nls_cost
|
|||
from .generate_c_code_external_cost import generate_c_code_external_cost
|
||||
from .acados_ocp import AcadosOcp
|
||||
from .acados_model import acados_model_strip_casadi_symbolics
|
||||
from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
|
||||
from .utils import is_column, is_empty, casadi_length, render_template,\
|
||||
format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
|
||||
set_up_imported_gnsf_model, get_acados_path, get_ocp_nlp_layout, get_python_interface_path
|
||||
set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path
|
||||
|
||||
|
||||
def make_ocp_dims_consistent(acados_ocp):
|
||||
|
@ -90,7 +90,7 @@ def make_ocp_dims_consistent(acados_ocp):
|
|||
raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
|
||||
f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n')
|
||||
|
||||
# cost
|
||||
## cost
|
||||
# initial stage - if not set, copy fields from path constraints
|
||||
if cost.cost_type_0 is None:
|
||||
cost.cost_type_0 = cost.cost_type
|
||||
|
@ -434,18 +434,14 @@ def make_ocp_dims_consistent(acados_ocp):
|
|||
if np.shape(opts.shooting_nodes)[0] != dims.N+1:
|
||||
raise Exception('inconsistent dimension N, regarding shooting_nodes.')
|
||||
|
||||
# time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
|
||||
# # identify constant time-steps: due to numerical reasons the content of time_steps might vary a bit
|
||||
# delta_time_steps = time_steps[1:] - time_steps[0:-1]
|
||||
# avg_time_steps = np.average(time_steps)
|
||||
# # criterion for constant time-step detection: the min/max difference in values normalized by the average
|
||||
# check_const_time_step = np.max(delta_time_steps)-np.min(delta_time_steps) / avg_time_steps
|
||||
# # if the criterion is small, we have a constant time-step
|
||||
# if check_const_time_step < 1e-9:
|
||||
# time_steps[:] = avg_time_steps # if we have a constant time-step: apply the average time-step
|
||||
time_steps = np.zeros((dims.N,))
|
||||
for i in range(dims.N):
|
||||
time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] # TODO use commented code above
|
||||
time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
|
||||
# identify constant time_steps: due to numerical reasons the content of time_steps might vary a bit
|
||||
avg_time_steps = np.average(time_steps)
|
||||
# criterion for constant time step detection: the min/max difference in values normalized by the average
|
||||
check_const_time_step = (np.max(time_steps)-np.min(time_steps)) / avg_time_steps
|
||||
# if the criterion is small, we have a constant time_step
|
||||
if check_const_time_step < 1e-9:
|
||||
time_steps[:] = avg_time_steps # if we have a constant time_step: apply the average time_step
|
||||
|
||||
opts.time_steps = time_steps
|
||||
|
||||
|
@ -525,8 +521,7 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_n
|
|||
|
||||
# strip shooting_nodes
|
||||
ocp_nlp_dict['solver_options'].pop('shooting_nodes', None)
|
||||
|
||||
dims_dict = acados_class2dict(acados_ocp.dims)
|
||||
dims_dict = format_class_dict(acados_ocp.dims.__dict__)
|
||||
|
||||
ocp_check_against_layout(ocp_nlp_dict, dims_dict)
|
||||
|
||||
|
@ -782,8 +777,15 @@ class AcadosOcpSolver:
|
|||
dlclose.argtypes = [c_void_p]
|
||||
|
||||
@classmethod
|
||||
def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True):
|
||||
def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None):
|
||||
"""
|
||||
Generates the code for an acados OCP solver, given the description in acados_ocp.
|
||||
:param acados_ocp: type AcadosOcp - description of the OCP for acados
|
||||
:param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
|
||||
:param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
|
||||
"""
|
||||
model = acados_ocp.model
|
||||
acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory)
|
||||
|
||||
if simulink_opts is None:
|
||||
simulink_opts = get_simulink_default_opts()
|
||||
|
@ -807,24 +809,91 @@ class AcadosOcpSolver:
|
|||
# dump to json
|
||||
ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file)
|
||||
|
||||
code_export_dir = acados_ocp.code_export_directory
|
||||
# render templates
|
||||
ocp_render_templates(acados_ocp, json_file)
|
||||
acados_ocp.json_file = json_file
|
||||
|
||||
if build:
|
||||
## Compile solver
|
||||
cwd=os.getcwd()
|
||||
os.chdir(code_export_dir)
|
||||
os.system('make clean_ocp_shared_lib')
|
||||
os.system('make ocp_shared_lib')
|
||||
os.chdir(cwd)
|
||||
|
||||
def __init__(self, model_name, N, code_export_dir):
|
||||
self.model_name = model_name
|
||||
self.N = N
|
||||
@classmethod
|
||||
def build(cls, code_export_dir, with_cython=False):
|
||||
"""
|
||||
Builds the code for an acados OCP solver, that has been generated in code_export_dir
|
||||
:param code_export_dir: directory in which acados OCP solver has been generated, see generate()
|
||||
:param with_cython: option indicating if the cython interface is build, default: False.
|
||||
"""
|
||||
cwd=os.getcwd()
|
||||
os.chdir(code_export_dir)
|
||||
if with_cython:
|
||||
os.system('make clean_ocp_cython')
|
||||
os.system('make ocp_cython')
|
||||
else:
|
||||
os.system('make clean_ocp_shared_lib')
|
||||
os.system('make ocp_shared_lib')
|
||||
os.chdir(cwd)
|
||||
|
||||
|
||||
@classmethod
|
||||
def create_cython_solver(cls, json_file):
|
||||
"""
|
||||
Returns an `AcadosOcpSolverCython` object.
|
||||
|
||||
This is an alternative Cython based Python wrapper to the acados OCP solver in C.
|
||||
This offers faster interaction with the solver, because getter and setter calls, which include shape checking are done in compiled C code.
|
||||
|
||||
The default wrapper `AcadosOcpSolver` is based on ctypes.
|
||||
"""
|
||||
with open(json_file, 'r') as f:
|
||||
acados_ocp_json = json.load(f)
|
||||
code_export_directory = acados_ocp_json['code_export_directory']
|
||||
|
||||
importlib.invalidate_caches()
|
||||
rel_code_export_directory = os.path.relpath(code_export_directory)
|
||||
acados_ocp_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_ocp_solver_pyx')
|
||||
|
||||
AcadosOcpSolverCython = getattr(acados_ocp_solver_pyx, 'AcadosOcpSolverCython')
|
||||
return AcadosOcpSolverCython(acados_ocp_json['model']['name'],
|
||||
acados_ocp_json['solver_options']['nlp_solver_type'],
|
||||
acados_ocp_json['dims']['N'])
|
||||
|
||||
|
||||
def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True):
|
||||
|
||||
self.solver_created = False
|
||||
self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{self.model_name}.so'
|
||||
if generate:
|
||||
self.generate(acados_ocp, json_file=json_file, simulink_opts=simulink_opts)
|
||||
|
||||
# load json, store options in object
|
||||
with open(json_file, 'r') as f:
|
||||
acados_ocp_json = json.load(f)
|
||||
self.N = acados_ocp_json['dims']['N']
|
||||
self.model_name = acados_ocp_json['model']['name']
|
||||
self.solver_options = acados_ocp_json['solver_options']
|
||||
|
||||
acados_lib_path = acados_ocp_json['acados_lib_path']
|
||||
code_export_directory = acados_ocp_json['code_export_directory']
|
||||
|
||||
if build:
|
||||
self.build(code_export_directory, with_cython=False)
|
||||
|
||||
# Load acados library to avoid unloading the library.
|
||||
# This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
|
||||
# Unloading a library which uses OpenMP results in a segfault (on any platform?).
|
||||
# see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
|
||||
# or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
|
||||
libacados_name = 'libacados.so'
|
||||
libacados_filepath = os.path.join(acados_lib_path, libacados_name)
|
||||
self.__acados_lib = CDLL(libacados_filepath)
|
||||
# find out if acados was compiled with OpenMP
|
||||
try:
|
||||
self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
|
||||
except AttributeError as e:
|
||||
self.__acados_lib_uses_omp = False
|
||||
if self.__acados_lib_uses_omp:
|
||||
print('acados was compiled with OpenMP.')
|
||||
else:
|
||||
print('acados was compiled without OpenMP.')
|
||||
|
||||
self.shared_lib_name = f'{code_export_directory}/libacados_ocp_solver_{self.model_name}.so'
|
||||
|
||||
# get shared_lib
|
||||
self.shared_lib = CDLL(self.shared_lib_name)
|
||||
|
@ -842,6 +911,8 @@ class AcadosOcpSolver:
|
|||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
|
||||
self.status = 0
|
||||
|
||||
|
||||
def __get_pointers_solver(self):
|
||||
"""
|
||||
|
@ -864,6 +935,10 @@ class AcadosOcpSolver:
|
|||
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p
|
||||
self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").restype = c_void_p
|
||||
self.sens_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p
|
||||
self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule)
|
||||
|
@ -872,46 +947,26 @@ class AcadosOcpSolver:
|
|||
getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p
|
||||
self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)
|
||||
|
||||
# treat parameters separately
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
|
||||
self._set_param = getattr(self.shared_lib, f"{self.model_name}_acados_update_params")
|
||||
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
|
||||
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
|
||||
|
||||
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
|
||||
def solve(self):
|
||||
"""
|
||||
Solve the ocp with current input.
|
||||
"""
|
||||
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int
|
||||
status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
|
||||
return status
|
||||
self.status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
|
||||
|
||||
return self.status
|
||||
|
||||
|
||||
def set_new_time_steps(self, new_time_steps):
|
||||
"""
|
||||
Set new time steps before solving. Only reload library without code generation but with new time steps.
|
||||
Set new time steps.
|
||||
Recreates the solver if N changes.
|
||||
|
||||
:param new_time_steps: vector of new time steps for the solver
|
||||
:param new_time_steps: 1 dimensional np array of new time steps for the solver
|
||||
|
||||
.. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
|
||||
.. note:: This allows for different use-cases: either set a new size of time_steps or a new distribution of
|
||||
the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
|
||||
do not require a new code export and compilation.
|
||||
"""
|
||||
|
@ -921,15 +976,14 @@ class AcadosOcpSolver:
|
|||
raise Exception('Solver was not yet created!')
|
||||
|
||||
# check if time steps really changed in value
|
||||
if np.array_equal(self.acados_ocp.solver_options.time_steps, new_time_steps):
|
||||
if np.array_equal(self.solver_options['time_steps'], new_time_steps):
|
||||
return
|
||||
|
||||
N = new_time_steps.size
|
||||
model = self.acados_ocp.model
|
||||
new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double))
|
||||
|
||||
# check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
|
||||
if self.acados_ocp.solver_options.time_steps.size == N:
|
||||
if len(self.solver_options['time_steps']) == N:
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int
|
||||
assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0
|
||||
|
@ -941,11 +995,6 @@ class AcadosOcpSolver:
|
|||
getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)
|
||||
|
||||
# store N and new time steps
|
||||
self.N = self.acados_ocp.dims.N = N
|
||||
self.acados_ocp.solver_options.time_steps = new_time_steps
|
||||
self.acados_ocp.solver_options.Tsim = self.acados_ocp.solver_options.time_steps[0]
|
||||
|
||||
# create solver with new time steps
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int
|
||||
|
@ -956,6 +1005,75 @@ class AcadosOcpSolver:
|
|||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
|
||||
# store time_steps, N
|
||||
self.solver_options['time_steps'] = new_time_steps
|
||||
self.N = N
|
||||
self.solver_options['Tsim'] = self.solver_options['time_steps'][0]
|
||||
|
||||
|
||||
def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
|
||||
"""
|
||||
Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
|
||||
This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
|
||||
the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
|
||||
:param qp_solver_cond_N: new number of condensing stages for the solver
|
||||
|
||||
.. note:: This function can only be used in combination with a partial condensing QP solver.
|
||||
|
||||
.. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
|
||||
necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
|
||||
`qp_solver_cond_N < N`.
|
||||
"""
|
||||
# unlikely but still possible
|
||||
if not self.solver_created:
|
||||
raise Exception('Solver was not yet created!')
|
||||
if self.N < qp_solver_cond_N:
|
||||
raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
|
||||
if self.solver_options['qp_solver_cond_N'] != qp_solver_cond_N:
|
||||
self.solver_created = False
|
||||
|
||||
# recreate the solver
|
||||
fun_name = f'{self.model_name}_acados_update_qp_solver_cond_N'
|
||||
getattr(self.shared_lib, fun_name).argtypes = [c_void_p, c_int]
|
||||
getattr(self.shared_lib, fun_name).restype = c_int
|
||||
assert getattr(self.shared_lib, fun_name)(self.capsule, qp_solver_cond_N) == 0
|
||||
|
||||
# store the new value
|
||||
self.solver_options['qp_solver_cond_N'] = qp_solver_cond_N
|
||||
self.solver_created = True
|
||||
|
||||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
|
||||
|
||||
def eval_param_sens(self, index, stage=0, field="ex"):
|
||||
"""
|
||||
Calculate the sensitivity of the curent solution with respect to the initial state component of index
|
||||
|
||||
:param index: integer corresponding to initial state index in range(nx)
|
||||
"""
|
||||
|
||||
field_ = field
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
# checks
|
||||
if not isinstance(index, int):
|
||||
raise Exception('AcadosOcpSolver.eval_param_sens(): index must be Integer.')
|
||||
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
|
||||
nx = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
|
||||
|
||||
if index < 0 or index > nx:
|
||||
raise Exception(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
|
||||
|
||||
# actual eval_param
|
||||
self.shared_lib.ocp_nlp_eval_param_sens.argtypes = [c_void_p, c_char_p, c_int, c_int, c_void_p]
|
||||
self.shared_lib.ocp_nlp_eval_param_sens.restype = None
|
||||
self.shared_lib.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
|
||||
|
||||
return
|
||||
|
||||
|
||||
def get(self, stage_, field_):
|
||||
"""
|
||||
|
@ -978,23 +1096,30 @@ class AcadosOcpSolver:
|
|||
|
||||
out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
|
||||
# mem_fields = ['sl', 'su']
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
sens_fields = ['sens_u', "sens_x"]
|
||||
all_fields = out_fields + sens_fields
|
||||
|
||||
if (field_ not in out_fields):
|
||||
field = field_
|
||||
|
||||
if (field_ not in all_fields):
|
||||
raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(field_, out_fields))
|
||||
\n Possible values are {}. Exiting.'.format(field_, all_fields))
|
||||
|
||||
if not isinstance(stage_, int):
|
||||
raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')
|
||||
|
||||
if stage_ < 0 or stage_ > self.N:
|
||||
raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_))
|
||||
|
||||
if stage_ == self.N and field_ == 'pi':
|
||||
raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
|
||||
.format(field_, stage_))
|
||||
|
||||
if field_ in sens_fields:
|
||||
field = field_.replace('sens_', '')
|
||||
|
||||
field = field.encode('utf-8')
|
||||
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p]
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
|
||||
|
@ -1015,6 +1140,11 @@ class AcadosOcpSolver:
|
|||
# [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
# self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
|
||||
# self.nlp_dims, self.nlp_solver, stage_, field, out_data)
|
||||
elif field_ in sens_fields:
|
||||
self.shared_lib.ocp_nlp_out_get.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_get(self.nlp_config, \
|
||||
self.nlp_dims, self.sens_out, stage_, field, out_data)
|
||||
|
||||
return out
|
||||
|
||||
|
@ -1029,6 +1159,7 @@ class AcadosOcpSolver:
|
|||
- res_comp: residual wrt complementarity conditions
|
||||
- qp_stat: status of QP solver
|
||||
- qp_iter: number of QP iterations
|
||||
- alpha: SQP step size
|
||||
- qp_res_stat: stationarity residual of the last QP solution
|
||||
- qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
|
||||
- qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution
|
||||
|
@ -1036,19 +1167,18 @@ class AcadosOcpSolver:
|
|||
"""
|
||||
stat = self.get_stats("statistics")
|
||||
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
|
||||
print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter')
|
||||
if stat.shape[0]>7:
|
||||
if self.solver_options['nlp_solver_type'] == 'SQP':
|
||||
print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha')
|
||||
if stat.shape[0]>8:
|
||||
print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
|
||||
for jj in range(stat.shape[1]):
|
||||
print('{:d}\t{:e}\t{:e}\t{:e}\t{:e}\t{:d}\t{:d}'.format( \
|
||||
int(stat[0][jj]), stat[1][jj], stat[2][jj], \
|
||||
stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj])))
|
||||
if stat.shape[0]>7:
|
||||
print(f'{int(stat[0][jj]):d}\t{stat[1][jj]:e}\t{stat[2][jj]:e}\t{stat[3][jj]:e}\t' +
|
||||
f'{stat[4][jj]:e}\t{int(stat[5][jj]):d}\t{int(stat[6][jj]):d}\t{stat[7][jj]:e}\t')
|
||||
if stat.shape[0]>8:
|
||||
print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
|
||||
stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj]))
|
||||
stat[8][jj], stat[9][jj], stat[10][jj], stat[11][jj]))
|
||||
print('\n')
|
||||
elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
|
||||
print('\niter\tqp_stat\tqp_iter')
|
||||
if stat.shape[0]>3:
|
||||
print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
|
||||
|
@ -1108,6 +1238,7 @@ class AcadosOcpSolver:
|
|||
with open(filename, 'r') as f:
|
||||
solution = json.load(f)
|
||||
|
||||
print(f"loading iterate {filename}")
|
||||
for key in solution.keys():
|
||||
(field, stage) = key.split('_')
|
||||
self.set(int(stage), field, np.array(solution[key]))
|
||||
|
@ -1117,62 +1248,99 @@ class AcadosOcpSolver:
|
|||
"""
|
||||
Get the information of the last solver call.
|
||||
|
||||
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
|
||||
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter', 'residuals', 'qp_iter', 'alpha']
|
||||
|
||||
Available fileds:
|
||||
- time_tot: total CPU time previous call
|
||||
- time_lin: CPU time for linearization
|
||||
- time_sim: CPU time for integrator
|
||||
- time_sim_ad: CPU time for integrator contribution of external function calls
|
||||
- time_sim_la: CPU time for integrator contribution of linear algebra
|
||||
- time_qp: CPU time qp solution
|
||||
- time_qp_solver_call: CPU time inside qp solver (without converting the QP)
|
||||
- time_qp_xcond: time_glob: CPU time globalization
|
||||
- time_solution_sensitivities: CPU time for previous call to eval_param_sens
|
||||
- time_reg: CPU time regularization
|
||||
- sqp_iter: number of SQP iterations
|
||||
- qp_iter: vector of QP iterations for last SQP call
|
||||
- statistics: table with info about last iteration
|
||||
- stat_m: number of rows in statistics matrix
|
||||
- stat_n: number of columns in statistics matrix
|
||||
- residuals: residuals of last iterate
|
||||
- alpha: step sizes of SQP iterations
|
||||
"""
|
||||
|
||||
fields = ['time_tot', # total cpu time previous call
|
||||
'time_lin', # cpu time for linearization
|
||||
'time_sim', # cpu time for integrator
|
||||
'time_sim_ad', # cpu time for integrator contribution of external function calls
|
||||
'time_sim_la', # cpu time for integrator contribution of linear algebra
|
||||
'time_qp', # cpu time qp solution
|
||||
'time_qp_solver_call', # cpu time inside qp solver (without converting the QP)
|
||||
double_fields = ['time_tot',
|
||||
'time_lin',
|
||||
'time_sim',
|
||||
'time_sim_ad',
|
||||
'time_sim_la',
|
||||
'time_qp',
|
||||
'time_qp_solver_call',
|
||||
'time_qp_xcond',
|
||||
'time_glob', # cpu time globalization
|
||||
'time_reg', # cpu time regularization
|
||||
'sqp_iter', # number of SQP iterations
|
||||
'qp_iter', # vector of QP iterations for last SQP call
|
||||
'statistics', # table with info about last iteration
|
||||
'time_glob',
|
||||
'time_solution_sensitivities',
|
||||
'time_reg'
|
||||
]
|
||||
fields = double_fields + [
|
||||
'sqp_iter',
|
||||
'qp_iter',
|
||||
'statistics',
|
||||
'stat_m',
|
||||
'stat_n',]
|
||||
'stat_n',
|
||||
'residuals',
|
||||
'alpha',
|
||||
]
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
if (field_ not in fields):
|
||||
raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(fields, fields))
|
||||
|
||||
if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
|
||||
out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_int64))
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
return out
|
||||
|
||||
# TODO: just return double instead of np.
|
||||
elif field_ in double_fields:
|
||||
out = np.zeros((1,))
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
return out
|
||||
|
||||
elif field_ == 'statistics':
|
||||
sqp_iter = self.get_stats("sqp_iter")
|
||||
stat_m = self.get_stats("stat_m")
|
||||
stat_n = self.get_stats("stat_n")
|
||||
|
||||
min_size = min([stat_m, sqp_iter+1])
|
||||
|
||||
out = np.ascontiguousarray(
|
||||
np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
return out
|
||||
|
||||
elif field_ == 'qp_iter':
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
|
||||
out = full_stats[6, :]
|
||||
elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
out = full_stats[2, :]
|
||||
if self.solver_options['nlp_solver_type'] == 'SQP':
|
||||
return full_stats[6, :]
|
||||
elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
|
||||
return full_stats[2, :]
|
||||
|
||||
elif field_ == 'alpha':
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.solver_options['nlp_solver_type'] == 'SQP':
|
||||
return full_stats[7, :]
|
||||
else: # self.solver_options['nlp_solver_type'] == 'SQP_RTI':
|
||||
raise Exception("alpha values are not available for SQP_RTI")
|
||||
|
||||
elif field_ == 'residuals':
|
||||
return self.get_residuals()
|
||||
|
||||
else:
|
||||
out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
if not field_ == 'qp_iter':
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
|
||||
return out
|
||||
raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.'
|
||||
+ f'\n Possible values are {fields}.')
|
||||
|
||||
|
||||
def get_cost(self):
|
||||
|
@ -1201,7 +1369,7 @@ class AcadosOcpSolver:
|
|||
Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
|
||||
"""
|
||||
# compute residuals if RTI
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
if self.solver_options['nlp_solver_type'] == 'SQP_RTI':
|
||||
self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
|
@ -1230,9 +1398,7 @@ class AcadosOcpSolver:
|
|||
|
||||
|
||||
# Note: this function should not be used anymore, better use cost_set, constraints_set
|
||||
|
||||
def set(self, stage_, field_, value_):
|
||||
|
||||
"""
|
||||
Set numerical data inside the solver.
|
||||
|
||||
|
@ -1253,6 +1419,7 @@ class AcadosOcpSolver:
|
|||
cost_fields = ['y_ref', 'yref']
|
||||
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
|
||||
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
|
||||
mem_fields = ['xdot_guess']
|
||||
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
|
@ -1294,18 +1461,25 @@ class AcadosOcpSolver:
|
|||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
if field_ in constraints_fields:
|
||||
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
|
||||
elif field_ in cost_fields:
|
||||
self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
|
||||
elif field_ in out_fields:
|
||||
self.shared_lib.ocp_nlp_out_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage, field, value_data_p)
|
||||
# elif field_ in mem_fields:
|
||||
# self.shared_lib.ocp_nlp_set(self.nlp_config, \
|
||||
# self.nlp_solver, stage, field, value_data_p)
|
||||
|
||||
elif field_ in mem_fields:
|
||||
self.shared_lib.ocp_nlp_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_set(self.nlp_config, \
|
||||
self.nlp_solver, stage, field, value_data_p)
|
||||
return
|
||||
|
||||
|
||||
|
@ -1364,9 +1538,8 @@ class AcadosOcpSolver:
|
|||
raise Exception("Unknown api: '{}'".format(api))
|
||||
|
||||
if value_shape != tuple(dims):
|
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
|
||||
' for field "{}" with dimension {} (you have {})'.format( \
|
||||
field_, tuple(dims), value_shape))
|
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
@ -1433,8 +1606,8 @@ class AcadosOcpSolver:
|
|||
raise Exception("Unknown api: '{}'".format(api))
|
||||
|
||||
if value_shape != tuple(dims):
|
||||
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
|
||||
' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
|
||||
raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
@ -1490,11 +1663,11 @@ class AcadosOcpSolver:
|
|||
"""
|
||||
Set options of the solver.
|
||||
|
||||
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
|
||||
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC'
|
||||
:param value: of type int, float
|
||||
"""
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
|
||||
string_fields = ['globalization']
|
||||
|
||||
# check field availability and type
|
||||
|
@ -1522,10 +1695,10 @@ class AcadosOcpSolver:
|
|||
|
||||
if field_ == 'rti_phase':
|
||||
if value_ < 0 or value_ > 2:
|
||||
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
|
||||
raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
|
||||
'take only values 0, 1, 2 for SQP-RTI-type solvers')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
|
||||
if self.solver_options['nlp_solver_type'] != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
|
||||
'take only value 0 for SQP-type solvers')
|
||||
|
||||
# encode
|
||||
|
|
|
@ -1,402 +0,0 @@
|
|||
import sys
|
||||
import os
|
||||
import json
|
||||
import numpy as np
|
||||
from datetime import datetime
|
||||
|
||||
from ctypes import POINTER, CDLL, c_void_p, c_int, cast, c_double, c_char_p
|
||||
|
||||
from copy import deepcopy
|
||||
|
||||
from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
|
||||
from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
|
||||
from .generate_c_code_gnsf import generate_c_code_gnsf
|
||||
from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics
|
||||
from .generate_c_code_constraint import generate_c_code_constraint
|
||||
from .generate_c_code_nls_cost import generate_c_code_nls_cost
|
||||
from .generate_c_code_external_cost import generate_c_code_external_cost
|
||||
from .acados_ocp import AcadosOcp
|
||||
from .acados_model import acados_model_strip_casadi_symbolics
|
||||
from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
|
||||
format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
|
||||
set_up_imported_gnsf_model, get_acados_path
|
||||
|
||||
|
||||
class AcadosOcpSolverFast:
|
||||
dlclose = CDLL(None).dlclose
|
||||
dlclose.argtypes = [c_void_p]
|
||||
|
||||
def __init__(self, model_name, N, code_export_dir):
|
||||
|
||||
self.solver_created = False
|
||||
self.N = N
|
||||
self.model_name = model_name
|
||||
|
||||
self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model_name}.so'
|
||||
|
||||
# get shared_lib
|
||||
self.shared_lib = CDLL(self.shared_lib_name)
|
||||
|
||||
# create capsule
|
||||
getattr(self.shared_lib, f"{model_name}_acados_create_capsule").restype = c_void_p
|
||||
self.capsule = getattr(self.shared_lib, f"{model_name}_acados_create_capsule")()
|
||||
|
||||
# create solver
|
||||
getattr(self.shared_lib, f"{model_name}_acados_create").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_create").restype = c_int
|
||||
assert getattr(self.shared_lib, f"{model_name}_acados_create")(self.capsule)==0
|
||||
self.solver_created = True
|
||||
|
||||
# get pointers solver
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").restype = c_void_p
|
||||
self.nlp_opts = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").restype = c_void_p
|
||||
self.nlp_dims = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").restype = c_void_p
|
||||
self.nlp_config = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").restype = c_void_p
|
||||
self.nlp_out = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").restype = c_void_p
|
||||
self.nlp_in = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").restype = c_void_p
|
||||
self.nlp_solver = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver")(self.capsule)
|
||||
|
||||
|
||||
def solve(self):
|
||||
"""
|
||||
Solve the ocp with current input.
|
||||
"""
|
||||
model_name = self.model_name
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_solve").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_solve").restype = c_int
|
||||
status = getattr(self.shared_lib, f"{model_name}_acados_solve")(self.capsule)
|
||||
return status
|
||||
|
||||
def cost_set(self, start_stage_, field_, value_, api='warn'):
|
||||
self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
|
||||
return
|
||||
|
||||
def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
|
||||
"""
|
||||
Set numerical data in the cost module of the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
|
||||
:param value: of appropriate size
|
||||
"""
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64)
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
dim = np.product(value_.shape[1:])
|
||||
|
||||
start_stage = c_int(start_stage_)
|
||||
end_stage = c_int(end_stage_)
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
|
||||
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
|
||||
|
||||
value_shape = value_.shape
|
||||
expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
|
||||
if len(value_shape) == 2:
|
||||
value_shape = (value_shape[0], value_shape[1], 0)
|
||||
|
||||
elif len(value_shape) == 3:
|
||||
if api=='old':
|
||||
pass
|
||||
elif api=='warn':
|
||||
if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
|
||||
raise Exception("Ambiguity in API detected.\n"
|
||||
"Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n"
|
||||
"Are you seeing this error suddenly in previously running code? Read on.\n"
|
||||
" You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) +
|
||||
" acados_template now correctly passes on any matrices to acados in column major format.\n" +
|
||||
" Two options to fix this error: \n" +
|
||||
" * Add api='old' to cost_set to restore old incorrect behaviour\n" +
|
||||
" * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " +
|
||||
"such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
|
||||
"If there is no such manipulation, then you have probably been getting an incorrect solution before.")
|
||||
# Get elements in column major order
|
||||
value_ = np.ravel(value_, order='F')
|
||||
elif api=='new':
|
||||
# Get elements in column major order
|
||||
value_ = np.ravel(value_, order='F')
|
||||
else:
|
||||
raise Exception("Unknown api: '{}'".format(api))
|
||||
|
||||
if value_shape != expected_shape:
|
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension',
|
||||
' for field "{}" with dimension {} (you have {})'.format(
|
||||
field_, expected_shape, value_shape))
|
||||
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
|
||||
self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
|
||||
return
|
||||
|
||||
def constraints_set(self, start_stage_, field_, value_, api='warn'):
|
||||
self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
|
||||
return
|
||||
|
||||
def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
|
||||
"""
|
||||
Set numerical data in the constraint module of the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi']
|
||||
:param value: of appropriate size
|
||||
"""
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
value_ = value_.astype(float)
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
dim = np.product(value_.shape[1:])
|
||||
|
||||
start_stage = c_int(start_stage_)
|
||||
end_stage = c_int(end_stage_)
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
|
||||
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
|
||||
|
||||
value_shape = value_.shape
|
||||
expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
|
||||
if len(value_shape) == 2:
|
||||
value_shape = (value_shape[0], value_shape[1], 0)
|
||||
elif len(value_shape) == 3:
|
||||
if api=='old':
|
||||
pass
|
||||
elif api=='warn':
|
||||
if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
|
||||
raise Exception("Ambiguity in API detected.\n"
|
||||
"Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n"
|
||||
"Are you seeing this error suddenly in previously running code? Read on.\n"
|
||||
" You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) +
|
||||
" acados_template now correctly passes on any matrices to acados in column major format.\n" +
|
||||
" Two options to fix this error: \n" +
|
||||
" * Add api='old' to constraints_set to restore old incorrect behaviour\n" +
|
||||
" * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " +
|
||||
"such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
|
||||
"If there is no such manipulation, then you have probably been getting an incorrect solution before.")
|
||||
# Get elements in column major order
|
||||
value_ = np.ravel(value_, order='F')
|
||||
elif api=='new':
|
||||
# Get elements in column major order
|
||||
value_ = np.ravel(value_, order='F')
|
||||
else:
|
||||
raise Exception("Unknown api: '{}'".format(api))
|
||||
if value_shape != expected_shape:
|
||||
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
|
||||
' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape))
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
|
||||
self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
|
||||
return
|
||||
|
||||
# Note: this function should not be used anymore, better use cost_set, constraints_set
|
||||
def set(self, stage_, field_, value_):
|
||||
"""
|
||||
Set numerical data inside the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
|
||||
|
||||
.. note:: regarding lam, t: \n
|
||||
the inequalities are internally organized in the following order: \n
|
||||
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
|
||||
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
|
||||
|
||||
.. note:: pi: multipliers for dynamics equality constraints \n
|
||||
lam: multipliers for inequalities \n
|
||||
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
|
||||
sl: slack variables of soft lower inequality constraints \n
|
||||
su: slack variables of soft upper inequality constraints \n
|
||||
"""
|
||||
cost_fields = ['y_ref', 'yref']
|
||||
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
|
||||
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
|
||||
mem_fields = ['sl', 'su']
|
||||
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
value_ = value_.astype(float)
|
||||
|
||||
model_name = self.model_name
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
|
||||
stage = c_int(stage_)
|
||||
|
||||
# treat parameters separately
|
||||
if field_ == 'p':
|
||||
getattr(self.shared_lib, f"{model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_update_params").restype = c_int
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
|
||||
assert getattr(self.shared_lib, f"{model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0
|
||||
else:
|
||||
if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
|
||||
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
|
||||
\nPossible values are {}. Exiting.".format(field, \
|
||||
constraints_fields + cost_fields + out_fields + ['p']))
|
||||
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p]
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
|
||||
|
||||
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage_, field)
|
||||
|
||||
if value_.shape[0] != dims:
|
||||
msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
|
||||
msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
|
||||
raise Exception(msg)
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
if field_ in constraints_fields:
|
||||
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
|
||||
elif field_ in cost_fields:
|
||||
self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p)
|
||||
elif field_ in out_fields:
|
||||
self.shared_lib.ocp_nlp_out_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage, field, value_data_p)
|
||||
elif field_ in mem_fields:
|
||||
self.shared_lib.ocp_nlp_set.argtypes = \
|
||||
[c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_set(self.nlp_config, \
|
||||
self.nlp_solver, stage, field, value_data_p)
|
||||
return
|
||||
|
||||
|
||||
def get_slice(self, start_stage_, end_stage_, field_):
|
||||
"""
|
||||
Get the last solution of the solver:
|
||||
|
||||
:param start_stage: integer corresponding to shooting node that indicates start of slice
|
||||
:param end_stage: integer corresponding to shooting node that indicates end of slice
|
||||
:param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
|
||||
|
||||
.. note:: regarding lam, t: \n
|
||||
the inequalities are internally organized in the following order: \n
|
||||
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
|
||||
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
|
||||
|
||||
.. note:: pi: multipliers for dynamics equality constraints \n
|
||||
lam: multipliers for inequalities \n
|
||||
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
|
||||
sl: slack variables of soft lower inequality constraints \n
|
||||
su: slack variables of soft upper inequality constraints \n
|
||||
"""
|
||||
out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
|
||||
mem_fields = ['sl', 'su']
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
|
||||
if (field_ not in out_fields + mem_fields):
|
||||
raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(field_, out_fields))
|
||||
|
||||
if not isinstance(start_stage_, int):
|
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
|
||||
|
||||
if not isinstance(end_stage_, int):
|
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
|
||||
|
||||
if start_stage_ >= end_stage_:
|
||||
raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
|
||||
|
||||
if start_stage_ < 0 or end_stage_ > self.N + 1:
|
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p]
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
|
||||
|
||||
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, start_stage_, field)
|
||||
|
||||
out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
if (field_ in out_fields):
|
||||
self.shared_lib.ocp_nlp_out_get_slice.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data)
|
||||
elif field_ in mem_fields:
|
||||
self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
|
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
|
||||
|
||||
return out
|
||||
|
||||
def get_cost(self):
|
||||
"""
|
||||
Returns the cost value of the current solution.
|
||||
"""
|
||||
# compute cost internally
|
||||
self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p]
|
||||
self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
# create output array
|
||||
out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
# call getter
|
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
|
||||
field = "cost_value".encode('utf-8')
|
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
|
||||
|
||||
return out[0]
|
|
@ -39,22 +39,19 @@ cimport cython
|
|||
from libc cimport string
|
||||
|
||||
cimport acados_solver_common
|
||||
# TODO: make this import more clear? it is not a general solver, but problem specific.
|
||||
cimport acados_solver
|
||||
|
||||
cimport numpy as cnp
|
||||
|
||||
import os
|
||||
import json
|
||||
from datetime import datetime
|
||||
import numpy as np
|
||||
|
||||
|
||||
cdef class AcadosOcpSolverFast:
|
||||
cdef class AcadosOcpSolverCython:
|
||||
"""
|
||||
Class to interact with the acados ocp solver C object.
|
||||
|
||||
:param model_name:
|
||||
:param N:
|
||||
"""
|
||||
|
||||
cdef acados_solver.nlp_solver_capsule *capsule
|
||||
|
@ -62,19 +59,26 @@ cdef class AcadosOcpSolverFast:
|
|||
cdef acados_solver_common.ocp_nlp_dims *nlp_dims
|
||||
cdef acados_solver_common.ocp_nlp_config *nlp_config
|
||||
cdef acados_solver_common.ocp_nlp_out *nlp_out
|
||||
cdef acados_solver_common.ocp_nlp_out *sens_out
|
||||
cdef acados_solver_common.ocp_nlp_in *nlp_in
|
||||
cdef acados_solver_common.ocp_nlp_solver *nlp_solver
|
||||
|
||||
cdef str model_name
|
||||
cdef int N
|
||||
cdef int status
|
||||
cdef bint solver_created
|
||||
|
||||
def __cinit__(self, str model_name, int N):
|
||||
self.model_name = model_name
|
||||
self.N = N
|
||||
cdef str model_name
|
||||
cdef int N
|
||||
|
||||
cdef str nlp_solver_type
|
||||
|
||||
def __cinit__(self, model_name, nlp_solver_type, N):
|
||||
|
||||
self.solver_created = False
|
||||
|
||||
self.N = N
|
||||
self.model_name = model_name
|
||||
self.nlp_solver_type = nlp_solver_type
|
||||
|
||||
# create capsule
|
||||
self.capsule = acados_solver.acados_create_capsule()
|
||||
|
||||
|
@ -82,11 +86,21 @@ cdef class AcadosOcpSolverFast:
|
|||
assert acados_solver.acados_create(self.capsule) == 0
|
||||
self.solver_created = True
|
||||
|
||||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
self.status = 0
|
||||
|
||||
|
||||
def __get_pointers_solver(self):
|
||||
"""
|
||||
Private function to get the pointers for solver
|
||||
"""
|
||||
# get pointers solver
|
||||
self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
|
||||
self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
|
||||
self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
|
||||
self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
|
||||
self.sens_out = acados_solver.acados_get_sens_out(self.capsule)
|
||||
self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
|
||||
self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
|
||||
|
||||
|
@ -100,15 +114,112 @@ cdef class AcadosOcpSolverFast:
|
|||
|
||||
def set_new_time_steps(self, new_time_steps):
|
||||
"""
|
||||
Set new time steps before solving. Only reload library without code generation but with new time steps.
|
||||
Set new time steps.
|
||||
Recreates the solver if N changes.
|
||||
|
||||
:param new_time_steps: vector of new time steps for the solver
|
||||
:param new_time_steps: 1 dimensional np array of new time steps for the solver
|
||||
|
||||
.. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
|
||||
the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
|
||||
do not require a new code export and compilation.
|
||||
"""
|
||||
raise NotImplementedError()
|
||||
|
||||
raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature")
|
||||
# # unlikely but still possible
|
||||
# if not self.solver_created:
|
||||
# raise Exception('Solver was not yet created!')
|
||||
|
||||
# ## check if time steps really changed in value
|
||||
# # get time steps
|
||||
# cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64)
|
||||
# assert acados_solver.acados_get_time_steps(self.capsule, self.N, <double *> old_time_steps.data)
|
||||
|
||||
# if np.array_equal(old_time_steps, new_time_steps):
|
||||
# return
|
||||
|
||||
# N = new_time_steps.size
|
||||
# cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64)
|
||||
|
||||
# # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
|
||||
# if len(old_time_steps) == N:
|
||||
# assert acados_solver.acados_update_time_steps(self.capsule, N, <double *> value.data) == 0
|
||||
|
||||
# else: # recreate the solver with the new time steps
|
||||
# self.solver_created = False
|
||||
|
||||
# # delete old memory (analog to __del__)
|
||||
# acados_solver.acados_free(self.capsule)
|
||||
|
||||
# # create solver with new time steps
|
||||
# assert acados_solver.acados_create_with_discretization(self.capsule, N, <double *> value.data) == 0
|
||||
|
||||
# self.solver_created = True
|
||||
|
||||
# # get pointers solver
|
||||
# self.__get_pointers_solver()
|
||||
|
||||
# # store time_steps, N
|
||||
# self.time_steps = new_time_steps
|
||||
# self.N = N
|
||||
|
||||
|
||||
def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
|
||||
"""
|
||||
Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
|
||||
This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
|
||||
the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
|
||||
:param qp_solver_cond_N: new number of condensing stages for the solver
|
||||
|
||||
.. note:: This function can only be used in combination with a partial condensing QP solver.
|
||||
|
||||
.. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
|
||||
necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
|
||||
`qp_solver_cond_N < N`.
|
||||
"""
|
||||
raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature")
|
||||
|
||||
# # unlikely but still possible
|
||||
# if not self.solver_created:
|
||||
# raise Exception('Solver was not yet created!')
|
||||
# if self.N < qp_solver_cond_N:
|
||||
# raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
|
||||
# if self.qp_solver_cond_N != qp_solver_cond_N:
|
||||
# self.solver_created = False
|
||||
|
||||
# # recreate the solver
|
||||
# acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N)
|
||||
|
||||
# # store the new value
|
||||
# self.qp_solver_cond_N = qp_solver_cond_N
|
||||
# self.solver_created = True
|
||||
|
||||
# # get pointers solver
|
||||
# self.__get_pointers_solver()
|
||||
|
||||
|
||||
def eval_param_sens(self, index, stage=0, field="ex"):
|
||||
"""
|
||||
Calculate the sensitivity of the curent solution with respect to the initial state component of index
|
||||
|
||||
:param index: integer corresponding to initial state index in range(nx)
|
||||
"""
|
||||
|
||||
field_ = field
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
# checks
|
||||
if not isinstance(index, int):
|
||||
raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.')
|
||||
|
||||
cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
|
||||
|
||||
if index < 0 or index > nx:
|
||||
raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
|
||||
|
||||
# actual eval_param
|
||||
acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
|
||||
|
||||
return
|
||||
|
||||
|
||||
def get(self, int stage, str field_):
|
||||
|
@ -134,14 +245,14 @@ cdef class AcadosOcpSolverFast:
|
|||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ not in out_fields:
|
||||
raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
|
||||
raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(field_, out_fields))
|
||||
|
||||
if stage < 0 or stage > self.N:
|
||||
raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
|
||||
if stage == self.N and field_ == 'pi':
|
||||
raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
|
||||
raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\
|
||||
.format(field_, stage))
|
||||
|
||||
cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
|
||||
|
@ -179,6 +290,7 @@ cdef class AcadosOcpSolverFast:
|
|||
:param filename: if not set, use model_name + timestamp + '.json'
|
||||
:param overwrite: if false and filename exists add timestamp to filename
|
||||
"""
|
||||
import json
|
||||
if filename == '':
|
||||
filename += self.model_name + '_' + 'iterate' + '.json'
|
||||
|
||||
|
@ -212,6 +324,7 @@ cdef class AcadosOcpSolverFast:
|
|||
"""
|
||||
Loads the iterate stored in json file with filename into the ocp solver.
|
||||
"""
|
||||
import json
|
||||
if not os.path.isfile(filename):
|
||||
raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
|
||||
|
||||
|
@ -228,34 +341,53 @@ cdef class AcadosOcpSolverFast:
|
|||
Get the information of the last solver call.
|
||||
|
||||
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
|
||||
Available fileds:
|
||||
- time_tot: total CPU time previous call
|
||||
- time_lin: CPU time for linearization
|
||||
- time_sim: CPU time for integrator
|
||||
- time_sim_ad: CPU time for integrator contribution of external function calls
|
||||
- time_sim_la: CPU time for integrator contribution of linear algebra
|
||||
- time_qp: CPU time qp solution
|
||||
- time_qp_solver_call: CPU time inside qp solver (without converting the QP)
|
||||
- time_qp_xcond: time_glob: CPU time globalization
|
||||
- time_solution_sensitivities: CPU time for previous call to eval_param_sens
|
||||
- time_reg: CPU time regularization
|
||||
- sqp_iter: number of SQP iterations
|
||||
- qp_iter: vector of QP iterations for last SQP call
|
||||
- statistics: table with info about last iteration
|
||||
- stat_m: number of rows in statistics matrix
|
||||
- stat_n: number of columns in statistics matrix
|
||||
- residuals: residuals of last iterate
|
||||
- alpha: step sizes of SQP iterations
|
||||
"""
|
||||
fields = ['time_tot', # total cpu time previous call
|
||||
'time_lin', # cpu time for linearization
|
||||
'time_sim', # cpu time for integrator
|
||||
'time_sim_ad', # cpu time for integrator contribution of external function calls
|
||||
'time_sim_la', # cpu time for integrator contribution of linear algebra
|
||||
'time_qp', # cpu time qp solution
|
||||
'time_qp_solver_call', # cpu time inside qp solver (without converting the QP)
|
||||
|
||||
double_fields = ['time_tot',
|
||||
'time_lin',
|
||||
'time_sim',
|
||||
'time_sim_ad',
|
||||
'time_sim_la',
|
||||
'time_qp',
|
||||
'time_qp_solver_call',
|
||||
'time_qp_xcond',
|
||||
'time_glob', # cpu time globalization
|
||||
'time_reg', # cpu time regularization
|
||||
'sqp_iter', # number of SQP iterations
|
||||
'qp_iter', # vector of QP iterations for last SQP call
|
||||
'statistics', # table with info about last iteration
|
||||
'time_glob',
|
||||
'time_solution_sensitivities',
|
||||
'time_reg'
|
||||
]
|
||||
fields = double_fields + [
|
||||
'sqp_iter',
|
||||
'qp_iter',
|
||||
'statistics',
|
||||
'stat_m',
|
||||
'stat_n',]
|
||||
|
||||
field = field_
|
||||
field = field.encode('utf-8')
|
||||
|
||||
if (field_ not in fields):
|
||||
raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
|
||||
\n Possible values are {}. Exiting.'.format(fields, fields))
|
||||
'stat_n',
|
||||
'residuals',
|
||||
'alpha',
|
||||
]
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
|
||||
return self.__get_stat_int(field)
|
||||
|
||||
elif field_.startswith('time'):
|
||||
elif field_ in double_fields:
|
||||
return self.__get_stat_double(field)
|
||||
|
||||
elif field_ == 'statistics':
|
||||
|
@ -266,14 +398,24 @@ cdef class AcadosOcpSolverFast:
|
|||
return self.__get_stat_matrix(field, stat_n+1, min_size)
|
||||
|
||||
elif field_ == 'qp_iter':
|
||||
NotImplementedError("TODO! Cython not aware if SQP or SQP_RTI")
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
|
||||
out = full_stats[6, :]
|
||||
elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
out = full_stats[2, :]
|
||||
if self.nlp_solver_type == 'SQP':
|
||||
return full_stats[6, :]
|
||||
elif self.nlp_solver_type == 'SQP_RTI':
|
||||
return full_stats[2, :]
|
||||
|
||||
elif field_ == 'alpha':
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.nlp_solver_type == 'SQP':
|
||||
return full_stats[7, :]
|
||||
else: # self.nlp_solver_type == 'SQP_RTI':
|
||||
raise Exception("alpha values are not available for SQP_RTI")
|
||||
|
||||
elif field_ == 'residuals':
|
||||
return self.get_residuals()
|
||||
|
||||
else:
|
||||
NotImplementedError("TODO!")
|
||||
raise NotImplementedError("TODO!")
|
||||
|
||||
|
||||
def __get_stat_int(self, field):
|
||||
|
@ -312,10 +454,9 @@ cdef class AcadosOcpSolverFast:
|
|||
"""
|
||||
Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
|
||||
"""
|
||||
# TODO: check if RTI, only eval then
|
||||
# if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
|
||||
# compute residuals if RTI
|
||||
acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
if self.nlp_solver_type == 'SQP_RTI':
|
||||
acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
# create output array
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64))
|
||||
|
@ -363,6 +504,7 @@ cdef class AcadosOcpSolverFast:
|
|||
cost_fields = ['y_ref', 'yref']
|
||||
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
|
||||
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
|
||||
mem_fields = ['xdot_guess']
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
|
@ -373,7 +515,7 @@ cdef class AcadosOcpSolverFast:
|
|||
assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
|
||||
else:
|
||||
if field_ not in constraints_fields + cost_fields + out_fields:
|
||||
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
|
||||
raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\
|
||||
\nPossible values are {}. Exiting.".format(field, \
|
||||
constraints_fields + cost_fields + out_fields + ['p']))
|
||||
|
||||
|
@ -381,7 +523,7 @@ cdef class AcadosOcpSolverFast:
|
|||
self.nlp_dims, self.nlp_out, stage, field)
|
||||
|
||||
if value_.shape[0] != dims:
|
||||
msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
|
||||
msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_)
|
||||
msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
|
||||
raise Exception(msg)
|
||||
|
||||
|
@ -394,6 +536,9 @@ cdef class AcadosOcpSolverFast:
|
|||
elif field_ in out_fields:
|
||||
acados_solver_common.ocp_nlp_out_set(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_out, stage, field, <void *> value.data)
|
||||
elif field_ in mem_fields:
|
||||
acados_solver_common.ocp_nlp_set(self.nlp_config, \
|
||||
self.nlp_solver, stage, field, <void *> value.data)
|
||||
|
||||
|
||||
def cost_set(self, int stage, str field_, value_):
|
||||
|
@ -422,9 +567,8 @@ cdef class AcadosOcpSolverFast:
|
|||
value = np.asfortranarray(value_)
|
||||
|
||||
if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
|
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
|
||||
' for field "{}" with dimension {} (you have {})'.format( \
|
||||
field_, tuple(dims), value_shape))
|
||||
raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
|
||||
|
@ -456,8 +600,8 @@ cdef class AcadosOcpSolverFast:
|
|||
value = np.asfortranarray(value_)
|
||||
|
||||
if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
|
||||
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
|
||||
' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
|
||||
raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
|
||||
|
@ -494,8 +638,8 @@ cdef class AcadosOcpSolverFast:
|
|||
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
|
||||
:param value: of type int, float
|
||||
"""
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
|
||||
string_fields = ['globalization']
|
||||
|
||||
# encode
|
||||
|
@ -512,10 +656,10 @@ cdef class AcadosOcpSolverFast:
|
|||
|
||||
if field_ == 'rti_phase':
|
||||
if value_ < 0 or value_ > 2:
|
||||
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
|
||||
raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
|
||||
'take only values 0, 1, 2 for SQP-RTI-type solvers')
|
||||
if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
|
||||
if self.nlp_solver_type != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
|
||||
'take only value 0 for SQP-type solvers')
|
||||
|
||||
int_value = value_
|
||||
|
@ -536,7 +680,7 @@ cdef class AcadosOcpSolverFast:
|
|||
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
|
||||
|
||||
else:
|
||||
raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
|
||||
raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\
|
||||
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
|
||||
|
||||
|
||||
|
|
|
@ -70,28 +70,28 @@ class AcadosSimDims:
|
|||
|
||||
@nx.setter
|
||||
def nx(self, nx):
|
||||
if type(nx) == int and nx > 0:
|
||||
if isinstance(nx, int) and nx > 0:
|
||||
self.__nx = nx
|
||||
else:
|
||||
raise Exception('Invalid nx value, expected positive integer. Exiting.')
|
||||
|
||||
@nz.setter
|
||||
def nz(self, nz):
|
||||
if type(nz) == int and nz > -1:
|
||||
if isinstance(nz, int) and nz > -1:
|
||||
self.__nz = nz
|
||||
else:
|
||||
raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@nu.setter
|
||||
def nu(self, nu):
|
||||
if type(nu) == int and nu > -1:
|
||||
if isinstance(nu, int) and nu > -1:
|
||||
self.__nu = nu
|
||||
else:
|
||||
raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
|
||||
|
||||
@np.setter
|
||||
def np(self, np):
|
||||
if type(np) == int and np > -1:
|
||||
if isinstance(np, int) and np > -1:
|
||||
self.__np = np
|
||||
else:
|
||||
raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
|
||||
|
@ -302,6 +302,7 @@ class AcadosSim:
|
|||
self.code_export_directory = 'c_generated_code'
|
||||
"""Path to where code will be exported. Default: `c_generated_code`."""
|
||||
|
||||
self.cython_include_dirs = ''
|
||||
self.__parameter_values = np.array([])
|
||||
|
||||
@property
|
||||
|
|
|
@ -215,6 +215,24 @@ class AcadosSimSolver:
|
|||
model_name = self.sim_struct.model.name
|
||||
self.model_name = model_name
|
||||
|
||||
# Load acados library to avoid unloading the library.
|
||||
# This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
|
||||
# Unloading a library which uses OpenMP results in a segfault (on any platform?).
|
||||
# see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
|
||||
# or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
|
||||
libacados_name = 'libacados.so'
|
||||
libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name)
|
||||
self.__acados_lib = CDLL(libacados_filepath)
|
||||
# find out if acados was compiled with OpenMP
|
||||
try:
|
||||
self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
|
||||
except AttributeError as e:
|
||||
self.__acados_lib_uses_omp = False
|
||||
if self.__acados_lib_uses_omp:
|
||||
print('acados was compiled with OpenMP.')
|
||||
else:
|
||||
print('acados was compiled without OpenMP.')
|
||||
|
||||
# Ctypes
|
||||
shared_lib = f'{code_export_dir}/libacados_sim_solver_{model_name}.so'
|
||||
self.shared_lib = CDLL(shared_lib)
|
||||
|
|
|
@ -95,6 +95,7 @@ cdef extern from "acados_c/ocp_nlp_interface.h":
|
|||
|
||||
# solver
|
||||
void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
|
||||
void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out)
|
||||
void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
|
||||
|
||||
# get/set
|
||||
|
|
|
@ -125,134 +125,134 @@
|
|||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{# acados flags #}
|
||||
ACADOS_FLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
ACADOS_FLAGS += -DACADOS_WITH_QPOASES
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
|
||||
ACADOS_FLAGS += -DACADOS_WITH_OSQP
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
|
||||
ACADOS_FLAGS += -DACADOS_WITH_QPDUNES
|
||||
{%- endif %}
|
||||
# # Debugging
|
||||
# ACADOS_FLAGS += -g3
|
||||
# define sources and use make's implicit rules to generate object files (*.o)
|
||||
|
||||
MODEL_OBJ=
|
||||
# model
|
||||
MODEL_SRC=
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.o
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.o
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.o
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.o
|
||||
{%- endif %}
|
||||
{%- else %}
|
||||
MODEL_OBJ+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
|
||||
{%- endif %}
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
|
||||
{%- endif %}
|
||||
{%- else %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
MODEL_OBJ := $(MODEL_SRC:.c=.o)
|
||||
|
||||
|
||||
OCP_OBJ=
|
||||
# optimal control problem - mostly CasADi exports
|
||||
OCP_SRC=
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.o
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.o
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.o
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.o
|
||||
{%- endif %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.o
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.o
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.o
|
||||
{%- endif %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
|
||||
{%- elif cost_type_0 == "EXTERNAL" %}
|
||||
{% if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
|
||||
{% else %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
|
||||
{% endif %}
|
||||
{%- if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
|
||||
{%- else %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
|
||||
{%- elif cost_type == "EXTERNAL" %}
|
||||
{% if cost.cost_ext_fun_type == "casadi" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
|
||||
{% elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
|
||||
{% endif %}
|
||||
{%- if cost.cost_ext_fun_type == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
|
||||
{%- elif cost_type_e == "EXTERNAL" %}
|
||||
{% if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
|
||||
{% elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
|
||||
OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
|
||||
{% endif %}
|
||||
{%- if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
OCP_OBJ+= acados_solver_{{ model.name }}.o
|
||||
OCP_SRC+= acados_solver_{{ model.name }}.c
|
||||
OCP_OBJ := $(OCP_SRC:.c=.o)
|
||||
|
||||
# for sim solver
|
||||
SIM_SRC= acados_sim_solver_{{ model.name }}.c
|
||||
SIM_OBJ := $(SIM_SRC:.c=.o)
|
||||
|
||||
SIM_OBJ=
|
||||
SIM_OBJ+= acados_sim_solver_{{ model.name }}.o
|
||||
# for target example
|
||||
EX_SRC= main_{{ model.name }}.c
|
||||
EX_OBJ := $(EX_SRC:.c=.o)
|
||||
EX_EXE := $(EX_SRC:.c=)
|
||||
|
||||
EX_OBJ=
|
||||
EX_OBJ+= main_{{ model.name }}.o
|
||||
|
||||
EX_SIM_OBJ=
|
||||
EX_SIM_OBJ+= main_sim_{{ model.name }}.o
|
||||
# for target example_sim
|
||||
EX_SIM_SRC= main_sim_{{ model.name }}.c
|
||||
EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
|
||||
EX_SIM_EXE := $(EX_SIM_SRC:.c=)
|
||||
|
||||
# combine model, sim and ocp object files
|
||||
OBJ=
|
||||
OBJ+= $(MODEL_OBJ)
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
|
@ -271,233 +271,103 @@ EXTERNAL_LIB+= {{ model_external_shared_lib_name }}
|
|||
INCLUDE_PATH = {{ acados_include_path }}
|
||||
LIB_PATH = {{ acados_lib_path }}
|
||||
|
||||
{%- if solver_options.integrator_type == "DISCRETE" %}
|
||||
all: clean casadi_fun example
|
||||
# preprocessor flags for make's implicit rules
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
CPPFLAGS += -DACADOS_WITH_QPOASES
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
|
||||
CPPFLAGS += -DACADOS_WITH_OSQP
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
|
||||
CPPFLAGS += -DACADOS_WITH_QPDUNES
|
||||
{%- endif %}
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/acados
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
|
||||
{# c-compiler flags #}
|
||||
# define the c-compiler flags for make's implicit rules
|
||||
CFLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
|
||||
# # Debugging
|
||||
# CFLAGS += -g3
|
||||
|
||||
# linker flags
|
||||
LDFLAGS+= -L$(LIB_PATH)
|
||||
|
||||
# link to libraries
|
||||
LDLIBS+= -lacados
|
||||
LDLIBS+= -lhpipm
|
||||
LDLIBS+= -lblasfeo
|
||||
LDLIBS+= -lm
|
||||
LDLIBS+= {{ link_libs }}
|
||||
|
||||
# libraries
|
||||
LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so
|
||||
LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so
|
||||
LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
|
||||
|
||||
# virtual targets
|
||||
.PHONY : all clean
|
||||
|
||||
#all: clean example_sim example shared_lib
|
||||
{% if solver_options.integrator_type == "DISCRETE" -%}
|
||||
all: clean example
|
||||
shared_lib: ocp_shared_lib
|
||||
{%- else %}
|
||||
all: clean casadi_fun example_sim example
|
||||
all: clean example_sim example
|
||||
shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
|
||||
{%- endif %}
|
||||
|
||||
CASADI_MODEL_SOURCE=
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
|
||||
# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun_jac_y.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_jac_y_uhat.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_get_matrices_fun.c
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" and model.dyn_ext_fun_type == "casadi" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun.c
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
CASADI_CON_PHI_SOURCE=
|
||||
CASADI_CON_PHI_SOURCE+= {{ model.name }}_phi_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
CASADI_CON_PHI_E_SOURCE=
|
||||
CASADI_CON_PHI_E_SOURCE+= {{ model.name }}_phi_e_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
CASADI_CON_H_SOURCE=
|
||||
CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt.c
|
||||
CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
# some linker targets
|
||||
example: $(EX_OBJ) $(OBJ)
|
||||
$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
|
||||
$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
{% if solver_options.integrator_type != "DISCRETE" -%}
|
||||
bundled_shared_lib: $(OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims_nh_e > 0 %}
|
||||
CASADI_CON_H_E_SOURCE=
|
||||
CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
|
||||
CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
|
||||
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
CASADI_COST_Y_SOURCE=
|
||||
CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun.c
|
||||
CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_hess.c
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
CASADI_COST_Y_E_SOURCE=
|
||||
CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun.c
|
||||
CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_hess.c
|
||||
{%- endif %}
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
CASADI_COST_Y_0_SOURCE=
|
||||
CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun.c
|
||||
CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_hess.c
|
||||
{%- endif %}
|
||||
|
||||
casadi_fun:
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
( cd {{ model.name }}_model {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_E_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
|
||||
( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_E_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_E_SOURCE))
|
||||
{%- endif %}
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_0_SOURCE))
|
||||
{%- endif %}
|
||||
|
||||
main:
|
||||
gcc $(ACADOS_FLAGS) -c main_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
-I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
|
||||
main_sim:
|
||||
gcc $(ACADOS_FLAGS) -c main_sim_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/
|
||||
|
||||
ocp_solver:
|
||||
gcc $(ACADOS_FLAGS) -c acados_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
-I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
|
||||
sim_solver:
|
||||
gcc $(ACADOS_FLAGS) -c acados_sim_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
-I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
|
||||
example: ocp_solver main
|
||||
gcc $(ACADOS_FLAGS) -o main_{{ model.name }} $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \
|
||||
-lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-I $(INCLUDE_PATH)/acados/ \
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
-I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
|
||||
example_sim: sim_solver main_sim
|
||||
gcc $(ACADOS_FLAGS) -o main_sim_{{ model.name }} $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) -L $(LIB_PATH) \
|
||||
-lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/acados/ \
|
||||
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
|
||||
bundled_shared_lib: casadi_fun ocp_solver sim_solver
|
||||
gcc $(ACADOS_FLAGS) -shared -o libacados_solver_{{ model.name }}.so $(OBJ) \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-L $(LIB_PATH) \
|
||||
-lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
|
||||
ocp_shared_lib: casadi_fun ocp_solver
|
||||
gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
|
||||
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
|
||||
{%- else %}
|
||||
|
||||
ocp_shared_lib: casadi_fun ocp_solver
|
||||
gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
|
||||
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
|
||||
{%- endif %}
|
||||
|
||||
# Cython targets
|
||||
ocp_cython_c: ocp_shared_lib
|
||||
cython \
|
||||
-o acados_ocp_solver_pyx.c \
|
||||
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
|
||||
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
|
||||
-I {{ code_export_directory }} \
|
||||
|
||||
ocp_cython_o: ocp_cython_c
|
||||
clang $(ACADOS_FLAGS) -c -O2 \
|
||||
$(CC) $(ACADOS_FLAGS) -c -O2 \
|
||||
-fPIC \
|
||||
-o acados_ocp_solver_pyx.o \
|
||||
-I /usr/include/python3.8 \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-I {{ cython_include_dirs }} \
|
||||
acados_ocp_solver_pyx.c \
|
||||
|
||||
ocp_cython: ocp_cython_o
|
||||
clang $(ACADOS_FLAGS) -shared \
|
||||
$(CC) $(ACADOS_FLAGS) -shared \
|
||||
-o acados_ocp_solver_pyx.so \
|
||||
-Wl,-rpath=$(LIB_PATH) \
|
||||
acados_ocp_solver_pyx.o \
|
||||
$(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
|
||||
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo -lqpOASES_e \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
|
||||
sim_shared_lib: casadi_fun sim_solver
|
||||
gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
|
||||
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
|
||||
{{ link_libs }} \
|
||||
-lm \
|
||||
$(LDFLAGS) $(LDLIBS)
|
||||
|
||||
{%- if os and os == "pc" %}
|
||||
|
||||
|
@ -510,15 +380,27 @@ clean_ocp_shared_lib:
|
|||
del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
|
||||
del \Q acados_solver_{{ model.name }}.o 2>nul
|
||||
|
||||
clean_ocp_cython:
|
||||
del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
|
||||
del \Q acados_solver_{{ model.name }}.o 2>nul
|
||||
del \Q acados_ocp_solver_pyx.so 2>nul
|
||||
del \Q acados_ocp_solver_pyx.o 2>nul
|
||||
|
||||
{%- else %}
|
||||
|
||||
clean:
|
||||
rm -f *.o
|
||||
rm -f *.so
|
||||
rm -f main_{{ model.name }}
|
||||
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
|
||||
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
|
||||
$(RM) $(EX_EXE) $(EX_SIM_EXE)
|
||||
|
||||
clean_ocp_shared_lib:
|
||||
rm -f libacados_ocp_solver_{{ model.name }}.so
|
||||
rm -f acados_solver_{{ model.name }}.o
|
||||
$(RM) $(LIBACADOS_OCP_SOLVER)
|
||||
$(RM) $(OCP_OBJ)
|
||||
|
||||
clean_ocp_cython:
|
||||
$(RM) libacados_ocp_solver_{{ model.name }}.so
|
||||
$(RM) acados_solver_{{ model.name }}.o
|
||||
$(RM) acados_ocp_solver_pyx.so
|
||||
$(RM) acados_ocp_solver_pyx.o
|
||||
|
||||
{%- endif %}
|
||||
|
|
|
@ -63,7 +63,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
mexPrintf("{{ model.name }}_acados_create() -> success!\n");
|
||||
|
||||
// get pointers to nlp solver related objects
|
||||
ocp_nlp_plan *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
|
||||
ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
|
||||
|
@ -238,14 +238,18 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
|
||||
{%- endif %}
|
||||
{% elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
l_ptr = mxGetData(gnsf_phi_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
|
||||
l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
|
||||
l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
|
||||
{% elif solver_options.integrator_type == "DISCRETE" %}
|
||||
|
|
|
@ -69,7 +69,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
|
||||
// plan
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
|
||||
ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0];
|
||||
ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0];
|
||||
// config
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
|
||||
ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
|
||||
|
@ -404,7 +404,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
}
|
||||
else if (!strcmp(field, "init_z"))
|
||||
{
|
||||
sim_solver_plan sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == IRK)
|
||||
{
|
||||
|
@ -426,7 +426,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
}
|
||||
else if (!strcmp(field, "init_xdot"))
|
||||
{
|
||||
sim_solver_plan sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == IRK)
|
||||
{
|
||||
|
@ -448,7 +448,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
|||
}
|
||||
else if (!strcmp(field, "init_gnsf_phi"))
|
||||
{
|
||||
sim_solver_plan sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == GNSF)
|
||||
{
|
||||
|
|
|
@ -164,12 +164,17 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
{%- endif %}
|
||||
|
||||
{% elif solver_options.integrator_type == "GNSF" -%}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
|
||||
capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
|
||||
capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
|
||||
|
@ -194,6 +199,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
|
||||
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
|
||||
|
@ -201,6 +207,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
|
||||
|
@ -212,7 +220,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
{% endif %}
|
||||
|
||||
// sim plan & config
|
||||
sim_solver_plan plan;
|
||||
sim_solver_plan_t plan;
|
||||
plan.sim_solver = {{ solver_options.integrator_type }};
|
||||
|
||||
// create correct config based on plan
|
||||
|
@ -307,14 +315,18 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
|||
"expl_ode_hess", capsule->sim_expl_ode_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_fun", capsule->sim_gnsf_phi_fun);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
|
||||
{%- endif %}
|
||||
|
@ -409,10 +421,14 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
|
|||
external_function_param_casadi_free(capsule->sim_expl_ode_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_fun);
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y);
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun);
|
||||
{% endif %}
|
||||
|
||||
|
@ -445,10 +461,14 @@ int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, doubl
|
|||
capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
|
||||
capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
|
||||
capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
|
||||
{% endif %}
|
||||
|
||||
|
|
|
@ -37,7 +37,7 @@
|
|||
#define MDL_START
|
||||
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -78,9 +78,10 @@ typedef struct {{ model.name }}_solver_capsule
|
|||
// acados objects
|
||||
ocp_nlp_in *nlp_in;
|
||||
ocp_nlp_out *nlp_out;
|
||||
ocp_nlp_out *sens_out;
|
||||
ocp_nlp_solver *nlp_solver;
|
||||
void *nlp_opts;
|
||||
ocp_nlp_plan *nlp_solver_plan;
|
||||
ocp_nlp_plan_t *nlp_solver_plan;
|
||||
ocp_nlp_config *nlp_config;
|
||||
ocp_nlp_dims *nlp_dims;
|
||||
|
||||
|
@ -186,6 +187,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
|
|||
* nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
|
||||
*/
|
||||
int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
|
||||
/**
|
||||
* This function is used for updating an already initialized solver with a different number of qp_cond_N.
|
||||
*/
|
||||
int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N);
|
||||
int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
|
||||
int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
|
||||
int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
|
||||
|
@ -193,11 +198,12 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsu
|
|||
|
||||
ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
|
||||
void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
|
||||
ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
|
|
|
@ -1,3 +1,36 @@
|
|||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
cimport acados_solver_common
|
||||
|
||||
cdef extern from "acados_solver_{{ model.name }}.h":
|
||||
|
@ -8,6 +41,11 @@ cdef extern from "acados_solver_{{ model.name }}.h":
|
|||
int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
|
||||
|
||||
int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
|
||||
|
||||
int acados_create_with_discretization "{{ model.name }}_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
|
||||
int acados_update_time_steps "{{ model.name }}_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
|
||||
int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
|
||||
|
||||
int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
|
||||
int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
|
||||
int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
|
||||
|
@ -15,6 +53,7 @@ cdef extern from "acados_solver_{{ model.name }}.h":
|
|||
|
||||
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
|
||||
void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
|
||||
|
|
|
@ -37,7 +37,7 @@
|
|||
#define MDL_START
|
||||
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
|
|
|
@ -156,11 +156,12 @@ int main()
|
|||
for (int ii = 0; ii < NTIMINGS; ii++)
|
||||
{
|
||||
// initialize solution
|
||||
for (int i = 0; i <= nlp_dims->N; i++)
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
|
||||
}
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
|
||||
status = {{ model.name }}_acados_solve(acados_ocp_capsule);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
|
||||
|
|
|
@ -46,10 +46,14 @@ SOURCES = { ...
|
|||
'{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',...
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',...
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',...
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',...
|
||||
|
|
|
@ -47,10 +47,14 @@ SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
|
|||
'{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c '
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c '
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c '
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c '
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c '
|
||||
{%- endif %}
|
||||
];
|
||||
|
|
|
@ -125,15 +125,15 @@ classdef {{ model.name }}_mex_solver < handle
|
|||
if strcmp(field, 'stat')
|
||||
stat = obj.get('stat');
|
||||
{%- if solver_options.nlp_solver_type == "SQP" %}
|
||||
fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter');
|
||||
if size(stat,2)>7
|
||||
fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha');
|
||||
if size(stat,2)>8
|
||||
fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
|
||||
end
|
||||
fprintf('\n');
|
||||
for jj=1:size(stat,1)
|
||||
fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
|
||||
if size(stat,2)>7
|
||||
fprintf('\t%e\t%e\t%e\t%e', stat(jj,8), stat(jj,9), stat(jj,10), stat(jj,11));
|
||||
fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8));
|
||||
if size(stat,2)>8
|
||||
fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12));
|
||||
end
|
||||
fprintf('\n');
|
||||
end
|
||||
|
|
|
@ -90,14 +90,7 @@ int {{ model.name }}_impl_dae_hess_n_out(void);
|
|||
|
||||
{% elif solver_options.integrator_type == "GNSF" %}
|
||||
/* GNSF Functions */
|
||||
// used to import model matrices
|
||||
int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
|
||||
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
// phi_fun
|
||||
int {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *);
|
||||
|
@ -121,7 +114,7 @@ const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int);
|
|||
const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void);
|
||||
int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void);
|
||||
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
// f_lo_fun_jac_x1k1uz
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *);
|
||||
|
@ -129,6 +122,15 @@ const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int);
|
|||
const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
// used to import model matrices
|
||||
int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
|
||||
|
||||
{% elif solver_options.integrator_type == "ERK" %}
|
||||
/* explicit ODE */
|
||||
|
|
|
@ -254,22 +254,6 @@ def format_class_dict(d):
|
|||
return out
|
||||
|
||||
|
||||
def acados_class2dict(class_instance):
|
||||
"""
|
||||
removes the __ artifact from class to dict conversion
|
||||
"""
|
||||
|
||||
d = dict(class_instance.__dict__)
|
||||
out = {}
|
||||
for k, v in d.items():
|
||||
if isinstance(v, dict):
|
||||
v = format_class_dict(v)
|
||||
|
||||
out_key = k.split('__', 1)[-1]
|
||||
out[k.replace(k, out_key)] = v
|
||||
return out
|
||||
|
||||
|
||||
def get_ocp_nlp_layout():
|
||||
python_interface_path = get_python_interface_path()
|
||||
abs_path = os.path.join(python_interface_path, 'acados_layout.json')
|
||||
|
@ -433,6 +417,13 @@ def set_up_imported_gnsf_model(acados_formulation):
|
|||
acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
|
||||
acados_formulation.model.get_matrices_fun = get_matrices_fun
|
||||
|
||||
# get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},...
|
||||
# {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,...
|
||||
# nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO});
|
||||
get_matrices_out = get_matrices_fun(0)
|
||||
acados_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
|
||||
acados_formulation.model.gnsf['purely_linear'] = int(get_matrices_out[13])
|
||||
|
||||
if "f_lo_fun_jac_x1k1uz" in gnsf:
|
||||
f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
|
||||
acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
|
||||
|
|
|
@ -43,11 +43,19 @@ generated_files = [
|
|||
f'{gen}/lat_cost/lat_cost_y_0_fun.h',
|
||||
] + build_files
|
||||
|
||||
acados_dir = '#third_party/acados'
|
||||
source_list = ['lat_mpc.py',
|
||||
f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
|
||||
f'{acados_dir}/aarch64/lib/libacados.so',
|
||||
f'{acados_dir}/x86_64/lib/libacados.so',
|
||||
f'{acados_dir}/larch64/lib/libacados.so',
|
||||
]
|
||||
|
||||
lenv = env.Clone()
|
||||
lenv.Clean(generated_files, Dir(gen))
|
||||
|
||||
lenv.Command(generated_files,
|
||||
["lat_mpc.py"],
|
||||
source_list,
|
||||
f"cd {Dir('.').abspath} && python3 lat_mpc.py")
|
||||
|
||||
lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
|
||||
|
|
|
@ -11,18 +11,19 @@ from selfdrive.modeld.constants import T_IDXS
|
|||
if __name__ == '__main__': # generating code
|
||||
from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
|
||||
else:
|
||||
# from pyextra.acados_template import AcadosOcpSolverFast
|
||||
from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast # pylint: disable=no-name-in-module, import-error
|
||||
from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython # pylint: disable=no-name-in-module, import-error
|
||||
|
||||
LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
|
||||
EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
|
||||
JSON_FILE = "acados_ocp_lat.json"
|
||||
JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
|
||||
X_DIM = 4
|
||||
P_DIM = 2
|
||||
MODEL_NAME = 'lat'
|
||||
ACADOS_SOLVER_TYPE = 'SQP_RTI'
|
||||
|
||||
def gen_lat_model():
|
||||
model = AcadosModel()
|
||||
model.name = 'lat'
|
||||
model.name = MODEL_NAME
|
||||
|
||||
# set up states & controls
|
||||
x_ego = SX.sym('x_ego')
|
||||
|
@ -58,7 +59,7 @@ def gen_lat_model():
|
|||
return model
|
||||
|
||||
|
||||
def gen_lat_mpc_solver():
|
||||
def gen_lat_ocp():
|
||||
ocp = AcadosOcp()
|
||||
ocp.model = gen_lat_model()
|
||||
|
||||
|
@ -103,7 +104,7 @@ def gen_lat_mpc_solver():
|
|||
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
|
||||
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
|
||||
ocp.solver_options.integrator_type = 'ERK'
|
||||
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
|
||||
ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
|
||||
ocp.solver_options.qp_solver_iter_max = 1
|
||||
ocp.solver_options.qp_solver_cond_N = 1
|
||||
|
||||
|
@ -117,7 +118,7 @@ def gen_lat_mpc_solver():
|
|||
|
||||
class LateralMpc():
|
||||
def __init__(self, x0=np.zeros(X_DIM)):
|
||||
self.solver = AcadosOcpSolverFast('lat', N)
|
||||
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
|
||||
self.reset(x0)
|
||||
|
||||
def reset(self, x0=np.zeros(X_DIM)):
|
||||
|
@ -173,5 +174,6 @@ class LateralMpc():
|
|||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ocp = gen_lat_mpc_solver()
|
||||
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
|
||||
ocp = gen_lat_ocp()
|
||||
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
|
||||
# AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
|
||||
|
|
|
@ -50,11 +50,19 @@ generated_files = [
|
|||
f'{gen}/long_cost/long_cost_y_0_fun.h',
|
||||
] + build_files
|
||||
|
||||
acados_dir = '#third_party/acados'
|
||||
source_list = ['long_mpc.py',
|
||||
f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
|
||||
f'{acados_dir}/aarch64/lib/libacados.so',
|
||||
f'{acados_dir}/x86_64/lib/libacados.so',
|
||||
f'{acados_dir}/larch64/lib/libacados.so',
|
||||
]
|
||||
|
||||
lenv = env.Clone()
|
||||
lenv.Clean(generated_files, Dir(gen))
|
||||
|
||||
lenv.Command(generated_files,
|
||||
["long_mpc.py"],
|
||||
source_list,
|
||||
f"cd {Dir('.').abspath} && python3 long_mpc.py")
|
||||
|
||||
lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
|
||||
|
|
|
@ -11,14 +11,14 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
|
|||
if __name__ == '__main__': # generating code
|
||||
from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
|
||||
else:
|
||||
# from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast
|
||||
from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast # pylint: disable=no-name-in-module, import-error
|
||||
from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython # pylint: disable=no-name-in-module, import-error
|
||||
|
||||
from casadi import SX, vertcat
|
||||
|
||||
MODEL_NAME = 'long'
|
||||
LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
|
||||
EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
|
||||
JSON_FILE = "acados_ocp_long.json"
|
||||
JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
|
||||
|
||||
SOURCES = ['lead0', 'lead1', 'cruise']
|
||||
|
||||
|
@ -38,6 +38,7 @@ A_CHANGE_COST = 200.
|
|||
DANGER_ZONE_COST = 100.
|
||||
CRASH_DISTANCE = .5
|
||||
LIMIT_COST = 1e6
|
||||
ACADOS_SOLVER_TYPE = 'SQP_RTI'
|
||||
|
||||
|
||||
# Fewer timestamps don't hurt performance and lead to
|
||||
|
@ -65,7 +66,7 @@ def desired_follow_distance(v_ego, v_lead):
|
|||
|
||||
def gen_long_model():
|
||||
model = AcadosModel()
|
||||
model.name = 'long'
|
||||
model.name = MODEL_NAME
|
||||
|
||||
# set up states & controls
|
||||
x_ego = SX.sym('x_ego')
|
||||
|
@ -97,7 +98,7 @@ def gen_long_model():
|
|||
return model
|
||||
|
||||
|
||||
def gen_long_mpc_solver():
|
||||
def gen_long_ocp():
|
||||
ocp = AcadosOcp()
|
||||
ocp.model = gen_long_model()
|
||||
|
||||
|
@ -172,7 +173,7 @@ def gen_long_mpc_solver():
|
|||
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
|
||||
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
|
||||
ocp.solver_options.integrator_type = 'ERK'
|
||||
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
|
||||
ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
|
||||
ocp.solver_options.qp_solver_cond_N = 1
|
||||
|
||||
# More iterations take too much time and less lead to inaccurate convergence in
|
||||
|
@ -195,7 +196,7 @@ class LongitudinalMpc:
|
|||
self.source = SOURCES[2]
|
||||
|
||||
def reset(self):
|
||||
self.solver = AcadosOcpSolverFast('long', N)
|
||||
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
|
||||
self.v_solution = np.zeros(N+1)
|
||||
self.a_solution = np.zeros(N+1)
|
||||
self.prev_a = np.array(self.a_solution)
|
||||
|
@ -388,5 +389,6 @@ class LongitudinalMpc:
|
|||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ocp = gen_long_mpc_solver()
|
||||
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
|
||||
ocp = gen_long_ocp()
|
||||
AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
|
||||
# AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
|
||||
|
|
Binary file not shown.
Binary file not shown.
|
@ -13,19 +13,19 @@ elif [ -f /EON ]; then
|
|||
fi
|
||||
|
||||
if [ ! -d acados_repo/ ]; then
|
||||
#git clone https://github.com/acados/acados.git $DIR/acados
|
||||
git clone https://github.com/commaai/acados.git $DIR/acados_repo
|
||||
git clone https://github.com/acados/acados.git $DIR/acados_repo
|
||||
# git clone https://github.com/commaai/acados.git $DIR/acados_repo
|
||||
fi
|
||||
cd acados_repo
|
||||
git fetch
|
||||
git checkout 92b85c61f7358a1b08b7cd30aeb9d32ad15942e8
|
||||
git checkout 105e06df87f06ea02df4af825867c946b31defdd
|
||||
git submodule update --recursive --init
|
||||
|
||||
# build
|
||||
mkdir -p build
|
||||
cd build
|
||||
cmake -DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET ..
|
||||
make -j4 install
|
||||
make -j20 install
|
||||
|
||||
INSTALL_DIR="$DIR/$ARCHNAME"
|
||||
rm -rf $INSTALL_DIR
|
||||
|
@ -33,8 +33,10 @@ mkdir -p $INSTALL_DIR
|
|||
|
||||
rm $DIR/acados_repo/lib/*.json
|
||||
|
||||
rm -rf $DIR/include
|
||||
cp -r $DIR/acados_repo/include $DIR
|
||||
cp -r $DIR/acados_repo/lib $INSTALL_DIR
|
||||
rm -rf $DIR/../../pyextra/acados_template
|
||||
cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/../../pyextra
|
||||
#pip3 install -e $DIR/acados/interfaces/acados_template
|
||||
|
||||
|
|
|
@ -234,11 +234,7 @@ typedef struct ocp_nlp_out
|
|||
|
||||
// NOTE: the inequalities are internally organized in the following order:
|
||||
// [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
|
||||
|
||||
int sqp_iter;
|
||||
int qp_iter;
|
||||
double inf_norm_res;
|
||||
double total_time;
|
||||
|
||||
void *raw_memory; // Pointer to allocated memory, to be used for freeing
|
||||
|
||||
|
@ -274,11 +270,16 @@ typedef struct ocp_nlp_opts
|
|||
double levenberg_marquardt; // LM factor to be added to the hessian before regularization
|
||||
int reuse_workspace;
|
||||
int num_threads;
|
||||
int print_level;
|
||||
|
||||
// TODO: move to separate struct?
|
||||
ocp_nlp_globalization_t globalization;
|
||||
int full_step_dual;
|
||||
int line_search_use_sufficient_descent;
|
||||
int globalization_use_SOC;
|
||||
double alpha_min;
|
||||
double alpha_reduction;
|
||||
double eps_sufficient_descent;
|
||||
} ocp_nlp_opts;
|
||||
|
||||
//
|
||||
|
@ -316,6 +317,8 @@ typedef struct ocp_nlp_res
|
|||
acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
|
||||
//
|
||||
ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
|
@ -351,7 +354,7 @@ typedef struct ocp_nlp_memory
|
|||
bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
|
||||
struct blasfeo_dvec *sim_guess;
|
||||
|
||||
int *sqp_iter; // pointer to iteration number
|
||||
int *sqp_iter; // pointer to iteration number
|
||||
|
||||
} ocp_nlp_memory;
|
||||
|
||||
|
@ -375,8 +378,12 @@ typedef struct ocp_nlp_workspace
|
|||
void **cost; // cost_workspace
|
||||
void **constraints; // constraints_workspace
|
||||
|
||||
ocp_nlp_out *tmp_nlp_out;
|
||||
ocp_nlp_out *weight_merit_fun;
|
||||
// for globalization: -> move to module?!
|
||||
ocp_nlp_out *tmp_nlp_out;
|
||||
ocp_nlp_out *weight_merit_fun;
|
||||
struct blasfeo_dvec tmp_nxu;
|
||||
struct blasfeo_dvec tmp_ni;
|
||||
struct blasfeo_dvec dxnext_dy;
|
||||
|
||||
} ocp_nlp_workspace;
|
||||
|
||||
|
@ -392,6 +399,8 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims
|
|||
* function
|
||||
************************************************/
|
||||
|
||||
void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
void ocp_nlp_initialize_qp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
|
@ -409,7 +418,8 @@ void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, oc
|
|||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha);
|
||||
//
|
||||
double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
|
||||
int check_early_termination);
|
||||
//
|
||||
double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
|
|
|
@ -84,6 +84,7 @@ typedef struct
|
|||
/* memory */
|
||||
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
|
||||
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
|
||||
// get shooting node gap x_next(x_n, u_n) - x_{n+1}
|
||||
struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
|
||||
struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
|
||||
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);
|
||||
|
|
|
@ -65,11 +65,6 @@ extern "C" {
|
|||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int dummy;
|
||||
} ocp_nlp_reg_noreg_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
|
||||
//
|
||||
|
@ -84,12 +79,6 @@ void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opt
|
|||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int dummy;
|
||||
} ocp_nlp_reg_noreg_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
|
||||
//
|
||||
|
|
|
@ -68,7 +68,6 @@ typedef struct
|
|||
int qp_warm_start; // qp_warm_start in all but the first sqp iterations
|
||||
bool warm_start_first_qp; // to set qp_warm_start in first iteration
|
||||
int rti_phase; // only phase 0 at the moment
|
||||
int print_level; // verbosity
|
||||
int initialize_t_slacks; // 0-false or 1-true
|
||||
|
||||
} ocp_nlp_sqp_opts;
|
||||
|
@ -107,6 +106,7 @@ typedef struct
|
|||
double time_sim;
|
||||
double time_sim_la;
|
||||
double time_sim_ad;
|
||||
double time_solution_sensitivities;
|
||||
|
||||
// statistics
|
||||
double *stat;
|
||||
|
|
|
@ -64,7 +64,6 @@ typedef struct
|
|||
int qp_warm_start; // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
|
||||
bool warm_start_first_qp; // to set qp_warm_start in first iteration
|
||||
int rti_phase; // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both)
|
||||
int print_level; // verbosity
|
||||
|
||||
} ocp_nlp_sqp_rti_opts;
|
||||
|
||||
|
@ -100,6 +99,7 @@ typedef struct
|
|||
double time_reg;
|
||||
double time_tot;
|
||||
double time_glob;
|
||||
double time_solution_sensitivities;
|
||||
|
||||
// statistics
|
||||
double *stat;
|
||||
|
|
|
@ -69,28 +69,32 @@ void ocp_nlp_out_print(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out);
|
|||
void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res);
|
||||
|
||||
// ocp qp
|
||||
// TODO: move printing routines below that print qp structures to HPIPM!
|
||||
void print_ocp_qp_dims(ocp_qp_dims *dims);
|
||||
|
||||
// void print_dense_qp_dims(dense_qp_dims *dims);
|
||||
|
||||
void print_ocp_qp_in(ocp_qp_in *qp_in);
|
||||
|
||||
void print_ocp_qp_in_to_file(FILE *file, ocp_qp_in *qp_in);
|
||||
|
||||
void print_ocp_qp_out(ocp_qp_out *qp_out);
|
||||
|
||||
void print_ocp_qp_out_to_file(FILE *file, ocp_qp_out *qp_out);
|
||||
|
||||
void print_ocp_qp_res(ocp_qp_res *qp_res);
|
||||
|
||||
void print_dense_qp_in(dense_qp_in *qp_in);
|
||||
// void print_ocp_qp_in_to_string(char string_out[], ocp_qp_in *qp_in);
|
||||
|
||||
// void print_ocp_qp_out_to_string(char string_out[], ocp_qp_out *qp_out);
|
||||
|
||||
void print_ocp_qp_res(ocp_qp_res *qp_res);
|
||||
|
||||
// void print_colmaj_ocp_qp_in(colmaj_ocp_qp_in *qp);
|
||||
|
||||
// void print_colmaj_ocp_qp_in_to_file(colmaj_ocp_qp_in *qp);
|
||||
|
||||
// void print_colmaj_ocp_qp_out(char *filename, colmaj_ocp_qp_in *qp, colmaj_ocp_qp_out *out);
|
||||
|
||||
void print_dense_qp_in(dense_qp_in *qp_in);
|
||||
|
||||
void print_qp_info(qp_info *info);
|
||||
|
||||
// void acados_warning(char warning_string[]);
|
||||
|
|
|
@ -107,13 +107,13 @@ typedef enum
|
|||
|
||||
|
||||
/// Structure to store the configuration of a non-linear program
|
||||
typedef struct ocp_nlp_plan
|
||||
typedef struct ocp_nlp_plan_t
|
||||
{
|
||||
/// QP solver configuration.
|
||||
ocp_qp_solver_plan ocp_qp_solver_plan;
|
||||
ocp_qp_solver_plan_t ocp_qp_solver_plan;
|
||||
|
||||
/// Simulation solver configuration for each stage.
|
||||
sim_solver_plan *sim_solver_plan;
|
||||
sim_solver_plan_t *sim_solver_plan;
|
||||
|
||||
/// Nlp solver type.
|
||||
ocp_nlp_solver_t nlp_solver;
|
||||
|
@ -133,7 +133,7 @@ typedef struct ocp_nlp_plan
|
|||
/// Horizon length.
|
||||
int N;
|
||||
|
||||
} ocp_nlp_plan;
|
||||
} ocp_nlp_plan_t;
|
||||
|
||||
|
||||
/// Structure to store the state/configuration for the non-linear programming solver
|
||||
|
@ -151,7 +151,7 @@ typedef struct ocp_nlp_solver
|
|||
/// default/invalid state.
|
||||
///
|
||||
/// \param N Horizon length
|
||||
ocp_nlp_plan *ocp_nlp_plan_create(int N);
|
||||
ocp_nlp_plan_t *ocp_nlp_plan_create(int N);
|
||||
|
||||
/// Destructor for plan struct, frees memory.
|
||||
///
|
||||
|
@ -162,7 +162,7 @@ void ocp_nlp_plan_destroy(void* plan_);
|
|||
/// Constructs an nlp configuration struct from a plan.
|
||||
///
|
||||
/// \param plan The plan (user nlp configuration).
|
||||
ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan plan);
|
||||
ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan_t plan);
|
||||
|
||||
/// Desctructor of the nlp configuration.
|
||||
///
|
||||
|
@ -372,6 +372,14 @@ void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *
|
|||
//
|
||||
void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
|
||||
|
||||
/// Computes the residuals.
|
||||
///
|
||||
/// \param solver The solver struct.
|
||||
/// \param nlp_in The inputs struct.
|
||||
/// \param nlp_out The output struct.
|
||||
void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
|
||||
|
||||
|
||||
//
|
||||
void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out);
|
||||
|
||||
|
|
|
@ -105,7 +105,7 @@ typedef enum {
|
|||
typedef struct
|
||||
{
|
||||
ocp_qp_solver_t qp_solver;
|
||||
} ocp_qp_solver_plan;
|
||||
} ocp_qp_solver_plan_t;
|
||||
|
||||
|
||||
/// Linear ocp configuration.
|
||||
|
@ -127,7 +127,7 @@ void ocp_qp_xcond_solver_config_initialize_from_plan(
|
|||
/// Constructs a qp solver config and Initializes with default values.
|
||||
///
|
||||
/// \param plan The qp solver plan struct.
|
||||
ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan plan);
|
||||
ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan_t plan);
|
||||
|
||||
/// Destructor for config struct, frees memory.
|
||||
///
|
||||
|
|
|
@ -57,7 +57,7 @@ typedef enum
|
|||
typedef struct
|
||||
{
|
||||
sim_solver_t sim_solver;
|
||||
} sim_solver_plan;
|
||||
} sim_solver_plan_t;
|
||||
|
||||
|
||||
|
||||
|
@ -74,7 +74,7 @@ typedef struct
|
|||
|
||||
/* config */
|
||||
//
|
||||
sim_config *sim_config_create(sim_solver_plan plan);
|
||||
sim_config *sim_config_create(sim_solver_plan_t plan);
|
||||
//
|
||||
void sim_config_destroy(void *config);
|
||||
|
||||
|
|
|
@ -185,9 +185,14 @@ void blasfeo_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec
|
|||
void blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi);
|
||||
// y <= alpha*x
|
||||
void blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
|
||||
// z[idx] += alpha * x
|
||||
void blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
|
||||
// z[idx] <= alpha * x
|
||||
void blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
|
||||
void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
|
||||
// z <= alpha * x[idx]
|
||||
void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
|
||||
// z += alpha * x[idx]
|
||||
void blasfeo_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
|
||||
|
||||
void blasfeo_dveccl(int m,
|
||||
struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
|
||||
|
|
|
@ -182,6 +182,8 @@ void blasfeo_ref_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi,
|
|||
void blasfeo_ref_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
|
||||
void blasfeo_ref_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
|
||||
void blasfeo_ref_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
|
||||
// z += alpha * x[idx]
|
||||
void blasfeo_ref_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
|
||||
|
||||
void blasfeo_ref_dveccl(int m,
|
||||
struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
|
||||
|
|
|
@ -127,6 +127,8 @@ float blasfeo_svecex1(struct blasfeo_svec *sx, int xi);
|
|||
void blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
|
||||
void blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
|
||||
void blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
|
||||
// z += alpha * x[idx]
|
||||
void blasfeo_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
|
||||
void blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
|
||||
void blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
|
||||
void blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
|
||||
|
|
|
@ -128,6 +128,8 @@ float blasfeo_ref_svecex1(struct blasfeo_svec *sx, int xi);
|
|||
void blasfeo_ref_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
|
||||
void blasfeo_ref_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
|
||||
void blasfeo_ref_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
|
||||
// z += alpha * x[idx]
|
||||
void blasfeo_ref_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
|
||||
void blasfeo_ref_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
|
||||
void blasfeo_ref_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
|
||||
void blasfeo_ref_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in New Issue