Ipopt finished with status Restoration_Failed


#1

I try to solve a nonlinear model predictive control problem about tracking certain trajectory.
Following is the code and I don’t really understand the meaning of “WARNING: Ipopt finished with status Restoration_Failed WARNING: Not solved to optimality, status: Error”.

Also I found that the result didn’t follow the constraint "@defVar(mpc, zmin[i] <= z[t=1:T+1,i=1:n] <= zmax[i]) " The reason is “Ipopt finished with status Restoration_Failed”? or the problem isn’t defined well?

Please help, thanks!

using JuMP, Ipopt
using PyPlot
using PyCall
using Distances
@pyimport scipy.spatial as spa
@pyimport matplotlib.pyplot as plts
@pyimport numpy as np

#-----------All relevant parameters for the model-------------
C0=0.4771*0.0525   # m*=0.0525
C1fw=0.5979*0.0525
Cc=0.365  
C1sd=0.192*0.0525
CM0=7.571*0.0525
CM1=0.229*0.0525
I=3*10.0^-5
Ir=8*10.0^-7
M=0.04  #car weight(kg)
lf=0.029
lr=0.033
rr=0.008  #rear tire radius
B=20  #magic formula, tire stiffness
Cr=1.222    #-> C = 2 - 2/pi*asin(F_max,dynamic/F_max,static)
Cf=1.2186
Dr=0.15
Df=0.137
k=0.8
w=0.03     #width

n=7  #number of state
m=2  #number of input

path="/home/yard/Desktop/orca_input_0.08_0.1.txt"

#------states: z== X,Y,psi,Vx,Vy,omega,omega_r (7 states) ---------------
z0 = [0;0;0;0;0;0;0]   #initial state   
zT = []  

zmax=[1 ; 2; 100; 1.2; 1.2; 16; 224]    
zmin=[-1;-1; 0  ;  0 ;  0 ; 0 ;  0 ]   

T = 10  # MPC horizon
dt=0.1 # Smapling time
Tsim = 10  # Length of time(steps) we simulate
Treplanning = 1 # Every Treplanning an MPC is solved

#--------------input: [delta,D] : delta , Dutycycle----------------------
umax=[0.4 ; 0.1]
umin=[0 ; 0] 

function solveMPC(lf,lr,n,m,T,z0,zT,zmin,zmax,umax,dt)
    mpc = Model(solver=IpoptSolver(print_level=0))
    @defVar(mpc,  zmin[i] <= z[t=1:T+1,i=1:n] <= zmax[i])   
    @defVar(mpc,  umin[i] <= u[t=1:T,i=1:m] <= umax[i])     
    @defVar(mpc,  C1[1:T])
    @defVar(mpc,  alpha[1:T])
    
    @setObjective(mpc, Min,
    sum{(z[t,1]-zT[t,1])^2+(z[t,2]-zT[t,2])^2+(z[t,3]-zT[t,3])^2+(u[k,1]-u[k+1,1])^2+(u[k,2]-u[k+1,2])^2,  k=1:T-1,t= 2:T+1 } )
    
    @NLexpression(mpc, sfy[i=1:T] , (z[i,4]*sin(u[i,1]))-((lf*z[i,6]+z[i,5])*cos(u[i,1])) )
    @NLexpression(mpc, srx[i=1:T] , rr*z[i,7]-z[i,4] )
    @NLexpression(mpc, sry[i=1:T] , z[i,6]*lr-z[i,5] )
    @NLexpression(mpc, sr[i=1:T] , ((srx[i])^2+(sry[i])^2)^0.5  )
   
    @NLexpression(mpc, Ffy[i=1:T] , Df*sin(Cf*atan(B*sfy[i])) )
    @NLexpression(mpc, Fr[i=1:T] , Dr*sin(Cr*atan(B*sr[i])) )
    @NLexpression(mpc, Frx[i=1:T] , Fr[i]*srx[i]/(sr[i]+10.0^(-6)))
    @NLexpression(mpc, Fry[i=1:T] , k*Fr[i]*sry[i]/(sr[i]+10.0^(-6)))

   for i=1:T
        @addNLConstraint(mpc, C1[i] == ifelse( Frx[i]-Ffy[i]*sin(u[i,1])+M*z[i,5]*z[i,6] > 0, C1fw , C1sd) )
        @addNLConstraint(mpc, alpha[i] == ifelse( abs(alpha[i])>1 , ifelse(alpha[i]>0 , 1, -1) , rr*z[i,7]*10) )  #this approximation of the sign?function is used in order to improve the numerical performance     
   end

    @NLexpression(mpc, Fres1[i=1:T] , rr*z[i,7]*C1[i]+alpha[i]*C0 )
    @NLexpression(mpc, Fres2[i=1:T] , alpha[i]*abs(Fry[i])*Cc )
    @NLexpression(mpc, Fres[i=1:T] , Fres1[i]+Fres2[i] )

# Link state and control across the horizon  
     for t = 1:T
        @addNLConstraint(mpc, z[t+1,1] == z[t,1] + dt*(z[t,4]*cos(z[t,3])-z[t,5]*sin(z[t,3]))  )
        @addNLConstraint(mpc, z[t+1,2] == z[t,2] + dt*(z[t,4]*sin(z[t,3])+z[t,5]*cos(z[t,3]))  )
        @addNLConstraint(mpc, z[t+1,3] == z[t,3] + dt*z[t,6] )
        @addNLConstraint(mpc, z[t+1,4] == z[t,4] + dt*(Frx[t]-Ffy[t]*sin(u[t,1])+M*z[t,5]*z[t,6])/M )  
        @addNLConstraint(mpc, z[t+1,5] == z[t,5] + dt*(Fry[t]+Ffy[t]*cos(u[t,1])-M*z[t,4]*z[t,6])/M )
        @addNLConstraint(mpc, z[t+1,6] == z[t,6] + dt*(lf*Ffy[t]*cos(u[t,1])-lr*Fry[t])/I )
        @addNLConstraint(mpc, z[t+1,7] == ifelse( z[t,7] >400 && (rr*u[t,2]*(CM0-rr*z[t,7]*CM1)-rr*(Fres[t]+Frx[t]))/Ir > 0 , z[t,7] , z[t,7]+dt*(rr*u[t,2]*(CM0-rr*z[t,7]*CM1)-rr*(Fres[t]+Frx[t]))/Ir)   )
     end

    # Initial conditions
    @addConstraint(mpc, z[1,:] .== z0)
    # Final state
    # Solve the NLP
    solve(mpc)
    return getvalue(u), getvalue(z)        #getValue ~ getvalue
end

#-------------Car slip dynamic------------------------------------
function zdot_fun(z,u,lf,lr)
    delta=u[1]
    D=u[2]
    phi=z[3]
    vx=z[4]
    vy=z[5]
    omega=z[6]
    omega_r=z[7]

    sfy = vx*sin(delta)-((lf*omega+vy)*cos(delta))
    srx = rr*omega_r-vx
    sry = omega*lr-vy
    sr = (srx^2+sry^2)^0.5
    Ffy = Df*sin(Cf*atan(B*sfy))
    Fr = Dr*sin(Cr*atan(B*sr))
    Frx = Fr*srx/(sr+10.0^(-6))
    Fry = k*Fr*sry/(sr+10.0^(-6))    
    
    if Frx-Ffy*sin(delta)+M*vy*omega > 0
      C1=C1fw
    else
      C1=C1sd
    end
    
    alpha = rr*omega_r*10
    if abs(alpha) > 1
      alpha = sign(alpha)
    end 
    
    Fres1 = rr*omega_r*C1+alpha*C0
    Fres2 = alpha*abs(Fry)*Cc
    Fres = Fres1 + Fres2

    xdot=vx*cos(phi)-vy*sin(phi)  
    ydot=vx*sin(phi)+vy*cos(phi)
    phidot= omega  
    vxdot=(Frx-Ffy*sin(delta)+M*vy*omega)/M
    vydot=(Fry+Ffy*cos(delta)-M*vx*omega)/M
    omegadot=(lf*Ffy*cos(delta)-lr*Fry)/I
    omega_rdot=(rr*D*(CM0-rr*omega_r*CM1)-rr*(Fres+Frx)) /Ir                  
    zdot = [xdot;ydot;phidot;vxdot;vydot;omegadot;omega_rdot]
    return zdot
end

#------------- Simulate car -------------------------------------
function simulate_car_MPC(T,Tsim,Tr,z0,zT,zmax,umax,lf,lr,n,m,dt)
    u_history = zeros(m,Tsim)
    z_history = zeros(n,Tsim)
    start=1
    z_t = z0[:]
    for t = 0:Tr:Tsim-1
        # SHRINKING HORIZON MPC
        #if start > 
        ref = find_nearest(z_t,zT)
        #print(start)
        u_vec, z_vec= solveMPC(lf,lr,n,m,T,z_t,ref,zmin,zmax,umax,dt)
        #print(length(u_vec))
      #  print("-MPC solved-")
        for k = 1:min(Tr,length(u_vec))
              u_t=u_vec[k,:]
              z_history[:,t+k] = z_t[:]
              u_history[:,t+k] = u_t[:]
              z_t = z_t + dt*zdot_fun(z_t,u_t,lf,lr)        

        end
    end
    return  u_history, z_history
end

#--------------- Get reference tragectory ------------------------
function get_trajectory()
 
    f = np.loadtxt(path,usecols = (0,1,2,3,4,5,6))    # 7 state:X Y psi vx vy omega omega_r
    print(size(f))
    return f
end    
#-------------Find the nearest point on the reference of the car ----------------
function find_nearest(x,f)
    dis = Array(Float64,size(f,1))
    ref = Array(Float64,T+1)
    for i in 1:size(f,1)
        dis[i] = euclidean(x[1:2],f[i,1:2])
    end

    refstep=10  
    ref_start = indmin(dis)+1
  
    if (ref_start+T*refstep > size(f,1) ) && (ref_start < size(f,1))
        #print(size(f,1)-ref_start)
	r=(size(f,1)-ref_start)%refstep
	if r != 0
               
            ref = [ f[ref_start:refstep:size(f,1)-r,:] ; f[refstep-r:refstep:(T*refstep-(size(f,1)-ref_start)+r),:]   ]
        else
	    ref = [ f[ref_start:refstep:size(f,1),:] ; f[1:refstep:1+(T*refstep-(size(f,1)-ref_start)),:]   ]
        end
    elseif ref_start > size(f,1)
        ref_start = ref_start - size(f,1)
        ref = f[ref_start:refstep:ref_start+T*refstep,:]
    elseif ref_start == size(f,1)
	ref = f[1:refstep:T*refstep+1,:]
    else     
        ref = f[ref_start:refstep:ref_start+T*refstep,:]
        #print(ref)
    end
    #println(ref_start)
    
    return ref
end
#--------------------------------Main ------------------------------
# Closed-Loop MPC
f = get_trajectory()
println("finished load trajectory!")
# Open Loop MPC
tic()
u_vec,z_vec = simulate_car_MPC(T,Tsim,Treplanning,z0,f,zmax,umax,lf,lr,n,m,dt)
toc()

I use julia 0.6.2, Ipopt 0.3.0 and JuMP 0.18.1.


#2

Welcome to the Julia forum!

You posted twice, can you delete one of the posts? Also, to comment in-line code, use single back-ticks `. Last, your example is very long, which means people will potentially have to spend a long time reading your code to be able to help. The shorter the code example (but still running, a so called minimal working example MWE), the quicker you’ll get answers (and the less you impose on the time of others).

And sorry, I can’t help you with your problem!


#3

Actually, I don’t know how to delete what I posted.
About MWE, I will try to do that. Thank you !


#4

Just FYI, despite being on the latest released version of Julia and JuMP, you seem to be using deprecated syntax; you should probably change your code to the new syntax. Also, as @mauro3 noted, please do provide a minimal runnable example, or at the very least one with minimal dependencies. Presumably, PyPlot isn’t needed to show this behavior. As a result, I haven’t actually run your code.

From the Ipopt docs:

Restoration Failed:
Console Message:
EXIT: Restoration Failed!
This indicates that the restoration phase failed to find a feasible point that was acceptable to the
filter line search for the original problem. This could happen if the problem is highly degenerate,
does not satisfy the constraint qualification, or if your NLP code provides incorrect derivative
information

(emphasis mine)

I’d check for bad derivatives first, since it’s easiest to debug. Since you seem to be using only ‘standard’ functions in your nonlinear expressions, automatic differentiation should be providing Ipopt with correct gradients. But to be more confident that gradients are correct, you can pass the keyword arguments derivative_test = "first-order", or derivative_test = "second-order", and check_derivatives_for_naninf = "yes" into the IpoptSolver constructor.

There’s a good chance that the problem has numerical issues though. I see some divisions in your constraints that could very well lead to bad behavior, and atans in the magic tire formula could lead to vanishing derivatives. The ifelse's look scary as well.