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, i.e., the world component must be included in all models.

Pendulum

To start, we load the required packages

using ModelingToolkit
using Multibody, JuliaSimCompiler
using OrdinaryDiffEq # Contains the ODE solver we will use
using Plots

We then access the world frame and time variable from the Multibody module

t = Multibody.t
world = Multibody.world
show(stdout, MIME"text/plain"(), world)
Model world with 12 (18) equations
States (9):
  (frame_b₊r_0(t))[1] [defaults to 0.0]: Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame
  (frame_b₊r_0(t))[2] [defaults to 0.0]: Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame
  (frame_b₊r_0(t))[3] [defaults to 0.0]: Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame
  (frame_b₊f(t))[1] [defaults to 0.0]: Cut force resolved in connector frame
  (frame_b₊f(t))[2] [defaults to 0.0]: Cut force resolved in connector frame
  (frame_b₊f(t))[3] [defaults to 0.0]: Cut force resolved in connector frame
  (frame_b₊tau(t))[1] [defaults to 0.0]: Cut torque resolved in connector frame
  (frame_b₊tau(t))[2] [defaults to 0.0]: Cut torque resolved in connector frame
  (frame_b₊tau(t))[3] [defaults to 0.0]: Cut torque resolved in connector frame
Parameters (4):
  n[1] [defaults to 0]: gravity direction of world
  n[2] [defaults to -1]: gravity direction of world
  n[3] [defaults to 0]: gravity direction of world
  g [defaults to 9.81]: gravitational acceleration of world

Unless otherwise specified, the world defaults to have a gravitational field pointing in the negative $y$ direction and a gravitational acceleration of $9.81$.

Modeling the pendulum

Our simple pendulum will initially consist of a Body (point mass) and a Revolute joint (the pivot joint). We construct these elements by calling their constructors

@named joint = Revolute(n = [0, 0, 1], isroot = true)
@named body = Body(; m = 1, isroot = false, r_cm = [0.5, 0, 0])

The n argument to Revolute 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 state of the joint will serve as the state variables for the system.

The Body is constructed by providing its mass, m, and the vector r_cm from its mounting 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", which is otherwise needed in order to have at least one component that has a potential state.

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. In this example, the first frame of the joint is attached to the world frame, and the body is attached to the second frame of the joint, i.e., the joint allows the body to swing back and forth. 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.

connections = [
    connect(world.frame_b, joint.frame_a)
    connect(joint.frame_b, body.frame_a)
]

With all components and connections defined, we can create an ODESystem like so:

@named model = ODESystem(connections, t, systems=[world, joint, body])

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

ssys = structural_simplify(model, allow_parameter = false)

\[ \begin{align} \frac{\mathrm{d} joint_{+}phi\left( t \right)}{\mathrm{d}t} =& joint_{+}w\left( t \right) \\ \frac{\mathrm{d} joint_{+}w\left( t \right)}{\mathrm{d}t} =& joint_{+}phiˍtt\left( t \right) \\ 0 =& - joint_{+}tau\left( t \right) - joint_{+}frame_{b_{+}tau(t)_1} joint_{+}n_1 - joint_{+}frame_{b_{+}tau(t)_2} joint_{+}n_2 - joint_{+}frame_{b_{+}tau(t)_3} joint_{+}n_3 \end{align} \]

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.

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, title="Pendulum")
Example block output

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

3D Animation

Multibody.jl supports automatic 3D rendering of mechanisms, we use this feature to illustrate the result of the simulation below:

import CairoMakie # GLMakie is another alternative, suitable for interactive plots
Multibody.render(model, sol; z = -3, filename = "pendulum.gif") # Use "pendulum.mp4" for a video file

animation

By default, the world frame is indicated using the convention x: red, y: green, z: blue. The animation shows how the simple Body represents a point mass at a particular distance r_cm away from its mounting flange frame_a. To model a more physically motivated pendulum rod, we could have used a BodyShape component, which has two mounting flanges instead of one. The BodyShape component is shown in several of the examples available in the example sections of the documentation.

Adding damping

To add damping to the pendulum such that the pendulum will eventually come to rest, we add a Damper 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 (1-dimensional components). 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.

@named damper = Rotational.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 = joint.phi, title="Damped pendulum")
Example block output

This time we see that the pendulum loses energy and eventually comes to rest at the stable equilibrium point $\pi / 2$.

Multibody.render(model, sol; z = -3, filename = "pendulum_damped.gif")

animation damped

A linear pendulum?

When we think of a pendulum, we typically think of a rotary pendulum that is rotating around a pivot point like in the examples above. A mass suspended in a spring can be though of as a linear pendulum (often referred to as a harmonic oscillator rather than a pendulum), and we show here how we can construct a model of such a device. This time around, we make use of a Prismatic joint rather than a Revolute joint. A prismatic joint has one positional degree of freedom, compared to the single rotational degree of freedom for the revolute joint.

Spring with mass

@named body_0 = Body(; m = 1, isroot = false, r_cm = [0, 0, 0])
@named damper = Translational.Damper(d=1)
@named spring = Translational.Spring(c=10)
@named joint = Prismatic(n = [0, 1, 0], useAxisFlange = true)

connections = [connect(world.frame_b, joint.frame_a)
               connect(damper.flange_b, spring.flange_b, joint.axis)
               connect(joint.support, damper.flange_a, spring.flange_a)
               connect(body_0.frame_a, joint.frame_b)]

@named model = ODESystem(connections, t, systems = [world, joint, body_0, damper, spring])
ssys = structural_simplify(IRSystem(model))

prob = ODEProblem(ssys, [damper.s_rel => 1, D(D(joint.s)) => 0], (0, 30))

sol = solve(prob, Rodas4())
Plots.plot(sol, idxs = joint.s, title="Mass-spring-damper system")
Example block output

As is hopefully evident from the little code snippet above, this linear pendulum model has a lot in common with the rotary pendulum. In this example, we connected both the spring and a damper to the same axis flange in the joint. This time, the components came from the Translational submodule of ModelingToolkitStandardLibrary rather than the Rotational submodule. Also here do we pass useAxisFlange when we create the joint to make sure that it is equipped with the flanges support and axis needed to connect the translational components.

Multibody.render(model, sol; z = -5, filename = "linear_pend.gif", framerate=24)

linear pendulum

Why do we need a joint?

In the example above, we introduced a prismatic joint to model the oscillating motion of the mass-spring system. In reality, we can suspend a mass in a spring without any joint, so why do we need one here? The answer is that we do not, in fact, need the joint, but if we connect the spring directly to the world, we need to make the body (mass) the root object of the kinematic tree instead:

@named root_body = Body(; m = 1, isroot = true, r_cm = [0, 1, 0], phi0 = [0, 1, 0])
@named multibody_spring = Multibody.Spring(c=10)

connections = [connect(world.frame_b, multibody_spring.frame_a)
                connect(root_body.frame_a, multibody_spring.frame_b)]

@named model = ODESystem(connections, t, systems = [world, multibody_spring, root_body])
ssys = structural_simplify(IRSystem(model))

defs = Dict(collect(multibody_spring.r_rel_0 .=> [0, 1, 0])...,
            collect(root_body.r_0 .=> [0, 0, 0])...,
            collect((D.(root_body.phi)) .=> [0, 0, 0])...,
            collect(D.(D.(root_body.phi)) .=> [0, 0, 0])...,
            collect(D.(root_body.phid) .=> [0, 0, 0])...,
)

prob = ODEProblem(ssys, defs, (0, 30))

sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-5 .* randn.())
plot(sol, idxs = multibody_spring.r_rel_0[2], title="Mass-spring system without joint")
Example block output

Here, we used a Multibody.Spring instead of connecting a Translational.Spring to a joint. The Translational.Spring, alongside other components from ModelingToolkitStandardLibrary.Mechanical, is a 1-dimensional object, whereas multibody components are 3-dimensional objects.

Internally, the Multibody.Spring contains a Translational.Spring, attached between two flanges, so we could actually add a damper to the system as well:

push!(connections, connect(multibody_spring.spring2d.flange_a, damper.flange_a))
push!(connections, connect(multibody_spring.spring2d.flange_b, damper.flange_b))

@named model = ODESystem(connections, t, systems = [world, multibody_spring, root_body, damper])
ssys = structural_simplify(IRSystem(model))
prob = ODEProblem(ssys, defs, (0, 30))

sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-5 .* randn.())
plot(sol, idxs = multibody_spring.r_rel_0[2], title="Mass-spring-damper without joint")
Example block output

The figure above should look identical to the simulation of the mass-spring-damper further above.

Going 3D

The systems we have modeled so far have all been planar mechanisms. We now extend this to a 3-dimensional system, the Furuta pendulum.

This pendulum, sometimes referred to as a rotary pendulum, has two joints, one in the "shoulder", which is typically configured to rotate around the gravitational axis, and one in the "elbow", which is typically configured to rotate around the axis of the upper arm. The upper arm is attached to the shoulder joint, and the lower arm is attached to the elbow joint. The tip of the pendulum is attached to the lower arm.

using ModelingToolkit, Multibody, JuliaSimCompiler, OrdinaryDiffEq, Plots
import ModelingToolkitStandardLibrary.Mechanical.Rotational.Damper as RDamper
import Multibody.Rotations
W(args...; kwargs...) = Multibody.world

@mtkmodel FurutaPendulum begin
    @components begin
        world = W()
        shoulder_joint = Revolute(n = [0, 1, 0], isroot = true, useAxisFlange = true)
        elbow_joint    = Revolute(n = [0, 0, 1], isroot = true, useAxisFlange = true, phi0=0.1)
        upper_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0, 0.6], radius=0.05)
        lower_arm = BodyShape(; m = 0.1, isroot = false, r = [0, 0.6, 0], radius=0.05)
        tip = Body(; m = 0.3, isroot = false)

        damper1 = RDamper(d = 0.07)
        damper2 = RDamper(d = 0.07)
    end
    @equations begin
        connect(world.frame_b, shoulder_joint.frame_a)
        connect(shoulder_joint.frame_b, upper_arm.frame_a)
        connect(upper_arm.frame_b, elbow_joint.frame_a)
        connect(elbow_joint.frame_b, lower_arm.frame_a)
        connect(lower_arm.frame_b, tip.frame_a)

        connect(shoulder_joint.axis, damper1.flange_a)
        connect(shoulder_joint.support, damper1.flange_b)

        connect(elbow_joint.axis, damper2.flange_a)
        connect(elbow_joint.support, damper2.flange_b)

    end
end

@named model = FurutaPendulum()
model = complete(model)
ssys = structural_simplify(IRSystem(model))

prob = ODEProblem(ssys, [model.shoulder_joint.phi => 0.0, model.elbow_joint.phi => 0.1], (0, 12))
sol = solve(prob, Rodas4())
plot(sol, layout=4)
Example block output
import CairoMakie
Multibody.render(model, sol, z=-3, R = Rotations.RotXYZ(0.2, -0.2, 0), filename = "furuta.gif")

furuta

Orientations and directions

Let's break down how to think about directions and orientations when building 3D mechanisms. In the example above, we started with the shoulder joint, this joint rotated around the gravitational axis, n = [0, 1, 0]. When this joint is positioned in joint coordinate shoulder_joint.phi = 0, its frame_a and frame_b will coincide. When the joint rotates, frame_b will rotate around the axis n of frame_a. The frame_a of the joint is attached to the world, so the joint will rotate around the world's y-axis:

function get_R(frame, t)
    reshape(sol(t, idxs=vec(ori(frame).R.mat')), 3, 3)
end
function get_r(frame, t)
    sol(t, idxs=collect(frame.r_0))
end
get_R(model.shoulder_joint.frame_b, 0)
3×3 reshape(::StaticArraysCore.SizedVector{9, Float64, Vector{Float64}}, 3, 3) with eltype Float64:
 1.0  0.0  0.0
 0.0  1.0  0.0
 0.0  0.0  1.0

we see that at time $t = 0$, we have no rotation of frame_b around the $y$ axis of the world (frames are always resolved in the world frame), but a second into the simulation, we have:

get_R(model.shoulder_joint.frame_b, 1)
3×3 reshape(::StaticArraysCore.SizedVector{9, Float64, Vector{Float64}}, 3, 3) with eltype Float64:
 0.881221  0.0  -0.472705
 0.0       1.0   0.0
 0.472705  0.0   0.881221

Here, the frame_b has rotated around the $y$ axis of the world.

The next body is the upper arm. This body has an extent of 0.6 in the $z$ direction, as measured in its local frame_a

get_r(model.upper_arm.frame_b, 0)
3-element Vector{Float64}:
 0.0
 0.0
 0.6

One second into the simulation, the upper arm has rotated around the $y$ axis of the world

get_r(model.upper_arm.frame_b, 1)
3-element Vector{Float64}:
 -0.28362313604333333
  0.0
  0.5287323677447265

If we look at the variable model.upper_arm.r, we do not see this rotation!

arm_r = sol(1, idxs=collect(model.upper_arm.r))
3-element Vector{Float64}:
 0.0
 0.0
 0.6

The reason is that this variable is resolved in the local frame_a and not in the world frame. To transform this variable to the world frame, we may multiply with the rotation matrix of frame_a which is always resolved in the world frame:

get_R(model.upper_arm.frame_a, 1)*arm_r
3-element Vector{Float64}:
 -0.28362313604333333
  0.0
  0.5287323677447265

We now get the same result has when we asked for the translation vector of frame_b above.

Slightly more formally, let $R_A^B$ denote the rotation matrix that rotates a vector expressed in a frame $A$ into one that is expressed in frame $B$, i.e., $r_B = R_B^A r_A$. We have then just performed the transformation $r_W = R_W^A r_A$, where $W$ denotes the world frame, and $A$ denotes body.frame_a.

The next joint, the elbow joint, has the rotational axis n = [0, 0, 1]. This indicates that the joint rotates around the $z$-axis of its frame_a. Since the upper arm was oriented along the $z$ direction, the joint is rotating around the axis that coincides with the upper arm.

The lower arm is finally having an extent along the $y$-axis. At the final time when the pendulum motion has been fully damped, we see that the second frame of this body ends up with an $y$-coordinate of -0.6:

get_r(model.lower_arm.frame_b, 12)
3-element Vector{Float64}:
 -0.00904034738980951
 -0.5999999672725873
  0.5999319222978268

If we rotate the vector of extent of the lower arm to the world frame, we indeed see that the only coordinate that is nonzero is the $y$ coordinate:

get_R(model.lower_arm.frame_a, 12)*sol(12, idxs=collect(model.lower_arm.r))
3-element Vector{Float64}:
 -0.00019815237782794845
 -0.5999999672725873
 -2.920487096080702e-6

The reason that the latter vector differs from get_r(model.lower_arm.frame_b, 12) above is that get_r(model.lower_arm.frame_b, 12) has been translated as well. To both translate and rotate model.lower_arm.r into the world frame, we must use the full transformation matrix $T_W^A \in SE(3)$:

function get_T(frame, t)
    R = get_R(frame, t)
    r = get_r(frame, t)
    [R r; 0 0 0 1]
end

r_A = sol(12, idxs=collect(model.lower_arm.r))
r_A = [r_A; 1] # Homogeneous coordinates

get_T(model.lower_arm.frame_a, 12)*r_A
4-element Vector{Float64}:
 -0.00904034738980951
 -0.5999999672725873
  0.5999319222978268
  1.0

the vector is now coinciding with get_r(model.lower_arm.frame_b, 12).