From 1dab5f103142fae25baf490642366c25f2b80d20 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <baggepinnen@gmail.com>
Date: Fri, 28 Apr 2023 13:55:22 +0200
Subject: [PATCH] Dzhanibekov effect

---
 src/Multibody.jl | 57 ++++++++++++++++++++++++------------------------
 test/runtests.jl | 31 ++++++++++++++++++++++++++
 2 files changed, 59 insertions(+), 29 deletions(-)

diff --git a/src/Multibody.jl b/src/Multibody.jl
index b9a277dd..50b88ce0 100644
--- 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
index 9a46a08d..7e7cf4d4 100644
--- 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")