def create_nlp(self, var, par, obj, con, options):
jit = options['codegen']
if options['verbose'] >= 2:
print 'Building nlp ... ',
if jit['jit']:
print('[jit compilation with flags %s]' %
(','.join(jit['jit_options']['flags']))),
t0 = time.time()
nlp = MXFunction('nlp', nlpIn(x=var, p=par), nlpOut(f=obj, g=con))
solver = NlpSolver('solver', 'ipopt', nlp, options['solver'])
grad_f, jac_g = nlp.gradient('x', 'f'), nlp.jacobian('x', 'g')
hess_lag = solver.hessLag()
var, par = struct_symSX(var), struct_symSX(par)
nlp = SXFunction('nlp', [var, par], nlp([var, par]), jit)
grad_f = SXFunction('grad_f', [var, par], grad_f([var, par]), jit)
jac_g = SXFunction('jac_g', [var, par], jac_g([var, par]), jit)
lam_f, lam_g = SX.sym('lam_f'), SX.sym('lam_g', con.size)
hess_lag = SXFunction('hess_lag', [var, par, lam_f, lam_g],
hess_lag([var, par, lam_f, lam_g]), jit)
options['solver'].update({'grad_f': grad_f, 'jac_g': jac_g,
'hess_lag': hess_lag})
problem = NlpSolver('solver', 'ipopt', nlp, options['solver'])
t1 = time.time()
if options['verbose'] >= 2:
print 'in %5f s' % (t1-t0)
return problem, (t1-t0)
def generate_system_parameters(options, architecture):
parameters_dict = {}
# extract parametric options
parametric_options = options['params']
parameters_dict['theta0'] = struct_op.generate_nested_dict_struct(parametric_options)
# optimization parameters
parameters_dict['phi'] = generate_optimization_parameters()
# define vortex induction linearization parameters
use_vortex_linearization = options['aero']['vortex']['use_linearization']
if use_vortex_linearization:
vortex_linearization_list, _ = generate_structure(options, architecture)
vortex_linearization_params, _ = struct_op.generate_variable_struct(vortex_linearization_list)
parameters_dict['lin'] = vortex_linearization_params
# generate nested casadi structure for system parameters
if use_vortex_linearization:
parameters = cas.struct_symSX([
cas.entry('theta0', struct=parameters_dict['theta0']),
cas.entry('phi', struct=parameters_dict['phi']),
cas.entry('lin', struct=parameters_dict['lin'])
])
else:
parameters = cas.struct_symSX([
cas.entry('theta0', struct= parameters_dict['theta0']),
cas.entry('phi', struct= parameters_dict['phi'])
])
return parameters, parameters_dict
def get_collocation_variables_struct(self, variables_dict):
if 'xl' in list(variables_dict.keys()):
coll_var = cas.struct_symSX([
cas.entry('xd', struct=variables_dict['xd']),
cas.entry('xa', struct=variables_dict['xa']),
cas.entry('xl', struct=variables_dict['xl'])
])
else:
coll_var = cas.struct_symSX([
cas.entry('xd', struct=variables_dict['xd']),
cas.entry('xa', struct=variables_dict['xa']),
])
return coll_var
def construct_upd_z(self):
# check if we have linear equality constraints
self._lineq_updz, A, b = self._check_for_lineq()
if not self._lineq_updz:
self._construct_upd_z_nlp()
x_i = struct_symSX(self.q_i_struct)
x_j = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
par = struct_symSX(self.par_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 SX 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])[0]
b = b([par])[0]
# build KKT system
E = rho*SX.eye(A.shape[1])
l, x = vertcat([l_i.cat, l_ij.cat]), vertcat([x_i.cat, x_j.cat])
f = -(l + rho*x)
G = vertcat([horzcat([E, A.T]),
horzcat([A, SX.zeros(A.shape[0], A.shape[0])])])
h = vertcat([-f, b])
z = solve(G, h)
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, compile_time = self.father.create_function(
'upd_z', inp, out, self.options)
self.problem_upd_z = prob
def generate_nested_dict_struct(v):
# empty entry list
entry_list = []
# iterate over all dict values
for k1, v1 in v.items():
if isinstance(v1, dict):
# if value is a dict, recursively generate subdict struct
substruct = generate_nested_dict_struct(v1)
entry_list.append(cas.entry(k1, struct=substruct))
else:
if isinstance(v1, float) or isinstance(v1, int):
shape = (1, 1)
else:
shape = v1.shape
# append value to entry list
entry_list.append(cas.entry(k1, shape=shape))
# make overall structure
subdict_struct = cas.struct_symSX(entry_list)
return subdict_struct
def _defineOptimizationVariablesAndBounds(self):
# definition of constraints on x and u
lb_x = 20 * np.array([-3, -2, -2, -2, -2, -2])
ub_x = 20 * np.array([3, 2, 2, 2, 2, 2])
lb_u = -4 * np.ones((self.nu, 1))
ub_u = 4 * np.ones((self.nu, 1))
opt_x = struct_symSX([
entry('x', shape=self.nx, repeat=[self.N + 1, self.K + 1]),
entry('u', shape=self.nu, repeat=[self.N])
])
# initialize bounds with all zero according to opt_x structure
lb_opt_x = opt_x(0)
ub_opt_x = opt_x(0)
# set bounds
lb_opt_x['x'] = lb_x
ub_opt_x['x'] = ub_x
lb_opt_x['u'] = lb_u
ub_opt_x['u'] = ub_u
return opt_x, lb_opt_x, ub_opt_x
def create_plan_fc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=belief),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
# Regularize controls
J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = BF([V['X',k], V['U',k]])
g.append(x_next - V['X',k+1])
# Penalize uncertainty
J += 1e-1 * ca.trace(V['X',k,'S']) * dt
g = ca.vertcat(g)
# log-probability, doesn't work with full collocation
#Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
#J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
# ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
# S(t=0) = S0
lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def generate_variable_struct(variable_list):
structs = {}
for name in list(variable_list.keys()):
structs[name] = cas.struct_symSX([
cas.entry(variable_list[name][i][0],
shape=variable_list[name][i][1])
for i in range(len(variable_list[name]))
])
variable_struct = cas.struct_symSX([
cas.entry(name, struct=structs[name])
for name in list(variable_list.keys())
])
return variable_struct, structs
def setup_constraint_structure(nlp_numerics_options, model, formulation):
constraints_entry_list = make_constraints_entry_list(nlp_numerics_options, formulation.constraints, model)
# Constraints structure
g_struct = cas.struct_symSX(constraints_entry_list)
return g_struct
def setup_nlp_p_fix(V, model):
# fixed system parameters
p_fix = cas.struct_symSX([(
cas.entry('ref', struct=V), # tracking reference for cost function
cas.entry('weights',
struct=model.variables) # weights for cost function
)])
return p_fix
def create_belief_plan(cls, model, warm_start=False,
x0=0, lam_x0=0, lam_g0=0):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X', repeat=model.n+1, struct=model.x),
cat.entry('U', repeat=model.n, struct=model.u)
)
])
# Box constraints
[lbx, ubx] = cls._create_box_constraints(model, V)
# Non-linear constraints
[g, lbg, ubg] = cls._create_belief_nonlinear_constraints(model, V)
# Objective function
J = cls._create_belief_objective_function(model, V)
# Formulate non-linear problem
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
op = {# Linear solver
#'linear_solver': 'ma57',
# Warm start
# 'warm_start_init_point': 'yes',
# Termination
'max_iter': 1500,
'tol': 1e-6,
'constr_viol_tol': 1e-5,
'compl_inf_tol': 1e-4,
# Acceptable termination
'acceptable_tol': 1e-3,
'acceptable_iter': 5,
'acceptable_obj_change_tol': 1e-2,
# NLP
# 'fixed_variable_treatment': 'make_constraint',
# Quasi-Newton
'hessian_approximation': 'limited-memory',
'limited_memory_max_history': 5,
'limited_memory_max_skipping': 1}
if warm_start:
op['warm_start_init_point'] = 'yes'
op['fixed_variable_treatment'] = 'make_constraint'
# Initialize solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, op)
# Solve
if warm_start:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg,
lam_x0=lam_x0, lam_g0=lam_g0)
else:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
return V(sol['x']), sol['lam_x'], sol['lam_g']
def get_structure(self, cstr_type):
cstr_list = self.get_list(cstr_type)
entry_list = []
for cstr in cstr_list:
joined_name = cstr.name
local = cas.entry(joined_name, shape=cstr.expr.shape)
entry_list.append(local)
return cas.struct_symSX(entry_list)
def make_stage_constraint_struct(model):
# make entry list to check if not empty
entry_list = [cas.entry('collocation', shape =model.dynamics(model.variables, model.parameters).size())]
if list(model.constraints.keys()): # check if not empty
entry_list.append(cas.entry('path_constraints', struct = model.constraints))
# stage constraints structure -- necessary for interleaving
stage_constraints = cas.struct_symSX(entry_list)
return stage_constraints
def construct_upd_l(self):
# create parameters
x_i = struct_symSX(self.q_i_struct)
z_i = struct_symSX(self.q_i_struct)
z_ij = struct_symSX(self.q_ij_struct)
l_i = struct_symSX(self.q_i_struct)
l_ij = struct_symSX(self.q_ij_struct)
x_j = struct_symSX(self.q_ij_struct)
t = SX.sym('t')
T = SX.sym('T')
rho = SX.sym('rho')
t0 = t/T
inp = [x_i, z_i, z_ij, l_i, l_ij, x_j, t, T, rho]
# put symbols in SX structs (necessary for transformation)
x_i = self.q_i_struct(x_i)
z_i = self.q_i_struct(z_i)
z_ij = self.q_ij_struct(z_ij)
l_i = self.q_i_struct(l_i)
l_ij = self.q_ij_struct(l_ij)
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, l_i], tf, self.q_i)
self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij)
# update lambda
l_i_new = self.q_i_struct(l_i.cat + rho*(x_i.cat - z_i.cat))
l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat))
tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0)
self._transform_spline(l_i_new, tf, self.q_i)
self._transform_spline(l_ij_new, tf, self.q_ij)
out = [l_i_new, l_ij_new]
# create problem
prob, compile_time = self.father.create_function(
'upd_l', inp, out, self.options)
self.problem_upd_l = prob
def get_phase_fix_theta(variables_dict):
entry_list = []
for name in list(variables_dict['theta'].keys()):
if name == 't_f':
entry_list.append(cas.entry('t_f', shape=(2, 1)))
else:
entry_list.append(
cas.entry(name, shape=variables_dict['theta'][name].shape))
theta = cas.struct_symSX(entry_list)
return theta
def make_entry_list(eqs_dict, ineqs_dict):
# make entry list for all non-empty dicts
entry_list = []
if eqs_dict: # check if not empty
# equality constraint struct
eq_struct = cas.struct_symSX([
cas.entry(name, shape=eqs_dict[name].size())
for name in list(eqs_dict.keys())
])
entry_list.append(cas.entry('equality', struct=eq_struct))
if ineqs_dict: # check if not empty
# inequality constraint struct
ineq_struct = cas.struct_symSX([
cas.entry(name, shape=ineqs_dict[name].size())
for name in list(ineqs_dict.keys())
])
entry_list.append(cas.entry('inequality', struct=ineq_struct))
return entry_list
def setup_nlp_cost():
cost = cas.struct_symSX([
(cas.entry('tracking'), cas.entry('u_regularisation'),
cas.entry('ddq_regularisation'),
cas.entry('gamma'), cas.entry('iota'), cas.entry('psi'),
cas.entry('tau'), cas.entry('eta'), cas.entry('nu'),
cas.entry('upsilon'), cas.entry('fictitious'), cas.entry('power'),
cas.entry('t_f'), cas.entry('theta_regularisation'),
cas.entry('nominal_landing'), cas.entry('compromised_battery'),
cas.entry('transition'), cas.entry('slack'))
])
return cost
def create_simple_plan(x0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Objective function
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
J = 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN
# Regularize controls
J += 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and non-linear control constraints
g = []
for k in range(N_sim):
# Multiple shooting
[x_next] = F([V['X',k], V['U',k], dt])
g.append(x_next - V['X',k+1])
g = ca.vertcat(g)
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
opts = {}
#opts['hessian_approximation'] = 'limited-memory'
opts['linear_solver'] = 'ma57'
# Create a solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v <= v_max
lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = x0
# Solve the NLP
sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
return V(sol['x'])
def __get_sstruct(dictionary):
"""Convert a dictionary to a casadi struct with the same structure and entries for 0th, 1st and 2nd derivatives
:type dictionary: dict
:param dictionary: variables to be put into the struct
:rtype: casadi.struct_symSX
"""
struct_list = []
for key in list(dictionary.keys()):
if key[0] != 'd':
struct_list += [(key, dictionary[key].shape)]
sub_struct = ct.struct_symSX([
ct.entry(struct_list[i][0], shape=struct_list[i][1])
for i in range(len(struct_list))
])
dsub_struct = ct.struct_symSX([
ct.entry('d' + struct_list[i][0], shape=struct_list[i][1])
for i in range(len(struct_list))
])
ddsub_struct = ct.struct_symSX([
ct.entry('dd' + struct_list[i][0], shape=struct_list[i][1])
for i in range(len(struct_list))
])
sstruct = ct.struct_symSX([
ct.entry('var', struct=sub_struct),
ct.entry('dvar', struct=dsub_struct),
ct.entry('ddvar', struct=ddsub_struct)
])
return sstruct
def generate_system_parameters(options):
# extract parametric options
parametric_options = options['params']
parameters_dict = {}
# generate nested casadi structure for system parameters
parameters_dict['theta0'] = struct_op.generate_nested_dict_struct(parametric_options)
parameters_dict['phi'] = generate_optimization_parameters()
parameters = cas.struct_symSX([
cas.entry('theta0', struct= parameters_dict['theta0']),
cas.entry('phi', struct= parameters_dict['phi'])]
)
return parameters, parameters_dict
def generate_optimization_parameters():
# variable system parameters
p_dec = cas.struct_symSX([(
cas.entry('gamma'), # force homotopy variable
cas.entry('tau'), # tether drag homotopy variable
cas.entry('iota'), # induction homotopy variable
cas.entry('psi'), # power homotopy variable
cas.entry('eta'), # nominal landing homotopy variable
cas.entry('nu'), # compromised landing homotopy variable
cas.entry('upsilon'), # transition homotopy variable
)])
optimization_parameters = p_dec
return optimization_parameters
def test_affine_constraint():
var = struct_symSX([entry('x', shape=(2, 1))])
linear_coefficient = np.eye(2) * 2
affine_coefficient = np.ones((2, ))
lower_bound = np.zeros((2, 1))
upper_bound = np.ones((2, 1))
constr = AffineConstraint.gen(var, linear_coefficient, affine_coefficient,
lower_bound, upper_bound)
g, lb, ub = constr[0]
assert (lb == (lower_bound - affine_coefficient)).all()
assert (ub == (upper_bound - affine_coefficient)).all()
assert '@1=2, [(@1*x_0), (@1*x_1)]' == str(g)
assert np.equal(Function('g_fun', [var['x']], [g])([2, 2]), [4, 4]).all()
def gen(NX, NU, M):
"""gen Generates a casadi struct containing the symbolic optimization parameters
x_cur (the initial state), x_ref (the reference states) and u_cur (the reference controls).
x_cur is a <NX>x<1> vector, x_ref is a <NX>x<M+1> matrix, u_ref is a <NU>x<M> matrix
:param NX: Number of reference state variables
:param NU: Number of reference control variables
:param M: Prediction horizon length
:returns: A casadi struct_symSX object
"""
params = struct_symSX([
entry('x_cur', shape=NX),
entry('x_ref', shape=(NX, M + 1)),
entry('u_ref', shape=(NU, M))
])
return params
def create_plan(cls, model, warm_start=False,
x0=0, lam_x0=0, lam_g0=0):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X', repeat=model.n+1, struct=model.x),
cat.entry('U', repeat=model.n, struct=model.u)
)
])
# Box constraints
[lbx, ubx] = cls._create_box_constraints(model, V)
# Force the catcher to always look forward
# lbx['U', :, 'theta'] = ubx['U', :, 'theta'] = 0
# Non-linear constraints
[g, lbg, ubg] = cls._create_nonlinear_constraints(model, V)
# Objective function
J = cls._create_objective_function(model, V, warm_start)
# Formulate non-linear problem
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J, g=g))
op = {# Linear solver
#'linear_solver': 'ma57',
# Acceptable termination
'acceptable_iter': 5}
if warm_start:
op['warm_start_init_point'] = 'yes'
op['fixed_variable_treatment'] = 'make_constraint'
# Initialize solver
solver = ca.NlpSolver('solver', 'ipopt', nlp, op)
# Solve
if warm_start:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg,
lam_x0=lam_x0, lam_g0=lam_g0)
else:
sol = solver(x0=x0, lbx=lbx, ubx=ubx, lbg=lbg, ubg=ubg)
return V(sol['x']), sol['lam_x'], sol['lam_g']
def setup_output_structure(nlp_numerics_options, model_outputs, form_outputs):
# create outputs
# n_o_t_e: !!! outputs are defined at nodes where both state and algebraic variables
# are defined. In the radau case, algebraic variables are not defined on the interval
# nodes, note however that algebraic variables might be discontinuous so the same
# holds for the outputs!!!
nk = nlp_numerics_options['n_k']
entry_tuple = ()
if nlp_numerics_options['discretization'] == 'direct_collocation':
# extract collocation parameters
d = nlp_numerics_options['collocation']['d']
scheme = nlp_numerics_options['collocation']['scheme']
if scheme != 'radau':
# define outputs on interval and collocation nodes
entry_tuple += (
cas.entry('outputs', repeat=[nk], struct=model_outputs),
cas.entry('coll_outputs', repeat=[nk, d],
struct=model_outputs),
)
else:
# define outputs on collocation nodes
entry_tuple += (cas.entry('coll_outputs',
repeat=[nk, d],
struct=model_outputs), )
elif nlp_numerics_options['discretization'] == 'multiple_shooting':
# define outputs on interval nodes
entry_tuple += (cas.entry('outputs', repeat=[nk],
struct=model_outputs), )
Outputs = cas.struct_symSX([entry_tuple] +
[cas.entry('final', struct=form_outputs)])
return Outputs
def get_costs_struct(V):
costs_struct = cas.struct_symSX([
cas.entry("tracking_cost"),
cas.entry("ddq_regularisation_cost"),
cas.entry("u_regularisation_cost"),
cas.entry("fictitious_cost"),
cas.entry("theta_regularisation_cost"),
cas.entry("slack_cost")
] + [cas.entry(name + '_cost') for name in struct_op.subkeys(V, 'phi')] + [
cas.entry("time_cost"),
cas.entry("power_cost"),
cas.entry("nominal_landing_cost"),
cas.entry("transition_cost"),
cas.entry("compromised_battery_cost"),
cas.entry("tracking_problem_cost"),
cas.entry("power_problem_cost"),
cas.entry("general_problem_cost"),
cas.entry("objective")
])
return costs_struct
def gen(NX, NU, M, stateBounds=None, controlBounds=None):
"""gen Generates a casadi struct containing the symbolic optimization variables required
for direct multiple shooting. x is a <NX>x<M+1> matrix, u is a <NU>x<M> matrix.
:param NX: Number of state variables
:param NU: Number of control variables
:param M: Prediction horizon length
:returns: A casadi struct_symSX object
"""
# decision (free) variables
variables = struct_symSX(
[entry('x', shape=(NX, M + 1)),
entry('u', shape=(NU, M))])
# symbolic bounds
bx = ca.SX.sym('bx', NX)
bu = ca.SX.sym('bu', NU)
# bounds struct, must be identical to variables struct in dimensions and keys
bounds = struct_SX([
entry('x', expr=ca.repmat(bx, 1, M + 1)),
entry('u', expr=ca.repmat(bu, 1, M))
])
boundsFun = ca.Function('varBoundsFun', [bx, bu], [bounds.cat])
if stateBounds is None:
stateBounds = np.multiply(np.ones((2, NX)),
np.array((-np.inf, np.inf), ndmin=2).T)
if controlBounds is None:
controlBounds = np.multiply(np.ones((2, NU)),
np.array((-np.inf, np.inf), ndmin=2).T)
lbw = boundsFun(stateBounds[0, :], controlBounds[0, :])
ubw = boundsFun(stateBounds[1, :], controlBounds[1, :])
return variables, lbw, ubw
def create_simple_plan(x0, F, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final position
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
def shift_trajectory(state, u):
##########################################################
## state and u in form (state, N)
state_ = np.concatenate((state.T[1:], state.T[-1:]))
u_ = np.concatenate((u.T[1:], u.T[-1:]))
return u_, state_
if __name__ == '__main__':
T = 0.2 # sampling time [s]
N = 100 # prediction horizon
rob_diam = 0.3 # [m]
v_max = 0.6
omega_max = np.pi / 4.0
states = ca_tools.struct_symSX([(ca_tools.entry('x'), ca_tools.entry('y'),
ca_tools.entry('theta'))])
x, y, theta = states[...]
n_states = states.size
controls = ca_tools.struct_symSX([(ca_tools.entry('v'),
ca_tools.entry('omega'))])
v, omega = controls[...]
n_controls = controls.size
## rhs
rhs = ca_tools.struct_SX(states)
rhs['x'] = v * ca.cos(theta)
rhs['y'] = v * ca.sin(theta)
rhs['theta'] = omega
## function
#xlb = np.array([0.5, 0.8, 0])
#xub = np.array([2.5, 2.0, 400])
#<<ENDCHUNK>>
# Make optimizers.
x0 = np.array([.05 * cs, .75 * Ts, .5 * hs])
u0 = np.array([Tcs, Fs])
umax = np.array([.05 * Tcs, .15 * Fs])
dumax = .2 * umax
xlb = np.array([-np.inf, -np.inf, -np.inf])
xub = np.array([np.inf, np.inf, np.inf])
# Create variables struct.
var = ctools.struct_symSX([(
ctools.entry("x", shape=(Nx, ), repeat=Nt + 1),
ctools.entry("u", shape=(Nu, ), repeat=Nt),
ctools.entry("Du", shape=(Nu, ), repeat=Nt),
)])
varlb = var(-np.inf)
varub = var(np.inf)
varguess = var(0)
# Create parameters struct.
par = ctools.struct_symSX([
ctools.entry("x_sp", shape=(Nx, ), repeat=Nt + 1),
ctools.entry("u_sp", shape=(Nu, ), repeat=Nt),
ctools.entry("uprev", shape=(Nu, 1)),
ctools.entry("Ad", repeat=Nt, shape=(Nx, Nx)),
ctools.entry("Bd", repeat=Nt, shape=(Nx, Nu)),
ctools.entry("fd", repeat=Nt, shape=(Nx, 1))
])
from casadi import *
from casadi.tools import struct_symSX, struct_SX, entry
a = struct_symSX([
entry('a', shape=(3, 1)),
entry('b', shape=(3, 3)),
entry('c', shape=(3, 1))
])
b = struct_symSX([entry('a_struct', struct=a)])
t = t0 + T
u_end = ca.horzcat(u[:, 1:], u[:, -1])
return t, st, u_end
if __name__ == '__main__':
T = 0.2 # sampling time [s]
N = 100 # prediction horizon
rob_diam = 0.3 # [m]
v_max = 0.6
omega_max = np.pi/4.0
states = ca_tools.struct_symSX([
(
ca_tools.entry('x'),
ca_tools.entry('y'),
ca_tools.entry('theta')
)
])
x, y, theta = states[...]
n_states = states.size
controls = ca_tools.struct_symSX([
(
ca_tools.entry('v'),
ca_tools.entry('omega')
)
])
v, omega = controls[...]
n_controls = controls.size
def create_plan_pc(b0, BF, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final coordinate
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
r_unit = r / (ca.norm_2(r) + 1e-2)
d_gaze = d - r_unit
# Regularize controls
J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
bk = cat.struct_SX(belief)
bk['S'] = b0['S']
g, lbg, ubg = [], [], []
for k in range(N_sim):
# Belief propagation
bk['m'] = V['X',k]
[bk_next] = BF([ bk, V['U',k] ])
bk_next = belief(bk_next) # simplify indexing
# Multiple shooting
g.append(bk_next['m'] - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Control constraints
g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi']))
lbg.append(-ca.inf)
ubg.append(ca.DMatrix([0]))
# Advance time
bk = bk_next
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Simple cost
J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate
J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity
#J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity
J += 1e1 * ca.trace(bk_next['S']) # uncertainty
# log-probability cost
#Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
#J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# State constraints
# catcher can look within the upper hemisphere
lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max
# Control constraints
# 0 <= F
lbx['U',:,'F'] = 0
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# -pi <= psi <= pi
lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = b0['m']
# Take care of the time
#lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf
# Simulation ends when the ball touches the ground
#lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
N_ctrl = 2 # mpc replan horizon
N_delay = 3 # delay in perception
# Model
nx, nu, nz = 7, 2, 5
v_max = 10 # max speed
w_max = 1 # max rotation speed
a, b = 7.5, 2.5 # max speed restriction constants
# %% =========================================================================
# Variables
# ============================================================================
# State
state = cat.struct_symSX(['x_b','y_b','vx_b','vy_b',
'x_c','y_c','phi'])
# Control
control = cat.struct_symSX(['v','w'])
# Observation
observation = cat.struct_SX([
cat.entry('x_b', expr = state['x_b']),
cat.entry('y_b', expr = state['y_b']),
cat.entry('x_c', expr = state['x_c']),
cat.entry('y_c', expr = state['y_c']),
cat.entry('phi', expr = state['phi'])
])
# Belief state (mu, Sigma)
belief = cat.struct_symSX([
N_ctrl = 2 # mpc replan horizon
N_delay = 3 # delay in perception
# Model
nx, nu, nz = 9, 3, 6
w_max = 1 # max rotation speed
a, b = 4, 2 # max force restriction constants
g0 = 9.81 # gravity
# %% =========================================================================
# Variables
# ============================================================================
# State
state = cat.struct_symSX(["x_b", "y_b", "z_b", "vx_b", "vy_b", "vz_b", "x_c", "y_c", "phi"])
# Control
control = cat.struct_symSX(["v", "w", "psi"])
# Observation
observation = cat.struct_SX(
[
cat.entry("x_b", expr=state["x_b"]),
cat.entry("y_b", expr=state["y_b"]),
cat.entry("z_b", expr=state["z_b"]),
cat.entry("x_c", expr=state["x_c"]),
cat.entry("y_c", expr=state["y_c"]),
cat.entry("phi", expr=state["phi"]),
]
)
l = casadi.Function("l", [x, u], [lfunc])
Pffunc = casadi.mtimes(x.T, x)
Pf = casadi.Function("Pf", [x], [Pffunc])
# Bounds on u.
uub = 1
ulb = -.75
# Make optimizers.
x0 = np.array([0, 1])
# Create variables struct.
var = ctools.struct_symSX([(
ctools.entry("x", shape=(Nx, ), repeat=Nt + 1),
ctools.entry("xc", shape=(Nx, Nc), repeat=Nt),
ctools.entry("u", shape=(Nu, ), repeat=Nt),
)])
varlb = var(-np.inf)
varub = var(np.inf)
varguess = var(0)
# Adjust the relevant constraints.
for t in range(Nt):
varlb["u", t, :] = ulb
varub["u", t, :] = uub
# Now build up constraints and objective.
obj = casadi.SX(0)
con = []
for t in range(Nt):
def create_optim(self):
# Initialize trajectory lists (each list item, one time-step):
self.mpc_obj_x = mpc_obj_x = struct_symSX([
entry('x', repeat=self.N_steps+1, struct=self.mpc_xk),
entry('u', repeat=self.N_steps, struct=self.mpc_uk),
entry('eps', repeat=self.N_steps+1, struct=self.mpc_eps)
])
# Note that:
# x = [x_0, x_1, ... , x_N+1] (N+1 elements)
# u = [u_0, u_1, ... , u_N] (N elements)
# For the optimization variable x_0 we introduce the simple equality constraint that it has
# to be equal to the parameter x0 (mpc_obj_p)
self.mpc_obj_p = mpc_obj_p = struct_symSX([
entry('tvp', repeat=self.N_steps, struct=self.mpc_tvpk),
entry('x0', struct=self.mpc_xk),
entry('p', struct=self.mpc_pk),
entry('pN', struct=self.mpc_pN)
])
# Dummy struct with symbolic variables
aux_struct = struct_symSX([
entry('aux', repeat=self.N_steps, struct=self.mpc_aux_expr)
])
# Create mutable symbolic expression from the struct defined above.
self.mpc_obj_aux = mpc_obj_aux = struct_SX(aux_struct)
# Initialize objective value:
obj = 0
# Initialize constraings:
cons = []
cons_ub = []
cons_lb = []
# Equality constraint for first state:
cons.append(mpc_obj_x['x', 0]-mpc_obj_p['x0'])
cons_lb.append(np.zeros(self.mpc_xk.shape))
cons_ub.append(np.zeros(self.mpc_xk.shape))
# Recursively evaluate system equation and add stage cost and stage constraints:
for k in range(self.N_steps):
mpc_xk_next = self.mpc_problem['model'](mpc_obj_x['x', k], mpc_obj_x['u', k], mpc_obj_p['tvp', k], mpc_obj_p['p'])
# State constraint:
cons.append(mpc_xk_next-mpc_obj_x['x', k+1])
cons_lb.append(np.zeros(self.mpc_xk.shape))
cons_ub.append(np.zeros(self.mpc_xk.shape))
# Add the "stage cost" to the objective
obj += self.mpc_problem['stage_cost'](mpc_obj_x['x', k], mpc_obj_x['u', k], mpc_obj_x['eps', k], mpc_obj_p['tvp', k], mpc_obj_p['p'])
# Constraints for the current step
cons.append(self.mpc_problem['cons'](mpc_obj_x['x', k], mpc_obj_x['u', k], mpc_obj_x['eps', k], mpc_obj_p['tvp', k], mpc_obj_p['p']))
cons_lb.append(self.mpc_problem['cons_lb'])
cons_ub.append(self.mpc_problem['cons_ub'])
# Calculate auxiliary values:
mpc_obj_aux['aux', k] = self.mpc_problem['aux'](mpc_obj_x['x', k], mpc_obj_x['u', k], mpc_obj_x['eps', k], mpc_obj_p['tvp', k], mpc_obj_p['p'])
# Terminal cost:
obj += self.mpc_problem['terminal_cost'](mpc_obj_x['x', -1], mpc_obj_x['eps', -1])
# Terminal set:
cons.append(self.mpc_problem['tcons'](mpc_obj_x['x', -1], mpc_obj_x['eps', -1], mpc_obj_p['pN']))
cons_lb.append(self.mpc_problem['tcons_lb'])
cons_ub.append(self.mpc_problem['tcons_ub'])
# Upper and lower bounds on objective x:
self.mpc_obj_x_lb = self.mpc_obj_x(0)
self.mpc_obj_x_ub = self.mpc_obj_x(0)
self.mpc_obj_x_lb['x', :] = self.mpc_xk_lb
self.mpc_obj_x_ub['x', :] = self.mpc_xk_ub
self.mpc_obj_x_lb['u', :] = self.mpc_uk_lb
self.mpc_obj_x_ub['u', :] = self.mpc_uk_ub
self.mpc_obj_x_lb['eps', :] = self.mpc_epsk_lb
self.mpc_obj_x_ub['eps', :] = self.mpc_epsk_ub
optim_dict = {'x': mpc_obj_x, # Optimization variable
'f': obj, # objective
'g': vertcat(*cons), # constraints
'p': mpc_obj_p} # parameters
self.cons_lb = vertcat(*cons_lb)
self.cons_ub = vertcat(*cons_ub)
# Use the structured data obj_x and obj_p and create identically organized copies with numerical values (initialized to zero)
self.mpc_obj_x_num = self.mpc_obj_x(0)
self.mpc_obj_p_num = self.mpc_obj_p(0)
self.mpc_obj_aux_num = self.mpc_obj_aux(0)
# TODO: Make optimization option available to user.
# Create casadi optimization object:
opts = {'ipopt.linear_solver': 'ma27', 'error_on_fail': False, 'ipopt.tol': 1e-8, 'ipopt.max_iter': 300}
self.optim = nlpsol('optim', 'ipopt', optim_dict, opts)
if self.silent:
opts['ipopt.print_level'] = 0
opts['ipopt.sb'] = "yes"
opts['print_time'] = 0
# Create function to calculate buffer memory from parameter and optimization variable trajectories
self.aux_fun = Function('aux_fun', [mpc_obj_x, mpc_obj_p], [mpc_obj_aux])
u_ = np.zeros((N_, 2))
for i in range(N_):
u_[i] = data[i * 5:i * 5 + 2].T
x_[i] = data[i * 5 + 2:i * 5 + 5].T
x_[-1] = data[-3:].T
return u_, x_
if __name__ == '__main__':
T = 0.5 # sampling time [s]
N = 8 # prediction horizon
rob_diam = 0.3 # [m]
v_max = 0.6
omega_max = np.pi / 4.0
states = ca_tools.struct_symSX([(ca_tools.entry('x'), ca_tools.entry('y'),
ca_tools.entry('theta'))])
x, y, theta = states[...]
n_states = states.size
controls = ca_tools.struct_symSX([(ca_tools.entry('v'),
ca_tools.entry('omega'))])
v, omega = controls[...]
n_controls = controls.size
## rhs
rhs = ca_tools.struct_SX(states)
rhs['x'] = v * ca.cos(theta)
rhs['y'] = v * ca.sin(theta)
rhs['theta'] = omega
## function
def create_plan_pc(b0, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final means
m_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
m_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dm_bc = m_bN - m_cN
# Regularize controls
J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
bk = cat.struct_SX(belief)
bk['S'] = b0['S']
g = []
lbg = []
ubg = []
for k in range(N_sim):
# Belief propagation
bk['m'] = V['X',k]
[bk_next] = BF([ bk, V['U',k] ])
bk_next = belief(bk_next) # simplify indexing
# Multiple shooting
g.append(bk_next['m'] - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Control constraints
g.append(V['U',k,'v'] - a - b * ca.cos(V['U',k,'psi']))
lbg.append(-ca.inf)
ubg.append(ca.DMatrix([0]))
# Advance time
bk = bk_next
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Simple cost
J += 1e1 * (ca.mul(dm_bc.T, dm_bc) + ca.trace(bk_next['S']))
# log-probability cost
#Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
#J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# 0 <= v
lbx['U',:,'v'] = 0
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# -pi <= psi <= pi
lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = b0['m']
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
nu = 2
# Initial condition
x0 = ca.DMatrix([10, 6, ca.pi])
# Identity matrix
Inx = ca.DMatrix.eye(nx)
# ----------------------------------------------------------------------------
# Dynamics
# ----------------------------------------------------------------------------
# Continuous dynamics
dt_sym = ca.SX.sym('dt')
state = cat.struct_symSX(['x', 'y', 'phi'])
control = cat.struct_symSX(['v', 'w'])
rhs = cat.struct_SX(state)
rhs['x'] = control['v'] * ca.cos(state['phi'])
rhs['y'] = control['v'] * ca.sin(state['phi'])
rhs['phi'] = control['w']
f = ca.SXFunction('Continuous dynamics', [state, control], [rhs])
# Discrete dynamics
state_next = state + dt_sym * f([state, control])[0]
op = {'input_scheme': ['state', 'control', 'dt'],
'output_scheme': ['state_next']}
F = ca.SXFunction('Discrete dynamics',
[state, control, dt_sym], [state_next], op)
Fj_x = F.jacobian('state')
Fj_u = F.jacobian('control')
def make_constraint_struct(eqs_dict, ineqs_dict):
entry_list = make_entry_list(eqs_dict, ineqs_dict)
constraint_struct = cas.struct_symSX(entry_list)
return constraint_struct
def test(gamma_scale=1.):
architecture = archi.Architecture({1: 0})
options = {}
options['induction'] = {}
options['induction']['vortex_gamma_scale'] = gamma_scale
options['induction']['vortex_wake_nodes'] = 2
options['induction']['vortex_far_convection_time'] = 1.
options['induction']['vortex_u_ref'] = 1.
options['induction']['vortex_position_scale'] = 1.
kite = architecture.kite_nodes[0]
xd_struct = cas.struct([
cas.entry("wx_" + str(kite) + "_ext_0", shape=(3, 1)),
cas.entry("wx_" + str(kite) + "_ext_1", shape=(3, 1)),
cas.entry("wx_" + str(kite) + "_int_0", shape=(3, 1)),
cas.entry("wx_" + str(kite) + "_int_1", shape=(3, 1))
])
xl_struct = cas.struct([cas.entry("wg_" + str(kite) + "_0")])
var_struct = cas.struct_symSX(
[cas.entry('xd', struct=xd_struct),
cas.entry('xl', struct=xl_struct)])
variables = var_struct(0.)
variables['xd', 'wx_' + str(kite) + '_ext_0'] = 0.5 * vect_op.yhat_np()
variables['xd', 'wx_' + str(kite) + '_int_0'] = -0.5 * vect_op.yhat_np()
variables['xd', 'wx_' + str(kite) +
'_ext_1'] = variables['xd', 'wx_' + str(kite) +
'_ext_0'] + vect_op.xhat_np()
variables['xd', 'wx_' + str(kite) +
'_int_1'] = variables['xd', 'wx_' + str(kite) +
'_int_0'] + vect_op.xhat_np()
variables['xl', 'wg_' + str(kite) + '_0'] = 1. / gamma_scale
test_list = get_list(options, variables, architecture)
filaments = test_list.shape[1]
filament_count_test = filaments - 6
if not (filament_count_test == 0):
message = 'filament list does not work as expected. difference in number of filaments in test_list = ' + str(
filament_count_test)
awelogger.logger.error(message)
raise Exception(message)
LE_expected = cas.DM(np.array([0., -0.5, 0., 0., 0.5, 0., 1.]))
PE_expected = cas.DM(np.array([0., 0.5, 0., 1., 0.5, 0., 1.]))
TE_expected = cas.DM(np.array([1., -0.5, 0., 0., -0.5, 0., 1.]))
expected_filaments = {
'leading edge': LE_expected,
'positive edge': PE_expected,
'trailing edge': TE_expected
}
for type in expected_filaments.keys():
expected_filament = expected_filaments[type]
expected_in_list = expected_filament_in_list(test_list,
expected_filament)
if not expected_in_list:
message = 'filament list does not work as expected. ' + type + \
' test filament not in test_list.'
awelogger.logger.error(message)
with np.printoptions(precision=3, suppress=True):
print('test_list:')
print(np.array(test_list))
raise Exception(message)
NE_not_expected = cas.DM(np.array([1., -0.5, 0., 1., 0.5, 0., -1.]))
not_expected_filaments = {'negative edge': NE_not_expected}
for type in not_expected_filaments.keys():
not_expected_filament = not_expected_filaments[type]
is_reasonable = not (expected_filament_in_list(test_list,
not_expected_filament))
if not is_reasonable:
message = 'filament list does not work as expected. ' + type + \
' test filament in test_list.'
awelogger.logger.error(message)
with np.printoptions(precision=3, suppress=True):
print('test_list:')
print(np.array(test_list))
raise Exception(message)
return test_list
def problem_formulation(self):
""" MPC states for stage k"""
self.mpc_xk = struct_symSX([
entry('s_buffer', shape=(self.n_out, 1)),
entry('ds_buffer_source', shape=(self.n_in,1))
])
# States at next time-step. Same structure as mpc_xk. Will be assigned expressions later on.
self.mpc_xk_next = struct_SX(self.mpc_xk)
""" MPC control inputs for stage k"""
self.mpc_uk = struct_symSX([
entry('dv_in', shape=(self.n_in, 1)),
entry('dv_out', shape=(self.n_out, 1)),
])
""" MPC soft constraints"""
self.mpc_eps = struct_symSX([
entry('s_buffer', shape=(self.n_out, 1)),
])
eps_s_buffer = self.mpc_eps['s_buffer']
""" MPC time-varying parameters for stage k"""
self.mpc_tvpk = struct_symSX([
entry('u_prev', struct=self.mpc_uk),
entry('v_out_max', shape=(self.n_out, 1)),
entry('s_buffer_source', shape=(self.n_in, 1)),
entry('v_out_source', shape=(self.n_in, 1)),
entry('dv_out_source_fix', shape=(self.n_in,1)),
entry('time_fac'),
])
""" MPC parameters for stage k"""
self.mpc_pk = struct_symSX([
# Note: Pb is defined "transposed", as casadi will raise an error for n_out=1, since it cant handle row vectors.
entry('Pb', shape=(np.sum(self.n_in), self.n_out)),
entry('control_delta', shape=1),
])
""" MPC parameters for stage N"""
self.mpc_pN = struct_symSX([
entry('s_buffer_source_N', shape=(self.n_in,1)),
])
""" Memory """
# Buffer memory
s_buffer = self.mpc_xk['s_buffer']
""" Incoming packet stream """
# Allowed/accepted incoming packet stream:
v_in = self.v_in_max_total-self.mpc_uk['dv_in']
""" Outgoing packet stream """
# Outgoing packet stream:
v_out = self.v_out_max_total-self.mpc_uk['dv_out']
# Maximum value for v_out (determined by target server):
v_out_max = self.mpc_tvpk['v_out_max']
""" Load information """
# memory info incoming servers
s_buffer_source = self.mpc_tvpk['s_buffer_source']
""" Source Node """
# Adjusted buffer memory of source:
ds_buffer_source = self.mpc_xk['ds_buffer_source']
# Predicted outgoing packet stream:
v_out_source = self.mpc_tvpk['v_out_source']
# v_out_source adjusted:
dv_out_source = v_in - v_out_source
# Corrected buffer memory of source:
s_buffer_source_corr = s_buffer_source - ds_buffer_source
# ds_buffer_source_next:
self.mpc_xk_next['ds_buffer_source'] = ds_buffer_source + dv_out_source*self.dt
""" Circuit matching """
# Assignment Matrix: Which element of each input is assigned to which output buffer:
# Note: Pb is defined "transposed", as casadi will raise an error for n_out=1, since it cant handle row vectors.
Pb = self.mpc_pk['Pb'].T
""" System dynamics"""
# system dynamics, constraints and objective definition:
s_tilde_next = s_buffer + self.dt*Pb@(v_in)
s_buffer_next = s_tilde_next - self.dt*v_out
self.mpc_xk_next['s_buffer'] = s_buffer_next
""" Objective """
stage_cost = 0
# Objective function with fairness formulation:
#s_buffer_source_split = (s_buffer_source+eps)/(sum1(s_buffer_source+eps))
time_fac = self.mpc_tvpk['time_fac']
stage_cost += 10*sum1(time_fac/(self.n_in)*self.mpc_uk['dv_in']**2)
stage_cost += sum1(time_fac/(self.n_out)*self.mpc_uk['dv_out']**2)
stage_cost += 1e5*sum1(eps_s_buffer)
# Control delta regularization
stage_cost += self.mpc_pk['control_delta']*sum1((self.mpc_uk-self.mpc_tvpk['u_prev'])**2)
# Terminal cost:
terminal_cost = 1e5*sum1(eps_s_buffer)
""" Constraints"""
self.mpc_xk_lb = self.mpc_xk(0)
self.mpc_xk_lb['ds_buffer_source'] = -np.inf
self.mpc_xk_ub = self.mpc_xk(np.inf)
# All inputs with lower bound 0 and upper bound infinity
self.mpc_uk_lb = self.mpc_uk(0)
self.mpc_uk_ub = self.mpc_uk(np.inf)
# All eps with lower bound 0 and upper bound infinity
self.mpc_epsk_lb = self.mpc_eps(0)
self.mpc_epsk_ub = self.mpc_eps(np.inf)
# Further constraints on states and inputs:
# Note lb and ub must be lists to be concatenated lateron
dv_out_source_fix = self.mpc_tvpk['dv_out_source_fix']
cons_list = [
{'lb': [0]*self.n_in, 'eq': v_in, 'ub': [np.inf]*self.n_in}, # v_in must be greater than 0.
{'lb': [0]*self.n_out, 'eq': v_out, 'ub': [np.inf]*self.n_out}, # v_out cant be negative
{'lb': [0]*self.n_out, 'eq': v_out_max-v_out, 'ub': [np.inf]*self.n_out}, # outgoing packet stream cant be greater than what is allowed individually
{'lb': [-np.inf], 'eq': sum1(v_in), 'ub': [self.v_in_max_total]}, # sum of all incoming traffic can't exceed v_in_max_total
{'lb': [-np.inf], 'eq': sum1(v_out), 'ub': [self.v_out_max_total]}, # outgoing packet stream cant be greater than what is allowed in total.
{'lb': [0]*self.n_out, 'eq': self.s_c_max_total+eps_s_buffer-s_buffer, 'ub': [np.inf]*self.n_out},
{'lb': [0]*self.n_in, 'eq': s_buffer_source_corr, 'ub': [np.inf]*self.n_in}, # Adjusted s_buffer_source must be >0
{'lb': [0]*self.n_in, 'eq': dv_out_source_fix*dv_out_source, 'ub': [0]*self.n_in},
]
assert np.all([type(cons_list_i['lb']) == list for cons_list_i in cons_list])
assert np.all([type(cons_list_i['ub']) == list for cons_list_i in cons_list])
cons = vertcat(*[con_i['eq'] for con_i in cons_list])
cons_lb = np.concatenate([con_i['lb'] for con_i in cons_list])
cons_ub = np.concatenate([con_i['ub'] for con_i in cons_list])
# Terminal constraints:
tcons_list = [
{'lb': [0]*self.n_out, 'eq': self.s_c_max_total+eps_s_buffer-s_buffer, 'ub': [np.inf]*self.n_out},
{'lb': [0]*self.n_out, 'eq': self.mpc_pN['s_buffer_source_N']-ds_buffer_source, 'ub': [np.inf]*self.n_in}
]
tcons = vertcat(*[tcon_i['eq'] for tcon_i in tcons_list])
tcons_lb = np.concatenate([tcon_i['lb'] for tcon_i in tcons_list])
tcons_ub = np.concatenate([tcon_i['ub'] for tcon_i in tcons_list])
""" Summarize auxiliary / intermediate variables in mpc_aux with their respective expression """
bandwidth_load_in = sum1(v_in)/self.v_in_max_total
bandwidth_load_out = sum1(v_out)/self.v_out_max_total
# For debugging: Add intermediate variables to mpc_aux_expr and query them after solving the optimization problem.
self.mpc_aux_expr = struct_SX([
entry('v_in', expr=v_in),
entry('v_out', expr=v_out),
entry('bandwidth_load_in', expr=bandwidth_load_in),
entry('bandwidth_load_out', expr=bandwidth_load_out),
entry('s_buffer_source_corr', expr=s_buffer_source_corr)
])
""" Problem dictionary """
mpc_problem = {}
mpc_problem['cons'] = Function('cons', [self.mpc_xk, self.mpc_uk, self.mpc_eps, self.mpc_tvpk, self.mpc_pk], [cons])
mpc_problem['cons_lb'] = cons_lb
mpc_problem['cons_ub'] = cons_ub
mpc_problem['tcons'] = Function('tcons', [self.mpc_xk, self.mpc_eps, self.mpc_pN], [tcons])
mpc_problem['tcons_lb'] = tcons_lb
mpc_problem['tcons_ub'] = tcons_ub
mpc_problem['stage_cost'] = Function('stage_cost', [self.mpc_xk, self.mpc_uk, self.mpc_eps, self.mpc_tvpk, self.mpc_pk], [stage_cost])
mpc_problem['terminal_cost'] = Function('terminal_cost', [self.mpc_xk, self.mpc_eps], [terminal_cost])
mpc_problem['model'] = Function('model', [self.mpc_xk, self.mpc_uk, self.mpc_tvpk, self.mpc_pk], [self.mpc_xk_next])
mpc_problem['aux'] = Function('aux', [self.mpc_xk, self.mpc_uk, self.mpc_eps, self.mpc_tvpk, self.mpc_pk], [self.mpc_aux_expr])
self.mpc_problem = mpc_problem