Ipopt finished with status Restoration_Failed(don't reply,posted twice)


#1

I try to solve a nonlinear model predictive control problem about tracking certain trajectory.
Following is the code and generally the result will be like

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************

WARNING: Ipopt finished with status Restoration_Failed
WARNING: Not solved to optimality, status: Error
WARNING: Not solved to optimality, status: UserLimit
WARNING: Not solved to optimality, status: UserLimit
WARNING: Not solved to optimality, status: UserLimit
WARNING: Not solved to optimality, status: UserLimit
WARNING: Not solved to optimality, status: UserLimit
WARNING: Not solved to optimality, status: UserLimit
WARNING: Not solved to optimality, status: UserLimit
WARNING: Not solved to optimality, status: UserLimit
WARNING: Ipopt finished with status Restoration_Failed
WARNING: Not solved to optimality, status: Error
elapsed time: 86.297790172 seconds

First, 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 will not follow the constraint " @defVar(mpc, zmin[i] <= z[t=1:T+1,i=1:n] <= zmax[i])", and maybe the reason is “status Restoration_Failed”? or other reasons?

I"m pretty sure that the car dynamic model is correct because I test it in matlab and works fine.

Here is the code

#---using orca drift model to solve nmpc problem 
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 
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) )    
   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)
    #beta= atan( L_f / (L_f + L_r) * tan(u[1]))
    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)

        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
  
    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()

#---------------------Plot result---------------------------
function plot_car(x,y,psi,delta,lf,lr,w,pt)
    A=[x+lf*cos(psi)+w*sin(psi),y+lf*sin(psi)-w*cos(psi)]
    B=[x-lr*cos(psi)+w*sin(psi),y-lr*sin(psi)-w*cos(psi)]
    C=[x+lf*cos(psi)-w*sin(psi),y+lf*sin(psi)+w*cos(psi)]
    D=[x-lr*cos(psi)-w*sin(psi),y-lr*sin(psi)+w*cos(psi)]
    #wheels
    lw=0.02
    E=[A[1]+lw*cos(psi+delta),A[2]+lw*sin(psi+delta)]
    F=[C[1]+lw*cos(psi+delta),C[2]+lw*sin(psi+delta)]
    
    pt.plot([B[1] ,A[1]],[B[2], A[2]],"bo-")
    pt.plot([C[1] ,D[1]],[C[2], D[2]],"bo-")
    pt.plot([C[1] ,A[1]],[C[2], A[2]],"bo-")
    pt.plot([B[1] ,D[1]],[B[2], D[2]],"bo-")
    pt.plot([A[1] ,E[1]],[A[2], E[2]],"go-")
    pt.plot([C[1] ,F[1]],[C[2], F[2]],"go-")
end

fig = figure()
plts.subplot(2,2,3)
ax = plts.axes()

for i=1:10:size(f,1)
    ax[:plot](f[i,1],f[i,2], "ro")
end

for i=1:1:Tsim
    ax[:plot](z_vec[1,i],z_vec[2,i], "bo")
end

for i=1:2:Tsim    #20
    plot_car(z_vec[1,i],z_vec[2,i],z_vec[3,i],u_vec[1,i],lf,lr,0.03,plts)
end

plts.figure()
plts.subplot(2,1,1)
plts.plot(u_vec[1,:],"r")
plts.xlabel("Steering angle")
plts.ylabel("rad")
plts.subplot(2,1,2)
plts.plot(u_vec[2,:],"r")
plts.xlabel("Dutycycle")

plts.figure()
plts.subplots_adjust(top=0.97, bottom=0.06, left=0.06, right=0.97, hspace=0.51)
plts.subplot(7,1,1)
plts.plot(z_vec[1,:],"r")
plts.xlabel("X")
plts.ylabel("m")
plts.subplot(7,1,2)
plts.plot(z_vec[2,:],"r")
plts.xlabel("Y")
plts.ylabel("m")
plts.subplot(7,1,3)
plts.plot(z_vec[3,:],"r")
plts.xlabel("psi")
plts.ylabel("rad")
plts.subplot(7,1,4)
plts.plot(z_vec[4,:],"r")
plts.xlabel("Vx")
plts.ylabel("m/s")
plts.subplot(7,1,5)
plts.plot(z_vec[5,:],"r")
plts.xlabel("Vy")
plts.ylabel("m/s")
plts.subplot(7,1,6)
plts.plot(z_vec[6,:],"r")
plts.xlabel("omega")
plts.ylabel("rad/s")
plts.subplot(7,1,7)
plts.plot(z_vec[7,:],"r")
plts.xlabel("omega_r")
plts.ylabel("rad/s")

plts.show()

I’m using Julia 0.6.2, Ipopt 0.3.0 and JuMP 0.18.1.