add Tether_07b.jl

src/Tether_07b.jl

@@ -0,0 +1,169 @@
+# Tutorial example simulating a 3D mass-spring system with a nonlinear spring (no spring forces
+# for l < l_0), n tether segments, tether drag and reel-in and reel-out. 
+using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra, Timers, Parameters
+using ModelingToolkit: t_nounits as t, D_nounits as D
+using ControlPlots
+
+@with_kw mutable struct Settings3 @deftype Float64
+    g_earth::Vector{Float64} = [0.0, 0.0, -9.81] # gravitational acceleration     [m/s²]
+    v_wind_tether::Vector{Float64} = [5.0, 0.0, 0.0]
+    rho = 1.225
+    cd_tether = 0.958
+    l0 = 50                                      # initial tether length             [m]
+    v_ro = 2                                     # reel-out speed                  [m/s]
+    d_tether = 4                                 # tether diameter                  [mm]
+    rho_tether = 724                             # density of Dyneema            [kg/m³]
+    c_spring = 614600                            # unit spring constant              [N]
+    damping = 473                                # unit damping constant            [Ns]
+    segments::Int64 = 3                          # number of tether segments         [-]
+    α0 = π/10                                    # initial tether angle            [rad]
+    duration = 0.15                              # duration of the simulation        [s]
+    save::Bool = false                           # save png files in folder video
+end
+
+function set_tether_diameter!(se, d; c_spring_4mm = 614600, damping_4mm = 473)
+    se.d_tether = d
+    se.c_spring = c_spring_4mm * (d/4.0)^2
+    se.damping = damping_4mm * (d/4.0)^2
+end
+
+function calc_initial_state(se)
+    POS0 = zeros(3, se.segments+1)
+    VEL0 = zeros(3, se.segments+1)
+    ACC0 = zeros(3, se.segments+1)
+    SEGMENTS0 = zeros(3, se.segments) 
+    UNIT_VECTORS0 = zeros(3, se.segments)
+    for i in 1:se.segments+1
+        l0 = -(i-1)*se.l0/se.segments
+        POS0[:, i] .= [sin(se.α0) * l0, 0, cos(se.α0) * l0]
+        VEL0[:, i] .= [0, 0, 0]
+    end
+    for i in 1:se.segments
+        ACC0[:, i+1] .= se.g_earth
+        UNIT_VECTORS0[:, i] .= [0, 0, 1.0]
+        SEGMENTS0[:, i] .= POS0[:, i+1] - POS0[:, i]
+    end
+    POS0, VEL0, ACC0, SEGMENTS0, UNIT_VECTORS0
+end
+
+function model(se)
+    POS0, VEL0, ACC0, SEGMENTS0, UNIT_VECTORS0 = calc_initial_state(se)
+    mass_per_meter = se.rho_tether * π * (se.d_tether/2000.0)^2
+    @parameters c_spring0=se.c_spring/(se.l0/se.segments) l_seg=se.l0/se.segments
+    @variables pos(t)[1:3, 1:se.segments+1]  = POS0
+    @variables vel(t)[1:3, 1:se.segments+1]  = VEL0
+    @variables acc(t)[1:3, 1:se.segments+1]  = ACC0
+    @variables segment(t)[1:3, 1:se.segments]  = SEGMENTS0
+    @variables unit_vector(t)[1:3, 1:se.segments]  = UNIT_VECTORS0
+    @variables length(t) = se.l0
+    @variables c_spring(t) = c_spring0
+    @variables damping(t) = se.damping  / l_seg
+    @variables m_tether_particle(t) = mass_per_meter * l_seg
+    @variables norm1(t)[1:se.segments] = l_seg * ones(se.segments)
+    @variables rel_vel(t)[1:3, 1:se.segments]  = zeros(3, se.segments)
+    @variables spring_vel(t)[1:se.segments] = zeros(se.segments)
+    @variables c_spr(t)[1:se.segments] = c_spring0 * ones(se.segments)
+    @variables spring_force(t)[1:3, 1:se.segments] = zeros(3, se.segments)
+    @variables v_apparent(t)[1:3, 1:se.segments] = zeros(3, se.segments)
+    @variables v_app_perp(t)[1:3, 1:se.segments] = zeros(3, se.segments)
+    @variables norm_v_app(t)[1:se.segments] = ones(se.segments)
+    @variables half_drag_force(t)[1:3, 1:se.segments] = zeros(3, se.segments)
+    @variables total_force(t)[1:3, 1:se.segments+1] = zeros(3, se.segments+1)
+
+    eqs1 = vcat(D.(pos) .~ vel,
+                D.(vel) .~ acc)
+    eqs2 = vcat(eqs1...)
+    for i in se.segments:-1:1
+        eqs = [segment[:, i]      ~ pos[:, i+1] - pos[:, i],
+               norm1[i]           ~ norm(segment[:, i]),
+               unit_vector[:, i]  ~ -segment[:, i]/norm1[i],
+               rel_vel[:, i]      ~ vel[:, i+1] - vel[:, i],
+               spring_vel[i]      ~ -unit_vector[:, i] ⋅ rel_vel[:, i],
+               c_spr[i]           ~ c_spring * (norm1[i] > length/se.segments),
+               spring_force[:, i] ~ (c_spr[i] * (norm1[i] - (length/se.segments)) 
+                                     + damping * spring_vel[i]) * unit_vector[:, i],
+               v_apparent[:, i]   ~ se.v_wind_tether .- (vel[:, i] + vel[:, i+1])/2,
+               v_app_perp[:, i]   ~ v_apparent[:, i] - (v_apparent[:, i] ⋅ unit_vector[:, i]) .* unit_vector[:, i],
+               norm_v_app[i]      ~ norm(v_app_perp[:, i]),
+               half_drag_force[:, i] .~ 0.25 * se.rho * se.cd_tether * norm_v_app[i] * (norm1[i]*se.d_tether/1000.0)
+                                        * v_app_perp[:, i]]
+        if i == se.segments
+            push!(eqs, total_force[:, i+1] ~ spring_force[:, i] + half_drag_force[:,i])
+            push!(eqs, acc[:, i+1]       ~ se.g_earth + total_force[:, i+1] / (0.5*m_tether_particle))
+        elseif i == 1
+            push!(eqs, total_force[:, i] ~ spring_force[:, i] + half_drag_force[:,i])
+            push!(eqs, acc[:, i+1]       ~ se.g_earth + total_force[:, i+1] / (0.5*m_tether_particle))
+        else
+            push!(eqs, total_force[:, i] ~ spring_force[:, i] - spring_force[:, i+1] 
+                                           + half_drag_force[:,i] + half_drag_force[:,i+1])
+            push!(eqs, acc[:, i+1]       ~ se.g_earth + total_force[:, i] / m_tether_particle)
+        end
+        eqs2 = vcat(eqs2, reduce(vcat, eqs))
+    end
+    eqs = [acc[:, 1] .~ zeros(3),
+           length .~ se.l0 + se.v_ro*t,
+           c_spring .~ se.c_spring / (length/se.segments),
+           m_tether_particle .~ mass_per_meter * (length/se.segments),
+           damping  .~ se.damping  / (length/se.segments)]
+    eqs2 = vcat(eqs2, reduce(vcat, eqs))  
+
+    @named sys = ODESystem(Symbolics.scalarize.(reduce(vcat, Symbolics.scalarize.(eqs2))), t)
+    simple_sys = structural_simplify(sys)
+    simple_sys, pos, vel
+end
+
+function simulate(se, simple_sys)
+    dt = 0.02
+    tol = 1e-6
+    tspan = (0.0, se.duration)
+    ts    = 0:dt:se.duration
+    prob = ODEProblem(simple_sys, nothing, tspan)
+    elapsed_time = @elapsed sol = solve(prob, Rodas5P(), dt=dt, abstol=tol, reltol=tol, saveat=ts)
+    sol, elapsed_time
+end
+
+function play(se, sol, pos)
+    dt = 0.151
+    ylim = (-1.2*(se.l0+se.v_ro*se.duration), 0.5)
+    xlim = (-se.l0/2, se.l0/2)
+    mkpath("video")
+    z_max = 0.0
+    # text position
+    xy = (se.l0/4.2, z_max-7)
+    start = time_ns()
+    i = 1; j = 0
+    for time in 0:dt:se.duration
+        # while we run the simulation in steps of 20ms, we update the plot only every 150ms
+        # therefore we have to skip some steps of the result
+        while sol.t[i] < time
+            i += 1
+        end
+        plot2d(sol[pos][i], time; segments=se.segments, xlim, ylim, xy)
+        if se.save
+            ControlPlots.plt.savefig("video/"*"img-"*lpad(j,4,"0"))
+        end
+        j += 1
+        wait_until(start + 0.5*time*1e9)
+    end
+    if se.save
+        include("export_gif.jl")
+    end
+    nothing
+end
+
+function main()
+    global sol, pos
+    se = Settings3()
+    set_tether_diameter!(se, 4)
+    simple_sys, pos, vel = model(se)
+    sol, elapsed_time = simulate(se, simple_sys)
+    # println("sol and pos ", sol, "\n\t", pos)
+    println(sol[pos][1])
+    play(se, sol, pos)
+end
+
+if (! @isdefined __BENCH__) || __BENCH__ == false
+    main()
+end
+__BENCH__ = false
+sol[pos]