def acados_settings_dyn(Tf, N, modelparams = "modelparams.yaml"):
#create render arguments
ocp = AcadosOcp()
#load model
model, constraints = dynamic_model(modelparams)
# define acados ODE
model_ac = AcadosModel()
model_ac.f_impl_expr = model.f_impl_expr
model_ac.f_expl_expr = model.f_expl_expr
model_ac.x = model.x
model_ac.xdot = model.xdot
#inputvector
model_ac.u = model.u
#parameter vector
model_ac.p = model.p
#parameter vector
model_ac.z = model.z
#external cost function
model_ac.cost_expr_ext_cost = model.stage_cost
#model_ac.cost_expr_ext_cost_e = 0
model_ac.con_h_expr = model.con_h_expr
model_ac.name = model.name
ocp.model = model_ac
# set dimensions
nx = model.x.size()[0]
nu = model.u.size()[0]
nz = model.z.size()[0]
np = model.p.size()[0]
#ny = nu + nx
#ny_e = nx
ocp.dims.nx = nx
ocp.dims.nz = nz
#ocp.dims.ny = ny
#ocp.dims.ny_e = ny_e
ocp.dims.nu = nu
ocp.dims.np = np
ocp.dims.nh = 1
#ocp.dims.ny = ny
#ocp.dims.ny_e = ny_e
ocp.dims.nbx = 3
ocp.dims.nsbx = 0
ocp.dims.nbu = nu
#number of soft on h constraints
ocp.dims.nsh = 1
ocp.dims.ns = 1
ocp.dims.N = N
# set cost to casadi expression defined above
ocp.cost.cost_type = "EXTERNAL"
#ocp.cost.cost_type_e = "EXTERNAL"
ocp.cost.zu = 1000 * npy.ones((ocp.dims.ns,))
ocp.cost.Zu = 1000 * npy.ones((ocp.dims.ns,))
ocp.cost.zl = 1000 * npy.ones((ocp.dims.ns,))
ocp.cost.Zl = 1000 * npy.ones((ocp.dims.ns,))
#not sure if needed
#unscale = N / Tf
#constraints
#stagewise constraints for tracks with slack
ocp.constraints.uh = npy.array([0.00])
ocp.constraints.lh = npy.array([-10])
ocp.constraints.lsh = 0.1*npy.ones(ocp.dims.nsh)
ocp.constraints.ush = -0.1*npy.ones(ocp.dims.nsh)
ocp.constraints.idxsh = npy.array([0])
#ocp.constraints.Jsh = 1
# boxconstraints
# xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta']
ocp.constraints.lbx = npy.array([10, 10, 100, model.vx_min, model.vy_min, model.omega_min, model.d_min, model.delta_min, model.theta_min])
ocp.constraints.ubx = npy.array([10, 10, 100, model.vx_max, model.vy_max, model.omega_max, model.d_max, model.delta_max, model.theta_max])
ocp.constraints.idxbx = npy.arange(nx)
#ocp.constraints.lsbx= -0.1 * npy.ones(ocp.dims.nbx)
#ocp.constraints.usbx= 0.1 * npy.ones(ocp.dims.nbx)
#ocp.constraints.idxsbx= npy.array([6,7,8])
#print("ocp.constraints.idxbx: ",ocp.constraints.idxbx)
ocp.constraints.lbu = npy.array([model.ddot_min, model.deltadot_min, model.thetadot_min])
ocp.constraints.ubu = npy.array([model.ddot_max, model.deltadot_max, model.thetadot_max])
ocp.constraints.idxbu = npy.array([0, 1, 2])
# set intial condition
ocp.constraints.x0 = model.x0
# set QP solver and integration
ocp.solver_options.tf = Tf
# ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
ocp.solver_options.nlp_solver_type = "SQP"#"SQP_RTI" #
ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
ocp.solver_options.integrator_type = "ERK"
ocp.parameter_values = npy.zeros(np)
#ocp.solver_options.sim_method_num_stages = 4
#ocp.solver_options.sim_method_num_steps = 3
ocp.solver_options.nlp_solver_step_length = 0.01
ocp.solver_options.nlp_solver_max_iter = 50
ocp.solver_options.tol = 1e-4
#ocp.solver_options.print_level = 1
# ocp.solver_options.nlp_solver_tol_comp = 1e-1
# create solver
acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp_dynamic.json")
return constraints, model, acados_solver, ocp
def generate(self, dae=None, quad=None, name='tunempc', opts={}):
""" Create embeddable NLP solver
"""
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
# extract dimensions
nx = self.__nx
nu = self.__nu + self.__ns # treat slacks as pseudo-controls
# extract reference
ref = self.__ref
xref = np.squeeze(self.__ref[0][:nx], axis=1)
uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1)
# sampling time
self.__ts = opts['tf'] / self.__N
# create acados model
model = AcadosModel()
model.x = ca.MX.sym('x', nx)
model.u = ca.MX.sym('u', nu)
model.p = []
model.name = name
# detect input type
if dae is None:
model.f_expl_expr = self.__F(x0=model.x,
p=model.u)['xf'] / self.__ts
opts['integrator_type'] = 'ERK'
opts['sim_method_num_stages'] = 1
opts['sim_method_num_steps'] = 1
else:
n_in = dae.n_in()
if n_in == 2:
# xdot = f(x, u)
if 'integrator_type' in opts:
if opts['integrator_type'] in ['IRK', 'GNSF']:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_impl_expr = xdot - dae(model.x,
model.u[:self.__nu])
model.f_expl_expr = xdot
elif opts['integrator_type'] == 'ERK':
model.f_expl_expr = dae(model.x, model.u[:self.__nu])
else:
raise ValueError('Provide numerical integrator type!')
else:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_expl_expr = xdot
if n_in == 3:
# f(xdot, x, u) = 0
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu])
elif n_in == 4:
# f(xdot, x, u, z) = 0
nz = dae.size1_in(3)
z = ca.MX.sym('z', nz)
model.z = z
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu],
z)
else:
raise ValueError(
'Invalid number of inputs for system dynamics function.'
)
if self.__gnl is not None:
model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu],
model.u[self.__nu:])
if self.__type == 'economic':
if quad is None:
model.cost_expr_ext_cost = self.__cost(
model.x, model.u[:self.__nu]) / self.__ts
else:
model.cost_expr_ext_cost = self.__cost(model.x,
model.u[:self.__nu])
# create acados ocp
ocp = AcadosOcp()
ocp.model = model
ny = nx + nu
ny_e = nx
if 'integrator_type' in opts and opts['integrator_type'] == 'GNSF':
from acados_template import acados_dae_model_json_dump
import os
acados_dae_model_json_dump(model)
# Set up Octave to be able to run the following:
## if using a virtual python env, the following lines can be added to the env/bin/activate script:
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/
# export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/
# echo
# echo "OCTAVE_PATH=$OCTAVE_PATH"
status = os.system(
"octave --eval \"convert({})\"".format("\'" + model.name +
"_acados_dae.json\'"))
# load gnsf from json
with open(model.name + '_gnsf_functions.json', 'r') as f:
import json
gnsf_dict = json.load(f)
ocp.gnsf_model = gnsf_dict
# set horizon length
ocp.dims.N = self.__N
# set cost module
if self.__type == 'economic':
# set cost function type to external (provided in model)
ocp.cost.cost_type = 'EXTERNAL'
if quad is not None:
ocp.solver_options.cost_discretization = 'INTEGRATOR'
elif self.__type == 'tracking':
# set weighting matrices
ocp.cost.W = self.__Href[0][0]
# set-up linear least squares cost
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.W_e = np.zeros((nx, nx))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term
axis = 1
)
ocp.cost.yref_e = np.zeros((ny_e, ))
if n_in == 4: # DAE flag
ocp.cost.Vz = np.zeros((ny, nz))
# if 'custom_hessian' in opts:
# self.__custom_hessian = opts['custom_hessian']
# initial condition
ocp.constraints.x0 = xref
# set inequality constraints
ocp.constraints.constr_type = 'BGH'
if self.__S['C'] is not None:
C = self.__S['C'][0][:, :nx]
D = self.__S['C'][0][:, nx:]
lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][0] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ocp.constraints.lg = np.squeeze(lg, axis=1)
ocp.constraints.ug = np.squeeze(ug, axis=1)
ocp.constraints.C = C
ocp.constraints.D = D
if 'usc' in self.__vars:
if 'us' in self.__vars:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['us'],
self.__vars['usc']
]
else:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['usc']
]
Jsg = ca.Function(
'Jsg', [self.__vars['usc']],
[ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0)
self.__Jsg = Jsg.full()[:-self.__nsc, :]
ocp.constraints.Jsg = self.__Jsg
ocp.cost.Zl = np.zeros((self.__nsc, ))
ocp.cost.Zu = np.zeros((self.__nsc, ))
ocp.cost.zl = np.squeeze(self.__scost.full(),
axis=1) / self.__ts
ocp.cost.zu = np.squeeze(self.__scost.full(),
axis=1) / self.__ts
# set nonlinear equality constraints
if self.__gnl is not None:
ocp.constraints.lh = np.zeros(self.__ns, )
ocp.constraints.uh = np.zeros(self.__ns, )
# terminal constraint:
x_term = self.__p_operator(model.x)
Jbx = ca.Function('Jbx', [model.x],
[ca.jacobian(x_term, model.x)])(0.0)
ocp.constraints.Jbx_e = Jbx.full()
ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
for option in list(opts.keys()):
if hasattr(ocp.solver_options, option):
setattr(ocp.solver_options, option, opts[option])
self.__acados_ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
self.__acados_integrator = AcadosSimSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
# set initial guess
self.__set_acados_initial_guess()
return self.__acados_ocp_solver, self.__acados_integrator
def generate(self, dae, name='tunempc', opts={}):
""" Create embeddable NLP solver
"""
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver
# extract dimensions
nx = self.__nx
nu = self.__nu + self.__ns # treat slacks as pseudo-controls
# extract reference
ref = self.__ref
xref = np.squeeze(self.__ref[0][:nx], axis=1)
uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1)
# create acados model
model = AcadosModel()
model.x = ca.MX.sym('x', nx)
model.u = ca.MX.sym('u', nu)
model.p = []
model.name = name
# detect input type
n_in = dae.n_in()
if n_in == 2:
# xdot = f(x, u)
if 'integrator_type' in opts:
if opts['integrator_type'] == 'IRK':
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_impl_expr = xdot - dae(model.x,
model.u[:self.__nu])
model.f_expl_expr = xdot
elif opts['integrator_type'] == 'ERK':
model.f_expl_expr = dae(model.x, model.u[:self.__nu])
else:
raise ValueError('Provide numerical integrator type!')
else:
xdot = ca.MX.sym('xdot', nx)
model.xdot = xdot
model.f_expl_expr = xdot
if n_in == 3:
# f(xdot, x, u) = 0
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu])
elif n_in == 4:
# f(xdot, x, u, z) = 0
nz = dae.size1_in(3)
z = ca.MX.sym('z', nz)
model.z = z
model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu], z)
else:
raise ValueError(
'Invalid number of inputs for system dynamics function.')
if self.__gnl is not None:
model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu],
model.u[self.__nu:])
if self.__type == 'economic':
model.cost_expr_ext_cost = self.__cost(model.x,
model.u[:self.__nu])
# create acados ocp
ocp = AcadosOcp()
ocp.model = model
ny = nx + nu
ny_e = nx
# set horizon length
ocp.dims.N = self.__N
# set cost module
if self.__type == 'economic':
# set cost function type to external (provided in model)
ocp.cost.cost_type = 'EXTERNAL'
else:
# set weighting matrices
if self.__type == 'tracking':
ocp.cost.W = self.__Href[0][0]
# set-up linear least squares cost
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.W_e = np.zeros((nx, nx))
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx, :nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx:, :] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term
axis = 1
)
ocp.cost.yref_e = np.zeros((ny_e, ))
if n_in == 4: # DAE flag
ocp.cost.Vz = np.zeros((ny, nz))
# initial condition
ocp.constraints.x0 = xref
# set inequality constraints
ocp.constraints.constr_type = 'BGH'
if self.__S['C'] is not None:
C = self.__S['C'][0][:, :nx]
D = self.__S['C'][0][:, nx:]
lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes(
D, uref).full()
ug = 1e8 - self.__S['e'][0] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ocp.constraints.lg = np.squeeze(lg, axis=1)
ocp.constraints.ug = np.squeeze(ug, axis=1)
ocp.constraints.C = C
ocp.constraints.D = D
if 'usc' in self.__vars:
if 'us' in self.__vars:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['us'],
self.__vars['usc']
]
else:
arg = [
self.__vars['x'], self.__vars['u'], self.__vars['usc']
]
Jsg = ca.Function(
'Jsg', [self.__vars['usc']],
[ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0)
self.__Jsg = Jsg.full()[:-self.__nsc, :]
ocp.constraints.Jsg = self.__Jsg
ocp.cost.Zl = np.zeros((self.__nsc, ))
ocp.cost.Zu = np.zeros((self.__nsc, ))
ocp.cost.zl = np.squeeze(self.__scost.full(), axis=1)
ocp.cost.zu = np.squeeze(self.__scost.full(), axis=1)
# set nonlinear equality constraints
if self.__gnl is not None:
ocp.constraints.lh = np.zeros(self.__ns, )
ocp.constraints.uh = np.zeros(self.__ns, )
# terminal constraint:
x_term = self.__p_operator(model.x)
Jbx = ca.Function('Jbx', [model.x],
[ca.jacobian(x_term, model.x)])(0.0)
ocp.constraints.Jbx_e = Jbx.full()
ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(),
axis=1)
for option in list(opts.keys()):
setattr(ocp.solver_options, option, opts[option])
self.__acados_ocp_solver = AcadosOcpSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
self.__acados_integrator = AcadosSimSolver(ocp,
json_file='acados_ocp_' +
model.name + '.json')
# set initial guess
self.__set_acados_initial_guess()
return self.__acados_ocp_solver, self.__acados_integrator
