def get_body_axes(q_upper, q_lower):
tether = q_upper - q_lower
yhat = vect_op.yhat()
ehat_z = vect_op.normalize(tether)
ehat_x = vect_op.normed_cross(yhat, tether)
ehat_y = vect_op.normed_cross(ehat_z, ehat_x)
return ehat_x, ehat_y, ehat_z
def get_local_wind_reference_frame(init_options):
u_hat = vect_op.xhat_np()
n_rot_hat, y_rot_hat, z_rot_hat = tools_init.get_rotor_reference_frame(init_options)
if vect_op.norm(u_hat - n_rot_hat) < 1.e-5:
v_hat = y_rot_hat
w_hat = z_rot_hat
else:
w_hat = vect_op.normed_cross(u_hat, n_rot_hat)
v_hat = vect_op.normed_cross(w_hat, u_hat)
return u_hat, v_hat, w_hat
def get_rotor_reference_frame(init_options):
n_rot_hat = get_ehat_tether(init_options)
n_hat_is_x_hat = vect_op.abs(
vect_op.norm(n_rot_hat - vect_op.xhat_np())) < 1.e-4
if n_hat_is_x_hat:
y_rot_hat = vect_op.yhat_np()
z_rot_hat = vect_op.zhat_np()
else:
u_hat = vect_op.xhat_np()
z_rot_hat = vect_op.normed_cross(u_hat, n_rot_hat)
y_rot_hat = vect_op.normed_cross(z_rot_hat, n_rot_hat)
return n_rot_hat, y_rot_hat, z_rot_hat
def get_orbit_cone_parameters(options, model, l_t):
# get rotation plane axes:
# zhat is along tether (out)
# xhat is along earth-fixed yhat
# yhat is up and out, back towards the wind
ehat_tether = get_ehat_tether(options)
ehat_side = vect_op.yhat()
ehat_up = vect_op.normed_cross(ehat_tether, ehat_side)
# get radius and height of the cones in use
# two cone types specified, based on main tether (single kite option) and secondary tether (multi-kite option)
# radius is dependent on flight velocity
# height is a dependent
hypotenuse_list = cas.vertcat(l_t, options['theta']['l_s'])
[radius, t_f_guess] = estimate_radius_and_flight_time(options, model)
height_list = []
for hdx in range(hypotenuse_list.shape[0]):
hypotenuse = hypotenuse_list[hdx]
height = (hypotenuse**2. - radius**2.)**0.5
height_list = cas.vertcat(height_list, height)
return height_list, radius, ehat_tether, ehat_side, ehat_up
def __get_kite_axis(sstates, node_str, parent_str, grandparent_str,
trajectory_type):
"""Compute kite axis from states for a specific node
:type sstates: casadi.struct_symSX
:param sstates: states and their derivatives
:type node_str: str
:param node_str: node index
:type parent_str: str
:param parent_str: parent node index
:type grandparent_str: str
:param grandparent_str: grandparent node index
:type trajectory_type: str
:param trajectory_type: type of trajectory that is being optimized
:rtype: casadi.SX, casadi.DM, casadi.SX
"""
if node_str == '10':
e_hat_3 = vect_op.normalize(sstates['var']['q' + node_str] -
sstates['var']['q' + parent_str])
elif trajectory_type == 'nominal_landing':
e_hat_3 = cas.DM([0, 0, 1])
else:
e_hat_3 = vect_op.normalize(sstates['var']['q' + parent_str] -
sstates['var']['q' + grandparent_str])
e_hat_1 = -vect_op.normalize(sstates['dvar']['q' + node_str])
e_hat_2 = vect_op.normed_cross(e_hat_3, e_hat_1)
return e_hat_1, e_hat_2, e_hat_3
def draw_kite_vertical(ax,
q,
r,
length,
height,
b_ref,
c_ref,
kite_color,
side,
body_cross_sections_per_meter,
naca="0012"):
r_dcm = np.array(cas.reshape(r, (3, 3)))
ehat_1 = np.reshape(r_dcm[:, 0], (3, 1))
ehat_3 = np.reshape(r_dcm[:, 2], (3, 1))
new_ehat1 = ehat_1
new_ehat2 = ehat_3
new_ehat3 = np.array(vect_op.normed_cross(new_ehat1, new_ehat2))
r_new = np.array(cas.horzcat(new_ehat1, new_ehat2, new_ehat3))
horizontal_space = (3. * length / 4. - c_ref / 3.) * ehat_1
pos = q + horizontal_space + ehat_3 * height / 2.
draw_lifting_surface(ax, pos, r_new, height, c_ref, c_ref / 2., c_ref / 4.,
kite_color, side, body_cross_sections_per_meter, naca)
def get_rotating_reference_frame(t, init_options, model, node, ret):
n_rot_hat = get_ehat_tether(init_options)
ehat_normal = n_rot_hat
ehat_radial = get_ehat_radial(t, init_options, model, node, ret)
ehat_tangential = vect_op.normed_cross(ehat_normal, ehat_radial)
return ehat_normal, ehat_radial, ehat_tangential
def get_velocity_vector_from_psi(init_options, groundspeed, psi):
n_rot_hat, _, _ = get_rotor_reference_frame(init_options)
ehat_normal = n_rot_hat
ehat_radial = get_ehat_radial_from_azimuth(init_options, psi)
ehat_tangential = vect_op.normed_cross(ehat_normal, ehat_radial)
sign = get_rotation_direction_sign(init_options)
velocity = sign * groundspeed * ehat_tangential
return velocity
def get_kite_dcm(t, init_options, model, node, ret):
velocity = get_velocity_vector(t, init_options, model, node, ret)
ehat_normal, ehat_radial, ehat_tangential = get_rotating_reference_frame(
t, init_options, model, node, ret)
forwards_speed = cas.mtimes(velocity.T, ehat_tangential)
forwards_sign = forwards_speed / vect_op.norm(forwards_speed)
ehat_forwards = forwards_sign * ehat_tangential
ehat1 = -1. * ehat_forwards
ehat3 = ehat_normal
ehat2 = vect_op.normed_cross(ehat3, ehat1)
kite_dcm = cas.horzcat(ehat1, ehat2, ehat3)
return kite_dcm
def get_outputs(options, atmos, wind, variables, outputs, parameters,
architecture):
parent_map = architecture.parent_map
kite_nodes = architecture.kite_nodes
xd = variables['xd']
u = variables['u']
elevation_angle = indicators.get_elevation_angle(xd)
for n in kite_nodes:
parent = parent_map[n]
# get relevant variables for kite n
q = xd['q' + str(n) + str(parent)]
dq = xd['dq' + str(n) + str(parent)]
r = cas.reshape(xd['r' + str(n) + str(parent)], (3, 3))
ehat_span = r[:, 1]
ehat_chord = r[:, 0]
# wind parameters
rho_infty = atmos.get_density(q[2])
uw_infty = wind.get_velocity(q[2])
# apparent air velocity
if options['induction_model'] == 'actuator':
ua = actuator_disk_flow.get_kite_effective_velocity(
options, variables, wind, n, parent, architecture)
else:
ua = uw_infty - dq
# relative air speed
norm_ua_squared = cas.mtimes(ua.T, ua)
ua_norm = norm_ua_squared**0.5
# angle of attack and sideslip angle
alpha = indicators.get_alpha(ua, r)
beta = indicators.get_beta(ua, r)
if int(options['surface_control']) == 0:
delta = u['delta' + str(n) + str(parent)]
omega = xd['omega' + str(n) + str(parent)]
[CF, CM] = stability_derivatives.stability_derivatives(
options, alpha, beta, ua, omega, delta, parameters)
elif int(options['surface_control']) == 1:
delta = xd['delta' + str(n) + str(parent)]
omega = xd['omega' + str(n) + str(parent)]
[CF, CM] = stability_derivatives.stability_derivatives(
options, alpha, beta, ua, omega, delta, parameters)
else:
raise ValueError('unsupported surface_control chosen: %i',
options['surface_control'])
# body-_frameforcecomponents
# notice that these are unusual because an apparent wind reference coordinate system is in use.
# see below (get_coeffs_from_control_surfaces) for information
CA = CF[0]
CY = CF[1]
CN = CF[2]
Cl = CM[0]
Cm = CM[1]
Cn = CM[2]
dynamic_pressure = 1. / 2. * rho_infty * norm_ua_squared
planform_area = parameters['theta0', 'geometry', 's_ref']
ftilde_aero = cas.mtimes(r, CF)
f_aero = dynamic_pressure * planform_area * ftilde_aero
ehat_drag = vect_op.normalize(ua)
f_drag = cas.mtimes(cas.mtimes(f_aero.T, ehat_drag), ehat_drag)
ehat_lift = vect_op.normed_cross(ua, ehat_span)
f_lift = cas.mtimes(cas.mtimes(f_aero.T, ehat_lift), ehat_lift)
f_side = f_aero - f_drag - f_lift
drag_cross_lift = indicators.convert_from_body_to_wind_axes(
alpha, beta, CF)
CD = drag_cross_lift[0]
CS = drag_cross_lift[1]
CL = drag_cross_lift[2]
b_ref = parameters['theta0', 'geometry', 'b_ref']
c_ref = parameters['theta0', 'geometry', 'c_ref']
reference_lengths = cas.diag(cas.vertcat(b_ref, c_ref, b_ref))
m_aero = dynamic_pressure * planform_area * cas.mtimes(
reference_lengths, CM)
aero_coefficients = {}
aero_coefficients['CD'] = CD
aero_coefficients['CS'] = CS
aero_coefficients['CL'] = CL
aero_coefficients['CA'] = CA
aero_coefficients['CN'] = CN
aero_coefficients['CY'] = CY
aero_coefficients['Cl'] = Cl
aero_coefficients['Cm'] = Cm
aero_coefficients['Cn'] = Cn
outputs = indicators.collect_kite_aerodynamics_outputs(
options, atmos, ua, ua_norm, aero_coefficients, f_aero, f_lift,
f_drag, f_side, m_aero, ehat_chord, ehat_span, r, q, n, outputs,
parameters)
outputs = indicators.collect_environmental_outputs(
atmos, wind, q, n, outputs)
outputs = indicators.collect_aero_validity_outputs(
options, xd, ua, n, parent, outputs, parameters)
outputs = indicators.collect_local_performance_outputs(
options, atmos, wind, variables, CL, CD, elevation_angle, ua, n,
parent, outputs, parameters)
outputs = indicators.collect_power_balance_outputs(
variables, n, outputs, architecture)
return outputs
def get_windings(nlp_options, model, V, outputs={}):
if 'winding' not in list(outputs.keys()):
outputs['winding'] = {}
nk = nlp_options['n_k']
parent_map = model.architecture.parent_map
kite_nodes = model.architecture.kite_nodes
ehat_tether_x = 0.
ehat_tether_y = 0.
ehat_tether_z = 0.
total_steps = float(nk)
# TODO: in case of collocation, include collocation node info weighted with quad_weights
# TODO: weight with time constant in case of phase fixing!
for kdx in range(nk):
local_ehat = vect_op.normalize(V['xd', kdx, 'q10'])
ehat_tether_x += local_ehat[0] / total_steps
ehat_tether_y += local_ehat[1] / total_steps
ehat_tether_z += local_ehat[2] / total_steps
ehat_tether = vect_op.normalize(
cas.vertcat(ehat_tether_x, ehat_tether_y, ehat_tether_z))
outputs['winding']['ehat_tether'] = ehat_tether
ehat_side_a = vect_op.normed_cross(vect_op.yhat_np(), ehat_tether)
# right handed coordinate system -> x/re: _a, y/im: _b, z/out: _tether
ehat_side_b = vect_op.normed_cross(ehat_tether, ehat_side_a)
# now project the path onto this plane
for n in kite_nodes:
parent = parent_map[n]
theta_start = 0.
theta_end = 0.
# find the origin of the plane
origin = np.zeros((3, 1))
for kdx in range(nk):
q = V['xd', kdx, 'q' + str(n) + str(parent)]
q_in_plane = q - vect_op.dot(q, ehat_tether) * ehat_tether
origin = origin + q_in_plane / total_steps
# recenter the plane about origin
for kdx in range(nk):
q = V['xd', kdx, 'q' + str(n) + str(parent)]
q_in_plane = q - vect_op.dot(q, ehat_tether) * ehat_tether
q_recentered = q_in_plane - origin
q_next = V['xd', kdx + 1, 'q' + str(n) + str(parent)]
q_next_in_plane = q_next - vect_op.dot(q_next,
ehat_tether) * ehat_tether
q_next_recentered = q_next_in_plane - origin
delta_q = q_next_recentered - q_recentered
x = vect_op.dot(q_recentered, ehat_side_a)
y = vect_op.dot(q_recentered, ehat_side_b)
r_squared = x**2. + y**2.
dx = vect_op.dot(delta_q, ehat_side_a)
dy = vect_op.dot(delta_q, ehat_side_b)
# dx = vect_op.dot(dq_in_plane, ehat_side_a)
# dy = vect_op.dot(dq_in_plane, ehat_side_b)
dtheta = (x * dy - y * dx) / (r_squared + 1.0e-4)
theta_end += dtheta
winding = (theta_end - theta_start) / 2. / np.pi
outputs['winding']['winding' + str(n)] = winding
return outputs
def guess_values_at_time(t, init_options, model, formulation, tf_guess,
ntp_dict):
n_min_0 = ntp_dict['n_min_0']
d_min_0 = ntp_dict['d_min_0']
n_min_f = ntp_dict['n_min_f']
d_min_f = ntp_dict['d_min_f']
l_s = model.variable_bounds['theta']['l_s']['lb'] * init_options['theta'][
'l_s']
l_i = model.variable_bounds['theta']['l_i']['lb'] * init_options['theta'][
'l_i']
ret = {}
for name in struct_op.subkeys(model.variables, 'xd'):
ret[name] = 0.0
parent_map = model.architecture.parent_map
parent_map[0] = -1
number_of_nodes = model.architecture.number_of_nodes
V_0 = formulation.xi_dict['V_pickle_initial']
V_f = formulation.xi_dict['V_pickle_terminal']
q_n = {}
q_0 = {}
q_f = {}
phi_0 = {}
phi_f = {}
phi_n = {}
theta_0 = {}
theta_f = {}
theta_n = {}
dphi_n = {}
dtheta_n = {}
x_t_0 = {}
x_t_f = {}
dq_n = {}
dl_0 = formulation.xi_dict['V_pickle_initial'][
'coll_var', n_min_0, d_min_0, 'xd', 'dl_t'] * init_options['xd']['l_t']
l_0 = formulation.xi_dict['V_pickle_initial'][
'coll_var', n_min_0, d_min_0, 'xd', 'l_t'] * init_options['xd']['l_t']
l_f = formulation.xi_dict['V_pickle_terminal'][
'coll_var', n_min_f, d_min_f, 'xd', 'l_t'] * init_options['xd']['l_t']
dl_f = formulation.xi_dict['V_pickle_terminal'][
'coll_var', n_min_f, d_min_f, 'xd', 'dl_t'] * init_options['xd']['l_t']
c1 = (l_f - l_0 - 0.5 * tf_guess * dl_0 -
0.5 * tf_guess * dl_f) / (1. / 6. * tf_guess**3 - 0.25 * tf_guess**3)
c2 = (dl_f - dl_0 - 0.5 * tf_guess**2 * c1) / tf_guess
c3 = dl_0
c4 = l_0
l_t = 1. / 6. * t**3 * c1 + 0.5 * t**2 * c2 + t * c3 + c4
dl_t = 0.5 * t**2 * c1 + t * c2 + c3
ddl_t = t**2 * c2 + t * c1
q_0['q0-1'] = cas.DM([0.0, 0.0, 0.0])
q_f['q0-1'] = cas.DM([0.0, 0.0, 0.0])
for node in range(1, number_of_nodes):
parent = parent_map[node]
node_str = 'q' + str(node) + str(parent)
q_0[node_str] = V_0['coll_var', n_min_0, d_min_0, 'xd', node_str]
q_f[node_str] = V_f['coll_var', n_min_f, d_min_f, 'xd', node_str]
for node in range(1, number_of_nodes):
parent = parent_map[node]
grandparent = parent_map[parent]
tether_str = 'q' + str(node) + str(parent)
x_t_0[tether_str] = vect_op.normalize(
q_0['q' + str(node) + str(parent)] -
q_0['q' + str(parent) + str(grandparent)])
x_t_f[tether_str] = vect_op.normalize(
q_f['q' + str(node) + str(parent)] -
q_f['q' + str(parent) + str(grandparent)])
vec_plus = vect_op.normalize(q_0['q31'] - q_0['q21'])
vec_par = vect_op.normalize(x_t_0['q21'] * l_s + 0.5 *
(q_0['q31'] - q_0['q21']))
vec_orth = vect_op.normed_cross(vec_par, vec_plus)
for node in range(2, number_of_nodes):
x_0 = cas.mtimes(vec_par.T, l_s * x_t_0['q' + str(node) + str(parent)])
y_0 = cas.mtimes(vec_plus.T,
l_s * x_t_0['q' + str(node) + str(parent)])
z_0 = cas.mtimes(vec_orth.T,
l_s * x_t_0['q' + str(node) + str(parent)])
theta_0['q' + str(node) + str(parent)] = np.arcsin(z_0 / l_s)
phi_0['q' + str(node) + str(parent)] = np.arctan2(y_0, x_0)
x_f = cas.mtimes(vec_par.T, l_s * x_t_f['q' + str(node) + str(parent)])
y_f = cas.mtimes(vec_plus.T,
l_s * x_t_f['q' + str(node) + str(parent)])
z_f = cas.mtimes(vec_orth.T,
l_s * x_t_f['q' + str(node) + str(parent)])
theta_f['q' + str(node) + str(parent)] = np.arcsin(z_f / l_s)
phi_f['q' + str(node) + str(parent)] = np.arctan2(y_f, x_f)
for node in range(2, number_of_nodes):
parent = parent_map[node]
phi_n['q' + str(node) + str(parent)] = phi_0['q' + str(node) + str(
parent)] + t / tf_guess * (phi_f['q' + str(node) + str(parent)] -
phi_0['q' + str(node) + str(parent)])
dphi_n['q' + str(node) + str(parent)] = 1. / tf_guess * (
phi_f['q' + str(node) + str(parent)] -
phi_0['q' + str(node) + str(parent)])
theta_n['q' + str(node) + str(parent)] = theta_0['q' + str(node) + str(
parent)] + t / tf_guess * (theta_f['q' + str(node) + str(parent)] -
theta_0['q' + str(node) + str(parent)])
dtheta_n['q' + str(node) + str(parent)] = 1. / tf_guess * (
theta_f['q' + str(node) + str(parent)] -
theta_0['q' + str(node) + str(parent)])
a = cas.mtimes((q_f['q10'] - q_0['q10']).T, (q_f['q10'] - q_0['q10']))
b = 2 * cas.mtimes(q_0['q10'].T, (q_f['q10'] - q_0['q10']))
c = cas.mtimes(q_0['q10'].T, q_0['q10']) - l_t**2
dc = -2 * l_t * dl_t
D = b**2 - 4 * a * c
dD = -4 * a * dc
x1 = (-b + np.sqrt(D)) / (2 * a)
x2 = (-b - np.sqrt(D)) / (2 * a)
s = x2
ds = -1. / (2 * a) * 1. / (2 * np.sqrt(D)) * dD
e_t = 1. / l_t * (q_0['q10'] + s * (q_f['q10'] - q_0['q10']))
de_t = 1. / l_t * (ds * (q_f['q10'] - q_0['q10'])) - 1. / l_t**2 * dl_t * (
q_0['q10'] + s * (q_f['q10'] - q_0['q10']))
q_n['q10'] = l_t * e_t
dq_n['q10'] = dl_t * e_t + l_t * de_t
for node in range(2, number_of_nodes):
parent = parent_map[node]
nstr = 'q' + str(node) + str(parent)
q_n[nstr] = q_n['q10'] + (
np.sin(phi_n[nstr]) * np.cos(theta_n[nstr]) * vec_plus +
np.cos(phi_n[nstr]) * np.cos(theta_n[nstr]) * vec_par +
np.sin(theta_n[nstr]) * vec_orth) * l_s
dq_n[nstr] = dq_n['q10'] + (
-np.cos(phi_n[nstr]) * np.sin(theta_n[nstr]) * dtheta_n[nstr] *
dphi_n[nstr] * vec_plus + np.sin(phi_n[nstr]) *
np.sin(theta_n[nstr]) * dtheta_n[nstr] * dphi_n[nstr] * vec_par +
np.cos(theta_n[nstr]) * dtheta_n[nstr] * vec_orth) * l_s
for node in range(1, number_of_nodes):
parent = parent_map[node]
ret['q' + str(node) + str(parent)] = q_n['q' + str(node) + str(parent)]
ret['dq' + str(node) + str(parent)] = dq_n['q' + str(node) +
str(parent)]
ret['l_t'] = l_t
ret['dl_t'] = dl_t
return ret
def initial_node_variables_for_standard_path(t,
options,
model,
formulation,
ret={}):
trajectory_type = options['type']
number_of_nodes = model.architecture.number_of_nodes
parent_map = model.architecture.parent_map
kite_nodes = model.architecture.kite_nodes
level_siblings = get_all_level_siblings(options, model)
ua_norm = options['ua_norm']
kite_dof = model.kite_dof
[height_list, radius, ehat_tether, ehat_side,
ehat_up] = get_orbit_cone_parameters(options, model, ret['l_t'])
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(
options, parent_position, node, ret['l_t'])
ret['dq' + str(node) + str(parent)] = np.zeros((3, 1))
else:
if parent == 0:
height = height_list[0]
else:
height = height_list[1]
omega_norm = ua_norm / radius
omega_vector = ehat_tether * omega_norm
psi = get_azimuthal_angle(t, level_siblings, node, parent,
omega_norm)
ehat_radial = ehat_side * np.cos(psi) + ehat_up * np.sin(psi)
tether_vector = ehat_radial * radius + ehat_tether * height
position = parent_position + tether_vector
ehat_tangential = -1. * ehat_side * np.sin(psi) + ehat_up * np.cos(
psi)
velocity = ua_norm * ehat_tangential
ehat1 = -1. * ehat_tangential
ehat3 = ehat_tether
ehat2 = vect_op.normed_cross(ehat3, ehat1)
dcm = cas.horzcat(ehat1, ehat2, ehat3)
dcm_column = cas.reshape(dcm, (9, 1))
ret['q' + str(node) + str(parent)] = position
ret['dq' + str(node) + str(parent)] = velocity
if int(kite_dof) == 6:
ret['omega' + str(node) + str(parent)] = omega_vector
ret['r' + str(node) + str(parent)] = dcm_column
return ret
def __process_interpolation_Variables(interpolation_variables, configurations,
model):
"""Create casadi functions mapping interpolation variables to states
:type interpolation_variables: dict
:param interpolation_variables: variables defining the interpolation
:type configurations: dict
:param configurations: parameters defining a given configuration
:type model: awebox.model_dir.model
:param model: system model
:rtype: dict, casadi.struct_symSX
"""
# initialize dict
rotation_matrix = {}
rotation_matrix['q00'] = np.eye(3)
rotation_matrix['dq00'] = np.zeros([3, 3])
kite_nodes = model.architecture.kite_nodes
parent_nodes = model.architecture.parent_map
parent_nodes[0] = 0
number_of_nodes = model.architecture.number_of_nodes
kite_dof = model.kite_dof
trajectory_type = model.options['trajectory']['type']
conf_0 = configurations['conf_0']
conf_f = configurations['conf_f']
l_s = configurations['l_s']
l_i = configurations['l_i']
## generate casadi expressions
# generate symbolic variables
sinterp = __get_sstruct(interpolation_variables)
sstates = {}
sstates['var'] = {}
sstates['var']['q00'] = 0.
sstates['dvar'] = {}
sstates['dvar']['q00'] = 0.
sstates['ddvar'] = {}
sstates['ddvar']['q00'] = 0.
# generate dict to store functions in
sstates_functions = {}
# generate casadi functions: inteprolation variables -> states
l_t = sinterp['var', 'l_t']
for node in range(1, number_of_nodes):
parent_node = parent_nodes[node]
node_str = str(node) + str(parent_node)
grandparent_node = parent_nodes[parent_node]
parent_str = str(parent_node) + str(grandparent_node)
grandgrandparent_node = parent_nodes[grandparent_node]
grandparent_str = str(grandparent_node) + str(grandgrandparent_node)
if (node == 1) and (
node not in kite_nodes
): # first node parameterized with main tether length
a = cas.mtimes((conf_f['q' + node_str] - conf_0['q' + node_str]).T,
(conf_f['q' + node_str] - conf_0['q' + node_str]))
if a == 0:
e_t = vect_op.normalize(conf_0['q' + node_str])
else:
b = 2 * cas.mtimes(
conf_0['q' + node_str].T,
(conf_f['q' + node_str] - conf_0['q' + node_str]))
c = cas.mtimes(conf_0['q' + node_str].T,
conf_0['q' + node_str]) - l_t**2
D = b**2 - 4 * a * c
x1 = (-b + np.sqrt(D)) / (2 * a)
x2 = (-b - np.sqrt(D)) / (2 * a)
#if x2 >= 0:
# s = x2
#else:
# s = x1
s = x1
e_t = 1. / l_t * (
conf_0['q' + node_str] + s *
(conf_f['q' + node_str] - conf_0['q' + node_str]))
sstates['var']['q' + node_str] = l_t * e_t
rotation_matrix = compute_rotation_matrices(
sinterp.prefix['var'], node_str, parent_str, rotation_matrix)
else:
if (node in kite_nodes):
if node == 1:
tether_length = l_t
else:
tether_length = l_s
Phi = sinterp['var', 'Phi' + node_str]
Omega = sinterp['var', 'Omega' + node_str]
parent = sstates['var']['q' + parent_str]
grandparent = sstates['var']['q' + grandparent_str]
radius = tether_length * np.sin(Phi)
l_x = tether_length * np.cos(Phi)
# define axis of rotation
if node != 1:
axis_of_rot = parent - grandparent
axis_of_rot = vect_op.normalize(axis_of_rot)
else:
inclination = sinterp['var', 'inclination' + node_str]
axis_of_rot = np.zeros([3, 1])
axis_of_rot[0] = np.cos(inclination)
axis_of_rot[2] = np.sin(inclination)
e_hat_x = axis_of_rot
e_hat_y = vect_op.normed_cross(e_hat_x, vect_op.zhat())
e_hat_z = vect_op.normed_cross(e_hat_y, e_hat_x)
e_hat_r = e_hat_z * np.sin(Omega) + e_hat_y * np.cos(Omega)
sstates['var']['q' + node_str] = sstates['var'][
'q' + parent_str] + e_hat_r * radius + e_hat_x * l_x
else:
tether_length = l_i
rotation_matrix = compute_rotation_matrices(
sinterp.prefix['var'], node_str, parent_str,
rotation_matrix)
tether_vector = vect_op.xhat()
tether_vector = cas.mtimes(rotation_matrix['q' + node_str],
tether_vector)
sstates['var']['q' + node_str] = sstates['var'][
'q' + parent_str] + tether_vector * tether_length
sstates['dvar']['q' + node_str] = cas.mtimes(
cas.jacobian(sstates['var']['q' + node_str], sinterp['var']),
sinterp['dvar'])
# create rotational kinematics
if int(kite_dof) == 6:
# iterate over all kite ndoes
if node in kite_nodes:
# get node strings
parent = parent_nodes[node]
node_str = str(node) + str(parent)
grandparent = parent_nodes[parent]
parent_str = str(parent) + str(grandparent)
grandgrandparent = parent_nodes[grandparent]
grandparent_str = str(grandparent) + str(grandgrandparent)
# compute dcm matrix for node
e_hat_1, e_hat_2, e_hat_3 = __get_kite_axis(
sstates, node_str, parent_str, grandparent_str,
trajectory_type)
dcm = ct.horzcat(e_hat_1, e_hat_2, e_hat_3)
dcm_column = ct.reshape(dcm, (9, 1))
# compute rotation around axis
omega_norm = vect_op.norm(
sstates['dvar']['q' + node_str]) / radius
if trajectory_type == 'lift_mode':
omega_vector = vect_op.normalize(axis_of_rot) * omega_norm
elif trajectory_type == 'nominal_landing':
omega_vector = cas.DM([0, 0, 0])
# put in state dict
sstates['omega' + node_str] = omega_vector
sstates['r' + node_str] = dcm_column
# generate functions
sstates_functions['q' + node_str] = cas.Function(
'q' + node_str, [sinterp], [sstates['var']['q' + node_str]])
sstates_functions['dq' + node_str] = cas.Function(
'dq' + node_str, [sinterp], [sstates['dvar']['q' + node_str]])
if int(kite_dof) == 6 and node in kite_nodes:
sstates_functions['r' + node_str] = cas.Function(
'r' + node_str, [sinterp], [sstates['r' + node_str]])
sstates_functions['omega' + node_str] = cas.Function(
'omega' + node_str, [sinterp], [sstates['omega' + node_str]])
return sstates_functions, sinterp
def get_outputs(options, atmos, wind, variables, outputs, parameters,
architecture):
parent_map = architecture.parent_map
kite_nodes = architecture.kite_nodes
xd = variables['xd']
elevation_angle = indicators.get_elevation_angle(xd)
for n in kite_nodes:
parent = parent_map[n]
# get relevant variables for kite n
q = xd['q' + str(n) + str(parent)]
dq = xd['dq' + str(n) + str(parent)]
coeff = xd['coeff' + str(n) + str(parent)]
# wind parameters
rho_infty = atmos.get_density(q[2])
uw_infty = wind.get_velocity(q[2])
# apparent air velocity
if options['induction_model'] == 'actuator':
ua = actuator_disk_flow.get_kite_effective_velocity(
options, variables, wind, n, parent, architecture)
else:
ua = uw_infty - dq
# relative air speed
ua_norm = vect_op.smooth_norm(ua, epsilon=1e-8)
# ua_norm = mtimes(ua.T, ua) ** 0.5
# in kite body:
if parent > 0:
grandparent = parent_map[parent]
qparent = xd['q' + str(parent) + str(grandparent)]
else:
qparent = np.array([0., 0., 0.])
ehat_r = (q - qparent) / vect_op.norm(q - qparent)
ehat_t = vect_op.normed_cross(ua, ehat_r)
ehat_s = vect_op.normed_cross(ehat_t, ua)
# roll angle
psi = coeff[1]
ehat_l = cas.cos(psi) * ehat_s + cas.sin(psi) * ehat_t
ehat_span = cas.cos(psi) * ehat_t - cas.sin(psi) * ehat_s
ehat_chord = ua / ua_norm
# implicit direct cosine matrix (for plotting only)
r = cas.horzcat(ehat_chord, ehat_span, ehat_l)
# lift and drag coefficients
CL = coeff[0]
CD = parameters['theta0', 'aero', 'CD0'] + 0.02 * CL**2
# lift and drag force
f_lift = CL * 1. / 2. * rho_infty * cas.mtimes(
ua.T, ua) * parameters['theta0', 'geometry', 's_ref'] * ehat_l
f_drag = CD * 1. / 2. * rho_infty * ua_norm * parameters['theta0',
'geometry',
's_ref'] * ua
f_side = cas.DM(np.zeros((3, 1)))
f_aero = f_lift + f_drag
m_aero = cas.DM(np.zeros((3, 1)))
CA = CD
CN = CL
CY = cas.DM(0.)
aero_coefficients = {}
aero_coefficients['CD'] = CD
aero_coefficients['CL'] = CL
aero_coefficients['CA'] = CA
aero_coefficients['CN'] = CN
aero_coefficients['CY'] = CY
outputs = indicators.collect_kite_aerodynamics_outputs(
options, atmos, ua, ua_norm, aero_coefficients, f_aero, f_lift,
f_drag, f_side, m_aero, ehat_chord, ehat_span, r, q, n, outputs,
parameters)
outputs = indicators.collect_environmental_outputs(
atmos, wind, q, n, outputs)
outputs = indicators.collect_aero_validity_outputs(
options, xd, ua, n, parent, outputs, parameters)
outputs = indicators.collect_local_performance_outputs(
options, atmos, wind, variables, CL, CD, elevation_angle, ua, n,
parent, outputs, parameters)
outputs = indicators.collect_power_balance_outputs(
variables, n, outputs, architecture)
return outputs