From 39319bde333ef2812bd538f30a5efc92ff42b110 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 30 Jul 2024 14:10:49 +0200 Subject: [PATCH 1/3] simplifying but unstable after a while --- docs/src/examples/kinematic_loops.md | 70 +++++++++++++++++++++++++++- 1 file changed, 69 insertions(+), 1 deletion(-) diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index 8a95d609..9ef0477a 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/docs/src/examples/kinematic_loops.md @@ -143,4 +143,72 @@ Multibody.render(fourbar2, sol; x=-2, y = 2, z = 3, filename = "fourbar2.gif") # nothing # hide ``` -![animation](fourbar2.gif) \ No newline at end of file +![animation](fourbar2.gif) + + +## Centrifugal governor + +The centrifugal governor is a device used to regulate the speed of a steam engine. The mechanism consists of two balls connected to a rotating shaft. As the shaft rotates, the balls move outwards due to centrifugal force, which in turn moves a lever that regulates the steam flow and thus the engine speed. The mechanism is a kinematic loop with a prismatic joint and three revolute joints per arm. + +```@example kinloop +W(;kwargs...) = Multibody.world + +arm_r = 0.03 + +@mtkmodel GovernorArm begin + # @structural_parameters begin + # end + @components begin + j1 = Revolute(n = [0, 0, 1], phi0 = -deg2rad(10), isroot=true, radius=1.1arm_r) + # j2 = Revolute(n = [0, 0, 1], isroot=false, iscut=true) + j2 = RevolutePlanarLoopConstraint(n = [0, 0, 1], radius=1.1arm_r) + j3 = Revolute(n = [0, 0, 1], isroot=false, radius=1.1arm_r) + # TODO: add heavy ball + b1 = BodyShape(r = [0.1, 0, 0], radius=arm_r) + b2 = BodyShape(r = [-0.1, 0, 0], radius=arm_r) + frame_a = Frame() + frame_b = Frame() + end + @equations begin + connect(frame_a, j1.frame_a) + connect(j1.frame_b, b1.frame_a) + connect(b1.frame_b, j2.frame_a) + connect(j2.frame_b, b2.frame_a) + connect(b2.frame_b, j3.frame_a) + connect(j3.frame_b, frame_b) + end +end + + +r = 0.02 +l = 0.05 +@mtkmodel Governor begin + @components begin + world = W() + arm1 = GovernorArm() + # arm2 = GovernorArm() + cylinder = BodyCylinder(r = [0, -l, 0], diameter = 2r, inner_diameter = 0.03) + mount1 = FixedTranslation(r = [-r, -l/2, 0]) + # mount2 = FixedTranslation(r = [r, -l/2, 0]) + prismatic = Prismatic(n = [0, -1, 0], s0 = 0.1, radius=0.8arm_r) + end + @equations begin + # connect(arm1.frame_a, world.frame_b, arm2.frame_a, prismatic.frame_a) + # connect(arm1.frame_b, mount1.frame_b) + # connect(arm2.frame_b, mount2.frame_b) + # connect(mount1.frame_a, cylinder.frame_a, mount2.frame_a, prismatic.frame_b) + + + connect(arm1.frame_a, world.frame_b, prismatic.frame_a) + connect(arm1.frame_b, mount1.frame_b) + connect(mount1.frame_a, cylinder.frame_a, prismatic.frame_b) + end +end + +@named model = Governor() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +prob = ODEProblem(ssys, [], (0.0, 1.0)) +sol = solve(prob, FBDF(autodiff=true)) +plot(sol) +``` \ No newline at end of file From c1147d0706c176f6b374c301d784caa46497b5d2 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Tue, 30 Jul 2024 14:38:46 +0200 Subject: [PATCH 2/3] six-dim descriptor --- docs/src/examples/kinematic_loops.md | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index 9ef0477a..ec9ce1d6 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/docs/src/examples/kinematic_loops.md @@ -159,11 +159,12 @@ arm_r = 0.03 # @structural_parameters begin # end @components begin - j1 = Revolute(n = [0, 0, 1], phi0 = -deg2rad(10), isroot=true, radius=1.1arm_r) - # j2 = Revolute(n = [0, 0, 1], isroot=false, iscut=true) + j1 = Revolute(n = [0, 0, 1], phi0 = deg2rad(10), isroot=true, radius=1.1arm_r) + # j2 = Revolute(n = [0, 0, 1], isroot=true, state_priority=99) j2 = RevolutePlanarLoopConstraint(n = [0, 0, 1], radius=1.1arm_r) j3 = Revolute(n = [0, 0, 1], isroot=false, radius=1.1arm_r) - # TODO: add heavy ball + # j3 = RevolutePlanarLoopConstraint(n = [0, 0, 1], radius=1.1arm_r) + # todo: add heavy ball b1 = BodyShape(r = [0.1, 0, 0], radius=arm_r) b2 = BodyShape(r = [-0.1, 0, 0], radius=arm_r) frame_a = Frame() @@ -190,16 +191,19 @@ l = 0.05 cylinder = BodyCylinder(r = [0, -l, 0], diameter = 2r, inner_diameter = 0.03) mount1 = FixedTranslation(r = [-r, -l/2, 0]) # mount2 = FixedTranslation(r = [r, -l/2, 0]) - prismatic = Prismatic(n = [0, -1, 0], s0 = 0.1, radius=0.8arm_r) + rev = Revolute(n = [0, 1, 0], w0 = 30, radius=1.01arm_r, state_priority=100) + prismatic = Prismatic(n = [0, -1, 0], s0 = 0.1, radius=0.8arm_r, state_priority=101) end @equations begin - # connect(arm1.frame_a, world.frame_b, arm2.frame_a, prismatic.frame_a) + connect(world.frame_b, rev.frame_a) + # connect(arm1.frame_a, rev.frame_b, arm2.frame_a, prismatic.frame_a) # connect(arm1.frame_b, mount1.frame_b) # connect(arm2.frame_b, mount2.frame_b) # connect(mount1.frame_a, cylinder.frame_a, mount2.frame_a, prismatic.frame_b) - connect(arm1.frame_a, world.frame_b, prismatic.frame_a) + + connect(arm1.frame_a, rev.frame_b, prismatic.frame_a) connect(arm1.frame_b, mount1.frame_b) connect(mount1.frame_a, cylinder.frame_a, prismatic.frame_b) end @@ -208,7 +212,12 @@ end @named model = Governor() model = complete(model) ssys = structural_simplify(IRSystem(model)) -prob = ODEProblem(ssys, [], (0.0, 1.0)) -sol = solve(prob, FBDF(autodiff=true)) + +display([unknowns(ssys) diag(ssys.mass_matrix)]) +## +prob = ODEProblem(ssys, [ + model.rev.w => 32.5 +], (0.0, 1.0)) +sol = solve(prob, Rodas5P(autodiff=true), abstol=1e-8, reltol=1e-8)#, initializealg = ShampineCollocationInit()) plot(sol) ``` \ No newline at end of file From f2b596f0b518bf0efe42f96e3099f875605f2da9 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Wed, 31 Jul 2024 10:33:54 +0200 Subject: [PATCH 3/3] tweak tutorial --- docs/src/examples/kinematic_loops.md | 63 +++++++++++++++++++--------- 1 file changed, 43 insertions(+), 20 deletions(-) diff --git a/docs/src/examples/kinematic_loops.md b/docs/src/examples/kinematic_loops.md index ec9ce1d6..85dad701 100644 --- a/docs/src/examples/kinematic_loops.md +++ b/docs/src/examples/kinematic_loops.md @@ -148,25 +148,28 @@ nothing # hide ## Centrifugal governor -The centrifugal governor is a device used to regulate the speed of a steam engine. The mechanism consists of two balls connected to a rotating shaft. As the shaft rotates, the balls move outwards due to centrifugal force, which in turn moves a lever that regulates the steam flow and thus the engine speed. The mechanism is a kinematic loop with a prismatic joint and three revolute joints per arm. +The centrifugal governor is a device used to regulate the speed of a steam engine. The mechanism consists of two balls connected to a rotating shaft. As the shaft rotates, the balls move outwards due to centrifugal force, which in turn moves a lever that regulates the steam flow and thus the engine speed. The mechanism is a kinematic loop with a prismatic joint and three revolute joints per arm. The whole mechanism is allowed to rotate around its center axis by means of yet another revolute joint. ```@example kinloop +import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica W(;kwargs...) = Multibody.world -arm_r = 0.03 +arm_r = 0.02 +r = 0.02 +l = 0.05 @mtkmodel GovernorArm begin # @structural_parameters begin # end @components begin - j1 = Revolute(n = [0, 0, 1], phi0 = deg2rad(10), isroot=true, radius=1.1arm_r) + j1 = Revolute(n = [0, 0, 1], phi0 = deg2rad(10), isroot=true, radius=1.1arm_r, state_priority=10) # j2 = Revolute(n = [0, 0, 1], isroot=true, state_priority=99) j2 = RevolutePlanarLoopConstraint(n = [0, 0, 1], radius=1.1arm_r) j3 = Revolute(n = [0, 0, 1], isroot=false, radius=1.1arm_r) # j3 = RevolutePlanarLoopConstraint(n = [0, 0, 1], radius=1.1arm_r) - # todo: add heavy ball - b1 = BodyShape(r = [0.1, 0, 0], radius=arm_r) - b2 = BodyShape(r = [-0.1, 0, 0], radius=arm_r) + b1 = BodyShape(r = [0.1, -0.01, 0], radius=arm_r) + b2 = BodyShape(r = [-0.1, -0.01, 0], radius=arm_r) + # ball = Body(m=0.1, radius=1.5arm_r) frame_a = Frame() frame_b = Frame() end @@ -174,36 +177,36 @@ arm_r = 0.03 connect(frame_a, j1.frame_a) connect(j1.frame_b, b1.frame_a) connect(b1.frame_b, j2.frame_a) - connect(j2.frame_b, b2.frame_a) + connect(j2.frame_b, b2.frame_a)#, ball.frame_a) connect(b2.frame_b, j3.frame_a) connect(j3.frame_b, frame_b) end end -r = 0.02 -l = 0.05 @mtkmodel Governor begin @components begin world = W() arm1 = GovernorArm() # arm2 = GovernorArm() cylinder = BodyCylinder(r = [0, -l, 0], diameter = 2r, inner_diameter = 0.03) - mount1 = FixedTranslation(r = [-r, -l/2, 0]) - # mount2 = FixedTranslation(r = [r, -l/2, 0]) - rev = Revolute(n = [0, 1, 0], w0 = 30, radius=1.01arm_r, state_priority=100) - prismatic = Prismatic(n = [0, -1, 0], s0 = 0.1, radius=0.8arm_r, state_priority=101) + mount1 = FixedTranslation(r = [r, -l/2, 0]) + # mount2 = FixedTranslation(r = [-r, -l/2, 0]) + # rev = Revolute(n = [0, 1, 0], w0 = 30, radius=1.01arm_r, state_priority=101) + prismatic = Prismatic(n = [0, -1, 0], s0 = 0.1, radius=0.8arm_r, state_priority=100, axisflange=true) + damper = TranslationalModelica.Damper(d=500) end @equations begin - connect(world.frame_b, rev.frame_a) + # connect(world.frame_b, rev.frame_a) # connect(arm1.frame_a, rev.frame_b, arm2.frame_a, prismatic.frame_a) # connect(arm1.frame_b, mount1.frame_b) # connect(arm2.frame_b, mount2.frame_b) # connect(mount1.frame_a, cylinder.frame_a, mount2.frame_a, prismatic.frame_b) + connect(prismatic.axis, damper.flange_a) + connect(prismatic.support, damper.flange_b) - - connect(arm1.frame_a, rev.frame_b, prismatic.frame_a) + connect(arm1.frame_a, world.frame_b, prismatic.frame_a) connect(arm1.frame_b, mount1.frame_b) connect(mount1.frame_a, cylinder.frame_a, prismatic.frame_b) end @@ -216,8 +219,28 @@ ssys = structural_simplify(IRSystem(model)) display([unknowns(ssys) diag(ssys.mass_matrix)]) ## prob = ODEProblem(ssys, [ - model.rev.w => 32.5 -], (0.0, 1.0)) -sol = solve(prob, Rodas5P(autodiff=true), abstol=1e-8, reltol=1e-8)#, initializealg = ShampineCollocationInit()) -plot(sol) + # model.rev.w => 32.5; + model.prismatic.s => 0.07; + model.arm1.j1.phi => 1; + model.arm1.j3.phi => 0.5; + # model.rev.w => 1.5; + model.world.render => true; +], (0.0, 40)) + + +using ModelingToolkit.NonlinearSolve +nlsolve = TrustRegion() + +sol = solve(prob, FBDF(autodiff=true), abstol=1e-8, reltol=1e-8, initializealg = BrownFullBasicInit(; nlsolve)) +# sol = solve(prob, FBDF(autodiff=true), abstol=1e-8, reltol=1e-8, initializealg = ShampineCollocationInit(; nlsolve)) + + +# u = prob.u0 +# du = similar(u) +# for i = 1:length(sol.t) +# u = sol.u[i] +# @show prob.f(du, u, prob.p, 0) +# end + +plot(sol, layout=6) ``` \ No newline at end of file