```julia
using Pkg
Pkg.activate(@__DIR__)
Pkg.instantiate()

using LinearAlgebra, Symbolics, DifferentialEquations, JLD2
```

    [32m[1m  Activating[22m[39m environment at `~/Research/symbolics_double_pendulum/Project.toml`
    ┌ Info: Precompiling Symbolics [0c5d862f-8b57-4792-8d23-62f2024744c7]
    └ @ Base loading.jl:1317
    ┌ Info: Precompiling DifferentialEquations [0c46a032-eb83-5123-abaf-570d42b7fbaa]
    └ @ Base loading.jl:1317



```julia
struct DoublePendulum{T}
	m1::T
	m2::T
	l1::T
	l2::T
end

n = 2 # number of states
model = DoublePendulum(1.0, 1.0, 1.0, 1.0) # model
```




    DoublePendulum{Float64}(1.0, 1.0, 1.0, 1.0)




```julia
# kinematics
function kinematics_1(model::DoublePendulum, q)
	θ1, θ2 = q

	[0.5 * model.l1 * sin(θ1);
	 -0.5 * model.l1 * cos(θ1)]
end

function kinematics_2(model::DoublePendulum, q)
	θ1, θ2 = q

	[model.l1 * sin(θ1) + 0.5 * model.l2 * sin(θ1 + θ2);
	 -model.l1 * cos(θ1) - 0.5 * model.l2 * cos(θ1 + θ2)]
end
```




    kinematics_2 (generic function with 1 method)




```julia
# fast kinematics functions
@variables q[1:n]
@variables q̇[1:n]

k1 = kinematics_1(model, q)
k2 = kinematics_2(model, q)

k1_exp = Symbolics.build_function(k1, q)
k2_exp = Symbolics.build_function(k2, q)

k1_func = eval(k1_exp[1])
k2_func = eval(k2_exp[1])
```




    #7 (generic function with 1 method)




```julia
# kinematics Jacobians
j1 = Symbolics.jacobian(k1, q, simplify = true)
j2 = Symbolics.jacobian(k2, q, simplify = true)

j1_exp = Symbolics.build_function(j1, q)
j2_exp = Symbolics.build_function(j2, q)

j1_func = eval(j1_exp[1])
j2_func = eval(j2_exp[1])
```




    #11 (generic function with 1 method)




```julia
# Lagrangian
function lagrangian(model, q, q̇)
	L = 0.0

	# mass 1
	v1 = j1_func(q) * q̇
	L += 0.5 * model.m1 * transpose(v1) * v1 		# kinetic energy
	L -= model.m1 * 9.81 * k1_func(q)[2]            # potential energy

	# mass 2
	v2 = j2_func(q) * q̇
	L += 0.5 * model.m2 * transpose(v2) * v2
	L -= model.m2 * 9.81 * k2_func(q)[2]

	return L
end

# fast Lagrangian
L = lagrangian(model, q, q̇)

# 
dLq = Symbolics.gradient(L, q, simplify = true)
dLq̇ = Symbolics.gradient(L, q̇, simplify = true)
ddL = Symbolics.hessian(L, [q; q̇], simplify = true)

# mass matrix
M = ddL[n .+ (1:n), n .+ (1:n)]
M = simplify.(M)

# dynamics bias
C = ddL[n .+ (1:n), 1:n] * q̇ - dLq
C = simplify.(C)
```




\begin{equation}
\left[
\begin{array}{c}
q\dot{_2} \left( \left( \cos\left( q{_1} \right) + 0.5 \cos\left( q{_1} + q{_2} \right) \right) \left(  - 0.25 q\dot{_1} \sin\left( q{_1} + q{_2} \right) - 0.25 q\dot{_2} \sin\left( q{_1} + q{_2} \right) \right) + \left( \sin\left( q{_1} \right) + 0.5 \sin\left( q{_1} + q{_2} \right) \right) \left( 0.25 q\dot{_1} \cos\left( q{_1} + q{_2} \right) + 0.25 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) + \left( 0.5 \cos\left( q{_1} \right) + 0.25 \cos\left( q{_1} + q{_2} \right) \right) \left(  - 0.5 q\dot{_1} \sin\left( q{_1} + q{_2} \right) - 0.5 q\dot{_2} \sin\left( q{_1} + q{_2} \right) \right) + \left( 0.5 \sin\left( q{_1} \right) + 0.25 \sin\left( q{_1} + q{_2} \right) \right) \left( 0.5 q\dot{_1} \cos\left( q{_1} + q{_2} \right) + 0.5 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) + 0.25 \cos\left( q{_1} + q{_2} \right) \left( q\dot{_1} \left( \sin\left( q{_1} \right) + 0.5 \sin\left( q{_1} + q{_2} \right) \right) + 0.5 q\dot{_2} \sin\left( q{_1} + q{_2} \right) \right) + 0.5 \cos\left( q{_1} + q{_2} \right) \left( 0.5 q\dot{_1} \left( \sin\left( q{_1} \right) + 0.5 \sin\left( q{_1} + q{_2} \right) \right) + 0.25 q\dot{_2} \sin\left( q{_1} + q{_2} \right) \right) - 0.25 \sin\left( q{_1} + q{_2} \right) \left( q\dot{_1} \left( \cos\left( q{_1} \right) + 0.5 \cos\left( q{_1} + q{_2} \right) \right) + 0.5 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) - 0.5 \sin\left( q{_1} + q{_2} \right) \left( 0.5 q\dot{_1} \left( \cos\left( q{_1} \right) + 0.5 \cos\left( q{_1} + q{_2} \right) \right) + 0.25 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) \right) + 14.715 \sin\left( q{_1} \right) + 4.905 \sin\left( q{_1} + q{_2} \right) \\
q\dot{_2} \left( 2 \cos\left( q{_1} + q{_2} \right) \left(  - 0.125 q\dot{_1} \sin\left( q{_1} + q{_2} \right) + 0.25 q\dot{_1} \left( \sin\left( q{_1} \right) + 0.5 \sin\left( q{_1} + q{_2} \right) \right) \right) - 0.25 \sin\left( q{_1} + q{_2} \right) \left( q\dot{_1} \left( \cos\left( q{_1} \right) + 0.5 \cos\left( q{_1} + q{_2} \right) \right) + 0.5 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) + 0.5 \sin\left( q{_1} + q{_2} \right) \left( 0.25 q\dot{_1} \cos\left( q{_1} + q{_2} \right) + 0.25 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) + 0.25 \sin\left( q{_1} + q{_2} \right) \left( 0.5 q\dot{_1} \cos\left( q{_1} + q{_2} \right) + 0.5 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) - 0.5 \sin\left( q{_1} + q{_2} \right) \left( 0.5 q\dot{_1} \left( \cos\left( q{_1} \right) + 0.5 \cos\left( q{_1} + q{_2} \right) \right) + 0.25 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) \right) + 4.905 \sin\left( q{_1} + q{_2} \right) - \left( q\dot{_1} \left( \cos\left( q{_1} \right) + 0.5 \cos\left( q{_1} + q{_2} \right) \right) + 0.5 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) \left(  - 0.25 q\dot{_1} \sin\left( q{_1} + q{_2} \right) - 0.25 q\dot{_2} \sin\left( q{_1} + q{_2} \right) \right) - \left( q\dot{_1} \left( \sin\left( q{_1} \right) + 0.5 \sin\left( q{_1} + q{_2} \right) \right) + 0.5 q\dot{_2} \sin\left( q{_1} + q{_2} \right) \right) \left( 0.25 q\dot{_1} \cos\left( q{_1} + q{_2} \right) + 0.25 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) - \left( 0.5 q\dot{_1} \cos\left( q{_1} + q{_2} \right) + 0.5 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) \left( 0.5 q\dot{_1} \left( \sin\left( q{_1} \right) + 0.5 \sin\left( q{_1} + q{_2} \right) \right) + 0.25 q\dot{_2} \sin\left( q{_1} + q{_2} \right) \right) - \left(  - 0.5 q\dot{_1} \sin\left( q{_1} + q{_2} \right) - 0.5 q\dot{_2} \sin\left( q{_1} + q{_2} \right) \right) \left( 0.5 q\dot{_1} \left( \cos\left( q{_1} \right) + 0.5 \cos\left( q{_1} + q{_2} \right) \right) + 0.25 q\dot{_2} \cos\left( q{_1} + q{_2} \right) \right) \\
\end{array}
\right]
\end{equation}





```julia
# dynamics
# ẋ = [q̇; M \ (-1.0 * C)]
ẋ = [q̇; M \ (-0.5 * q̇ -1.0 * C)] # joint friction
# ẋ = [q̇; M \ (-1.0 * (q - [π / 10; 0.0]) -1.0 * C)] # spring

ẋ = simplify.(ẋ)

ẋ_exp = Symbolics.build_function(ẋ, q, q̇)
dynamics = eval(ẋ_exp[1])
```




    #22 (generic function with 1 method)




```julia
# save dynamics function
path = joinpath(pwd(), "dynamics.jld2")
# @save path ẋ_exp
# @load path ẋ_exp
```




    "/home/taylor/Research/symbolics_double_pendulum/dynamics.jld2"




```julia
# DifferentialEquations.jl
function dynamics!(ẋ, x, p, t)
	ẋ .= dynamics(view(x, 1:n), view(x, n .+ (1:n)))
end

# simulate
x0 = [0.5 * π; 0.0; 0.0; 0.0]
tspan = (0.0, 10.0)
dt = 0.01
prob = ODEProblem(dynamics!, x0, tspan);
```


```julia
sol = solve(prob, Tsit5(), adaptive = false, dt = dt);
```


```julia
# MeshCat.jl
include(joinpath(pwd(), "visuals.jl"))
vis = Visualizer()
render(vis)
```

    ┌ Info: MeshCat server started. You can open the visualizer by visiting the following URL in your browser:
    │ http://127.0.0.1:8704
    └ @ MeshCat /home/taylor/.julia/packages/MeshCat/GlCMx/src/visualizer.jl:73





<div style="height: 500px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both">

</div>





```julia
visualize_double_pendulum!(vis, model, sol.u, Δt = dt)
```


```julia

```


```julia

```

828d158277d8b02d1471296941c66b04dad3dfce
ipynb
Jupyter Notebook
double pendulum.ipynb
thowell/symbolics_double_pendulum
1d8541f491fd61f7862ff2885d1f2c7cf30a4544
MIT
2021-04-21T20:56:02.000Z
2021-11-22T14:19:25.000Z
double pendulum.ipynb
thowell/symbolics_double_pendulum
1d8541f491fd61f7862ff2885d1f2c7cf30a4544
MIT
double pendulum.ipynb
thowell/symbolics_double_pendulum
1d8541f491fd61f7862ff2885d1f2c7cf30a4544
MIT
Qwen/Qwen-72B
1. YES
2. YES