-
-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* serialization of initial integrator * even faster by serializing only once * stable data type * update documentation * add check for model - less memory - remove float64 - move to simfloat * two point aero model * working two point aero model * xournal notes with formulas * implement new aero model * add comment to explain point numbers * implement kps4_3l functions * plot lift at different wing positions * add kps4 3 line settings * plot * bug free init * implement 3 lines * runs succesfully, not tested * find steady state runs but doesnt diverge * converging but not solving steady state * not solving steady state * notes * updated notes * change page order * change page order * Fix precompilation problem * add file to .gitignore * different tether length * remove Plots from Project * use package directly * correct bridle center distance * shorter name * update bridle center distance * add kps4_3l settings * solve forward slash in branch name * succesful but very slow step * working simulation, just slow * still really slow * benchmark test showing big mem alloc * add 3l test * a lot less memory in calc aero forces * magic memory alloc * remove a lot of heap alloc * working but slow * a lot less allocations * zero heap alloc in calc_aero_forces * no heap alloc * working no heap alloc version * fix E distance * flying makes sense and is fast * add kps4_3l to precompile workload * add orient calc func * export function * remove unnecesary print * remove controlplots dependency * remove unused import * remove unnecesary imports * calc azimuth elevation * need to test int history * fix keyword error * remove precompiling for 3 and 4 * fix issues after merge * fix benchmark mem alloc * add test cases for 3 line model * one 4p model test failing * succesful tests * fix precompile * no errors * generate odesystem * unbalanced system * succesful simple system * add steering connections * succesful steps * add pos * non-zero force * good tether forces * find steady state * working kps4_3l_test * working winches * 700 times realtime * really fast pos update * old version * add benchmark * reset kite * add kite vel * add reel out speeds * fast and neat state updates * very fast reset * remove reset arg * small fix * remove prints and controlplots * assert right steering pos * downgrade to stable version * start kite at segment num_E * add point E to kite * move tether force to point A * move forces back to c and d and increase steering force * remove brake * fix typo * reduce steering damping * remove controlplots * change to faster solver * longer episode * move functions to the right place * merge updated main and add examples * remove unused file * update tolerances * change integrator name * fix tests * remove testenv * remove xopp file * remove double import --------- Co-authored-by: Uwe Fechner <[email protected]> Co-authored-by: Uwe Fechner <[email protected]>
- Loading branch information
3 people
authored
Aug 16, 2024
1 parent
931ee6a
commit 5cae167
Showing
13 changed files
with
1,658 additions
and
218 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,4 +1,6 @@ | ||
| *.xopp | ||
| *.so | ||
| *.so.bak | ||
| /Manifest.toml | ||
| /Manifest.toml.1.7 | ||
| bin/kps-image-1.8-initial.so | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
Oops, something went wrong.