def get_cost_derivatives(V, P, f_fun): [H, g] = cas.hessian(f_fun, V) f_jacobian_fun = cas.Function('f_jacobian', [V, P], [g]) f_hessian_fun = cas.Function('f_hessian', [V, P], [H]) return [f_fun, f_jacobian_fun, f_hessian_fun]
def smooth_lagrange_poly(x, y): t = cas.SX.sym('t') d = len(x) # amount of parameters tau = cas.SX.sym('tau', d) # parameter as minimisation variable poly = 0 for j in range(d): # for all data points ... L = tau[j] for r in range(d): if r != j: L *= (t - x[r]) / (x[j] - x[r]) poly += L L_fun = cas.Function('L_fun', [t, tau], [poly]) ddL, _ = cas.hessian(poly, t) ddL_fun = cas.Function('ddL_fun', [t, tau], [ddL]) # ddL_fun = L_fun.hessian(0) # second order derivative to # [ddL,_,_] = ddL_fun([t,tau]) # minimise tau = fct output, incl penalize curvature res = 0.1 * sum([(L_fun(x[k], tau) - y[k])**2 for k in range(d)])[0] res += sum([ddL_fun(x[k], tau)[0]**2 * 1e5 for k in range(d)])[0] Cost = cas.Function('cost', [tau], [res]) nlp = {'x': tau, 'f': res} opts = {} opts['ipopt.print_level'] = 0 solver = cas.nlpsol("solver", "ipopt", nlp, opts) sol = solver(**{}) opts['ipopt.print_level'] = 0 tau_opt = sol['x'] # optimal parameter for polynomial return L_fun, tau_opt
def get_induced_velocity_at_kite(model_options, variables, parameters, architecture, kite, outputs): lin_params = parameters['lin'] var_sym = {} var_sym_cat = [] var_actual_cat = [] for var_type in variables.keys(): var_sym[var_type] = variables[var_type](cas.SX.sym( var_type, (variables[var_type].cat.shape))) var_sym_cat = cas.vertcat(var_sym_cat, var_sym[var_type].cat) var_actual_cat = cas.vertcat(var_actual_cat, variables[var_type].cat) columnized_list = outputs['vortex']['filament_list'] filament_list = vortex_filament_list.decolumnize(model_options, architecture, columnized_list) uind_sym = vortex_flow.get_induced_velocity_at_kite( model_options, filament_list, variables, architecture, kite) jac_sym = cas.jacobian(uind_sym, var_sym_cat) uind_fun = cas.Function('uind_fun', [var_sym_cat], [uind_sym]) jac_fun = cas.Function('jac_fun', [var_sym_cat], [jac_sym]) slope = jac_fun(lin_params) const = uind_fun(lin_params) uind_lin = cas.mtimes(slope, var_actual_cat) + const return uind_lin
def __poly_coeffs(self): """Compute coefficients of interpolating polynomials and their derivatives """ # discretization info nk = self.__n_k d = self.__d # choose collocation points tau_root = cas.vertcat(0.0, cas.collocation_points(d, self.__scheme)) # coefficients of the collocation equation coeff_collocation = np.zeros((d + 1, d + 1)) # coefficients of the continuity equation coeff_continuity = np.zeros(d + 1) # dimensionless time inside one control interval tau = cas.SX.sym('tau') # all collocation time points t = np.zeros((nk, d + 1)) for k in range(nk): for j in range(d + 1): t[k, j] = (k + tau_root[j]) # for all collocation points ls = [] for j in range(d + 1): # construct lagrange polynomials to get the polynomial basis at the # collocation point l = 1 for r in range(d + 1): if r != j: l *= (tau - tau_root[r]) / (tau_root[j] - tau_root[r]) lfcn = cas.Function('lfcn', [tau], [l]) ls = cas.vertcat(ls, l) # evaluate the polynomial at the final time to get the coefficients of # the continuity equation coeff_continuity[j] = lfcn([1.0]) # evaluate the time derivative of the polynomial at all collocation # points to get the coefficients of the continuity equation tfcn = cas.Function('lfcntan', [tau], [cas.jacobian(l, tau)]) for r in range(d + 1): coeff_collocation[j][r] = tfcn(tau_root[r]) # interpolating function for all polynomials lfcns = cas.Function('lfcns', [tau], [ls]) self.__coeff_continuity = coeff_continuity self.__coeff_collocation = coeff_collocation self.__coeff_fun = lfcns return None
def create_constraint_outputs(g_list, g_bounds, g_struct, V, P): g = g_struct(cas.vertcat(*g_list)) g_fun = cas.Function('g_fun',[V, P], [g.cat]) g_jacobian_fun = cas.Function('g_jacobian_fun',[V,P],[g.cat, cas.jacobian(g.cat, V.cat)]) g_bounds['lb'] = cas.vertcat(*g_bounds['lb']) g_bounds['ub'] = cas.vertcat(*g_bounds['ub']) return g, g_fun, g_jacobian_fun, g_bounds
def make_cost_function(V, P, component_costs): f = [] for cost in list(component_costs.keys()): f = cas.vertcat(f, component_costs[cost]) f = cas.sum1(f) f_fun = cas.Function('f', [V, P], [f]) [H, g] = cas.hessian(f, V) f_jacobian_fun = cas.Function('f_jacobian', [V, P], [g]) f_hessian_fun = cas.Function('f_hessian', [V, P], [H]) return [f_fun, f_jacobian_fun, f_hessian_fun]
def generate_integral_constraints(options, variables, parameters, model): eqs_dict = {} ineqs_dict = {} constraint_list = [] integral_constants_list = [] ineqs_list = [] eqs_list = [] ineqs_constants_dict = {} [ periodic, initial_conditions, param_initial_conditions, param_terminal_conditions, terminal_inequalities, integral_constraints ] = get_operation_conditions(options) if integral_constraints: ineqs_dict['terminal_battery'] = make_terminal_battery_integrand( options, variables, parameters, model) constraint_list.append(ineqs_dict['terminal_battery']) ineqs_list.append(ineqs_dict['terminal_battery']) # generate integral constraints - empty struct containing both equalities and inequalitiess integral_constraints_struct = make_constraint_struct(eqs_dict, ineqs_dict) integral_constraints_eqs_struct = make_constraint_struct(eqs_dict, []) integral_constraints_ineqs_struct = make_constraint_struct([], ineqs_dict) integral_constraints = integral_constraints_struct(*constraint_list) integral_ineqs_constraints = integral_constraints_ineqs_struct(*ineqs_list) integral_eqs_constraints = integral_constraints_eqs_struct(*eqs_list) integral_constraints_fun = {} integral_constraints_fun['inequality'] = cas.Function( 'integral_fun', [variables, parameters], [integral_ineqs_constraints.cat]) integral_constraints_fun['equality'] = cas.Function( 'integral_fun', [variables, parameters], [integral_eqs_constraints.cat]) # I(tf) = I(0) + int(dI) < I_margin # I(tf) - I_margin + int(dI) # I(tf) - I_margin = I_const if list(integral_constraints.keys()): ineqs_constants_dict[ 'terminal_battery'] = make_terminal_battery_constant(options) integral_constants_list.append( ineqs_constants_dict['terminal_battery']) integral_constants = integral_constraints_struct(*integral_constants_list) return integral_constraints_struct, integral_constraints_fun, integral_constants
def collect_equality_and_active_inequality_constraints(health_solver_options, nlp, solution, arg): var_sym = cas.SX.sym('var_sym', nlp.V.shape) p_sym = cas.SX.sym('p_sym', nlp.P.shape) ubx_sym = cas.SX.sym('ubx_sym', arg['ubx'].shape) lbx_sym = cas.SX.sym('lbx_sym', arg['lbx'].shape) lam_x_sym = cas.SX.sym('lam_x_sym', solution['lam_x'].shape) lam_g_sym = cas.SX.sym('lam_g_sym', solution['lam_g'].shape) p_fix_num = nlp.P(arg['p']) var_constraint_functions = collect_var_constraints(health_solver_options, nlp, arg, solution) [equality_constraints, eq_labels, eq_fun] = collect_equality_constraints(nlp) [active_inequality_constraints, active_ineq_labels, active_fun ] = collect_active_inequality_constraints(health_solver_options, nlp, solution, p_fix_num) equality_constraints = eq_fun(nlp.g_fun(var_sym, p_sym)) active_inequality_constraints = active_fun(nlp.g_fun(var_sym, p_sym)) equality_lambdas = eq_fun(lam_g_sym) active_inequality_lambdas = active_fun(lam_g_sym) all_active_var_bounds = var_constraint_functions['all_act_fun'](var_sym, lbx_sym, ubx_sym) all_active_var_lambdas = var_constraint_functions['all_act_lam_fun']( lam_x_sym) all_active_var_labels = var_constraint_functions['all_act_labels'] stacked_constraints = cas.vertcat(equality_constraints, active_inequality_constraints, all_active_var_bounds) stacked_cstr_fun = cas.Function('stacked_cstr_fun', [var_sym, p_sym, lbx_sym, ubx_sym], [stacked_constraints]) stacked_lambdas = cas.vertcat(equality_lambdas, active_inequality_lambdas, all_active_var_lambdas) stacked_lam_fun = cas.Function('stacked_lam_fun', [lam_x_sym, lam_g_sym], [stacked_lambdas]) stacked_labels = eq_labels + active_ineq_labels + all_active_var_labels return stacked_cstr_fun, stacked_lam_fun, stacked_labels
def generate_periodic_constraints(options, initial_model_variables, terminal_model_variables): eqs_dict = {} ineqs_dict = {} constraint_list = [] [ periodic, initial_conditions, param_initial_conditions, param_terminal_conditions, terminal_inequalities, integral_constraints ] = get_operation_conditions(options) # list all periodic equalities ==> put SX expressions in dict if periodic: eqs_dict['state_periodicity'] = make_periodicity_equality( initial_model_variables, terminal_model_variables) constraint_list.append(eqs_dict['state_periodicity']) # list all periodic inequalities ==> put SX expressions in dict # generate periodic constraints - empty struct periodic_constraints_struct = make_constraint_struct(eqs_dict, ineqs_dict) # fill in struct and create function periodic_constraints = periodic_constraints_struct( cas.vertcat(*constraint_list)) periodic_constraints_fun = cas.Function( 'periodic_constraints_fun', [initial_model_variables, terminal_model_variables], [periodic_constraints.cat]) return periodic_constraints_struct, periodic_constraints_fun
def collect_type_constraints(nlp, is_equality): constraints = [] list_names = [] constraint_sym = [] # list the evaluated constraints at solution g = nlp.g g_sym = cas.SX.sym('g_sym', g.shape) for gdx in range(g.shape[0]): cstr_name = g.getCanonicalIndex(gdx) condition = 'inequality' in cstr_name if is_equality: condition = not condition if condition: constraints = cas.vertcat(constraints, g.cat[gdx]) constraint_sym = cas.vertcat(constraint_sym, g_sym[gdx]) name_list_strings = list(map(str, cstr_name)) name_list = [name + '_' for name in name_list_strings[:-1] ] + [name_list_strings[-1]] list_names += [''.join(name_list)] cstr_fun = cas.Function('cstr_fun', [g_sym], [constraint_sym]) return constraints, list_names, cstr_fun
def make_symbolic_filament_and_sum(options, filament_list, include_normal_info=False): epsilon = options['induction']['vortex_epsilon'] # define the symbolic function n_symbolics = filament_list.shape[0] u_ind = cas.DM.zeros((3, 1)) if n_symbolics > 0: seg_data_sym = cas.SX.sym('seg_data_sym', (n_symbolics, 1)) if include_normal_info: filament_sym = biot_savart.filament_normal(seg_data_sym, epsilon=epsilon) else: filament_sym = biot_savart.filament(seg_data_sym, epsilon=epsilon) filament_fun = cas.Function('filament_fun', [seg_data_sym], [filament_sym]) # evaluate the symbolic function u_ind = vortex_tools.evaluate_symbolic_on_segments_and_sum( filament_fun, filament_list) return u_ind
def collect_type_constraints(nlp, cstr_type): found_cstrs = [] found_names = [] found_syms = [] # list the evaluated constraints at solution ocp_cstr_list = nlp.ocp_cstr_list name_list = ocp_cstr_list.get_name_list('all') g = nlp.g g_sym = cas.SX.sym('g_sym', g.shape) for cstr in ocp_cstr_list.get_list('all'): local_name = cstr.name if cstr.cstr_type == cstr_type: indices = [ idx for idx, name in enumerate(name_list) if name == local_name ] for idx in indices: found_cstrs = cas.vertcat(found_cstrs, g[idx]) found_syms = cas.vertcat(found_syms, g_sym[idx]) found_names += [local_name] cstr_fun = cas.Function('cstr_fun', [g_sym], [found_syms]) return found_cstrs, found_names, cstr_fun
def __create_tracking_cost_fun(self, W): """ Create casadi.Function to compute tracking cost at one time instant. """ w = ct.SX.sym('w',W.shape[0]) w_ref = ct.SX.sym('w_ref', W.shape[0]) return ct.Function('tracking_cost', [w, w_ref], [ct.mtimes((w-w_ref).T,(w-w_ref))])
def build_integrator(self, options, model): print('Building integrator...') # get model variables variables = model.variables # construct the DAE variables x = cas.struct_SX([cas.entry('xd', expr = variables['xd'])]) # differential states z = cas.struct_SX([cas.entry('xddot', expr = variables['xddot']), # state derivatives cas.entry('xa', expr = variables['xa']), # algebraic variables cas.entry('xl', expr = variables['xl']), # lifted variables ]) p = cas.struct_SX([cas.entry('u', expr = variables['u']), # dae parameters cas.entry('theta', expr = variables['theta']), cas.entry('phi', expr = model.parameters)]) # scale xddot with t_f time_scaled_variables = scale_xddot(variables) # model equations alg = model.dynamics(time_scaled_variables, model.parameters) ode = variables['xddot'] # create dae dae = {'x': x.cat, 'z': z.cat, 'p': p.cat, 'alg': alg,'ode': ode} # system dynamics f = cas.Function('f', [x, z, p], [ode, alg], ['x', 'z', 'p'], ['ode', 'alg']) # create integrator and rootfinder if cas.sprank(cas.jacobian(alg,z)) < z.cat.size()[0]: # check dae index raise ValueError('jacobian of dynamics is structurally rank-deficient: DAE is not of index 1!') else: # create integrator I = cas.integrator('I', options['integrator']['type'], dae, {'tf': 1.0 / self.__N_sim}) # create rootfinder g = cas.Function('g',[z.cat,x.cat,p.cat],[alg]) G = cas.rootfinder('G', 'newton', g, {'linear_solver': 'csparse'}) self.__integrator = I self.__rootfinder = G self.__variables_dict = model.variables_dict self.__phi = model.parameters self.__dae = dae self.__f = f self.__x = x self.__z = z self.__p = p
def get_component_cost_function(component_costs, V, P): component_cost_fun = {} for name in list(component_costs.keys()): component_cost_fun[name + '_fun'] = cas.Function( name + '_fun', [V, P], [component_costs[name]]) return component_cost_fun
def get_element_drag_and_moment_fun(wind, atmos, cd_tether_fun): info_sym = cas.SX.sym('info_sym', (16, 1)) q_upper = info_sym[:3] q_lower = info_sym[3:6] dq_upper = info_sym[6:9] dq_lower = info_sym[9:12] diam = info_sym[12] q_seg_avg = info_sym[13:16] q_average = (q_upper + q_lower) / 2. zz = q_average[2] ua = get_uapp(q_upper, q_lower, dq_upper, dq_lower, wind) epsilon = 1.e-6 ua_norm = vect_op.smooth_norm(ua, epsilon) ehat_ua = vect_op.smooth_normalize(ua, epsilon) tether = q_upper - q_lower length_sq = cas.mtimes(tether.T, tether) length_parallel_to_wind = cas.mtimes(tether.T, ehat_ua) length_perp_to_wind = vect_op.smooth_sqrt( length_sq - length_parallel_to_wind**2., epsilon) re_number = reynolds.get_reynolds_number(atmos, ua, diam, q_upper, q_lower) cd = cd_tether_fun(re_number) density = atmos.get_density(zz) drag = cd * 0.5 * density * ua_norm * diam * length_perp_to_wind * ua moment_arm = q_average - q_seg_avg moment = vect_op.cross(moment_arm, drag) element_drag_fun = cas.Function('element_drag_fun', [info_sym], [drag]) element_moment_fun = cas.Function('element_moment_fun', [info_sym], [moment]) return element_drag_fun, element_moment_fun
def build_rootfinder(self): """ Create rootfinder function for fully implicit dae object. """ # create rootfinder g = cas.Function('g',[self.__z.cat,self.__x.cat,self.__p.cat],[self.__dae['alg']]) G = cas.rootfinder('G', 'fast_newton', g, {'jit': True}) self.__rootfinder = G return None
def make_cost_function(V, P, component_costs): ccc = component_costs.cat f = 0. for idx in range(ccc.shape[0]): f += ccc[idx] # f = cas.sum1(component_costs.cat) f_fun = cas.Function('f', [V, P], [f]) return f_fun
def get_component_cost_function(component_costs, V, P): ccc = component_costs.cat component_cost_fun = {} for idx in range(ccc.shape[0]): local_expr = ccc[idx] local_name = component_costs.keys()[idx] component_cost_fun[local_name + '_fun'] = cas.Function( local_name + '_fun', [V, P], [local_expr]) return component_cost_fun
def rk4root(name, dae, rootfinder, options): """explicit RK4 integrator for DAE systems, using rootfinder for the algebraic equations @param name integrator name @param dae dae object @param rootfinder newton solver for algebraic equations @param options integrator options @return I rk4root integrator function """ # discretization info N = options['number_of_finite_elements'] h = options['tf'] / N # dae functions odef = cas.Function('odef', [dae['x'], dae['p'], dae['z']], [dae['ode']]) quadf = cas.Function('quadf', [dae['x'], dae['p'], dae['z']], [dae['quad']]) # initialize x0 = dae['x'](cas.MX.sym('x', dae['x'].shape)) z0 = dae['z'](cas.MX.sym('z', dae['z'].shape)) p = dae['p'](cas.MX.sym('p', dae['p'].shape)) qf = 0.0 for i in range(N): # first iteration starts with x0 if i == 0: xf = x0 #rk4 with rootfinder step [xf, zf, qf] = rk4root_step(odef, rootfinder, quadf, h, xf, z0, p, qf) I = cas.Function(name, [x0, z0, p], [xf, zf, qf], ['x0', 'z0', 'p'], ['xf', 'zf', 'qf']) return I
def get_local_slack_penalty_function(): var_sym = cas.SX.sym('var_sym', (1, 1)) ref_sym = cas.SX.sym('ref_sym', (1, 1)) weight_sym = cas.SX.sym('weight_sym', (1, 1)) diff = (var_sym - ref_sym) slack = weight_sym * diff slack_fun = cas.Function('slack_fun', [var_sym, ref_sym, weight_sym], [slack]) return slack_fun
def generate_lagrangian(health_solver_options, nlp, arg, solution): awelogger.logger.info('compute the lagrangian...') vars_sym = cas.SX.sym('vars_sym', nlp.V.shape) p_sym = cas.SX.sym('p_sym', nlp.P.shape) lam_x_sym = cas.SX.sym('lam_x_sym', nlp.V.shape) constraints = nlp.g_fun(vars_sym, p_sym) lam_g_sym = cas.SX.sym('lam_g_sym', constraints.shape) var_constraint_functions = collect_var_constraints(health_solver_options, nlp, arg, solution) rel_fun = var_constraint_functions['rel_fun'] rel_lam_fun = var_constraint_functions['rel_lam_fun'] bounds = rel_fun(vars_sym, arg['lbx'], arg['ubx']) lam_x_bounds = rel_lam_fun(lam_x_sym) f_fun = nlp.f_fun objective = f_fun(vars_sym, p_sym) lagrangian = objective + cas.mtimes(lam_g_sym.T, constraints) + cas.mtimes( lam_x_bounds.T, bounds) [lagr_hessian, lagr_jacobian] = cas.hessian(lagrangian, vars_sym) lagr_fun = cas.Function('lagr_fun', [vars_sym, p_sym, lam_g_sym, lam_x_sym], [lagrangian]) lagr_jacobian_fun = cas.Function('lagr_jacobian_fun', [vars_sym, p_sym, lam_g_sym, lam_x_sym], [lagr_jacobian]) lagr_hessian_fun = cas.Function('lagr_hessian_fun', [vars_sym, p_sym, lam_g_sym, lam_x_sym], [lagr_hessian]) return lagr_fun, lagr_jacobian_fun, lagr_hessian_fun
def get_segment_half_fun(): seg_info_sym = cas.SX.sym('seg_info_sym', (12, 1)) drag_earthfixed = seg_info_sym[6:9] force_upper = drag_earthfixed / 2. force_lower = drag_earthfixed / 2. forces = cas.vertcat(force_upper, force_lower) segment_half_fun = cas.Function('segment_half_fun', [seg_info_sym], [forces]) return segment_half_fun
def lagrange_poly(x, y): "creates an lagrange polynomial through each data point based on x and y data" t = cas.SX.sym('t') d = x.shape[0] # amount of parameters poly = 0 for j in range(d): # for all data points ... L = y[j] # parameter = fct output for r in range(d): if r != j: L *= (t - x[r]) / (x[j] - x[r]) poly += L lfcn = cas.Function('lfcn', [t], [poly]) return lfcn
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 sosc_check(health_solver_options, nlp, solution, arg): logging.debug('sosc check...') V = nlp.V P = nlp.P V_vals = V(solution['x']) lambda_g_vals = solution['lam_g'] lambda_x_vals = solution['lam_x'] p_fix_num = nlp.P(arg['p']) logging.debug('get constraints...') [stacked_cstr_fun, stacked_lam_fun, stacked_labels] = collect_equality_and_active_inequality_constraints( health_solver_options, nlp, solution, arg) relevant_constraints = stacked_cstr_fun(V, P, arg['lbx'], arg['ubx']) logging.debug('get jacobian...') linearization = cas.jacobian(relevant_constraints, V) linearization_fun = cas.Function('linearization_fun', [V, P], [linearization]) linearization_eval = np.array(linearization_fun(V_vals, p_fix_num)) logging.debug('find the null space...') pdb.set_trace() null = vect_op.null( linearization_eval, health_solver_options['sosc']['reduced_hessian_null_tol']) logging.debug('make lagrangian') [lagr_fun, lagr_jacobian_fun, lagr_hessian_fun] = generate_lagrangian(health_solver_options, nlp, arg, solution) lagr_hessian = lagr_hessian_fun(V_vals, p_fix_num, lambda_g_vals, lambda_x_vals) logging.debug('get reduced hessian') reduced_hessian = cas.mtimes(cas.mtimes(null.T, lagr_hessian), null) logging.debug('rank check on reduced hessian') full_rank_check(health_solver_options, reduced_hessian, 'reduced hessian', 'SOSC')
def get_wake_fix_constraints(options, variables, model): # this function is just the placeholder. For the applied constraint, see constraints.append_wake_fix_constraints() ineqs_dict = {} eqs_dict, constraint_list = vortex_fix.get_cstr_in_operation_format(options, variables, model, Collocation) # generate initial constraints - empty struct containing both equalities and inequalitiess wake_fix_constraints_struct = make_constraint_struct(eqs_dict, ineqs_dict) # fill in struct and create function wake_fix_constraints = wake_fix_constraints_struct(cas.vertcat(*constraint_list)) wake_fix_constraints_fun = cas.Function('wake_fix_constraints_fun', [variables], [wake_fix_constraints.cat]) return wake_fix_constraints, wake_fix_constraints_fun
def collect_active_inequality_constraints(health_solver_options, nlp, solution, p_fix_num): active_threshold = health_solver_options['thresh']['active'] v_vals = solution['x'] active_constraints = [] list_names = [] active_sym = [] [g_ineq, g_ineq_names, ineq_fun] = collect_inequality_constraints(nlp) # list the evaluated constraints at solution ocp_cstr_list = nlp.ocp_cstr_list g = nlp.g g_fun = nlp.g_fun g_vals = g_fun(v_vals, p_fix_num) g_sym = cas.SX.sym('g_sym', g.shape) g_names = ocp_cstr_list.get_name_list('all') # list the multipliers lambda at solution lam_vals = solution['lam_g'] g_ineq_vals = ineq_fun(g_vals) lambda_ineq_vals = ineq_fun(lam_vals) g_ineq_sym = ineq_fun(g_sym) if not g_ineq_sym.shape[0] == 0: for gdx in range(g_ineq.shape[0]): local_g = g_ineq_vals[gdx] local_lam = lambda_ineq_vals[gdx] local_name = g_ineq_names[gdx] # if eval_constraint is small, then constraint is active. or. # if lambda >> eval_constraint, then: constraint is active if local_lam**2. > (active_threshold * local_g)**2.: # append active constraints to active_list active_constraints = cas.vertcat(active_constraints, local_g) list_names += [local_name] active_sym = cas.vertcat(active_sym, g_ineq_sym[gdx]) active_fun = cas.Function('active_fun', [g_sym], [active_sym]) # return active_list return active_constraints, list_names, active_fun
def get_general_regularization_function(variables): weight_sym = cas.SX.sym('weight_sym', variables.cat.shape) var_sym = cas.SX.sym('var_sym', variables.cat.shape) ref_sym = cas.SX.sym('ref_sym', variables.cat.shape) diff = (var_sym - ref_sym) weight = cas.diag(weight_sym) diff_sq = cas.mtimes(cas.diag(diff), diff) reg = cas.mtimes(weight, diff_sq) reg_fun = cas.Function('reg_fun', [var_sym, ref_sym, weight_sym], [reg]) return reg_fun
def get_function(self, options, relevant_variables, relevant_parameters, cstr_type): expr_list = self.get_expression_list(cstr_type) # constraints function options if options['jit_code_gen']['include']: opts = { 'jit': True, 'compiler': options['jit_code_gen']['compiler'] } else: opts = {} # create function return cas.Function('cstr_fun', [relevant_variables, relevant_parameters], [expr_list], opts)