add point gravity model and space tutorial (#79)

* add point gravity model and space tutorial

* include space in make

Author: baggepinnen

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",
+                 "Bodies in space" => "examples/space.md",
              ],
              "3D rendering" => "rendering.md",
          ])

docs/src/examples/space.md

@@ -0,0 +1,74 @@
+# Bodies in space
+
+The default gravity model in Multibody.jl computes the gravitational acceleration according to
+```math
+a = g n
+```
+where ``g`` and ``n`` are properties of the `world`. The default values for these parameters are `g = 9.81` and `n = [0, -1, 0]`.
+
+This example demonstrates how to use the _point gravity_ model, in which the gravitational acceleration is always pointing towards the origin, with a magnitude determined by ``\mu`` and ``r``
+```math
+-\dfrac{\mu}{r^T r} \dfrac{r}{||r||}
+```
+where ``\mu`` is the gravity field constant (defaults to 3.986004418e14 for Earth) and ``r`` is the distance of a body from the origin.
+
+In this example, we set ``\mu = 1``, `point_gravity = true` and let two masses orbit an invisible attractor at the origin.
+
+```@example SPACE
+using Multibody
+using ModelingToolkit
+using Plots
+using JuliaSimCompiler
+using OrdinaryDiffEq
+
+t = Multibody.t
+D = Differential(t)
+W() = Multibody.world
+
+@mtkmodel PointGrav begin
+    @components begin
+        world = W()
+        body1 = Body(
+            m=1,
+            I_11=0.1,
+            I_22=0.1,
+            I_33=0.1,
+            r_0=[0,0.6,0],
+            isroot=true,
+            v_0=[1,0,0])
+        body2 = Body(
+            m=1,
+            I_11=0.1,
+            I_22=0.1,
+            I_33=0.1,
+            r_0=[0.6,0.6,0],
+            isroot=true,
+            v_0=[0.6,0,0])
+    end
+end
+@named model = PointGrav()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+defs = [
+    model.world.mu => 1
+    model.world.point_gravity => true # The gravity model is selected here
+    collect(model.body1.w_a) .=> 0
+    collect(model.body2.w_a) .=> 0
+    
+]
+prob = ODEProblem(ssys, defs, (0, 5))
+sol = solve(prob, Rodas4())
+plot(sol)
+```
+
+
+```@example SPACE
+import CairoMakie
+Multibody.render(model, sol, filename = "space.gif")
+nothing # hide
+```
+![space](space.gif)
+
+
+## Turning gravity off
+To simulate without gravity, or with a gravity corresponding to that of another celestial body, set the value of ``g`` or ``\mu`` appropriately for the chosen gravity model.

src/components.jl

@@ -38,21 +38,23 @@ end
 """
     World(; name, render=true)
 """
-@component function World(; name, render=true)
+@component function World(; name, render=true, point_gravity=false)
     # World should have
     # 3+3+9+3 // r_0+f+R.R+τ
     # - (3+3) // (f+t)
     # = 12 equations 
     @named frame_b = Frame()
     @parameters n[1:3]=[0, -1, 0] [description = "gravity direction of world"]
     @parameters g=9.81 [description = "gravitational acceleration of world"]
+    @parameters mu=3.986004418e14 [description = "Gravity field constant [m³/s²] (default = field constant of earth)"]
     @parameters render=render
+    @parameters point_gravity = point_gravity
     O = ori(frame_b)
     eqs = Equation[collect(frame_b.r_0) .~ 0;
                    O ~ nullrotation()
                    # vec(D(O).R .~ 0); # QUESTION: not sure if I should have to add this, should only have 12 equations according to modelica paper
                    ]
-    ODESystem(eqs, t, [], [n; g; render]; name, systems = [frame_b])
+    ODESystem(eqs, t, [], [n; g; mu; point_gravity; render]; name, systems = [frame_b])
 end
 
 """
@@ -61,7 +63,17 @@ The world component is the root of all multibody models. It is a fixed frame wit
 const world = World(; name = :world)
 
 "Compute the gravity acceleration, resolved in world frame"
-gravity_acceleration(r) = GlobalScope(world.g) * GlobalScope.(world.n) # NOTE: This is hard coded for now to use the the standard, parallel gravity model
+function gravity_acceleration(r)
+    inner_gravity(GlobalScope(world.point_gravity), GlobalScope(world.mu), GlobalScope(world.g), GlobalScope.(collect(world.n)), collect(r))
+end
+
+function inner_gravity(point_gravity, mu, g, n, r)
+    # This is slightly inefficient, producing three if statements, one for each array entry. The function registration for array-valued does not work properly so this is a workaround for now. Hitting, among other problems, https://github.com/SciML/ModelingToolkit.jl/issues/2808
+    gvp = -(mu/(r'r))*(r/_norm(r))
+    gvu = g * n
+    ifelse.(point_gravity==true, gvp, gvu)
+end
+
 
 @component function Fixed(; name, r = [0, 0, 0])
     systems = @named begin frame_b = Frame() end
@@ -221,6 +233,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
               phi0 = zeros(3),
               phid0 = zeros(3),
               r_0 = 0,
+              v_0 = 0,
               radius = 0.05,
               v_0 = 0,
               w_a = 0,

test/runtests.jl

@@ -147,6 +147,47 @@ sol = solve(prob, Rodas4())
 @test sol(1, idxs=powersensor.power.u) ≈ -1.94758 atol=1e-2
 end
 
+# ==============================================================================
+## Point gravity ======================
+# ==============================================================================
+@mtkmodel PointGrav begin
+    @components begin
+        world = W()
+        body1 = Body(
+            m=1,
+            I_11=0.1,
+            I_22=0.1,
+            I_33=0.1,
+            r_0=[0,0.6,0],
+            isroot=true,
+            v_0=[1,0,0])
+        body2 = Body(
+            m=1,
+            I_11=0.1,
+            I_22=0.1,
+            I_33=0.1,
+            r_0=[0.6,0.6,0],
+            isroot=true,
+            v_0=[0.6,0,0])
+    end
+end
+@named model = PointGrav()
+model = complete(model)
+ssys = structural_simplify(IRSystem(model))
+defs = [
+    model.world.mu => 1
+    model.world.point_gravity => true
+    collect(model.body1.w_a) .=> 0
+    collect(model.body2.w_a) .=> 0
+    
+]
+prob = ODEProblem(ssys, defs, (0, 5))
+sol = solve(prob, Rodas4())
+
+@test sol(5, idxs=model.body2.r_0) ≈ [0.7867717, 0.478463, 0] atol=1e-1
+# plot(sol)
+
+
 # ==============================================================================
 ## Simple pendulum from Modelica "First Example" tutorial ======================
 # ==============================================================================