Model Predictive Controller only works for certain values

Specific Domains Optimization (Mathematical)
1

I am using JuMP and Ipopt to solve an online optimization for a Furuta pendulum. Quite quickly I got it running in simulation but for the real pendulum I had some trouble. The trouble was solved by substituting one of the inputs with a large negative number. My guess is that this simplifies the optimization making it faster and less prone to failing. If that is the case, why does it work that way? Could it be a scaling issue?

In the code below the phidot, theta and thetadot are the measured values from the pendulum. The phi angle is replaced with -1000.0 which makes the controller work.

function run_mpc(init_phi, init_phidot, init_theta, init_thetadot, u_last)
    model = Model(Ipopt.Optimizer)
    MOI.set(model, MOI.Silent(), true)

    num_time_steps = 10 #10 for 0.02 and 6 for 0.04
    Mc = 3
    max_speed = 40
    
    @variables model begin
        phi[1:num_time_steps]
       	-max_speed <= phidot[1:num_time_steps] <= max_speed
        theta[1:num_time_steps]
        -max_speed <= thetadot[1:num_time_steps] <= max_speed
        u[1:num_time_steps-1]
    end
    
    set_start_value.(u, vcat(u_last[2:num_time_steps-1],0.0))

    # Dynamics constraints
    @NLconstraint(model, [i=2:num_time_steps],
                phi[i] == phidot[i-1]*h + phi[i-1])
    @NLconstraint(model, [i=2:num_time_steps],
                phidot[i] == h*(1/(alfa*beta-gamma^2+(beta^2+gamma^2)*sin(theta[i-1])^2)*(beta*gamma*(sin(theta[i-1])^2-1)*sin(theta[i-1])*phidot[i-1]^2-2*beta^2*cos(theta[i-1])*sin(theta[i-1])*phidot[i-1]*thetadot[i-1]+beta*gamma*sin(theta[i-1])*thetadot[i-1]^2-gamma*delta*cos(theta[i-1])*sin(theta[i-1])+beta*u[i-1]))+phidot[i-1])
    @NLconstraint(model, [i=2:num_time_steps],
                theta[i] == thetadot[i-1]*h + theta[i-1])
    @NLconstraint(model, [i=2:num_time_steps],
                thetadot[i] == h*(1/(alfa*beta-gamma^2+(beta^2+gamma^2)*sin(theta[i-1])^2)*(beta*(alfa+beta*sin(theta[i-1])^2)*cos(theta[i-1])*sin(theta[i-1])*phidot[i-1]^2+2*beta*gamma*(1-sin(theta[i-1])^2)*sin(theta[i-1])*phidot[i-1]*thetadot[i-1]-gamma^2*cos(theta[i-1])*sin(theta[i-1])*thetadot[i-1]^2+delta*(alfa+beta*sin(theta[i-1])^2)*sin(theta[i-1])-gamma*cos(theta[i-1])*u[i-1]))+thetadot[i-1])
          #@constraint(model, [i=Mc+1:num_time_steps-1],
          #u[i] == u[Mc])  

    # Cost function: minimize final position and final velocity
    @objective(model, Min, 
        4*sum(theta[num_time_steps].^2) + 1*phidot[num_time_steps]^2 + 1*sum(thetadot[num_time_steps-1:num_time_steps].^2))

    # Initial conditions:
    @NLconstraint(model, phi[1] == init_phi)
    @NLconstraint(model, phidot[1] == init_phidot)
    @NLconstraint(model, theta[1] == init_theta)
    @NLconstraint(model, thetadot[1] == init_thetadot)

    optimize!(model)
    return getvalue.(u)
end
1 Like
2

Hi!

Quite quickly I got it running in simulation but for the real pendulum I had some trouble.

What do you mean by “trouble”? Are you getting an error from Julia? Is the optimization taking too long? Is the solution not good enough?

If Julia does not throw any error, could you expand on the different behaviors you observed between “simulation” and “real pendulum”? It could be useful to see Ipopt’s log in both situations.

3

I can’t reproduce your issues, because I don’t know the arguments to your function. Take a read of: Please read: make it easier to help you

Some things that might help:

  • use fix(phi[1] init_phi) instead of adding the initial value constraints.
  • Add sensible bounds on phi, theta, and u
  • Consider rewriting phi and theta as a @NLexpression instead of a variable. (It seems your only real decision variable is u.) For example, something like:
    phi = [@NLexpression(model, init_phi)]
for i in 2:num_time_steps
    push!(phi, @NLexpression(model, phidot[i - 1] * h + phi[i - 1]))
end
2 Likes
4

Hi!

Thanks for tips. Unfortunately I can not provide a working example since it runs against a physical pendulum, the inputs are measured angles and angular velocities from the pendulum. I can provide a working example that runs against a simulation but that one works completely fine so I am not sure what that will provide. Would it be helpful to include a the code with a simulation of the model?

Here is the full code that runs against the physical pendulum.

using JuMP
using Ipopt
using Plots
 
h = 0.02 #0.02 might work as well 0.04 does not miss many deadlines
J = 0.000154
M = 0.0
ma = 0.0
mp = 5.44/1000
la = 43/1000
lp = 32.3*2/1000
g = 9.81

alfa = J + (M + 1/3*ma + mp)*la^2
beta = (M + 1/3*mp)*lp^2
gamma = (M + 1/2*mp)*la*lp
delta = (M + 1/2*mp)*g*lp

function run_mpc(init_phi, init_phidot, init_theta, init_thetadot, u_last)
    model = Model(Ipopt.Optimizer)
    MOI.set(model, MOI.Silent(), true)

    num_time_steps = 10 #10 for 0.02 and 6 for 0.04
    Mc = 3
    max_speed = 40
    
    @variables model begin
        phi[1:num_time_steps]
       	-max_speed <= phidot[1:num_time_steps] <= max_speed
        theta[1:num_time_steps]
        -max_speed <= thetadot[1:num_time_steps] <= max_speed
        u[1:num_time_steps-1]
    end
    
    set_start_value.(u, vcat(u_last[2:num_time_steps-1],0.0))

    # Dynamics constraints
    @NLconstraint(model, [i=2:num_time_steps],
                phi[i] == phidot[i-1]*h + phi[i-1])
    @NLconstraint(model, [i=2:num_time_steps],
                phidot[i] == h*(1/(alfa*beta-gamma^2+(beta^2+gamma^2)*sin(theta[i-1])^2)*(beta*gamma*(sin(theta[i-1])^2-1)*sin(theta[i-1])*phidot[i-1]^2-2*beta^2*cos(theta[i-1])*sin(theta[i-1])*phidot[i-1]*thetadot[i-1]+beta*gamma*sin(theta[i-1])*thetadot[i-1]^2-gamma*delta*cos(theta[i-1])*sin(theta[i-1])+beta*u[i-1]))+phidot[i-1])
    @NLconstraint(model, [i=2:num_time_steps],
                theta[i] == thetadot[i-1]*h + theta[i-1])
    @NLconstraint(model, [i=2:num_time_steps],
                thetadot[i] == h*(1/(alfa*beta-gamma^2+(beta^2+gamma^2)*sin(theta[i-1])^2)*(beta*(alfa+beta*sin(theta[i-1])^2)*cos(theta[i-1])*sin(theta[i-1])*phidot[i-1]^2+2*beta*gamma*(1-sin(theta[i-1])^2)*sin(theta[i-1])*phidot[i-1]*thetadot[i-1]-gamma^2*cos(theta[i-1])*sin(theta[i-1])*thetadot[i-1]^2+delta*(alfa+beta*sin(theta[i-1])^2)*sin(theta[i-1])-gamma*cos(theta[i-1])*u[i-1]))+thetadot[i-1])
          #@constraint(model, [i=Mc+1:num_time_steps-1],
          #u[i] == u[Mc])  

    # Cost function: minimize final position and final velocity
    @objective(model, Min, 
        4*sum(theta[num_time_steps].^2) + 1*phidot[num_time_steps]^2 + 1*sum(thetadot[num_time_steps-1:num_time_steps].^2))

    # Initial conditions:
    @NLconstraint(model, phi[1] == init_phi)
    @NLconstraint(model, phidot[1] == init_phidot)
    @NLconstraint(model, theta[1] == init_theta)
    @NLconstraint(model, thetadot[1] == init_thetadot)

    optimize!(model)
    #if(termination_status(model) == MOI.LOCALLY_SOLVED)
        return getvalue.(u)
    #else
        #return u_last
   #end
end


#Global variables needed for running
ts = []
xs = []
θ_old = 0
revs = 0
ϕ, dϕdt, θ, dθdt = get_x()
θ -= pi
u_former = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
u = run_mpc(-1000.6823725703555965, dϕdt, θ, dθdt, u_former) 
sleep(0.01)
starting_time = time()

enable!()

for i in 1:600
    global u_former
    #Next wakeup
    wakeup = time()+h
    
    #Get values
    ϕ, dϕdt, θ, dθdt = get_x()
    set_torque!(u_former[2])
    
    #Handle differences between model and actual process
    if θ_old > 1.5*pi && θ < 0.5*pi
        revs += 1
    elseif θ_old < 0.5*pi && θ > 1.5*pi
        revs -= 1
    end
    θ_old = θ
    θ = θ + revs*2*pi - pi
    
    #Calculate output and set it
    t = time()
    u = run_mpc(-1000.6823725703555965, dϕdt, θ, dθdt, u_former) 
    println("Execution time: ",(time() - t)) 
    println("U is: ",u)
    set_torque!(u[1])
    
    #Save values for plotting
    elapsed_time = time() - starting_time
    push!(ts, elapsed_time)
    push!(xs, [ϕ, dϕdt, θ, dθdt,u[1]])
    sleep(0.001)
    
    #Sleep
    sleeps = wakeup-time()
    if sleeps > 0
        println("Sleeping")
        sleep(sleeps)
    end
    u_former = u
end 
#-195.6823725703555965

set_torque!(0.0)
disable!()

#plotting
phis = [ x[1] for x in xs ]
thetas = [ x[3] for x in xs ];
thetadots = [ x[4] for x in xs ];
us = [ x[5] for x in xs ];
plot(ts, [thetas,us], xlabel = "Time [s]", ylabel = "Pendulum angle [rad]", label = ["theta" "u"],marker=:dots)
5

Hi!

Yes I realized I was a bit unclear in the post. No errors from julia, sometimes the optimization fails but it seems like the execution time becomes the problem.

In simulation the pendulum works as expected, all optimizations are solved and the pendulum does a perfect swing up all the time. On the real pendulum nothing happens if phi is close to zero, the pendulum does not move. However since we are not interested in controlling phi it was noted that when phi was replace with a constant large negative number the optimization works and the pendulum does a beautiful swing up.

One thing that should be noted is that the pendulum will time out if the optimization takes longer than 0.1 seconds (we are using a sampling time of 0.02 so hopefully that should not be possible). This does seem to be the problem with the optimization, one of the early optimization times out when phi is not -1000.

So basically my question is: how can the optimization be helped by replacing a number that is not optimized?

6

If the optimization is taking too long, this usually comes from 2 factors: (i) time per interior-point iteration, or (ii) the number of interior-point iterations.
Since, IIUC, your problem’s structure does not change from one optimization run to another, I suspect Ipopt takes a high number of iterations. One way to check this is to activate the log, and look at it.

A high number of interior-point iterations generally indicates numerical issues.
This may be caused by unstable derivatives or irregularly small/large coefficients in the data.

Unfortunately, there is not general answer to this, other than taking a deep look into the model / data.
Something that sometimes helps is to add a small convex term to the objective (e.g. a quadratic term) to make it more “convex” and prevent iterates from diverging.

7

Okay! Yeah I suspected there might be some numerical issues at hand. What do you mean by “add a small convex term to the objective (e.g. a quadratic term)”? The objective function right now only consists of quadratic terms?

8

“add a small convex term to the objective (e.g. a quadratic term)”

If x is your decision variable and the objective is f(x), you may change it to, e.g., f(x) + \lambda x^{2} for some small \lambda > 0. (I’m assuming minimization here).
In general, doing so can improve the numerics: if H is the hessian of f, then the Hessian of the objective becomes H + \lambda I, which is numerically nicer to work with.

1 Like
9

Thanks I’ll try that!