PID with Mass Spring Dramper using ModelingToolkit.jl

Brad_Carman · April 12, 2021, 12:37am

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:

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.
If I solve as is, then I get a “dt <= dtmin. Aborting”. Maybe I just need a different solver?
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())

ChrisRackauckas · April 12, 2021, 1:33pm

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

Brad_Carman · April 12, 2021, 1:44pm

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.

Brad_Carman · April 12, 2021, 2:10pm

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)

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.

FHell · April 12, 2021, 2:11pm

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)

Brad_Carman · April 12, 2021, 2:21pm

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

Brad_Carman · April 12, 2021, 2:41pm

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.

tshort · April 12, 2021, 2:42pm

You may be hitting https://github.com/SciML/ModelingToolkit.jl/issues/963.

If so, a fix is in the works (https://github.com/SciML/ModelingToolkit.jl/pull/964).

Brad_Carman · April 13, 2021, 3:27pm

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?

Brad_Carman · April 13, 2021, 6:11pm

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)

ChrisRackauckas · April 13, 2021, 6:46pm

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…

Brad_Carman · April 13, 2021, 10:23pm

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!

baggepinnen · December 3, 2022, 7:51am

For people stumbling upon this topic (as I did when googling for something related ), we have a little tutorial for how to build and control a mass-spring-damper system (actually, a double-mass system) with MTK and standard-library components.
https://help.juliahub.com/juliasimcontrol/dev/examples/mtk_control/