multibody example

Author: Brad Carman

src/Mechanical/MultiBody2D/components.jl

@@ -1,56 +1,101 @@
-function Link(; name, m, l, I, g)
+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
     end
 
     vars = @variables begin
         A(t) = 0
         dA(t) = 0
         ddA(t) = 0
 
-        x_cm(t) = l / 2
-        y_cm(t) = 0
-
-        dx_cm(t) = 0
-        dy_cm(t) = 0
-
-        ddx_cm(t) = 0
-        ddy_cm(t) = 0
-
         fx1(t) = 0
         fy1(t) = 0
 
-        x1(t) = 0
+        fx2(t) = 0
+        fy2(t) = 0
+
+        x1(t) = x1_0
         dx1(t) = 0
+        
+        y1(t) = y1_0
+        dy1(t) = 0
+        
+
+        x2(t) = l+x1_0
+        dx2(t) = 0
+        
+
+        y2(t) = 0
+        dy2(t) = 0
+        
+
+        x_cm(t) = l/2+x1_0
+        dx_cm(t) = 0
+        ddx_cm(t) = 0
+
+        y_cm(t) = 0
+        dy_cm(t) = 0
+        ddy_cm(t) = 0
     end
 
     @named TX1 = MechanicalPort()
+    @named TY1 = MechanicalPort()
 
-    eqs = [D(x_cm) ~ dx_cm
-           D(y_cm) ~ dy_cm
-           D(dx_cm) ~ ddx_cm
-           D(dy_cm) ~ ddy_cm
+    @named TX2 = MechanicalPort()
+    @named TY2 = MechanicalPort()
+
+    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
+           m * ddx_cm ~ fx1 + fx2
 
            # y forces
-           m * ddy_cm ~ m * g + fy1
+           m * ddy_cm ~ m * g + fy1 + fy2
 
            # torques
-           I * ddA ~ m * g * (l / 2) * cos(A)
+           I * ddA ~ -fy1 * (x2 - x1)/2 + fy2 * (x2 - x1)/2 + fx1 * (y2 - y1)/2 - fx2 * (y2 - y1)/2
 
            # geometry
-           x_cm ~ l * cos(A) / 2 + x1
-           y_cm ~ l * sin(A) / 2
+           x2 ~ l * cos(A) + x1
+           y2 ~ l * sin(A) + y1
+
+           x_cm ~ l * cos(A)/2 + x1
+           y_cm ~ l * sin(A)/2 + y1
+
            TX1.f ~ fx1
-           D(x1) ~ TX1.v]
+           TX1.v ~ dx1
+
+           TY1.f ~ fy1
+           TY1.v ~ dy1
+           
+           TX2.f ~ fx2
+           TX2.v ~ dx2
+
+           TY2.f ~ fy2
+           TY2.v ~ dy2]
 
-    return ODESystem(eqs, t, vars, pars; name = name, systems = [TX1],
-                     defaults = [TX1.v => 0])
+    return ODESystem(eqs, t, vars, pars; name = name, systems = [TX1, TY1, TX2, TY2],
+                     defaults = [TX1.v => 0, TY1.v => 0, TX2.v => 0, TY2.v => 0])
 end

test/Mechanical/multibody.jl

@@ -3,31 +3,109 @@ using ModelingToolkitStandardLibrary.Mechanical.MultiBody2D
 using ModelingToolkitStandardLibrary.Mechanical.Translational
 using DifferentialEquations
 # using Setfield
+using Test
 
 @parameters t
 
-@named link = Link(; m = 1, l = 10, I = π * 8.33, g = -9.807)
+@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 cart = Mass(; m = 1, s_0 = 0)
 # @named force = SineForce(;amp=3e3, freq=15)
-# @named fixed = Fixed()
+@named fixed = Fixed()
 # @named m1 = Mass(;m=0.5)
 # @named m2 = Mass(;m=0.5)
 
 eqs = [
-    connect(link.TX1, cart.flange), #, force.flange)
-    # connect(link.TY1, fixed.flange)
-    # connect(link.TX2, m1.flange)
-    # connect(link.TY2, m2.flange)
+    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 = [link, cart])
+@named model = ODESystem(eqs, t, [], []; systems = [link1, link2, cart, fixed])
 
+#TODO: This gives an error
 sys = structural_simplify(model)
-# 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, 5), []; jac = true)
+# 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.0001, adaptive = false)
-# sol = solve(prob)
+sol = solve(prob, ImplicitEuler(nlsolve = NEWTON), dt = 0.001, adaptive = false)
+
+@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))
+
+    m1x1 = @lift($s[idx[link1.x1]])
+    m1x2 = @lift($s[idx[link1.x2]])
+    m2x1 = @lift($s[idx[link2.x1]])
+    m2x2 = @lift($s[idx[link2.x2]])
+
+    m1y1 = @lift($s[idx[link1.y1]])
+    m1y2 = @lift($s[idx[link1.y2]])
+    m2y1 = @lift($s[idx[link2.y1]])
+    m2y2 = @lift($s[idx[link2.y2]])
+
+    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[4,:])
+
+    # scatter!(a, tm, m1x)
+    # scatter!(a, tm, m2x)
+    # ylims!(a, -60, 30)
+
+    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
+    end
+    =#
+
+    nothing
+end
+
+plot_link(sol, sys, 20)
+
+=#