def extract_time_grid(model, nlp, formulation, init_options, V_init, ntp_dict):
tf_guess = V_init['theta', 't_f']
# extract time grid
tgrid_xd = nlp.time_grids['x'](tf_guess)
if 'coll' in list(nlp.time_grids.keys()):
tgrid_coll = nlp.time_grids['coll'](tf_guess)
d = nlp.d
n_k = nlp.n_k
for k in range(n_k + 1):
t = tgrid_xd[k]
ret = guess_values_at_time(t, init_options, model, formulation,
tf_guess, ntp_dict)
for name in struct_op.subkeys(model.variables, 'xd'):
V_init['xd', k, name] = ret[name]
if nlp.discretization == 'direct_collocation' and (k < n_k):
for j in range(d):
t = tgrid_coll[k, j]
ret = guess_values_at_time(t, init_options, model, formulation,
tf_guess, ntp_dict)
for name in struct_op.subkeys(model.variables, 'xd'):
V_init['coll_var', k, j, 'xd', name] = ret[name]
return V_init
def get_control_frequency(nlp_options, model, V, outputs):
if 'control_freq' not in list(outputs.keys()):
outputs['control_freq'] = {}
nk = nlp_options['n_k']
for delta in struct_op.subkeys(model.variables, 'u'):
if ('delta' in delta) and (not 'ddelta' in delta):
variable = V['u', 0, delta]
for dim in range(variable.shape[0]):
control_freq = vect_op.estimate_1d_frequency(
cas.vertcat(*V['u', :, delta, dim]),
dt=V['theta', 't_f'] / nk)
outputs['control_freq'][delta + '_' + str(dim)] = control_freq
for delta in struct_op.subkeys(model.variables, 'xd'):
if ('delta' in delta) and (not 'ddelta' in delta):
variable = V['xd', 0, delta]
for dim in range(variable.shape[0]):
control_freq = vect_op.estimate_1d_frequency(
cas.vertcat(*V['xd', :, delta, dim]),
dt=V['theta', 't_f'] / nk)
outputs['control_freq'][delta + '_' + str(dim)] = control_freq
return outputs
def initialize_bound_update_counter(model, schedule, formulation):
bound_update_counter = {}
for name in list(model.parameters_dict['phi'].keys()):
bound_update_counter[name] = 0
for name in struct_op.subkeys(model.variables, 'theta'):
bound_update_counter[name] = 0
for name in struct_op.subkeys(model.variables, 'u'):
if 'fict' in name:
bound_update_counter[name] = 0
if 'ddl_t' in list(model.variables_dict['u'].keys()):
bound_update_counter['ddl_t'] = 0
elif 'dddl_t' in list(model.variables_dict['u'].keys()):
bound_update_counter['dddl_t'] = 0
if 'dl_t' in list(schedule['bounds'].keys()):
bound_update_counter['dl_t'] = 0
if 'l_t' in list(schedule['bounds'].keys()):
bound_update_counter['l_t'] = 0
return bound_update_counter
def define_bound_update_schedule(model, nlp, formulation):
bound_schedule = create_empty_bound_update_schedule(
model, nlp, formulation)
for name in list(model.parameters_dict['phi'].keys()):
bound_schedule[name][1] = ['lb', 'phi', 'final']
bound_schedule[name][2] = ['ub', 'phi', 'final']
for name in struct_op.subkeys(model.variables, 'theta'):
bound_schedule[name][1] = ['lb', 'theta', 'final']
bound_schedule[name][2] = ['ub', 'theta', 'final']
for name in struct_op.subkeys(model.variables, 'u'):
if 'fict' in name:
bound_schedule[name][1] = ['lb', 'u', 'final']
bound_schedule[name][2] = ['ub', 'u', 'final']
if 'ddl_t' in list(model.variables_dict['u'].keys()):
bound_schedule['ddl_t'][1] = ['lb', 'u', 'final']
bound_schedule['ddl_t'][2] = ['ub', 'u', 'final']
elif 'dddl_t' in list(model.variables_dict['u'].keys()):
bound_schedule['dddl_t'][1] = ['lb', 'u', 'final']
bound_schedule['dddl_t'][2] = ['ub', 'u', 'final']
if 'dl_t' in list(bound_schedule.keys()):
bound_schedule['dl_t'][1] = ['lb', 'xd', 'final']
bound_schedule['dl_t'][2] = ['ub', 'xd', 'final']
if 'l_t' in list(bound_schedule.keys()):
bound_schedule['l_t'][1] = ['lb', 'xd', 'final']
bound_schedule['l_t'][2] = ['ub', 'xd', 'final']
return bound_schedule
def find_ddq_regularisation(nlp_numerics_options, V, P, xdot, outputs):
nk = nlp_numerics_options['n_k']
direct_collocation, multiple_shooting, d, scheme, int_weights = extract_discretization_info(
nlp_numerics_options)
ddq_regularisation = 0.
for kdx in range(nk):
if multiple_shooting:
for name in set(struct_op.subkeys(outputs, 'xddot_from_var')):
ddq_regularisation += cas.mtimes(xdot['xd', kdx, name[1:]].T,
xdot['xd', kdx, name[1:]])
elif direct_collocation:
for jdx in range(d):
for name in set(struct_op.subkeys(outputs, 'xddot_from_var')):
if 'ddq' in name:
ddq_regularisation += int_weights[jdx] * cas.mtimes(
xdot['coll_xd', kdx, jdx, name[1:]].T,
xdot['coll_xd', kdx, jdx, name[1:]])
return ddq_regularisation
def create_empty_bound_update_schedule(model, nlp, formulation):
bound_schedule = {}
for name in list(model.parameters_dict['phi'].keys()):
bound_schedule[name] = {}
for name in struct_op.subkeys(model.variables, 'theta'):
bound_schedule[name] = {}
if 'ddl_t' in list(model.variables_dict['u'].keys()):
bound_schedule['ddl_t'] = {}
elif 'dddl_t' in list(model.variables_dict['u'].keys()):
bound_schedule['dddl_t'] = {}
elif 'dv_sq' in list(model.variables_dict['u'].keys()):
bound_schedule['dv_sq'] = {}
# check if phase fix
if nlp.V['theta', 't_f'].shape[0] > 1:
bound_schedule['dl_t'] = {}
bound_schedule['l_t'] = {}
for name in struct_op.subkeys(model.variables, 'u'):
if 'fict' in name:
bound_schedule[name] = {}
return bound_schedule
def initial_guess_actuator_xd(init_options, nlp, formulation, model, V_init):
level_siblings = model.architecture.get_all_level_siblings()
time_final = init_options['precompute']['time_final']
omega_norm = init_options['precompute']['angular_speed']
tgrid_coll = nlp.time_grids['coll'](time_final)
dict = {}
dict['a'] = cas.DM(init_options['xd']['a'])
dict['asin'] = cas.DM(0.)
dict['acos'] = cas.DM(0.)
dict['ct'] = 4. * dict['a'] * (1. - dict['a'])
dict['bar_varrho'] = cas.DM(1.)
var_type = 'xd'
for name in struct_op.subkeys(model.variables, var_type):
name_stripped, _ = struct_op.split_name_and_node_identifier(name)
if 'psi' in name_stripped:
V_init = set_azimuth_variables(V_init, init_options, name, model, nlp, tgrid_coll, level_siblings, omega_norm)
elif name_stripped in dict.keys():
V_init = tools_init.insert_dict(dict, var_type, name, name_stripped, V_init)
return V_init
def find_nominal_landing_cost(V, P, variables, nlp_options):
pos_weight = nlp_options['landing']['cost']['position_weight']
vel_weight = nlp_options['landing']['cost']['velocity_weight']
q_end = {}
dq_end = {}
for name in struct_op.subkeys(variables, 'xd'):
if name[0] == 'q':
q_end[name] = V['xd', -1, name]
elif name[:2] == 'dq':
dq_end[name] = V['xd', -1, name]
velocity_end = 0.0
position_end = 0.0
for position in list(q_end.keys()):
position_end += cas.mtimes(q_end[position].T, q_end[position])
position_end *= 1. / len(list(q_end.keys()))
for velocity in list(dq_end.keys()):
velocity_end += cas.mtimes(dq_end[velocity].T, dq_end[velocity])
velocity_end *= 1. / len(list(dq_end.keys()))
nominal_landing_cost = P['cost', 'nominal_landing'] * (
vel_weight * velocity_end + pos_weight * position_end)
return nominal_landing_cost
def set_nontime_system_parameters(init_options, model, V_init):
for name in set(struct_op.subkeys(model.variables, 'theta')) - set(['t_f'
]):
print(init_options['theta'].keys())
print(name)
if name in list(init_options['theta'].keys()):
V_init['theta', name] = init_options['theta'][name]
elif name[:3] == 'l_c':
layer = int(name[3:])
kites = model.architecture.kites_map[layer]
q_first = V_init['xd', 0, 'q{}{}'.format(
kites[0], model.architecture.parent_map[kites[0]])]
q_second = V_init['xd', 0, 'q{}{}'.format(
kites[1], model.architecture.parent_map[kites[1]])]
V_init['theta', name] = np.linalg.norm(q_first - q_second)
if init_options['cross_tether_attachment'] == 'wing_tip':
V_init['theta', name] += -init_options['sys_params_num'][
'geometry']['b_ref']
elif name[:6] == 'diam_c':
V_init['theta', name] = init_options['theta']['diam_c']
elif name[:7] == 'k_gear':
V_init['theta', name] = 1
else:
raise ValueError("please specify an initial value for variable '" +
name + "' of type 'theta'")
return V_init
def scale_xddot(variables):
time_scaled_variables = variables(variables.cat)
for name in struct_op.subkeys(variables, 'xddot'):
time_scaled_variables['xddot',name] = variables['xddot',name]/variables['theta','t_f']
return time_scaled_variables
def get_regularization_weights(variables, P, nlp_options):
sorting_dict = get_regularization_sorting_dict()
weights = variables(P['p', 'weights'])
for var_type in set(variables.keys()):
category = sorting_dict[var_type]['category']
exceptions = sorting_dict[var_type]['exceptions']
for var_name in set(struct_op.subkeys(variables, var_type)):
name, _ = struct_op.split_name_and_node_identifier(var_name)
if (not name in exceptions.keys()) and (not category == None):
shortened_cat_name = category[:-5]
normalization = nlp_options['cost']['normalization'][
shortened_cat_name]
factor = P['cost', shortened_cat_name]
weights[var_type,
var_name] = weights[var_type,
var_name] * factor / normalization
elif (name
in exceptions.keys()) and (not exceptions[name] == None):
shortened_cat_name = exceptions[name][:-5]
normalization = nlp_options['cost']['normalization'][
shortened_cat_name]
factor = P['cost', shortened_cat_name]
weights[var_type,
var_name] = weights[var_type,
var_name] * factor / normalization
return weights
def time_derivative(expr, variables, architecture):
# notice that the the chain rule
# df/dt = partial f/partial t
# + (partial f/partial x1)(partial x1/partial t)
# + (partial f/partial x2)(partial x2/partial t)...
# doesn't care about the scaling of the component functions, as long as
# the units of (partial xi) will cancel
# it is here assumed that (partial f/partial t) = 0.
vars_scaled = variables['scaled']
deriv = 0.
# (partial f/partial xi) for variables with trivial derivatives
deriv_vars = struct_op.subkeys(vars_scaled, 'xddot')
for deriv_name in deriv_vars:
deriv_type = struct_op.get_variable_type(variables['SI'], deriv_name)
var_name = deriv_name[1:]
var_type = struct_op.get_variable_type(variables['SI'], var_name)
q_sym = vars_scaled[var_type, var_name]
dq_sym = vars_scaled[deriv_type, deriv_name]
deriv += cas.mtimes(cas.jacobian(expr, q_sym), dq_sym)
# (partial f/partial xi) for kite rotation matrices
kite_nodes = architecture.kite_nodes
for kite in kite_nodes:
parent = architecture.parent_map[kite]
r_name = 'r{}{}'.format(kite, parent)
kite_has_6dof = r_name in struct_op.subkeys(vars_scaled, 'xd')
if kite_has_6dof:
r_kite = vars_scaled['xd', r_name]
dcm_kite = cas.reshape(r_kite, (3, 3))
omega = vars_scaled['xd', 'omega{}{}'.format(kite, parent)]
dexpr_dr = cas.jacobian(expr, r_kite)
dr_dt = cas.reshape(
cas.mtimes(vect_op.skew(omega), cas.inv(dcm_kite.T)), (9, 1))
deriv += cas.mtimes(dexpr_dr, dr_dt)
return deriv
def initialize_cost_update_counter(P):
cost_update_counter = {}
for name in struct_op.subkeys(P, 'cost'):
cost_update_counter[name] = -1
return cost_update_counter
def plot_algebraic_vars(V, model):
counter = 0
plt.figure(4)
for state in struct_op.subkeys(model.variables, 'xa'):
counter += 1
plt.subplot(2, 2, counter)
for state_dim in range(V['xa', 0, 0, state].shape[0]):
plt.plot(cas.vertcat(*np.array(V['xa', :, :, state, state_dim])))
plt.title(state)
plt.subplots_adjust(wspace=0.3, hspace=0.6)
def guess_values_at_time(t, init_options, model):
ret = {}
for name in struct_op.subkeys(model.variables, 'xd'):
ret[name] = 0.0
ret['e'] = 0.0
ret['l_t'] = init_options['xd']['l_t']
ret['dl_t'] = 0.0
number_of_nodes = model.architecture.number_of_nodes
parent_map = model.architecture.parent_map
kite_nodes = model.architecture.kite_nodes
kite_dof = model.kite_dof
for node in range(1, number_of_nodes):
parent = parent_map[node]
if parent == 0:
parent_position = np.zeros((3, 1))
else:
grandparent = parent_map[parent]
parent_position = ret['q' + str(parent) + str(grandparent)]
if not node in kite_nodes:
ret['q' + str(node) + str(parent)] = get_tether_node_position(init_options, parent_position, node, ret['l_t'])
ret['dq' + str(node) + str(parent)] = np.zeros((3, 1))
else:
height = init_options['precompute']['height']
radius = init_options['precompute']['radius']
ehat_normal, ehat_radial, ehat_tangential = tools.get_rotating_reference_frame(t, init_options, model, node, ret)
tether_vector = ehat_radial * radius + ehat_normal * height
position = parent_position + tether_vector
velocity = tools.get_velocity_vector(t, init_options, model, node, ret)
ret['q' + str(node) + str(parent)] = position
ret['dq' + str(node) + str(parent)] = velocity
dcm = tools.get_kite_dcm(t, init_options, model, node, ret)
if init_options['cross_tether']:
if init_options['cross_tether_attachment'] in ['com','stick']:
dcm = get_cross_tether_dcm(init_options, dcm)
dcm_column = cas.reshape(dcm, (9, 1))
omega_vector = tools.get_omega_vector(t, init_options, model, node, ret)
if int(kite_dof) == 6:
ret['omega' + str(node) + str(parent)] = omega_vector
ret['r' + str(node) + str(parent)] = dcm_column
return ret
def scale_variable(variables, var_si, scaling):
var_scaled = variables(copy.deepcopy(var_si))
for variable_type in list(variables.keys()):
subkeys = struct_op.subkeys(variables, variable_type)
for name in subkeys:
local_si = var_scaled[variable_type, name]
var_scaled[variable_type, name] = struct_op.var_si_to_scaled(variable_type, name, local_si, scaling)
return var_scaled
def plot_controls(V, model):
counter = 0
plt.figure(2)
for state in struct_op.subkeys(model.variables, 'u'):
counter += 1
plt.subplot(4, 3, counter)
for state_dim in range(V['u', 0, state].shape[0]):
plt.plot(cas.vertcat(*np.array(V['u', :, state, state_dim])))
plt.title(state)
plt.subplots_adjust(wspace=0.3, hspace=0.6)
def define_costs_to_update(P, formulation):
updates = {}
initial_updates = {}
initial_updates[0] = struct_op.subkeys(P, 'cost')
fictitious_updates = {}
fictitious_updates[0] = ['gamma', 'fictitious']
fictitious_updates[1] = []
tether_updates = {}
tether_updates[0] = ['tau']
tether_updates[1] = []
induction_updates = {}
induction_updates[0] = ['iota']
induction_updates[1] = []
tether_release_updates = {}
tether_release_updates[0] = []
power_updates = {}
power_updates[0] = ['power', 'psi']
power_updates[1] = ['tracking']
nominal_landing_updates = {}
nominal_landing_updates[0] = ['nominal_landing', 'eta']
nominal_landing_updates[1] = ['tracking']
transition_updates = {}
transition_updates[0] = ['transition', 'upsilon']
transition_updates[1] = ['tracking']
compromised_landing_updates = {}
compromised_landing_updates[0] = ['compromised_battery', 'nu']
compromised_landing_updates[1] = []
final_updates = {}
final_updates[0] = []
updates['initial'] = initial_updates
updates['fictitious'] = fictitious_updates
updates['tether'] = tether_updates
updates['induction'] = induction_updates
updates['tether_release'] = tether_release_updates
updates['power'] = power_updates
updates['transition'] = transition_updates
updates['nominal_landing'] = nominal_landing_updates
updates['compromised_landing'] = compromised_landing_updates
updates['final'] = final_updates
return updates
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 find_fictitious(nlp_numerics_options, V, P, variables):
nk = nlp_numerics_options['n_k']
fictitious = 0.
for kdx in range(nk):
for name in set(struct_op.subkeys(variables, 'u')):
if 'fict' in name:
difference = V['u', kdx, name] - P['p', 'ref', 'u', kdx, name]
fictitious += P['p', 'weights', 'u', name][0] * cas.mtimes(
difference.T, difference)
return fictitious
def find_regularisation(nlp_numerics_options, V, P, variables):
nk = nlp_numerics_options['n_k']
regularisation = 0.
for kdx in range(nk):
for name in set(struct_op.subkeys(variables, 'u')) - set(['ddl_t']):
if not 'fict' in name:
difference = V['u', kdx, name] - P['p', 'ref', 'u', kdx, name]
regularisation += P['p', 'weights', 'u', name][0] * cas.mtimes(
difference.T, difference)
return regularisation
def get_local_tracking_function(variables, P):
# initialization tracking
tracking = 0.
for name in set(struct_op.subkeys(variables, 'xd')) - set('e'):
difference = variables['xd', name] - P['p', 'ref', name]
tracking += P['p', 'weights', name][0] * cas.mtimes(
difference.T, difference)
for name in set(struct_op.subkeys(variables, 'xa')):
difference = variables['xa', name] - P['p', 'ref', name]
tracking += P['p', 'weights', name][0] * cas.mtimes(
difference.T, difference)
for name in set(struct_op.subkeys(variables, 'xl')):
difference = variables['xl', name] - P['p', 'ref', name]
tracking += P['p', 'weights', name][0] * cas.mtimes(
difference.T, difference)
tracking_fun = cas.Function('tracking_fun', [variables, P], [tracking])
return tracking_fun
def initial_guess_actuator_xl(init_options, nlp, formulation, model, V_init):
level_siblings = model.architecture.get_all_level_siblings()
time_final = init_options['precompute']['time_final']
omega_norm = init_options['precompute']['angular_speed']
tgrid_coll = nlp.time_grids['coll'](time_final)
u_hat, v_hat, w_hat = get_local_wind_reference_frame(init_options)
uzero_matr = cas.horzcat(u_hat, v_hat, w_hat)
uzero_matr_cols = cas.reshape(uzero_matr, (9, 1))
n_rot_hat, y_rot_hat, z_rot_hat = tools_init.get_rotor_reference_frame(init_options)
rot_matr = cas.horzcat(n_rot_hat, y_rot_hat, z_rot_hat)
rot_matr_cols = cas.reshape(rot_matr, (9, 1))
dict = {}
dict['rot_matr'] = rot_matr_cols
dict['area'] = cas.DM(1.)
dict['cmy'] = cas.DM(0.)
dict['cmz'] = cas.DM(0.)
dict['uzero_matr'] = uzero_matr_cols
dict['g_vec_length'] = cas.DM(1.)
dict['n_vec_length'] = cas.DM(1.)
dict['z_vec_length'] = cas.DM(1.)
dict['u_vec_length'] = cas.DM(1.)
dict['varrho'] = cas.DM(1.)
dict['qzero'] = cas.DM(1.)
dict['gamma'] = get_gamma_angle(init_options)
dict['cosgamma'] = np.cos(dict['gamma'])
dict['singamma'] = np.sin(dict['gamma'])
dict['chi'] = get_chi_angle(init_options, 'xl')
dict['coschi'] = np.cos(dict['chi'])
dict['sinchi'] = np.sin(dict['chi'])
dict['tanhalfchi'] = np.tan(dict['chi'] / 2.)
dict['sechalfchi'] = 1. / np.cos(dict['chi'] / 2.)
dict['LL'] = cas.DM([0.375, 0., 0., 0., -1., 0., 0., -1., 0.])
dict['corr'] = 1. - init_options['xd']['a']
var_type = 'xl'
for name in struct_op.subkeys(model.variables, 'xl'):
name_stripped, _ = struct_op.split_name_and_node_identifier(name)
if 'psi' in name_stripped:
V_init = set_azimuth_variables(V_init, init_options, name, model, nlp, tgrid_coll, level_siblings, omega_norm)
elif name_stripped in dict.keys():
V_init = tools_init.insert_dict(dict, var_type, name, name_stripped, V_init)
return V_init
def get_general_reg_costs_function(variables, V):
var_sym = cas.SX.sym('var_sym', variables.cat.shape)
ref_sym = cas.SX.sym('ref_sym', variables.cat.shape)
weight_sym = cas.SX.sym('weight_sym', variables.cat.shape)
reg_fun = get_general_regularization_function(variables)
regs = variables(reg_fun(var_sym, ref_sym, weight_sym))
sorting_dict = get_regularization_sorting_dict()
reg_costs_struct = get_costs_struct(V)
reg_costs = reg_costs_struct(cas.SX.zeros(reg_costs_struct.shape))
slack_fun = get_local_slack_penalty_function()
for var_type in set(variables.keys()):
category = sorting_dict[var_type]['category']
exceptions = sorting_dict[var_type]['exceptions']
for var_name in set(struct_op.subkeys(variables, var_type)):
name, _ = struct_op.split_name_and_node_identifier(var_name)
if (not name in exceptions.keys()) and (not category == None):
reg_costs[category] = reg_costs[category] + cas.sum1(
regs[var_type, var_name])
elif (name in exceptions.keys()) and (
not exceptions[name] == None) and (not exceptions[name]
== 'slack'):
exc_category = exceptions[name]
reg_costs[exc_category] = reg_costs[exc_category] + cas.sum1(
regs[var_type, var_name])
elif (name in exceptions.keys()) and (exceptions[name] == 'slack'):
exc_category = exceptions[name]
for idx in variables.f[var_type, var_name]:
var_loc = var_sym[idx]
ref_loc = ref_sym[idx]
weight_loc = weight_sym[idx]
pen_loc = slack_fun(var_loc, ref_loc, weight_loc)
reg_costs[exc_category] = reg_costs[exc_category] + pen_loc
reg_costs_list = reg_costs.cat
reg_costs_fun = cas.Function('reg_costs_fun',
[var_sym, ref_sym, weight_sym],
[reg_costs_list])
return reg_costs_fun, reg_costs_struct
def set_p_fix_num(V_ref, nlp, model, options):
# --------------------
# parameter values
# --------------------
# build reference parameters, references of cost function should match the
# initial guess
logging.info('generate OCP parameter values...')
P = nlp.P
p_fix_num = P(0.)
p_fix_num['p', 'weights'] = 1.0e-8
# weights and references
for variable_type in set(model.variables.keys()) - set(['xddot']):
for name in struct_op.subkeys(model.variables, variable_type):
# set weights
var_name = struct_op.get_node_variable_name(name)
if var_name in list(options['weights'].keys()): # global variable
p_fix_num['p', 'weights', variable_type, name] = options['weights'][var_name]
else:
p_fix_num['p', 'weights', variable_type, name] = 1.0
# set references
if variable_type == 'u':
p_fix_num['p', 'ref', variable_type, :, name] = V_ref[variable_type, :, name]
elif variable_type == 'theta':
p_fix_num['p', 'ref', variable_type, name] = V_ref[variable_type, name]
elif variable_type in {'xd','xl','xa'}:
if variable_type in list(V_ref.keys()):
p_fix_num['p', 'ref', variable_type, :, name] = V_ref[variable_type, :, name]
if 'coll_var' in list(V_ref.keys()):
p_fix_num['p', 'ref', 'coll_var', :, :, variable_type, name] = V_ref['coll_var', :, :, variable_type, name]
# system parameters
param_options = options['initialization']['sys_params_num']
for param_type in list(param_options.keys()):
if isinstance(param_options[param_type],dict):
for param in list(param_options[param_type].keys()):
if isinstance(param_options[param_type][param],dict):
for subparam in list(param_options[param_type][param].keys()):
p_fix_num['theta0',param_type,param,subparam] = param_options[param_type][param][subparam]
else:
p_fix_num['theta0',param_type,param] = options['initialization']['sys_params_num'][param_type][param]
else:
p_fix_num['theta0',param_type] = param_options[param_type]
return p_fix_num
def collect_guess(interpolation_variables, states, model):
number_of_nodes = model.architecture.number_of_nodes
parent_nodes = model.architecture.parent_map
continuous_guess = {}
for name in struct_op.subkeys(model.variables, 'xd'):
continuous_guess[name] = 0.0
continuous_guess['e'] = 0.0
for state in list(states.keys()):
continuous_guess[state] = states[state]
continuous_guess['l_t'] = interpolation_variables['l_t']
continuous_guess['dl_t'] = interpolation_variables['dl_t']
continuous_guess['ddl_t'] = interpolation_variables['ddl_t']
return continuous_guess
def print_homotopy_values(nlp, solution, p_fix_num):
V = nlp.V
# print the phi values:
awelogger.logger.debug("{0:.<30}:".format('homotopy parameter values'))
for phi_val in struct_op.subkeys(V, 'phi'):
awelogger.logger.debug(" {0:>20} = {1:5}".format(
phi_val, str(V(solution['x'])['phi', phi_val])))
awelogger.logger.debug('')
# print the cost components
[cost_fun, cost_struct] = nlp.cost_components
awelogger.logger.debug("{0:.<30}:".format('objective components'))
for name in list(cost_fun.keys()):
if 'problem' not in name and 'objective' not in name:
awelogger.logger.debug(" {0:>20} = {1:5}".format(
name[0:-9], str(cost_fun[name](V(solution['x']), p_fix_num))))
awelogger.logger.debug('')
def __get_states(sstates_functions, model, interpolation_variables, sinterp):
"""Get states from interpolation variables
:type sstates_functions: dict
:param sstates_functions: casadi functions mapping interpolation variables to states
:type model: awebox.model_dir.model
:param model: system model
:type interpolation_variables: dict
:param interpolation_variables: variables defining the interpolation
:type sinterp: casadi.struct_symSX
:param sinterp: interpolation variables
:rtype: dict
"""
number_of_nodes = model.architecture.number_of_nodes
parent_nodes = model.architecture.parent_map
kite_dof = model.kite_dof
kite_nodes = model.architecture.kite_nodes
# initialize dict
states = {}
# fill sinterp with numerical values
ninterp = sinterp(0.0)
for key in list(sinterp.keys()):
for subkey in struct_op.subkeys(sinterp, key):
ninterp[key, subkey] = interpolation_variables[subkey]
for node in range(1, number_of_nodes):
node_str = str(node) + str(parent_nodes[node])
states['q' + node_str] = sstates_functions['q' + node_str](ninterp)
states['dq' + node_str] = sstates_functions['dq' + node_str](ninterp)
if int(kite_dof) == 6 and node in kite_nodes:
states['omega' + node_str] = sstates_functions['omega' +
node_str](ninterp)
states['r' + node_str] = sstates_functions['r' + node_str](ninterp)
return states
def make_periodicity_equality(initial_model_variables, terminal_model_variables, options):
periodicity_cstr = []
for name in struct_op.subkeys(initial_model_variables, 'xd'):
not_unselected_induction_model = variable_does_not_belong_to_unselected_induction_model(name, options)
if (not name[0] == 'e') and (not name[0] == 'w') and (not name[:2] == 'dw') and (not name[:3] == 'psi') and not_unselected_induction_model:
initial_value = vect_op.columnize(initial_model_variables['xd', name])
final_value = vect_op.columnize(terminal_model_variables['xd', name])
difference = initial_value - final_value
periodicity_cstr = cas.vertcat(periodicity_cstr, difference)
periodicity_eq = periodicity_cstr
return periodicity_eq
def make_periodicity_equality(initial_model_variables,
terminal_model_variables):
periodicity_cstr = []
for name in set(struct_op.subkeys(initial_model_variables, 'xd')):
if not name[0] == 'e' and not name[0] == 'w': # and not name[0] == 'a':
initial_value = vect_op.columnize(initial_model_variables['xd',
name])
final_value = vect_op.columnize(terminal_model_variables['xd',
name])
difference = initial_value - final_value
periodicity_cstr = cas.vertcat(periodicity_cstr, difference)
periodicity_eq = periodicity_cstr
return periodicity_eq