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