From 5db558756f87fb5e0096e0ff91ccf5acf83de1c4 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 20 Jun 2024 10:12:40 +0200 Subject: [PATCH] add swing tutorial --- docs/make.jl | 1 + docs/src/examples/swing.md | 163 +++++++++++++++++++++++++++++++++++++ src/joints.jl | 5 +- 3 files changed, 168 insertions(+), 1 deletion(-) create mode 100644 docs/src/examples/swing.md diff --git a/docs/make.jl b/docs/make.jl index 5f3b6491..e35f65a4 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -31,6 +31,7 @@ makedocs(; "Kinematic loops" => "examples/kinematic_loops.md", "Industrial robot" => "examples/robot.md", "Ropes, cables and chains" => "examples/ropes_and_cables.md", + "Swing" => "examples/swing.md", "Bodies in space" => "examples/space.md", ], "3D rendering" => "rendering.md", diff --git a/docs/src/examples/swing.md b/docs/src/examples/swing.md new file mode 100644 index 00000000..44957554 --- /dev/null +++ b/docs/src/examples/swing.md @@ -0,0 +1,163 @@ +# Swing + +In this example, we will model a swing consisting of a rigid seat suspended in 4 ropes, mounted symmetrically in a ceiling. Each rope is modeled as a stiff rod with a small point mass at the center of gravity, terminated by a parallel spring-damper to model slight flexibility in the ropes. The ceiling mounting points are modeled as spherical joints, i.e., they do not transmit any torque in any direction. The rim of the seat is modeled as 4 rigid bodies configured in a square, as well as one point mass representing the load, located slightly below the rim assembly. + +![animation](swing.gif) + + +We start by defining a single rope component and attach it to a body in order to verify that it's working. + +```@example SWING +using Multibody +using ModelingToolkit +using Plots +using JuliaSimCompiler +using OrdinaryDiffEq + +t = Multibody.t +D = Differential(t) +world = Multibody.world + +W(args...; kwargs...) = Multibody.world + +@mtkmodel SwingRope begin + @components begin + frame_a = Frame() + frame_b = Frame() + joint1 = Spherical(isroot=true, state=true, d=0.001) + rope = BodyShape(r=[0.0,-1,0], m=0.05, radius=0.01) + spring = Spring(c = inv(0.04/60), m=0.01) + damper = Damper(d = 50.0) + end + @equations begin + connect(frame_a, joint1.frame_a) + connect(joint1.frame_b, rope.frame_a) + + connect(rope.frame_b, spring.frame_a, damper.frame_a) + connect(spring.frame_b, damper.frame_b, frame_b) + end +end + +@mtkmodel SimpleSwing begin + @structural_parameters begin + h = 2 + w = 0.4 + end + @components begin + world = W() + upper_trans1 = FixedTranslation(r=[-w/2, 0, 0]) + rope1 = SwingRope(rope.r=[-w/2, h, -w/2]) + body = Body(m=6, isroot=true, I_11=0.1, I_22=0.1, I_33=0.1) + damper = Damper(d=10.0) + end + @equations begin + connect(world.frame_b, upper_trans1.frame_a) + connect(rope1.frame_a, upper_trans1.frame_b) + # connect(world.frame_b, rope1.frame_a) + connect(rope1.frame_b, body.frame_a) + + connect(world.frame_b, damper.frame_a) + connect(body.frame_a, damper.frame_b) + end +end +@named model = SimpleSwing() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +prob = ODEProblem(ssys, [ + collect(model.body.v_0) .=> 0; + collect(model.body.w_a) .=> 0; +], (0, 4)) +sol = solve(prob, ImplicitEuler(autodiff=false), reltol=5e-3) +@assert SciMLBase.successful_retcode(sol) +``` +This makes for a rather interesting-looking springy pendulum! + +```@example SWING +import CairoMakie +Multibody.render(model, sol; z = -5, filename = "simple_swing.gif") # Use "simple_swing.mp4" for a video file +nothing # hide +``` + +Next, we create the full swing assembly + +```@example SWING +@mtkmodel Swing begin + @structural_parameters begin + h = 2 + w = 0.4 + end + @components begin + world = W() + upper_trans1 = FixedTranslation(r=[-w/2, 0, 0]) + upper_trans2 = FixedTranslation(r=[ w/2, 0, 0]) + rope1 = SwingRope(rope.r=[-w/2, -h, -w/2]) + rope2 = SwingRope(rope.r=[-w/2, -h, w/2]) + rope3 = SwingRope(rope.r=[ w/2, -h, -w/2]) + rope4 = SwingRope(rope.r=[ w/2, -h, w/2]) + + body_back = BodyShape(m=0.1, r = [w, 0, 0]) + body_front = BodyShape(m=0.1, r = [w, 0, 0]) + body_left = BodyShape(m=0.1, r = [0, 0, w]) + body_right = BodyShape(m=0.1, r = [0, 0, -w]) + + body = Body(m=6, isroot=true, r_cm = [w/2, -w/2, w/2], vel_from_R=true) + + damper = Damper(d=0.5) + end + @equations begin + # Rope assembly + connect(world.frame_b, upper_trans1.frame_a, upper_trans2.frame_a) + connect(rope1.frame_a, rope2.frame_a, upper_trans1.frame_b) + connect(rope3.frame_a, rope4.frame_a, upper_trans2.frame_b) + + # Body assembly + connect(body_back.frame_a, body_left.frame_a, rope1.frame_b) + connect(body_left.frame_b, body_front.frame_a, rope2.frame_b) + connect(body_front.frame_b, body_right.frame_a, rope4.frame_b) + connect(body_right.frame_b, rope3.frame_b) # Don't close the rigid kinematic loop + connect(body_back.frame_a, body.frame_a) + + # World damping (damps swing motion) + connect(world.frame_b, damper.frame_a) + connect(body.frame_a, damper.frame_b) + end +end + +@named model = Swing() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) + +d = 10 +dj = 0.01 +prob = ODEProblem(ssys, [ + collect(model.body_right.body.r_0) .=> [0, -2, 0.5]; + # collect(model.body_left.body.r_0) .=> [0, -2, -0.5]; + collect(model.body_right.body.v_0) .=> 0; + model.damper.d => 1; + model.rope1.damper.d => d; + model.rope2.damper.d => d; + model.rope3.damper.d => d; + model.rope4.damper.d => d; + + model.rope1.joint1.d => dj; + model.rope2.joint1.d => dj; + model.rope3.joint1.d => dj; + model.rope4.joint1.d => dj; +], (0.0, 6)) + + +@time sol = solve(prob, ImplicitEuler(autodiff=false), reltol=1e-2) +@assert SciMLBase.successful_retcode(sol) + +Plots.plot(sol, idxs = [collect(model.body.r_0);]) +``` + +```@example SWING +import CairoMakie +Multibody.render(model, sol; y = -1, z = -3, lookat = [0, -1, 0], filename = "swing.gif") # Use "swing.mp4" for a video file +nothing # hide +``` + +![animation](swing.gif) + +There is an initial transient in the beginning where the springs in the ropes are vibrating substantially, but after about a second this transient is damped out (thanks in part to the, in this case desired, numerical damping contributed by the implicit Euler solver). \ No newline at end of file diff --git a/src/joints.jl b/src/joints.jl index c19bc58c..7b6e9e0f 100644 --- a/src/joints.jl +++ b/src/joints.jl @@ -180,6 +180,8 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin radius = 0.1, quat = false, ) + + dnum = d @named begin ptf = PartialTwoFrames() R_rel = NumRotationMatrix() @@ -188,6 +190,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin pars = @parameters begin radius = radius, [description = "radius of the joint in animations"] color[1:4] = color, [description = "color of the joint in animations (RGBA)"] + d = d end @unpack frame_a, frame_b = ptf # @parameters begin # Currently not using parameters due to these appearing in if statements @@ -199,7 +202,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin ] end # torque balance - if d <= 0 + if dnum <= 0 eqs = [zeros(3) .~ collect(frame_a.tau) zeros(3) .~ collect(frame_b.tau) collect(frame_b.r_0) .~ collect(frame_a.r_0)]