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

Author: baggepinnen

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

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

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)

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")

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))
+