From 3bf6cf6d803c7c8cc91ef55b440581e86a3130a6 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Thu, 3 Oct 2024 03:15:50 +0200 Subject: [PATCH] make it possible to change `world.n` (#166) * make it possible to change `world.n` * add test file * tweak tutorial * fix some instances of double worlds --- docs/src/examples/pendulum.md | 25 +++++++++++------ docs/src/examples/suspension.md | 20 ++++++++------ src/components.jl | 25 +++++++++++++++-- test/runtests.jl | 5 ++++ test/test_world.jl | 49 +++++++++++++++++++++++++++++++++ 5 files changed, 104 insertions(+), 20 deletions(-) create mode 100644 test/test_world.jl diff --git a/docs/src/examples/pendulum.md b/docs/src/examples/pendulum.md index 658a136d..484bf834 100644 --- a/docs/src/examples/pendulum.md +++ b/docs/src/examples/pendulum.md @@ -326,11 +326,18 @@ We start by putting the model together and control it in open loop using a simpl import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica import ModelingToolkitStandardLibrary.Blocks using Plots -W(args...; kwargs...) = Multibody.world gray = [0.5, 0.5, 0.5, 1] @mtkmodel Cartpole begin + @structural_parameters begin + use_world = false + end @components begin - world = W() + if use_world + fixed = World() + else + # In case we wrap this model in an outer model below, we place the world there instead + fixed = Fixed() + end cart = BodyShape(m = 1, r = [0.2, 0, 0], color=[0.2, 0.2, 0.2, 1], shape="box") mounting_point = FixedTranslation(r = [0.1, 0, 0]) prismatic = Prismatic(n = [1, 0, 0], axisflange = true, color=gray, state_priority=100) @@ -347,7 +354,7 @@ gray = [0.5, 0.5, 0.5, 1] w(t) end @equations begin - connect(world.frame_b, prismatic.frame_a) + connect(fixed.frame_b, prismatic.frame_a) connect(prismatic.frame_b, cart.frame_a, mounting_point.frame_a) connect(mounting_point.frame_b, revolute.frame_a) connect(revolute.frame_b, pendulum.frame_a) @@ -363,7 +370,7 @@ gray = [0.5, 0.5, 0.5, 1] end @mtkmodel CartWithInput begin @components begin - world = W() + world = World() cartpole = Cartpole() input = Blocks.Cosine(frequency=1, amplitude=1) end @@ -388,14 +395,14 @@ nothing # hide ### Adding feedback -We will attempt to stabilize the pendulum in the upright position by using feedback control. To design the contorller, we linearize the model in the upward equilibrium position and design an infinite-horizon LQR controller using ControlSystems.jl. We then connect the controller to the motor on the cart. See also [RobustAndOptimalControl.jl: Control design for a pendulum on a cart](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/cartpole/) for a similar example with more detail on the control design. +We will attempt to stabilize the pendulum in the upright position by using feedback control. To design the controller, we linearize the model in the upward equilibrium position and design an infinite-horizon LQR controller using ControlSystems.jl. We then connect the controller to the motor on the cart. See also [RobustAndOptimalControl.jl: Control design for a pendulum on a cart](https://juliacontrol.github.io/RobustAndOptimalControl.jl/dev/cartpole/) for a similar example with more detail on the control design. ### Linearization We start by linearizing the model in the upward equilibrium position using the function `ModelingToolkit.linearize`. ```@example pendulum import ModelingToolkit: D_nounits as D using LinearAlgebra -@named cp = Cartpole() +@named cp = Cartpole(use_world = true) cp = complete(cp) inputs = [cp.u] # Input to the linearized system outputs = [cp.x, cp.phi, cp.v, cp.w] # These are the outputs of the linearized system @@ -437,7 +444,7 @@ LQGSystem(args...; kwargs...) = ODESystem(observer_controller(lqg); kwargs...) @mtkmodel CartWithFeedback begin @components begin - world = W() + world = World() cartpole = Cartpole() reference = Blocks.Step(start_time = 5, height=0.5) control_saturation = Blocks.Limiter(y_max = 10) # To limit the control signal magnitude @@ -479,7 +486,7 @@ Below, we add also an energy-based swing-up controller. For more details this ki ```@example pendulum "Compute total energy, kinetic + potential, for a body rotating around the z-axis of the world" function energy(body, w) - g = world.g + g = GlobalScope(world.g_inner) m = body.m d2 = body.r_cm[1]^2 + body.r_cm[2]^2 # Squared distance from I = body.I_33 + m*d2 # Parallel axis theorem @@ -491,7 +498,7 @@ normalize_angle(x::Number) = mod(x+3.1415, 2pi)-3.1415 @mtkmodel CartWithSwingup begin @components begin - world = W() + world = World() cartpole = Cartpole() L = Blocks.MatrixGain(K = Lmat) # Here we use the LQR controller instead control_saturation = Blocks.Limiter(y_max = 12) # To limit the control signal magnitude diff --git a/docs/src/examples/suspension.md b/docs/src/examples/suspension.md index c374898d..d39b66c2 100644 --- a/docs/src/examples/suspension.md +++ b/docs/src/examples/suspension.md @@ -101,7 +101,7 @@ mirror = false dir = mirror ? -1 : 1 end @components begin - world = W() + fixed = Fixed() chassis_frame = Frame() suspension = QuarterCarSuspension(; spring=true, mirror, rod_radius) @@ -115,7 +115,7 @@ mirror = false wheel_position.s_ref.u ~ amplitude*(sin(2pi*freq*t)) # Displacement of wheel connect(wheel_position.flange, wheel_prismatic.axis) - connect(world.frame_b, actuation_position.frame_a) + connect(fixed.frame_b, actuation_position.frame_a) connect(actuation_position.frame_b, wheel_prismatic.frame_a) connect(wheel_prismatic.frame_b, actuation_rod.frame_a,) connect(actuation_rod.frame_b, suspension.r123.frame_ib) @@ -200,7 +200,7 @@ In the example below, we extend the previous example to a half-car model with tw rod_radius = 0.02 end @components begin - world = W() + world = World() mass = BodyShape(m=ms, r = [0,0,-wheel_base], radius=0.1, color=[0.4, 0.4, 0.4, 0.3]) excited_suspension_r = SuspensionWithExcitation(; suspension.spring=true, mirror=false, rod_radius, actuation_position.r = [0, 0, (CD+wheel_base/2)], @@ -329,7 +329,7 @@ end rod_radius = 0.02 end @components begin - world = W() + world = World() mass = Body(m=ms, r_cm = 0.5DA*normalize([0, 0.2, 0.2*sin(t5)])) excited_suspension = ExcitedWheelAssembly(; rod_radius) body_upright = Prismatic(n = [0, 1, 0], render = false, state_priority=1000) @@ -375,7 +375,7 @@ nothing # hide rod_radius = 0.02 end @components begin - world = W() + world = World() mass = BodyShape(m=ms, r = [0,0,-wheel_base], radius=0.1, color=[0.4, 0.4, 0.4, 0.3]) excited_suspension_r = ExcitedWheelAssembly(; mirror=false, rod_radius) excited_suspension_l = ExcitedWheelAssembly(; mirror=true, rod_radius) @@ -451,9 +451,9 @@ transparent_gray = [0.4, 0.4, 0.4, 0.3] rod_radius = 0.02 end @components begin - world = W() + world = World() front_axle = BodyShape(m=ms/4, r = [0,0,-wheel_base], radius=0.1, color=transparent_gray) - back_front = BodyShape(m=ms/2, r = [2, 0, 0], radius=0.2, color=transparent_gray, isroot=true, state_priority=Inf, quat=false) + back_front = BodyShape(m=ms/2, r = [-2, 0, 0], radius=0.2, color=transparent_gray, isroot=true, state_priority=Inf, quat=false) back_axle = BodyShape(m=ms/4, r = [0,0,-wheel_base], radius=0.1, color=transparent_gray) excited_suspension_fr = ExcitedWheelAssembly(; mirror=false, rod_radius, freq = 10) @@ -508,9 +508,13 @@ defs = [ model.excited_suspension_fl.suspension.cs => 5*4000 model.excited_suspension_fl.suspension.r2.phi => -0.6031 + model.excited_suspension_fr.wheel.frame_a.render => true # To visualize one wheel rolling + model.excited_suspension_fr.wheel.frame_a.radius => 0.01 + model.excited_suspension_fr.wheel.frame_a.length => 0.3 + model.ms => 1500 - model.back_front.body.r_0[1] => -2.0 + model.back_front.body.r_0[1] => 0 model.back_front.body.r_0[2] => 0.193 model.back_front.body.r_0[3] => 0.0 model.back_front.body.v_0[1] => 1 diff --git a/src/components.jl b/src/components.jl index c4c2a2fc..65f761e9 100644 --- a/src/components.jl +++ b/src/components.jl @@ -47,18 +47,37 @@ end g0 = g mu0 = mu @named frame_b = Frame() - @parameters n[1:3]=n0 [description = "gravity direction of world"] + + @parameters n[1:3] = n0 [description = "gravity direction"] @parameters g=g0 [description = "gravitational acceleration of world"] @parameters mu=mu0 [description = "Gravity field constant [m³/s²] (default = field constant of earth)"] @parameters render=render @parameters point_gravity = point_gravity + + @variables n_inner(t)[1:3] + @variables g_inner(t) + @variables mu_inner(t) + @variables render_inner(t) + @variables point_gravity_inner(t) + n = Symbolics.scalarize(n) + n_inner = GlobalScope.(Symbolics.scalarize(n_inner)) + g_inner = GlobalScope(g_inner) + mu_inner = GlobalScope(mu_inner) + render_inner = GlobalScope(render_inner) + point_gravity_inner = GlobalScope(point_gravity_inner) + O = ori(frame_b) eqs = Equation[ collect(frame_b.r_0) .~ 0; O ~ nullrotation() + n_inner .~ n + g_inner ~ g + mu_inner ~ mu + render_inner ~ render + point_gravity_inner ~ point_gravity ] - ODESystem(eqs, t, [], [n; g; mu; point_gravity; render]; name, systems = [frame_b])#, defaults=[n => n0; g => g0; mu => mu0]) + ODESystem(eqs, t, [n_inner; g_inner; mu_inner; render_inner; point_gravity_inner], [n; g; mu; point_gravity; render]; name, systems = [frame_b])#, defaults=[n => n0; g => g0; mu => mu0]) end """ @@ -68,7 +87,7 @@ const world = World(; name = :world) "Compute the gravity acceleration, resolved in world frame" function gravity_acceleration(r) - inner_gravity(GlobalScope(world.point_gravity), GlobalScope(world.mu), GlobalScope(world.g), GlobalScope.(collect(world.n)), collect(r)) + inner_gravity(GlobalScope(world.point_gravity), GlobalScope(world.mu), GlobalScope(world.g_inner), GlobalScope.(collect(world.n_inner)), collect(r)) end function inner_gravity(point_gravity, mu, g, n, r) diff --git a/test/runtests.jl b/test/runtests.jl index 1644d77c..eeadd19d 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -26,6 +26,11 @@ ssys = structural_simplify(model) @test length(unknowns(ssys)) == 0 # This example is completely rigid and should simplify down to zero state variables +@testset "world" begin + @info "Testing world" + include("test_world.jl") +end + @testset "urdf" begin @info "Testing urdf" include("test_urdf.jl") diff --git a/test/test_world.jl b/test/test_world.jl new file mode 100644 index 00000000..13c28231 --- /dev/null +++ b/test/test_world.jl @@ -0,0 +1,49 @@ +using Multibody, ModelingToolkit, JuliaSimCompiler, Test +@mtkmodel FallingBody begin + @components begin + my_world = World(g = 1, n = [0, 1, 0]) + body = Body(isroot=true) + end +end + +@named model = FallingBody() +model = complete(model) +ssys = structural_simplify(IRSystem(model)) +prob = ODEProblem(ssys, [], (0, 1)) +sol = solve(prob, Tsit5()) + +tv = 0:0.1:1 +@test iszero(sol(tv, idxs=model.body.r_0[1])) +@test sol(tv, idxs=model.body.r_0[2]) ≈ tv.^2 ./ 2 atol=1e-6 +@test iszero(sol(tv, idxs=model.body.r_0[3])) + + +# Change g +prob = ODEProblem(ssys, [model.my_world.g .=> 2], (0, 1)) +sol = solve(prob, Tsit5()) +@test iszero(sol(tv, idxs=model.body.r_0[1])) +@test sol(tv, idxs=model.body.r_0[2]) ≈ 2*tv.^2 ./ 2 atol=1e-6 +@test iszero(sol(tv, idxs=model.body.r_0[3])) + +# Change n +prob = ODEProblem(ssys, collect(model.my_world.n) .=> [1, 0, 0], (0, 1)) +sol = solve(prob, Tsit5()) +@test sol(tv, idxs=model.body.r_0[1]) ≈ tv.^2 ./ 2 atol=1e-6 +@test iszero(sol(tv, idxs=model.body.r_0[2])) +@test iszero(sol(tv, idxs=model.body.r_0[3])) + + +## World in more than one place +# This should result in too many equations +@mtkmodel FallingBodyOuter begin + @components begin + inner_model = FallingBody() + my_world_outer = World(g = 2, n = [0, 1, 0]) + body = Body(isroot=true) + end +end + +@named model = FallingBodyOuter() +model = complete(model) +@test_throws ModelingToolkit.ExtraEquationsSystemException structural_simplify(IRSystem(model)) +