double-pendulum-julia/pendulum.jl at master · marcus-k/double-pendulum-julia · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
using DifferentialEquations
using ProgressBars
using Printf
using Plots

g = 9.81    # acceleration due to gravity, in m/s^2
l₁ = 1.0    # length of pendulum 1 in m
l₂ = 2.0    # length of pendulum 2 in m
m₁ = 1.0    # mass of pendulum 1 in kg
m₂ = 1.0    # mass of pendulum 2 in kg

function deriv!(du, u, p, t)
    θ₁, ω₁, θ₂, ω₂ = u

    # From solving the lagrangian:
    Δθ = θ₂ - θ₁
    f₁ = (l₂ / l₁) * (m₂ / (m₁ + m₂)) * ω₂ * ω₂ * sin(Δθ) - (g / l₁) * sin(θ₁)
    f₂ = -(l₁ / l₂) * ω₁ * ω₁ * sin(Δθ) - (g / l₂) * sin(θ₂)
    α₁ = (l₂ / l₁) * (m₂ / (m₁ + m₂)) * cos(Δθ)
    α₂ = (l₁ / l₂) * cos(Δθ)

    du[1] = ω₁
    du[2] = (f₁ - α₁ * f₂) / (1 - α₁ * α₂)
    du[3] = ω₂
    du[4] = (-α₂ * f₁ + f₂) / (1 - α₁ * α₂)
end

# Initial conditions
θ₁ = 120.0
ω₁ = 0.0
θ₂ = -10.0
ω₂ = 0.0

t_range = (0.0, 100.0)
u = deg2rad.([θ₁, ω₁, θ₂, ω₂])

# Setup and solve ODE system
println("Starting solve.")
prob = ODEProblem(deriv!, u, t_range)
sol = solve(prob, reltol=1e-6)

# Create a gif
println("Starting gif creation.")
anim = @animate for t in tqdm(0:0.025:50)
    frame = sol(t)
    x₁ = l₁ * sin(frame[1])
    y₁ = -l₁ * cos(frame[1])
    x₂ = l₂ * sin(frame[3]) + x₁
    y₂ = -l₂ * cos(frame[3]) + y₁

    plot(
        [0, x₁, x₂], [0, y₁, y₂],
        xlim = (-3, 3),
        ylim = (-3, 3),
        label = @sprintf("t = %.2f", t),
        aspect_ratio = :equal,
        market = :circle,
        size = (500, 500)
    )

    scatter!([0, x₁, x₂], [0, y₁, y₂], label="")
end

gif(anim, "output.gif", fps=30)
println("Finished gif creation.")