Commit a5d30d80 by Jonas

### 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} [0m[1mModel pend_model with 10 equations[22m [0m[1mStates (10):[22m Differential(t)(pendulum₊x(t)) Differential(t)(pendulum₊y(t)) pendulum₊ω(t) pendulum₊x(t) pendulum₊y(t) pendulum₊θ(t) ⋮ [0m[1mParameters (5):[22m 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] [35mIncidence matrix:[39msparse([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!