def detect_nonlinear_inequalities(h):
x = ca.MX.sym('x', h.size1_in(0))
u = ca.MX.sym('u', h.size1_in(1))
h_expr = h(x,u)
# sort constraints according to (non-)linearity
h_nlin = []
h_lin = []
for k in range(h_expr.shape[0]):
if True in ca.which_depends(h_expr[k],ca.vertcat(x,u),2):
h_nlin.append(h_expr[k])
else:
h_lin.append(h_expr[k])
# update control vector (add slacks for nonlinear inequalities)
if len(h_nlin) > 0:
# function for nonlinear slacked inequalities
s = ca.MX.sym('s', len(h_nlin))
g = ca.Function('g',[x,u,s], [ca.vertcat(*h_nlin) - s])
# function for linear inequalities
h_lin.append(s) # slacks > 0
h = ca.Function('h', [x,u,s], [ca.vertcat(*h_lin)])
else:
g = None
return g, h
def __detect_state_dependent_constraints(self):
""" Detect which nonlinear equalities depend on states but not on controls.
"""
g_nl = self.__gnl(self.__vars['x'], self.__vars['u'],
self.__vars['us'])
self.__gnl_x_idx = []
for i in range(g_nl.shape[0]):
if not True in ca.which_depends(g_nl[i], self.__vars['u'], 1):
self.__gnl_x_idx.append(i)
self.__h_us_idx = [
idx + self.__h.size1_out(0) - self.__ns for idx in self.__gnl_x_idx
]
return None
def detect_nonlinear_inequalities(h):
# make constraints "time-varying"
if type(h) is not list:
h = [h]
# initialize update inequality list
h_new = []
g_new = []
# iterate over constraints
for k in range(len(h)):
x = ca.MX.sym('x', h[k].size1_in(0))
u = ca.MX.sym('u', h[k].size1_in(1))
h_expr = h[k](x, u)
# sort constraints according to (non-)linearity
h_nlin = []
h_lin = []
for i in range(h_expr.shape[0]):
if True in ca.which_depends(h_expr[i], ca.vertcat(x, u), 2):
h_nlin.append(h_expr[i])
else:
h_lin.append(h_expr[i])
# update control vector (add slacks for nonlinear inequalities)
if len(h_nlin) > 0:
# function for nonlinear slacked inequalities
s = ca.MX.sym('s', len(h_nlin))
g_new.append(ca.Function('g', [x, u, s],
[ca.vertcat(*h_nlin) - s]))
# function for linear inequalities
h_lin.append(s) # slacks > 0
h_new.append(ca.Function('h', [x, u, s], [ca.vertcat(*h_lin)]))
else:
g_new.append(None)
h_new.append(h[k])
return g_new, h_new
u_shape = model['dae']['p'].shape
u_shape = (u_shape[0] - 4, u_shape[1])
u = ca.MX.sym('u', *u_shape)
u_awe = ct.vertcat(0.0, 0.0, 0.0, u, 0.0)
# remove algebraic variable
z = model['rootfinder'](0.1, x_awe, u_awe)
nx = x.shape[0]
nu = u_shape[0]
constraints = ca.vertcat(-model['constraints'](x_awe, u_awe, z),
-model['var_bounds_fun'](x_awe, u_awe, z))
# remove redundant constraints
constraints_new = []
for i in range(constraints.shape[0]):
if True in ca.which_depends(constraints[i], ca.vertcat(x, u)):
constraints_new.append(constraints[i])
# create integrator
integrator = awe_integrators.rk4root('F', model['dae'], model['rootfinder'], {
'tf': 1 / N,
'number_of_finite_elements': 10
})
xf = integrator(x0=x_awe, p=u_awe, z0=0.1)['xf'][:-3]
qf = integrator(x0=x_awe, p=u_awe, z0=0.1)['qf']
sys = {
'f': ca.Function('F', [x, u], [xf, qf], ['x0', 'p'], ['xf', 'qf']),
'h': ca.Function('h', [x, u], [ca.vertcat(*constraints_new)])
}
def __init__(self,
N,
sys,
cost,
wref=None,
tuning=None,
lam_g_ref=None,
sensitivities=None,
options={}):
""" Constructor
"""
# store construction data
self.__N = N
self.__vars = sys['vars']
self.__nx = sys['vars']['x'].shape[0]
self.__nu = sys['vars']['u'].shape[0]
# nonlinear inequalities slacks
if 'us' in sys['vars']:
self.__ns = sys['vars']['us'].shape[0]
else:
self.__ns = 0
# mpc slacks
if 'usc' in sys['vars']:
self.__nsc = sys['vars']['usc'].shape[0]
self.__scost = sys['scost']
else:
self.__nsc = 0
# store system dynamics
self.__F = sys['f']
# store path constraints
if 'h' in sys:
self.__h = sys['h']
h_lin = self.__h(*self.__vars.values())
self.__h_x_idx = [
idx for idx in range(h_lin.shape[0])
if not True in ca.which_depends(
h_lin[idx], ct.vertcat(*list(self.__vars.values())[1:]))
]
else:
self.__h = None
# store slacked nonlinear inequality constraints
if 'g' in sys:
self.__gnl = sys['g']
self.__detect_state_dependent_constraints()
else:
self.__gnl = None
self.__h_us_idx = [] # no nonlinear state-dependent constraints
# store system sensitivities around steady state
self.__S = sys['S']
self.__cost = cost
# set options
self.__options = self.__default_options()
for option in options:
if option in self.__options:
self.__options[option] = options[option]
else:
raise ValueError(
'Unknown option for Pmpc class instance: "{}"'.format(
option))
# detect cost-type
if self.__cost.n_in() == 2:
# cost function of the form: l(x,u)
self.__type = 'economic'
# no tuning required
tuning = None
if self.__options['hessian_approximation'] == 'gauss_newton':
self.__options['hessian_approximation'] = 'exact'
Logger.logger.warning(
'Gauss-Newton Hessian approximation cannot be applied for economic MPC problem. Switched to exact Hessian.'
)
else:
# cost function of the form: (w-wref)'*H*(w-wref) + q'w
self.__type = 'tracking'
# check if tuning matrices are provided
assert tuning != None, 'Provide tuning matrices for tracking MPC!'
# periodicity operator
self.__p_operator = self.__options['p_operator']
self.__jac_p_operator = ca.Function('jac_p', [sys['vars']['x']], [
ca.jacobian(self.__p_operator(sys['vars']['x']), sys['vars']['x'])
])
self.__S = sensitivities
# construct MPC solver
self.__construct_solver()
# periodic indexing
self.__index = 0
self.__index_acados = 0
# create periodic reference
assert wref != None, 'Provide reference trajectory!'
self.__create_reference(wref, tuning, lam_g_ref)
# initialize log
self.__initialize_log()
# initialize acados solvers
self.__acados_ocp_solver = None
self.__acados_integrator = None
# solver initial guess
self.__set_initial_guess()
return None