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)
