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