Add state_priority annotation, bound MTK, and format

Author: YingboMa

Project.toml

@@ -12,7 +12,7 @@ Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7"
 
 [compat]
 IfElse = "0.1"
-ModelingToolkit = "8.24"
+ModelingToolkit = "8.26"
 OffsetArrays = "1"
 Symbolics = "4.9"
 julia = "1.6"

links.mp4

@@ -1,17 +1,17 @@
-function Link(; name, m, l, I, g, x1_0=0, y1_0=0)
+function Link(; name, m, l, I, g, x1_0 = 0, y1_0 = 0)
     pars = @parameters begin
         m = m
         l = l
         I = I
         g = g
-        x1_0=x1_0
-        y1_0=y1_0
+        x1_0 = x1_0
+        y1_0 = y1_0
     end
 
     vars = @variables begin
-        A(t) = 0
-        dA(t) = 0
-        ddA(t) = 0
+        (A(t) = 0), [state_priority = 10]
+        (dA(t) = 0), [state_priority = 10]
+        (ddA(t) = 0), [state_priority = 10]
 
         fx1(t) = 0
         fy1(t) = 0
@@ -21,20 +21,17 @@ function Link(; name, m, l, I, g, x1_0=0, y1_0=0)
 
         x1(t) = x1_0
         dx1(t) = 0
-        
+
         y1(t) = y1_0
         dy1(t) = 0
-        
 
-        x2(t) = l+x1_0
+        x2(t) = l + x1_0
         dx2(t) = 0
-        
 
         y2(t) = 0
         dy2(t) = 0
-        
 
-        x_cm(t) = l/2+x1_0
+        x_cm(t) = l / 2 + x1_0
         dx_cm(t) = 0
         ddx_cm(t) = 0
 
@@ -49,50 +46,38 @@ function Link(; name, m, l, I, g, x1_0=0, y1_0=0)
     @named TX2 = MechanicalPort()
     @named TY2 = MechanicalPort()
 
-    eqs = [
-           D(A) ~ dA
+    eqs = [D(A) ~ dA
            D(dA) ~ ddA
-
            D(x1) ~ dx1
-           
            D(y1) ~ dy1
-           
            D(x2) ~ dx2
-           
            D(y2) ~ dy2
-           
            D(x_cm) ~ dx_cm
            D(dx_cm) ~ ddx_cm
-
            D(y_cm) ~ dy_cm
            D(dy_cm) ~ ddy_cm
 
-
            # x forces
            m * ddx_cm ~ fx1 + fx2
 
            # y forces
            m * ddy_cm ~ m * g + fy1 + fy2
 
            # torques
-           I * ddA ~ -fy1 * (x2 - x1)/2 + fy2 * (x2 - x1)/2 + fx1 * (y2 - y1)/2 - fx2 * (y2 - y1)/2
+           I * ddA ~ -fy1 * (x2 - x1) / 2 + fy2 * (x2 - x1) / 2 + fx1 * (y2 - y1) / 2 -
+                     fx2 * (y2 - y1) / 2
 
            # geometry
            x2 ~ l * cos(A) + x1
            y2 ~ l * sin(A) + y1
-
-           x_cm ~ l * cos(A)/2 + x1
-           y_cm ~ l * sin(A)/2 + y1
-
+           x_cm ~ l * cos(A) / 2 + x1
+           y_cm ~ l * sin(A) / 2 + y1
            TX1.f ~ fx1
            TX1.v ~ dx1
-
            TY1.f ~ fy1
            TY1.v ~ dy1
-           
            TX2.f ~ fx2
            TX2.v ~ dx2
-
            TY2.f ~ fy2
            TY2.v ~ dy2]
 

src/Mechanical/MultiBody2D/components.jl

@@ -8,80 +8,70 @@ using Test
 @parameters t
 
 @named link1 = Link(; m = 1, l = 10, I = 84, g = -9.807)
-@named link2 = Link(; m = 1, l = 10, I = 84, g = -9.807, x1_0=10)
+@named link2 = Link(; m = 1, l = 10, I = 84, g = -9.807, x1_0 = 10)
 @named cart = Mass(; m = 1, s_0 = 0)
 # @named force = SineForce(;amp=3e3, freq=15)
 @named fixed = Fixed()
 # @named m1 = Mass(;m=0.5)
 # @named m2 = Mass(;m=0.5)
 
-eqs = [
-    connect(link1.TX1, cart.flange) #, force.flange)
-    connect(link1.TY1, fixed.flange)
-    connect(link1.TX2, link2.TX1)
-    connect(link1.TY2, link2.TY1)
-]
+eqs = [connect(link1.TX1, cart.flange) #, force.flange)
+       connect(link1.TY1, fixed.flange)
+       connect(link1.TX2, link2.TX1)
+       connect(link1.TY2, link2.TY1)]
 
 @named model = ODESystem(eqs, t, [], []; systems = [link1, link2, cart, fixed])
 
-#TODO: This gives an error
 sys = structural_simplify(model)
 
 # The below code does work...
 #=
-sys = expand_connections(model)
-@set! sys.eqs = [(typeof(eq.lhs) != ModelingToolkit.Connection) & !ModelingToolkit._iszero(eq.lhs) & !ModelingToolkit.isdifferential(eq.lhs) ? 0 ~ eq.rhs - eq.lhs : eq for eq in sys.eqs]
-
-prob = ODEProblem(sys, [], (0.0, 20), []; jac = true)
-NEWTON = NLNewton(check_div = false, always_new = true, max_iter = 100, relax = 4 // 10)
-sol = solve(prob, ImplicitEuler(nlsolve = NEWTON), dt = 0.001, adaptive = false)
+unset_vars = setdiff(states(sys), keys(ModelingToolkit.defaults(sys)))
+prob = ODEProblem(sys, unset_vars .=> 0.0, (0.0, 20), []; jac = true)
+sol = solve(prob, Rodas5P())
 
 @test sol[cart.s][end] ≈ 4.767 atol=1e-3
 
 plot(sol, idxs = [cart.s])
 =#
 
-
 #=
 using CairoMakie
 f = Figure()
 a = Axis(f[1,1],xlabel="time [s]", ylabel="cart x pos. [m]")
 lines!(a, sol.t, sol[cart.s])
 f
 
-
-
 function plot_link(sol, sys, tmax)
     tm = Observable(0.0)
     idx = Dict(reverse.(enumerate(states(sys))))
 
     fig = Figure()
     a = Axis(fig[1,1], aspect=DataAspect(), )
     hidedecorations!(a)
-    s = @lift(sol($tm))
+    s = @lift(sol($tm, idxs=[link1.x1, link1.x2, link2.x1, link2.x2, link1.y1, link1.y2, link2.y1, link2.y2]))
 
-    m1x1 = @lift($s[idx[link1.x1]])
-    m1x2 = @lift($s[idx[link1.x2]])
-    m2x1 = @lift($s[idx[link2.x1]])
-    m2x2 = @lift($s[idx[link2.x2]])
+    m1x1 = @lift($s[1])
+    m1x2 = @lift($s[2])
+    m2x1 = @lift($s[3])
+    m2x2 = @lift($s[4])
 
-    m1y1 = @lift($s[idx[link1.y1]])
-    m1y2 = @lift($s[idx[link1.y2]])
-    m2y1 = @lift($s[idx[link2.y1]])
-    m2y2 = @lift($s[idx[link2.y2]])
+    m1y1 = @lift($s[5])
+    m1y2 = @lift($s[6])
+    m2y1 = @lift($s[7])
+    m2y2 = @lift($s[8])
 
     sz1 = 0.5
     # lines!(a, [-sz1, sz1, sz1, -sz1, -sz1], @lift([$m1x1, $m1x1, $m1x1+sz1*2, $m1x1+sz1*2, $m1x1]))
 
-    
     lines!(a, @lift([$m1x1, $m1x2]), @lift([$m1y1, $m1y2]), linewidth=10, color=:blue)
     lines!(a, @lift([$m2x1, $m2x2]), @lift([$m2y1, $m2y2]), linewidth=10, color=:red)
-    
+
     CairoMakie.ylims!(a, -40, 20)
     CairoMakie.xlims!(a, -20, 40)
 
     # a = Axis(fig[1, 1], xlabel="time [s]", ylabel="position [m]")
-    # lines!(a, sol.t, sol[2,:])   
+    # lines!(a, sol.t, sol[2,:])
     # lines!(a, sol.t, sol[4,:])
 
     # scatter!(a, tm, m1x)
@@ -91,12 +81,11 @@ function plot_link(sol, sys, tmax)
     framerate = 30
     timestamps = range(0, tmax, step=1/framerate)
 
-    
     record(fig, "links.mp4", timestamps;
             framerate = framerate) do t
         tm[] = t
     end
-    
+
     #=
     CairoMakie.Makie.Record(fig, timestamps; framerate=framerate) do t
         tm[] = t
@@ -108,4 +97,4 @@ end
 
 plot_link(sol, sys, 20)
 
-=#
+=#