def get_gamma_residual(model_options, wind, parent, variables, parameters,
architecture):
uzero_hat_var = get_uzero_hat_var(variables, parent)
vzero_hat_var = get_vzero_hat_var(variables, parent)
n_hat_var = general_geom.get_n_hat_var(variables, parent)
u_comp = cas.mtimes(n_hat_var.T, uzero_hat_var)
v_comp = cas.mtimes(n_hat_var.T, vzero_hat_var)
gamma_var = get_gamma_var(variables, parent)
cosgamma_var = get_cosgamma_var(variables, parent)
singamma_var = get_singamma_var(variables, parent)
g_vec_length_var = get_g_vec_length_var(variables, parent)
f_cosproj = g_vec_length_var * cosgamma_var - u_comp
f_sinproj = g_vec_length_var * singamma_var - v_comp
f_cos = np.cos(gamma_var) - cosgamma_var
f_sin = np.sin(gamma_var) - singamma_var
resi = cas.vertcat(f_cos, f_sin, f_cosproj, f_sinproj)
return resi
def draw_kite_fuselage(ax, q, r, length, kite_color, side, num_per_meter, naca="0006"):
r_dcm = np.array(cas.reshape(r, (3, 3)))
total_width = np.float(naca[2:]) / 100. * length
num_spanwise = np.ceil(total_width * num_per_meter / 2.)
ypos = np.arange(-1. * num_spanwise, num_spanwise + 1.) / num_spanwise / 2.
for y in ypos:
yloc = cas.mtimes(r_dcm, vect_op.yhat_np()) * y * total_width
basic_shell = get_naca_shell(length, naca) * (1 - (2. * y)**2.)
horizontal_shell = []
for idx in range(basic_shell[:, 0].shape[0]):
new_point = q + yloc + np.array(cas.mtimes(r_dcm, basic_shell[idx, :].T))
horizontal_shell = cas.vertcat(horizontal_shell, new_point.T)
horizontal_shell = np.array(horizontal_shell)
make_side_plot(ax, horizontal_shell, side, kite_color)
def rotation_matrix(theta, phi, psi):
# rotation angles are defined positive, for clockwise rotation when
# looking from origin along positive axis
ori_pre_rot = np.eye(3)
rot_x = cas.MX.zeros(3, 3)
rot_x[0, 0] = 1.0
rot_x[1, 1] = np.cos(phi)
rot_x[1, 2] = np.sin(phi)
rot_x[2, 1] = -1.0 * np.sin(phi)
rot_x[2, 2] = np.cos(phi)
rot_y = cas.MX.zeros(3, 3)
rot_y[0, 0] = np.cos(theta)
rot_y[0, 2] = -1.0 * np.sin(theta)
rot_y[1, 1] = 1.0
rot_y[2, 0] = np.sin(theta)
rot_y[2, 2] = np.cos(theta)
rot_z = cas.MX.zeros(3, 3)
rot_z[0, 0] = np.cos(psi)
rot_z[0, 1] = np.sin(psi)
rot_z[1, 0] = -1.0 * np.sin(psi)
rot_z[1, 1] = np.cos(psi)
rot_z[2, 2] = 1.0
ori_post_rot = cas.mtimes(cas.mtimes(rot_x, rot_y),
cas.mtimes(rot_z, ori_pre_rot))
return ori_post_rot
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 get_radius_of_curvature(variables, kite, parent):
dq = variables['xd']['dq' + str(kite) + str(parent)]
ddq = variables['xddot']['ddq' + str(kite) + str(parent)]
gamma_dot = dq
gamma_ddot = ddq
# from frenet vectors + curvature definition
# r = || gamma' || / (e1' cdot e2)
# e1 = gamma' / || gamma' ||
# e1' = ( gamma" || gamma' ||^2 - gamma' (gamma' cdot gamma") ) / || gamma' ||^3
# e2 = ebar2 / || ebar2 ||
# ebar2 = gamma" - (gamma' cdot gamma") gamma' / || gamma' ||^2
# ....
# r = || gamma' ||^4 // || gamma" || gamma' ||^2 - gamma' (gamma' cdot gamma") ||
num = cas.mtimes(gamma_dot.T, gamma_dot)**2. + 1.0e-8
den_vec = gamma_ddot * cas.mtimes(gamma_dot.T,
gamma_dot) - gamma_dot * cas.mtimes(
gamma_dot.T, gamma_ddot)
den = vect_op.smooth_norm(den_vec)
radius = num / den
return radius
def get_radius_inequality(model_options, variables, kite, parent, parameters):
# no projection included...
b_ref = parameters['theta0', 'geometry', 'b_ref']
half_span = b_ref / 2.
num_ref = model_options['model_bounds']['anticollision_radius']['num_ref']
# half_span - radius < 0
# half_span * den - num < 0
dq = variables['xd']['dq' + str(kite) + str(parent)]
ddq = variables['xddot']['ddq' + str(kite) + str(parent)]
gamma_dot = cas.vertcat(0., dq[1], dq[2])
gamma_ddot = cas.vertcat(0., ddq[1], ddq[2])
num = cas.mtimes(gamma_dot.T, gamma_dot)**2.
den_vec = gamma_ddot * cas.mtimes(gamma_dot.T,
gamma_dot) - gamma_dot * cas.mtimes(
gamma_dot.T, gamma_ddot)
den = vect_op.norm(den_vec)
inequality = (half_span * den - num) / num_ref
return inequality
def collect_power_balance_outputs(options, architecture, variables,
base_aerodynamic_quantities, outputs):
kite = base_aerodynamic_quantities['kite']
if 'power_balance' not in list(outputs.keys()):
# initialize
outputs['power_balance'] = {}
# kite velocity
parent = architecture.parent_map[kite]
dq = variables['xd']['dq' + str(kite) + str(parent)]
f_lift_earth = outputs['aerodynamics']['f_lift_earth' + str(kite)]
f_drag_earth = outputs['aerodynamics']['f_drag_earth' + str(kite)]
f_side_earth = outputs['aerodynamics']['f_side_earth' + str(kite)]
# get lift, drag and aero-moment power
outputs['power_balance']['P_lift' + str(kite)] = cas.mtimes(
f_lift_earth.T, dq)
outputs['power_balance']['P_drag' + str(kite)] = cas.mtimes(
f_drag_earth.T, dq)
outputs['power_balance']['P_side' + str(kite)] = cas.mtimes(
f_side_earth.T, dq)
if int(options['kite_dof']) == 6:
omega = variables['xd']['omega' + str(kite) + str(parent)]
m_aero_body = outputs['aerodynamics']['m_aero_body' + str(kite)]
outputs['power_balance']['P_moment' + str(kite)] = cas.mtimes(
m_aero_body.T, omega)
return outputs
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 coll_interpolator(time_grid, name, dim, var_type):
"""Interpolating function
@param time_grid list with time points
@param name xd variable name
@param dim xd variable dimension index
"""
vals = []
for t in time_grid:
kdx, tau = struct_op.calculate_kdx(nlp_params, V, t)
if var_type == 'xd':
poly_vars = cas.vertcat(
V['xd', kdx, name, dim], *V['coll_var', kdx, :, 'xd',
name, dim])
vals = cas.vertcat(
vals, cas.mtimes(poly_vars.T, self.__coeff_fun(tau)))
elif var_type in ['u', 'xa', 'xl']:
poly_vars = cas.vertcat(*V['coll_var', kdx, :, var_type,
name, dim])
vals = cas.vertcat(
vals, cas.mtimes(poly_vars.T, self.__coeff_fun_u(tau)))
elif var_type in ['int_out']:
poly_vars = cas.vertcat(
integral_outputs['int_out', kdx, name, dim],
*integral_outputs['coll_int_out', kdx, :, name, dim])
vals = cas.vertcat(
vals, cas.mtimes(poly_vars.T, self.__coeff_fun(tau)))
return vals
def add_node_kinetic(node, options, node_masses_si, variables_si, parameters, outputs, architecture):
parent = architecture.parent_map[node]
label = str(node) + str(parent)
node_has_a_kite = node in architecture.kite_nodes
kites_have_6dof = int(options['kite_dof']) == 6
node_has_rotational_energy = node_has_a_kite and kites_have_6dof
mass = node_masses_si['m' + label]
dq = variables_si['xd']['dq' + label]
e_kin_trans = 0.5 * mass * cas.mtimes(dq.T, dq)
if node_has_rotational_energy:
omega = variables_si['xd']['omega' + label]
j_kite = parameters['theta0', 'geometry', 'j']
e_kin_rot = 0.5 * cas.mtimes(cas.mtimes(omega.T, j_kite), omega)
else:
e_kin_rot = cas.DM.zeros((1, 1))
e_kinetic = e_kin_trans + e_kin_rot
outputs['e_kinetic']['q' + label] = e_kinetic
return outputs
def draw_lifting_surface(ax,
q,
r,
b_ref,
c_tipn,
c_root,
c_tipp,
kite_color,
side,
body_cross_sections_per_meter,
naca="0012"):
r_dcm = np.array(cas.reshape(r, (3, 3)))
num_spanwise = np.ceil(b_ref * body_cross_sections_per_meter / 2.)
ypos = np.arange(-1. * num_spanwise, num_spanwise + 1.) / num_spanwise / 2.
leading_edges = []
trailing_edges = []
for y in ypos:
yloc = cas.mtimes(r_dcm, vect_op.yhat_np()) * y * b_ref
s = np.abs(y) / 0.5 # 1 at tips and 0 at root
if y < 0:
c_local = c_root * (1. - s) + c_tipn * s
else:
c_local = c_root * (1. - s) + c_tipp * s
basic_shell = get_naca_shell(c_local, naca)
basic_leading_ege = basic_shell[np.argmin(basic_shell[:, 0]), :]
basic_trailing_ege = basic_shell[np.argmax(basic_shell[:, 0]), :]
new_leading_edge = q + yloc + np.array(
cas.mtimes(r_dcm, basic_leading_ege.T))
new_trailing_edge = q + yloc + np.array(
cas.mtimes(r_dcm, basic_trailing_ege.T))
leading_edges = cas.vertcat(leading_edges, new_leading_edge.T)
trailing_edges = cas.vertcat(trailing_edges, new_trailing_edge.T)
horizontal_shell = []
for idx in range(basic_shell[:, 0].shape[0]):
new_point = q + yloc + np.array(
cas.mtimes(r_dcm, basic_shell[idx, :].T))
horizontal_shell = cas.vertcat(horizontal_shell, new_point.T)
horizontal_shell = np.array(horizontal_shell)
make_side_plot(ax, horizontal_shell, side, kite_color)
make_side_plot(ax, leading_edges, side, kite_color)
make_side_plot(ax, trailing_edges, side, kite_color)
return None
def get_tether_length_constraint(options, vars_si, parameters, architecture):
xd_si = vars_si['xd']
theta_si = vars_si['theta']
g_dict = {}
number_of_nodes = architecture.number_of_nodes
parent_map = architecture.parent_map
kite_nodes = architecture.kite_nodes
com_attachment = (options['tether']['attachment'] == 'com')
stick_attachment = (options['tether']['attachment'] == 'stick')
kite_has_6dof = (int(options['kite_dof']) == 6)
if not (com_attachment or stick_attachment):
message = 'Unknown tether attachment option: {}'.format(options['tether']['attachment'])
awelogger.logger.error(message)
raise Exception(message)
for node in range(1, number_of_nodes):
parent = parent_map[node]
q_si = xd_si['q' + str(node) + str(parent)]
node_is_a_kite = (node in kite_nodes)
has_extended_arm = node_is_a_kite and stick_attachment
if has_extended_arm and not kite_has_6dof:
message = 'Stick tether attachment option not implemented for 3DOF kites'
awelogger.logger.error(message)
raise Exception(message)
if has_extended_arm:
dcm = cas.reshape(xd_si['r{}{}'.format(node, parent)], (3, 3))
current_node = q_si + cas.mtimes(dcm, parameters['theta0', 'geometry', 'r_tether'])
else:
current_node = q_si
if node == 1:
previous_node = cas.DM.zeros((3, 1))
segment_length = xd_si['l_t']
elif node in kite_nodes:
grandparent = parent_map[parent]
previous_node = xd_si['q' + str(parent) + str(grandparent)]
segment_length = theta_si['l_s']
else:
grandparent = parent_map[parent]
previous_node = xd_si['q' + str(parent) + str(grandparent)]
segment_length = theta_si['l_i']
# holonomic constraint
seg_vector = (current_node - previous_node)
length_constraint = 0.5 * (cas.mtimes(seg_vector.T, seg_vector) - segment_length ** 2.0)
g_dict['c' + str(node) + str(parent)] = length_constraint
return g_dict
def get_reduced_hessian(health_solver_options, cstr_jacobian_eval,
lagr_hessian_eval):
awelogger.logger.info('compute reduced hessian...')
null_tolerance = health_solver_options['tol']['reduced_hessian_null']
null = scila.null_space(cstr_jacobian_eval, null_tolerance)
reduced_hessian = cas.mtimes(cas.mtimes(null.T, lagr_hessian_eval), null)
return reduced_hessian
def from_earthfixed_to_body(earthfixed_vector, q_upper, q_lower):
[ehat_x, ehat_y, ehat_z] = get_body_axes(q_upper, q_lower)
body_x = cas.mtimes(earthfixed_vector.T, ehat_x)
body_y = cas.mtimes(earthfixed_vector.T, ehat_y)
body_z = cas.mtimes(earthfixed_vector.T, ehat_z)
body_vector = cas.vertcat(body_x, body_y, body_z)
return body_vector
def generate_rotational_dynamics(variables, f_nodes, parameters, outputs,
architecture):
kite_nodes = architecture.kite_nodes
parent_map = architecture.parent_map
j_inertia = parameters['theta0', 'geometry', 'j']
xd = variables['SI']['xd']
xddot = variables['SI']['xddot']
cstr_list = mdl_constraint.MdlConstraintList()
for kite in kite_nodes:
parent = parent_map[kite]
moment = f_nodes['m' + str(kite) + str(parent)]
rlocal = cas.reshape(xd['r' + str(kite) + str(parent)], (3, 3))
drlocal = cas.reshape(xddot['dr' + str(kite) + str(parent)], (3, 3))
omega = xd['omega' + str(kite) + str(parent)]
omega_skew = vect_op.skew(omega)
domega = xddot['domega' + str(kite) + str(parent)]
tether_moment = outputs['tether_moments']['n{}{}'.format(kite, parent)]
# moment = J dot(omega) + omega x (J omega) + [tether moment which is zero if holonomic constraints do not depend on omega]
J_dot_omega = cas.mtimes(j_inertia, domega)
omega_cross_J_omega = vect_op.cross(omega,
cas.mtimes(j_inertia, omega))
omega_derivative = moment - (J_dot_omega + omega_cross_J_omega +
tether_moment)
rotational_2nd_law = omega_derivative / vect_op.norm(
cas.diag(j_inertia))
rotation_dynamics_cstr = cstr_op.Constraint(expr=rotational_2nd_law,
name='rotation_dynamics' +
str(kite),
cstr_type='eq')
cstr_list.append(rotation_dynamics_cstr)
# Rdot = R omega_skew -> R ( kappa/2 (I - R.T R) + omega_skew )
baumgarte = parameters['theta0', 'kappa_r']
orthonormality = baumgarte / 2. * (cas.DM_eye(3) -
cas.mtimes(rlocal.T, rlocal))
ref_frame_deriv_matrix = drlocal - (cas.mtimes(
rlocal, orthonormality + omega_skew))
ref_frame_derivative = cas.reshape(ref_frame_deriv_matrix, (9, 1))
ortho_cstr = cstr_op.Constraint(expr=ref_frame_derivative,
name='ref_frame_deriv' + str(kite),
cstr_type='eq')
cstr_list.append(ortho_cstr)
return cstr_list, outputs
def get_kite_radius_proj_squared(model_options, kite, variables, architecture):
radius_vec = get_kite_radius_vector(model_options, kite, variables, architecture)
parent = architecture.parent_map[kite]
nhat_vec = get_nhat_var(variables, parent)
hypotenuse_squared = cas.mtimes(radius_vec.T, radius_vec)
normal_squared = cas.mtimes(radius_vec.T, nhat_vec)**2.
radius_proj_squared = hypotenuse_squared - normal_squared
return radius_proj_squared
def get_beta(ua, r):
ehat1 = r[:, 0] # chordwise, from le to te
ehat2 = r[:, 1] # spanwise, from pe to ne
ehat3 = r[:, 2] # up
y_component = cas.mtimes(ua.T, ehat2)
x_component = vect_op.smooth_abs(cas.mtimes(ua.T, ehat1))
# x component had better be positive
beta = y_component / x_component
return beta
def get_beta(ua, r):
ehat1 = r[:, 0] # chordwise, from le to te
ehat2 = r[:, 1] # spanwise, from pe to ne
ehat3 = r[:, 2] # up
y_component = cas.mtimes(ua.T, ehat2)
x_component = vect_op.smooth_abs(cas.mtimes(ua.T, ehat1))
# x component had better be positive
# the small angle approximation of:
# beta = cas.arctan(y_component / x_component)
beta = y_component / x_component
return beta
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 test_horizontal():
name = 'horizontal'
xhat = vect_op.xhat_np()
yhat = vect_op.yhat_np()
zhat = vect_op.zhat_np()
ehat_x = vect_op.xhat_np()
ehat_y = vect_op.yhat_np()
ehat_z = vect_op.zhat_np()
q_upper = 5. * xhat
q_lower = 2. * xhat
dir = 'x'
transformed = from_earth_to_body(xhat, q_upper, q_lower)
reference = ehat_z
diff = transformed - reference
resi = cas.mtimes(diff.T, diff)
epsilon = 1.e-12
if resi > epsilon:
awelogger.logger.error('tether frame transformation test (' + name +
' ' + dir + ') gives error of size: ' +
str(resi))
dir = 'y'
transformed = from_earth_to_body(yhat, q_upper, q_lower)
reference = ehat_y
diff = transformed - reference
resi = cas.mtimes(diff.T, diff)
epsilon = 1.e-12
if resi > epsilon:
awelogger.logger.error('tether frame transformation test (' + name +
' ' + dir + ') gives error of size: ' +
str(resi))
dir = 'z'
transformed = from_earth_to_body(zhat, q_upper, q_lower)
reference = -1. * ehat_x
diff = transformed - reference
resi = cas.mtimes(diff.T, diff)
epsilon = 1.e-12
if resi > epsilon:
awelogger.logger.error('tether frame transformation test (' + name +
' ' + dir + ') gives error of size: ' +
str(resi))
return None
def __set_acados_reference(self):
for i in range(self.__N):
# periodic index
idx = (self.__index_acados + i) % self.__Nref
# reference
xref = np.squeeze(self.__ref[idx][:self.__nx], axis=1)
uref = np.squeeze(self.__ref[idx][self.__nx:self.__nx + self.__nu +
self.__ns],
axis=1)
if self.__type == 'tracking':
# construct output reference with gradient term
yref = np.squeeze(
ca.vertcat(xref,uref).full() - \
ct.mtimes(
np.linalg.inv(self.__Href[idx][0]), # inverse of weighting matrix
self.__qref[idx][0].T).full(), # gradient term
axis = 1
)
self.__acados_ocp_solver.set(i, 'yref', yref)
# update tuning matrix
self.__acados_ocp_solver.cost_set(i, 'W', self.__Href[idx][0])
# update constraint bounds
if self.__h is not None:
C = self.__S['C'][idx][:, :self.__nx]
D = self.__S['C'][idx][:, self.__nx:]
lg = -self.__S['e'][idx] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
ug = 1e8 - self.__S['e'][idx] + ct.mtimes(
C, xref).full() + ct.mtimes(D, uref).full()
self.__acados_ocp_solver.constraints_set(
i, 'lg', np.squeeze(lg, axis=1))
self.__acados_ocp_solver.constraints_set(
i, 'ug', np.squeeze(ug, axis=1))
# update terminal constraint
idx = (self.__index_acados + self.__N) % self.__Nref
x_term = np.squeeze(self.__p_operator(self.__ref[idx][:self.__nx]),
axis=1)
self.__acados_ocp_solver.set(self.__N, 'lbx', x_term)
self.__acados_ocp_solver.set(self.__N, 'ubx', x_term)
return None
def tracking_cost(nw):
""" Create tracking cost function for variables nw
"""
# reference parameters
w = ca.MX.sym('w', (nw, 1))
wref = ca.MX.sym('wref', (nw, 1))
H = ca.MX.sym('H', (nw, nw))
q = ca.MX.sym('H', (nw, 1))
# cost definition
dw = w - wref
obj = 0.5*ct.mtimes(dw.T, ct.mtimes(H, dw)) + ct.mtimes(q.T,dw)
return ca.Function('tracking_cost',[w, wref, H, q],[obj])
def momentum_correction(options, generalized_coordinates, system_variables,
node_masses, outputs, architecture):
"""Compute momentum correction for translational lagrangian dynamics of an open system.
Here the system is "open" because the main tether mass is changing in time. During reel-out,
momentum is injected in the system, and during reel-in, momentum is extracted.
It is assumed that the tether mass is concentrated at the main tether node.
See "Lagrangian Dynamics for Open Systems", R. L. Greene and J. J. Matese 1981, Eur. J. Phys. 2, 103.
@return: lagrangian_momentum_correction - correction term that can directly be added to rhs of transl_dyn
"""
# initialize
xgcdot = generalized_coordinates['scaled']['xgcdot'].cat
lagrangian_momentum_correction = cas.DM.zeros(xgcdot.shape)
use_wound_tether = options['tether']['use_wound_tether']
if not use_wound_tether:
for node in range(1, architecture.number_of_nodes):
label = str(node) + str(architecture.parent_map[node])
mass = node_masses['m' + label]
mass_flow = tools.time_derivative(mass, system_variables,
architecture)
# velocity of the mass particles leaving the system
velocity = system_variables['SI']['xd']['dq' + label]
# see formula in reference
lagrangian_momentum_correction += mass_flow * cas.mtimes(
velocity.T, cas.jacobian(velocity, xgcdot)).T
return lagrangian_momentum_correction
def __construct_sensitivities(self):
""" Construct NLP sensitivities
"""
# convenience
w = self.__w
p = self.__p
# cost function
self.__f_fun = ca.Function('f_fun', [w, p], [self.__f])
self.__jacf_fun = ca.Function('jacf_fun', [w, p],
[ca.jacobian(self.__f, self.__w)])
# constraints
self.__g_fun = ca.Function('g_fun', [w, p], [self.__g])
self.__jacg_fun = ca.Function('jacg_fun', [w, p],
[ca.jacobian(self.__g, self.__w)])
self.__gzeros = np.zeros((self.__g.shape[0], 1))
# exact hessian
lam_g = ca.MX.sym('lam_g', self.__g.shape)
lag = self.__f + ct.mtimes(lam_g.T, self.__g)
self.__jlag_fun = ca.Function('jLag', [w, p, lam_g],
[ca.jacobian(lag, w)])
if self.__options['hessian_approximation'] == 'exact':
self.__H_fun = ca.Function('H_fun', [w, p, lam_g],
[ca.hessian(lag, w)[0]])
else:
self.__H_fun = self.__options['hessian_approximation']
def find_compromised_battery_cost(nlp_numerics_options, V, P,
emergency_scenario, model):
n_k = nlp_numerics_options['n_k']
if (len(model.architecture.kite_nodes) == 1
or nlp_numerics_options['system_model']['kite_dof'] == 6
or emergency_scenario[0] != 'broken_battery'):
compromised_battery_cost = cas.DM(0.0)
elif emergency_scenario[0] == 'broken_battery':
actuator_len = V['u', 0, 'dcoeff21'].shape[0]
broken_actuator = slice(0, actuator_len)
broken_kite = emergency_scenario[1]
broken_kite_parent = model.architecture.parent_map[broken_kite]
compromised_battery_cost = 0.0
for j in range(n_k):
broken_str = 'dcoeff' + str(broken_kite) + str(broken_kite_parent)
compromised_battery_cost += cas.mtimes(
V['u', j, broken_str, broken_actuator].T, V['u', j, broken_str,
broken_actuator])
compromised_battery_cost *= 1. / n_k
compromised_battery_cost = P[
'cost', 'compromised_battery'] * compromised_battery_cost
return compromised_battery_cost
def find_theta_regularisation_cost(V, P):
difference = V['theta'] - P['p', 'ref', 'theta']
theta_regularisation_cost = P['cost', 'theta'] * cas.mtimes(
difference.T, difference)
return theta_regularisation_cost
def get_reynolds(options, atmos, ua, q, parameters):
norm_ua = cas.mtimes(ua.T, ua) ** 0.5
rho_infty = atmos.get_density( q[2])
mu_infty = atmos.get_viscosity( q[2])
c_ref = parameters['theta0','geometry','c_ref']
reynolds = rho_infty * norm_ua * c_ref / mu_infty
return reynolds
def collect_local_performance_outputs(options, atmos, wind, variables, CL, CD, elevation_angle, ua, n, parent, outputs,parameters):
xd = variables['xd']
q = xd['q' + str(n) + str(parent)]
if 'local_performance' not in list(outputs.keys()):
outputs['local_performance'] = {}
[CR, f_crosswind, p_loyd, loyd_speed, loyd_phf] = get_loyd_comparison(options, atmos, wind, xd, n, parent, CL, CD, parameters, elevation_angle)
norm_ua = cas.mtimes(ua.T, ua)**0.5
outputs['local_performance']['CR' + str(n)] = CR
outputs['local_performance']['f_crosswind' + str(n)] = f_crosswind
outputs['local_performance']['p_loyd' + str(n)] = p_loyd
outputs['local_performance']['loyd_speed' + str(n)] = loyd_speed
outputs['local_performance']['loyd_phf' + str(n)] = loyd_phf
outputs['local_performance']['radius' + str(n)] = get_radius_of_curvature(variables, n, parent)
outputs['local_performance']['speed_ratio' + str(n)] = norm_ua / vect_op.norm(wind.get_velocity(q[2]))
outputs['local_performance']['speed_ratio_loyd' + str(n)] = loyd_speed / vect_op.norm(wind.get_velocity(q[2]))
outputs['local_performance']['radius_of_curvature' + str(n)] = get_radius_of_curvature(variables, n, parent)
return outputs
def approximate_tip_radius(model_options, variables, kite, architecture, tip,
parameters):
b_ref = parameters['theta0', 'geometry', 'b_ref']
half_span_proj = b_ref / 2.
parent = architecture.parent_map[kite]
radial_vector = get_kite_radial_vector(model_options, kite, variables,
architecture, parameters)
if int(model_options['kite_dof']) == 6:
r_column = variables['xd']['r' + str(kite) + str(parent)]
r = cas.reshape(r_column, (3, 3))
ehat2 = r[:, 1] # spanwise, from pe to ne
ehat2_proj_radial = vect_op.smooth_abs(
cas.mtimes(radial_vector.T, ehat2))
half_span_proj = b_ref * ehat2_proj_radial / 2.
radius = get_kite_radius(model_options, kite, variables, architecture,
parameters)
tip_radius = radius
if ('int' in tip) or (tip == 0):
tip_radius = tip_radius - half_span_proj
elif ('ext' in tip) or (tip == 1):
tip_radius = tip_radius + half_span_proj
else:
raise Exception('invalid tip designated')
return tip_radius
def collect_contributions(parameters, inputs):
stab_derivs = extract_derivs_from_parameters(parameters)
coeffs = {}
for deriv_name in stab_derivs.keys():
if not deriv_name == 'frame':
coeffs[deriv_name] = 0.
for input_name in stab_derivs[deriv_name].keys():
deriv_stack = stab_derivs[deriv_name][input_name]
deriv_length = deriv_stack.shape[0]
if not input_name in inputs.keys():
message = 'desired stability derivative input ' + input_name + ' is not recognized. ' \
+ 'The following inputs are defined: ' + repr(inputs.keys())
awelogger.logger.error(message)
input_val = inputs[input_name]
input_stack = []
for ldx in range(deriv_length):
input_stack = cas.vertcat(input_stack,
input_val**(ldx + 1))
contrib_from_input = cas.mtimes(deriv_stack.T, input_stack)
coeffs[deriv_name] += contrib_from_input
return coeffs
def mtimes(*args, **kwargs):
"""
More flexible version casadi.tools.mtimes.
Matrix multiplies all of the given arguments and returns the result. If any
inputs are Casadi's SX or MX data types, uses Casadi's mtimes. Otherwise,
uses a sequence of np.dot operations.
Keyword arguments forcedot or forcemtimes can be set to True to pick one
behavior or another.
"""
# Get keyword arguments.
forcemtimes = kwargs.pop("forcemtimes", None)
forcedot = kwargs.pop("forcedot", False)
if len(kwargs) > 0:
raise TypeError("Invalid keywords: %s" % kwargs.keys())
# Pick whether to use mul or dot.
if forcemtimes:
if forcedot:
raise ValueError("forcemtimes and forcedot can't both be True!")
useMul = True
elif forcedot:
useMul = False
else:
useMul = False
symtypes = set(["SX", "MX"])
for a in args:
atype = getattr(a, "type_name", lambda : None)()
if atype in symtypes:
useMul = True
break
# Now actually do multiplication.
ans = ctools.mtimes(args) if useMul else reduce(np.dot, args)
return ans
