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