Fix/improve for mpc

Project.toml

@@ -44,10 +44,10 @@ Interpolations = "0.15"
 KitePodModels = "0.3.6"
 KiteUtils = "0.9.0"
 LaTeXStrings = "1.4.0"
-ModelingToolkit = "~9.42.0"
+ModelingToolkit = "9.49.0"
 NLsolve = "4.5"
-OrdinaryDiffEqBDF = "1.1.1"
-OrdinaryDiffEqCore = "1.4.0"
+OrdinaryDiffEqBDF = "1.1.2"
+OrdinaryDiffEqCore = "1.7.1"
 OrdinaryDiffEqSDIRK = "1.1.0"
 PackageCompiler = "2.0"
 Parameters = "0.12"

bin/create_sys_image

@@ -55,8 +55,7 @@ else
     exit 1
 fi
 
-totalmem=$(grep MemTotal /proc/meminfo | awk '{print $2 / 1024}')
-totalmem=${totalmem%.*}
+totalmem=$(grep MemTotal /proc/meminfo | awk '{printf "%i", $2 / 1024}')
 if [[ $totalmem -lt 27000 ]]; then
     echo "Warning: Less than 27GB of memory detected. Using only one thread for sysimage compilation."
     export JULIA_IMAGE_THREADS=1

bin/run_julia

@@ -13,7 +13,7 @@ if [[ $julia_major == "1.1" ]]; then
 fi
 
 if [ -d .git ] || git rev-parse --git-dir > /dev/null 2>&1 ; then
-    branch=$(git rev-parse --abbrev-ref HEAD)
+    branch=$(git rev-parse --abbrev-ref HEAD | sed 's/\//-/g')
 else
     branch=""
 fi

data/settings_3l.yaml

@@ -51,7 +51,7 @@ kite:
     cd_list:   [   0.5,    0.5,    0.5,   1.0,   0.2, 0.1,  0.2,  1.0,   0.5,   0.5,   0.5]
 
 kps4: 
-    alpha_zero:   10.0      # should be 4..10                      [degrees]
+    alpha_zero:   6.0      # should be 4..10                      [degrees]
     alpha_ztip:   10.0      #                                      [degrees]
     m_k:           0.2      # relative nose distance; increasing m_k increases C2 of the turn-rate law
     rel_nose_mass: 0.47     # relative nose mass

examples/simple_3l_control.jl

@@ -0,0 +1,99 @@
+using Revise, KiteModels, LinearAlgebra
+using Base: summarysize
+
+using Pkg
+if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+end
+using ControlPlots, StatsBase
+
+set = deepcopy(load_settings("system_3l.yaml"))
+# set.elevation = 71
+dt = 0.05
+total_time = 10.0
+
+steps = Int(round(total_time / dt))
+logger = Logger(3*set.segments + 6, steps)
+
+s::KPS4_3L = KPS4_3L(KCU(set))
+s.set = update_settings()
+s.set.abs_tol = 0.0006
+s.set.rel_tol = 0.001
+s.set.l_tether = 21.0
+# s.set.damping = 473
+s.set.elevation = 87
+init_set_values = [-0.1, -0.1, -120.0]
+@time KiteModels.init_sim!(s; prn=true, torque_control=true, init_set_values, ϵ=1e-3, flap_damping=0.1)
+# @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
+steering = init_set_values
+amount = 0.3
+sign = 1
+for i in 1:steps
+    global total_step_time, sys_state, steering, sign
+    time = (i-1) * dt
+    Core.println("time: ", time)
+    # if time > 1.0 steering = [-1, -20, -200.0] end
+    # if time > 5.0 steering = [-10, -1, -200.0] end
+    steering .= -winch_force(s)*0.11
+    if (2.0 > time > 0.0)  steering[2] += 5.0 end
+    if (2.0 > time > 0.0)  steering[1] -= 5.0 end
+    if (4.0 > time > 2.0)  steering[2] -= 1.0 end
+    if (4.0 > time > 2.0)  steering[1] += 1.0 end
+    if (10.0 > time > 4.0)  steering[2] -= 2.0 end
+    if (10.0 > time > 4.0)  steering[1] += 2.0 end
+
+    # if time < 1.0
+    #     steering[1] = 0.6
+    #     steering[2] = -0.6
+    # else
+    #     steering .= 0.0
+    # end
+
+    sys_state.var_01 =  rad2deg(s.get_flap_angle(s.integrator)[1])
+    sys_state.var_02 =  rad2deg(s.get_flap_angle(s.integrator)[2])
+    sys_state.var_03 =  rad2deg(s.integrator[s.simple_sys.power_angle])
+    sys_state.var_04 =  s.tether_lengths[1]
+    sys_state.var_05 =  s.tether_lengths[2]
+    sys_state.var_06 =  s.tether_lengths[3]
+    sys_state.var_07 =  s.integrator[s.simple_sys.turn_rate_y]
+    sys_state.var_08 =  s.integrator[s.simple_sys.heading_y]
+    sys_state.var_09 =  s.integrator[s.simple_sys.turn_rate_y] / (s.get_flap_angle(s.integrator)[2] - s.get_flap_angle(s.integrator)[1])
+    sys_state.var_10 =  (s.integrator[s.simple_sys.flap_vel][2] - s.integrator[s.simple_sys.flap_vel][1])
+    sys_state.var_11 =  clamp((s.integrator[s.simple_sys.flap_vel][1] - s.integrator[s.simple_sys.flap_vel][2]) /
+                            (s.get_tether_vels(s.integrator)[1] - s.get_tether_vels(s.integrator)[2]) * 100, -20, 10)
+
+    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=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,  logger.var_05_vec, logger.var_06_vec], 
+            [rad2deg.(logger.var_07_vec), rad2deg.(logger.var_08_vec)], 
+            [logger.var_09_vec, logger.var_11_vec],
+            [logger.var_10_vec]; 
+        ylabels=["steering", "length", "heading [deg]", "ratio", "velocity"], 
+        labels=[
+            ["steering pos C", "steering pos D", "power angle"], 
+            ["left tether", "right tether", "middle tether"], 
+            ["turn_rate_y", "heading_y"],
+            ["turn_rate / flap difference", "flap speed difference / tether speed difference"],
+            ["flap speed difference"]],
+        fig="Steering and heading MTK model")
+display(p)

scripts/load_polars.jl

@@ -9,28 +9,27 @@ alphas, d_flap_angles, cl_matrix, cd_matrix, c_te_matrix = deserialize(joinpath(
 
 function replace_nan!(matrix)
     rows, cols = size(matrix)
-
-    distance = 10
+    distance = max(rows, cols)
     for i in 1:rows
         for j in 1:cols
             if isnan(matrix[i, j])
                 neighbors = []
                 for d in 1:distance
                     found = false
                     if i-d >= 1 && !isnan(matrix[i-d, j]);
-                        push!(neighbors, matrix[i-1, j])
+                        push!(neighbors, matrix[i-d, j])
                         found = true
                     end
                     if i+d <= rows && !isnan(matrix[i+d, j])
-                        push!(neighbors, matrix[i+1, j])
+                        push!(neighbors, matrix[i+d, j])
                         found = true
                     end
                     if j-d >= 1 && !isnan(matrix[i, j-d])
-                        push!(neighbors, matrix[i, j-1])
+                        push!(neighbors, matrix[i, j-d])
                         found = true
                     end
                     if j+d <= cols && !isnan(matrix[i, j+d])
-                        push!(neighbors, matrix[i, j+1])
+                        push!(neighbors, matrix[i, j+d])
                         found = true
                     end
                     if found; break; end
@@ -48,9 +47,9 @@ replace_nan!(cl_matrix) # TODO: RAD2DEG
 replace_nan!(cd_matrix)
 replace_nan!(c_te_matrix)
 
-cl_interp = linear_interpolation((alphas, d_flap_angles), cl_matrix; extrapolation_bc = NaN)
-cd_interp = linear_interpolation((alphas, d_flap_angles), cd_matrix; extrapolation_bc = NaN)
-c_te_interp = linear_interpolation((alphas, d_flap_angles), c_te_matrix; extrapolation_bc = NaN)
+cl_interp = extrapolate(scale(interpolate(cl_matrix, BSpline(Quadratic())), alphas, d_flap_angles), NaN)
+cd_interp = extrapolate(scale(interpolate(cd_matrix, BSpline(Quadratic())), alphas, d_flap_angles), NaN)
+c_te_interp = extrapolate(scale(interpolate(c_te_matrix, BSpline(Quadratic())), alphas, d_flap_angles), NaN)
 
 function plot_values(alphas, d_flap_angles, matrix, interp, name)
     fig = plt.figure()
@@ -65,7 +64,7 @@ function plot_values(alphas, d_flap_angles, matrix, interp, name)
     X_int = collect(int_d_flap_angles) .+ zeros(length(int_alphas))'
     Y_int = collect(int_alphas)' .+ zeros(length(int_d_flap_angles))
 
-    # ax.plot_wireframe(X_data, Y_data, matrix, edgecolor="royalblue", lw=0.5, rstride=5, cstride=5, alpha=0.6)
+    ax.plot_wireframe(X_data, Y_data, matrix, edgecolor="royalblue", lw=0.5, rstride=5, cstride=5, alpha=0.6)
     ax.plot_wireframe(X_int, Y_int, interp_matrix, edgecolor="orange", lw=0.5, rstride=5, cstride=5, alpha=0.6)
     plt.xlabel("Alpha")
     plt.ylabel("Flap angle")
@@ -77,10 +76,11 @@ function plot_values(alphas, d_flap_angles, matrix, interp, name)
 end
 
 
-# plot_values(alphas, d_flap_angles, cl_matrix, cl_interp, "Cl")
-# plot_values(alphas, d_flap_angles, cd_matrix, cd_interp, "Cd")
-# plot_values(alphas, d_flap_angles, c_te_matrix, c_te_interp, "C_te")
+plot_values(alphas, d_flap_angles, cl_matrix, cl_interp, "Cl")
+plot_values(alphas, d_flap_angles, cd_matrix, cd_interp, "Cd")
+plot_values(alphas, d_flap_angles, c_te_matrix, c_te_interp, "C_te")
 display(plot(alphas, cd_interp.(alphas, 0.0)))
+# @show gradient(cd_interp, 10, 0)
 
 # @benchmark cd_interp(rad2deg(rand()),rad2deg(rand()))
 # Dierckx