diff --git a/.gitignore b/.gitignore index 20aec0bc..6ec0dc52 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,6 @@ *.xopp +*.so +*.so.bak /Manifest.toml /Manifest.toml.1.7 bin/kps-image-1.8-initial.so diff --git a/Project.toml b/Project.toml index 7434e668..d7f61940 100644 --- a/Project.toml +++ b/Project.toml @@ -6,22 +6,28 @@ 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] @@ -29,11 +35,12 @@ 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" diff --git a/data/settings_3l.yaml b/data/settings_3l.yaml index 9000d4c6..fc8c875a 100644 --- a/data/settings_3l.yaml +++ b/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] diff --git a/examples/3l_kite_environment.jl b/examples/3l_kite_environment.jl new file mode 100644 index 00000000..5b775b12 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/torque_controlled_3l.jl b/examples/torque_controlled_3l.jl new file mode 100644 index 00000000..7ac3adc9 --- /dev/null +++ b/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 diff --git a/examples/torque_controlled_mtk.jl b/examples/torque_controlled_mtk.jl new file mode 100644 index 00000000..f7be6176 --- /dev/null +++ b/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 diff --git a/src/KPS4_3L.jl b/src/KPS4_3L.jl index c5daf0d1..bdbd0dc1 100644 --- a/src/KPS4_3L.jl +++ b/src/KPS4_3L.jl @@ -68,6 +68,10 @@ $(TYPEDFIELDS) set::Settings "Reference to the atmospheric model as implemented in the package AtmosphericModels" am::AtmosphericModel = AtmosphericModel() + "Function for calculation the lift coefficent, using a spline based on the provided value pairs." + calc_cl = Spline1D(se().alpha_cl, se().cl_list) + "Function for calculation the drag coefficent, using a spline based on the provided value pairs." + calc_cd = Spline1D(se().alpha_cd, se().cd_list) "Reference to the motor models as implemented in the package WinchModels. index 1: middle motor, index 2: left motor, index 3: right motor" motors::SVector{3, AbstractWinchModel} "Iterations, number of calls to the function residual!" @@ -87,19 +91,20 @@ $(TYPEDFIELDS) "spring force of the current tether segment, output of calc_particle_forces!" spring_force::T = zeros(S, 3) "last winch force" - last_forces::SVector{3,T} = [zeros(S, 3) for _ in 1:3] + winch_forces::SVector{3,T} = [zeros(S, 3) for _ in 1:3] "a copy of the residual one (pos,vel) for debugging and unit tests" res1::SVector{P, T} = zeros(SVector{P, T}) "a copy of the residual two (vel,acc) for debugging and unit tests" res2::SVector{P, T} = zeros(SVector{P, T}) "a copy of the actual positions as output for the user" pos::SVector{P, T} = zeros(SVector{P, T}) + stable_pos::SVector{P, T} = zeros(SVector{P, T}) vel::SVector{P, T} = zeros(SVector{P, T}) posd::SVector{P, T} = zeros(SVector{P, T}) veld::SVector{P, T} = zeros(SVector{P, T}) "velocity vector of the kite" vel_kite::T = zeros(S, 3) - vel_connection::T = zeros(S, 3) + steering_vel::T = zeros(S, 3) "unstressed segment lengths of the three tethers [m]" segment_lengths::T = zeros(S, 3) "lift coefficient of the kite, depending on the angle of attack" @@ -115,9 +120,9 @@ $(TYPEDFIELDS) # "reel out speed at the last time step" # last_reel_out_speeds::T = zeros(S, 3) "unstretched tether length" - l_tethers::T = zeros(S, 3) + tether_lengths::T = zeros(S, 3) "lengths of the connections of the steering tethers to the kite" - l_connections::MVector{2, S} = zeros(S, 2) + steering_pos::MVector{2, S} = zeros(S, 2) "air density at the height of the kite" rho::S = 0.0 # "actual relative depower setting, must be between 0 .. 1.0" @@ -134,10 +139,8 @@ $(TYPEDFIELDS) springs::MVector{Q, SP} = zeros(SP, Q) "vector of the forces, acting on the particles" forces::SVector{P, T} = zeros(SVector{P, T}) - "synchronous speed of the motor/ generator" - set_speeds::KVec3 = zeros(KVec3) - "set_torque of the motor/generator" - set_torques::KVec3 = zeros(KVec3) + "synchronous speed or torque of the motor/ generator" + set_values::KVec3 = zeros(KVec3) torque_control::Bool = false "x vector of kite reference frame" e_x::T = zeros(S, 3) @@ -154,6 +157,21 @@ $(TYPEDFIELDS) num_D::Int64 = 0 "Point number of A" + "mtk variables" + mtk::Bool = false + + set_values_idx::Union{ModelingToolkit.ParameterIndex, Nothing} = nothing + v_wind_gnd_idx::Union{ModelingToolkit.ParameterIndex, Nothing} = nothing + prob::Union{OrdinaryDiffEq.ODEProblem, Nothing} = nothing + get_pos::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + get_steering_pos::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + get_line_acc::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + get_kite_vel::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + get_winch_forces::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + get_tether_lengths::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + get_tether_speeds::Union{SymbolicIndexingInterface.MultipleGetters, SymbolicIndexingInterface.TimeDependentObservedFunction, Nothing} = nothing + + half_drag_force::SVector{P, T} = zeros(SVector{P, T}) "residual variables" num_A::Int64 = 0 @@ -192,6 +210,7 @@ end Initialize the kite power model. """ function clear!(s::KPS4_3L) + s.iter = 0 s.t_0 = 0.0 # relative start time of the current time interval s.reel_out_speeds = zeros(3) # s.last_reel_out_speeds = zeros(3) @@ -201,11 +220,11 @@ function clear!(s::KPS4_3L) height = sin(deg2rad(s.set.elevation)) * (s.set.l_tether) s.v_wind .= s.v_wind_gnd * calc_wind_factor(s.am, height) - s.l_tethers .= [s.set.l_tether for _ in 1:3] + s.tether_lengths .= [s.set.l_tether for _ in 1:3] s.α_l = π/2 - s.set.min_steering_line_distance/(2*s.set.radius) s.α_r = π/2 + s.set.min_steering_line_distance/(2*s.set.radius) - s.segment_lengths .= s.l_tethers ./ s.set.segments + s.segment_lengths .= s.tether_lengths ./ s.set.segments s.num_E = s.set.segments*3+3 s.num_C = s.set.segments*3+3+1 s.num_D = s.set.segments*3+3+2 @@ -218,12 +237,19 @@ function clear!(s::KPS4_3L) s.drag_force .= [0.0, 0, 0] s.lift_force .= [0.0, 0, 0] s.rho = s.set.rho_0 + s.calc_cl = Spline1D(s.set.alpha_cl, s.set.cl_list) + s.calc_cd = Spline1D(s.set.alpha_cd, s.set.cd_list) end function KPS4_3L(kcu::KCU) set = kcu.set - s = KPS4_3L{SimFloat, KVec3, set.segments*3+2+KITE_PARTICLES, set.segments*3+KITE_SPRINGS_3L, SP}(set=kcu.set, motors=[AsyncMachine(set) for _ in 1:3]) + if set.winch_model == "TorqueControlledMachine" + s = KPS4_3L{SimFloat, KVec3, set.segments*3+2+KITE_PARTICLES, set.segments*3+KITE_SPRINGS_3L, SP}(set=kcu.set, motors=[TorqueControlledMachine(set) for _ in 1:3]) + println("Using torque control.") + else + s = KPS4_3L{SimFloat, KVec3, set.segments*3+2+KITE_PARTICLES, set.segments*3+KITE_SPRINGS_3L, SP}(set=kcu.set, motors=[AsyncMachine(set) for _ in 1:3]) + end s.num_E = s.set.segments*3+3 s.num_C = s.set.segments*3+3+1 s.num_D = s.set.segments*3+3+2 @@ -260,12 +286,12 @@ function update_sys_state!(ss::SysState, s::KPS4_3L, zoom=1.0) ss.orient .= calc_orient_quat(s) ss.elevation = calc_elevation(s) ss.azimuth = calc_azimuth(s) - ss.force = winch_force(s)[1] + ss.force = winch_force(s)[3] ss.heading = calc_heading(s) ss.course = calc_course(s) ss.v_app = norm(s.v_apparent) - ss.l_tether = s.l_tethers[1] - ss.v_reelout = s.reel_out_speeds[1] + ss.l_tether = s.tether_lengths[3] + ss.v_reelout = s.reel_out_speeds[3] ss.depower = 100 - ((s.δ_left + s.δ_right)/2) / ((s.set.middle_length + s.set.tip_length)/2) * 100 ss.steering = (s.δ_right - s.δ_left) / ((s.set.middle_length + s.set.tip_length)/2) * 100 ss.vel_kite .= s.vel_kite @@ -294,25 +320,40 @@ function SysState(s::KPS4_3L, zoom=1.0) t_sim = 0 depower = 100 - ((s.δ_left + s.δ_right)/2) / ((s.set.middle_length + s.set.tip_length)/2) * 100 steering = (s.δ_right - s.δ_left) / ((s.set.middle_length + s.set.tip_length)/2) * 100 - KiteUtils.SysState{P}(s.t_0, t_sim, 0, 0, orient, elevation, azimuth, s.l_tethers[1], s.reel_out_speeds[1], forces[1], depower, steering, + KiteUtils.SysState{P}(s.t_0, t_sim, 0, 0, orient, elevation, azimuth, s.tether_lengths[3], s.reel_out_speeds[3], forces[3], depower, steering, heading, course, v_app_norm, s.vel_kite, X, Y, Z, - s.l_tethers[2], s.l_tethers[3], s.reel_out_speeds[2], s.reel_out_speeds[3], + s.tether_lengths[1], s.tether_lengths[2], s.reel_out_speeds[1], s.reel_out_speeds[2], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) end -function next_step!(s::KPS4_3L, integrator; set_values=zeros(KVec3), torque_control=false, v_wind_gnd=s.set.v_wind, wind_dir=0.0, dt=1/s.set.sample_freq) - s.torque_control = torque_control - if !torque_control - s.set_speeds .= set_values - s.set_torques .= 0.0 - else - s.set_speeds .= 0.0 - s.set_torques .= set_values +function reset_sim!(s::KPS4_3L; stiffness_factor=0.035) + if s.mtk + clear!(s) + s.stiffness_factor = stiffness_factor + dt = 1/s.set.sample_freq + # 1. KenCarp4 + # TRBDF2, Rodas4P, Rodas5P, Kvaerno5, KenCarp4, radau, QNDF + integrator = OrdinaryDiffEq.init(s.prob, KenCarp4(autodiff=false); dt=dt, abstol=s.set.abs_tol, reltol=s.set.rel_tol, save_on=false) + update_pos!(s, integrator) + return integrator end - s.t_0 = integrator.t - set_v_wind_ground!(s, calc_height(s), v_wind_gnd, wind_dir) + println("Not an mtk model. Returning nothing.") + return nothing +end + +function next_step!(s::KPS4_3L, integrator; set_values=zeros(KVec3), v_wind_gnd=s.set.v_wind, wind_dir=0.0, dt=1/s.set.sample_freq) s.iter = 0 - if s.set.solver == "IDA" + set_v_wind_ground!(s, calc_height(s), v_wind_gnd, wind_dir) + s.set_values .= set_values + if s.mtk + integrator.ps[s.set_values_idx] .= set_values + integrator.ps[s.v_wind_gnd_idx] .= s.v_wind_gnd + end + s.t_0 = integrator.t + if s.mtk + OrdinaryDiffEq.step!(integrator, dt, true) + update_pos!(s, integrator) + elseif s.set.solver == "IDA" Sundials.step!(integrator, dt, true) else OrdinaryDiffEq.step!(integrator, dt, true) @@ -344,7 +385,7 @@ end Getter for the unstretched tether reel-out lenght (at zero force). """ -function unstretched_length(s::KPS4_3L) s.l_tethers[1] end +function unstretched_length(s::KPS4_3L) s.tether_lengths[3] end """ tether_length(s::KPS4_3L) @@ -447,8 +488,8 @@ function calc_aero_forces!(s::KPS4_3L, pos::SVector{N, KVec3}, vel::SVector{N, K s.lift_force .= s.L_C .+ s.L_D s.drag_force .= s.D_C .+ s.D_D - s.F_steering_c .= ((0.1 * (s.L_C ⋅ -s.e_z)) .* -s.e_z) - s.F_steering_d .= ((0.1 * (s.L_D ⋅ -s.e_z)) .* -s.e_z) + s.F_steering_c .= ((0.2 * (s.L_C ⋅ -s.e_z)) .* -s.e_z) + s.F_steering_d .= ((0.2 * (s.L_D ⋅ -s.e_z)) .* -s.e_z) s.forces[s.num_C] .+= (s.L_C .+ s.D_C) .- s.F_steering_c s.forces[s.num_D] .+= (s.L_D .+ s.D_D) .- s.F_steering_d s.forces[s.num_E-2] .+= s.F_steering_c @@ -494,10 +535,10 @@ The result is stored in the array s.forces. v_app_perp = s.v_apparent - s.v_apparent ⋅ unit_vector * unit_vector half_drag_force = (0.25 * rho * s.set.cd_tether * norm(v_app_perp) * area) * v_app_perp - + @inbounds s.forces[spring.p1] .+= half_drag_force + s.spring_force @inbounds s.forces[spring.p2] .+= half_drag_force - s.spring_force - if i <= 3 @inbounds s.last_forces[i%3+1] .= s.forces[spring.p1] end + if i <= 3 @inbounds s.winch_forces[(i-1)%3+1] .= s.forces[spring.p1] end nothing end @@ -531,7 +572,7 @@ Calculate the vectors s.res1 and calculate s.res2 using loops that iterate over all tether segments. """ function loop!(s::KPS4_3L, pos, vel, posd, veld) - L_0 = s.l_tethers[1] / s.set.segments + L_0 = s.tether_lengths / s.set.segments mass_per_meter = s.set.rho_tether * π * (s.set.d_tether/2000.0)^2 @@ -539,17 +580,17 @@ function loop!(s::KPS4_3L, pos, vel, posd, veld) s.res1[i] .= vel[i] .- posd[i] end # Compute the masses and forces - mass_tether_particle = mass_per_meter * s.segment_lengths[1] + mass_tether_particle = mass_per_meter .* s.segment_lengths # TODO: check if the next two lines are correct - damping = s.set.damping / L_0 - c_spring = s.set.c_spring / L_0 + damping = s.set.damping ./ L_0 + c_spring = s.set.c_spring ./ L_0 for i in 1:s.set.segments*3 - @inbounds s.masses[i] = mass_tether_particle - @inbounds s.springs[i] = SP(s.springs[i].p1, s.springs[i].p2, s.segment_lengths[i%3+1], c_spring, damping) + @inbounds s.masses[i] = mass_tether_particle[(i-1)%3+1] + @inbounds s.springs[i] = SP(s.springs[i].p1, s.springs[i].p2, s.segment_lengths[(i-1)%3+1], c_spring[(i-1)%3+1], damping[(i-1)%3+1]) end inner_loop!(s, pos, vel, s.v_wind_gnd, s.set.d_tether/1000.0) for i in s.num_E-2:s.num_E-1 - @inbounds s.forces[i] .+= SVector(0, 0, -G_EARTH) .+ 500.0 .* ((vel[i]-vel[s.num_C]) ⋅ s.e_z) .* s.e_z # TODO: more damping + @inbounds s.forces[i] .+= SVector(0, 0, -G_EARTH) .+ 500.0 .* ((vel[i]-vel[s.num_C]) ⋅ s.e_z) .* s.e_z F_xy = SVector(s.forces[i] .- (s.forces[i] ⋅ s.e_z) * s.e_z) @inbounds s.forces[i] .-= F_xy @inbounds s.forces[i+3] .+= F_xy @@ -653,8 +694,8 @@ function residual!(res, yd, y::MVector{S, SimFloat}, s::KPS4_3L, time) where S @assert isfinite(s.pos[4][3]) # core calculations - s.l_tethers .= lengths - s.l_connections .= connection_lengths + s.tether_lengths .= lengths + s.steering_pos .= connection_lengths s.segment_lengths .= lengths ./ s.set.segments calc_aero_forces!(s, s.pos, s.vel) loop!(s, s.pos, s.vel, s.posd, s.veld) @@ -662,11 +703,11 @@ function residual!(res, yd, y::MVector{S, SimFloat}, s::KPS4_3L, time) where S # winch calculations res[end-5:end-3] .= lengthsd .- reel_out_speeds for i in 1:3 - # @time res[end-3+i] = 1.0 - calc_acceleration(s.motors[i], 1.0, norm(s.forces[i%3+1]); set_speed=1.0, set_torque=s.set_torques[i], use_brake=true) + # @time res[end-3+i] = 1.0 - calc_acceleration(s.motors[i], 1.0, norm(s.forces[(i-1)%3+1]); set_speed=1.0, set_torque=s.set_values[i], use_brake=true) if !s.torque_control - res[end-3+i] = reel_out_speedsd[i] - calc_acceleration(s.motors[i], reel_out_speeds[i], norm(s.forces[i%3+1]); set_speed=s.set_speeds[i], set_torque=nothing, use_brake=true) + res[end-3+i] = reel_out_speedsd[i] - calc_acceleration(s.motors[i], reel_out_speeds[i], norm(s.forces[(i-1)%3+1]); set_speed=s.set_values[i], set_torque=nothing, use_brake=true) else - res[end-3+i] = reel_out_speedsd[i] - calc_acceleration(s.motors[i], reel_out_speeds[i], norm(s.forces[i%3+1]); set_speed=nothing, set_torque=s.set_torques[i], use_brake=true) + res[end-3+i] = reel_out_speedsd[i] - calc_acceleration(s.motors[i], reel_out_speeds[i], norm(s.forces[(i-1)%3+1]); set_speed=nothing, set_torque=s.set_values[i], use_brake=true) end end @@ -685,12 +726,12 @@ function residual!(res, yd, y::MVector{S, SimFloat}, s::KPS4_3L, time) where S # add connection residuals res[3*num_particles+1] = (s.res1[s.num_E-2]) ⋅ s.e_z - (s.res1[s.num_C] ⋅ s.e_z) - res[3*num_particles+2] = (s.res1[s.num_E-1]) ⋅ s.e_z - (s.res1[s.num_C] ⋅ s.e_z) + res[3*num_particles+2] = (s.res1[s.num_E-1]) ⋅ s.e_z - (s.res1[s.num_D] ⋅ s.e_z) res[6*num_particles+3] = (s.res2[s.num_E-2]) ⋅ s.e_z - (s.res2[s.num_C] ⋅ s.e_z) - res[6*num_particles+4] = (s.res2[s.num_E-1]) ⋅ s.e_z - (s.res2[s.num_C] ⋅ s.e_z) + res[6*num_particles+4] = (s.res2[s.num_E-1]) ⋅ s.e_z - (s.res2[s.num_D] ⋅ s.e_z) s.vel_kite .= s.vel[s.num_A] - s.vel_connection .= ((s.vel[s.num_E-2]-s.vel[s.num_C]) ⋅ s.e_z) + s.steering_vel .= ((s.vel[s.num_E-2]-s.vel[s.num_C]) ⋅ s.e_z) s.reel_out_speeds .= reel_out_speeds @assert isfinite(norm(res)) @@ -733,7 +774,7 @@ end Return the absolute value of the force at the winch as calculated during the last timestep. """ -function winch_force(s::KPS4_3L) norm.(s.last_forces) end +function winch_force(s::KPS4_3L) norm.(s.winch_forces) end # ==================== end of getter functions ================================================ @@ -741,7 +782,7 @@ function winch_force(s::KPS4_3L) norm.(s.last_forces) end function spring_forces(s::KPS4_3L) forces = zeros(SimFloat, s.num_A) for i in 1:s.set.segments*3 - forces[i] = s.springs[i].c_spring * (norm(s.pos[i+3] - s.pos[i]) - s.segment_lengths[i%3+1]) * s.stiffness_factor + forces[i] = s.springs[i].c_spring * (norm(s.pos[i+3] - s.pos[i]) - s.segment_lengths[(i-1)%3+1]) * s.stiffness_factor if forces[i] > 4000.0 println("Tether raptures for segment $i !") end @@ -822,5 +863,9 @@ function find_steady_state!(s::KPS4_3L; prn=false, delta = 0.0, stiffness_factor X00 = zeros(SimFloat, 5*s.set.segments+3) results = nlsolve(test_initial_condition!, X00, autoscale=true, xtol=2e-7, ftol=2e-7, iterations=s.set.max_iter) if prn println("\nresult: $results") end - init(s, results.zero) + if s.mtk + return init_pos(s, results.zero), nothing + else + return init(s, results.zero) + end end diff --git a/src/KPS4_3L_MTK.jl b/src/KPS4_3L_MTK.jl new file mode 100644 index 00000000..d5fc4fc6 --- /dev/null +++ b/src/KPS4_3L_MTK.jl @@ -0,0 +1,452 @@ + +function calc_acc_speed(tether_speed::SimFloat, norm_::SimFloat, set_speed::SimFloat) + return calc_acceleration(AsyncMachine(se("system_3l.yaml")), tether_speed, norm_; set_speed=set_speed, set_torque=nothing, use_brake=false) +end +@register_symbolic calc_acc_speed(tether_speed, norm_, set_speed) + +function calc_acc_torque(tether_speed::SimFloat, norm_::SimFloat, set_torque::SimFloat) # should have access to s somehow + return calc_acceleration(TorqueControlledMachine(se("system_3l.yaml")), tether_speed, norm_; set_speed=nothing, set_torque=set_torque, use_brake=false) +end +@register_symbolic calc_acc_torque(tether_speed, norm_, set_torque) + +""" + calc_aero_forces!(s::KPS4_3L, pos, vel) + +Calculates the aerodynamic forces acting on the kite particles. + +Parameters: +- pos: vector of the particle positions +- vel: vector of the particle velocities +- rho: air density [kg/m^3] +- rel_depower: value between 0.0 and 1.0 +- alpha_depower: depower angle [degrees] +- rel_steering: value between -1.0 and +1.0 + +Updates the vector s.forces of the first parameter. +""" +function calc_aero_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos, vel, t, e_x, e_y, e_z, rho) + n = s.set.aero_surfaces + @variables begin + δ_left(t) + δ_right(t) + E_c(t)[1:3] + v_cx(t)[1:3] + v_dx(t)[1:3] + v_dy(t)[1:3] + v_dz(t)[1:3] + v_cy(t)[1:3] + v_cz(t)[1:3] + y_lc(t) + y_ld(t) + end + E_c = collect(E_c) + v_cx = collect(v_cx) + v_dx = collect(v_dx) + v_dy = collect(v_dy) + v_dz = collect(v_dz) + v_cy = collect(v_cy) + v_cz = collect(v_cz) + + eqs2 = [ + eqs2 + δ_left ~ (pos[:,s.num_E-2].-pos[:,s.num_C]) ⋅ e_z + δ_right ~ (pos[:,s.num_E-1].-pos[:,s.num_D]) ⋅ e_z + E_c .~ pos[:,s.num_E] .+ e_z .* (-s.set.bridle_center_distance + s.set.radius) # in the aero calculations, E_c is the center of the circle shape on which the kite lies + v_cx .~ dot(vel[:,s.num_C], e_x).*e_x + v_dx .~ dot(vel[:,s.num_D], e_x).*e_x + v_dy .~ dot(vel[:,s.num_D], e_y).*e_y + v_dz .~ dot(vel[:,s.num_D], e_z).*e_z + v_cy .~ dot(vel[:,s.num_C], e_y).*e_y + v_cz .~ dot(vel[:,s.num_C], e_z).*e_z + y_lc ~ norm(pos[:,s.num_C] .- 0.5 .* (pos[:,s.num_C].+pos[:,s.num_D])) + y_ld ~ -norm(pos[:,s.num_D] .- 0.5 .* (pos[:,s.num_C].+pos[:,s.num_D])) + ] + + # integrating loop variables + @variables begin + F(t)[1:3, 1:n*2] + e_r(t)[1:3, 1:n*2] + y_l(t)[1:n*2] + v_kite(t)[1:3, 1:n*2] + v_a(t)[1:3, 1:n*2] + e_drift(t)[1:3, 1:n*2] + v_a_xr(t)[1:3, 1:n*2] + aoa(t)[1:n*2] + dL_dα(t)[1:3, 1:n*2] + dD_dα(t)[1:3, 1:n*2] + L_C(t)[1:3] + L_D(t)[1:3] + D_C(t)[1:3] + D_D(t)[1:3] + F_steering_c(t)[1:3] + F_steering_d(t)[1:3] + d(t)[1:n*2] + end + F = collect(F) + e_r = collect(e_r) + y_l = collect(y_l) + v_kite = collect(v_kite) + v_a = collect(v_a) + e_drift = collect(e_drift) + v_a_xr = collect(v_a_xr) + aoa = collect(aoa) + dL_dα = collect(dL_dα) + dD_dα = collect(dD_dα) + L_C = collect(L_C) + L_D = collect(L_D) + D_C = collect(D_C) + D_D = collect(D_D) + l_c_eq::SizedArray{Tuple{3}, Symbolics.Equation} = SizedArray{Tuple{3}, Symbolics.Equation}(collect(L_C .~ 0)) + l_d_eq::SizedArray{Tuple{3}, Symbolics.Equation} = SizedArray{Tuple{3}, Symbolics.Equation}(collect(L_D .~ 0)) + d_c_eq::SizedArray{Tuple{3}, Symbolics.Equation} = SizedArray{Tuple{3}, Symbolics.Equation}(collect(D_C .~ 0)) + d_d_eq::SizedArray{Tuple{3}, Symbolics.Equation} = SizedArray{Tuple{3}, Symbolics.Equation}(collect(D_D .~ 0)) + F_steering_c = collect(F_steering_c) + F_steering_d = collect(F_steering_d) + kite_length = zeros(MVector{n*2, SimFloat}) + α = zero(SimFloat) + α_0 = zero(SimFloat) + α_middle = zero(SimFloat) + dα = zero(SimFloat) + # Calculate the lift and drag + α_0 = pi/2 - s.set.width/2/s.set.radius + α_middle = pi/2 + dα = (α_middle - α_0) / n + for i in 1:n*2 + if i <= n + α = α_0 + -dα/2 + i*dα + else + α = pi - (α_0 + -dα/2 + (i-n)*dα) + end + + eqs2 = [ + eqs2 + F[:,i] ~ E_c .+ e_y.*cos(α).*s.set.radius .- e_z.*sin(α).*s.set.radius + e_r[:,i] ~ (E_c .- F[:,i])./norm(E_c .- F[:,i]) + y_l[i] ~ cos(α) * s.set.radius + α < π/2 ? + v_kite[:,i] ~ ((v_cx .- v_dx)./(y_lc .- y_ld).*(y_l[i] .- y_ld) .+ v_dx) .+ v_cy .+ v_cz : + v_kite[:,i] ~ ((v_cx .- v_dx)./(y_lc .- y_ld).*(y_l[i] .- y_ld) .+ v_dx) .+ v_dy .+ v_dz + v_a[:,i] ~ s.v_wind .- v_kite[:,i] + e_drift[:,i] ~ (e_r[:,i] × e_x) + v_a_xr[:,i] ~ v_a[:,i] .- (v_a[:,i] ⋅ e_drift[:,i]) .* e_drift[:,i] + ] + if α < π/2 + kite_length[i] = (s.set.tip_length + (s.set.middle_length-s.set.tip_length)*α*s.set.radius/(0.5*s.set.width)) + else + kite_length[i] = (s.set.tip_length + (s.set.middle_length-s.set.tip_length)*(π-α)*s.set.radius/(0.5*s.set.width)) + end + eqs2 = [ + eqs2 + α < s.α_l ? + d[i] ~ δ_left : + α > s.α_r ? + d[i] ~ δ_right : + d[i] ~ (δ_right - δ_left) / (s.α_r - s.α_l) * (α - s.α_l) + (δ_left) + aoa[i] ~ -asin((v_a_xr[:,i]./norm(v_a_xr[:,i])) ⋅ e_r[:,i]) + asin(clamp(d[i]/kite_length[i], -1.0, 1.0)) + dL_dα[:,i] .~ 0.5*rho*(norm(v_a_xr[:,i]))^2*s.set.radius*kite_length[i]*rad_cl_mtk(aoa[i]) .* ((v_a_xr[:,i] × e_drift[:,i]) ./ norm(v_a_xr[:,i] × e_drift[:,i])) + dD_dα[:,i] .~ 0.5*rho*norm(v_a_xr[:,i])*s.set.radius*kite_length[i]*rad_cd_mtk(aoa[i]) .* v_a_xr[:,i] # the sideways drag cannot be calculated with the C_d formula + ] + if i <= n + [l_c_eq[j] = (L_C[j] ~ l_c_eq[j].rhs + dL_dα[j,i] * dα) for j in 1:3] + [d_c_eq[j] = (D_C[j] ~ d_c_eq[j].rhs + dD_dα[j,i] * dα) for j in 1:3] + else + [l_d_eq[j] = (L_D[j] ~ l_d_eq[j].rhs + dL_dα[j,i] * dα) for j in 1:3] + [d_d_eq[j] = (D_D[j] ~ d_d_eq[j].rhs + dD_dα[j,i] * dα) for j in 1:3] + end + end + + eqs2 = [ + eqs2 + l_c_eq + d_c_eq + l_d_eq + d_d_eq + F_steering_c ~ ((0.2 * (L_C ⋅ -e_z)) .* -e_z) + F_steering_d ~ ((0.2 * (L_D ⋅ -e_z)) .* -e_z) + ] + + force_eqs[:,s.num_C] .= (force[:,s.num_C] .~ (L_C .+ D_C) .- F_steering_c) + force_eqs[:,s.num_D] .= (force[:,s.num_D] .~ (L_D .+ D_D) .- F_steering_d) + force_eqs[:,s.num_E-2] .= (force[:,s.num_E-2] .~ F_steering_c) + force_eqs[:,s.num_E-1] .= (force[:,s.num_E-1] .~ F_steering_d) + return eqs2, force_eqs +end + +""" + calc_particle_forces!(s::KPS4_3L, pos1, pos2, vel1, vel2, spring, d_tether, rho, i) + +Calculate the drag force and spring force of the tether segment, defined by the parameters pos1, pos2, vel1 and vel2 +and distribute it equally on the two particles, that are attached to the segment. +The result is stored in the array s.forces. +""" +@inline function calc_particle_forces_mtk!(s::KPS4_3L, eqs2, force_eqs, force, pos1, pos2, vel1, vel2, length, c_spring, damping, rho, i, + l_0, k, c, segment, rel_vel, av_vel, norm1, unit_vector, k1, k2, c1, spring_vel, + spring_force, v_apparent, v_wind_tether, area, v_app_perp, half_drag_force) + d_tether = s.set.d_tether/1000.0 + eqs2 = [ + eqs2 + i <= s.set.segments*3 ? l_0 ~ length[(i-1)%3+1] : l_0 ~ s.springs[i].length # Unstressed length + i <= s.set.segments*3 ? k ~ c_spring[(i-1)%3+1] * s.stiffness_factor : k ~ s.springs[i].c_spring * s.stiffness_factor # Spring constant + i <= s.set.segments*3 ? c ~ damping[(i-1)%3+1] : c ~ s.springs[i].damping # Damping coefficient + segment .~ pos1 - pos2 + rel_vel .~ vel1 - vel2 + av_vel .~ 0.5 * (vel1 + vel2) + norm1 ~ norm(segment) + unit_vector .~ segment / norm1 + k1 ~ 0.25 * k # compression stiffness kite segments + k2 ~ 0.1 * k # compression stiffness tether segments + c1 ~ 6.0 * c # damping kite segments + spring_vel .~ rel_vel ⋅ unit_vector + ] + + if i >= s.num_E-2 # kite springs + for j in 1:3 + eqs2 = [ + eqs2 + spring_force[j] ~ ifelse( + (norm1 - l_0) > 0.0, + (k*(l_0 - norm1) - c1 * spring_vel) * unit_vector[j], + (k1*(l_0 - norm1) - c * spring_vel) * unit_vector[j] + ) + ] + end + else + for j in 1:3 + eqs2 = [ + eqs2 + spring_force[j] ~ ifelse( + (norm1 - l_0) > 0.0, + (k*(l_0 - norm1) - c * spring_vel) * unit_vector[j], + (k2*(l_0 - norm1) - c * spring_vel) * unit_vector[j] + ) + ] + end + end + eqs2 = [ + eqs2 + v_apparent .~ v_wind_tether .- av_vel + area ~ norm1 * d_tether + v_app_perp ~ v_apparent .- v_apparent ⋅ unit_vector * unit_vector + half_drag_force .~ (0.25 * rho * s.set.cd_tether * norm(v_app_perp) * area) .* v_app_perp + ] + + for j in 1:3 + force_eqs[j, s.springs[i].p1] = (force[j,s.springs[i].p1] ~ force_eqs[j, s.springs[i].p1].rhs + (half_drag_force[j] + spring_force[j])) + force_eqs[j, s.springs[i].p2] = (force[j,s.springs[i].p2] ~ force_eqs[j, s.springs[i].p2].rhs + (half_drag_force[j] - spring_force[j])) + end + + # if i <= 3 @inbounds s.last_forces[(i-1)%3+1] .~ s.forces[spring.p1] end + return eqs2, force_eqs +end + + + +"""calc_aero_forces + inner_loop_mtk!(s::KPS4_3L, pos, vel, v_wind_gnd, segments, d_tether) + +Calculate the forces, acting on all particles. + +Output:length +- s.forces +- s.v_wind_tether +""" +@inline function inner_loop_mtk!(s::KPS4_3L, eqs2, force_eqs, t, force, pos, vel, length, c_spring, damping, v_wind_gnd) + @variables begin + height(t)[eachindex(s.springs)] + rho(t)[eachindex(s.springs)] + v_wind_tether(t)[1:3, eachindex(s.springs)] + + l_0(t)[eachindex(s.springs)] + k(t)[eachindex(s.springs)] + c(t)[eachindex(s.springs)] + segment(t)[1:3,eachindex(s.springs)] + rel_vel(t)[1:3, eachindex(s.springs)] + av_vel(t)[1:3, eachindex(s.springs)] + norm1(t)[eachindex(s.springs)] + unit_vector(t)[1:3, eachindex(s.springs)] + k1(t)[eachindex(s.springs)] + k2(t)[eachindex(s.springs)] + c1(t)[eachindex(s.springs)] + spring_vel(t)[eachindex(s.springs)] + spring_force(t)[1:3, eachindex(s.springs)] + v_apparent(t)[1:3, eachindex(s.springs)] + area(t)[eachindex(s.springs)] + v_app_perp(t)[1:3, eachindex(s.springs)] + half_drag_force(t)[1:3, eachindex(s.springs)] + end + v_wind_tether = collect(v_wind_tether) + segment = collect(segment) + rel_vel = collect(rel_vel) + av_vel = collect(av_vel) + unit_vector = collect(unit_vector) + spring_force = collect(spring_force) + v_apparent = collect(v_apparent) + v_app_perp = collect(v_app_perp) + half_drag_force = collect(half_drag_force) + + for i in eachindex(s.springs) + p1 = s.springs[i].p1 # First point nr. + p2 = s.springs[i].p2 + eqs2 = [ + eqs2 + height[i] ~ 0.5 * (pos[:,p1][3] + pos[:,p2][3]) + rho[i] ~ calc_rho(s.am, height[i]) + v_wind_tether[:,i] .~ calc_wind_factor(s.am, height[i]) .* v_wind_gnd + ] + + # TODO: @assert height > 0 + eqs2, force_eqs = calc_particle_forces_mtk!(s, eqs2, force_eqs, force, pos[:,p1], pos[:,p2], vel[:,p1], vel[:,p2], length, c_spring, damping, rho[i], i, + l_0[i], k[i], c[i], segment[:,i], rel_vel[:,i], av_vel[:,i], norm1[i], + unit_vector[:,i], k1[i], k2[i], c1[i], spring_vel[i], + spring_force[:,i], v_apparent[:,i], v_wind_tether[:,i], area[i], v_app_perp[:,i], half_drag_force[:,i]) + end + + return eqs2, force_eqs +end + +function update_pos!(s, integrator) + pos = s.get_pos(integrator) + s.steering_pos .= s.get_steering_pos(integrator) + [s.pos[i] .= pos[:,i] for i in 1:s.num_A] + s.veld[s.num_E-2] .= s.get_line_acc(integrator) + s.vel_kite .= s.get_kite_vel(integrator) + winch_forces = s.get_winch_forces(integrator) + [s.winch_forces[i] .= (winch_forces[:,i]) for i in 1:3] + s.tether_lengths .= s.get_tether_lengths(integrator) + s.reel_out_speeds .= s.get_tether_speeds(integrator) + calc_kite_ref_frame!(s, s.pos[s.num_E], s.pos[s.num_C], s.pos[s.num_D]) + + @assert all(abs.(s.steering_pos) .<= s.set.tip_length) + nothing +end + +function model!(s::KPS4_3L, pos_; torque_control=false) + pos2_ = zeros(3, s.num_A) + [pos2_[:,i] .= pos_[i] for i in 1:s.num_A] + @parameters begin + set_values[1:3] = s.set_values + v_wind_gnd[1:3] = s.v_wind_gnd + end + @independent_variables t + @variables begin + pos(t)[1:3, 1:s.num_A] = pos2_ # left right middle + vel(t)[1:3, 1:s.num_A] = zeros(3, s.num_A) # left right middle + acc(t)[1:3, 1:s.num_A] = zeros(3, s.num_A) # left right middle + tether_length(t)[1:3] = s.tether_lengths + steering_pos(t)[1:2] = s.steering_pos + steering_vel(t)[1:2] = zeros(2) + steering_acc(t)[1:2] = zeros(2) + tether_speed(t)[1:3] = zeros(3) # left right middle + segment_length(t)[1:3] = zeros(3) # left right middle + mass_tether_particle(t)[1:3] # left right middle + damping(t)[1:3] = s.set.damping ./ s.tether_lengths ./ s.set.segments # left right middle + c_spring(t)[1:3] = s.set.c_spring ./ s.tether_lengths ./ s.set.segments # left right middle + P_c(t)[1:3] = 0.5 .* (s.pos[s.num_C]+s.pos[s.num_D]) + e_x(t)[1:3] + e_y(t)[1:3] + e_z(t)[1:3] + force(t)[1:3, 1:s.num_A] = zeros(3, s.num_A) # left right middle + rho_kite(t) = 0.0 + end + # Collect the arrays into variables + pos = collect(pos) + vel = collect(vel) + acc = collect(acc) + v_wind_gnd = collect(v_wind_gnd) + tether_length = collect(tether_length) + steering_pos = collect(steering_pos) + steering_vel = collect(steering_vel) + steering_acc = collect(steering_acc) + tether_speed = collect(tether_speed) + segment_length = collect(segment_length) + mass_tether_particle = collect(mass_tether_particle) + damping = collect(damping) + c_spring = collect(c_spring) + P_c = collect(P_c) + e_x = collect(e_x) + e_y = collect(e_y) + e_z = collect(e_z) + force = collect(force) + + D = Differential(t) + + eqs1 = [] + mass_per_meter = s.set.rho_tether * π * (s.set.d_tether/2000.0)^2 + + [eqs1 = vcat(eqs1, D.(pos[:,i]) .~ 0.0) for i in 1:3] + [eqs1 = vcat(eqs1, D.(pos[:,i]) .~ vel[:,i]) for i in 4:s.num_E-3] + # println("div eqs1 segments ", div()) + eqs1 = [eqs1; D.(steering_pos) .~ steering_vel] + [eqs1 = vcat(eqs1, D.(pos[:,i]) .~ vel[:,i]) for i in s.num_E:s.num_A] + [eqs1 = vcat(eqs1, D.(vel[:,i]) .~ 0.0) for i in 1:3] + [eqs1 = vcat(eqs1, D.(vel[:,i]) .~ acc[:,i]) for i in 4:s.num_E-3] + eqs1 = [eqs1; D.(steering_vel) .~ steering_acc] + [eqs1 = vcat(eqs1, D.(vel[:,i]) .~ acc[:,i]) for i in s.num_E:s.num_A] + + eqs1 = vcat(eqs1, D.(tether_length) .~ tether_speed) + if torque_control + eqs1 = vcat(eqs1, D.(tether_speed) .~ [calc_acc_torque(tether_speed[i], norm(force[:,(i-1)%3+1]), set_values[i]) for i in 1:3]) + else + eqs1 = vcat(eqs1, D.(tether_speed) .~ [calc_acc_speed(tether_speed[i], norm(force[:,(i-1)%3+1]), set_values[i]) for i in 1:3]) + end + + # Compute the masses and forces + eqs2 = [] + force_eqs::SizedArray{Tuple{3, s.num_A}, Symbolics.Equation} = SizedArray{Tuple{3, s.num_A}, Symbolics.Equation}(undef) + force_eqs[:,:] .= (force[:,:] .~ 0) + + eqs2 = [ + eqs2 + pos[:,s.num_E-2] .~ pos[:,s.num_C] .+ e_z .* steering_pos[1] + pos[:,s.num_E-1] .~ pos[:,s.num_D] .+ e_z .* steering_pos[2] + vel[:,s.num_E-2] .~ vel[:,s.num_C] .+ e_z .* steering_vel[1] + vel[:,s.num_E-1] .~ vel[:,s.num_D] .+ e_z .* steering_vel[2] + acc[:,s.num_E-2] .~ acc[:,s.num_C] .+ e_z .* steering_acc[1] + acc[:,s.num_E-1] .~ acc[:,s.num_D] .+ e_z .* steering_acc[2] + segment_length .~ tether_length ./ s.set.segments + mass_tether_particle .~ mass_per_meter .* segment_length + damping .~ s.set.damping ./ segment_length + c_spring .~ s.set.c_spring ./ segment_length + P_c ~ 0.5 .* (pos[:,s.num_C]+pos[:,s.num_D]) + e_y .~ (pos[:,s.num_C] .- pos[:,s.num_D]) ./ norm(pos[:,s.num_C] .- pos[:,s.num_D]) + e_z .~ (pos[:,s.num_E] .- P_c) ./ norm(pos[:,s.num_E] .- P_c) + e_x .~ cross(e_y, e_z) + rho_kite ~ calc_rho(s.am, pos[3,s.num_A]) + ] + + eqs2, force_eqs = calc_aero_forces_mtk!(s, eqs2, force_eqs, force, pos, vel, t, e_x, e_y, e_z, rho_kite) + eqs2, force_eqs = inner_loop_mtk!(s, eqs2, force_eqs, t, force, pos, vel, segment_length, c_spring, damping, v_wind_gnd) + + for i in 1:3 + eqs2 = vcat(eqs2, vcat(force_eqs[:,i])) + eqs2 = vcat(eqs2, acc[:,i] .~ 0) + end + for i in 4:s.num_E-3 + eqs2 = vcat(eqs2, vcat(force_eqs[:,i])) + eqs2 = vcat(eqs2, acc[:,i] .~ [0.0; 0.0; -9.81] .+ (force[:,i] ./ mass_tether_particle[(i-1)%3+1])) + end + for i in s.num_E-2:s.num_E-1 + flap_resistance = [50.0 * ((vel[:,i]-vel[:,s.num_C]) ⋅ e_z) * e_z[j] for j in 1:3] + [force_eqs[j,i] = force[j,i] ~ force_eqs[j,i].rhs + [0.0; 0.0; -9.81][j] + flap_resistance[j] for j in 1:3] + tether_rhs = [force_eqs[j,i].rhs for j in 1:3] + kite_rhs = [force_eqs[j,i+3].rhs for j in 1:3] + f_xy = dot(tether_rhs, e_z) .* e_z + force_eqs[:,i] .= force[:,i] .~ tether_rhs .- f_xy + force_eqs[:,i+3] .= force[:,i+3] .~ kite_rhs .+ f_xy + eqs2 = vcat(eqs2, vcat(force_eqs[:,i])) + eqs2 = vcat(eqs2, steering_acc[i-s.num_E+3] ~ (force[:,i] ./ mass_tether_particle[(i-1)%3+1]) ⋅ e_z - (acc[:,i+3] ⋅ e_z)) + end + for i in s.num_E:s.num_A + eqs2 = vcat(eqs2, vcat(force_eqs[:,i])) + eqs2 = vcat(eqs2, acc[:,i] .~ [0.0; 0.0; -9.81] .+ (force[:,i] ./ s.masses[i])) + end + + eqs = vcat(eqs1, eqs2) + + # @assert false + println("making mtk model") + @time @named sys = ODESystem(eqs, t) + println("making simple sys") + @time simple_sys = structural_simplify(sys) + return simple_sys, sys +end diff --git a/src/KiteModels.jl b/src/KiteModels.jl index 41c0a916..a1c15bc1 100644 --- a/src/KiteModels.jl +++ b/src/KiteModels.jl @@ -34,7 +34,7 @@ Scientific background: http://arxiv.org/abs/1406.6218 =# module KiteModels using PrecompileTools: @setup_workload, @compile_workload -using Dierckx, StaticArrays, Rotations, LinearAlgebra, Parameters, NLsolve, DocStringExtensions, OrdinaryDiffEq, Serialization +using Dierckx, StaticArrays, Rotations, LinearAlgebra, Parameters, NLsolve, DocStringExtensions, OrdinaryDiffEq, Serialization, DataInterpolations import Sundials using Reexport, Pkg @reexport using KitePodModels @@ -50,11 +50,12 @@ import KiteUtils.SysState # import Sundials.step! import OrdinaryDiffEq.init import OrdinaryDiffEq.step! +using ModelingToolkit, SymbolicIndexingInterface export KPS3, KPS4, KPS4_3L, KVec3, SimFloat, ProfileLaw, EXP, LOG, EXPLOG # constants and types export calc_set_cl_cd!, copy_examples, copy_bin, update_sys_state! # helper functions -export clear!, find_steady_state!, residual! # low level workers -export init_sim!, next_step! # high level workers +export clear!, find_steady_state!, residual!, model! # low level workers +export init_sim!, reset_sim!, next_step!, init_pos_vel, update_pos! # high level workers export pos_kite, calc_height, calc_elevation, calc_azimuth, calc_heading, calc_course, calc_orient_quat, load_history # getters export winch_force, lift_drag, lift_over_drag, unstretched_length, tether_length, v_wind_kite # getters export save_history # setter / saver @@ -95,8 +96,10 @@ const SVec3 = SVector{3, SimFloat} # disadvantage: changing the cl and cd curves requires a restart of the program const calc_cl = Spline1D(se().alpha_cl, se().cl_list) const calc_cd = Spline1D(se().alpha_cd, se().cd_list) -const rad_cl = Spline1D(deg2rad.(se(SYS_3L).alpha_cl), se(SYS_3L).cl_list) -const rad_cd = Spline1D(deg2rad.(se(SYS_3L).alpha_cd), se(SYS_3L).cd_list) +const rad_cl = Spline1D(deg2rad.(se().alpha_cl), se().cl_list, k=3) +const rad_cd = Spline1D(deg2rad.(se().alpha_cd), se().cd_list, k=3) +const rad_cl_mtk = CubicSpline(se().cl_list, deg2rad.(se().alpha_cl); extrapolate=true) +const rad_cd_mtk = CubicSpline(se().cd_list, deg2rad.(se().alpha_cd); extrapolate=true) """ abstract type AbstractKiteModel @@ -121,10 +124,11 @@ end steady_state_history_file = joinpath(get_data_path(), ".steady_state_history.bin") -include("KPS4.jl") # include code, specific for the four point kite model -include("KPS4_3L.jl") # include code, specific for the four point kite model -include("KPS3.jl") # include code, specific for the one point kite model -include("init.jl") # functions to calculate the inital state vector, the inital masses and initial springs +include("KPS4.jl") # include code, specific for the four point kite model +include("KPS4_3L.jl") # include code, specific for the four point 3 line kite model +include("KPS4_3L_MTK.jl") # include code, specific for the four point 3 line kite model +include("KPS3.jl") # include code, specific for the one point kite model +include("init.jl") # functions to calculate the inital state vector, the inital masses and initial springs # Calculate the lift and drag coefficient as a function of the angle of attack alpha. function set_cl_cd!(s::AKM, alpha) @@ -175,6 +179,7 @@ Getter for the unstretched tether reel-out lenght (at zero force). """ function unstretched_length(s::AKM) s.l_tether end + """ lift_drag(s::AKM) @@ -255,6 +260,16 @@ function orient_euler(s::AKM) SVector(roll, pitch, yaw) end +function calc_orient_quat(s::AKM) + x, _, z = kite_ref_frame(s) + pos_kite_ = pos_kite(s) + pos_before = pos_kite_ .+ z + + rotation = rot(pos_kite_, pos_before, -x) + q = QuatRotation(rotation) + return Rotations.params(q) +end + """ calc_elevation(s::AKM) @@ -343,16 +358,6 @@ end # var_05::MyFloat # end -function calc_orient_quat(s::AKM) - x, _, z = kite_ref_frame(s) - pos_kite_ = pos_kite(s) - pos_before = pos_kite_ .+ z - - rotation = rot(pos_kite_, pos_before, -x) - q = QuatRotation(rotation) - return Rotations.params(q) -end - function update_sys_state!(ss::SysState, s::AKM, zoom=1.0) ss.time = s.t_0 pos = s.pos @@ -494,13 +499,15 @@ Parameters: Returns: An instance of a DAE integrator. """ -function init_sim!(s::AKM; t_end=1.0, stiffness_factor=0.035, prn=false, steady_state_history=nothing) +function init_sim!(s::AKM; t_end=1.0, stiffness_factor=0.035, prn=false, steady_state_history=nothing, mtk=false, torque_control=false) clear!(s) s.stiffness_factor = stiffness_factor - + if isa(s, KPS4_3L) + s.mtk = mtk + s.torque_control = torque_control + end + found = false - y0 = nothing - yd0 = nothing if isa(steady_state_history, SteadyStateHistory) while length(steady_state_history) > 1000 # around 1MB, 1ms max per for loop pop!(steady_state_history) @@ -511,7 +518,7 @@ function init_sim!(s::AKM; t_end=1.0, stiffness_factor=0.035, prn=false, steady_ if fields_equal(akm_steady_state_pair[1], s) if prn println("Found similar steady state, ") end for field in fieldnames(typeof(s)) - setfield!(s, field, getfield(akm_steady_state_pair[1], field)) # deepcopy?? + setfield!(s, field, getfield(akm_steady_state_pair[1], field)) end y0 = akm_steady_state_pair[2] yd0 = akm_steady_state_pair[3] @@ -525,34 +532,57 @@ function init_sim!(s::AKM; t_end=1.0, stiffness_factor=0.035, prn=false, steady_ end if !found y0, yd0 = KiteModels.find_steady_state!(s; stiffness_factor=stiffness_factor, prn=prn) - y0 = Vector{SimFloat}(y0) - yd0 = Vector{SimFloat}(yd0) + if !mtk + y0 = Vector{SimFloat}(y0) + yd0 = Vector{SimFloat}(yd0) + end if isa(steady_state_history, SteadyStateHistory) pushfirst!(steady_state_history, (deepcopy(s), deepcopy(y0), deepcopy(yd0))) end end - if s.set.solver=="IDA" + if isa(s, KPS4_3L) && s.mtk + solver = KenCarp4(autodiff=false) # TRBDF2, Rodas4P, Rodas5P, Kvaerno5, KenCarp4, radau, QNDF + elseif s.set.solver=="IDA" solver = Sundials.IDA(linear_solver=Symbol(s.set.linear_solver), max_order = s.set.max_order) elseif s.set.solver=="DImplicitEuler" solver = DImplicitEuler(autodiff=false) elseif s.set.solver=="DFBDF" - solver = DFBDF(autodiff=false, max_order=Val{s.set.max_order}()) + solver = DFBDF(autodiff=false, max_order=Val{s.set.max_order}()) else println("Error! Invalid solver in settings.yaml: $(s.set.solver)") return nothing end + dt = 1/s.set.sample_freq tspan = (0.0, dt) abstol = s.set.abs_tol # max error in m/s and m - differential_vars = ones(Bool, length(y0)) - prob = DAEProblem{true}(residual!, yd0, y0, tspan, s; differential_vars) - integrator = OrdinaryDiffEq.init(prob, solver; abstol=abstol, reltol=s.set.rel_tol, save_everystep=false) + if isa(s, KPS4_3L) && s.mtk + simple_sys, _ = model!(s, y0; torque_control=torque_control) + s.prob = ODEProblem(simple_sys, nothing, tspan) + integrator = OrdinaryDiffEq.init(s.prob, solver; dt=dt, abstol=s.set.abs_tol, reltol=s.set.rel_tol, save_on=false) + s.set_values_idx = parameter_index(integrator.f, :set_values) + s.v_wind_gnd_idx = parameter_index(integrator.f, :v_wind_gnd) + s.get_pos = getu(integrator.sol, simple_sys.pos[:,:]) + s.get_steering_pos = getu(integrator.sol, simple_sys.steering_pos) + s.get_line_acc = getu(integrator.sol, simple_sys.acc[:,s.num_E-2]) + s.get_kite_vel = getu(integrator.sol, simple_sys.vel[:,s.num_A]) + s.get_winch_forces = getu(integrator.sol, simple_sys.force[:,1:3]) + s.get_tether_lengths = getu(integrator.sol, simple_sys.tether_length) + s.get_tether_speeds = getu(integrator.sol, simple_sys.tether_speed) + update_pos!(s, integrator) + return integrator + else + differential_vars = ones(Bool, length(y0)) + prob = DAEProblem{true}(residual!, yd0, y0, tspan, s; differential_vars) + end + integrator = OrdinaryDiffEq.init(prob, solver; abstol=abstol, reltol=s.set.rel_tol, save_everystep=false) return integrator end + """ next_step!(s::AKM, integrator; set_speed = nothing, set_torque=nothing, v_wind_gnd=s.set.v_wind, wind_dir=0.0, dt=1/s.set.sample_freq) diff --git a/src/init.jl b/src/init.jl index fde913a4..520204f2 100644 --- a/src/init.jl +++ b/src/init.jl @@ -194,8 +194,8 @@ function init_pos_vel_acc(s::KPS4_3L, X=zeros(5*s.set.segments+3); delta = 0.0) e_z = normalize(vec_c) distance_c_l = 0.0 # distance between c and left steering line # distance_c_l = s.set.tip_length/2 # distance between c and left steering line - s.l_tethers[2] = norm(pos[s.num_C] + e_z .* (X[s.set.segments*2+6] + distance_c_l)) # find the right steering tether length - s.l_tethers[3] = s.l_tethers[2] + s.tether_lengths[1] = norm(pos[s.num_C] + e_z .* (X[s.set.segments*2+6] + distance_c_l)) # find the right steering tether length + s.tether_lengths[2] = s.tether_lengths[1] pos[s.num_E-2] .= pos[s.num_C] + e_z .* (distance_c_l) pos[s.num_E-1] .= pos[s.num_E-2] .* [1.0, -1.0, 1.0] @@ -211,7 +211,7 @@ function init_pos_vel_acc(s::KPS4_3L, X=zeros(5*s.set.segments+3); delta = 0.0) # set vel and acc for i in 1:s.num_A vel[i] .= [delta, delta, delta] - acc[i] .= [delta, delta, -9.81] + acc[i] .= [delta, delta, delta] end for i in eachindex(pos) @@ -235,6 +235,11 @@ function init_pos_vel(s::KPS4_3L, X=zeros(5*s.set.segments+3)) return pos, vel end +function init_pos(s::KPS4_3L, X=zeros(5*s.set.segments+3)) + pos, _, _ = init_pos_vel_acc(s, X) + return pos +end + function init_inner(s::KPS4, X=zeros(2 * (s.set.segments+KITE_PARTICLES-1)+1); old=false, delta=0.0) pos, vel, acc = init_pos_vel_acc(s, X; old=old, delta=delta) vcat(pos[2:end], vel[2:end]), vcat(vel[2:end], acc[2:end]) @@ -273,7 +278,7 @@ end # implemented function init(s::KPS4_3L, X=zeros(5*s.set.segments+3); delta=0.0) y_, yd_ = init_inner(s, X; delta = delta) - y = vcat(reduce(vcat, y_), reduce(vcat,[s.l_tethers, zeros(3)])) + y = vcat(reduce(vcat, y_), reduce(vcat,[s.tether_lengths, zeros(3)])) yd = vcat(reduce(vcat, yd_), zeros(6)) MVector{6*(s.num_A-5)+4+6, SimFloat}(y), MVector{6*(s.num_A-5)+4+6, SimFloat}(yd) end @@ -285,4 +290,5 @@ function rotate_in_xz(vec, angle) result[2] = vec[2] result[3] = cos(angle) * vec[3] + sin(angle) * vec[1] result -end \ No newline at end of file +end + diff --git a/test/3line.jl b/test/3line.jl index 96ccfde9..4571863d 100644 --- a/test/3line.jl +++ b/test/3line.jl @@ -35,7 +35,7 @@ function main() println(v_ro) println("δ_left ", s.δ_left) println("δ_right ", s.δ_right) - println("tether lengths ", s.l_tethers) + println("tether lengths ", s.tether_lengths) println("heading ", calc_heading(s)) while time() - last_time < dt sleep(0.01) @@ -90,7 +90,7 @@ main() # println(i) # println("δ_left ", s.δ_left) # println("δ_right ", s.δ_right) -# println("lengths ", s.l_tethers) +# println("lengths ", s.tether_lengths) # # println("Pos \t", s.pos) # println(check_key_pressed()) # if i < 70 diff --git a/test/test_kps4_3l.jl b/test/test_kps4_3l.jl index 3f30512b..f6f0c6a3 100644 --- a/test/test_kps4_3l.jl +++ b/test/test_kps4_3l.jl @@ -2,9 +2,10 @@ using Test, BenchmarkTools, StaticArrays, LinearAlgebra, KiteUtils using KiteModels, KitePodModels if ! @isdefined kcu - const kcu = KCU(se()) + const kcu = KCU(se("system_3l.yaml")) end if ! @isdefined kps4_3l + kcu.set.winch_model = "AsyncMachine" const kps4_3l = KPS4_3L(kcu) end @@ -24,8 +25,7 @@ function set_defaults() kps4_3l.set.min_steering_line_distance = 1.0 kps4_3l.set.width = 3.0 kps4_3l.set.aero_surfaces = 3 - KiteModels.clear!(kps4_3l) - + kps4_3l.set.sim_settings = "3l_settings.yaml" kps4_3l.set.sim_time = 100.0 kps4_3l.set.abs_tol = 0.006 @@ -34,11 +34,13 @@ function set_defaults() kps4_3l.set.physical_model = "KPS4_3L" kps4_3l.set.version = 2 kps4_3l.set.cl_list = [0.0, 0.5, 0.0, 0.08, 0.125, 0.15, 0.0, 1.0, 1.0, 0.0, -0.5, 0.0] + kps4_3l.set.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_3l.set.alpha_zero = 10.0 kps4_3l.set.d_tether = 1.0 kps4_3l.set.v_wind_ref = [15.51, 0.0] kps4_3l.set.depower = 25.0 kps4_3l.set.alpha = 0.08163 + KiteModels.clear!(kps4_3l) # kps4_3l.set. end @@ -313,30 +315,30 @@ end kps4_3l.forces[i] .= zeros(3) end KiteModels.calc_aero_forces!(kps4_3l, pos, vel) - forces=[[0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [2.4687949917792396 0.0 7.049771852225747] # last left tether point - [2.3668951093984174 0.0 6.758791465054874] # last right tether point - [0.0 0.0 0.0] - [13.409099608866077 28.40311527705575 71.69832571718838] # C - [13.94856542663672 -26.584094467198202 68.09920030055184] # D - [0.0 0.0 0.0]] + forces=[[0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [4.937589983558479 0.0 14.099543704451493] + [4.733790218796835 0.0 13.517582930109748] + [0.0 0.0 0.0] + [10.940304617086838 28.40311527705575 64.64855386496264] + [11.581670317238302 -26.584094467198202 61.340408835496966] + [0.0 0.0 0.0]] for i in 1:kps4_3l.num_A @test all(forces[i,:] .≈ kps4_3l.forces[i]) # println(kps4_3l.forces[i]) @@ -354,7 +356,7 @@ function init2() height = 134.14733504839947 kps4_3l.v_wind .= kps4_3l.v_wind_gnd * calc_wind_factor(kps4_3l.am, height) kps4_3l.stiffness_factor = 0.5 - lengths = [150.0, 155.0, 155.0] + lengths = [155.0, 155.0, 150.0] kps4_3l.segment_lengths .= lengths/se().segments for i in 1:kps4_3l.num_A kps4_3l.forces[i] .= zeros(3) @@ -370,17 +372,17 @@ end for i in 1:kps4_3l.num_A @test all(kps4_3l.res1[i] .≈ zeros(3)) end - forces=[[-31064.16965783944 -712.3204098262238 -53804.90057314559] - [-31064.169502164772 712.3372148232743 -53804.90034290006] + forces=[[-28813.450404866944 -660.7101060126089 -49906.54047342375] + [-28813.45025799939 660.7259090755075 -49906.5402584326] [-30729.92840691776 0.0013636636282290742 -53225.9626508769] - [0.17157069324093754 -0.004958560715522253 -0.09894191905186744] - [0.17152404242369812 -0.0023358284656751493 -0.09910919728281442] + [0.17156628970769816 -0.004457593638107937 -0.09894954620540375] + [0.17152844600423123 -0.0018348613912166911 -0.09910157005651854] [0.1571784067673434 0.0029938448442505183 -0.09074699545453768] - [0.19480217916498077 0.0022213468986365115 -0.11249849921296118] - [0.1948872556167771 0.005200264806262567 -0.11244936261937255] + [0.19480217916861875 0.0022213468986365115 -0.11249849921296118] + [0.1948872556204151 0.005200264806262567 -0.11244936261937255] [0.1785358289744181 0.003400650929770767 -0.10307770891813561] - [0.2084614284831332 0.0023771057706198917 -0.12038673261122312] - [0.20855247040162794 0.00556490116207442 -0.12033415061887354] + [0.20846142847949523 0.0023771057706198917 -0.12038673261122312] + [0.20855247039798996 0.00556490116207442 -0.12033415061887354] [0.19106700172415003 0.0036393388566165795 -0.11031258488219464] [0.21847280094880261 0.0024912672780601497 -0.12616831258492311] [0.2185682151830406 0.005832156755786855 -0.12611320532596437] @@ -388,11 +390,11 @@ end [0.2264609992998885 0.0025823581802342233 -0.13078150703950087] [0.22655990226121503 0.006045403330631416 -0.1307243848350481] [0.2075919257949863 0.003954097961792049 -0.11985325427667703] - [20174.258526167578 0.0 57608.639182632935] - [20174.25854070689 0.0 57608.63922415069] + [18712.336826052975 0.0 53434.046117617705] + [18712.33684059229 0.0 53434.046159135476] [30774.854649875913 0.0030858773048274557 53292.16389482203] - [10895.382458266646 664.4115738589938 -3847.481062098329] - [10895.488732974576 -664.3039387407784 -3847.5544444373004] + [10106.584909812285 612.8007690783015 -3571.248089177784] + [10106.691184520216 -612.6931339600861 -3571.32147151677] [-55.05875505245474 -0.09799707002383684 1.2534231229097088]] for i in 1:kps4_3l.num_A @test isapprox(forces[i,:], kps4_3l.forces[i], rtol=1e-4) @@ -401,26 +403,26 @@ end res2 = [[0.0 0.0 0.0] [0.0 0.0 0.0] [0.0 0.0 0.0] - [-12.069093444861192 0.3488085960206197 16.770042208222268] - [-12.065811805894151 0.16431321393456688 16.781809349582215] + [-11.679468076955835 0.3034531007520357 16.546043940241628] + [-11.676891846349527 0.12490918280546429 16.556393046228916] [-11.056671993074616 -0.21060119593929524 16.193572551304943] - [-13.703305962068383 -0.1562600394430286 17.723676128248606] - [-13.709290641778484 -0.3658112041120464 17.720219628024815] + [-13.261263834507382 -0.1512193930093825 17.468396253143812] + [-13.267055460033287 -0.3540108426890771 17.46505125292724] [-12.559054011176961 -0.23921785865328948 17.06097321410335] - [-14.664162115840123 -0.16721685465035355 18.278571747075553] - [-14.670566434671947 -0.3914614487345753 18.274872881219174] + [-14.191124627984719 -0.16182276256485825 18.005392013298923] + [-14.197322355886486 -0.378833660065718 18.001812465695977] [-13.440555927578416 -0.25600829552129734 17.569908583091962] - [-15.368409371108799 -0.17524751463706703 18.685275407493435] - [-15.375121259294568 -0.4102614703073016 18.681398902448784] + [-14.87265423010529 -0.16959436900361324 18.3989762008001] + [-14.879149605768937 -0.3970272293296467 18.395224744305274] [-14.086964139530433 -0.26832077925840525 17.9431125374496] - [-15.930336997174587 -0.18165527921240585 19.009789308439366] - [-15.937294298912287 -0.42526224223389947 19.005771062605575] + [-15.416455158556053 -0.17579543149587662 18.713021911392936] + [-15.423188031205438 -0.41154410538764463 18.70913328639249] [-14.602997187287517 -0.2781499386027814 18.241044359683094] - [-8.514919217442267e6 0.0 -2.43147924485373e7] - [-8.514919223578852e6 0.0 -2.431479246606063e7] + [-7.897885266740787e6 0.0 -2.255282007253699e7] + [-7.897885272877371e6 0.0 -2.2552820090060327e7] [-2.5936686003189688e6 -0.26007411443615613 -4.491391163606531e6] - [-87163.05966613317 -5315.292590871951 30789.658496786633] - [-87163.90986379661 5314.431509926228 30790.245555498404] + [-80852.67927849828 -4902.406152626412 28579.794713422274] + [-80853.52947616173 4901.545071680689 28580.381772134162] [220.23502020981897 0.39198828009534736 4.796307508361165]] for i in 1:kps4_3l.num_A @test isapprox(res2[i,:], kps4_3l.res2[i], rtol=1e-4) @@ -433,32 +435,10 @@ end kps4_3l.stiffness_factor = 0.04 res = zeros(MVector{6*(kps4_3l.num_A-5)+4+6, SimFloat}) y0, yd0 = KiteModels.init(kps4_3l; delta=1e-6) - y0s =[4.490372205878421, 0.10298757652176786, 7.780950480262341, 4.490372205878421, -0.10298757652176786, - 7.780950480262341, 4.166666666666668, 1.0e-6, 7.216878364870322, 8.980744411756842, 0.20597515304353572, - 15.561900960524682, 8.980744411756842, -0.20597515304353572, 15.561900960524682, 8.333333333333336, 1.0e-6, - 14.433756729740644, 13.471116617635262, 0.3089627295653036, 23.342851440787022, 13.471116617635262, - -0.3089627295653036, 23.342851440787022, 12.500000000000004, 1.0e-6, 21.650635094610966, 17.961488823513683, - 0.41195030608707145, 31.123801921049363, 17.961488823513683, -0.41195030608707145, 31.123801921049363, - 16.66666666666667, 1.0e-6, 28.867513459481287, 22.451861029392106, 0.5149378826088393, 38.904752401311704, - 22.451861029392106, -0.5149378826088393, 38.904752401311704, 20.83333333333334, 1.0e-6, 36.08439182435161, - 25.000000000000007, 1.0e-6, 43.30127018922193, 26.942233235270525, 0.6179254591306071, 46.685702881574045, - 26.942233235270525, -0.6179254591306071, 46.685702881574045, 27.646090205747516, 0.013239546805801463, - 46.27933087019816, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, - 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, - 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, - 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, - 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 50.0, 53.90566405419164, - 53.90566405419164, 0.0, 0.0, 0.0] - yd0s = [1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, - 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, - 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, - 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, - 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, - -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, - 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, - 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, - -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 1.0e-6, 1.0e-6, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0] + y0s = [4.490372205878421, 0.10298757652176786, 7.780950480262341, 4.490372205878421, -0.10298757652176786, 7.780950480262341, 4.166666666666668, 1.0e-6, 7.216878364870322, 8.980744411756842, 0.20597515304353572, 15.561900960524682, 8.980744411756842, -0.20597515304353572, 15.561900960524682, 8.333333333333336, 1.0e-6, 14.433756729740644, 13.471116617635262, 0.3089627295653036, 23.342851440787022, 13.471116617635262, -0.3089627295653036, 23.342851440787022, 12.500000000000004, 1.0e-6, 21.650635094610966, 17.961488823513683, 0.41195030608707145, 31.123801921049363, 17.961488823513683, -0.41195030608707145, 31.123801921049363, 16.66666666666667, 1.0e-6, 28.867513459481287, 22.451861029392106, 0.5149378826088393, 38.904752401311704, 22.451861029392106, -0.5149378826088393, 38.904752401311704, 20.83333333333334, 1.0e-6, 36.08439182435161, 25.000000000000007, 1.0e-6, 43.30127018922193, 26.942233235270525, 0.6179254591306071, 46.685702881574045, 26.942233235270525, -0.6179254591306071, 46.685702881574045, 27.646090205747516, 0.013239546805801463, 46.27933087019816, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 53.90566405419164, 53.90566405419164, 50.0, 0.0, 0.0, 0.0] + yd0s = [1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + # println(y0) + # println(yd0) @test all(y0 .≈ y0s) @test all(yd0 .≈ yd0s) @@ -470,32 +450,34 @@ end @test height ≈ 46.27933087019816 res1_= zeros(MVector{(kps4_3l.num_A), KVec3}) @test res1 == res1_ - res2_ =[[0.0 0.0 0.0] - [0.0 0.0 0.0] - [0.0 0.0 0.0] - [-36.19870597246316 -0.4127203625766845 20.914719411722622] - [-36.21464522961104 -0.9663952327158092 20.90534703312241] - [-33.16183456642719 -0.6318028861961738 19.164888750753427] - [-41.124107605793945 -0.46898424806589395 23.73885483497869] - [-41.14205559862839 -1.0974259472846015 23.72847977503416] - [-37.67716103350859 -0.7176525759598683 21.752919642347337] - [-44.00766676374068 -0.5018690276378873 25.403386777889366] - [-44.02687324824136 -1.1743760979874625 25.392284231334877] - [-40.32166678257849 -0.7680238865638919 23.279725748446353] - [-46.12113304640005 -0.5259714953350568 26.623383296643127] - [-46.14126192575945 -1.2307756927533196 26.611747548381196] - [-42.260891416297945 -0.8049613377752156 24.399337610189463] - [-47.80749276932336 -0.5452031428922882 27.59683293994661] - [-47.82835763853061 -1.2757775998701422 27.584771743691014] - [-43.80899056925542 -0.8344488158083441 25.29313307824504] - [-349.4896579462506 0.00017994216739759087 -609.0021540731703] - [-335.70416803282905 0.00017284441535465636 -584.9802900169846] - [-320.38753260380525 -0.9850270455038719 -435.63794206931027] - [-254.62802485602307 -315.80551879718183 -811.4612593230728] - [-1190.464532876995 -528.2876381560295 -260.09644073323926] - [484.9562144240868 419.3822884418879 -270.2434095557314]] - - # print(res2[2]) + res2_ =[[0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [-33.576530377010556 -0.38282988815729174 29.208411662359655] + [-33.591305552166176 -0.8963637655564114 29.199734330656238] + [-33.16183456642719 -0.6318028861961738 28.97488975075343] + [-38.14451435598529 -0.43500453818836243 31.82888830200402] + [-38.16116195058352 -1.017913319895357 31.8192649533944] + [-37.67716103350859 -0.7176525759598683 31.56292064234734] + [-40.81914902429016 -0.46550669426878305 33.37281877027291] + [-40.83696392819467 -1.0892881485455548 33.3625206441397] + [-40.32166678257849 -0.7680238865638919 33.08972674844635] + [-42.779486884607266 -0.4878628493400021 34.5044220444638] + [-42.79815735246308 -1.1416013847855135 34.493629349052206] + [-42.260891416297945 -0.8049613377752156 34.20933861018946] + [-44.34366362984511 -0.5057010931465664 35.407341672896614] + [-44.36301676237147 -1.1833427378553365 35.39615435476668] + [-43.80899056925542 -0.8344488158083441 35.10313407824504] + [-345.2546735104606 0.00017776169578435534 -601.6224946605902] + [-331.46918359703903 0.00017066394374142083 -577.6006306044044] + [-320.38753260380525 -0.9850270455038719 -425.82794106931027] + [-254.62802485602307 -315.80551879718183 -801.6512583230729] + [-1190.464532876995 -528.2876381560295 -250.28643973323926] + [484.9562144240868 419.3822884418879 -260.4334085557314]] + + # for r in res2 + # println(r) + # end @test res2_[1, :] == res2[1] @test all(res2_[4, :] .≈ res2[4]) for i in 1:kps4_3l.num_A @@ -524,19 +506,17 @@ end end @testset "test_find_steady_state" begin - init_100() + init_50() clear!(kps4_3l) height = sin(deg2rad(kps4_3l.set.elevation)) * kps4_3l.set.l_tether kps4_3l.v_wind .= kps4_3l.v_wind_gnd * calc_wind_factor(kps4_3l.am, height) res1, res2 = find_steady_state!(kps4_3l; stiffness_factor=0.035, prn=true) - @test sum(res2) ≈ -9.81*(kps4_3l.num_A-5) # velocity and acceleration must be near zero. the three ground points and two last tether points dont have gravitational acceleration + @test sum(res2) ≈ 0.0 # not moving pre_tension = KiteModels.calc_pre_tension(kps4_3l) - println(kps4_3l.l_tethers) - println("length: ", tether_length(kps4_3l)) @test pre_tension > 1.000001 @test pre_tension < 1.01 - @test unstretched_length(kps4_3l) ≈ 100.0 # initial, unstreched tether lenght - @test isapprox(tether_length(kps4_3l), 99.80297620107189, rtol=1e-2) + @test unstretched_length(kps4_3l) ≈ 50.0 # initial, unstreched tether lenght + @test isapprox(tether_length(kps4_3l), 51.31951395939359, rtol=1e-2) # @test winch_force(kps) ≈ 276.25776695110034 # initial force at the winch [N] # lift, drag = lift_drag(kps) # @test lift ≈ 443.63303000106197 # initial lift force of the kite [N] @@ -552,7 +532,7 @@ end y0, yd0 = KiteModels.init(kps4_3l) forces = spring_forces(kps4_3l) ref_forces = [-3493.569454791926, -3493.569454791926, -3455.751918948772, -3493.569454791926, -3493.569454791926, -3455.751918948772, -3493.569454791926, -3493.569454791926, -3455.751918948772, -3493.569454791926, -3493.569454791926, -3455.751918948772, -3493.5694547919256, -3493.5694547919256, -3455.751918948772, -3493.5694547919265, -3493.5694547919265, -3455.751918948773, 2.1602770047500774, 2.1602770047559905, 2.1602770047559905, 2.160277004745103, 2.160277004793414, 2.160277004793414] - # println(forces) + # println("forces ", forces) for i in 1:kps4_3l.num_A @test isapprox(forces[i], ref_forces[i], atol=1e-6, rtol=1e-4) end @@ -561,39 +541,40 @@ end function simulate(integrator, steps) iter = 0 for i in 1:steps - KiteModels.next_step!(kps4_3l, integrator, set_values=[0.0, 0.0, 0.0], torque_control=false) + KiteModels.next_step!(kps4_3l, integrator, set_values=[0.0, 0.0, 0.0]) iter += kps4_3l.iter end return iter/steps end @testset "test_simulate " begin - STEPS = 100 + STEPS = 50 + kps4_3l.set.solver = "DFBDF" # println("finding steady state") init_50() integrator = KiteModels.init_sim!(kps4_3l; stiffness_factor=0.035, prn=false) # println("\nStarting simulation...") - simulate(integrator, 100) + simulate(integrator, STEPS) av_steps = simulate(integrator, STEPS-10) if Sys.isapple() println("isapple $av_steps") - @test isapprox(av_steps, 168, rtol=0.6) + @test isapprox(av_steps, 835.25, rtol=0.6) else println("not apple $av_steps") - @test isapprox(av_steps, 168, rtol=0.6) + @test isapprox(av_steps, 835.25, rtol=0.6) end lift, drag = KiteModels.lift_drag(kps4_3l) # println(lift, " ", drag) # 703.7699568972286 161.44746368100536 - @test isapprox(lift, 452.92869682967137, rtol=0.05) + @test isapprox(lift, 404.2596735903995, rtol=0.05) sys_state = SysState(kps4_3l) update_sys_state!(sys_state, kps4_3l) # TODO Add testcase with varying reelout speed end @testset "Steady state history" begin - STEPS = 100 + STEPS = 50 kps4_3l.set.solver = "DFBDF" # println("finding steady state") init_50() @@ -601,14 +582,14 @@ end integrator = KiteModels.init_sim!(kps4_3l; stiffness_factor=0.035, prn=false, steady_state_history=steady_state_history) # println("\nStarting simulation...") - simulate(integrator, 100) + simulate(integrator, STEPS) lift1, drag1 = KiteModels.lift_drag(kps4_3l) save_history(steady_state_history) steady_state_history2 = load_history() init_50() integrator = KiteModels.init_sim!(kps4_3l; stiffness_factor=0.035, prn=false, steady_state_history=steady_state_history2) - simulate(integrator, 100) + simulate(integrator, STEPS) lift2, drag2 = KiteModels.lift_drag(kps4_3l) @test lift2 == lift1 diff --git a/test/test_kps4_3l_mtk.jl b/test/test_kps4_3l_mtk.jl new file mode 100644 index 00000000..bb18ac6f --- /dev/null +++ b/test/test_kps4_3l_mtk.jl @@ -0,0 +1,627 @@ +using Test, BenchmarkTools, StaticArrays, LinearAlgebra, KiteUtils +using KiteModels, KitePodModels + +if ! @isdefined kcu + const kcu = KCU(se("system_3l.yaml")) +end +if ! @isdefined kps4_3l + const kps4_3l = KPS4_3L(kcu) +end + +pos, vel = nothing, nothing + +@testset verbose = true "KPS4_3L tests...." begin + +function set_defaults() + KiteModels.clear!(kps4_3l) + kps4_3l.set.segments = 6 + kps4_3l.set.l_tether = 50.0 + kps4_3l.set.elevation = 70.8 + kps4_3l.set.radius = 2.0 + kps4_3l.set.bridle_center_distance = 4.0 + kps4_3l.set.middle_length = 1.2 + kps4_3l.set.tip_length = 0.6 + kps4_3l.set.min_steering_line_distance = 1.0 + kps4_3l.set.width = 3.0 + kps4_3l.set.aero_surfaces = 3 + + kps4_3l.set.sim_settings = "3l_settings.yaml" + kps4_3l.set.sim_time = 100.0 + kps4_3l.set.abs_tol = 0.006 + kps4_3l.set.rel_tol = 0.01 + kps4_3l.set.max_iter = 10000 + kps4_3l.set.physical_model = "KPS4_3L" + kps4_3l.set.version = 2 + kps4_3l.set.cl_list = [0.0, 0.5, 0.0, 0.08, 0.125, 0.15, 0.0, 1.0, 1.0, 0.0, -0.5, 0.0] + kps4_3l.set.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_3l.set.alpha_zero = 10.0 + kps4_3l.set.d_tether = 1.0 + kps4_3l.set.v_wind_ref = [15.51, 0.0] + kps4_3l.set.depower = 25.0 + kps4_3l.set.alpha = 0.08163 + KiteModels.clear!(kps4_3l) + # kps4_3l.set. +end + +function init_100() + KiteModels.clear!(kps4_3l) + kps4_3l.set.l_tether = 100.0 + kps4_3l.set.elevation = 70.0 + kps4_3l.set.v_wind = 15.51 + kps4_3l.set.mass = 0.5 + kps4_3l.set.c_s = 0.6 + kps4_3l.set.damping = 473.0 # unit damping coefficient + kps4_3l.set.c_spring = 614600.0 # unit spring coefficent + kps4_3l.set.width = 3.0 +end + +function init_50() + KiteModels.clear!(kps4_3l) + kps4_3l.set.l_tether = 50.0 + kps4_3l.set.elevation = 70.0 + kps4_3l.set.v_wind = 15.51 + kps4_3l.set.mass = 0.5 + kps4_3l.set.c_s = 0.6 + kps4_3l.set.damping = 473.0 # unit damping coefficient + kps4_3l.set.c_spring = 614600.0 # unit spring coefficent + kps4_3l.set.width = 3.0 +end + +function init3() + KiteModels.clear!(kps4_3l) + kps4_3l.set.l_tether = 150.0 # - kps4_3l.set.height_k - kps4_3l.set.h_bridle + kps4_3l.set.area = 10.18 + kps4_3l.set.mass = 6.21 + kps4_3l.set.c_s = 0.6 + kps4_3l.set.damping = 473.0 # unit damping coefficient + kps4_3l.set.c_spring = 614600.0 # unit spring coefficent + kps4_3l.set.width = 4.9622 + kps4_3l.set.elevation = 70.7 + kps4_3l.set.profile_law = Int(EXPLOG) + pos, vel = KiteModels.init_pos_vel(kps4_3l) + posd = copy(vel) + veld = zero(vel) + height = 134.14733504839947 + kps4_3l.v_wind .= kps4_3l.v_wind_gnd * calc_wind_factor(kps4_3l.am, height) + kps4_3l.stiffness_factor = 1.0 + KiteModels.init_springs!(kps4_3l) + return pos, vel, posd, veld +end + +set_defaults() + +@testset "calc_rho " begin + @test isapprox(calc_rho(kps4_3l.am, 0.0), 1.225, atol=1e-5) + @test isapprox(calc_rho(kps4_3l.am, 100.0), 1.210756, atol=1e-5) +end + +@testset "initial_kite_ref_frame" begin + vec_c = [-15., 0., -25.98076211] + v_app = [10.4855, 0, -3.08324] + x, y, z = KiteModels.initial_kite_ref_frame(vec_c, v_app) + @test x == [-0.8660254037549957, 0.0, 0.5000000000509968] + @test y == [0.0, 1.0, 0.0] + @test z == [-0.5000000000509968, 0.0, -0.8660254037549957] +end + +@testset "get_particles " begin + init_50() + particles = KiteModels.get_particles(kps4_3l.set.width, kps4_3l.set.radius, kps4_3l.set.middle_length, kps4_3l.set.tip_length, kps4_3l.set.bridle_center_distance) + @test particles[1] == [75.0, 0.0, 129.90381057] # E + @test particles[2] == [76.95106065350437, 0.6180085233533619, 133.28314675005853] # C + @test particles[3] == [76.95106065350437, -0.6180085233533616, 133.28314675005853] # D + @test particles[4] == [77.64618475974454, 1.1102230246251565e-16, 132.881816660146] # A +end + +@testset "init_springs! " begin + init_50() + sp = KiteModels.init_springs!(kps4_3l) + # test springs + @test length(sp) == 6*3 + 6 + @test sp[1].p1 == 1 + @test sp[1].p2 == 4 + for i in 1:6*3 + @test sp[i].length ≈ 8.333333333333334 + @test sp[i].c_spring ≈ 5183.627878423158 + @test sp[i].damping ≈ 56.76 + end + @test sp[18].p1 == 18 + @test sp[18].p2 == 21 + @test sp[19].p1 == 21 + @test sp[19].p2 == 24 + @test sp[19].length ≈ 3.9830222651726808 + @test sp[19].c_spring ≈ 67782.8544993504 + @test sp[19].damping ≈ 118.75404366575728 + @test sp[20].length ≈ 3.949967399446711 + @test sp[21].length ≈ 3.949967399446711 + @test sp[22].length ≈ 1.2357698432973823 + @test sp[23].length ≈ 1.0128116611547708 + @test sp[24].c_spring ≈ 266565.4721629595 +end + +@testset "init_masses! " begin + init_50() + m = KiteModels.init_masses!(kps4_3l) + @test m[1] ≈ 0.0 + for i in 4:18 + @test m[i] ≈ 0.004738568919164605 + end + @test m[19] ≈ 0.0023692844595823025 + @test m[20] ≈ 0.0023692844595823025 + @test m[21] ≈ 0.01186537657358817 + @test m[22] ≈ 0.125 + @test m[23] ≈ 0.125 + @test m[end] ≈ 0.25 +end + +@testset "calc_particle_forces! " begin + init_50() + pos1 = KVec3(1.0, 2.0, 3.0) + pos2 = KVec3(2.0, 3.0, 4.0) + vel1 = KVec3(3.0, 4.0, 5.0) + vel2 = KVec3(4.0, 5.0, 6.0) + rho = kps4_3l.set.rho_0 + for i in 1:kps4_3l.num_A + kps4_3l.forces[i] .= zeros(3) + end + bytes = 0 + for i in 1:length(kps4_3l.springs) + spring = kps4_3l.springs[i] + kps4_3l.stiffness_factor = 0.5 + kps4_3l.v_wind_tether .= KVec3(8.0, 0.1, 0.0) + bytes = @allocated KiteModels.calc_particle_forces!(kps4_3l, pos1, pos2, vel1, vel2, spring, se().d_tether/1000.0, rho, i) + end + # @test bytes == 0 + res=[[-931.0208419755234 -931.0559150592786 -931.0602499347989] + [-931.0208419755234 -931.0559150592786 -931.0602499347989] + [-931.0208419755234 -931.0559150592786 -931.0602499347989] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [0.04965402868720048 -0.020492138823328787 -0.029161889863871693] + [931.0704960042106 931.0354229204553 931.031088044935] + [931.0704960042106 931.0354229204553 931.031088044935] + [-11867.4612536364 -11867.601545971422 -11867.618885473503] + [96000.29073757448 96000.18551832321 96000.17251369666] + [28808.982257032563 28808.8770377813 28808.86403315474] + [-112010.44332079432 -112010.54854004558 -112010.56154467215]] + for i in 1:kps4_3l.num_A + @test all(res[i,:] .≈ kps4_3l.forces[i]) + # println(kps4_3l.forces[i]) + end +end + +@testset "init " begin + init_50() + kps4_3l.set.elevation = 60.0 + pos, vel, acc = KiteModels.init_pos_vel_acc(kps4_3l, delta = 1e-6) + pos1 = [[0.0 1.0e-6 0.0] + [0.0 1.0e-6 0.0] + [0.0 1.0e-6 0.0] + [4.491843442217564 0.10300158722556031 7.780101061565895] + [4.491843442217564 -0.10300158722556031 7.780101061565895] + [4.166666666666668 1.0e-6 7.216878364870322] + [8.983686884435128 0.20600317445112062 15.56020212313179] + [8.983686884435128 -0.20600317445112062 15.56020212313179] + [8.333333333333336 1.0e-6 14.433756729740644] + [13.475530326652692 0.30900476167668095 23.340303184697685] + [13.475530326652692 -0.30900476167668095 23.340303184697685] + [12.500000000000004 1.0e-6 21.650635094610966] + [17.967373768870257 0.41200634890224125 31.12040424626358] + [17.967373768870257 -0.41200634890224125 31.12040424626358] + [16.66666666666667 1.0e-6 28.867513459481287] + [22.45921721108782 0.5150079361278016 38.90050530782948] + [22.45921721108782 -0.5150079361278016 38.90050530782948] + [20.83333333333334 1.0e-6 36.08439182435161] + [26.951060653305383 0.6180095233533619 46.68060636939537] + [26.951060653305383 -0.6180095233533619 46.68060636939537] + [25.000000000000007 1.0e-6 43.30127018922193] + [26.951060653305383 0.6180095233533619 46.68060636939537] + [26.951060653305383 -0.6180095233533619 46.68060636939537] + [27.64618475956919 1.000000000139778e-6 46.27927627952376] + ] + for i in 1:kps4_3l.num_A + @test all(pos1[i,:] .≈ pos[i]) + # println(pos[i]) + end +end + + +@testset "initial_residual " begin + function split_res(res, num_particles) + pos=res[1:3*particles] + vel=res[3*particles+1:6*particles] + pos, vel + end + init3() + res = zeros(MVector{6*(kps4_3l.num_A-5)+4+6, SimFloat}) + X00 = zeros(SimFloat, 5*kps4_3l.set.segments+3) + y0, yd0 = KiteModels.init(kps4_3l, X00) + num_particles = div(length(y0)-6-4, 6) + residual!(res, yd0, y0, kps4_3l, 0.0) + res_pos, res_vel = res[1:3*num_particles+2], res[3*num_particles+3:6*num_particles+4] + @test res_pos == zeros(length(res_pos)) + # in the second test we check if the norm of the accelerations of the tether particles is low + # @test (norm(res_vel)) < 15.0 + # println(res_vel) +end + +@testset "inner_loop! " begin + init_50() + kps4_3l.set.elevation = 60.0 + kps4_3l.set.profile_law = Int(EXP) + for i in 1:kps4_3l.num_A + kps4_3l.forces[i] .= zeros(3) + end + pos, vel, acc = KiteModels.init_pos_vel_acc(kps4_3l, delta = 1e-6) + v_wind_gnd = KVec3(7.0, 0.1, 0.0) + kps4_3l.stiffness_factor = 0.5 + segments = kps4_3l.set.segments + d_tether = kps4_3l.set.d_tether/1000.0 + KiteModels.inner_loop!(kps4_3l, pos, vel, v_wind_gnd, d_tether) + forces = [[-691.7059635853744 -15.86212132213054 -1198.250303656443] + [-691.7059267098825 15.865405658555337 -1198.2502791771315] + [-719.8767233654121 0.0013636636282290742 -1247.0283971963331] + [0.17151129393744213 0.0018017696171863662 -0.09904480166824214] + [0.1715834423043816 0.0044245018272306424 -0.09900631367599999] + [0.15717840676779815 0.0029938448442505183 -0.09074699545817566] + [0.19480217915588582 0.0022213468985956553 -0.11249849922182875] + [0.1948872556117749 0.005200264806310528 -0.11244936262801275] + [0.17853582897430442 0.003400650929770767 -0.10307770891836299] + [0.20846142849427451 0.0023771057706643006 -0.12038673261440636] + [0.20855247041777147 0.005564901162090408 -0.12033415061682717] + [0.1910670017234679 0.0036393388566165795 -0.11031258487832929] + [0.21847280093288646 0.0024912672782040346 -0.1261683125815125] + [0.21856821517053504 0.005832156755717577 -0.12611320531914316] + [0.20025615129509333 0.003814369514879717 -0.11561794285694305] + [0.2264609993015938 0.0025823581803336992 -0.13078150702381208] + [0.22655990226348877 0.006045403330494636 -0.13072438482117832] + [0.2075919258348904 0.003954097961792049 -0.11985325426326199] + [691.899119571082 15.864477896223933 1198.138752653517] + [691.8991698100529 -15.860095319097438 1198.1387816694394] + [8610.386516090239 0.0015829749097520107 11921.484451159264] + [-4005.3977950461667 14519.255397533867 -1104.130875838349] + [-4005.375232614203 -14519.232357143173 -1104.1559349816287] + [120.79342186625172 -0.0162820172326974 -8466.429543362992]] + for i in 1:kps4_3l.num_A + @test isapprox(forces[i,:], kps4_3l.forces[i], rtol=1e-4) + # println(kps4_3l.forces[i]) + end +end + +@testset "calc_aero_forces! " begin + init_50() + kps4_3l.set.elevation = 60.0 + kps4_3l.set.profile_law = Int(EXP) + for i in 1:kps4_3l.num_A + kps4_3l.forces[i] .= zeros(3) + end + pos, vel, acc = KiteModels.init_pos_vel_acc(kps4_3l, delta = 1e-6) + rho = 1.25 + kps4_3l.v_wind .= KVec3(8.0, 0.2, 0.0) + for i in 1:kps4_3l.num_A + kps4_3l.forces[i] .= zeros(3) + end + KiteModels.calc_aero_forces!(kps4_3l, pos, vel) + forces=[[0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [2.4687949917792396 0.0 7.049771852225747] # last left tether point + [2.3668951093984174 0.0 6.758791465054874] # last right tether point + [0.0 0.0 0.0] + [13.409099608866077 28.40311527705575 71.69832571718838] # C + [13.94856542663672 -26.584094467198202 68.09920030055184] # D + [0.0 0.0 0.0]] + for i in 1:kps4_3l.num_A + @test all(forces[i,:] .≈ kps4_3l.forces[i]) + # println(kps4_3l.forces[i]) + end +end + +function init2() + init_50() + kps4_3l.set.elevation = 60.0 + kps4_3l.set.profile_law = Int(EXP) + pos, vel, acc = KiteModels.init_pos_vel_acc(kps4_3l, delta = 1e-6) + posd = copy(vel) + veld = zero(vel) + kps4_3l.v_wind_gnd .= [7.0, 0.1, 0.0] + height = 134.14733504839947 + kps4_3l.v_wind .= kps4_3l.v_wind_gnd * calc_wind_factor(kps4_3l.am, height) + kps4_3l.stiffness_factor = 0.5 + lengths = [155.0, 155.0, 150.0] + kps4_3l.segment_lengths .= lengths/se().segments + for i in 1:kps4_3l.num_A + kps4_3l.forces[i] .= zeros(3) + end + KiteModels.init_springs!(kps4_3l) + return pos, vel, posd, veld +end + +@testset "test_loop " begin + init2() + pos, vel, posd, veld = init2() + KiteModels.loop!(kps4_3l, pos, vel, posd, veld) + for i in 1:kps4_3l.num_A + @test all(kps4_3l.res1[i] .≈ zeros(3)) + end + forces=[[-28813.450404866944 -660.7101060126089 -49906.54047342375] + [-28813.45025799939 660.7259090755075 -49906.5402584326] + [-30729.92840691776 0.0013636636282290742 -53225.9626508769] + [0.17156628970769816 -0.004457593638107937 -0.09894954620540375] + [0.17152844600423123 -0.0018348613912166911 -0.09910157005651854] + [0.1571784067673434 0.0029938448442505183 -0.09074699545453768] + [0.19480217916861875 0.0022213468986365115 -0.11249849921296118] + [0.1948872556204151 0.005200264806262567 -0.11244936261937255] + [0.1785358289744181 0.003400650929770767 -0.10307770891813561] + [0.20846142847949523 0.0023771057706198917 -0.12038673261122312] + [0.20855247039798996 0.00556490116207442 -0.12033415061887354] + [0.19106700172415003 0.0036393388566165795 -0.11031258488219464] + [0.21847280094880261 0.0024912672780601497 -0.12616831258492311] + [0.2185682151830406 0.005832156755786855 -0.12611320532596437] + [0.2002561513108958 0.003814369514879717 -0.11561794285807991] + [0.2264609992998885 0.0025823581802342233 -0.13078150703950087] + [0.22655990226121503 0.006045403330631416 -0.1307243848350481] + [0.2075919257949863 0.003954097961792049 -0.11985325427667703] + [18712.336826052975 0.0 53434.046117617705] + [18712.33684059229 0.0 53434.046159135476] + [30774.854649875913 0.0030858773048274557 53292.16389482203] + [10106.584909812285 612.8007690783015 -3571.248089177784] + [10106.691184520216 -612.6931339600861 -3571.32147151677] + [-55.05875505245474 -0.09799707002383684 1.2534231229097088]] + for i in 1:kps4_3l.num_A + @test isapprox(forces[i,:], kps4_3l.forces[i], rtol=1e-4) + # println(kps4_3l.forces[i]') + end + res2 = [[0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [-11.679468076955835 0.3034531007520357 16.546043940241628] + [-11.676891846349527 0.12490918280546429 16.556393046228916] + [-11.056671993074616 -0.21060119593929524 16.193572551304943] + [-13.261263834507382 -0.1512193930093825 17.468396253143812] + [-13.267055460033287 -0.3540108426890771 17.46505125292724] + [-12.559054011176961 -0.23921785865328948 17.06097321410335] + [-14.191124627984719 -0.16182276256485825 18.005392013298923] + [-14.197322355886486 -0.378833660065718 18.001812465695977] + [-13.440555927578416 -0.25600829552129734 17.569908583091962] + [-14.87265423010529 -0.16959436900361324 18.3989762008001] + [-14.879149605768937 -0.3970272293296467 18.395224744305274] + [-14.086964139530433 -0.26832077925840525 17.9431125374496] + [-15.416455158556053 -0.17579543149587662 18.713021911392936] + [-15.423188031205438 -0.41154410538764463 18.70913328639249] + [-14.602997187287517 -0.2781499386027814 18.241044359683094] + [-7.897885266740787e6 0.0 -2.255282007253699e7] + [-7.897885272877371e6 0.0 -2.2552820090060327e7] + [-2.5936686003189688e6 -0.26007411443615613 -4.491391163606531e6] + [-80852.67927849828 -4902.406152626412 28579.794713422274] + [-80853.52947616173 4901.545071680689 28580.381772134162] + [220.23502020981897 0.39198828009534736 4.796307508361165]] + for i in 1:kps4_3l.num_A + @test isapprox(res2[i,:], kps4_3l.res2[i], rtol=1e-4) + # println(kps4_3l.res2[i]') + end +end + +@testset "test_residual! " begin + + kps4_3l.stiffness_factor = 0.04 + res = zeros(MVector{6*(kps4_3l.num_A-5)+4+6, SimFloat}) + y0, yd0 = KiteModels.init(kps4_3l; delta=1e-6) + y0s = [4.490372205878421, 0.10298757652176786, 7.780950480262341, 4.490372205878421, -0.10298757652176786, 7.780950480262341, 4.166666666666668, 1.0e-6, 7.216878364870322, 8.980744411756842, 0.20597515304353572, 15.561900960524682, 8.980744411756842, -0.20597515304353572, 15.561900960524682, 8.333333333333336, 1.0e-6, 14.433756729740644, 13.471116617635262, 0.3089627295653036, 23.342851440787022, 13.471116617635262, -0.3089627295653036, 23.342851440787022, 12.500000000000004, 1.0e-6, 21.650635094610966, 17.961488823513683, 0.41195030608707145, 31.123801921049363, 17.961488823513683, -0.41195030608707145, 31.123801921049363, 16.66666666666667, 1.0e-6, 28.867513459481287, 22.451861029392106, 0.5149378826088393, 38.904752401311704, 22.451861029392106, -0.5149378826088393, 38.904752401311704, 20.83333333333334, 1.0e-6, 36.08439182435161, 25.000000000000007, 1.0e-6, 43.30127018922193, 26.942233235270525, 0.6179254591306071, 46.685702881574045, 26.942233235270525, -0.6179254591306071, 46.685702881574045, 27.646090205747516, 0.013239546805801463, 46.27933087019816, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 53.90566405419164, 53.90566405419164, 50.0, 0.0, 0.0, 0.0] + yd0s = [1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + # println(y0) + # println(yd0) + @test all(y0 .≈ y0s) + @test all(yd0 .≈ yd0s) + + time = 0.0 + @test length(res) == length(y0) + residual!(res, yd0, y0, kps4_3l, time) + res1, res2 = kps4_3l.res1, kps4_3l.res2 + height = calc_height(kps4_3l) + @test height ≈ 46.27933087019816 + res1_= zeros(MVector{(kps4_3l.num_A), KVec3}) + @test res1 == res1_ + res2_ =[[0.0 0.0 0.0] + [0.0 0.0 0.0] + [0.0 0.0 0.0] + [-33.576530377010556 -0.38282988815729174 29.208411662359655] + [-33.591305552166176 -0.8963637655564114 29.199734330656238] + [-33.16183456642719 -0.6318028861961738 28.97488975075343] + [-38.14451435598529 -0.43500453818836243 31.82888830200402] + [-38.16116195058352 -1.017913319895357 31.8192649533944] + [-37.67716103350859 -0.7176525759598683 31.56292064234734] + [-40.81914902429016 -0.46550669426878305 33.37281877027291] + [-40.83696392819467 -1.0892881485455548 33.3625206441397] + [-40.32166678257849 -0.7680238865638919 33.08972674844635] + [-42.779486884607266 -0.4878628493400021 34.5044220444638] + [-42.79815735246308 -1.1416013847855135 34.493629349052206] + [-42.260891416297945 -0.8049613377752156 34.20933861018946] + [-44.34366362984511 -0.5057010931465664 35.407341672896614] + [-44.36301676237147 -1.1833427378553365 35.39615435476668] + [-43.80899056925542 -0.8344488158083441 35.10313407824504] + [-345.2546735104606 0.00017776169578435534 -601.6224946605902] + [-331.46918359703903 0.00017066394374142083 -577.6006306044044] + [-320.38753260380525 -0.9850270455038719 -425.82794106931027] + [-254.62802485602307 -315.80551879718183 -801.6512583230729] + [-1190.464532876995 -528.2876381560295 -250.28643973323926] + [484.9562144240868 419.3822884418879 -260.4334085557314]] + + # for r in res2 + # println(r) + # end + @test res2_[1, :] == res2[1] + @test all(res2_[4, :] .≈ res2[4]) + for i in 1:kps4_3l.num_A + if ! all(res2_[i, :] .≈ res2[i]) + # println(i, res2_[i, :], " ", res2[i]) + @test_broken all(res2_[i, :] .≈ res2[i]) + else + @test all(res2_[i, :] .≈ res2[i]) + end + # println(res2[i]') + end + +end + +@testset "test_getters" begin + x, y, z = kite_ref_frame(kps4_3l) + @test all(x .≈ [-0.8673285322802265, 0.0, 0.49773609181228007]) + @test all(y .≈ [0.0, 1.0, 0.0]) + @test all(z .≈ [-0.49773609181228007, 2.5626999001639186e-7, -0.8673285322802265]) + @test all(orient_euler(kps4_3l) .≈ [1.5707963267948966, -0.5209866063773279, 1.5707963267948966]) + @test all(pos_kite(kps4_3l) .≈ [27.646090205747516, 0.013239546805801463, 46.27933087019816]) + @test calc_elevation(kps4_3l) ≈ 1.0323095578497548 + @test calc_azimuth(kps4_3l) ≈ -0.000478893966385608 + @test calc_heading(kps4_3l) ≈ 0.0004154220077356996 + calc_course(kps4_3l) # the course for vel_kite = zero is undefined +end + +@testset "test_find_steady_state" begin + init_50() + clear!(kps4_3l) + height = sin(deg2rad(kps4_3l.set.elevation)) * kps4_3l.set.l_tether + kps4_3l.v_wind .= kps4_3l.v_wind_gnd * calc_wind_factor(kps4_3l.am, height) + res1, res2 = find_steady_state!(kps4_3l; stiffness_factor=0.035, prn=true) + @test sum(res2) ≈ 0.0 # not moving + pre_tension = KiteModels.calc_pre_tension(kps4_3l) + @test pre_tension > 1.000001 + @test pre_tension < 1.01 + @test unstretched_length(kps4_3l) ≈ 50.0 # initial, unstreched tether lenght + @test isapprox(tether_length(kps4_3l), 51.31951395939359, rtol=1e-2) +# @test winch_force(kps) ≈ 276.25776695110034 # initial force at the winch [N] +# lift, drag = lift_drag(kps) +# @test lift ≈ 443.63303000106197 # initial lift force of the kite [N] +# @test drag ≈ 94.25223134952152 # initial drag force of the kite [N] +# @test lift_over_drag(kps) ≈ 4.706870316479931 # initlal lift-over-drag +# @test norm(v_wind_kite(kps)) ≈ 9.107670173739065 # inital wind speed at the height of the kite [m/s] +end + +@testset "test_spring_forces " begin + init2() + kps4_3l.stiffness_factor = 0.04 + res = zeros(MVector{6*(kps4_3l.set.segments+4), SimFloat}) + y0, yd0 = KiteModels.init(kps4_3l) + forces = spring_forces(kps4_3l) + ref_forces = [-3493.569454791926, -3493.569454791926, -3455.751918948772, -3493.569454791926, -3493.569454791926, -3455.751918948772, -3493.569454791926, -3493.569454791926, -3455.751918948772, -3493.569454791926, -3493.569454791926, -3455.751918948772, -3493.5694547919256, -3493.5694547919256, -3455.751918948772, -3493.5694547919265, -3493.5694547919265, -3455.751918948773, 2.1602770047500774, 2.1602770047559905, 2.1602770047559905, 2.160277004745103, 2.160277004793414, 2.160277004793414] + # println("forces ", forces) + for i in 1:kps4_3l.num_A + @test isapprox(forces[i], ref_forces[i], atol=1e-6, rtol=1e-4) + end +end + +function simulate(integrator, steps) + iter = 0 + for i in 1:steps + KiteModels.next_step!(kps4_3l, integrator, set_values=[0.0, 0.0, 0.0]) + iter += kps4_3l.iter + end + return iter/steps +end + +@testset "test_simulate " begin + STEPS = 100 + kps4_3l.set.solver = "DFBDF" + # println("finding steady state") + init_50() + integrator = KiteModels.init_sim!(kps4_3l; stiffness_factor=0.035, prn=false) + # println("\nStarting simulation...") + simulate(integrator, STEPS) + av_steps = simulate(integrator, STEPS-10) + if Sys.isapple() + println("isapple $av_steps") + @test isapprox(av_steps, 168, rtol=0.6) + else + println("not apple $av_steps") + @test isapprox(av_steps, 168, rtol=0.6) + end + + lift, drag = KiteModels.lift_drag(kps4_3l) + # println(lift, " ", drag) # 703.7699568972286 161.44746368100536 + @test isapprox(lift, 452.92869682967137, rtol=0.05) + sys_state = SysState(kps4_3l) + update_sys_state!(sys_state, kps4_3l) + # TODO Add testcase with varying reelout speed +end + +@testset "Steady state history" begin + STEPS = 100 + kps4_3l.set.solver = "DFBDF" + # println("finding steady state") + init_50() + steady_state_history = load_history() + + integrator = KiteModels.init_sim!(kps4_3l; stiffness_factor=0.035, prn=false, steady_state_history=steady_state_history) + # println("\nStarting simulation...") + simulate(integrator, STEPS) + lift1, drag1 = KiteModels.lift_drag(kps4_3l) + save_history(steady_state_history) + + steady_state_history2 = load_history() + init_50() + integrator = KiteModels.init_sim!(kps4_3l; stiffness_factor=0.035, prn=false, steady_state_history=steady_state_history2) + simulate(integrator, STEPS) + lift2, drag2 = KiteModels.lift_drag(kps4_3l) + + @test lift2 == lift1 + @test drag2 == drag1 + + @test isapprox(lift1, 849.1874782889682, rtol=0.05) +end + +# @testset "Raptures" begin +# kps4_3l_ = KPS4_3L(KCU(se())) +# integrator = KiteModels.init_sim!(kps4_3l_; stiffness_factor=0.035, prn=false) +# kps4_3l_.set.version = 2 +# kps4_3l_.stiffness_factor = 3 +# @test maximum(spring_forces(kps4_3l_)) > 20000 +# end + +# # TODO Add test for winch_force +# @testset "test_copy_examples" begin +# cd(tempdir()) +# copy_examples() +# @test isdir("examples") +# cd("examples") +# @test isfile("reel_out_4p.jl") +# end + +# @testset "test_copy_bin" begin +# cd(tempdir()) +# copy_bin() +# @test isdir("bin") +# cd("bin") +# @test isfile("create_sys_image") +# end + +# println(kps4_3l.set) + +end +nothing \ No newline at end of file