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 list_filaments_kiteobs_and_normal_info(filament_list, options, variables,
parameters, kite_obs, architecture,
include_normal_info):
n_filaments = filament_list.shape[1]
parent_obs = architecture.parent_map[kite_obs]
point_obs = variables['xd']['q' + str(kite_obs) + str(parent_obs)]
seg_list = list_filament_observer_and_normal_info(point_obs, filament_list,
options)
if include_normal_info:
n_vec_val = unit_normal.get_n_vec(options, parent_obs, variables,
parameters, architecture)
n_hat = vect_op.normalize(n_vec_val)
n_hat_ext = []
for jdx in range(3):
n_hat_ext = cas.vertcat(
n_hat_ext,
vect_op.ones_sx((1, n_filaments)) * n_hat[jdx])
seg_list = cas.vertcat(seg_list, n_hat_ext)
return seg_list
def get_n_hat(model_options, parent, variables, parameters, architecture):
n_vec = get_n_vec(model_options, parent, variables, parameters,
architecture)
n_hat = vect_op.normalize(n_vec)
return n_hat
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_stagger_distance(options, model, q_init, q_parent, n, parent):
ehat_l_init = vect_op.normalize(q_init - q_parent)
scale_stagger = model.scaling['xd']['q' + str(n) + str(parent)]
stagger = options['tracking']['stagger_distance'] / scale_stagger
if parent == 0:
q_ref = stagger * ehat_l_init / 2.
else:
q_ref = stagger * ehat_l_init
return q_ref
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 guess_values_at_time(t, init_options, model, formulation, tf_guess, ntp_dict):
n_min = ntp_dict['n_min']
d_min = ntp_dict['d_min']
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
kite_nodes = model.architecture.kite_nodes
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']
q_n = {}
q_0 = {}
x_t = {}
dq_n = {}
q_0['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, d_min, 'xd', node_str]
for node in range(1, number_of_nodes):
parent = parent_map[node]
grandparent = parent_map[parent]
tether_str = 't' + str(node) + str(parent)
x_t[tether_str] = vect_op.normalize(
q_0['q' + str(node) + str(parent)] - q_0['q' + str(parent) + str(grandparent)])
dl_0 = formulation.xi_dict['V_pickle_initial']['coll_var', n_min, d_min, 'xd', 'dl_t'] * init_options['xd']['l_t']
l_0 = formulation.xi_dict['V_pickle_initial']['coll_var', n_min, d_min, 'xd', 'l_t'] * init_options['xd']['l_t']
l_f = model.variable_bounds['xd']['q10']['lb'][2] / x_t['t10'][2]
dl_f = 0
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_n['q0-1'] = cas.DM([0.0, 0.0, 0.0])
for node in range(1, number_of_nodes):
parent = parent_map[node]
grandparent = parent_map[parent]
if node == 1:
seg_length = l_t
elif node in kite_nodes:
seg_length = l_s
else:
seg_length = l_i
q_n['q' + str(node) + str(parent)] = x_t['t' + str(node) + str(parent)] * seg_length + q_n[
'q' + str(parent) + str(grandparent)]
if node == 1:
dq_n['dq' + str(node) + str(parent)] = dl_t * x_t['t' + str(node) + str(parent)]
else:
dq_n['dq' + str(node) + str(parent)] = dq_n['dq10']
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['dq' + str(node) + str(parent)]
ret['l_t'] = l_t
ret['dl_t'] = dl_t
# ret['ddl_t'] = ddl_t
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
