From dad8645575107260d1f1edec40f10ce63c1d0724 Mon Sep 17 00:00:00 2001 From: Brad Carman Date: Tue, 4 Oct 2022 18:27:24 -0400 Subject: [PATCH] working 5 link model --- src/Mechanical/MultiBody2D/MultiBody2D.jl | 5 +- src/Mechanical/MultiBody2D/components.jl | 109 ++++++++++++++++++---- src/Mechanical/MultiBody2D/utils.jl | 12 +++ test/Mechanical/multibody.jl | 107 ++++++++++++++------- 4 files changed, 178 insertions(+), 55 deletions(-) create mode 100644 src/Mechanical/MultiBody2D/utils.jl diff --git a/src/Mechanical/MultiBody2D/MultiBody2D.jl b/src/Mechanical/MultiBody2D/MultiBody2D.jl index ed9ccb80d..a39e53abd 100644 --- a/src/Mechanical/MultiBody2D/MultiBody2D.jl +++ b/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 diff --git a/src/Mechanical/MultiBody2D/components.jl b/src/Mechanical/MultiBody2D/components.jl index e2c427b80..cdcd27753 100644 --- a/src/Mechanical/MultiBody2D/components.jl +++ b/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 + diff --git a/src/Mechanical/MultiBody2D/utils.jl b/src/Mechanical/MultiBody2D/utils.jl new file mode 100644 index 000000000..f8330b9c9 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/test/Mechanical/multibody.jl b/test/Mechanical/multibody.jl index fc3445b32..19c03f537 100644 --- a/test/Mechanical/multibody.jl +++ b/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) -=# +=# \ No newline at end of file