Feat/xfoil polars (#76)

* use dierkcx instead of datainterpolations * pass motor to calc acc * getting there * add xfoil * add model formulas * make scripts * working model at around 1 times realtime * looks pretty good * oscillations because of no perpendicular damping * better creation and loading of polars * works but oscillates * working speed control * small fix * working example * proper init * works for shorter lines as well * working stable state * fast and works well * fix project toml * add better flap damping * fixed wrong ram force * improve ram pressure * faster init * add default foil shape file * faster solver * add error handling * add assertion * better polars * remove DataInterpolations * fix drag, oscillation and heading direction * add gravity to flaps * no stabilizing * solve overdetermined system * fix tests * use float32 * fix precompile and remove dtmin * use succes function * don't use reinit * make model! usable from outside package * set s.torque_control in init * add set_values to variables * temp remove modelingtoolkit version * update mtk to latest * remove mtk version requirements * remove datainterp * fix 2 segment model * rm true statement * change to bin and interpolations.jl * fixed tests * use smooth sign * fix rad deg mistake * fix annoying test * positive height * add bin and dat file * fix test data path --------- Co-authored-by: Uwe Fechner <uwe.fechner.msc@gmail.com> Co-authored-by: Uwe Fechner <fechner@aenarete.eu>
ufechner7 · Oct 4, 2024 · dba6998 · dba6998
1 parent c456936
commit dba6998
Show file tree

Hide file tree

Showing 18 changed files with 1,262 additions and 645 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,3 +1,4 @@
+*.obj
 *.xopp
 *.so
 *.so.bak

diff --git a/Project.toml b/Project.toml
@@ -5,10 +5,11 @@ 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"
+Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59"
 KitePodModels = "9de5dc81-f971-414a-927b-652b2f41c539"
 KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
@@ -29,20 +30,20 @@ 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.1"
-DataInterpolations = "6.2.0"
 Dierckx = "0.5"
 DiffEqBase = "6.152.2"
 DocStringExtensions = "0.8, 0.9"
 Documenter = "1.0"
 KitePodModels = "0.3.4"
 KiteUtils = "0.7.12"
 LaTeXStrings = "1.3.1"
-ModelingToolkit = "~9.38.0"
+ModelingToolkit = "9.41.0"
 NLsolve = "4.5"
 OrdinaryDiffEqBDF = "1.1.1"
 OrdinaryDiffEqCore = "1.4.0"

diff --git a/data/MH82.dat b/data/MH82.dat
@@ -0,0 +1,69 @@
+MH 82  13.31% - http://airfoiltools.com/airfoil/details?airfoil=mh82-il - polars created
+  1.00000000  0.00000000
+  0.99661459  0.00010716
+  0.98648759  0.00057417
+  0.96976242  0.00167959
+  0.94674359  0.00367565
+  0.91787545  0.00675919
+  0.88371761  0.01105153
+  0.84491677  0.01659048
+  0.80217789  0.02332527
+  0.75623764  0.03112511
+  0.70784007  0.03978495
+  0.65771739  0.04904355
+  0.60657341  0.05859424
+  0.55507239  0.06810553
+  0.50383005  0.07723291
+  0.45340892  0.08563711
+  0.40431563  0.09299494
+  0.35700062  0.09901144
+  0.31186073  0.10342997
+  0.26924218  0.10603084
+  0.22944828  0.10664471
+  0.19275386  0.10507496
+  0.15919742  0.10111480
+  0.12865471  0.09486478
+  0.10114620  0.08660842
+  0.07670830  0.07665259
+  0.05541508  0.06541510
+  0.03742189  0.05331020
+  0.02286177  0.04072192
+  0.01180557  0.02807192
+  0.00430956  0.01589603
+  0.00048929  0.00484950
+  0.00007086  0.00175419
+  0.00002622 -0.00101996
+  0.00034816 -0.00353243
+  0.00078039 -0.00502846
+  0.00143495 -0.00631603
+  0.00242331 -0.00744430
+  0.00376965 -0.00852044
+  0.00632469 -0.01007170
+  0.00946271 -0.01156191
+  0.01867771 -0.01481702
+  0.03671504 -0.01897587
+  0.05993648 -0.02230735
+  0.08812913 -0.02462698
+  0.12123607 -0.02596768
+  0.15896894 -0.02658615
+  0.20083798 -0.02664454
+  0.24631809 -0.02624375
+  0.29484983 -0.02546371
+  0.34584166 -0.02437799
+  0.39867639 -0.02304567
+  0.45271686 -0.02152507
+  0.50731291 -0.01986154
+  0.56180823 -0.01809995
+  0.61554743 -0.01627195
+  0.66788318 -0.01440561
+  0.71818292 -0.01251060
+  0.76584748 -0.01056846
+  0.81037800 -0.00854699
+  0.85134987 -0.00650607
+  0.88832129 -0.00457027
+  0.92082546 -0.00287905
+  0.94837899 -0.00154846
+  0.97049733 -0.00064794
+  0.98672053 -0.00016877
+  0.99665170 -0.00001402
+  1.00000000  0.00000000
diff --git a/data/polars.bin b/data/polars.bin
diff --git a/data/settings_3l.yaml b/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.bin"   # 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]
@@ -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]
 

diff --git a/examples/3l_kite_environment.jl b/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

diff --git a/examples/simple_3l_speed_control.jl b/examples/simple_3l_speed_control.jl
@@ -0,0 +1,93 @@
+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 = 9.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)
+
+println("stepping")
+total_step_time = 0.0
+toc()
+steering = [0.0, 0.0, 0.0]
+amount = 0.6
+sign = 1
+for i in 1:steps
+    time = (i-1) * dt
+    Core.println("time: ", time)
+    global total_step_time, sys_state, steering, sign
+    if s.tether_lengths[1] > s.tether_lengths[2] + 0.1
+        sign = -1
+    elseif s.tether_lengths[1] < s.tether_lengths[2] - 0.1
+        sign = 1
+    end
+    steering[1] += sign * dt * amount
+    steering[2] -= sign * dt * amount
+
+    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.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 =  rad2deg(s.integrator[s.simple_sys.flap_vel[1]])
+    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)
+
+    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)
+    log!(logger, sys_state)
+    l = s.set.l_tether+10
+    # plot2d(s.pos, time; zoom=true, front=true, 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", "Length", "heading [deg]", "Angle / Force", "Force", "Vel"], 
+        labels=[
+            ["Steering Pos C", "Steering Pos D"], 
+            ["Left tether", "Right tether"], 
+            "heading",
+            ["Flap angle", "Flap vel"] ,
+            ["Drag C", "Drag D"],
+            ["Vel par", "Vel perp"]],
+        fig="Steering and heading MTK model")
+display(p)