Start to MultiBody2D

src/Mechanical/MultiBody2D/MultiBody2D.jl

@@ -6,7 +6,10 @@ using ..Translational
 @parameters t
 D = Differential(t)
 
-export Link
+export Link, RevoluteJoint, MultiBody2Translational
 include("components.jl")
 
+export RigidBody2DPort
+include("utils.jl")
+
 end

src/Mechanical/MultiBody2D/components.jl

@@ -40,20 +40,25 @@ function Link(; name, m, l, I, g, x1_0 = 0, y1_0 = 0)
         ddy_cm(t) = 0
     end
 
-    @named TX1 = MechanicalPort()
-    @named TY1 = MechanicalPort()
+    @named M1 = RigidBody2DPort()
+    @named M2 = RigidBody2DPort()
 
-    @named TX2 = MechanicalPort()
-    @named TY2 = MechanicalPort()
+    # let ----------------------------------
+    Δx = (x2 - x1)/2
+    Δy = (y2 - y1)/2
 
     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
 
@@ -64,23 +69,89 @@ function Link(; name, m, l, I, g, x1_0 = 0, y1_0 = 0)
            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 ~ Δx*fy2 - Δy*fx2 - Δx*fy1 + Δy*fx1 + M1.T_z + M2.T_z
+
 
            # 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
-           TX1.f ~ fx1
-           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, TY1, TX2, TY2],
-                     defaults = [TX1.v => 0, TY1.v => 0, TX2.v => 0, TY2.v => 0])
+
+           x_cm ~ Δx + x1
+           y_cm ~ Δy + y1
+
+           M1.f_x ~ fx1
+           M1.f_y ~ fy1
+
+           M1.dx ~ dx1
+           M1.dy ~ dy1
+           M1.dA ~ dA
+
+           M2.f_x ~ fx2
+           M2.f_y ~ fy2
+
+           M2.dx ~ dx2
+           M2.dy ~ dy2
+           M2.dA ~ dA
+
+           ]
+
+    return ODESystem(eqs, t, vars, pars; name = name, systems = [M1, M2],
+    defaults = [M1.dx => 0, M1.dy => 0, M1.dA => 0, M2.dx => 0, M2.dy => 0, M2.dA => 0])
 end
+
+
+function RevoluteJoint(; name, d=0)
+    pars = @parameters begin
+        d=d
+    end
+
+    vars = @variables begin
+        dA(t) = 0
+        T_z(t) = 0
+    end
+
+    @named M1 = RigidBody2DPort()
+    @named M2 = RigidBody2DPort()
+
+    eqs = [dA ~ M1.dA - M2.dA
+
+        T_z ~ dA*d
+
+        M1.T_z ~ +T_z
+        M2.T_z ~ -T_z
+
+        M1.f_x ~ -M2.f_x
+        M1.f_y ~ -M2.f_y
+        M1.dx ~ M2.dx
+        M1.dy ~ M2.dy]
+
+    return ODESystem(eqs, t, vars, pars; name = name, systems = [M1, M2],
+                     defaults = [M1.dx => 0, M1.dy => 0, M1.dA => 0, M2.dx => 0, M2.dy => 0, M2.dA => 0])
+end
+
+function MultiBody2Translational(; name)
+
+    @named M = RigidBody2DPort()
+    @named T = MechanicalPort()
+
+    eqs = [
+            M.f_x ~ -T.f
+
+            M.dx ~ T.v
+            M.dy ~ 0
+            M.dA ~ 0
+           ]
+    return compose(ODESystem(eqs, t, [], []; name = name, defaults = [M.dx => 0, M.dy => 0, M.dA => 0, T.v => 0, T.f => 0]),
+                   M, T)
+
+end
+
+# function FixedFrame(; name)
+#     @named T = RigidBody2DPort()
+#     eqs = [T.v_x ~ 0
+#            T.v_y ~ 0
+#            T.dA ~ 0]
+#     return compose(ODESystem(eqs, t, [], []; name = name, defaults = [T.v_x => 0, T.v_y => 0, T.dA => 0]),
+#                    T)
+# end
+

src/Mechanical/MultiBody2D/utils.jl

@@ -0,0 +1,12 @@
+@connector function RigidBody2DPort(; name)
+    pars = []
+    vars = @variables begin
+        dx(t) = 0
+        f_x(t), [connect = Flow]
+        dy(t) = 0
+        f_y(t), [connect = Flow]
+        dA(t) = 0
+        T_z(t), [connect = Flow]
+    end
+    ODESystem(Equation[], t, vars, pars, name = name, defaults = [f_x => 0, f_y => 0, T_z=>0])
+end

test/Mechanical/multibody.jl

@@ -2,45 +2,62 @@ using ModelingToolkit
 using ModelingToolkitStandardLibrary.Mechanical.MultiBody2D
 using ModelingToolkitStandardLibrary.Mechanical.Translational
 using DifferentialEquations
-# using Setfield
+using Setfield
 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 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)
+@named link1 = Link(; m = 1, l = 10, I = 85, g = -9.807)
+@named link2 = Link(; m = 1, l = 10, I = 85, g = -9.807, x1_0=10)
+@named link3 = Link(; m = 1, l = 10, I = 85, g = -9.807, x1_0=20)
+@named link4 = Link(; m = 1, l = 10, I = 85, g = -9.807, x1_0=30)
+@named link5 = Link(; m = 1, l = 10, I = 85, g = -9.807, x1_0=40)
 
-eqs = [connect(link1.TX1, cart.flange) #, force.flange)
-       connect(link1.TY1, fixed.flange)
-       connect(link1.TX2, link2.TX1)
-       connect(link1.TY2, link2.TY1)]
+@named joint1 = RevoluteJoint(d=1)
+@named joint2 = RevoluteJoint(d=200)
+@named joint3 = RevoluteJoint(d=200)
+@named joint4 = RevoluteJoint(d=200)
+@named joint5 = RevoluteJoint(d=200)
 
-@named model = ODESystem(eqs, t, [], []; systems = [link1, link2, cart, fixed])
+@named cart = Mass(; m = 5, s_0 = 0)
+@named mb2t = MultiBody2Translational()
 
-sys = structural_simplify(model)
+eqs = [
 
-# The below code does work...
-#=
+    connect(cart.flange, mb2t.T)
+
+    connect(mb2t.M,   joint1.M1)
+    connect(link1.M1, joint1.M2)
+    connect(link1.M2, joint2.M1)
+    connect(link2.M1, joint2.M2)
+    connect(link2.M2, joint3.M1)
+    connect(link3.M1, joint3.M2)
+    connect(link3.M2, joint4.M1)
+    connect(link4.M1, joint4.M2)
+    connect(link4.M2, joint5.M1)
+    connect(link5.M1, joint5.M2)
+    # connect_mbs(link5.M2)
+
+       ]
+
+@named model = ODESystem(eqs, t, [], []; systems = [cart, mb2t, link1, link2, link3, link4, link5, joint1, joint2, joint3, joint4, joint5]) 
+
+sys = structural_simplify(model)
 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
+NEWTON = NLNewton(check_div = false, always_new = true, max_iter = 100, relax = 4 // 10)
+sol = solve(prob, ImplicitEuler(nlsolve = NEWTON), dt = 4e-4, adaptive = false)
 
-plot(sol, idxs = [cart.s])
-=#
+@test sol[link1.x1][end] ≈ 18.48185172484587
 
 #=
 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
+begin
+    f = Figure()
+    a = Axis(f[1,1],xlabel="time [s]", ylabel="cart x pos. [m]")
+    lines!(a, sol.t, sol[link1.x1])
+    f
+end
 
 function plot_link(sol, sys, tmax)
     tm = Observable(0.0)
@@ -49,26 +66,46 @@ function plot_link(sol, sys, tmax)
     fig = Figure()
     a = Axis(fig[1,1], aspect=DataAspect(), )
     hidedecorations!(a)
-    s = @lift(sol($tm, idxs=[link1.x1, link1.x2, link2.x1, link2.x2, link1.y1, link1.y2, link2.y1, link2.y2]))
+    s = @lift(sol($tm, idxs=[link1.x1, link1.x2, link1.y1, link1.y2, link2.x1, link2.x2, link2.y1, link2.y2, link3.x1, link3.x2, link3.y1, link3.y2, link4.x1, link4.x2, link4.y1, link4.y2, link5.x1, link5.x2, link5.y1, link5.y2]))
 
     m1x1 = @lift($s[1])
     m1x2 = @lift($s[2])
-    m2x1 = @lift($s[3])
-    m2x2 = @lift($s[4])
+    m1y1 = @lift($s[3])
+    m1y2 = @lift($s[4])
 
-    m1y1 = @lift($s[5])
-    m1y2 = @lift($s[6])
+    m2x1 = @lift($s[5])
+    m2x2 = @lift($s[6])
     m2y1 = @lift($s[7])
     m2y2 = @lift($s[8])
 
+    m3x1 = @lift($s[9])
+    m3x2 = @lift($s[10])
+    m3y1 = @lift($s[11])
+    m3y2 = @lift($s[12])
+
+    m4x1 = @lift($s[13])
+    m4x2 = @lift($s[14])
+    m4y1 = @lift($s[15])
+    m4y2 = @lift($s[16])
+
+    m5x1 = @lift($s[17])
+    m5x2 = @lift($s[18])
+    m5y1 = @lift($s[19])
+    m5y2 = @lift($s[20])
+
     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)
+    hlines!(a, 0, color=:gray)
+    # scatter!(a, m1x1[], marker=:square, markersize=10, color=:black)
+    lines!(a, @lift([$m1x1, $m1x2]) , @lift([$m1y1, $m1y2]), linewidth=10, color=:blue)
+    lines!(a, @lift([$m2x1, $m2x2]) , @lift([$m2y1, $m2y2]), linewidth=10, color=:red)
+    lines!(a, @lift([$m3x1, $m3x2]) , @lift([$m3y1, $m3y2]), linewidth=10, color=:blue)
+    lines!(a, @lift([$m4x1, $m4x2]) , @lift([$m4y1, $m4y2]), linewidth=10, color=:red)
+    lines!(a, @lift([$m5x1, $m5x2]) , @lift([$m5y1, $m5y2]), linewidth=10, color=:blue)
 
-    CairoMakie.ylims!(a, -40, 20)
-    CairoMakie.xlims!(a, -20, 40)
+    CairoMakie.ylims!(a, -50, 20)
+    CairoMakie.xlims!(a, -20, 50)
 
     # a = Axis(fig[1, 1], xlabel="time [s]", ylabel="position [m]")
     # lines!(a, sol.t, sol[2,:])
@@ -97,4 +134,4 @@ end
 
 plot_link(sol, sys, 20)
 
-=#
+=#