Feat/faster tether

.gitignore

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

Project.toml

@@ -6,34 +6,41 @@ version = "0.6.4"
 [deps]
 AtmosphericModels = "c59cac55-771d-4f45-b14d-1c681463a295"
 BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf"
+DataInterpolations = "82cc6244-b520-54b8-b5a6-8a565e85f1d0"
 Dierckx = "39dd38d3-220a-591b-8e3c-4c3a8c710a94"
 DiffEqBase = "2b5f629d-d688-5b77-993f-72d75c75574e"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 KitePodModels = "9de5dc81-f971-414a-927b-652b2f41c539"
 KiteUtils = "90980105-b163-44e5-ba9f-8b1c83bb0533"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56"
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a"
 Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
+Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
+Revise = "295af30f-e4ad-537b-8983-00126c2a3abe"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 Serialization = "9e88b42a-f829-5b0c-bbe9-9e923198166b"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
+SteadyStateDiffEq = "9672c7b4-1e72-59bd-8a11-6ac3964bc41f"
 Sundials = "c3572dad-4567-51f8-b174-8c6c989267f4"
+SymbolicIndexingInterface = "2efcf032-c050-4f8e-a9bb-153293bab1f5"
 WinchModels = "7dcfa46b-7979-4771-bbf4-0aee0da42e1f"
 
 [compat]
 AtmosphericModels = "0.2"
 BenchmarkTools = "1.2, 1.3"
 ControlPlots = "0.1.4"
 Dierckx = "0.5"
-DiffEqBase = "6.145"
+DiffEqBase = "6.152.2"
 DocStringExtensions = "0.8, 0.9"
 Documenter = "1.0"
 KitePodModels = "0.3.3"
 KiteUtils = "0.7.7"
+ModelingToolkit = "9.30.0"
 NLsolve = "4.5"
 OrdinaryDiffEq = "6.66, 6.74"
 PackageCompiler = "2.0"
@@ -44,6 +51,7 @@ Reexport = "1.1, 1.2"
 Rotations = "1.3"
 StaticArrays = "1.9"
 Sundials = "4.24"
+SymbolicIndexingInterface = "0.3.27"
 WinchModels = "0.3.2"
 julia = "1.10"
 

data/settings_3l.yaml

@@ -16,8 +16,8 @@ initial:
     depower:   25.0        # initial depower settings    [%]
 
 solver:
-    abs_tol: 0.006        # absolute tolerance of the DAE solver [m, m/s]
-    rel_tol: 0.01         # relative tolerance of the DAE solver [-]
+    abs_tol: 0.0006        # absolute tolerance of the DAE solver [m, m/s]
+    rel_tol: 0.001         # relative tolerance of the DAE solver [-]
     solver: "DFBDF"        # DAE solver, IDA or DImplicitEuler, DFBDF
     linear_solver: "GMRES" # can be GMRES or Dense or LapackDense (only for IDA)
     max_order: 4           # maximal order, usually between 3 and 5
@@ -62,7 +62,7 @@ kps4_3l:
     tip_length: 0.62
     min_steering_line_distance: 1.0
     width:         4.1     # width of the kite                          [m]
-    aero_surfaces: 3      # number of aerodynamic surfaces in 3 line model
+    aero_surfaces: 5      # number of aerodynamic surfaces in 3 line model
 
 kcu:
     kcu_mass: 8.4                # mass of the kite control unit   [kg]
@@ -90,13 +90,19 @@ tether:
                            # SK75: 109 to 132 GPa according to datasheet
 
 winch:
+    winch_model: "TorqueControlledMachine" # or TorqueControlledMachine
     max_force: 4000        # maximal (nominal) tether force; short overload allowed [N]
-    v_ro_max:  8.0         # maximal reel-out speed                      [m/s]
-    v_ro_min: -8.0         # minimal reel-out speed (=max reel-in speed) [m/s]
+    v_ro_max:  8.0         # maximal reel-out speed                          [m/s]
+    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²]
+    f_coulomb: 122.0       # coulomb friction                                [N]
+    c_vf: 30.6             # coefficient for the viscous friction            [Ns/m]
 
 environment:
-    v_wind: 15.51             # wind speed at reference height          [m/s]
-    v_wind_ref: [15.51, 0.0]  # wind speed vector at reference height   [m/s]
+    v_wind: 9.51             # wind speed at reference height          [m/s]
+    v_wind_ref: [9.51, 0.0]  # wind speed vector at reference height   [m/s]
     temp_ref: 15.0           # temperature at reference height         [°C]
     height_gnd: 0.0          # height of groundstation above see level [m]
     h_ref:  6.0              # reference height for the wind speed     [m]

examples/3l_kite_environment.jl

@@ -0,0 +1,130 @@
+module Environment
+
+using KiteModels, StaticArrays, LinearAlgebra, OrdinaryDiffEq, Parameters
+export reset, step, render
+
+const StateVec = MVector{11, Float32}
+
+@with_kw mutable struct Env
+    kcu::KCU = KCU(se())
+    s::KPS4_3L = KPS4_3L(kcu)
+    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)
+    sys_state::SysState = SysState(s)
+    state::StateVec = zeros(StateVec)
+    state_d::StateVec = zeros(StateVec)
+    state_dd::StateVec = zeros(StateVec)
+    last_state::StateVec = zeros(StateVec)
+    last_state_d::StateVec = zeros(StateVec)
+    wanted_elevation::Float64 = 0.0
+    wanted_azimuth::Float64 = 0.0
+    wanted_tether_length::Float64 = 0.0
+    max_force::Float64 = 0.0
+    rotation::Float64 = 0.0
+end
+
+const e = Env()
+
+function step(reel_out_torques; prn=false)
+    reel_out_torques = Vector{Float64}(reel_out_torques) .* 100
+
+    old_heading = calc_heading(e.s)
+    if prn
+        KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques, torque_control=false)
+    else
+        redirect_stdout(devnull) do
+            KiteModels.next_step!(e.s, e.integrator; set_values=reel_out_torques, torque_control=true)
+        end
+    end
+    println(e.s.pos[end])
+    _calc_rotation(old_heading, calc_heading(e.s))
+    update_sys_state!(e.sys_state, e.s)
+    e.i += 1
+    return _calc_state(e.s)
+end
+
+function reset(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)
+    e.max_force = Float32(force)
+    e.rotation = 0.0
+    if length(e.logger) > 1
+        name = String(name)
+        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.sys_state = SysState(e.s)
+    e.i = 1
+    return _calc_state(e.s)
+end
+
+function render()
+    if(e.i <= e.max_render_length)
+        log!(e.logger, e.sys_state)
+    end
+end
+
+function _calc_state(s::KPS4_3L)
+    _calc_reward(s)
+    e.state .= vcat(
+        _calc_reward(s),                # length 1
+        calc_orient_quat(s),            # length 4
+        s.tether_lengths,                    # length 3 # normalize to min and max 
+
+        KiteModels.calc_tether_elevation(s),       # length 1
+        KiteModels.calc_tether_azimuth(s),         # length 1
+        sum(winch_force(s)),            # length 1
+    )
+    if e.i == 1
+        e.state_d .= zeros(StateVec)
+    else
+        e.state_d .= (e.state .- e.last_state) / e.s.set.sample_freq
+    end
+    if e.i <= 2
+        e.state_dd .= zeros(StateVec)
+    else
+        e.state_dd .= (e.state_d .- e.last_state_d) / e.s.set.sample_freq
+    end
+    e.last_state_d .= e.state_d
+    e.last_state .= e.state
+    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 ||
+        !(-2*π < e.rotation < 2*π) ||
+        s.tether_lengths[1] > e.wanted_tether_length*1.5 ||
+        s.tether_lengths[1] < e.wanted_tether_length*0.95 ||
+        sum(winch_force(s)) > e.max_force)
+        return 0.0
+    end
+    force_component = _calc_force_component(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)
+    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])
+    force_component = tether_force ⋅ wanted_force_vector
+    return force_component
+end
+
+function _calc_rotation(old_heading, new_heading)
+    d_rotation = new_heading - old_heading
+    if d_rotation > 1
+        d_rotation -= 2*pi
+    elseif d_rotation < -1
+        d_rotation += 2*pi
+    end
+    e.rotation += d_rotation
+    return nothing
+end
+
+end

examples/torque_controlled_3l.jl

@@ -0,0 +1,76 @@
+using Revise, KiteModels, OrdinaryDiffEq, ControlPlots, LinearAlgebra, Plots
+using Base: summarysize
+# using SteadyStateDiffEq
+
+update_settings()
+set = se("system_3l.yaml")
+set.abs_tol = 0.006
+set.rel_tol = 0.01
+steps = 150
+dt = 1/set.sample_freq
+tspan   = (0.0, dt) 
+# plot2d([[0,0,0]], 0)
+
+steering = [5,5,-30.0]
+
+
+println("Running models")
+s = KPS4_3L(KCU(set))
+integrator = KiteModels.init_sim!(s; stiffness_factor=0.1, prn=false, mtk=false, torque_control=true)
+
+println("compiling")
+total_new_time = 0.0
+for i in 1:5
+    global total_new_time += @elapsed next_step!(s, integrator; set_values=steering)
+end
+println("stepping")
+total_old_time = 0.0
+total_new_time = 0.0
+steering_poss = [[],[]]
+reel_out_speedss = [[],[]]
+headings = []
+for i in 1:steps
+    if i == 1
+        global steering = [5,5,-30.0] # left right middle
+    end
+    if i == 20
+        global steering = [10,10,-30]
+    end
+    if i == 50
+        global steering = [0,10.0,-40]
+    end
+    # if i == 10
+    #     global steering = [-0.5,10.5,-0]
+    # end
+    # if i == 20
+    #     global steering = [10.5,10.5,13]
+    # end
+    # println(s.steering_pos, "\t tether_lengths \t", s.reel_out_speeds)
+    heading = calc_heading(s)
+    # println(s.steering_pos)
+    if heading > pi
+        heading -= 2*pi
+    end
+    push!(headings, heading)
+    push!(steering_poss[1], s.steering_pos[1])
+    push!(steering_poss[2], s.steering_pos[2])
+    push!(reel_out_speedss[1], s.reel_out_speeds[1])
+    push!(reel_out_speedss[2], s.reel_out_speeds[2])
+    # println(norm.(s.winch_forces))
+    global total_new_time += @elapsed next_step!(s, integrator; set_values=steering)
+    # plot2d(s.pos, i-1; zoom=false, front=true, segments=set.segments)
+end
+# plot2d(s1.pos, steps; zoom=false, front=false, segments=set.segments)
+
+new_time = (dt*steps) / total_new_time
+println("times realtime new model: ", new_time)
+println("avg steptime new model: ", total_new_time/steps)
+
+Plots.plot(1:steps, [steering_poss, reel_out_speedss, headings], 
+    label=["Steering Pos 1" "Steering Pos 2" "Reel Out Speed 1" "Reel Out Speed 2" "Heading"],
+    linewidth=3)
+
+# 0.02147723333217222 rad with 500 damping
+# damping was too much...
+
+# 0.14807810376419894 with 50 damping

examples/torque_controlled_mtk.jl

@@ -0,0 +1,77 @@
+using Revise, KiteModels, OrdinaryDiffEq, ControlPlots, LinearAlgebra, Plots
+using Base: summarysize
+# using SteadyStateDiffEq
+
+update_settings()
+set = se("system_3l.yaml")
+set.abs_tol = 0.006
+set.rel_tol = 0.01
+steps = 150
+dt = 1/set.sample_freq
+tspan   = (0.0, dt) 
+# plot2d([[0,0,0]], 0)
+
+steering = [5,5,-30.0]
+
+
+println("Running models")
+if ! @isdefined mtk_kite; mtk_kite = KPS4_3L(KCU(set)); end;
+if ! @isdefined mtk_integrator; mtk_integrator = KiteModels.init_sim!(mtk_kite; stiffness_factor=0.1, prn=false, mtk=true, torque_control=true);
+else mtk_integrator = KiteModels.reset_sim!(mtk_kite; stiffness_factor=1.0); end;
+# mtk_integrator = KiteModels.init_sim!(mtk_kite; stiffness_factor=0.1, prn=true, mtk=true, torque_control=true)
+
+println("compiling")
+total_new_time = 0.0
+for i in 1:5
+    global total_new_time += @elapsed next_step!(mtk_kite, mtk_integrator; set_values=steering)
+end
+println("stepping")
+total_old_time = 0.0
+total_new_time = 0.0
+steering_poss = [[],[]]
+reel_out_speedss = [[],[]]
+headings = []
+for i in 1:steps
+    if i == 1
+        global steering = [5,5,-30.0] # left right middle
+    end
+    if i == 20
+        global steering = [10,10,-30]
+    end
+    if i == 50
+        global steering = [0,10.0,-40]
+    end
+    # if i == 10
+    #     global steering = [-0.5,10.5,-0]
+    # end
+    # if i == 20
+    #     global steering = [10.5,10.5,13]
+    # end
+    # println(mtk_kite.steering_pos, "\t tether_lengths \t", mtk_kite.reel_out_speeds)
+    heading = calc_heading(mtk_kite)
+    # println(mtk_kite.steering_pos)
+    if heading > pi
+        heading -= 2*pi
+    end
+    push!(headings, heading)
+    push!(steering_poss[1], mtk_kite.steering_pos[1])
+    push!(steering_poss[2], mtk_kite.steering_pos[2])
+    push!(reel_out_speedss[1], mtk_kite.reel_out_speeds[1])
+    push!(reel_out_speedss[2], mtk_kite.reel_out_speeds[2])
+    # println(norm.(mtk_kite.winch_forces))
+    global total_new_time += @elapsed next_step!(mtk_kite, mtk_integrator; set_values=steering)
+    # plot2d(mtk_kite.pos, i-1; zoom=false, front=true, segments=set.segments)
+end
+
+new_time = (dt*steps) / total_new_time
+println("times realtime new model: ", new_time)
+println("avg steptime new model: ", total_new_time/steps)
+
+Plots.plot(1:steps, [steering_poss, reel_out_speedss, headings], 
+    label=["Steering Pos 1" "Steering Pos 2" "Reel Out Speed 1" "Reel Out Speed 2" "Heading"],
+    linewidth=3)
+
+# 0.02147723333217222 rad with 500 damping
+# damping was too much...
+
+# 0.14807810376419894 with 50 damping