# Modeling for control using ModelingToolkit

## Example: PID control with ModelingToolkit

This example will demonstrate a simple workflow for simulation of a control system.

### The system model

We will consider a system consisting of two masses connected by a flexible element.

The code belows defines the system model, which also includes a damper to model dissipation of energy by the flexible element.

using ModelingToolkit, OrdinaryDiffEq, Plots
using ModelingToolkitStandardLibrary.Mechanical.Rotational
using ModelingToolkitStandardLibrary.Blocks: t, Sine
using ModelingToolkit: connect

# Parameters
m1 = 1
m2 = 1
k = 1000 # Spring stiffness
c = 10   # Damping coefficient

@named inertia1 = Inertia(; J = m1)
@named inertia2 = Inertia(; J = m2)

@named spring = Spring(; c = k)
@named damper = Damper(; d = c)

@named torque = Torque()

function SystemModel(u=nothing; name=:model)
eqs = [
connect(torque.flange, inertia1.flange_a)
connect(inertia1.flange_b, spring.flange_a, damper.flange_a)
connect(inertia2.flange_a, spring.flange_b, damper.flange_b)
]
if u !== nothing
push!(eqs, connect(torque.tau, u.output))
return @named model = ODESystem(eqs, t; systems = [torque, inertia1, inertia2, spring, damper, u])
end
ODESystem(eqs, t; systems = [torque, inertia1, inertia2, spring, damper], name)
end
SystemModel (generic function with 2 methods)

We are now ready to simulate the system. We simulate the response of the system to a sinusoidal input and plot how the states evolve in time

model = SystemModel(Sine(frequency=30/2pi, name=:u))
sys = structural_simplify(model)
prob = ODEProblem(sys, Pair[], (0.0, 1.0))
sol = solve(prob, Rodas5())
plot(sol)

The next step is to introduce a controller and close the loop. We will look at how the closed-loop system behaves with a step on the reference. To handle the discontinuous reference step, we tell the solver to continue even though the step size gets small using force_dtmin=true. We also show how to connect a reference filter to shape the reference (feedforward). The closed-loop system will thus look like this

                                  d│
│
┌───────┐        ┌────────┐    │   ┌────────┐
r  │       │      e │        │ u  ▼   │        │        y
────► filter├────+───►  PID   ├────+───►   P    ├───────┬───►
│       │   -▲   │        │        │        │       │
└───────┘    │   └────────┘        └────────┘       │
│                                      │
└──────────────────────────────────────┘
using ModelingToolkitStandardLibrary.Blocks: LimPID, SecondOrder, Step, RealOutput
#using ModelingToolkitStandardLibrary.Mechanical.Rotational: AngleSensor

function AngleSensor(;name)
@named flange = Flange()
@named phi = RealOutput()
eqs = [
phi.u ~ flange.phi
flange.tau ~ 0
]
return ODESystem(eqs, t, [], []; name=name, systems=[flange, phi])
end

@named r = Step(start_time=1)
model = SystemModel()
@named pid = LimPID(k = 400, Ti = 0.5, Td = 1, u_max=350)
@named filt = SecondOrder(d = 0.9, w = 10)
@named sensor = AngleSensor()

connections = [
connect(r.output, filt.input)
connect(filt.output, pid.reference)
connect(pid.ctr_output, model.torque.tau)
connect(model.inertia1.flange_b, sensor.flange)
connect(pid.measurement, sensor.phi)
]
closed_loop = structural_simplify(
ODESystem(connections, t, systems = [model, pid, filt, sensor, r], name = :closed_loop),
)
prob = ODEProblem(closed_loop, Pair[], (0.0, 4.0))
sol = solve(prob, Rodas5())
plot(
plot(sol, vars = [filt.y, model.inertia1.phi, model.inertia2.phi]),
plot(sol, vars = [pid.ctr_output.u], title = "Control signal"),
legend = :bottomright,
)

## Example: State feedback using ModelingToolkit

In this example we will instead simulate a state-feedback interconnection. We assume for now that all states are measurable and that a suitable feedback gain is given. The scenario this time is disturbance rejection, i.e., we simulate that a unit disturbance acts as a force on the second mass between 1 < t < 3. The system can be depicted like this, where the + node is represented by the Add block in the code.

       ┌─────┐  y
d      │     ├─────►
───►+──►model│
▲  │     ├──┐
│  └─────┘  │x
│           │
│   ┌───┐   │
└───┤ L │◄──┘
└───┘
using ModelingToolkitStandardLibrary.Blocks: MatrixGain, Add

@named d = Step(start_time=1, duration=2) # Disturbance
model = SystemModel() # Model with load disturbance

L = -[9.19578  6.82981  15.9409  -1.79881] # state-feedback gain
@named state_feedback = MatrixGain(L)
connections = [
[state_feedback.input.u[i] ~ [model.inertia1.w, model.inertia2.w, model.inertia1.phi, model.inertia2.phi][i] for i in 1:4]
]
closed_loop = ODESystem(connections, t, systems=[model, state_feedback, add, d], name=:closed_loop)
closed_loop = structural_simplify(closed_loop)
prob = ODEProblem(closed_loop, Pair[], (0., 10.))
sol = solve(prob, Rodas5(), dtmax=0.1); # set dtmax to prevent the solver from overstepping the entire disturbance
plot(plot(sol, vars=[model.inertia1.phi, model.inertia2.phi]), plot(sol, vars=[state_feedback.output.u], title="Control signal"))

## Example: Feedforward using an inverse model

In the examples above, we filtered the reference step using a second-order filter. This is important in order to get a feasible reference trajectory, indeed, a step in a positional reference is discontinuous, requiring unbounded velocities to realize. With the second-order filter, the filtered reference will have a continuous position and velocity profile. However, we relied solely on error-driven feedback to follow the reference, implying that an error has to arise before the controller performs any action. In order to increase the reference-following performance, we make use of feedforward, i.e., given a model of the controlled system, we can calculate ahead-of-time what control input we ought to send to the plant in order for it to follow the reference. A model that goes from desired output to an input is called an inverse model, i.e., it goes from output to input, as opposed to the standard simulation model which is in this context called a forward model.

Employing an inverse model in ModelingToolkit is straightforward due to its acausal nature. We need to create an identical copy of the model, we call it inverse_model, and then we connect it between the filtered reference and the input of the forward model. We connect it "in reverse", i.e., we connect the torque of the inverse to the torque of the forward model, and connect the "sensor output" of the inverse model to the desired sensor output that comes from the filtered reference.

The interconnection diagram will look like below, where M denotes model and iM denotes the inverse model. F denotes the reference filter.

                    ┌─────┐
│     │
┌──────►│ iM  ├─┐
│       │     │ │
│       └─────┘ │
│               │
┌─────┐ │       ┌─────┐ │  ┌─────┐
r  │     │ │       │     │ ▼  │     │
────┤  F  ├─┴──►+──►│ PID ├─+─►│  M  ├─┐y
│     │     ▲   │     │    │     │ │
└─────┘     │   └─────┘    └─────┘ │
│                      │
│   ┌─────┐            │
│   │     │            │
└───┤ -1  ◄────────────┘
│     │
└─────┘
@named add = Add() # To add the feedback and feedforward control signals
@named inverse_model = SystemModel()
@named inverse_sensor = AngleSensor()
connections = [
connect(r.output, filt.input)
connect(inverse_model.inertia1.flange_b, inverse_sensor.flange) # Attach the inverse sensor to the inverse model
connect(filt.output, pid.reference, inverse_sensor.phi) # the filtered reference now goes to both the PID controller and the inverse model input
connect(model.inertia1.flange_b, sensor.flange)
connect(pid.measurement, sensor.phi)
]
closed_loop = structural_simplify(
ODESystem(connections, t, systems = [model, inverse_model, pid, filt, sensor, inverse_sensor, r, add], name = :closed_loop),
)
prob = ODEProblem(closed_loop, Pair[], (0.0, 3.0))
sol = solve(prob, Rodas5())
plot(
plot(sol, vars = [filt.y, model.inertia1.phi, model.inertia2.phi]),
plot(sol, vars = [pid.ctr_output.u], title = "Feedback control signal", ylims=(-1,1)),
plot(sol, vars = [inverse_model.torque.tau.u], title = "Feedforward control signal"),
legend = :bottomright,
)

This time, we get perfect reference tracking and the PID controller has zero output! In practice, the result is not expected to be this perfect due to mismatch between the model used for feedforward and the actual plant, but this approach is nevertheless very useful in improving reference tracking in servo applications.