Add load_state and init!(s; initial_state) to continue a simulation

CHANGELOG.md

@@ -5,6 +5,9 @@ SPDX-License-Identifier: MIT
 ### KiteModels Unreleased
 #### Added
 - `save_state(filename, ss)` writes a `SysState` to a human-readable YAML file under the key `sys_state`, without the fields that KPS3 and KPS4 do not use (keyword `exclude`; `KiteModels.UNUSED_FIELDS_KPS3` also leaves out `side_slip`, `alpha3` and `alpha4`, which KPS3 does not have, and `aero_moment_KA_x/y/z`, which is always zero for KPS3), and the dictionary passed as keyword `meta_data` under the key `meta_data`. `examples/test_init_4p.jl` and `examples/test_init_1p.jl` use it to write `output/sys_state_<script name>.yaml` after 80 time steps, and `examples_3d/parking_wind_dir.jl` after 16 s simulation time, with the script name, the simulation time and the date and time of saving as meta data, and they fill in `t_sim`, the wall-clock time of the last time step. Strings, and keys of the meta data that are not plain identifiers, are written as quoted YAML strings.
+- `SysState(s)` and `update_sys_state!` (KPS3 and KPS4) store the internal state of the winch, the rate limited set speed `wm.last_set_speed` and the state of the brake `wm.brake` (0 or 1), in the free fields `var_13` and `var_14`, which are `NaN` without a winch model. `init!(s; initial_state)` restores them; saved while the winch ramps up the set speed, the tether length after one more second differs by 0.3 mm from the uninterrupted simulation instead of 0.5 m. Code that writes `var_13` or `var_14` itself should use other fields.
+- `load_state(filename)` reads a system state that `save_state` wrote and returns a new `SysState` and the dictionary of the meta data. The number of points and springs is taken from the file; fields that are not in the file keep their default values.
+- `init!(s; initial_state)` (KPS3 and KPS4) continues a simulation from a `SysState`, for example one that `load_state` read: the positions and velocities of the particles, the tether length, the reel-out speed, the set values and the internal state of the winch and the depower and steering of the KCU are taken from it, the accelerations are calculated from the residual, for KPS4 the pitch rate of the kite is calculated from the velocities of the particles instead of being set to zero, and the integrator continues at the time of the state. Saved after 40 steps and continued for 20 more, the positions stay within a millimetre of the uninterrupted simulation; for KPS3 this needs a tolerance of about 1e-6, because with the default of 1e-3 a turning KPS3 is only accurate to about half a metre, and the restarted solver takes different steps. `examples_3d/parking_wind_dir.jl` continues from its saved state if `LOAD_SYS_STATE` is true; if the file does not exist yet, it starts at zero and saves the state.
 - `SysState(s)` and `update_sys_state!` (KPS3 and KPS4) fill the field `spring_force` with the forces of all springs from `spring_forces`, including the damping force: the tether segments and, for KPS4, the springs of the kite. The new function `n_springs(s)` returns their number.
 - `SysState(s)` and `update_sys_state!` (KPS3 and KPS4) fill the fields `aero_force_KA_x/y/z` and `aero_moment_KA_x/y/z`, which were always zero: the total aerodynamic force and moment of the kite in the `KA` body frame (aft-right-up). The moment is taken about the origin of the `KA` frame, the centre between the kite particles C and D, and includes the pitch damping and steering couples. For KPS3, the force is the sum of the lift, drag and steering forces of the kite; the moment stays zero, because the kite is a point mass.
 - `SysState(s)` and `update_sys_state!` (KPS3 and KPS4) also fill the per-particle fields `aero_force_x/y/z` (the aerodynamic forces on the kite particles A, B, C and D) and `drag_force_x/y/z` (the drag of the tether, the bridle lines and the KCU), both in ENU. For KPS3, the aerodynamic force acts on the last particle, and the drag forces are those of the tether and the bridle. Both are recomputed from the current state in `update_sys_state!` with the new functions `calc_aero_forces` and `calc_drag_forces` (KPS4) and `calc_drag_force` (KPS3), which `calc_aero_forces!`, `calc_particle_forces!` and `calc_drag` now use too, so the residual is not slower. Without the evaluation of the residual below, `update_sys_state!` would take about 3 µs instead of 1.4 µs.

docs/src/functions.md

@@ -70,6 +70,7 @@ KiteUtils instead.
 
 ```@docs
 save_state
+load_state
 ```
 
 ## High level simulation interface
@@ -102,6 +103,7 @@ calc_aero_forces
 calc_aero_force_moment
 aero_point_forces
 calc_turn_rates
+calc_pitch_rate
 calc_particle_forces!
 calc_spring_force
 calc_drag_forces

examples_3d/parking_wind_dir.jl

@@ -5,7 +5,8 @@
 #
 # After 16 s simulation time, the system state is written to
 # output/sys_state_parking_wind_dir.yaml, with the script name, the simulation time and
-# the date and time of saving as meta data.
+# the date and time of saving as meta data. If LOAD_SYS_STATE is true and that file exists,
+# the simulation continues from it instead of starting at zero, and the state is not saved.
 
 using Pkg
 if dirname(Pkg.project().path) != @__DIR__
@@ -28,7 +29,6 @@ default_turbulence = get_default_turbulence()
 if default_turbulence !== nothing
     set.use_turbulence = default_turbulence
 end
-PLOT_HEADING_RATE = false
 
 include("parking_controller.jl")
 import .ParkingControllers as pcm
@@ -83,6 +83,8 @@ UPWIND_DIR        = -pi/2 # the direction the wind is coming from.
 UPWIND_DIR2       = -pi/2+deg2rad(90)     # Zero is at north; clockwise positive
 INTERPOLATE       = true  # interpolate the turbulence between the grid points of the wind field
 SAVE_TIME         = 16    # simulation time [s] after which the system state is saved
+PLOT_HEADING_RATE = false
+LOAD_SYS_STATE    = true  # continue from the state saved at SAVE_TIME, if it exists
 # end of user parameter section #
 
 viewer::Viewer3D = Viewer3D(SHOW_KITE, "WinchON")
@@ -100,29 +102,35 @@ STEERING::Vector{Float64}      = zeros(Int64(MAX_TIME/dt))
 HEADING_RATE::Vector{Float64}  = zeros(Int64(MAX_TIME/dt))
 BODY_RATE::Vector{Float64}     = zeros(Int64(MAX_TIME/dt))
 
+SCRIPT_NAME = splitext(basename(@__FILE__))[1]
+STATE_FILE  = joinpath(dirname(@__DIR__), "output", "sys_state_$(SCRIPT_NAME).yaml")
+
 # save a copy of sys_state with the time of the state; SysState.time is the start time of the
 # last time step
 function save_sys_state(sys_state, t_sim, time_)
     sys_state = deepcopy(sys_state)
     sys_state.t_sim = t_sim
     sys_state.time = time_
-    output_dir = joinpath(dirname(@__DIR__), "output")
-    mkpath(output_dir)
-    script_name = splitext(basename(@__FILE__))[1]
-    meta_data = Dict("script" => script_name, "time" => sys_state.time,
+    mkpath(dirname(STATE_FILE))
+    meta_data = Dict("script" => SCRIPT_NAME, "time" => sys_state.time,
                      "date_time" => Libc.strftime("%Y-%m-%d_%H:%M:%S", time()))
-    save_state(joinpath(output_dir, "sys_state_$(script_name).yaml"), sys_state; meta_data)
+    save_state(STATE_FILE, sys_state; meta_data)
 end
 
-function sim_parking(integrator)
+# initial_state is the loaded SysState, or nothing when starting at zero; the step i
+# continues from the time of the integrator
+function sim_parking(integrator, initial_state)
     upwind_dir=UPWIND_DIR
     start_time_ns = time_ns()
     clear_viewer(viewer)
-    i=1; j=0; k=0
+    i = round(Int, integrator.t/dt) + 1; j=0; k=0
     GC.gc()
     max_time = 0
     t_gc_tot = 0
-    sys_state = SysState(kps4)
+    # continuing with the loaded SysState keeps the rates, e.g. heading_rate, correct;
+    # update_sys_state! expects the start time of the last time step in sys_state.time
+    sys_state = isnothing(initial_state) ? SysState(kps4) : deepcopy(initial_state)
+    isnothing(initial_state) || (sys_state.time = integrator.t - dt)
     println("Kite position at start: ", round.(pos_kite(kps4), digits=2))
     if KiteUtils.PROJECT == "system.yaml"
         KiteViewers.update_system(viewer, sys_state; scale = 0.08, kite_scale=3)
@@ -165,7 +173,7 @@ function sim_parking(integrator)
             t_gc_tot += @elapsed GC.gc(false)
         end
         update_sys_state!(sys_state, kps4)
-        if i == round(Int, SAVE_TIME/dt)
+        if isnothing(initial_state) && i == round(Int, SAVE_TIME/dt)
             save_sys_state(sys_state, t_sim, integrator.t)
         end
         T[i] = dt * i
@@ -208,11 +216,21 @@ function sim_parking(integrator)
     return div(i, TIME_LAPSE_RATIO)
 end
 
+# returns the first step of the simulation
 function play_parking()
+    initial_state = nothing
+    if LOAD_SYS_STATE
+        if isfile(STATE_FILE)
+            initial_state = first(load_state(STATE_FILE))
+        else
+            println("No saved state in $STATE_FILE, starting at zero; it is saved after $SAVE_TIME s.")
+        end
+    end
     saved_turbulence = kps4.set.use_turbulence
     kps4.set.use_turbulence = 0.0
-    integrator = KiteModels.init!(kps4)
+    integrator = KiteModels.init!(kps4; initial_state)
     kps4.set.use_turbulence = saved_turbulence
+    i_start = round(Int, integrator.t/dt) + 1
     sys_state = SysState(kps4)
     if KiteUtils.PROJECT == "system.yaml"
         KiteViewers.update_system(viewer, sys_state; scale = 0.08, kite_scale=3)
@@ -221,7 +239,7 @@ function play_parking()
     end
     toc()
     try
-        sim_parking(integrator)
+        sim_parking(integrator, initial_state)
     catch e
         if isa(e, AssertionError)
             println("AssertionError! Halting simulation.")
@@ -231,10 +249,16 @@ function play_parking()
         end
     end
     GC.enable(true)
+    i_start
 end
 
-play_parking()
+i_start = play_parking()
 stop(viewer)
+# plot only the simulated steps
+for vec in (T, AZIMUTH, AZIMUTH_EAST, UPWIND_DIR_, AV_UPWIND_DIR, V_WIND_KITE, HEADING, SET_STEERING,
+            STEERING, HEADING_RATE, BODY_RATE)
+    deleteat!(vec, 1:i_start-1)
+end
 v = filter(!=(0.0), V_WIND_KITE)
 ti = std(v) / mean(v) * 100
 println("Turbulence intensity (wind speed magnitude): $(round(ti, digits=2)) %")

src/KPS4.jl

@@ -576,6 +576,20 @@ function calc_turn_rates(s::KPS4)
     fromKS2KA_body(SVec3(ω ⋅ x, ω ⋅ y, ω ⋅ z))
 end
 
+"""
+    calc_pitch_rate(s::KPS4)
+
+Time derivative of the pitch angle of [`orient_euler`](@ref) in [rad/s], calculated from the
+angular velocity of the kite, so from the positions and velocities of the particles. `next_step!`
+approximates the same rate by the change of the pitch angle during the last time step.
+"""
+function calc_pitch_rate(s::KPS4)
+    # fromKS2KA_body is its own inverse, so this gives the turn rates in KS (front-right-down)
+    _, q, r = fromKS2KA_body(calc_turn_rates(s))
+    roll, _, _ = orient_euler(s)
+    q * cos(roll) - r * sin(roll)
+end
+
 """
     inner_loop!(s::KPS4, pos, vel, v_wind_gnd, segments, d_tether)