Homec4science
Diffusion test_package (simple_kite) Browse

R1517/src/kitemodel/kite.cpp
81ed414c68b1simple_kite

Last Change
Raw File

kite.cpp
View Options

#include "kite.h"
using namespace casadi;
namespace kite_utils
{
KiteProperties LoadProperties(const std::string &filename)
{
//read YAML config file
YAML::Node config = YAML::LoadFile(filename);
//create properties object and fill in with data
KiteProperties props;
props.Name = config["name"].as<std::string>();
props.Geometry.WingSpan = config["geometry"]["b"].as<double>();
props.Geometry.MAC = config["geometry"]["c"].as<double>();
props.Geometry.AspectRatio = config["geometry"]["AR"].as<double>();
props.Geometry.WingSurfaceArea = config["geometry"]["S"].as<double>();
props.Geometry.TaperRatio = config["geometry"]["lam"].as<double>();
props.Geometry.HTailsurface = config["geometry"]["St"].as<double>();
props.Geometry.TailLeverArm = config["geometry"]["lt"].as<double>();
props.Geometry.FinSurfaceArea = config["geometry"]["Sf"].as<double>();
props.Geometry.FinLeverArm = config["geometry"]["lf"].as<double>();
props.Geometry.AerodynamicCenter = config["geometry"]["Xac"].as<double>();
props.Inertia.Mass = config["inertia"]["mass"].as<double>();
props.Inertia.Ixx = config["inertia"]["Ixx"].as<double>();
props.Inertia.Iyy = config["inertia"]["Iyy"].as<double>();
props.Inertia.Izz = config["inertia"]["Izz"].as<double>();
props.Inertia.Ixz = config["inertia"]["Ixz"].as<double>();
props.Aerodynamics.CL0 = config["aerodynamic"]["CL0"].as<double>();
props.Aerodynamics.CL0_tail = config["aerodynamic"]["CL0_tail"].as<double>();
props.Aerodynamics.CLa_total = config["aerodynamic"]["CLa_total"].as<double>();
props.Aerodynamics.CLa_wing = config["aerodynamic"]["CLa_wing"].as<double>();
props.Aerodynamics.CLa_tail = config["aerodynamic"]["CLa_tail"].as<double>();
props.Aerodynamics.e_oswald = config["aerodynamic"]["e_oswald"].as<double>();
props.Aerodynamics.CD0_total = config["aerodynamic"]["CD0_total"].as<double>();
props.Aerodynamics.CD0_wing = config["aerodynamic"]["CD0_wing"].as<double>();
props.Aerodynamics.CD0_tail = config["aerodynamic"]["CD0_tail"].as<double>();
props.Aerodynamics.CYb = config["aerodynamic"]["CYb"].as<double>();
props.Aerodynamics.CYb_vtail = config["aerodynamic"]["CYb_vtail"].as<double>();
props.Aerodynamics.Cm0 = config["aerodynamic"]["Cm0"].as<double>();
props.Aerodynamics.Cma = config["aerodynamic"]["Cma"].as<double>();
props.Aerodynamics.Cn0 = config["aerodynamic"]["Cn0"].as<double>();
props.Aerodynamics.Cnb = config["aerodynamic"]["Cnb"].as<double>();
props.Aerodynamics.Cl0 = config["aerodynamic"]["Cl0"].as<double>();
props.Aerodynamics.Clb = config["aerodynamic"]["Clb"].as<double>();
props.Aerodynamics.CLq = config["aerodynamic"]["CLq"].as<double>();
props.Aerodynamics.Cmq = config["aerodynamic"]["Cmq"].as<double>();
props.Aerodynamics.CYr = config["aerodynamic"]["CYr"].as<double>();
props.Aerodynamics.Cnr = config["aerodynamic"]["Cnr"].as<double>();
props.Aerodynamics.Clr = config["aerodynamic"]["Clr"].as<double>();
props.Aerodynamics.CYp = config["aerodynamic"]["CYp"].as<double>();
props.Aerodynamics.Clp = config["aerodynamic"]["Clp"].as<double>();
props.Aerodynamics.Cnp = config["aerodynamic"]["Cnp"].as<double>();
props.Aerodynamics.CLde = config["aerodynamic"]["CLde"].as<double>();
props.Aerodynamics.CYdr = config["aerodynamic"]["CYdr"].as<double>();
props.Aerodynamics.Cmde = config["aerodynamic"]["Cmde"].as<double>();
props.Aerodynamics.Cndr = config["aerodynamic"]["Cndr"].as<double>();
props.Aerodynamics.Cldr = config["aerodynamic"]["Cldr"].as<double>();
props.Aerodynamics.CDde = config["aerodynamic"]["CDde"].as<double>();
if(config["tether_length"])
props.Tether_length = config["tether_length"].as<double>();
else
{
std::cout << "WARNING: KITE DYNAMICS: TETHER LENGTH NOT SPECIFIED- USING DEFAULT VALUE \n";
props.Tether_length = 50;
}
return props;
}
SX quat_multiply(const SX &q1, const SX &q2)
{
SX s1 = q1[0];
SX v1 = q1(Slice(1,4),0);
SX s2 = q2[0];
SX v2 = q2(Slice(1,4),0);
SX s = (s1 * s2) - SX::dot(v1, v2);
SX v = SX::cross(v1, v2) + (s1 * v2) + (s2 * v1);
SXVector tmp{s,v};
return SX::vertcat(tmp);
}
SX quat_inverse(const SX &q)
{
SXVector tmp{q[0], -q[1], -q[2], -q[3]};
return SX::vertcat(tmp);
}
SX heaviside(const SX &x, const double K)
{
return K / (1 + exp(-4 * x));
}
SX rk4_symbolic(const SX &x,
const SX &u,
Function &func,
const SX &h)
{
SXVector res = func(SXVector{x, u});
SX k1 = res[0];
res = func(SXVector{x + 0.5 * h * k1, u});
SX k2 = res[0];
res = func(SXVector{x + 0.5 * h * k2, u});
SX k3 = res[0];
res = func(SXVector{x + h * k3, u});
SX k4 = res[0];
return x + (h/6) * (k1 + 2*k2 + 2*k3 + k4);
}
}
KiteDynamics::KiteDynamics(const KiteProperties &KiteProps, const AlgorithmProperties &AlgoProps)
{
/** enviromental constants */
const double g = 9.80665; /** gravitational acceleration [m/s2] [WGS84] */
const double ro = 1.2985; /** standart atmospheric density [kg/m3] [Standart Atmosphere 1976] */
/** --------------------- **/
/** Geometric parameters **/
/** -------------------- **/
double b = KiteProps.Geometry.WingSpan;
double c = KiteProps.Geometry.MAC;
double AR = KiteProps.Geometry.AspectRatio;
double S = KiteProps.Geometry.WingSurfaceArea;
//double lam = KiteProps.Geometry.TaperRatio;
//double St = KiteProps.Geometry.HTailsurface;
//double lt = KiteProps.Geometry.TailLeverArm;
//double Sf = KiteProps.Geometry.FinSurfaceArea;
//double lf = KiteProps.Geometry.FinLeverArm;
//double Xac = KiteProps.Geometry.AerodynamicCenter;
/** @todo: get rid of all magic numbers **/
//double Xcg = 0.031/c; /** Center of Gravity (CoG) wrt leading edge [1/c] **/
//double Vf = (Sf * lf) / (S * b); /** fin volume coefficient [] **/
//double Vh = (lt * St) / (S * c); /** horizontal tail volume coefficient [] **/
/** --------------------------- **/
/** Mass and inertia parameters **/
/** --------------------------- **/
double Mass = KiteProps.Inertia.Mass;
double Ixx = KiteProps.Inertia.Ixx;
double Iyy = KiteProps.Inertia.Iyy;
double Izz = KiteProps.Inertia.Izz;
double Ixz = KiteProps.Inertia.Ixz;
/** ------------------------------- **/
/** Static aerodynamic coefficients **/
/** ------------------------------- **/
double CL0 = KiteProps.Aerodynamics.CL0;
//double CL0_t = KiteProps.Aerodynamics.CL0_tail;
double CLa_tot = KiteProps.Aerodynamics.CLa_total;
//double CLa_w = KiteProps.Aerodynamics.CLa_wing;
//double CLa_t = KiteProps.Aerodynamics.CLa_tail;
double e_o = KiteProps.Aerodynamics.e_oswald;
//double dw = CLa_tot / (pi * e_o * AR); /** downwash acting at the tail [] **/
double CD0_tot = KiteProps.Aerodynamics.CD0_total;
//double CD0_w = KiteProps.Aerodynamics.CD0_wing;
//double CD0_t = KiteProps.Aerodynamics.CD0_tail;
double CYb = KiteProps.Aerodynamics.CYb;
//double CYb_vt = KiteProps.Aerodynamics.CYb_vtail;
double Cm0 = KiteProps.Aerodynamics.Cm0;
double Cma = KiteProps.Aerodynamics.Cma;
double Cn0 = KiteProps.Aerodynamics.Cn0;
double Cnb = KiteProps.Aerodynamics.Cnb;
double Cl0 = KiteProps.Aerodynamics.Cl0;
double Clb = KiteProps.Aerodynamics.Clb;
double CLq = KiteProps.Aerodynamics.CLq;
double Cmq = KiteProps.Aerodynamics.Cmq;
double CYr = KiteProps.Aerodynamics.CYr;
double Cnr = KiteProps.Aerodynamics.Cnr;
double Clr = KiteProps.Aerodynamics.Clr;
double CYp = KiteProps.Aerodynamics.CYp;
double Clp = KiteProps.Aerodynamics.Clp;
double Cnp = KiteProps.Aerodynamics.Cnp;
/** ------------------------------ **/
/** Aerodynamic effects of control **/
/** ------------------------------ **/
double CLde = KiteProps.Aerodynamics.CLde;
double CYdr = KiteProps.Aerodynamics.CYdr;
double Cmde = KiteProps.Aerodynamics.Cmde;
double Cndr = KiteProps.Aerodynamics.Cndr;
double Cldr = KiteProps.Aerodynamics.Cldr;
//double CDde = KiteProps.Aerodynamics.CDde;
//double CL_daoa = -2 * CLa_t * Vh * dw;
//double Cm_daoa = -2 * CLa_t * Vh * (lt/c) * dw;
/** -------------------------- **/
/** State variables definition **/
/** -------------------------- **/
SX r = SX::sym("r", 3); /** position of the CoG in the Inertial Reference Frame (IRF) [m] **/
SX q = SX::sym("q", 4); /** body attitude relative to IRF [unit quaternion] **/
SX v = SX::sym("v", 3); /** linear velocity of the CoG in the Body Reference Frame (BRF) [m/s] **/
SX w = SX::sym("w", 3); /** glider angular velocity in BRF [rad/s] **/
/** ---------------------------- **/
/** Control variables definition **/
/** ---------------------------- **/
/** @todo: consider more detailed propeller model **/
SX T = SX::sym("T"); /** propeller propulsion : applies along X-axis in BRF [N] **/
SX dE = SX::sym("dE"); /** elevator deflection [positive down] [rad] **/
SX dR = SX::sym("dR"); /** rudder deflection [rad] **/
//SX dA = SX::sym("dA"); /** aileron deflection [reserved, but not used] [rad] **/
//SX dF = SX::sym("dF"); /** flaps deflection [reserved, but not used] **/
/** ---------------------------- **/
/** Tether **/
/** ---------------------------- **/
double Lt = KiteProps.Tether_length;
/** @todo: read algorithm params here **/
/** @todo: add wind to the model **/
SX V = SX::norm_2(v);
SX V2 = SX::dot(v, v);
SX ss = asin(v[1] / V); /** side slip angle [rad] (v(3)/v(1)) // small angle assumption **/
SX aoa = atan2(v[2] , v[0]); /** angle of attack definition [rad] (v(2)/L2(v)) **/
SX dyn_press = 0.5 * ro * V2; /** dynamic pressure **/
SX CD = CD0_tot + pow(CL0 + CLa_tot * aoa, 2) / (pi * e_o * AR); /** total drag coefficient **/
/** ------------------------- **/
/** Dynamic Equations: Forces */
/** ------------------------- **/
SX LIFT = (CL0 + CLa_tot * aoa) * dyn_press * S +
(0.25 * CLq * c * S * ro) * V * w[1];
SX DRAG = CD * dyn_press * S;
SX SF = (CYb * ss + CYdr * dR) * dyn_press * S +
0.25 * (CYr * w[2] + CYp * w[0]) * (b * ro * S) * V;
/** Compute transformation betweeen WRF and BRF: qw_b **/
/** qw_b = q(aoa) * q(-ss); **/
SX q_aoa = SX::vertcat({cos(aoa / 2), 0, sin(aoa / 2), 0});
SX q_ss = SX::vertcat({cos(-ss / 2), 0, 0, sin(-ss / 2)});
SX qw_b = kite_utils::quat_multiply(q_aoa, q_ss);
SX qw_b_inv = kite_utils::quat_inverse(qw_b);
/** Aerodynamic forces in BRF: Faer0_b = qw_b * [0; -DRAG; SF; -LIFT] * qw_b_inv */
SX qF_tmp = kite_utils::quat_multiply(qw_b_inv, SX::vertcat({0, -DRAG, 0, -LIFT}));
SX qF_q = kite_utils::quat_multiply(qF_tmp, qw_b);
SX Faero_b = qF_q(Slice(1,4), 0);
SX Zde = (-CLde) * dE * dyn_press * S;
SX FdE_tmp = kite_utils::quat_multiply(kite_utils::quat_inverse(q_aoa),
SX::vertcat({0, 0, 0, Zde}));
SX qFdE = kite_utils::quat_multiply(FdE_tmp, q_aoa);
SX FdE = qFdE(Slice(1,4), 0);
Faero_b = Faero_b + FdE + SX::vertcat({0, SF, 0});
/** Gravity force in BRF */
SX qG = kite_utils::quat_multiply(kite_utils::quat_inverse(q),
SX::vertcat({0,0,0,g}));
SX qG_q = kite_utils::quat_multiply(qG, q);
SX G_b = qG_q(Slice(1,4), 0);
/** Propulsion force in BRF */
SX T_b = SX::vertcat({T, 0, 0});
/** Tether force */
/** value: using smooth ramp approximation */
SX d_ = SX::norm_2(r);
/** spring term */
/** @todo: put all coefficients in the config */
const double Ks = 15 * Mass;
const double Kd = 10 * Mass;
SX Rv = ((d_ - Lt));
SX Rs = -Rv * (r / d_);
/** damping term */
SX qvi = kite_utils::quat_multiply(q, SX::vertcat({0,v}));
SX qvi_q = kite_utils::quat_multiply(qvi, kite_utils::quat_inverse(q));
SX vi = qvi_q(Slice(1,4), 0);
SX Rd = (-r / d_) * SX::dot(r, vi) / d_;
SX R = ( Ks * Rs + Kd * Rd) * kite_utils::heaviside(d_ - Lt, 1);
/** BRF */
SX qR = kite_utils::quat_multiply(kite_utils::quat_inverse(q),
SX::vertcat({0,R}));
SX qR_q = kite_utils::quat_multiply(qR, q);
SX R_b = qR_q(Slice(1,4), 0);
/** Total external forces devided by glider's mass (linear acceleration) */
/** @todo : ADD TETHER FORCE */
auto v_dot = (Faero_b + T_b + R_b)/Mass + G_b - SX::cross(w,v);
/** ------------------------- */
/** Dynamic Equation: Moments */
/** ------------------------- */
/** Rolling Aerodynamic Moment */
SX L = (Cl0 + Clb * ss + Cldr * dR) * dyn_press * S * b +
(Clr * w[2] + Clp * w[0]) * (0.25 * ro * std::pow(b, 2) * S) * V;
/** Pitching Aerodynamic Moment */
SX M = (Cm0 + Cma * aoa + Cmde * dE) * dyn_press * S * c +
Cmq * (0.25 * S * std::pow(c, 2) * ro) * w[1] * V;
/** Yawing Aerodynamic Moment */
SX N = (Cn0 + Cnb * ss + Cndr * dR) * dyn_press * S * b +
(Cnp * w[0] + Cnr * w[2]) * (0.25 * S * std::pow(b, 2) * ro) * V;
/** Aircraft Inertia Matrix */
SXVector j_vec{Ixx, Iyy, Izz};
auto J = SX::diag(SX::vertcat(j_vec));
J(0,2) = Ixz;
J(2,0) = Ixz;
/** Angular motion equationin BRF */
/** Moments transformation SRF -> BRF */
SX T_tmp = kite_utils::quat_multiply(kite_utils::quat_inverse(q_aoa),
SX::vertcat({0, L, M, N}));
SX Trot = kite_utils::quat_multiply(T_tmp, q_aoa);
auto Maero = Trot(Slice(1,4), 0);
auto w_dot = SX::mtimes( SX::inv(J), (Maero - SX::cross(w, SX::mtimes(J, w))) );
/** ----------------------------- */
/** Kinematic Equations: Position */
/** ----------------------------- */
/** Aircraft position in the IRF */
SX qv = kite_utils::quat_multiply(q, SX::vertcat({0,v}));
SX qv_q = kite_utils::quat_multiply(qv, kite_utils::quat_inverse(q));
auto r_dot = qv_q(Slice(1,4), 0);
/** ----------------------------- */
/** Kinematic Equations: Attitude */
/** ----------------------------- */
/** Aircraft attitude wrt to IRF */
auto q_dot = 0.5 * kite_utils::quat_multiply(q, SX::vertcat({0, w}));
/** Complete dynamics of the Kite */
auto state = SX::vertcat({v, w, r, q});
auto control = SX::vertcat({T, dE, dR});
auto dynamics = SX::vertcat({v_dot, w_dot, r_dot, q_dot});
Function dyn_func = Function("dynamics", {state, control}, {dynamics});
/** compute dynamics state Jacobian */
SX d_jacobian = SX::jacobian(dynamics,state);
Function dyn_jac = Function("dyn_jacobian",{state, control}, {d_jacobian});
/** define RK4 integrator scheme */
SX X = SX::sym("X", 13);
SX U = SX::sym("U", 3);
SX dT = SX::sym("dT");
/** get symbolic expression for RK4 integrator */
SX sym_integrator = kite_utils::rk4_symbolic(X, U, dyn_func, dT);
Function RK4_INT = Function("RK4", {X,U,dT},{sym_integrator});
/** CVODES integrator */
/** @todo: make smarter initialisation of integrator */
double h = 0.02;
SXDict ode = {{"x", state}, {"p", control}, {"ode", dynamics}};
Dict opts = {{"tf", h}};
Function CVODES_INT = integrator("CVODES_INT", "cvodes", ode, opts);
/** assign class atributes */
this->State = state;
this->Control = control;
this->SymDynamics = dynamics;
this->SymIntegartor = sym_integrator;
this->SymJacobian = d_jacobian;
this->NumDynamics = dyn_func;
this->NumJacobian = dyn_jac;
/** return integrator function */
if(AlgoProps.Integrator == IntType::CVODES)
this->NumIntegrator = CVODES_INT;
else
this->NumIntegrator = RK4_INT;
}
c4science · Help