Feat/xfoil polars

.gitignore

@@ -1,6 +1,8 @@
 *.xopp
 *.so
 *.so.bak
+polars.csv
+*.dat
 /Manifest.toml
 /Manifest.toml.1.7
 bin/kps-image-1.8-initial.so

Project.toml

@@ -5,9 +5,9 @@ version = "0.6.6"
 
 [deps]
 AtmosphericModels = "c59cac55-771d-4f45-b14d-1c681463a295"
-DataInterpolations = "82cc6244-b520-54b8-b5a6-8a565e85f1d0"
 Dierckx = "39dd38d3-220a-591b-8e3c-4c3a8c710a94"
 DiffEqBase = "2b5f629d-d688-5b77-993f-72d75c75574e"
+Distributed = "8ba89e20-285c-5b6f-9357-94700520ee1b"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 KitePodModels = "9de5dc81-f971-414a-927b-652b2f41c539"
 KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533"
@@ -29,12 +29,12 @@ Sundials = "c3572dad-4567-51f8-b174-8c6c989267f4"
 SymbolicIndexingInterface = "2efcf032-c050-4f8e-a9bb-153293bab1f5"
 Timers = "21f18d07-b854-4dab-86f0-c15a3821819a"
 WinchModels = "7dcfa46b-7979-4771-bbf4-0aee0da42e1f"
+Xfoil = "19641d66-a62d-11e8-2441-8f57a969a9c4"
 
 [compat]
 AtmosphericModels = "0.2"
 BenchmarkTools = "1.2, 1.3"
 ControlPlots = "0.2.0"
-DataInterpolations = "6.2.0"
 Dierckx = "0.5"
 DiffEqBase = "6.152.2"
 DocStringExtensions = "0.8, 0.9"

data/naca2412.dat

@@ -0,0 +1,35 @@
+1.0000     0.0013
+0.9500     0.0114
+0.9000     0.0208
+0.8000     0.0375
+0.7000     0.0518
+0.6000     0.0636
+0.5000     0.0724
+0.4000     0.0780
+0.3000     0.0788
+0.2500     0.0767
+0.2000     0.0726
+0.1500     0.0661
+0.1000     0.0563
+0.0750     0.0496
+0.0500     0.0413
+0.0250     0.0299
+0.0125     0.0215
+0.0000     0.0000
+0.0125     -0.0165
+0.0250     -0.0227
+0.0500     -0.0301
+0.0750     -0.0346
+0.1000     -0.0375
+0.1500     -0.0410
+0.2000     -0.0423
+0.2500     -0.0422
+0.3000     -0.0412
+0.4000     -0.0380
+0.5000     -0.0334
+0.6000     -0.0276
+0.7000     -0.0214
+0.8000     -0.0150
+0.9000     -0.0082
+0.9500     -0.0048
+1.0000     -0.0013

data/settings_3l.yaml

@@ -11,7 +11,7 @@ system:
 
 initial:
     l_tether: 50.0        # initial tether length       [m]
-    elevation: 70.8        # initial elevation angle   [deg]
+    elevation: 85.0        # initial elevation angle   [deg]
     v_reel_out: 0.0        # initial reel out speed    [m/s]
     depower:   25.0        # initial depower settings    [%]
 
@@ -37,6 +37,8 @@ depower:
 
 kite:
     model: "data/kite.obj" # 3D model of the kite
+    foil_file: "data/MH82.dat"  # filename for the foil shape
+    polar_file: "data/polars.csv"   # filename for the polars
     physical_model: "KPS4_3L" # name of the kite model to use (KPS3 or KPS4)
     version: 2             # version of the model to use
     mass:  0.9             # kite mass [kg]
@@ -63,6 +65,7 @@ kps4_3l:
     min_steering_line_distance: 1.0
     width:         4.1     # width of the kite                          [m]
     aero_surfaces: 5      # number of aerodynamic surfaces in 3 line model
+    flap_height: 0.044     # height at the start of the flap of a normalized kite segment
 
 kcu:
     kcu_mass: 8.4                # mass of the kite control unit   [kg]
@@ -81,7 +84,7 @@ bridle:
     rel_damping: 6.0          # relative damping of the kite spring (relative to main tether)
 
 tether:
-    d_tether:  1           # tether diameter                 [mm]
+    d_tether:  2           # tether diameter                 [mm]
     cd_tether: 0.958       # drag coefficient of the tether
     damping: 473.0         # unit damping coefficient        [Ns]
     c_spring: 614600.0     # unit spring constant coefficient [N]
@@ -96,7 +99,7 @@ winch:
     v_ro_min: -8.0         # minimal reel-out speed (=max reel-in speed)     [m/s]
     drum_radius: 0.110    # radius of the drum                              [m]
     gear_ratio: 1.0        # gear ratio of the winch                         [-]   
-    inertia_total: 0.104   # total inertia, as seen from the motor/generator [kgm²]
+    inertia_total: 0.024   # total inertia, as seen from the motor/generator [kgm²]
     f_coulomb: 122.0       # coulomb friction                                [N]
     c_vf: 30.6             # coefficient for the viscous friction            [Ns/m]
 

examples/3l_kite_environment.jl

@@ -1,7 +1,8 @@
 module Environment
 
-using KiteModels, StaticArrays, LinearAlgebra, OrdinaryDiffEqCore, OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, Parameters
-export reset, step, render
+using KiteModels, StaticArrays, LinearAlgebra, Parameters
+import OrdinaryDiffEqCore: ODEIntegrator
+export reset, step, render, Env
 
 const StateVec = MVector{11, Float32}
 
@@ -11,7 +12,7 @@ const StateVec = MVector{11, Float32}
     max_render_length::Int = 10000
     i::Int = 1
     logger::Logger = Logger(s.num_A, max_render_length)
-    integrator::OrdinaryDiffEq.ODEIntegrator = KiteModels.init_sim!(s; stiffness_factor=0.04, prn=false, mtk=true, torque_control=true)
+    integrator::ODEIntegrator = KiteModels.init_sim!(s; prn=false, torque_control=true)
     sys_state::SysState = SysState(s)
     state::StateVec = zeros(StateVec)
     state_d::StateVec = zeros(StateVec)
@@ -25,27 +26,26 @@ const StateVec = MVector{11, Float32}
     rotation::Float64 = 0.0
 end
 
-const e = Env()
-
-function step(reel_out_torques; prn=false)
-    reel_out_torques = Vector{Float64}(reel_out_torques) .* 100
+function step(e::Env, reel_out_torques; prn=false)
+    reel_out_torques = Vector{Float64}(reel_out_torques)
 
     old_heading = calc_heading(e.s)
     if prn
-        KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques, torque_control=false)
+        KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques)
     else
-        redirect_stdout(devnull) do
-            KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques, torque_control=true)
+        redirect_stderr(devnull) do
+            redirect_stdout(devnull) do
+                KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques)
+            end
         end
     end
-    println(e.s.pos[end])
-    _calc_rotation(old_heading, calc_heading(e.s))
+    _calc_rotation(e, old_heading, calc_heading(e.s))
     update_sys_state!(e.sys_state, e.s)
     e.i += 1
-    return _calc_state(e.s)
+    return (e.integrator.last_stepfail, _calc_state(e, e.s))
 end
 
-function reset(name="sim_log", elevation=0.0, azimuth=0.0, tether_length=50.0, force=5000.0, log=false)
+function reset(e::Env, name="sim_log", elevation=0.0, azimuth=0.0, tether_length=50.0, force=5000.0, log=false)
     e.wanted_elevation = Float32(elevation)
     e.wanted_azimuth = Float32(azimuth)
     e.wanted_tether_length = Float32(tether_length)
@@ -56,24 +56,22 @@ function reset(name="sim_log", elevation=0.0, azimuth=0.0, tether_length=50.0, f
         save_log(e.logger, basename(name))
     end
     update_settings()
-    @time e.logger = Logger(e.s.num_A, e.max_render_length)
-    println(e.integrator)
-    @time e.integrator = KiteModels.reset_sim!(e.s; stiffness_factor=0.04)
+    e.logger = Logger(e.s.num_A, e.max_render_length)
+    e.integrator = KiteModels.init_sim!(e.s; prn=false, torque_control=true)
     e.sys_state = SysState(e.s)
     e.i = 1
-    return _calc_state(e.s)
+    return _calc_state(e, e.s)
 end
 
-function render()
+function render(e::Env)
     if(e.i <= e.max_render_length)
         log!(e.logger, e.sys_state)
     end
 end
 
-function _calc_state(s::KPS4_3L)
-    _calc_reward(s)
+function _calc_state(e::Env, s::KPS4_3L)
     e.state .= vcat(
-        _calc_reward(s),                # length 1
+        _calc_speed_reward(e,s),                # length 1
         calc_orient_quat(s),            # length 4
         s.tether_lengths,                    # length 3 # normalize to min and max 
 
@@ -96,27 +94,57 @@ function _calc_state(s::KPS4_3L)
     return vcat(e.state, e.state_d, e.state_dd)
 end
 
-function _calc_reward(s::KPS4_3L)
-    if (KiteModels.calc_tether_elevation(s) < e.wanted_elevation ||
+function _calc_reward(e::Env, s::KPS4_3L)
+    if  s.vel_kite ⋅ s.e_x < 0 ||
+        (KiteModels.calc_tether_elevation(s) < e.wanted_elevation ||
         !(-2*π < e.rotation < 2*π) ||
-        s.tether_lengths[1] > e.wanted_tether_length*1.5 ||
-        s.tether_lengths[1] < e.wanted_tether_length*0.95 ||
+        s.tether_lengths[3] > e.wanted_tether_length*1.5 ||
+        s.tether_lengths[3] < e.wanted_tether_length*0.95 ||
         sum(winch_force(s)) > e.max_force)
         return 0.0
     end
-    force_component = _calc_force_component(s)
+    force_component = _calc_force_component(e,s)
     reward = clamp(force_component / e.max_force, 0.0, 1.0) # range [-1, 1] clamped to [0, 1] because 0 is physical minimum
     return reward
 end
 
-function _calc_force_component(s::KPS4_3L)
+function _calc_speed_reward(e::Env, s::KPS4_3L)
+    speed = s.vel_kite ⋅ s.e_x
+    if  (KiteModels.calc_tether_elevation(s) < e.wanted_elevation ||
+        !(-2*π < e.rotation < 2*π) ||
+        s.tether_lengths[3] > e.wanted_tether_length*1.5 ||
+        s.tether_lengths[3] < e.wanted_tether_length*0.95 ||
+        sum(winch_force(s)) > e.max_force)
+        return 0.0
+    end
+    # wanted_minus_z = [cos(e.wanted_elevation)*cos(e.wanted_azimuth), cos(e.wanted_elevation)*-sin(e.wanted_azimuth), sin(e.wanted_elevation)]
+    # reward = speed * (-s.e_z ⋅ wanted_minus_z)
+    reward = speed
+    return reward
+end
+
+function _calc_direction_reward(e::Env, s::KPS4_3L)
+    if  s.vel_kite ⋅ s.e_x < 0.0 ||
+        (KiteModels.calc_tether_elevation(s) < e.wanted_elevation ||
+        !(-2*π < e.rotation < 2*π) ||
+        s.tether_lengths[3] > e.wanted_tether_length*1.5 ||
+        s.tether_lengths[3] < e.wanted_tether_length*0.95 ||
+        sum(winch_force(s)) > e.max_force)
+        return 0.01
+    end
+    wanted_direction = [cos(e.wanted_elevation)*cos(e.wanted_azimuth), cos(e.wanted_elevation)*-sin(e.wanted_azimuth), sin(e.wanted_elevation)]
+    reward = normalize(s.pos[6]) ⋅ wanted_direction + 1.0
+    return reward
+end
+
+function _calc_force_component(e::Env, s::KPS4_3L)
     wanted_force_vector = [cos(e.wanted_elevation)*cos(e.wanted_azimuth), cos(e.wanted_elevation)*-sin(e.wanted_azimuth), sin(e.wanted_elevation)]
-    tether_force = sum(s.forces[1:3])
+    tether_force = sum(s.winch_forces)
     force_component = tether_force ⋅ wanted_force_vector
     return force_component
 end
 
-function _calc_rotation(old_heading, new_heading)
+function _calc_rotation(e::Env, old_heading, new_heading)
     d_rotation = new_heading - old_heading
     if d_rotation > 1
         d_rotation -= 2*pi

examples/simple_3l_speed_control.jl

@@ -0,0 +1,105 @@
+using KiteModels, OrdinaryDiffEqCore, OrdinaryDiffEqBDF, OrdinaryDiffEqSDIRK, LinearAlgebra, Timers, Statistics
+using Base: summarysize
+tic()
+
+using Pkg
+if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+end
+using ControlPlots
+
+set = deepcopy(load_settings("system_3l.yaml"))
+# set.elevation = 71
+dt = 0.05
+total_time = 8.0
+
+steps = Int(round(total_time / dt))
+logger = Logger(3*set.segments + 6, steps)
+
+if !@isdefined s; s = KPS4_3L(KCU(set)); end
+s.set = update_settings()
+s.set.abs_tol = 0.0006
+s.set.rel_tol = 0.001
+s.set.l_tether = 50.0
+s.set.damping = 473
+s.set.elevation = 85
+println("init sim")
+@time KiteModels.init_sim!(s; prn=true, torque_control=false)
+# @time next_step!(s; set_values=[0.0, 0.0, 0.0], dt=2.0)
+println("vel ", mean(norm.(s.integrator[s.simple_sys.force])))
+sys_state = KiteModels.SysState(s)
+if sys_state.heading > pi
+    sys_state.heading -= 2*pi
+end
+
+println("stepping")
+total_step_time = 0.0
+toc()
+steering = [0.0, 0.0, 0.0]
+amount = 2.0
+sign = 1
+for i in 1:steps
+    time = (i-1) * dt
+    @show time
+    # println("vel ", norm(s.integrator[s.simple_sys.vel]))
+    global total_step_time, sys_state, steering
+    # steering = [0.0,0.0,1000.0] # left right middle
+    sign = ifelse(mod(floor(time-0.5), 2) == 0, 1, -1)
+    steering[1] -= sign * dt * amount
+    steering[2] += sign * dt * amount
+
+    if sys_state.heading > pi
+        sys_state.heading -= 2*pi
+    end
+    sys_state.var_01 =  rad2deg(s.flap_angle[1]) - 10
+    sys_state.var_02 =  rad2deg(s.flap_angle[2]) - 10
+    sys_state.var_03 =  s.pos[s.num_C][1]
+    sys_state.var_04 =  s.pos[s.num_D][1]
+    # sys_state.var_03 =  s.tether_lengths[1]
+    # sys_state.var_04 =  s.tether_lengths[2]
+    sys_state.var_05 =  s.tether_lengths[3]
+    sys_state.var_06 =  rad2deg(s.integrator[s.simple_sys.seg_flap_angle[div(s.set.aero_surfaces, 2)]] - s.integrator[s.simple_sys.aoa[div(s.set.aero_surfaces, 2)]])
+    sys_state.var_07 =  clamp(rad2deg(s.integrator[s.simple_sys.flap_vel[1]]), -20, 20)
+    sys_state.var_08 =  norm(s.D_C)
+    sys_state.var_09 =  norm(s.D_D)
+    sys_state.var_10 =  (s.integrator[s.simple_sys.vel[:, s.num_E-3]]) ⋅ s.e_z
+    sys_state.var_11 =  norm(s.integrator[s.simple_sys.vel[:, s.num_E-3]] .- (s.integrator[s.simple_sys.vel[:, s.num_E-3]]) ⋅ s.e_z)
+    # sys_state.var_09 =  norm(s.D_C + s.D_D)
+
+    # @show s.integrator[s.simple_sys.aoa[div(s.set.aero_surfaces, 2)]]
+
+    step_time = @elapsed next_step!(s; set_values=steering, dt=dt)
+    if time > total_time/2
+        total_step_time += step_time
+    end
+
+    KiteModels.update_sys_state!(sys_state, s)
+    if sys_state.heading > pi
+        sys_state.heading -= 2*pi
+    end
+    log!(logger, sys_state)
+    l = s.set.l_tether+10
+    plot2d(s.pos, time; zoom=true, front=false, xlim=(-l/2, l/2), ylim=(0, l))
+end
+
+times_reltime = (total_time/2) / total_step_time
+println("times realtime MTK model: ", times_reltime)
+# println("avg steptime MTK model:   ", total_step_time/steps)
+
+p=plotx(logger.time_vec, 
+            [logger.var_01_vec,  logger.var_02_vec], 
+            [logger.var_03_vec,  logger.var_04_vec], 
+            rad2deg.(logger.heading_vec), 
+            [logger.var_06_vec, logger.var_07_vec], 
+            [logger.var_08_vec, logger.var_09_vec],
+            [logger.var_10_vec, logger.var_11_vec]; 
+        ylabels=["Steering", "Pos", "heading [deg]", "Angle / Force", "Force", "Vel"], 
+        labels=[
+            ["Steering Pos C", "Steering Pos D"], 
+            ["X left", "X right"], 
+            "heading",
+            ["Flap angle", "Flap vel"] ,
+            ["Drag C", "Drag D"],
+            ["Vel par", "Vel perp"]],
+        fig="Steering and heading MTK model")
+display(p)