Hi everyone,
First, thank you some much for your nice work on acados!
I am using the Python interface to design an MPC controller for a group of 3 robots (point masses with a double integrator dynamic model for now). I am trying to add nonlinear constraints for collision avoidance. I used both euclidian distance constraints and control barrier functions.
My model is discrete and I use SQP and full condensing HPIPM for the solver. I get satisfying results when I set a maximum number of iterations for the qp solver equal or lower than 5. But I always reach the maximum of sqp iterations (here I set it to 100). However, if I increase qp_solver_iter_max in order to get a better solution, I get an qp error status 3. Would you have any idea why? The problem seems to be independent from the type of constraint I use.
Here is the code (ocp solver and closed-loop simulation) :
from acados_template import *
from double_integrator_model_discrete import export_2d_model_discrete
from utils import plot_multi_robots
import numpy as np
from scipy.linalg import block_diag
import casadi as ca
from model_integrator_discrete import simulate_discrete_model
from casadi import MX, vertcat
def main():
# create ocp object to formulate the OCP
print("Exporting solver ...")
N = 20
Tf = 1.0
Ts = Tf/N
r = 0.2
gamma = 2.0 # safety coefficient for the control barrier function
ocp = AcadosOcp()
# set model
model = export_2d_model_discrete()
ocp.model = model
ocp.model:AcadosModel = model
nx = model.x.size()[0]
nu = model.u.size()[0]
print("State size: ",nx)
print("Input size: ",nu)
# set dimensions
ocp.dims.N = N
ocp.dims.nx = nx
ocp.dims.nu = nu
ocp.dims.nbx = nx
ocp.dims.nbu = nu
# set cost with control barrier function
Qi = np.diag([30,30,20,20])
Qif = np.diag([100,100,10,10])
Q = block_diag(Qi,Qi,Qi)
Qf = block_diag(Qif,Qif,Qif)
Ri = np.diag([0.1,0.1])
R = block_diag(Ri,Ri,Ri)
# set goal
goal = np.array([2.0,4.0,0.0,0.0,6.0,2.0,0.0,0.0,6.0,6.0,0.0,0.0])
# the 'EXTERNAL' cost type can be used to define general cost terms
# NOTE: This leads to additional (exact) hessian contributions when using GAUSS_NEWTON hessian.
#ocp.cost.cost_type = "EXTERNAL"
#ocp.cost.cost_type_e = "EXTERNAL"
ocp.cost.cost_type = "LINEAR_LS"
ocp.cost.cost_type_e = "LINEAR_LS"
# states for constraints and costs
x = ocp.model.x
# inputs for constraints and costs
u = ocp.model.u
ny = nx+nu
ny_e = nx
# linear cost
ocp.cost.W_e = Q
ocp.cost.W = block_diag(Q, R)
ocp.cost.Vx = np.zeros((ny,nx))
ocp.cost.Vx[:nx,:nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[nx : nx + nu, 0:nu] = np.eye(nu)
ocp.cost.Vu = Vu
ocp.cost.Vx_e = np.eye(nx)
yref = np.zeros((ny,))
yref[:nx] = goal
ocp.cost.yref = yref
ocp.cost.yref_e = goal
# set states and inputs boundaries
Fmax = 2
xmax = 40
vmax = 15
x0 = np.array([6.0,4.0,0.0,0.0,2.0,6.0,0.0,0.0,2.0,2.0, 0.0, 0.0])
#ocp.constraints.constr_type ='BGP'
ubu = np.array([+Fmax,+Fmax,+Fmax,+Fmax,+Fmax,+Fmax])
ocp.constraints.lbu = -ubu
ocp.constraints.ubu = ubu
ubx = np.array([xmax,xmax,vmax,vmax,xmax,xmax,vmax,vmax,xmax,xmax,vmax,vmax])
ocp.constraints.lbx = -ubx
ocp.constraints.ubx = ubx
ocp.constraints.ubx_0 = ubx
ocp.constraints.lbx_0 = -ubx
ocp.constraints.ubx_e = ubx
ocp.constraints.lbx_e = -ubx
ocp.constraints.idxbu = np.arange(nu)
ocp.constraints.idxbx = np.arange(nx)
ocp.constraints.idxbx_e = np.arange(nx)
# intial condition constraint
ocp.constraints.x0 = x0
## Collision avoidance constraints
# dist12 = (x[0]-x[4])**2+(x[1]-x[5])**2
# dist13 = (x[0]-x[8])**2+(x[1]-x[9])**2
# dist32 = (x[8]-x[4])**2+(x[9]-x[5])**2
cbf12 = (x[0]-x[4])**2 + (x[1]-x[5])**2 - (2*r*r)
cbf12dot = 2*(x[0]-x[4])*(x[2]-x[6]) + 2*(x[1]-x[5])*(x[3]-x[7])
h12 = cbf12dot + gamma*cbf12
cbf13 = (x[0]-x[8])**2 + (x[1]-x[9])**2 - (2*r*r)
cbf13dot = 2*(x[0]-x[8])*(x[2]-x[10]) + 2*(x[1]-x[9])*(x[3]-x[11])
h13 = cbf13dot + gamma*cbf13
cbf32 = (x[8]-x[4])**2 + (x[9]-x[5])**2 - (2*r*r)
cbf32dot = 2*(x[8]-x[4])*(x[10]-x[6]) + 2*(x[9]-x[5])*(x[11]-x[7])
h32 = cbf32dot + gamma*cbf32
# dist1obs = (x[0]-4)**2+(x[1]-4)**2
ocp.constraints.lh = np.array([0.1,0.1,0.1])
ocp.constraints.uh = np.array([100.0,100.0,100.0])
ocp.model.con_h_expr = vertcat(h12,h13,h32)
ocp.constraints.uh_e = ocp.constraints.uh
ocp.constraints.lh_e = ocp.constraints.lh
ocp.model.con_h_expr_e = ocp.model.con_h_expr
# IF SOFT
ocp.constraints.idxsh = np.array([0])
ocp.constraints.idxsh_e = np.array([0])
Zh = 1e6 * np.ones(1)
zh = 1e4 * np.ones(1)
ocp.cost.zl = zh
ocp.cost.zu = zh
ocp.cost.Zl = Zh
ocp.cost.Zu = Zh
ocp.cost.zl_e = np.concatenate((ocp.cost.zl_e, zh))
ocp.cost.zu_e = np.concatenate((ocp.cost.zu_e, zh))
ocp.cost.Zl_e = np.concatenate((ocp.cost.Zl_e, Zh))
ocp.cost.Zu_e = np.concatenate((ocp.cost.Zu_e, Zh))
# set options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES
# PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM,
# PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP, FULL_CONDENSING_DAQP
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT'
ocp.solver_options.integrator_type = 'DISCRETE'
ocp.solver_options.regularize_method = 'CONVEXIFY'
ocp.solver_options.nlp_solver_max_iter = 100
ocp.solver_options.qp_solver_iter_max = 5
ocp.solver_options.levenberg_marquardt = 10.0
ocp.solver_options.print_level = 0
# set prediction horizon
ocp.solver_options.tf = Tf
ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_discrete.json')
print("Exported solver")
Nsim = 300
simX = np.ndarray((Nsim+1, nx))
simU = np.ndarray((Nsim, nu))
timings = np.zeros((Nsim,))
simX[0,:] = x0
for i in range(0,Nsim):
# first guess for u
# uinit = np.array([(goal[0]-simX[i, 0])/np.sqrt((goal[0]-simX[i, 0])**2+(goal[1]-simX[i, 1])**2),
# (goal[1]-simX[i, 1])/np.sqrt((goal[0]-simX[i, 0])**2+(goal[1]-simX[i, 1])**2),
# (goal[4]-simX[i, 4])/np.sqrt((goal[4]-simX[i, 4])**2+(goal[5]-simX[i, 5])**2),
# (goal[5]-simX[i, 5])/np.sqrt((goal[4]-simX[i, 4])**2+(goal[5]-simX[i, 5])**2),
# (goal[8]-simX[i, 8])/np.sqrt((goal[8]-simX[i, 8])**2+(goal[9]-simX[i, 9])**2),
# (goal[9]-simX[i, 9])/np.sqrt((goal[8]-simX[i, 8])**2+(goal[9]-simX[i, 9])**2) ])
uinit = np.zeros((6,))
for k in range(N):
ocp_solver.set(k, 'x', simX[i,:])
if k==0:
ocp_solver.set(k, 'u', uinit)
else:
ocp_solver.set(k, 'u', simU[i,:])
ocp_solver.set(N, 'x', simX[i,:])
#ocp_solver.set(0, "lbx", xcurrent)
#ocp_solver.set(0, "ubx", xcurrent)
# solve ocp and get next control input
ocp_solver.solve_for_x0(x0_bar = simX[i, :])
u = ocp_solver.get(0, "u")
simU[i,:] = u
#print("control at time", i, ":", u)
#print("state at time", i, ":", simX[i, :])
print("cost at time",i,":",ocp_solver.get_cost())
print("sqp iters:", ocp_solver.get_stats('sqp_iter'))
#ocp_solver.print_statistics()
ocp_solver.reset()
# simulate system
simX[i+1, :] = simulate_discrete_model(Ts,simX[i,:],u,nx)
plot_multi_robots(simX)
if __name__ == '__main__':
main()
Here are the model dynamics file and the plot function :
import sys
import numpy as np
from scipy.linalg import block_diag
from casadi import MX
from utils import *
import matplotlib.pyplot as plt
def simulate_discrete_model(Ts,x,u,nx):
# simulation options
m = 0.5
Aid = np.array([[1,0,Ts,0],[0,1,0,Ts],[0,0,1,0],[0,0,0,1]])
Ad = block_diag(Aid,Aid,Aid)
Bid = np.array([[0,0],[0,0],[Ts/m,0],[0,Ts/m]])
Bd = block_diag(Bid,Bid,Bid)
x = Ad @ x + Bd @ u #+ np.random.normal(0, 0.01, (nx,))
return x
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import animation
from IPython.display import HTML
def plot_multi_robots(X):
print("in plot funcion: ",X.shape)
x1 = X[:, 0]
y1 = X[:, 1]
x2 = X[:, 4]
y2 = X[:, 5]
x3 = X[:, 8]
y3 = X[:, 9]
goal = np.array([2.0,4.0,0.0,0.0,6.0,2.0,0.0,0.0,6.0,6.0,0.0,0.0])
# create a figure and axes
fig = plt.figure(figsize=(12, 5))
ax1 = plt.subplot()
# ax2 = plt.subplot(1,2,2)
# set up the subplots as needed
ax1.set_xlim((0, 12))
ax1.set_ylim((0, 12))
ax1.set_xlabel('x')
ax1.set_ylabel('y')
# create objects that will change in the animation. These are
# initially empty, and will be given new values for each frame
# in the animation.
txt_title = ax1.set_title('')
pt1, = ax1.plot([], [], 'g.', ms=20)
line1, = ax1.plot([], [], 'y', lw=2)
pt2, = ax1.plot([], [], 'g.', ms=20)
line2, = ax1.plot([], [], 'y', lw=2)
pt3, = ax1.plot([], [], 'g.', ms=20)
line3, = ax1.plot([], [], 'y', lw=2)
plt.plot(goal[0], goal[1], 'b*', ms=10)
plt.plot(goal[4], goal[5], 'b*', ms=10)
plt.plot(goal[8], goal[9], 'b*', ms=10)
#plt.show()
# animation function. This is called sequentially
X1plot =[]
Y1plot=[]
X2plot =[]
Y2plot=[]
X3plot =[]
Y3plot=[]
def drawframe(n):
X1plot.append(x1[n])
Y1plot.append(y1[n])
line1.set_data(X1plot[0:n+1],Y1plot[0:n+1])
pt1.set_data(X1plot[n],Y1plot[n])
X2plot.append(x2[n])
Y2plot.append(y2[n])
line2.set_data(X2plot[0:n+1],Y2plot[0:n+1])
pt2.set_data(X2plot[n],Y2plot[n])
X3plot.append(x3[n])
Y3plot.append(y3[n])
line3.set_data(X3plot[0:n+1],Y3plot[0:n+1])
pt3.set_data(X3plot[n],Y3plot[n])
txt_title.set_text('Frame = {0:4d}'.format(n))
return (pt1,line1,pt2,line2,pt3,line3)
anim = animation.FuncAnimation(fig, drawframe, frames=100, interval=100, blit=True,repeat=False)
HTML(anim.to_html5_video())
anim.save('ANIMATION.gif', writer='pillow', fps=60)
Thank you very much in advance!