GitHub

Pendulum–The "Hello World of multi-body dynamics"

This beginners tutorial will start by modeling 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 exactly once in all models, at the top level.

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
Model world with 19 (25) equations
Unknowns (16):
  (n_inner(t))[1]
  (n_inner(t))[2]
  (n_inner(t))[3]
  g_inner(t)
  mu_inner(t)
  render_inner(t)
  point_gravity_inner(t)
  (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]: Cut force resolved in connector frame
  (frame_b₊f(t))[2]: Cut force resolved in connector frame
  (frame_b₊f(t))[3]: Cut force resolved in connector frame
  (frame_b₊tau(t))[1]: Cut torque resolved in connector frame
  (frame_b₊tau(t))[2]: Cut torque resolved in connector frame
  (frame_b₊tau(t))[3]: Cut torque resolved in connector frame
Parameters (10):
  n[1] [defaults to 0.0]: gravity direction
  n[2] [defaults to -1.0]: gravity direction
  n[3] [defaults to 0.0]: gravity direction
  g [defaults to 9.80665]: gravitational acceleration of world
  mu [defaults to 3.986e14]: Gravity field constant [m³/s²] (default = field constant of earth)
  point_gravity [defaults to false]
  render [defaults to true]
  frame_b₊render [defaults to false]
  frame_b₊length [defaults to 1.0]
  frame_b₊radius [defaults to 0.1]

Unless otherwise specified, the world defaults to having a gravitational field pointing in the negative $y$ direction and a gravitational acceleration of $9.81$ (See Bodies in space for more options).

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])
model = complete(model)

The ODESystem is the fundamental model type in ModelingToolkit used for multibody-type models.

Before we can simulate the system, we must perform model check using the function multibody and compilation using structural_simplify

ssys = structural_simplify(multibody(model))
ScheduledSystem with state (2):
 joint₊phi
 joint₊w

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. To simulate the pendulum, we require two state variables, one for angle and one for angular velocity, we can see above that these state variables have indeed been chosen.

Multibody.multibody — Function
multibody(model)

Perform validity checks on the model, such as the precense of exactly one world component in the top level of the model, and transform the model into an IRSystem object for passing into structural_simplify.

source

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() # We may specify the initial condition here
prob = ODEProblem(ssys, defs, (0, 3.35))

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 GLMakie # GLMakie is another alternative, suitable for interactive plots
Multibody.render(model, sol; 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 with inertial properties at a particular distance r_cm away from its mounting flange frame_a. The cylinder that is shown connecting the pivot point to the body is for visualization purposes only, it does not have any inertial properties. 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 axisflange = 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.3)
@named joint = Revolute(n = [0, 0, 1], isroot = true, axisflange = 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])
model = complete(model)
ssys = structural_simplify(multibody(model))

prob = ODEProblem(ssys, [damper.phi_rel => 1], (0, 10))

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; 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.

@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], axisflange = 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])
model = complete(model)
ssys = structural_simplify(multibody(model))

prob = ODEProblem(ssys, [], (0, 10))

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 axisflange 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; 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])
model = complete(model)
ssys = structural_simplify(multibody(model))

defs = Dict(collect(root_body.r_0) .=> [0, 1e-3, 0]) # The spring has a singularity at zero length, so we start some distance away

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

sol = solve(prob, Rodas4())
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])
model = complete(model)
ssys = structural_simplify(multibody(model))
prob = ODEProblem(ssys, defs, (0, 10))

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

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

        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(multibody(model))

prob = ODEProblem(ssys, [model.shoulder_joint.phi => 0.0, model.elbow_joint.phi => 0.1], (0, 10))
sol = solve(prob, Rodas4())
plot(sol, layout=4)
Example block output

In the animation below, we visualize the path that the origin of the pendulum tip traces by providing the tip frame in a vector of frames passed to traces

import GLMakie
Multibody.render(model, sol, filename = "furuta.gif", traces=[model.tip.frame_a])

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:

get_rot(sol, model.shoulder_joint.frame_b, 0)
3×3 RotMatrix3{Float64} with indices SOneTo(3)×SOneTo(3):
 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:

R1 = get_rot(sol, model.shoulder_joint.frame_b, 1)
3×3 RotMatrix3{Float64} with indices SOneTo(3)×SOneTo(3):
 0.739387  0.0  -0.67328
 0.0       1.0   0.0
 0.67328   0.0   0.739387

Here, the frame_b has rotated around the $y$ axis of the world (if you are not familiar with rotation matrices, we can ask for the rotation axis and angle)

using Multibody.Rotations
rotation_axis(R1), rotation_angle(R1)
([6.734077305763844e-63, -1.0, 0.0], 0.7386364712179742)

This rotation axis and angle should correspond to the joint coordinate (the orientation described by an axis and an angle is invariant to a multiplication of both by -1)

sol(1, idxs=model.shoulder_joint.phi)
-0.7386364712179742
Convention

The convention used in get_rot is to return the rotation matrix $R_{World}^{Local}$ that rotates a coordinate from the local frame to the world frame, $r_{World} = R_{World}^{Local} r_{Local}$.

Here, we made use of the function get_rot, we will now make use of also get_trans and get_frame.

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_trans(sol, model.upper_arm.frame_b, 0)
3-element StaticArraysCore.SVector{3, Float64} with indices SOneTo(3):
 0.0
 0.0
 0.6

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

rb1 = get_trans(sol, model.upper_arm.frame_b, 1)
3-element StaticArraysCore.SVector{3, Float64} with indices SOneTo(3):
 -0.4039682171904861
  0.0
  0.44363236976120235

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_rot(sol, model.upper_arm.frame_a, 1)*arm_r
3-element StaticArraysCore.SVector{3, Float64} with indices SOneTo(3):
 -0.4039682171904861
  0.0
  0.44363236976120235

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_trans(sol, model.lower_arm.frame_b, 12)
3-element StaticArraysCore.SVector{3, Float64} with indices SOneTo(3):
 -0.0007983770794975084
 -0.599999999979659
  0.5999994688484717

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_rot(sol, model.lower_arm.frame_a, 12)*sol(12, idxs=collect(model.lower_arm.r))
3-element StaticArraysCore.SVector{3, Float64} with indices SOneTo(3):
  4.940552441510778e-6
 -0.599999999979659
  6.614727408270282e-9

The reason that the latter vector differs from get_trans(sol, model.lower_arm.frame_b, 12) above is that get_trans(sol, 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)$:

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

get_frame(sol, model.lower_arm.frame_a, 12)*r_A
4-element Vector{Float64}:
 -0.0007983770794975084
 -0.599999999979659
  0.5999994688484717
  1.0

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

Control-design example: Pendulum on cart

We will now demonstrate a complete workflow including

  • Modeling
  • Linearization
  • Control design

We will continue the pendulum theme and design an inverted pendulum on cart. The cart is modeled as BodyShape with specified mass, and shape = "box" to render it as a box in animations. The cart is moving along the $x$-axis by means of a Prismatic joint, and the pendulum is attached to the cart by means of a Revolute joint. The pendulum is a BodyCylinder with a diameter of 0.015, the mass and inertia properties are automatically computed using the geometrical dimensions and the density (which defaults to that of steel). A force is applied to the cart by means of a TranslationalModelica.Force component.

We start by putting the model together and control it in open loop using a simple periodic input signal:

import ModelingToolkitStandardLibrary.Mechanical.TranslationalModelica
import ModelingToolkitStandardLibrary.Blocks
using Plots
gray = [0.5, 0.5, 0.5, 1]
@mtkmodel Cartpole begin
    @structural_parameters begin
        use_world = false
    end
    @components begin
        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)
        revolute = Revolute(n = [0, 0, 1], axisflange = false, state_priority=100)
        pendulum = BodyCylinder(r = [0, 0.5, 0], diameter = 0.015, color=gray)
        motor = TranslationalModelica.Force(use_support = true)
        tip = Body(m = 0.05)
    end
    @variables begin
        u(t) = 0
        x(t)
        v(t)
        phi(t)
        w(t)
    end
    @equations begin
        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)
        connect(pendulum.frame_b, tip.frame_a)
        connect(motor.flange, prismatic.axis)
        connect(prismatic.support, motor.support)
        u ~ motor.f.u
        x ~ prismatic.s
        v ~ prismatic.v
        phi ~ revolute.phi
        w ~ revolute.w
    end
end
@mtkmodel CartWithInput begin
    @components begin
        world = World()
        cartpole = Cartpole()
        input = Blocks.Cosine(frequency=1, amplitude=1)
    end
    @equations begin
        connect(input.output, :u, cartpole.motor.f)
    end
end
@named model = CartWithInput()
model = complete(model)
ssys = structural_simplify(multibody(model))
prob = ODEProblem(ssys, [model.cartpole.prismatic.s => 0.0, model.cartpole.revolute.phi => 0.1], (0, 10))
sol = solve(prob, Tsit5())
plot(sol, layout=4)
Example block output

As usual, we render the simulation in 3D to get a better feel for the system:

import GLMakie
Multibody.render(model, sol, filename = "cartpole.gif", traces=[model.cartpole.pendulum.frame_b])

cartpole

Adding feedback

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 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.

import ModelingToolkit: D_nounits as D
using LinearAlgebra
@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
op = Dict([ # Operating point to linearize in
    cp.u => 0
    cp.revolute.phi => 0 # Pendulum pointing upwards
]
)
matrices, simplified_sys = linearize(multibody(cp), inputs, outputs; op)
matrices
(A = [0.0 0.0 0.0 1.0; 0.0 0.0 1.0 0.0; 4.474102070258828 0.0 0.0 0.0; 39.683507165892394 0.0 0.0 0.0], B = [0.0; 0.0; 0.8415814526118872; 2.3385955754360115;;], C = [0.0 1.0 0.0 0.0; 1.0 0.0 0.0 0.0; 0.0 0.0 1.0 0.0; 0.0 0.0 0.0 1.0], D = [0.0; 0.0; 0.0; 0.0;;])

This gives us the matrices $A,B,C,D$ in a linearized statespace representation of the system. To make these easier to work with, we load the control packages and call named_ss instead of linearize to get a named statespace object instead:

using ControlSystemsMTK
lsys = named_ss(multibody(cp), inputs, outputs; op) # identical to linearize, but packages the resulting matrices in a named statespace object for convenience
cp: NamedControlSystemsBase.StateSpace{ControlSystemsBase.Continuous, Float64}
A = 
  0.0                0.0  0.0  1.0
  0.0                0.0  1.0  0.0
  4.474102070258828  0.0  0.0  0.0
 39.683507165892394  0.0  0.0  0.0
B = 
 0.0
 0.0
 0.8415814526118872
 2.3385955754360115
C = 
 0.0  1.0  0.0  0.0
 1.0  0.0  0.0  0.0
 0.0  0.0  1.0  0.0
 0.0  0.0  0.0  1.0
D = 
 0.0
 0.0
 0.0
 0.0

Continuous-time state-space model
With state  names: revolute₊phi prismatic₊s prismatic₊v revolute₊w
     input  names: u(t)
     output names: x(t) phi(t) v(t) w(t)

LQR Control design

With a linear statespace object in hand, we can proceed to design an LQR or LQG controller. We will design both an LQR and an LQG controller in order to demonstrate two possible workflows.

The LQR contorller is designed using the function ControlSystemsBase.lqr, and it takes the two cost matrices Q1 and Q2 penalizing state deviation and control action respectively. The LQG controller is designed using RobustAndOptimalControl.LQGProblem, and this function additionally takes the covariance matrices r1, R2 for a Kalman filter. Before we call LQGProblem we partition the linearized system into an ExtendedStateSpace object using the partition function, this indicates what inputs of the system are available for control and what are considered disturbances, and what outputs of the system are available for measurement. In this case, we assume that we have access to the cart position and the pendulum angle, and we control the cart position. The remaining two outputs are still important for the performance, but we cannot measure them and will rely on the Kalman filter to estimate them. When we call observer_controller we get a linear system that represents the combined state estimator and state feedback controller. This linear system is then converted to an ODESystem by the function LQGSystem.

Since the function lqr operates on the state vector, and we have access to the specified output vector, we make use of the system $C$ matrix to reformulate the problem in terms of the outputs. This relies on the $C$ matrix being full rank, which is the case here since our outputs include a complete state realization of the system. This is of no concern when using the LQGProblem structure since we penalize outputs rather than the state in this case.

To make the simulation interesting, we make a change in the reference position of the cart after a few seconds. 

using ControlSystemsBase, RobustAndOptimalControl
C = lsys.C
Q1 = Diagonal([10, 10, 10, 1])
Q2 = Diagonal([0.1])

R1 = Diagonal([1])
R2 = Diagonal([0.01, 0.01])

lqg = LQGProblem(partition(lsys, u = [:u], y = [:x, :phi]), Q1, Q2, R1, R2)
Lmat = lqr(lsys, C'Q1*C, Q2)/C # Alternatively, compute LQR feedback gain. The multiplication by the C matrix is to handle the difference between state and output

LQGSystem(args...; kwargs...) = ODESystem(observer_controller(lqg); kwargs...)

@mtkmodel CartWithFeedback begin
    @components begin
        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
        # controller = Blocks.MatrixGain(K = Lmat) # uncomment to use LQR controller instead
        controller = LQGSystem()
    end
    begin
        namespaced_outputs = ModelingToolkit.renamespace.(:cartpole, outputs) # Give outputs correct namespace, they are variables in the cartpole system
    end
    @equations begin
        controller.input.u[1] ~ reference.output.u - namespaced_outputs[1] # reference cart position - cartpole.x
        controller.input.u[2] ~ 0 - namespaced_outputs[2] # cartpole.phi
        # controller.input.u[3] ~ 0 - namespaced_outputs[3] # cartpole.v # uncomment if using LQR controller instead
        # controller.input.u[4] ~ 0 - namespaced_outputs[4] # cartpole.w

        connect(controller.output, control_saturation.input)
        connect(control_saturation.output, cartpole.motor.f)
    end
end
@named model = CartWithFeedback()
model = complete(model)
ssys = structural_simplify(multibody(model))
prob = ODEProblem(ssys, [model.cartpole.prismatic.s => 0.1, model.cartpole.revolute.phi => 0.35], (0, 10))
sol = solve(prob, Tsit5())
cp = model.cartpole
plot(sol, idxs=[cp.prismatic.s, cp.revolute.phi, cp.motor.f.u], layout=3)
plot!(sol, idxs=model.reference.output.u, sp=1, l=(:black, :dash), legend=:bottomright)
Example block output
Multibody.render(model, sol, filename = "inverted_cartpole.gif", x=1, z=1)

inverted cartpole

Swing up

Below, we add also an energy-based swing-up controller. For more details this kind of swing-up controller, see Part 7: Control of rotary pendulum using Julia: Swing up control (YouTube)

"Compute total energy, kinetic + potential, for a body rotating around the z-axis of the world"
function energy(body, w)
    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
    r_cm_worldframe = Multibody.resolve1(ori(body.frame_a), body.r_cm)[2] # Rotate the distance from frame_a to the center of mass to the world frame
    1/2*I*w^2 + 2m*g*(body.frame_a.r_0[2] + r_cm_worldframe) # Assuming rotation around the z-axis
end

normalize_angle(x::Number) = mod(x+3.1415, 2pi)-3.1415

@mtkmodel CartWithSwingup begin
    @components begin
        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
    end
    @variables begin
        phi(t)
        w(t)
        E(t), [description = "Total energy of the pendulum"]
        u_swing(t), [description = "Swing-up control signal"]
        switching_condition(t)::Bool, [description = "Switching condition that indicates when stabilizing controller is active"]
    end
    @parameters begin
        Er = 3.825676486352941 # Total energy of the cartpole at the top equilibrium position
    end
    begin
        namespaced_outputs = ModelingToolkit.renamespace.(:cartpole, outputs) # Give outputs correct namespace, they are variables in the cartpole system
    end
    @equations begin
        phi ~ normalize_angle(cartpole.phi)
        w ~ cartpole.w
        E ~ energy(cartpole.pendulum.body, w) + energy(cartpole.tip, w)
        u_swing ~ 100*(E - Er)*sign(w*cos(phi-3.1415))

        L.input.u[1] ~ 0 - namespaced_outputs[1] # - cartpole.x
        L.input.u[2] ~ 0 - phi # cartpole.phi but normalized
        L.input.u[3] ~ 0 - namespaced_outputs[3] # cartpole.v
        L.input.u[4] ~ 0 - namespaced_outputs[4] # cartpole.w
        switching_condition ~ abs(phi) < 0.4
        control_saturation.input.u ~ ifelse(switching_condition, L.output.u, u_swing)
        connect(control_saturation.output, cartpole.motor.f)
    end
end
@named model = CartWithSwingup()
model = complete(model)
ssys = structural_simplify(multibody(model))
cp = model.cartpole
prob = ODEProblem(ssys, [cp.prismatic.s => 0.0, cp.revolute.phi => 0.99pi], (0, 5))
sol = solve(prob, Tsit5(), dt = 1e-2, adaptive=false)
plot(sol, idxs=[cp.prismatic.s, cp.revolute.phi, cp.motor.f.u, model.E], layout=4)
hline!([0, 2pi], sp=2, l=(:black, :dash), primary=false)
plot!(sol, idxs=[model.switching_condition], sp=2)
Example block output
Multibody.render(model, sol, filename = "swingup.gif", x=2, z=2)

inverted cartpole