First working components

Project.toml

@@ -1,15 +1,20 @@
 name = "Multibody"
 uuid = "e1cad5d1-98ef-44f9-a79a-9ca4547f95b9"
-authors = ["Yingbo Ma <[email protected]> and contributors"]
+authors = ["JuliaHub Inc."]
 version = "0.1.0"
 
 [deps]
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
+Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
+SymbolicIR = "f7e81a98-b176-4533-a916-1afd54a23a03"
 
 [compat]
 ModelingToolkit = "8.30"
 ModelingToolkitStandardLibrary = "1.8"
+Rotations = "1.4"
+SymbolicIR = "0.1"
 julia = "1"
 
 [extras]

docs/Project.toml

@@ -1,3 +1,6 @@
 [deps]
 Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"
+ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
+ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
 Multibody = "e1cad5d1-98ef-44f9-a79a-9ca4547f95b9"
+OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"

docs/make.jl

@@ -12,6 +12,9 @@ makedocs(;
                                   edit_link = nothing),
          pages = [
              "Home" => "index.md",
+             "Tutorials" => [
+                 "Hello world: Pendulum" => "examples/pendulum.md",
+             ],
          ])
 
 deploydocs(;

docs/src/examples/pendulum.md

@@ -0,0 +1,83 @@
+# Pendulum---The "Hello World of multi-body dynamics"
+This beginners tutorial will model a pendulum pivoted around the origin in the world frame. The world frame is a constant that lives inside the Multibody module, all multibody models are "grounded" in the same world. To start, we load the required packages
+```@example pendulum
+using ModelingToolkit
+using Multibody
+using OrdinaryDiffEq # Contains the ODE solver we will use
+using Plots
+```
+We then access the world frame and time variable from the Multibody module
+```@example pendulum
+t = Multibody.t
+world = Multibody.world
+```
+
+## Modeling the pendulum
+Our simple pendulum will initially consist of a [`Body`](@ref) and a [`Revolute`](@ref) joint (the pivot joint). We construct these elements by calling their constructors
+```@example pendulum
+@named joint = Revolute(n = [0, 0, 1], isroot = true)
+@named body = Body(; m = 1, isroot = false, r_cm = [0.5, 0, 0])
+nothing # hide
+```
+The `n` argument to [`Revolute`](@ref) denotes the rotational axis of the joint, this vector must have `norm(n) == 1`. We also indicate that the revolute joint is the root of the kinematic tree, i.e., the potential states of the joint will serve as the state variables for the system.
+
+The [`Body`](@ref) is constructed by providing its mass, `m`, and the vector `r_cm` from its first frame, `body.frame_a`, to the center of mass. Since the world by default has the gravity field pointing along the negative ``y`` axis, we place the center of mass along the ``x``-axis to make the pendulum swing back and forth. The body is not selected as the root of the kinematic tree, since we have a joint in this system, but if we had attached the body directly to, e.g., a spring, we could set the body to be the root and avoid having to introduce an "artificial joint".
+
+To connect the components together, we create a vector of connections using the `connect` function. A joint typically has two frames, `frame_a` and `frame_b`. The first frame of the joint is attached to the world frame, and the body is attached to the second joint frame. The order of the connections is not important for ModelingToolkit, but it's good practice to follow some convention, here, we start at the world and progress outwards in the kinematic tree.
+```@example pendulum
+connections = [
+    connect(world.frame_b, joint.frame_a)
+    connect(joint.frame_b, body.frame_a)
+]
+nothing # hide
+```
+
+With all components and connections defined, we can create an `ODESystem` like so:
+```@example pendulum
+@named model = ODESystem(connections, t, systems=[world, joint, body])
+nothing # hide
+```
+The `ODESystem` is the fundamental model type in ModelingToolkit used for multibody-type models.
+
+Before we can simulate the system, we must perform model compilation using [`structural_simplify`](@ref)
+```@example pendulum
+ssys = structural_simplify(model, allow_parameter = false)
+```
+This results in a simplified model with the minimum required variables and equations to be able to simulate the system efficiently. This step rewrites all `connect` statements into the appropriate equations, and removes any redundant variables and equations.
+
+We are now ready to create an `ODEProblem` and simulate it. We use the `Rodas4` solver from OrdinaryDiffEq.jl, and pass a dictionary for the initial conditions. We specify only initial condition for some variables, for those variables where no initial condition is specified, the default initial condition defined the model will be used.
+```@example pendulum
+D = Differential(t)
+defs = Dict(D(joint.phi) => 0, D(D(joint.phi)) => 0)
+prob = ODEProblem(ssys, defs, (0, 10))
+
+sol = solve(prob, Rodas4())
+plot(sol, idxs = joint.phi)
+```
+The solution `sol` can be plotted directly if the Plots package is loaded. The figure indicates that the pendulum swings back and forth without any damping. To add damping as well, we could add a `Damper` from the `ModelingToolkitStandardLibrary.Mechanical.Rotational` module to the revolute joint. We do this below
+
+## Adding damping
+To add damping to the pendulum such that the pendulum will eventually come to rest, we add a [`Damper`](@ref) to the revolute joint. The damping coefficient is given by `d`, and the damping force is proportional to the angular velocity of the joint. To add the damper to the revolute joint, we must create the joint with the keyword argument `useAxisFlange = true`, this adds two internal flanges to the joint to which you can attach components from the `ModelingToolkitStandardLibrary.Mechanical.Rotational` module. We then connect one of the flanges of the damper to the axis flange of the joint, and the other damper flange to the support flange which is rigidly attached to the world.
+```@example pendulum
+using ModelingToolkitStandardLibrary.Mechanical.Rotational
+
+@named damper = Damper(d = 0.1)
+@named joint = Revolute(n = [0, 0, 1], isroot = true, useAxisFlange = true)
+
+connections = [connect(world.frame_b, joint.frame_a)
+               connect(damper.flange_b, joint.axis)
+               connect(joint.support, damper.flange_a)
+               connect(body.frame_a, joint.frame_b)]
+
+@named model = ODESystem(connections, t, systems = [world, joint, body, damper])
+ssys = structural_simplify(model, allow_parameter = false)
+
+
+prob = ODEProblem(ssys, [damper.phi_rel => 1, D(joint.phi) => 0, D(D(joint.phi)) => 0],
+                  (0, 30))
+
+
+sol = solve(prob, Rodas4())
+plot(sol, idxs = collect(joint.phi))
+```
+This time we see that the pendulum loses energy and eventually comes to rest at the stable equilibrium point ``\pi / 2``.

src/Multibody.jl

@@ -1,11 +1,40 @@
 module Multibody
 
+using LinearAlgebra
 using ModelingToolkit
+import ModelingToolkitStandardLibrary.Mechanical.Rotational
+
 const t = let
     (@variables t)[1]
 end
 const D = Differential(t)
 
+"""
+    at_variables_t(args)
+
+Emulates the `@variables` macro but does never creates array variables. Also never introuces the variable names into the calling scope.
+"""
+function at_variables_t(args...; default = nothing)
+    xs = Symbolics.variables(args...; T = Symbolics.FnType)
+    xs = map(x -> x(t), xs)
+    if default !== nothing
+        xs = Symbolics.setdefaultval.(xs, default)
+    end
+    xs
+end
+
+export Orientation, RotationMatrix
+include("orientation.jl")
+
+export Frame
 include("frames.jl")
 
+include("interfaces.jl")
+
+export World, world, FixedTranslation, Revolute, Body
+include("components.jl")
+
+export Spring
+include("forces.jl")
+
 end