Commit a5d30d80 authored by Jonas's avatar Jonas
Browse files

Merge branch 'main' of gitlab.control.lth.se:regler/physical-modelling-omnibot

added Jonas exploration
parents fd86fe30 37ef73fc
This diff is collapsed.
This diff is collapsed.
%% Cell type:markdown id:2156b41b-7543-4563-b7fc-08f9e1c9db13 tags:
# Minimi
%% Cell type:code id:3f082482-7fca-415a-84a8-0b487bfaefb6 tags:
``` julia
using ModelingToolkit
using DifferentialEquations
@variables t
D = Differential(t)
const g = 9.82;
```
%% Cell type:code id:a587042a-a2ae-42f9-9b23-14c76d8abf5f tags:
``` julia
"""
Pin()
Create a Pin which constitutes a connection point between bodies
"""
function Pin(;name)
sts = @variables fx(t) fy(t) x(t) y(t) M(t) θ(t)
ODESystem(Equation[], t, sts, []; name=name)
end
```
%% Output
Pin
%% Cell type:code id:2fe2a3ed-35a7-45a7-9512-791d6c4ce280 tags:
``` julia
"""
Pendulum(l=3.0,m=1.0)
Create an ideal pendulum with length `l` and mass `m`
"""
function Pendulum(;name,l=3.0,m=1.0)
@named pin = Pin()
sts = @variables fx(t) fy(t) x(t) y(t) M(t) θ(t) vx(t) vy(t) ω(t)
ps = @parameters l=l m=m
eqs = [
M ~ l*m*D(ω)
M ~ pin.M + l*sin(θ)*m*g
vx ~ D(x)
vy ~ D(y)
ω ~ D(θ)
fx ~ pin.fx
fy ~ pin.fy - m*g
fx ~ m*D(vx)
fy ~ m*D(vy)
x ~ pin.x
y ~ pin.y
θ ~ pin.θ
]
compose(ODESystem(eqs, t, sts, ps; name=name),pin)
end
```
%% Output
Pendulum
%% Cell type:code id:0c0290df-5a2a-44ae-86fb-398c567d70be tags:
``` julia
"""
Anchor(x0=0.0,y0=0.0, θ0=0.0)
Create an anchor in position `x0`, `y0`, with angle `θ0`
"""
function Anchor(;name,x0=0.0,y0=0.0, θ0=0.0)
@named pin = Pin()
ps = @parameters x=x0 y=y0 θ=θ0
sts = @variables fx(t) fy(t) M(t)
eqs = [
pin.x ~ x
pin.y ~ y
pin.θ ~ θ
M + pin.M ~ 0.0
fx + pin.fx ~ 0.0
fy + pin.fy ~ 0.0
]
compose(ODESystem(eqs, t, sts, ps; name=name),pin)
end
```
%% Output
Anchor
%% Cell type:code id:6c9ee84b-bfa3-410e-809c-25f683af3332 tags:
``` julia
"""
connect_free(p1,p2)
Connects pins p1 and p2 such that they have the same position but may rotate freely.
"""
function connect_free(p1,p2)
[
p1.x ~ p2.x
p1.y ~ p2.y
p1.M ~ 0.0
p2.M ~ 0.0
p1.fx ~ -p2.fx
p1.fy ~ -p2.fy
]
end
```
%% Output
connect_free
%% Cell type:code id:7a1cb349-8e72-4c23-9152-bdd445216bdf tags:
``` julia
l = 3.0
mp = 1.0
y0 = 0.0
x0 = 0.0
θ0 = π/10
@named pendulum = Pendulum(m=mp,l=l)
@named anchor = Anchor(x0=x0,y0=y0,θ0=0.0)
pend_eqs = connect_free(pendulum.pin,anchor.pin)
```
%% Output
\begin{align}
\mathrm{pendulum_{+}pin_{+}x}\left( t \right) =& \mathrm{anchor_{+}apin_{+}x}\left( t \right) \\
\mathrm{pendulum_{+}pin_{+}y}\left( t \right) =& \mathrm{anchor_{+}apin_{+}y}\left( t \right) \\
\mathrm{pendulum_{+}pin_{+}M}\left( t \right) =& 0.0 \\
\mathrm{anchor_{+}apin_{+}M}\left( t \right) =& 0.0 \\
\mathrm{pendulum_{+}pin_{+}fx}\left( t \right) =& - \mathrm{anchor_{+}apin_{+}fx}\left( t \right) \\
\mathrm{pendulum_{+}pin_{+}fy}\left( t \right) =& - \mathrm{anchor_{+}apin_{+}fy}\left( t \right)
\end{align}
6-element Vector{Equation}:
pendulum₊pin₊x(t) ~ anchor₊apin₊x(t)
pendulum₊pin₊y(t) ~ anchor₊apin₊y(t)
pendulum₊pin₊M(t) ~ 0.0
anchor₊apin₊M(t) ~ 0.0
pendulum₊pin₊fx(t) ~ -anchor₊apin₊fx(t)
pendulum₊pin₊fy(t) ~ -anchor₊apin₊fy(t)
%% Cell type:code id:bfcdb840-1be7-40d0-814d-8673ce6f9cb4 tags:
``` julia
@named _pend_model = ODESystem(pend_eqs, t)
@named pend_model = compose(_pend_model,pendulum, anchor)
sys = structural_simplify(pend_model)
```
%% Output
\begin{align}
\frac{dpendulum_+\omega(t)}{dt} =& \frac{\mathrm{pendulum_{+}M}\left( t \right)}{pendulum_{+}l pendulum_{+}m} \\
\frac{dpendulum_{+}x(t)}{dt} =& \mathrm{pendulum_{+}vx}\left( t \right) \\
\frac{dpendulum_{+}y(t)}{dt} =& \mathrm{pendulum_{+}vy}\left( t \right) \\
\frac{dpendulum_+\theta(t)}{dt} =& \mathrm{pendulum_+\omega}\left( t \right) \\
\frac{dpendulum_{+}vx(t)}{dt} =& \frac{\mathrm{pendulum_{+}fx}\left( t \right)}{pendulum_{+}m} \\
\frac{dpendulum_{+}vy(t)}{dt} =& \frac{\mathrm{pendulum_{+}fy}\left( t \right)}{pendulum_{+}m} \\
\frac{dpendulum_{+}vx(t)}{dt} + \mathrm{\frac{d}{d t}}\left( \frac{dpendulum_{+}x(t)}{dt} \right) =& 0.0 \\
\mathrm{\frac{d}{d t}}\left( \frac{dpendulum_{+}x(t)}{dt} \right) =& -0.0 \\
\frac{dpendulum_{+}vy(t)}{dt} + \mathrm{\frac{d}{d t}}\left( \frac{dpendulum_{+}y(t)}{dt} \right) =& 0.0 \\
\mathrm{\frac{d}{d t}}\left( \frac{dpendulum_{+}y(t)}{dt} \right) =& -0.0
\end{align}
Model pend_model with 10 equations
States (10):
Differential(t)(pendulum₊x(t))
Differential(t)(pendulum₊y(t))
pendulum₊ω(t)
pendulum₊x(t)
pendulum₊y(t)
pendulum₊θ(t)
Parameters (5):
pendulum₊l [defaults to 3.0]
pendulum₊m [defaults to 1.0]
anchor₊x [defaults to 0.0]
anchor₊y [defaults to 0.0]
anchor₊θ [defaults to 0.0]
Incidence matrix:sparse([1, 2, 3, 4, 5, 7, 6, 9, 7, 8, 9, 10, 4, 1, 2, 3, 6, 5], [1, 2, 3, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 12, 13, 14, 15, 16], Num[×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×, ×], 10, 16)
%% Cell type:code id:206ff1fa-a5e4-478b-baf4-795279a38be1 tags:
``` julia
u0 = [
pendulum.θ => θ0
pendulum.ω => 0.0
pendulum.vx => 0.0
pendulum.vy => 0.0
pendulum.x => 0.0
pendulum.y => 0.0
];
```
%% Cell type:code id:1acf7fe3-ec45-4c3d-9659-19f9bff404c4 tags:
``` julia
prob = ODAEProblem(sys, u0, (0, 10.0))
```
%% Output
KeyError: key 16 not found
Stacktrace:
[1] getindex
@ ./dict.jl:481 [inlined]
[2] torn_system_with_nlsolve_jacobian_sparsity(state::TearingState{ODESystem}, var_eq_matching::ModelingToolkit.BipartiteGraphs.Matching{ModelingToolkit.BipartiteGraphs.Unassigned, Vector{Union{ModelingToolkit.BipartiteGraphs.Unassigned, Int64}}}, var_sccs::Vector{Vector{Int64}}, nlsolve_scc_idxs::Vector{Int64}, eqs_idxs::Vector{Int64}, states_idxs::Vector{Int64})
@ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:84
[3] build_torn_function(sys::ODESystem; expression::Bool, jacobian_sparsity::Bool, checkbounds::Bool, max_inlining_size::Nothing, kw::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})
@ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:346
[4] build_torn_function
@ ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:238 [inlined]
[5] ODAEProblem{true}(sys::ODESystem, u0map::Vector{Pair{Num, Float64}}, tspan::Tuple{Int64, Float64}, parammap::SciMLBase.NullParameters; callback::Nothing, use_union::Bool, kwargs::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})
@ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:529
[6] ODAEProblem (repeats 2 times)
@ ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:529 [inlined]
[7] ODAEProblem(::ODESystem, ::Vararg{Any}; kw::Base.Pairs{Symbol, Union{}, Tuple{}, NamedTuple{(), Tuple{}}})
@ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:506
[8] ODAEProblem(::ODESystem, ::Vararg{Any})
@ ModelingToolkit.StructuralTransformations ~/.julia/packages/ModelingToolkit/gG7nj/src/structural_transformation/codegen.jl:506
[9] top-level scope
@ In[49]:1
[10] eval
@ ./boot.jl:373 [inlined]
[11] include_string(mapexpr::typeof(REPL.softscope), mod::Module, code::String, filename::String)
@ Base ./loading.jl:1196
%% Cell type:code id:f447762c-48aa-4a19-8035-96dc2f2da2fa tags:
``` julia
```
This diff is collapsed.
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment