Dzhanibekov effect

JuliaComputing · Apr 28, 2023 · 1dab5f1 · 1dab5f1
1 parent 3e2b14d
commit 1dab5f1
Show file tree

Hide file tree

Showing 2 changed files with 59 additions and 29 deletions.
diff --git a/src/Multibody.jl b/src/Multibody.jl
@@ -44,38 +44,37 @@ function at_variables_t(args...; default = nothing)
  xs
 end
 
-using ModelingToolkit.SciMLBase
-import SymbolicIR: InitialType
-
+# using ModelingToolkit.SciMLBase
+# import SymbolicIR: InitialType
 # """
 # This method exist only temporarily so that we can set default values for dummy derivatives
 # """
-function SymbolicIR.SciMLBase.ODEProblem{true}(ss::SymbolicIR.ScheduledSystem, u0, tspan,
- p = SciMLBase.NullParameters;
- kwargs...)
- fun = ODEFunction{true}(ss)
- defs = ss.state.sys.info.defaults
- if u0 !== nothing && !(u0 isa InitialType)
- error("`u0` must be an array or a dictionary")
- end
- if p !== SciMLBase.NullParameters && !(p isa InitialType)
- error("`p` must be an array or a dictionary")
- end
- if u0 isa InitialType && eltype(u0) <: Pair
- u0 = Dict{SymbolicIR.IRElement, Any}(SymbolicIR.IRElement(SymbolicIR.SymbolicsConversion.extract_ir(k)) => v
- for (k, v) in u0)
- defs = merge(defs, u0)
- end
- if p isa InitialType && eltype(p) <: Pair
- p = Dict{SymbolicIR.IRElement, Any}(SymbolicIR.IRElement(SymbolicIR.SymbolicsConversion.SymbolicIR.extract_ir(k)) => v
- for (k, v) in p)
- defs = merge(defs, p)
- end
- u0 = Float64[get(defs, SymbolicIR.to_normal(v), 0.0)
- for v in ModelingToolkit.states(ss)]
- ps = Float64[defs[v] for v in ModelingToolkit.parameters(ss)]
- ODEProblem{true}(fun, u0, tspan, ps; kwargs...)
-end
+# function SymbolicIR.SciMLBase.ODEProblem{true}(ss::SymbolicIR.ScheduledSystem, u0, tspan,
+#  p = SciMLBase.NullParameters;
+#  kwargs...)
+#  fun = ODEFunction{true}(ss)
+#  defs = ss.state.sys.info.defaults
+#  if u0 !== nothing && !(u0 isa InitialType)
+#  error("`u0` must be an array or a dictionary")
+#  end
+#  if p !== SciMLBase.NullParameters && !(p isa InitialType)
+#  error("`p` must be an array or a dictionary")
+#  end
+#  if u0 isa InitialType && eltype(u0) <: Pair
+#  u0 = Dict{SymbolicIR.IRElement, Any}(SymbolicIR.IRElement(SymbolicIR.SymbolicsConversion.extract_ir(k)) => v
+#  for (k, v) in u0)
+#  defs = merge(defs, u0)
+#  end
+#  if p isa InitialType && eltype(p) <: Pair
+#  p = Dict{SymbolicIR.IRElement, Any}(SymbolicIR.IRElement(SymbolicIR.SymbolicsConversion.SymbolicIR.extract_ir(k)) => v
+#  for (k, v) in p)
+#  defs = merge(defs, p)
+#  end
+#  u0 = Float64[get(defs, SymbolicIR.to_normal(v), 0.0)
+#  for v in ModelingToolkit.states(ss)]
+#  ps = Float64[defs[v] for v in ModelingToolkit.parameters(ss)]
+#  ODEProblem{true}(fun, u0, tspan, ps; kwargs...)
+# end
 
 export Orientation, RotationMatrix, ori
 include("orientation.jl")

diff --git a/test/runtests.jl b/test/runtests.jl
@@ -798,3 +798,34 @@ sol = solve(prob, Rodas4())
 isinteractive() && plot(sol, idxs = body.r_0[2], title = "Free falling body")
 y = sol(0:0.1:10, idxs = body.r_0[2])
 @test y≈-9.81 / 2 .* (0:0.1:10) .^ 2 atol=1e-2 # Analytical solution to acceleration problem
+
+
+# ==============================================================================
+## Dzhanibekov effect ==========================================================
+# ==============================================================================
+world = Multibody.world
+@named freeMotion = FreeMotion(enforceStates = true, isroot = true)
+@named body = Body(m = 1, isroot = false, I_11 = 1, I_22 = 10, I_33 = 100)
+
+eqs = [connect(world.frame_b, freeMotion.frame_a)
+ connect(freeMotion.frame_b, body.frame_a)]
+
+@named model = ODESystem(eqs, t,
+ systems = [world;
+ freeMotion;
+ body])
+# ssys = structural_simplify(model, allow_parameters = false)
+ssys = structural_simplify(IRSystem(model))
+
+# Can't get initial condition to bite, Yingbo
+prob = ODEProblem(ssys,
+ [
+ freeMotion.v_rel_a .=> [0,1,0] # Movement in y direction
+ freeMotion.phi_d .=> [0.01, 3.0, 0.02] # Rotation around y axis
+ freeMotion.phi .=> 0.001randn.() 
+ world.g => 0 # In space
+ ], 
+ (0, 100))
+
+sol = solve(prob, Rodas4())
+isinteractive() && plot(sol, idxs = collect(freeMotion.phi), title = "Dzhanibekov effect")