def make_output_structure(outputs):
outputs_vec = []
full_list = []
outputs_dict = {}
for output_type in list(outputs.keys()):
local_list = []
for name in list(outputs[output_type].keys()):
# prepare empty entry list to generate substruct
local_list += [
cas.entry(name, shape=outputs[output_type][name].shape)
]
# generate vector with outputs - SX expressions
outputs_vec = cas.vertcat(outputs_vec, outputs[output_type][name])
# generate dict with sub-structs
outputs_dict[output_type] = cas.struct_symMX(local_list)
# prepare list with sub-structs to generate struct
full_list += [cas.entry(output_type, struct=outputs_dict[output_type])]
# generate "empty" structure
out_struct = cas.struct_symMX(full_list)
# generate structure with SX expressions
outputs_struct = out_struct(outputs_vec)
return [outputs_struct, outputs_dict]
def vars():
""" System states and controls
"""
x = ct.struct_symMX(['z', 'y', 'ez', 'ey'])
u = ct.struct_symMX(['u'])
return x, u
def vars():
""" System states and controls
"""
x = ct.struct_symMX(['X2', 'P2'])
u = ct.struct_symMX(['P100', 'F200'])
return x, u
def vars():
""" System states and controls
"""
x = ct.struct_symMX(['cA', 'cB', 'theta', 'thetaK'])
u = ct.struct_symMX(['Vdot', 'QdotK'])
return x, u
def construct_upd_z(self, problem=None, lineq_updz=True):
if problem is not None:
self.problem_upd_z = problem
self._lineq_updz = lineq_updz
return 0.
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
raise ValueError('For now, only equality constrained QP ' +
'z-updates are allowed!')
x_i = struct_symMX(self.q_i_struct)
x_j = struct_symMX(self.q_ij_struct)
l_i = struct_symMX(self.q_i_struct)
l_ij = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
par = struct_symMX(self.par_global_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t / T
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A(par.cat)
b = b(par.cat)
# build KKT system and solve it via schur complement method
l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
f = -(l + rho * x)
G = -(1 / rho) * mtimes(A, A.T)
h = b + (1 / rho) * mtimes(A, f)
mu = solve(G, h)
z = -(1 / rho) * (mtimes(A.T, mu) + f)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi + l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, buildtime = create_function('upd_z_' + str(self._index), inp,
out, self.options)
self.problem_upd_z = prob
return buildtime
def construct_upd_z(self, problem=None, lineq_updz=True):
if problem is not None:
self.problem_upd_z = problem
self._lineq_updz = lineq_updz
return 0.
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
raise ValueError('For now, only equality constrained QP ' +
'z-updates are allowed!')
x_i = struct_symMX(self.q_i_struct)
x_j = struct_symMX(self.q_ij_struct)
l_i = struct_symMX(self.q_i_struct)
l_ij = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
par = struct_symMX(self.par_global_struct)
inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat]
t0 = t/T
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
x_j = self.q_ij_struct(x_j)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, l_i], tf, self.q_i)
self._transform_spline([x_j, l_ij], tf, self.q_ij)
# fill in parameters
A = A(par.cat)
b = b(par.cat)
# build KKT system and solve it via schur complement method
l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat)
f = -(l + rho*x)
G = -(1/rho)*mtimes(A, A.T)
h = b + (1/rho)*mtimes(A, f)
mu = solve(G, h)
z = -(1/rho)*(mtimes(A.T, mu)+f)
l_qi = self.q_i_struct.shape[0]
l_qij = self.q_ij_struct.shape[0]
z_i_new = self.q_i_struct(z[:l_qi])
z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij])
# transform back
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(z_i_new, tf, self.q_i)
self._transform_spline(z_ij_new, tf, self.q_ij)
out = [z_i_new.cat, z_ij_new.cat]
# create problem
prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options)
self.problem_upd_z = prob
return buildtime
def construct_Xdot_struct(nlp_options, variables_dict):
''' Construct a symbolic structure for the
discretized state derivatives.
@param nlp_options - discretization options
@param model - awebox model
@return Vdot - discretized state derivatives sym. struct.
'''
# extract information
nk = nlp_options['n_k']
xd = variables_dict['xd']
# derivatives at interval nodes
entry_tuple = (cas.entry('xd', repeat=[nk], struct=xd), )
# add derivatives on collocation nodes
if nlp_options['discretization'] == 'direct_collocation':
d = nlp_options['collocation']['d']
entry_tuple += (cas.entry('coll_xd', repeat=[nk, d], struct=xd), )
# make new symbolic structure
Xdot = cas.struct_symMX([entry_tuple])
return Xdot
def construct_parameters(self):
entries = []
for label, child in self.children.items():
entries_child = []
for name, par in child._parameters.items():
entries_child.append(entry(name, shape=par.shape))
entries.append(entry(label, struct=struct(entries_child)))
self._par_struct = struct(entries)
return struct_symMX(self._par_struct)
def construct_parameters(self):
entries = []
for label, child in self.children.items():
entries_child = []
for name, par in child._parameters.items():
entries_child.append(entry(name, shape=par.shape))
entries.append(entry(label, struct=struct(entries_child)))
self._par_struct = struct(entries)
return struct_symMX(self._par_struct)
def get_component_cost_structure(component_costs):
list_of_entries = []
for name in list(component_costs.keys()):
list_of_entries += [cas.entry(name)]
component_cost_struct = cas.struct_symMX(list_of_entries)
return component_cost_struct
def construct_upd_l(self, problem=None):
if problem is not None:
self.problem_upd_l = problem
return 0.
# create parameters
z_ij = struct_symMX(self.q_ij_struct)
l_ij = struct_symMX(self.q_ij_struct)
x_j = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
inp = [x_j, z_ij, l_ij, t, T, rho]
# update lambda
l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat))
out = [l_ij_new]
# create problem
prob, buildtime = create_function('upd_l_'+str(self._index), inp, out, self.options)
self.problem_upd_l = prob
return buildtime
def construct_upd_l(self, problem=None):
if problem is not None:
self.problem_upd_l = problem
return 0.
# create parameters
z_ij = struct_symMX(self.q_ij_struct)
l_ij = struct_symMX(self.q_ij_struct)
x_j = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
rho = MX.sym('rho')
inp = [x_j, z_ij, l_ij, t, T, rho]
# update lambda
l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat))
out = [l_ij_new]
# create problem
prob, buildtime = create_function('upd_l_'+str(self._index), inp, out, self.options)
self.problem_upd_l = prob
return buildtime
def setup_nlp_p(V, model):
cost = setup_nlp_cost()
p_fix = setup_nlp_p_fix(V, model)
P = cas.struct_symMX([
cas.entry('p', struct=p_fix),
cas.entry('cost', struct=cost),
cas.entry('theta0', struct=model.parameters_dict['theta0'])
])
return P
def setup_nlp_p(V, model):
cost = setup_nlp_cost()
p_fix = setup_nlp_p_fix(V, model)
use_vortex_linearization = 'lin' in model.parameters_dict.keys()
if use_vortex_linearization:
P = cas.struct_symMX([
cas.entry('p', struct=p_fix),
cas.entry('cost', struct=cost),
cas.entry('theta0', struct=model.parameters_dict['theta0']),
cas.entry('lin', struct=V)
])
else:
P = cas.struct_symMX([
cas.entry('p', struct=p_fix),
cas.entry('cost', struct=cost),
cas.entry('theta0', struct=model.parameters_dict['theta0'])
])
return P
def generate_parameterization_settings(self, options):
[
periodic, initial_conditions, param_initial_conditions,
param_terminal_conditions, terminal_inequalities,
integral_constraints
] = operation.get_operation_conditions(options)
xi = cas.struct_symMX([(cas.entry('xi_0'), cas.entry('xi_f'))])
xi_bounds = {}
xi_bounds['xi_0'] = [0.0, 0.0]
xi_bounds['xi_f'] = [0.0, 0.0]
V_pickle_initial = None
plot_dict_pickle_initial = None
V_pickle_terminal = None
plot_dict_pickle_terminal = None
if param_initial_conditions:
xi_bounds['xi_0'] = [0.0, 1.0]
if options['trajectory']['type'] == 'compromised_landing':
xi_0 = options['landing']['xi_0_initial']
xi_bounds['xi_0'] = [xi_0, xi_0]
V_pickle_initial, plot_dict_pickle_initial = self.__get_V_pickle(
options, 'initial')
if param_terminal_conditions:
xi_bounds['xi_f'] = [0.0, 1.0]
V_pickle_terminal, plot_dict_pickle_terminal = self.__get_V_pickle(
options, 'terminal')
if param_terminal_conditions and param_initial_conditions:
for theta in struct_op.subkeys(V_pickle_initial, 'theta'):
diff = V_pickle_terminal['theta',
theta] - V_pickle_initial['theta',
theta]
if theta != 't_f':
if (float(diff) != 0.0):
raise ValueError(
'Parameters of initial and terminal trajectory are not identical.'
)
xi_dict = {}
xi_dict['V_pickle_initial'] = V_pickle_initial
xi_dict['plot_dict_pickle_initial'] = plot_dict_pickle_initial
xi_dict['V_pickle_terminal'] = V_pickle_terminal
xi_dict['plot_dict_pickle_terminal'] = plot_dict_pickle_terminal
xi_dict['xi'] = xi
xi_dict['xi_bounds'] = xi_bounds
self.__xi_dict = xi_dict
return None
def _check_for_lineq(self):
g = []
for con in self.global_constraints:
lb, ub = con[1], con[2]
g = vertcat(g, con[0] - lb)
if not isinstance(lb, np.ndarray):
lb, ub = [lb], [ub]
for k, _ in enumerate(lb):
if lb[k] != ub[k]:
return False, None, None
sym, jac = [], []
for child, q_i in self.q_i.items():
for name, ind in q_i.items():
var = self.distr_problem.father.get_variables(child,
name,
spline=False,
symbolic=True,
substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name, ind in q_ij.items():
var = self.distr_problem.father.get_variables(
child,
name,
spline=False,
symbolic=True,
substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for sym in symvar(jac):
if sym not in self.par_global.values():
return False, None, None
par = struct_symMX(self.par_global_struct)
A, b = jac, -g
for s in sym:
A = substitute(A, s, np.zeros(s.shape))
b = substitute(b, s, np.zeros(s.shape))
dep_b = [s.name() for s in symvar(b)]
dep_A = [s.name() for s in symvar(b)]
for name, sym in self.par_global.items():
if sym.name() in dep_b:
b = substitute(b, sym, par[name])
if sym.name() in dep_A:
A = substitute(A, sym, par[name])
A = Function('A', [par], [A]).expand()
b = Function('b', [par], [b]).expand()
return True, A, b
def get_collocation_variables_struct(self, variables_dict, u_param):
entry_list = [
cas.entry('xd', struct=variables_dict['xd']),
cas.entry('xa', struct=variables_dict['xa'])
]
if 'xl' in list(variables_dict.keys()):
entry_list += [cas.entry('xl', struct=variables_dict['xl'])]
if u_param == 'poly':
entry_list += [cas.entry('u', struct=variables_dict['u'])]
return cas.struct_symMX(entry_list)
def _check_for_lineq(self):
g = []
for con in self.global_constraints:
lb, ub = con[1], con[2]
g = vertcat(g, con[0] - lb)
if not isinstance(lb, np.ndarray):
lb, ub = [lb], [ub]
for k, _ in enumerate(lb):
if lb[k] != ub[k]:
return False, None, None
sym, jac = [], []
for child, q_i in self.q_i.items():
for name, ind in q_i.items():
var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name, ind in q_ij.items():
var = self.distr_problem.father.get_variables(child, name, spline=False, symbolic=True, substitute=False)
jj = jacobian(g, var)
jac = horzcat(jac, jj[:, ind])
sym.append(var)
for sym in symvar(jac):
if sym not in self.par_global.values():
return False, None, None
par = struct_symMX(self.par_global_struct)
A, b = jac, -g
for s in sym:
A = substitute(A, s, np.zeros(s.shape))
b = substitute(b, s, np.zeros(s.shape))
dep_b = [s.name() for s in symvar(b)]
dep_A = [s.name() for s in symvar(b)]
for name, sym in self.par_global.items():
if sym.name() in dep_b:
b = substitute(b, sym, par[name])
if sym.name() in dep_A:
A = substitute(A, sym, par[name])
A = Function('A', [par], [A]).expand()
b = Function('b', [par], [b]).expand()
return True, A, b
def _check_for_lineq(self):
g = []
for con in self.constraints:
lb, ub = con[1], con[2]
g = vertcat([g, con[0] - lb])
if not isinstance(lb, np.ndarray):
lb, ub = [lb], [ub]
for k in range(len(lb)):
if lb[k] != ub[k]:
return False, None, None
sym, jac = [], []
for child, q_i in self.q_i.items():
for name, ind in q_i.items():
var = child.get_variable(name, spline=False)
jj = jacobian(g, var)
jac = horzcat([jac, jj[:, ind]])
sym.append(var)
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name, ind in q_ij.items():
var = child.get_variable(name, spline=False)
jj = jacobian(g, var)
jac = horzcat([jac, jj[:, ind]])
sym.append(var)
for sym in symvar(jac):
if sym not in self.par_i.values():
return False, None, None
par = struct_symMX(self.par_struct)
A, b = jac, -g
for s in sym:
A = substitute(A, s, np.zeros(s.shape))
b = substitute(b, s, np.zeros(s.shape))
dep_b = [s.getName() for s in symvar(b)]
dep_A = [s.getName() for s in symvar(b)]
for name, sym in self.par_i.items():
if sym.getName() in dep_b:
b = substitute(b, sym, par[name])
if sym.getName() in dep_A:
A = substitute(A, sym, par[name])
A = MXFunction('A', [par], [A]).expand()
b = MXFunction('b', [par], [b]).expand()
return True, A, b
def setup_integral_output_structure(nlp_numerics_options, integral_outputs):
nk = nlp_numerics_options['n_k']
entry_tuple = ()
# interval outputs
entry_tuple += (cas.entry('int_out',
repeat=[nk + 1],
struct=integral_outputs), )
if nlp_numerics_options['discretization'] == 'direct_collocation':
d = nlp_numerics_options['collocation']['d']
entry_tuple += (cas.entry('coll_int_out',
repeat=[nk, d],
struct=integral_outputs), )
Integral_outputs_struct = cas.struct_symMX([entry_tuple])
return Integral_outputs_struct
def construct_upd_res(self, problem=None):
if problem is not None:
self.problem_upd_res = problem
return 0.
# create parameters
x_i = struct_symMX(self.q_i_struct)
z_i = struct_symMX(self.q_i_struct)
z_i_p = struct_symMX(self.q_i_struct)
z_ij = struct_symMX(self.q_ij_struct)
z_ij_p = struct_symMX(self.q_ij_struct)
x_j = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
t0 = t / T
rho = MX.sym('rho')
inp = [x_i, z_i, z_i_p, z_ij, z_ij_p, x_j, t, T, rho]
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
z_i = self.q_i_struct(z_i)
z_i_p = self.q_i_struct(z_i_p)
z_ij = self.q_ij_struct(z_ij)
z_ij_p = self.q_ij_struct(z_ij_p)
x_j = self.q_ij_struct(x_j)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, z_i_p], tf, self.q_i)
self._transform_spline([x_j, z_ij, z_ij_p], tf, self.q_ij)
# compute residuals
pr = mtimes((x_i.cat - z_i.cat).T, (x_i.cat - z_i.cat))
pr += mtimes((x_j.cat - z_ij.cat).T, (x_j.cat - z_ij.cat))
dr = rho * mtimes((z_i.cat - z_i_p.cat).T, (z_i.cat - z_i_p.cat))
dr += rho * mtimes((z_ij.cat - z_ij_p.cat).T, (z_ij.cat - z_ij_p.cat))
cr = rho * pr + dr
out = [pr, dr, cr]
# create problem
prob, buildtime = create_function('upd_res_' + str(self._index), inp,
out, self.options)
self.problem_upd_res = prob
return buildtime
def construct_upd_res(self, problem=None):
if problem is not None:
self.problem_upd_res = problem
return 0.
# create parameters
x_i = struct_symMX(self.q_i_struct)
z_i = struct_symMX(self.q_i_struct)
z_i_p = struct_symMX(self.q_i_struct)
z_ij = struct_symMX(self.q_ij_struct)
z_ij_p = struct_symMX(self.q_ij_struct)
x_j = struct_symMX(self.q_ij_struct)
t = MX.sym('t')
T = MX.sym('T')
t0 = t/T
rho = MX.sym('rho')
inp = [x_i, z_i, z_i_p, z_ij, z_ij_p, x_j, t, T, rho]
# put symbols in MX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
z_i = self.q_i_struct(z_i)
z_i_p = self.q_i_struct(z_i_p)
z_ij = self.q_ij_struct(z_ij)
z_ij_p = self.q_ij_struct(z_ij_p)
x_j = self.q_ij_struct(x_j)
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, z_i_p], tf, self.q_i)
self._transform_spline([x_j, z_ij, z_ij_p], tf, self.q_ij)
# compute residuals
pr = mtimes((x_i.cat-z_i.cat).T, (x_i.cat-z_i.cat))
pr += mtimes((x_j.cat-z_ij.cat).T, (x_j.cat-z_ij.cat))
dr = rho*mtimes((z_i.cat-z_i_p.cat).T, (z_i.cat-z_i_p.cat))
dr += rho*mtimes((z_ij.cat-z_ij_p.cat).T, (z_ij.cat-z_ij_p.cat))
cr = rho*pr + dr
out = [pr, dr, cr]
# create problem
prob, buildtime = create_function('upd_res_'+str(self._index), inp, out, self.options)
self.problem_upd_res = prob
return buildtime
def __init__(
self,
dynamics,
obstacles=[],
h=0.05,
horizon=10,
Q=None,
P=None,
R=None,
S=None,
Sl=None,
ulb=None,
uub=None,
xlb=None,
xub=None,
terminal_constraint=None,
solver_opts=None,
x_d=[0] * 12,
):
""" Initialize and build the MPC solver
# Arguments:
horizon: Prediction horizon in seconds
model: System model
# Optional Argumants:
Q: State penalty matrix, default=diag(1,...,1)
P: Termial penalty matrix, default=diag(1,...,1)
R: Input penalty matrix, default=diag(1,...,1)*0.01
ulb: Lower boundry input
uub: Upper boundry input
xlb: Lower boundry state
xub: Upper boundry state
terminal_constraint: Terminal condition on the state
* if None: No terminal constraint is used
* if zero: Terminal state is equal to zero
* if nonzero: Terminal state is bounded within +/- the constraint
solver_opts: Additional options to pass to the NLP solver
e.g.: solver_opts['print_time'] = False
solver_opts['ipopt.tol'] = 1e-8
"""
self.horizon = horizon
build_solver_time = -time.time()
self.dt = h
self.Nx, self.Nu = 12, 4 # TODO
Nopt = self.Nu + self.Nx
self.Nt = int(horizon)
self.dynamics = dynamics
# Initialize variables
self.set_cost_functions()
# Cost function weights
if P is None:
P = np.eye(self.Nx) * 10
if Q is None:
Q = np.eye(self.Nx)
if R is None:
R = np.eye(self.Nu) * 0.01
if S is None:
S = np.eye(self.Nx) * 1000
if Sl is None:
Sl = np.full((self.Nx, ), 1000)
self.Q = ca.MX(Q)
self.P = ca.MX(P)
self.R = ca.MX(R)
self.S = ca.MX(S)
self.Sl = ca.MX(Sl)
if xub is None:
xub = np.full((self.Nx), np.inf)
if xlb is None:
xlb = np.full((self.Nx), -np.inf)
if uub is None:
uub = np.full((self.Nu), np.inf)
if ulb is None:
ulb = np.full((self.Nu), -np.inf)
self.ulb = ulb
self.uub = uub
self.terminal_constraint = terminal_constraint
# Starting state parameters - add slack here
x0 = ca.MX.sym('x0', self.Nx)
x_ref = ca.MX.sym('x_ref', self.Nx * (self.Nt + 1))
u0 = ca.MX.sym('u0', self.Nu)
param_s = ca.vertcat(x0, x_ref, u0)
# Create optimization variables
opt_var = ctools.struct_symMX([(ctools.entry('u',
shape=(self.Nu, ),
repeat=self.Nt),
ctools.entry('x',
shape=(self.Nx, ),
repeat=self.Nt + 1),
ctools.entry('s',
shape=(self.Nx, ),
repeat=1))])
self.opt_var = opt_var
self.num_var = opt_var.size
# Decision variable boundries
self.optvar_lb = opt_var(-np.inf)
self.optvar_ub = opt_var(np.inf)
""" Set initial values """
obj = ca.MX(0)
con_eq = []
con_ineq = []
con_ineq_lb = []
con_ineq_ub = []
con_eq.append(opt_var['x', 0] - x0)
# Generate MPC Problem
for t in range(self.Nt):
# Get variables
x_t = opt_var['x', t]
u_t = opt_var['u', t]
# Dynamics constraint
x_t_next = self.dynamics(x_t, u_t)
con_eq.append(x_t_next - opt_var['x', t + 1])
# Input constraints
if uub is not None:
con_ineq.append(u_t)
con_ineq_ub.append(uub)
con_ineq_lb.append(np.full((self.Nu, ), -ca.inf))
if ulb is not None:
con_ineq.append(u_t)
con_ineq_ub.append(np.full((self.Nu, ), ca.inf))
con_ineq_lb.append(ulb)
# State constraints
if xub is not None:
con_ineq.append(x_t)
con_ineq_ub.append(xub)
con_ineq_lb.append(np.full((self.Nx, ), -ca.inf))
if xlb is not None:
con_ineq.append(x_t)
con_ineq_ub.append(np.full((self.Nx, ), ca.inf))
con_ineq_lb.append(xlb)
if obstacles is not None:
for obs in obstacles:
con_ineq.append(self.distance_obs(x_t, obs))
con_ineq_ub.append(ca.inf)
con_ineq_lb.append(ca.DM((obs.eps + obs.radius)**2))
obj += self.running_cost((x_t - x_ref[t * 12:12 + t * 12]), self.Q,
u_t, self.R)
# Terminal Cost
obj += self.terminal_cost(
(opt_var['x', self.Nt] - x_ref[(self.Nt) * 12:12 +
(self.Nt) * 12]), self.P)
# Terminal contraint
s_t = opt_var['s', 0]
con_ineq.append((opt_var['x', self.Nt] - x_ref[(self.Nt) * 12:12 +
(self.Nt) * 12])**2 -
s_t)
con_ineq_lb.append(np.full((self.Nx, ), 0.0))
con_ineq_ub.append(np.full((self.Nx, ), terminal_constraint**2))
obj += self.slack_cost(s_t, self.S, self.Sl)
# Equality constraints bounds are 0 (they are equality constraints),
# -> Refer to CasADi documentation
num_eq_con = ca.vertcat(*con_eq).size1()
num_ineq_con = ca.vertcat(*con_ineq).size1()
con_eq_lb = np.zeros((num_eq_con, 1))
con_eq_ub = np.zeros((num_eq_con, 1))
# Set constraints
con = ca.vertcat(*(con_eq + con_ineq))
self.con_lb = ca.vertcat(con_eq_lb, *con_ineq_lb)
self.con_ub = ca.vertcat(con_eq_ub, *con_ineq_ub)
# Build NLP Solver (can also solve QP)
nlp = dict(x=opt_var, f=obj, g=con, p=param_s)
options = {
'ipopt.print_level': 0,
'ipopt.mu_init': 0.01,
'ipopt.tol': 1e-4,
'ipopt.sb': 'yes',
'ipopt.warm_start_init_point': 'yes',
'ipopt.warm_start_bound_push': 1e-3,
'ipopt.warm_start_bound_frac': 1e-3,
'ipopt.warm_start_slack_bound_frac': 1e-3,
'ipopt.warm_start_slack_bound_push': 1e-3,
'ipopt.warm_start_mult_bound_push': 1e-3,
'ipopt.mu_strategy': 'adaptive',
'print_time': False,
'verbose': False,
'expand': True,
'ipopt.max_iter': 100
}
if solver_opts is not None:
options.update(solver_opts)
self.solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options)
# self.solver = ca.nlpsol('mpc_solver', 'sqpmethod', nlp, self.sol_options_sqp)
build_solver_time += time.time()
print('\n________________________________________')
print('# Time to build mpc solver: %f sec' % build_solver_time)
print('# Number of variables: %d' % self.num_var)
print('# Number of equality constraints: %d' % num_eq_con)
print('# Number of inequality constraints: %d' % num_ineq_con)
print('----------------------------------------')
pass
def __init__(self,
model,
dynamics,
horizon=10,
Q=None,
P=None,
R=None,
ulb=None,
uub=None,
xlb=None,
xub=None,
terminal_constraint=None,
solver_opts=None):
""" Initialize and build the MPC solver
# Arguments:
horizon: Prediction horizon in seconds
model: System model
# Optional Argumants:
Q: State penalty matrix
P: Termial penalty matrix
R: Input penalty matrix
ulb: Lower boundry input
uub: Upper boundry input
xlb: Lower boundry state
xub: Upper boundry state
terminal_constraint: Terminal condition on the state
* if None: No terminal constraint is used
* if zero: Terminal state is equal to zero
* if nonzero: Terminal state is bounded within +/- the constraint
solver_opts: Additional options to pass to the NLP solver
e.g.: solver_opts['print_time'] = False
solver_opts['ipopt.tol'] = 1e-8
"""
build_solver_time = -time.time()
self.dt = model.dt
self.Nx, self.Nu = len(model.x_eq), 1
Nopt = self.Nu + self.Nx
self.Nt = int(horizon / self.dt)
self.dynamics = dynamics
# Initialize variables
self.set_cost_functions()
self.x_sp = None
# Cost function weights
if P is None:
P = np.eye(self.Nx) * 10
if Q is None:
Q = np.eye(self.Nx)
if R is None:
R = np.eye(self.Nu) * 0.01
self.Q = ca.MX(Q)
self.P = ca.MX(P)
self.R = ca.MX(R)
if xub is None:
xub = np.full((self.Nx), np.inf)
if xlb is None:
xlb = np.full((self.Nx), -np.inf)
if uub is None:
uub = np.full((self.Nu), np.inf)
if ulb is None:
ulb = np.full((self.Nu), -np.inf)
# Starting state parameters - add slack here
x0 = ca.MX.sym('x0', self.Nx)
x0_ref = ca.MX.sym('x0_ref', self.Nx)
u0 = ca.MX.sym('u0', self.Nu)
param_s = ca.vertcat(x0, x0_ref, u0)
# Create optimization variables
opt_var = ctools.struct_symMX([(
ctools.entry('u', shape=(self.Nu, ), repeat=self.Nt),
ctools.entry('x', shape=(self.Nx, ), repeat=self.Nt + 1),
)])
self.opt_var = opt_var
self.num_var = opt_var.size
# Decision variable boundries
self.optvar_lb = opt_var(-np.inf)
self.optvar_ub = opt_var(np.inf)
""" Set initial values """
obj = ca.MX(0)
con_eq = []
con_ineq = []
con_ineq_lb = []
con_ineq_ub = []
con_eq.append(opt_var['x', 0] - x0)
# Generate MPC Problem
for t in range(self.Nt):
# Get variables
x_t = opt_var['x', t]
u_t = opt_var['u', t]
# Dynamics constraint
x_t_next = self.dynamics(x_t, u_t)
con_eq.append(x_t_next - opt_var['x', t + 1])
# Input constraints
if uub is not None:
con_ineq.append(u_t)
con_ineq_ub.append(uub)
con_ineq_lb.append(np.full((self.Nu, ), -ca.inf))
if ulb is not None:
con_ineq.append(u_t)
con_ineq_ub.append(np.full((self.Nu, ), ca.inf))
con_ineq_lb.append(ulb)
# State constraints
if xub is not None:
con_ineq.append(x_t)
con_ineq_ub.append(xub)
con_ineq_lb.append(np.full((self.Nx, ), -ca.inf))
if xlb is not None:
con_ineq.append(x_t)
con_ineq_ub.append(np.full((self.Nx, ), ca.inf))
con_ineq_lb.append(xlb)
# Objective Function / Cost Function
obj += self.running_cost((x_t - x0_ref), self.Q, u_t, self.R)
# Terminal Cost
obj += self.terminal_cost(opt_var['x', self.Nt] - x0_ref, self.P)
# Terminal contraint
if terminal_constraint is not None:
con_ineq.append(opt_var['x', self.Nt] - x0_ref)
con_ineq_lb.append(np.full((self.Nx, ), -terminal_constraint))
con_ineq_ub.append(np.full((self.Nx, ), terminal_constraint))
# Equality constraints bounds are 0 (they are equality constraints),
# -> Refer to CasADi documentation
num_eq_con = ca.vertcat(*con_eq).size1()
num_ineq_con = ca.vertcat(*con_ineq).size1()
con_eq_lb = np.zeros((num_eq_con, ))
con_eq_ub = np.zeros((num_eq_con, ))
# Set constraints
con = ca.vertcat(*con_eq, *con_ineq)
self.con_lb = ca.vertcat(con_eq_lb, *con_ineq_lb)
self.con_ub = ca.vertcat(con_eq_ub, *con_ineq_ub)
# Build NLP Solver (can also solve QP)
nlp = dict(x=opt_var, f=obj, g=con, p=param_s)
options = {
'ipopt.print_level': 0,
'ipopt.mu_init': 0.01,
'ipopt.tol': 1e-8,
'ipopt.warm_start_init_point': 'yes',
'ipopt.warm_start_bound_push': 1e-9,
'ipopt.warm_start_bound_frac': 1e-9,
'ipopt.warm_start_slack_bound_frac': 1e-9,
'ipopt.warm_start_slack_bound_push': 1e-9,
'ipopt.warm_start_mult_bound_push': 1e-9,
'ipopt.mu_strategy': 'adaptive',
'print_time': False,
'verbose': False,
'expand': False
}
if solver_opts is not None:
options.update(solver_opts)
self.solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options)
build_solver_time += time.time()
print('\n________________________________________')
print('# Time to build mpc solver: %f sec' % build_solver_time)
print('# Number of variables: %d' % self.num_var)
print('# Number of equality constraints: %d' % num_eq_con)
print('# Number of inequality constraints: %d' % num_ineq_con)
print('----------------------------------------')
pass
def get_xi_struct():
xi = cas.struct_symMX([(cas.entry('xi_0'), cas.entry('xi_f'))])
return xi
def __init__(self,
horizon,
model,
gp=None,
Q=None,
P=None,
R=None,
S=None,
lam=None,
lam_state=None,
ulb=None,
uub=None,
xlb=None,
xub=None,
terminal_constraint=None,
feedback=True,
percentile=None,
gp_method='TA',
costFunc='quad',
solver_opts=None,
discrete_method='gp',
inequality_constraints=None,
num_con_par=0,
hybrid=None,
Bd=None,
Bf=None):
""" Initialize and build the MPC solver
# Arguments:
horizon: Prediction horizon with control inputs
model: System model
# Optional Argumants:
gp: GP model
Q: State penalty matrix, default=diag(1,...,1)
P: Termial penalty matrix, default=diag(1,...,1)
if feedback is True, then P is the solution of the DARE,
discarding this option.
R: Input penalty matrix, default=diag(1,...,1)*0.01
S: Input rate of change penalty matrix, default=diag(1,...,1)*0.1
lam: Slack variable penalty for constraints, defalt=1000
lam_state: Slack variable penalty for violation of upper/lower
state boundy, defalt=None
ulb: Lower boundry input
uub: Upper boundry input
xlb: Lower boundry state
xub: Upper boundry state
terminal_constraint: Terminal condition on the state
* if None: No terminal constraint is used
* if zero: Terminal state is equal to zero
* if nonzero: Terminal state is bounded within +/- the constraint
* if not None and feedback is True, then the expected value of
the Lyapunov function E{x^TPx} < terminal_constraint
is used as a terminal constraint.
feedback: If true, use an LQR feedback function u= Kx + v
percentile: Measure how far from the contrain that is allowed,
P(X in constrained set) > percentile,
percentile= 1 - probability of violation,
default=0.95
gp_method: Method of propagating the uncertainty
Possible options:
'TA': Second order Taylor approximation
'ME': Mean equivalent approximation
costFunc: Cost function to use in the objective
'quad': Expected valaue of Quadratic Cost
'sat': Expected value of Saturating cost
solver_opts: Additional options to pass to the NLP solver
e.g.: solver_opts['print_time'] = False
solver_opts['ipopt.tol'] = 1e-8
discrete_method: 'gp' - Gaussian process model
'rk4' - Runga-Kutta 4 Integrator
'exact' - CVODES or IDEAS (for ODEs or DEAs)
'hybrid' - GP model for dynamic equations, and RK4
for kinematic equations
'd_hybrid' - Same as above, without uncertainty
'f_hybrid' - GP estimating modelling errors, with
RK4 computing the the actual model
num_con_par: Number of parameters to pass to the inequality function
inequality_constraints: Additional inequality constraints
Use a function with inputs (x, covar, u, eps) and
that returns a dictionary with inequality constraints and limits.
e.g. cons = dict(con_ineq=con_ineq_array,
con_ineq_lb=con_ineq_lb_array,
con_ineq_ub=con_ineq_ub_array
)
# NOTES:
* Differentiation of Sundails integrators is not supported with SX graph,
meaning that the solver option 'extend_graph' must be set to False
to use MX graph instead when using the 'exact' discrete method.
* At the moment the f_hybrid option is not finished implemented...
"""
build_solver_time = -time.time()
dt = model.sampling_time()
Ny, Nu, Np = model.size()
Nx = Nu + Ny
Nt = int(horizon / dt)
self.__dt = dt
self.__Nt = Nt
self.__Ny = Ny
self.__Nx = Nx
self.__Nu = Nu
self.__num_con_par = num_con_par
self.__model = model
self.__hybrid = hybrid
self.__gp = gp
self.__feedback = feedback
self.__discrete_method = discrete_method
""" Default penalty values """
if P is None:
P = np.eye(Ny)
if Q is None:
Q = np.eye(Ny)
if R is None:
R = np.eye(Nu) * 0.01
if S is None:
S = np.eye(Nu) * 0.1
if lam is None:
lam = 1000
self.__Q = Q
self.__P = P
self.__R = R
self.__S = S
self.__Bd = Bd
self.__Bf = Bf
if xub is None:
xub = np.full((Ny), np.inf)
if xlb is None:
xlb = np.full((Ny), -np.inf)
if uub is None:
uub = np.full((Nu), np.inf)
if ulb is None:
ulb = np.full((Nu), -np.inf)
""" Default percentile probability """
if percentile is None:
percentile = 0.95
quantile_x = np.ones(Ny) * norm.ppf(percentile)
quantile_u = np.ones(Nu) * norm.ppf(percentile)
Hx = ca.MX.eye(Ny)
Hu = ca.MX.eye(Nu)
""" Create parameter symbols """
mean_0_s = ca.MX.sym('mean_0', Ny)
mean_ref_s = ca.MX.sym('mean_ref', Ny)
u_0_s = ca.MX.sym('u_0', Nu)
covariance_0_s = ca.MX.sym('covariance_0', Ny * Ny)
K_s = ca.MX.sym('K', Nu * Ny)
P_s = ca.MX.sym('P', Ny * Ny)
con_par = ca.MX.sym('con_par', num_con_par)
param_s = ca.vertcat(mean_0_s, mean_ref_s, covariance_0_s, u_0_s, K_s,
P_s, con_par)
""" Select wich GP function to use """
if discrete_method is 'gp':
self.__gp.set_method(gp_method)
#TODO:Fix
if solver_opts['expand'] is not False and discrete_method is 'exact':
raise TypeError(
"Can't use exact discrete system with expanded graph")
""" Initialize state variance with the GP noise variance """
if gp is not None:
#TODO: Cannot use gp variance with hybrid model
self.__variance_0 = np.full((Ny), 1e-10) #gp.noise_variance()
else:
self.__variance_0 = np.full((Ny), 1e-10)
""" Define which cost function to use """
self.__set_cost_function(costFunc, mean_ref_s, P_s.reshape((Ny, Ny)))
""" Feedback function """
mean_s = ca.MX.sym('mean', Ny)
v_s = ca.MX.sym('v', Nu)
if feedback:
u_func = ca.Function(
'u', [mean_s, mean_ref_s, v_s, K_s],
[v_s + ca.mtimes(K_s.reshape((Nu, Ny)), mean_s - mean_ref_s)])
else:
u_func = ca.Function('u', [mean_s, mean_ref_s, v_s, K_s], [v_s])
self.__u_func = u_func
""" Create variables struct """
var = ctools.struct_symMX([(
ctools.entry('mean', shape=(Ny, ), repeat=Nt + 1),
ctools.entry('L',
shape=(int((Ny**2 - Ny) / 2 + Ny), ),
repeat=Nt + 1),
ctools.entry('v', shape=(Nu, ), repeat=Nt),
ctools.entry('eps', shape=(3, ), repeat=Nt + 1),
ctools.entry('eps_state', shape=(Ny, ), repeat=Nt + 1),
)])
num_slack = 3 #TODO: Make this a little more dynamic...
num_state_slack = Ny
self.__var = var
self.__num_var = var.size
# Decision variable boundries
self.__varlb = var(-np.inf)
self.__varub = var(np.inf)
""" Adjust hard boundries """
for t in range(Nt + 1):
j = Ny
k = 0
for i in range(Ny):
# Lower boundry of diagonal
self.__varlb['L', t, k] = 0
k += j
j -= 1
self.__varlb['eps', t] = 0
self.__varlb['eps_state', t] = 0
if xub is not None:
self.__varub['mean', t] = xub
if xlb is not None:
self.__varlb['mean', t] = xlb
if lam_state is None:
self.__varub['eps_state'] = 0
""" Input covariance matrix """
if discrete_method is 'hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
Nz_gp = Ny_gp + Nu_gp
covar_d_sx = ca.SX.sym('cov_d', Ny_gp, Ny_gp)
K_sx = ca.SX.sym('K', Nu, Ny)
covar_u_func = ca.Function(
'cov_u',
[covar_d_sx, K_sx],
# [K_sx @ covar_d_sx @ K_sx.T])
[ca.SX(Nu, Nu)])
covar_s = ca.SX(Nz_gp, Nz_gp)
covar_s[:Ny_gp, :Ny_gp] = covar_d_sx
# covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u)
covar_func = ca.Function('covar', [covar_d_sx], [covar_s])
elif discrete_method is 'f_hybrid':
#TODO: Fix this...
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
Nz_gp = Ny_gp + Nu_gp
# covar_x_s = ca.MX.sym('covar_x', Ny_gp, Ny_gp)
covar_d_sx = ca.SX.sym('cov_d', Ny_gp, Ny_gp)
K_sx = ca.SX.sym('K', Nu, Ny)
#
covar_u_func = ca.Function(
'cov_u',
[covar_d_sx, K_sx],
# [K_sx @ covar_d_sx @ K_sx.T])
[ca.SX(Nu, Nu)])
# cov_xu_func = ca.Function('cov_xu', [covar_x_sx, K_sx],
# [covar_x_sx @ K_sx.T])
# cov_xu = cov_xu_func(covar_x_s, K_s.reshape((Nu, Ny)))
# cov_u = covar_u_func(covar_x_s, K_s.reshape((Nu, Ny)))
covar_s = ca.SX(Nz_gp, Nz_gp)
covar_s[:Ny_gp, :Ny_gp] = covar_d_sx
# covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u)
covar_func = ca.Function('covar', [covar_d_sx], [covar_s])
else:
covar_x_s = ca.MX.sym('covar_x', Ny, Ny)
covar_x_sx = ca.SX.sym('cov_x', Ny, Ny)
K_sx = ca.SX.sym('K', Nu, Ny)
covar_u_func = ca.Function('cov_u', [covar_x_sx, K_sx],
[K_sx @ covar_x_sx @ K_sx.T])
cov_xu_func = ca.Function('cov_xu', [covar_x_sx, K_sx],
[covar_x_sx @ K_sx.T])
cov_xu = cov_xu_func(covar_x_s, K_s.reshape((Nu, Ny)))
cov_u = covar_u_func(covar_x_s, K_s.reshape((Nu, Ny)))
covar_s = ca.blockcat(covar_x_s, cov_xu, cov_xu.T, cov_u)
covar_func = ca.Function('covar', [covar_x_s], [covar_s])
""" Hybrid output covariance matrix """
if discrete_method is 'hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
covar_d_sx = ca.SX.sym('covar_d', Ny_gp, Ny_gp)
covar_x_sx = ca.SX.sym('covar_x', Ny, Ny)
u_s = ca.SX.sym('u', Nu)
cov_x_next_s = ca.SX(Ny, Ny)
cov_x_next_s[:Ny_gp, :Ny_gp] = covar_d_sx
#TODO: Missing kinematic states
covar_x_next_func = ca.Function(
'cov',
#[mean_s, u_s, covar_d_sx, covar_x_sx],
[covar_d_sx],
[cov_x_next_s])
""" f_hybrid output covariance matrix """
elif discrete_method is 'f_hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
# Nz_gp = Ny_gp + Nu_gp
covar_d_sx = ca.SX.sym('covar_d', Ny_gp, Ny_gp)
covar_x_sx = ca.SX.sym('covar_x', Ny, Ny)
# L_x = ca.SX.sym('L', ca.Sparsity.lower(Ny))
# L_d = ca.SX.sym('L', ca.Sparsity.lower(3))
mean_s = ca.SX.sym('mean', Ny)
u_s = ca.SX.sym('u', Nu)
# A_f = hybrid.rk4_jacobian_x(mean_s[Ny_gp:], mean_s[:Ny_gp])
# B_f = hybrid.rk4_jacobian_u(mean_s[Ny_gp:], mean_s[:Ny_gp])
# C = ca.horzcat(A_f, B_f)
# cov = ca.blocksplit(covar_x_s, Ny_gp, Ny_gp)
# cov[-1][-1] = covar_d_sx
# cov_i = ca.blockcat(cov)
# cov_f = C @ cov_i @ C.T
# cov[0][0] = cov_f
cov_x_next_s = ca.SX(Ny, Ny)
cov_x_next_s[:Ny_gp, :Ny_gp] = covar_d_sx
# cov_x_next_s[Ny_gp:, Ny_gp:] =
#TODO: Pre-solve the GP jacobian using the initial condition in the solve iteration
# jac_mean = ca.SX(Ny_gp, Ny)
# jac_mean = self.__gp.jacobian(mean_s[:Ny_gp], u_s, 0)
# A = ca.horzcat(jac_f, Bd)
# jac = Bf @ jac_f @ Bf.T + Bd @ jac_mean @ Bd.T
# temp = jac_mean @ covar_x_s
# temp = jac_mean @ L_s
# cov_i = ca.SX(Ny + 3, Ny + 3)
# cov_i[:Ny,:Ny] = covar_x_s
# cov_i[Ny:, Ny:] = covar_d_s
# cov_i[Ny:, :Ny] = temp
# cov_i[:Ny, Ny:] = temp.T
#TODO: This is just a new TA implementation... CLEAN UP...
covar_x_next_func = ca.Function(
'cov',
[mean_s, u_s, covar_d_sx, covar_x_sx],
#TODO: Clean up
# [A @ cov_i @ A.T])
# [Bd @ covar_d_s @ Bd.T + jac @ covar_x_s @ jac.T])
# [ca.blockcat(cov)])
[cov_x_next_s])
# Cholesky factorization of covariance function
# S_x_next_func = ca.Function( 'S_x', [mean_s, u_s, covar_d_s, covar_x_s],
# [Bd @ covar_d_s + jac @ covar_x_s])
L_s = ca.SX.sym('L', ca.Sparsity.lower(Ny))
L_to_cov_func = ca.Function('cov', [L_s], [L_s @ L_s.T])
covar_x_sx = ca.SX.sym('cov_x', Ny, Ny)
cholesky = ca.Function('cholesky', [covar_x_sx],
[ca.chol(covar_x_sx).T])
""" Set initial values """
obj = ca.MX(0)
con_eq = []
con_ineq = []
con_ineq_lb = []
con_ineq_ub = []
con_eq.append(var['mean', 0] - mean_0_s)
L_0_s = ca.MX(ca.Sparsity.lower(Ny), var['L', 0])
L_init = cholesky(covariance_0_s.reshape((Ny, Ny)))
con_eq.append(L_0_s.nz[:] - L_init.nz[:])
u_past = u_0_s
""" Build constraints """
for t in range(Nt):
# Input to GP
mean_t = var['mean', t]
u_t = u_func(mean_t, mean_ref_s, var['v', t], K_s)
L_x = ca.MX(ca.Sparsity.lower(Ny), var['L', t])
covar_x_t = L_to_cov_func(L_x)
if discrete_method is 'hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
covar_t = covar_func(covar_x_t[:Ny_gp, :Ny_gp])
elif discrete_method is 'd_hybrid':
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
covar_t = ca.MX(Ny_gp + Nu_gp, Ny_gp + Nu_gp)
elif discrete_method is 'gp':
covar_t = covar_func(covar_x_t)
else:
covar_t = ca.MX(Nx, Nx)
""" Select the chosen integrator """
if discrete_method is 'rk4':
mean_next_pred = model.rk4(mean_t, u_t, [])
covar_x_next_pred = ca.MX(Ny, Ny)
elif discrete_method is 'exact':
mean_next_pred = model.Integrator(x0=mean_t, p=u_t)['xf']
covar_x_next_pred = ca.MX(Ny, Ny)
elif discrete_method is 'd_hybrid':
# Deterministic hybrid GP model
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t,
covar_t)
mean_next_pred = ca.vertcat(
mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], []))
covar_x_next_pred = ca.MX(Ny, Ny)
elif discrete_method is 'hybrid':
# Hybrid GP model
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t,
covar_t)
mean_next_pred = ca.vertcat(
mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], []))
#covar_x_next_pred = covar_x_next_func(mean_t, u_t, covar_d,
# covar_x_t)
covar_x_next_pred = covar_x_next_func(covar_d)
elif discrete_method is 'f_hybrid':
#TODO: Hybrid GP model estimating model error
N_gp, Ny_gp, Nu_gp = self.__gp.get_size()
mean_d, covar_d = self.__gp.predict(mean_t[:Ny_gp], u_t,
covar_t)
mean_next_pred = ca.vertcat(
mean_d, hybrid.rk4(mean_t[Ny_gp:], mean_t[:Ny_gp], []))
covar_x_next_pred = covar_x_next_func(mean_t, u_t, covar_d,
covar_x_t)
else: # Use GP as default
mean_next_pred, covar_x_next_pred = self.__gp.predict(
mean_t, u_t, covar_t)
""" Continuity constraints """
mean_next = var['mean', t + 1]
con_eq.append(mean_next_pred - mean_next)
L_x_next = ca.MX(ca.Sparsity.lower(Ny), var['L', t + 1])
covar_x_next = L_to_cov_func(L_x_next).reshape((Ny * Ny, 1))
L_x_next_pred = cholesky(covar_x_next_pred)
con_eq.append(L_x_next_pred.nz[:] - L_x_next.nz[:])
""" Chance state constraints """
cons = self.__constraint(mean_next, L_x_next, Hx, quantile_x, xub,
xlb, var['eps_state', t])
con_ineq.extend(cons['con'])
con_ineq_lb.extend(cons['con_lb'])
con_ineq_ub.extend(cons['con_ub'])
""" Input constraints """
# cov_u = covar_u_func(covar_x_t, K_s.reshape((Nu, Ny)))
cov_u = ca.MX(Nu, Nu)
# cons = self.__constraint(u_t, cov_u, Hu, quantile_u, uub, ulb)
# con_ineq.extend(cons['con'])
# con_ineq_lb.extend(cons['con_lb'])
# con_ineq_ub.extend(cons['con_ub'])
if uub is not None:
con_ineq.append(u_t)
con_ineq_ub.extend(uub)
con_ineq_lb.append(np.full((Nu, ), -ca.inf))
if ulb is not None:
con_ineq.append(u_t)
con_ineq_ub.append(np.full((Nu, ), ca.inf))
con_ineq_lb.append(ulb)
""" Add extra constraints """
if inequality_constraints is not None:
cons = inequality_constraints(var['mean', t + 1], covar_x_next,
u_t, var['eps', t], con_par)
con_ineq.extend(cons['con_ineq'])
con_ineq_lb.extend(cons['con_ineq_lb'])
con_ineq_ub.extend(cons['con_ineq_ub'])
""" Objective function """
u_delta = u_t - u_past
obj += self.__l_func(var['mean', t], covar_x_t, u_t, cov_u, u_delta) \
+ np.full((1, num_slack),lam) @ var['eps', t]
if lam_state is not None:
obj += np.full(
(1, num_state_slack), lam_state) @ var['eps_state', t]
u_t = u_past
L_x = ca.MX(ca.Sparsity.lower(Ny), var['L', Nt])
covar_x_t = L_to_cov_func(L_x)
obj += self.__lf_func(var['mean', Nt], covar_x_t, P_s.reshape((Ny, Ny))) \
+ np.full((1, num_slack),lam) @ var['eps', Nt]
if lam_state is not None:
obj += np.full(
(1, num_state_slack), lam_state) @ var['eps_state', Nt]
num_eq_con = ca.vertcat(*con_eq).size1()
num_ineq_con = ca.vertcat(*con_ineq).size1()
con_eq_lb = np.zeros((num_eq_con, ))
con_eq_ub = np.zeros((num_eq_con, ))
""" Terminal contraint """
if terminal_constraint is not None and not feedback:
con_ineq.append(var['mean', Nt] - mean_ref_s)
num_ineq_con += Ny
con_ineq_lb.append(np.full((Ny, ), -terminal_constraint))
con_ineq_ub.append(np.full((Ny, ), terminal_constraint))
elif terminal_constraint is not None and feedback:
con_ineq.append(
self.__lf_func(var['mean', Nt], covar_x_t, P_s.reshape(
(Ny, Ny))))
num_ineq_con += 1
con_ineq_lb.append(0)
con_ineq_ub.append(terminal_constraint)
con = ca.vertcat(*con_eq, *con_ineq)
self.__conlb = ca.vertcat(con_eq_lb, *con_ineq_lb)
self.__conub = ca.vertcat(con_eq_ub, *con_ineq_ub)
""" Build solver object """
nlp = dict(x=var, f=obj, g=con, p=param_s)
options = {
'ipopt.print_level': 0,
'ipopt.mu_init': 0.01,
'ipopt.tol': 1e-8,
'ipopt.warm_start_init_point': 'yes',
'ipopt.warm_start_bound_push': 1e-9,
'ipopt.warm_start_bound_frac': 1e-9,
'ipopt.warm_start_slack_bound_frac': 1e-9,
'ipopt.warm_start_slack_bound_push': 1e-9,
'ipopt.warm_start_mult_bound_push': 1e-9,
'ipopt.mu_strategy': 'adaptive',
'print_time': False,
'verbose': False,
'expand': True
}
if solver_opts is not None:
options.update(solver_opts)
self.__solver = ca.nlpsol('mpc_solver', 'ipopt', nlp, options)
# First prediction used in the NLP, used in plot later
self.__var_prediction = np.zeros((Nt + 1, Ny))
self.__mean_prediction = np.zeros((Nt + 1, Ny))
self.__mean = None
build_solver_time += time.time()
print('\n________________________________________')
print('# Time to build mpc solver: %f sec' % build_solver_time)
print('# Number of variables: %d' % self.__num_var)
print('# Number of equality constraints: %d' % num_eq_con)
print('# Number of inequality constraints: %d' % num_ineq_con)
print('----------------------------------------')
def _construct_upd_z_nlp(self):
# construct variables
self._var_struct_updz = struct([entry('z_i', struct=self.q_i_struct),
entry('z_ij', struct=self.q_ij_struct)])
var = struct_symMX(self._var_struct_updz)
z_i = self.q_i_struct(var['z_i'])
z_ij = self.q_ij_struct(var['z_ij'])
# construct parameters
self._par_struct_updz = struct([entry('x_i', struct=self.q_i_struct),
entry('x_j', struct=self.q_ij_struct),
entry('l_i', struct=self.q_i_struct),
entry('l_ij', struct=self.q_ij_struct),
entry('t'), entry('T'), entry('rho'),
entry('par', struct=self.par_struct)])
par = struct_symMX(self._par_struct_updz)
x_i, x_j = self.q_i_struct(par['x_i']), self.q_ij_struct(par['x_j'])
l_i, l_ij = self.q_i_struct(par['l_i']), self.q_ij_struct(par['l_ij'])
t, T, rho = par['t'], par['T'], par['rho']
t0 = t/T
# transform spline variables: only consider future piece of spline
tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0)
self._transform_spline([x_i, z_i, l_i], tf, self.q_i)
self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij)
# construct constraints
constraints, lb, ub = [], [], []
for con in self.constraints:
c = con[0]
for sym in symvar(c):
for label, child in self.group.items():
if sym.getName() in child.symbol_dict:
name = child.symbol_dict[sym.getName()][1]
v = z_i[label, name]
ind = self.q_i[child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for nghb in self.q_ij.keys():
for label, child in nghb.group.items():
if sym.getName() in child.symbol_dict:
name = child.symbol_dict[sym.getName()][1]
v = z_ij[nghb.label, label, name]
ind = self.q_ij[nghb][child][name]
sym2 = MX.zeros(sym.size())
sym2[ind] = v
sym2 = reshape(sym2, sym.shape)
c = substitute(c, sym, sym2)
break
for name, s in self.par_i.items():
if s.getName() == sym.getName():
c = substitute(c, sym, par['par', name])
constraints.append(c)
lb.append(con[1])
ub.append(con[2])
self.lb_updz, self.ub_updz = lb, ub
# construct objective
obj = 0.
for child, q_i in self.q_i.items():
for name in q_i.keys():
x = x_i[child.label, name]
z = z_i[child.label, name]
l = l_i[child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
for nghb in self.q_ij.keys():
for child, q_ij in self.q_ij[nghb].items():
for name in q_ij.keys():
x = x_j[str(nghb), child.label, name]
z = z_ij[str(nghb), child.label, name]
l = l_ij[str(nghb), child.label, name]
obj += mul(l.T, x-z) + 0.5*rho*mul((x-z).T, (x-z))
# construct problem
prob, compile_time = self.father.create_nlp(var, par, obj,
constraints, self.options)
self.problem_upd_z = prob
def setup_nlp_v(nlp_numerics_options, model, formulation, Collocation):
# extract necessary inputs
variables_dict = model.variables_dict
nk = nlp_numerics_options['n_k']
# check if phase fix and adjust theta accordingly
if nlp_numerics_options['phase_fix']:
theta = get_phase_fix_theta(variables_dict)
else:
theta = variables_dict['theta']
# define interval struct entries for controls and states
entry_tuple = (
cas.entry('xd', repeat=[nk + 1], struct=variables_dict['xd']),
cas.entry('u', repeat=[nk], struct=variables_dict['u']),
)
# add additional variables according to provided options
if nlp_numerics_options['discretization'] == 'direct_collocation':
# add algebraic variables at interval except for radau case
if nlp_numerics_options['collocation']['scheme'] != 'radau':
if nlp_numerics_options['lift_xddot']:
entry_tuple += (
cas.entry(
'xddot', repeat=[nk], struct=variables_dict['xddot']
), # depends on implementation (e.g. not present for radau collocation)
)
if nlp_numerics_options['lift_xa']:
entry_tuple += (
cas.entry('xa', repeat=[nk], struct=variables_dict['xa']),
) # depends on implementation (e.g. not present for radau collocation)
if 'xl' in list(variables_dict.keys()):
entry_tuple += (
cas.entry('xl',
repeat=[nk],
struct=variables_dict['xl']),
) # depends on implementation (e.g. not present for radau collocation)
# add collocation node variables
d = nlp_numerics_options['collocation'][
'd'] # interpolating polynomial order
coll_var = Collocation.get_collocation_variables_struct(variables_dict)
entry_tuple += (cas.entry('coll_var', struct=coll_var, repeat=[nk,
d]), )
elif nlp_numerics_options['discretization'] == 'multiple_shooting':
# add slack variables for inequalities
if nlp_numerics_options[
'slack_constraints'] == True and model.constraints_dict[
'inequality']:
entry_tuple += (cas.entry(
'us', repeat=[nk],
struct=model.constraints_dict['inequality']), )
# multiple shooting: add algebraic variables at interval if lifted
if nlp_numerics_options['lift_xddot']:
entry_tuple += (
cas.entry(
'xddot', repeat=[nk], struct=variables_dict['xddot']
), # depends on implementation (e.g. not present for radau collocation)
)
if nlp_numerics_options['lift_xa']:
entry_tuple += (
cas.entry('xa', repeat=[nk], struct=variables_dict['xa']),
) # depends on implementation (e.g. not present for radau collocation)
if 'xl' in list(variables_dict.keys()):
entry_tuple += (
cas.entry('xl', repeat=[nk], struct=variables_dict['xl']),
) # depends on implementation (e.g. not present for radau collocation)
# add global entries
entry_list = [entry_tuple]
entry_list += [
cas.entry('theta', struct=theta),
cas.entry('phi', struct=model.parameters_dict['phi']),
cas.entry('xi', struct=formulation.xi_dict['xi'])
]
# generate structure
V = cas.struct_symMX(entry_list)
return V
def run(self, x0, u_sim, theta_sim, phi_sim):
# check consistency of initial conditions:
# to do: check values of g, gdot...
consistency = True
if consistency == False:
raise ValueError('provided initial conditions are not consistent!')
else:
# horizon length
N = len(u_sim)
# V_sim / simulation output structure
V_sim = cas.struct_symMX([
(
cas.entry('xd', repeat=[N, self.__N_sim], struct=self.__variables_dict['xd']),
cas.entry('xa', repeat=[N, self.__N_sim-1], struct=self.__variables_dict['xa']),
cas.entry('xl', repeat=[N, self.__N_sim-1], struct=self.__variables_dict['xl']),
cas.entry('u', repeat=[N], struct=self.__variables_dict['u']),
),
cas.entry('theta', struct=self.__variables_dict['theta']),
cas.entry('phi', struct=self.__phi)
])
# initialize solution vector
V0 = V_sim(0.)
# fill in controls and parameters
for i in range(N):
for name in list(self.__variables_dict['u'].keys()):
V0['u',i,name] = self.__variables_dict['u'](u_sim[i])[name]
V0['theta'] = theta_sim
# adjust time-scaling factor for integrator step size
V0['theta','t_f'] = V0['theta','t_f'] / N
V0['phi'] = phi_sim
# integrate / fill in states and alg vars
# initial state
x_sim = self.__x(x0)['xd']
z_sim = self.__z(0.0)
for i in range(N):
# dae parameters for this time step
p_sim = self.__p(0.)
p_sim['u'] = u_sim[i]
p_sim['theta'] = V0['theta']
p_sim['phi'] = V0['phi']
# state on initial time of shooting node
for name in list(self.__variables_dict['xd'].keys()):
V0['xd',i,0,name] = self.__variables_dict['xd'](x_sim)[name]
# integrate up to (including) the final time of the shooting node
for j in range(self.__N_sim-1):
print(j)
### TESTING
# [ode_test, alg_test] = self.__f(x_sim,0.,p_sim.cat)
# z_test = self.__rootfinder(0.,x_sim,p_sim.cat)
# xddot_test = self.__variables_dict['xddot'](self.__z(z_test)['xddot'])
# perform integration
res = self.__integrator(x0= x_sim, p = p_sim.cat, z0 = z_sim.cat)
# set new initial guess for algebraic variable
z_sim = self.__z(res['zf'])
# set algebraic variables
for name in list(self.__variables_dict['xa'].keys()):
V0['xa',i,j,name] = self.__variables_dict['xa'](z_sim['xa'])[name]
# set lifted variables
for name in list(self.__variables_dict['xl'].keys()):
V0['xl',i,j,name] = self.__variables_dict['xl'](z_sim['xl'])[name]
# set-up next state
x_sim = self.__x(res['xf'])['xd']
# set differential states
for name in list(self.__variables_dict['xd'].keys()):
V0['xd',i,j+1,name] = self.__variables_dict['xd'](x_sim)[name]
self.__status = 'I am a simulation.'
self.__V0 = V0
print('Simulation solved.')
return None
def __construct_solver(self):
""" Construct periodic NLP and solver.
"""
# system variables and dimensions
x = self.__vars['x']
u = self.__vars['u']
variables_entry = (ct.entry('x', shape=(self.__nx, ), repeat=self.__N),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
variables_entry += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
# nlp variables + bounds
w = ct.struct_symMX([variables_entry])
self.__lbw = w(-np.inf)
self.__ubw = w(np.inf)
# prepare dynamics and path constraints entry
constraints_entry = (ct.entry('dyn',
shape=(self.__nx, ),
repeat=self.__N), )
if self.__h is not None:
constraints_entry += (ct.entry('h',
shape=self.__h.size1_out(0),
repeat=self.__N), )
if self.__gnl is not None:
constraints_entry += (ct.entry('g',
shape=self.__gnl.size1_out(0),
repeat=self.__N), )
# create general constraints structure
g_struct = ct.struct_symMX([
constraints_entry,
])
# create symbolic constraint expressions
map_args = collections.OrderedDict()
map_args['x0'] = ct.horzcat(*w['x'])
map_args['p'] = ct.horzcat(*w['u'])
# evaluate function dynamics
F_constr = ct.horzsplit(
self.__F.map(self.__N, self.__parallelization)(**map_args)['xf'])
# generate constraints
constr = collections.OrderedDict()
constr['dyn'] = [
a - b for a, b in zip(F_constr, w['x', 1:] + [w['x', 0]])
]
if 'us' in self.__vars:
map_args['us'] = ct.horzcat(*w['us'])
if self.__h is not None:
constr['h'] = ct.horzsplit(
self.__h.map(self.__N,
self.__parallelization)(*map_args.values()))
if self.__gnl is not None:
constr['g'] = ct.horzsplit(
self.__gnl.map(self.__N,
self.__parallelization)(*map_args.values()))
# interleaving of constraints
repeated_constr = list(
itertools.chain.from_iterable(zip(*constr.values())))
# fill in constraint structure
self.__g = g_struct(ca.vertcat(*repeated_constr))
# constraint bounds
self.__lbg = g_struct(np.zeros(self.__g.shape))
self.__ubg = g_struct(np.zeros(self.__g.shape))
if self.__h is not None:
self.__ubg['h', :] = np.inf
# nlp cost
cost_map_fun = self.__cost.map(self.__N, self.__parallelization)
f = ca.sum2(cost_map_fun(map_args['x0'], map_args['p']))
# add phase fixing cost
self.__construct_phase_fixing_cost()
alpha = ca.MX.sym('alpha')
x0star = ca.MX.sym('x0star', self.__nx, 1)
f += self.__phase_fix_fun(alpha, x0star, w['x', 0])
# add slack regularization
# if 'us' in self.__vars:
# f += self.__reg_slack*ct.mtimes(ct.vertcat(*w['us']).T,ct.vertcat(*w['us']))
# NLP parameters
p = ca.vertcat(alpha, x0star)
self.__w = w
self.__g_fun = ca.Function('g_fun', [w, p], [self.__g])
# create IP-solver
prob = {'f': f, 'g': self.__g, 'x': w, 'p': p}
opts = {'ipopt': {'linear_solver': 'ma57'}, 'expand': False}
if Logger.logger.getEffectiveLevel() > 10:
opts['ipopt']['print_level'] = 0
opts['print_time'] = 0
opts['ipopt']['sb'] = 'yes'
self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts)
# create SQP-solver
prob['lbg'] = self.__lbg
prob['ubg'] = self.__ubg
self.__sqp_solver = sqp_method.Sqp(prob)
return None
def __construct_solver(self):
""" Construct periodic MPC solver
"""
# system variables and dimensions
x = self.__vars['x']
u = self.__vars['u']
# NLP parameters
if self.__type == 'economic':
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, 1)),
ct.entry('xN', shape=(self.__nx, 1))
])
# reassign for brevity
x0 = self.__p['x0']
xN = self.__p['xN']
if self.__type == 'tracking':
ref_vars = (ct.entry('x', shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
ref_vars += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
# reference trajectory
wref = ct.struct_symMX([ref_vars])
nw = self.__nx + self.__nu + self.__ns
tuning = ct.struct_symMX([ # tracking tuning
ct.entry('H', shape=(nw, nw), repeat=self.__N),
ct.entry('q', shape=(nw, 1), repeat=self.__N)
])
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape=(self.__nx, )),
ct.entry('wref', struct=wref),
ct.entry('tuning', struct=tuning)
])
# reassign for brevity
x0 = self.__p['x0']
wref = self.__p.prefix['wref']
tuning = self.__p.prefix['tuning']
xN = wref['x', -1]
# NLP variables
variables_entry = (ct.entry('x',
shape=(self.__nx, ),
repeat=self.__N + 1),
ct.entry('u', shape=(self.__nu, ), repeat=self.__N))
if 'us' in self.__vars:
variables_entry += (ct.entry('us',
shape=(self.__ns, ),
repeat=self.__N), )
self.__wref = ct.struct_symMX([variables_entry
]) # structure of reference
if 'usc' in self.__vars:
variables_entry += (ct.entry('usc',
shape=(self.__nsc, ),
repeat=self.__N), )
# nlp variables + bounds
w = ct.struct_symMX([variables_entry])
# variable bounds are implemented as inequalities
self.__lbw = w(-np.inf)
self.__ubw = w(np.inf)
# prepare dynamics and path constraints entry
constraints_entry = (ct.entry('dyn',
shape=(self.__nx, ),
repeat=self.__N), )
if self.__gnl is not None:
constraints_entry += (ct.entry('g',
shape=self.__gnl.size1_out(0),
repeat=self.__N), )
if self.__h is not None:
constraints_entry += (ct.entry('h',
shape=self.__h.size1_out(0),
repeat=self.__N), )
# terminal constraint
self.__nx_term = self.__p_operator.size1_out(0)
# create general constraints structure
g_struct = ct.struct_symMX([
ct.entry('init', shape=(self.__nx, 1)), constraints_entry,
ct.entry('term', shape=(self.__nx_term, 1))
])
# create symbolic constraint expressions
map_args = collections.OrderedDict()
map_args['x0'] = ct.horzcat(*w['x', :-1])
map_args['p'] = ct.horzcat(*w['u'])
F_constr = ct.horzsplit(self.__F.map(self.__N)(**map_args)['xf'])
# generate constraints
constr = collections.OrderedDict()
constr['dyn'] = [a - b for a, b in zip(F_constr, w['x', 1:])]
if 'us' in self.__vars:
map_args['us'] = ct.horzcat(*w['us'])
if self.__gnl is not None:
constr['g'] = ct.horzsplit(
self.__gnl.map(self.__N)(*map_args.values()))
if 'usc' in self.__vars:
map_args['usc'] = ct.horzcat(*w['usc'])
if self.__h is not None:
constr['h'] = ct.horzsplit(
self.__h.map(self.__N)(*map_args.values()))
repeated_constr = list(
itertools.chain.from_iterable(zip(*constr.values())))
term_constraint = self.__p_operator(w['x', -1] - xN)
self.__g = g_struct(
ca.vertcat(w['x', 0] - x0, *repeated_constr, term_constraint))
self.__lbg = g_struct(np.zeros(self.__g.shape))
self.__ubg = g_struct(np.zeros(self.__g.shape))
if self.__h is not None:
self.__ubg['h', :] = np.inf
for i in self.__h_us_idx + self.__h_x_idx: # rm constraints the only depend on x at k = 0
self.__lbg['h', 0, i] = -np.inf
# nlp cost
cost_map = self.__cost.map(self.__N)
if self.__type == 'economic':
cost_args = [ct.horzcat(*w['x', :-1]), ct.horzcat(*w['u'])]
elif self.__type == 'tracking':
if self.__ns != 0:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k], w['us', k])
for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k], wref['us', k])
for k in range(self.__N)
])
else:
cost_args_w = ct.horzcat(*[
ct.vertcat(w['x', k], w['u', k]) for k in range(self.__N)
])
cost_args_w_ref = ct.horzcat(*[
ct.vertcat(wref['x', k], wref['u', k])
for k in range(self.__N)
])
cost_args = [
cost_args_w, cost_args_w_ref,
ct.horzcat(*tuning['H']),
ct.horzcat(*tuning['q'])
]
if self.__options['hessian_approximation'] == 'gauss_newton':
if 'usc' not in self.__vars:
hess_gn = ct.diagcat(*tuning['H'],
ca.DM.zeros(self.__nx, self.__nx))
else:
hess_block = list(
itertools.chain.from_iterable(
zip(tuning['H'],
[ca.DM.zeros(self.__nsc, self.__nsc)] *
self.__N)))
hess_gn = ct.diagcat(*hess_block,
ca.DM.zeros(self.__nx, self.__nx))
J = ca.sum2(cost_map(*cost_args))
# add cost on slacks
if 'usc' in self.__vars:
J += ca.sum2(ct.mtimes(self.__scost.T, ct.horzcat(*w['usc'])))
# create solver
prob = {'f': J, 'g': self.__g, 'x': w, 'p': self.__p}
self.__w = w
self.__g_fun = ca.Function('g_fun', [self.__w, self.__p], [self.__g])
# create IPOPT-solver instance if needed
if self.__options['ipopt_presolve']:
opts = {
'ipopt': {
'linear_solver': 'ma57',
'print_level': 0
},
'expand': False
}
if Logger.logger.getEffectiveLevel() > 10:
opts['ipopt']['print_level'] = 0
opts['print_time'] = 0
opts['ipopt']['sb'] = 'yes'
self.__solver = ca.nlpsol('solver', 'ipopt', prob, opts)
# create hessian approximation function
if self.__options['hessian_approximation'] == 'gauss_newton':
lam_g = ca.MX.sym('lam_g', self.__g.shape) # will not be used
hess_approx = ca.Function('hess_approx',
[self.__w, self.__p, lam_g], [hess_gn])
elif self.__options['hessian_approximation'] == 'exact':
hess_approx = 'exact'
# create sqp solver
prob['lbg'] = self.__lbg
prob['ubg'] = self.__ubg
sqp_opts = {
'hessian_approximation': hess_approx,
'max_iter': self.__options['max_iter']
}
self.__sqp_solver = sqp_method.Sqp(prob, sqp_opts)
def __construct_solver(self):
""" Construct casadi.nlpsol Object based on MPC trial information.
"""
awelogger.logger.info("Constructing MPC solver object...")
if self.__cost_type == 'economic':
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape = (self.__nx,1))
])
if self.__cost_type == 'tracking':
# parameters
self.__p = ct.struct_symMX([
ct.entry('x0', shape = (self.__nx,)),
ct.entry('ref', struct = self.__trial.nlp.V)
])
# create P evaluator for use in NLP arguments
self.__create_P_fun()
# generate mpc constraints
g = self.__trial.nlp.g_fun(self.__trial.nlp.V, self.__P_fun(self.__p))
# generate cost function
f = self.__generate_objective()
# fill in nlp dict
nlp = {'x': self.__trial.nlp.V, 'p': self.__p, 'f': f, 'g': g}
# store nlp bounds
self.__trial.nlp.V_bounds['ub']['phi'] = 0.0
self.__trial.nlp.V_bounds['lb']['xi'] = 0.0
self.__trial.nlp.V_bounds['ub']['xi'] = 0.0
for name in list(self.__trial.model.variables_dict['u'].keys()):
if 'fict' in name:
self.__trial.nlp.V_bounds['lb']['coll_var',:,:,'u',name] = 0.0
self.__trial.nlp.V_bounds['ub']['coll_var',:,:,'u',name] = 0.0
self.__lbw = self.__trial.nlp.V_bounds['lb']
self.__ubw = self.__trial.nlp.V_bounds['ub']
self.__lbg = self.__trial.nlp.g_bounds['lb']
self.__ubg = self.__trial.nlp.g_bounds['ub']
awelogger.logger.level = awelogger.logger.getLogger().getEffectiveLevel()
opts = {}
opts['expand'] = self.__mpc_options['expand']
opts['ipopt.linear_solver'] = self.__mpc_options['linear_solver']
opts['ipopt.max_iter'] = self.__mpc_options['max_iter']
opts['ipopt.max_cpu_time'] = self.__mpc_options['max_cpu_time']
opts['jit'] = self.__mpc_options['jit']
if awelogger.logger.level > 10:
opts['ipopt.print_level'] = 0
opts['print_time'] = 0
self.__solver = ct.nlpsol('solver', 'ipopt', nlp, opts)
return None
