PID with Mass Spring Dramper using ModelingToolkit.jl

I’m attempting to using ModelingToolkit (for the first time) to solve a simple PID controller with mass/spring/damper. The PID output is y which I’m converting directly to a force driving the position x of the mass m with spring k and damping d. See code below.

A few problems:

  1. If I use sys = structural_simplify(dae_index_lowering(sys)) then the last equation is eliminated, which is essential, so I guess this isn’t the right thing to do.
  2. If I solve as is, then I get a “dt <= dtmin. Aborting”. Maybe I just need a different solver?
  3. If I change the ODEProblem to a DAEProblem (which I think is the correct problem type) then I get the error “ArgumentError: States (7) and initial conditions (2) are of different lengths.” when calling prob = DAEProblem(sys, u0, tspan, p). This error doesn’t make any sense to me, my u0 is definitely a size 7.

Any help is appreciated!

using ModelingToolkit

@parameters kp ki kd m k d
@variables t y(t) e(t) de(t) dde(t) x(t) dx(t) ddx(t)
D = Differential(t)

#set point
sp = t

sys = ODESystem([
    D(y) ~ kp*de + ki*e + kd*dde
    D(e) ~ de
    D(de) ~ dde
    D(x) ~ dx
    D(dx) ~ ddx
    0.0 ~ ( y )  - ( m*ddx + d*dx  + k*x )
    0.0 ~ ( e ) - ( sp - x )
])

# sys = structural_simplify(dae_index_lowering(sys))

u0 = zeros(7)
p = ones(6)
tspan = (0.0,1.0)
prob = ODEProblem(sys, u0, tspan, p)
sol = solve(prob, ImplicitEuler())
2 Likes

What do you mean? What’s wrong with eliminating it and then just plotting its result?

The last equation is key because the set point sp drives the system. The position of the mass x should follow the set point. The error between setpoint and position is e which is the input the PID controller. If it’s eliminated then the the results are just zero.

The fact that the equation is eliminated is maybe a clue that I have an error somewhere but I can’t find it. I’ll try to reproduce the system now in another way to check.

OK, just to confirm, the equations are OK. Using SimScape, here is the system:

component pid_system

    inputs
        sp = {0, 'm'}
    end
    
    parameters
       m = { 1, 'kg'}
       k = { 1, 'N/m'}
       d = { 1, 'N/(m/s)'}
       kp = { 1, 'N/m'}
       ki = { 1, 'N/(s*m)'}
       kd = { 1, 'N*s/m'}
    end
    
    variables
        x = {0, 'm'};
        dx = {0, 'm/s'};
        ddx = {0, 'm/s^2'};
        
        e = {0, 'm'}
        de = {0, 'm/s'};
        dde = {0, 'm/s^2'};
        
        y = {0, 'N'}
    end
    
    equations
        y.der == kp*de + ki*e + kd*dde
        e.der == de
        de.der == dde
        x.der == dx
        dx.der == ddx
        0.0 == ( y )  - ( m*ddx + d*dx  + k*x )
        0.0 == ( e ) - ( sp - x ) 
    end
    
end

The system is very simple, time (clock) is the set point (sp)
image

And the results of x compared with the set point are:

The solver used in SimScape is Backwards Euler. I’m assuming this is simlar to the ImplicitEuler from DifferentialEquations.jl.

This is not directly an answer to your question, but if you want to work with MTK to build systems that can follow later specified setpoints this is exactly what our BlockSystems does. Here is an implementation of a PT1 controller:

https://wuerfel.io/BlockSystems.jl/dev/generated/spacecraft/

It’s a thin package on top of MTK and you can access the underlying ODESystems at any point. The core thing is that we construct a right hand side function that takes input functions such as set points as arguments, in the Spacecraft example you can see this in these lines: odefun(du, u, p, t) = gen.f_ip(du, u, [targetfun(t)], p, t)

3 Likes

Thanks for this! BlockSystems.jl looks great, may be just what I need!

OK, I found the problem. My initial conditions needed to take into account the initial derivative of 1m/s at time 0 from the set point input. Therefor the initial code works with the following u0:

u0 = [
    1.0 #y
    0.0 #e
    1.0 #de
    0.0 #x
    0.0 #dx
    1e3 #dde
    1.0 #ddx
]

To try and simplify the initial condition back to all zeros, I changed my set point function to:

sp = (t - 1.0)*(t > 1.0)

But I can’t find a solver that is able to advance past 1s.

You may be hitting Issue with alias_elimination · Issue #963 · SciML/ModelingToolkit.jl · GitHub.

If so, a fix is in the works (Fix faulty alias elimination by YingboMa · Pull Request #964 · SciML/ModelingToolkit.jl · GitHub).

1 Like

I have now tried all suggested strategies from (Handling Instability When Solving ODE Problems - #5 by ChrisRackauckas) and have not found a solution that works. It seems there is something unique about my problem that is throwing a wrench in ModelingToolkit/DifferentialEquations.

I feel like the below code should be able to handle the discontinuity of the set point, but it can’t get past 1s where the discontinuity occurs.

using ModelingToolkit
using DifferentialEquations

@parameters kp ki kd m k d
@variables t y(t) e(t) de(t) dde(t) x(t) dx(t) ddx(t)
D = Differential(t)

#set point
sp(t) = t > 1 ? 1.0 : 0.0
@register sp(t)

sys = ODESystem([
    D(y) ~ kp*de + ki*e + kd*dde
    D(e) ~ de
    D(de) ~ dde
    D(x) ~ dx
    D(dx) ~ ddx
    0.0 ~ ( y )  - ( m*ddx + d*dx  + k*x )
    0.0 ~ ( e ) - ( sp(t) - x )
])

u0 = zeros(7)
p = ones(6)
tspan = (0.0,5.0)
prob = ODEProblem(sys, u0, tspan, p)

condition(u,t,integrator) = t - 1
affect!(integrator) = nothing
cb = ContinuousCallback(condition,affect!,save_positions=(false,false))

sol = solve(prob,Rodas4(autodiff=false), callback=cb)

Am I doing something wrong here?

OK, I finally got it working! The trick was to use adaptive=false. The solution is found in 17ms with 12,502 steps, which for my purposes is great. Note: Rodas4() solved without error but gave the wrong result. ImplicitEuler() and Trapezoid() both work great.

Here’s the code:

using ModelingToolkit
using DifferentialEquations
using PlotlyJS

@parameters kp ki kd m k d
@variables t y(t) e(t) de(t) dde(t) x(t) dx(t) ddx(t)
D = Differential(t)

#set point
setpoint(x) = x > 1 ? 1.0 : 0.0
sp(t) = setpoint(t)
@register sp(t)

sys = ODESystem([
    D(y) ~ kp*de + ki*e + kd*dde
    D(e) ~ de
    D(de) ~ dde
    D(x) ~ dx
    D(dx) ~ ddx
    0.0 ~ ( y )  - ( m*ddx + d*dx  + k*x )
    0.0 ~ ( e ) - ( sp(t) - x )
])

u0 = zeros(7)
p = ones(6)
tspan = (0.0,5.0)
prob = ODEProblem(sys, u0, tspan, p)
@time sol = solve(prob,ImplicitEuler(),adaptive=false, dt=4e-4)

u = hcat(sol.u...)

s = GenericTrace[]
push!(s, scatter(x = sol.t, y = setpoint.(sol.t), name="set point"))
push!(s, scatter(x = sol.t, y = u[4,:], name="x"))
plot(s)

1 Like

You should be using a DiscreteCallback instead. If any method that is accurate is not converging well, the solution isn’t to decrease the accuracy…

1 Like

Thanks for the input.

I tried the following without any luck

condition(u,t,integrator) = (t - 1.0) == 0.0
affect!(integrator) = nothing
cb = DiscreteCallback(condition,affect!,save_positions=(false,false))

sol = solve(prob,ImplicitEuler(autodiff=false), callback=cb)

Also, my previous statement was wrong, the run time is 17ms for 12,502 steps. This by the way beats Simulink/SimScape with Backwards Euler method (also Partioning method with 1rst order derivatives) by an order of magnitude. So all of my dreams have come true, ha! Thanks for the amazing work!