# Get error using python for NMPC

Hello:
When I used python acados to solve a NMPC problem.I got error messages:QP solver returned error status 3 in SQP iteration 1, QP iteration 1.Cause I’m a novice ,I don’t really know what go wrong.

There are my code:
import rospy
import os
import numpy as np
import random
import math

def init(self):
self.node = 40 # 预测步长
dt = 0.1 # 时间步长

``````    self.numAgent = 10
self.max_neig = 3
self.maxA = 1

# 邻域个体
self.neighborIndex = [[random.randint(0,9)]*self.max_neig]*self.numAgent

self.vReference = np.array([1,1,1])
self.uReference = np.array([1,1,1])
self.distReference = 1
self.max = 1
self.predictionHorizon = 4
# 模型参数
self.position = cs.MX.sym("position", 3*self.numAgent)
self.velocity = cs.MX.sym("velocity", 3*self.numAgent)
# self.acceleration = cs.MX.sym("acceleration", 3)
self.x = cs.vertcat(self.position, self.velocity)
self.x0 = [[0,0,-1.75], [0,0,0]]

# 控制输入量
self.u = cs.MX.sym("acceleration", 3*self.numAgent)

# # 建立动力学方程
# dynamic = cs.vertcat(0.5*self.u*dt*dt, self.u*dt)
dynamic = [0.5*self.u*dt*dt, self.u*dt]
self.f = cs.Function("function", [self.x, self.u], [cs.vcat(dynamic)], ["position", "acceleration"], ["dynamic"])

self.x_dot = cs.MX.sym("xdot", 60)
# self.f_expl_expr = cs.vertcat(self.velocity,  self.u)
# self.f_impl = self.f_expl_expr - self.x_dot

W_sep = 1
W_dir = 5
W_nav = 5
W_u = 4e-1

self.sym_sep = cs.MX.zeros(self.numAgent * self.max_neig, 1)
self.sym_dir = cs.MX.zeros(self.numAgent, 1)
self.sym_nav = cs.MX.zeros(self.numAgent, 1)

# self.sym_sep = np.array(self.numAgent * self.max_neig, 1)
# self.sym_dir = np.array(self.numAgent, 1)
# self.sym_nav = np.array(self.numAgent, 1)

# 距离障碍物最近距离
self.minDistObstacleAgent = cs.MX.zeros(self.numAgent, 1)
# 给出障碍物距离计算公式， 此处先假定障碍物在（10,10,3）
obstaclePosition = cs.MX.zeros(3, 1)
# obstaclePosition[0] = 10
# obstaclePosition[1] = 10
# obstaclePosition[2] = 3
for i in range(self.numAgent):
selfPosition = self.position[3 * i : 3 * i + 3]
dist = selfPosition-obstaclePosition
# selfPosition = np.array([self.position[3 * i], self.position[3 * i + 1], self.position[3 * i + 2]])
selfVelocity = np.array([self.velocity[3 * i], self.velocity[3 * i + 1], self.velocity[3 * i + 2]])
self.minDistObstacleAgent[i] = (dist[0]**2 + dist[1]**2 + dist[2]**2)**0.5

# agent之间距离
self.agentDist = cs.MX.zeros(int(self.numAgent*(self.numAgent-1)/2),1)
# 给出个体间距计算方法
index = 0
for i in range(self.numAgent):
selfPosition = np.array([self.position[3 * i], self.position[3 * i + 1], self.position[3 * i + 2]])
for j in range(i+1, self.numAgent, 1):
otherPosition = np.array([self.position[3 * j], self.position[3 * j + 1], self.position[3 * j + 2]])
self.agentDist[index] = self.calculateArrayLength(selfPosition - otherPosition)
index = index + 1

# 设置位置速度输入
initStatus = []
for i in range(60):
initStatus.append(random.randint(1, 6))

# self.runMPC(initStatus)

# 定义计算模长函数
def calculateArrayLength(self, array):
length = 0
for i in range(len(array)):
length += array[i]**2
return length**0.5

# cost function
def fitnessFunction(self):
for index in range(self.numAgent):
#规则后续再加
velocity = np.array([self.velocity[index*3], self.velocity[index*3]+1, self.velocity[index*3]+2])
systemDirection[index] = 1-(velocity*self.uReference)^2/(velocity*velocity)
return expr_y,expr_y_e

# constraints
def constraints(self):
systemDistance = cs.MX.zeros(self.numAgent*(self.numAgent-1)/2, 1)
systemObstacleAvoidence = cs.MX.zeros()
self.expr_h = cs.vertcat(self.u, systemDistance, systemObstacleAvoidence)

# constraints bounds

def optimizer(self):
ocpModel.name = "cl_swarming"
ocpModel.f_expl_expr = self.f(self.x, self.u)
x_dot = cs.MX.sym("x_dot", 60, 1)
ocpModel.f_impl_expr = x_dot - self.f(self.x, self.u)
ocpModel.x = self.x
ocpModel.xdot = x_dot
ocpModel.u = self.u
ocpModel.p = []

for i in range(self.numAgent):
selfPosition = np.array([self.position[3 * i, 0], self.position[3 * i + 1], self.position[3 * i + 2]])
selfVelocity = np.array([self.velocity[3*i], self.velocity[3*i+1], self.velocity[3*i+2]])
# neighborIndex = self.neighborIndex(i)
for j, index in enumerate(self.neighborIndex[i]):
neighborPosition = np.array([self.position[3 * index], self.position[3 * index + 1], self.position[3 * index + 2]])
dist = self.calculateArrayLength(selfPosition-neighborPosition)
self.sym_sep[i*self.max_neig+j] = (dist-self.distReference**2)/self.max_neig
self.sym_dir[i] = 1-self.calculateArrayLength(self.uReference*selfVelocity)/self.calculateArrayLength(selfVelocity)
a = self.calculateArrayLength(selfVelocity)
self.sym_nav[i] = self.calculateArrayLength(selfVelocity) - self.calculateArrayLength(self.vReference)

# ocpModel.cost_y_expr = cs.vertcat(self.sym_sep, self.sym_dir, self.sym_nav, self.u)
# ocpModel.cost_y_expr_e = cs.vertcat(self.sym_sep, self.sym_dir, self.sym_nav)

# ocpModel.T = self.predictionHorizon
# ocpModel.dimNx = 6*self.numAgent
# ocpModel.dimNu = 3*self.numAgent
# # ocpModel.dimNy = len(expr_y)
# # ocpModel.dimNye = len(expr_y_e)
# # ocpModel.dimNh = 3*self.numAgent# + 周围个体+周围障碍物
# # ocpModel.dimNhe = 3 * self.numAgent +
# ocpModel.symX = self.x
# ocpModel.symU = self.u
# ocpModel.symXdot = self.x_dot
# ocpModel.costType = 'nonlinear_ls'
# ocpModel.costTypeE = 'nonlinear_ls'
# # ocpModel.costExprY = expr_h
# # ocpModel.costExprYE =expr_h
# # ocpModel.costW = W
# # ocpModel.costWE = We
# # ocpModel.costYRef =
# # ocpModel.costYRefE =
# ocpModel.dynType = 'explicit'
# ocpModel.dynExprF = self.f_impl
# ocpModel.constraintsX0 = self.x0
# ocpModel.constraintsExprH = self.expr_h
# # ocpModel.constraintsLh =
# # ocpModel.constraintsUh =

ocp.model = ocpModel
ocp.dims.N = self.node
ocp.solver_options.tf = self.predictionHorizon
ocp.dims.np = 0
ocp.parameter_values = np.zeros(0)
# 设置约束条件与适应度函数
ocp.cost.cost_type = "NONLINEAR_LS"
ocp.cost.cost_type_e = "NONLINEAR_LS"
# 规则权重
w_spretion = 1
w_direction = 1
w_u = 4e-1
ocp.cost.W = np.diag(np.concatenate((w_spretion*np.ones(self.max_neig*self.numAgent),
w_direction*np.ones(self.numAgent),
w_u*w_spretion*np.ones(3*self.numAgent))))
ocp.cost.W_e = np.diag(np.concatenate((w_spretion * np.ones(self.max_neig*self.numAgent),
w_direction * np.ones(self.numAgent),
nx = 60
ny = 90
nu = 30
# ocp.cost.Vx = np.zeros((ny, nx))
# ocp.cost.Vx[:nx, :nx] = np.eye(nx)
# ocp.cost.Vu = np.zeros((ny, nu))
# ocp.cost.Vu[-30:, -30:] = np.eye(nu)
# ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref = np.zeros(self.max_neig*self.numAgent+self.numAgent+self.numAgent+3*self.numAgent)
ocp.cost.yref_e = np.zeros(self.max_neig*self.numAgent+self.numAgent+self.numAgent)
ocp.model.cost_y_expr = cs.vertcat(self.sym_sep, self.sym_dir, self.sym_nav, self.u)
ocp.model.cost_y_expr_e = cs.vertcat(self.sym_sep, self.sym_dir, self.sym_nav)
# ocp.cost.yref = cs.vertcat(self.sym_sep, self.sym_dir, self.sym_nav, self.u)
# ocp.cost.yref_e = cs.vertcat(self.sym_sep, self.sym_dir, self.sym_nav)

ocp.constraints.x0 = np.zeros(nx)
lhU = -self.maxA*np.ones(nu)
uhU = self.maxA*np.ones(nu)
uhAgentColl = 300*300*np.ones(int(self.numAgent*(self.numAgent-1)/2))
uhObstacleColl = np.array([1e16]*self.numAgent)
ocp.constraints.lh = np.concatenate((lhU, lhAgentColl, lhObstacleColl))
ocp.constraints.uh = np.concatenate((uhU, uhAgentColl, uhObstacleColl))
ocp.model.con_h_expr = cs.vertcat(self.u, self.agentDist, self.minDistObstacleAgent)
ocp.constraints.x0 = np.array([0]*self.numAgent*6)
# ocp.parameter_values = np.array([0])

ocp.solver_options.nlp_solver_type = 'SQP'
ocp.solver_options.regularize_method = "NO_REGULARIZE"

# ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'  # FULL_CONDENSING_QPOASES
# ocp.solver_options.hessian_approx = 'EXACT'  # GAUSS_NEWTON, EXACT
# ocp.solver_options.regularize_method = 'CONVEXIFY'  # GAUSS_NEWTON, EXACT
# ocp.solver_options.integrator_type = 'ERK'
# ocp.solver_options.print_level = 0
# ocp.solver_options.nlp_solver_type = 'SQP'  # SQP_RTI, SQP

ocp.solver_options.nlp_solver_max_iter = 4
ocp.solver_options.nlp_solver_tol_stat = 1.0
ocp.solver_options.nlp_solver_tol_eq = 1e-1
ocp.solver_options.nlp_solver_tol_ineq = 1e-2
ocp.solver_options.nlp_solver_tol_comp = 1e-1
ocp.solver_options.nlp_solver_step_length = 0.05
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
ocp.solver_options.qp_solver_iter_max = 10
ocp.solver_options.qp_solver_cond_N = int(self.node/2)
ocp.solver_options.qp_solver_warm_start = 0
ocp.solver_options.integrator_type = "ERK"
ocp.solver_options.sim_method_num_stages = 4
ocp.solver_options.sim_method_num_steps = 3

# ocp.solver_options.integrator_type = "IRK"  # ERK, IRK, GNSF
# ocp.solver_options.sens_forw = True
# ocp.solver_options.sens_hess = True
# ocp.solver_options.sens_algebraic = False
# ocp.solver_options.output_z = False
# ocp.solver_options.jac_reuse = False

# def setState(self, x):

def neighborIndex(self, index):
rank = list()
selfPosition = np.array([self.position[3*index], self.position[3*index+1], self.position[3*index+2]])
for i in range(self.numAgent):
if i != index:
pos = np.array([self.position[3*i], self.position[3*i+1], self.position[3*i+2]])
distance = self.calculateArrayLength(selfPosition-pos)
rank.append((i, distance))
rank.sort(key=lambda x:x[1])
neighborIndex = []
for i in range(self.max_neig):
neighborIndex.append(rank[i][0])
return neighborIndex

def runMPC(self, initState):
u_init = np.array([0]*30)
x_init = initState
x_init = np.stack(x_init)

for i in range(self.node):

``````

initx = np.array([1]*60)
print(acceleration)

Are there anybody could help me?

Cheers,

simple

Actually, I’m converting others matlab code to python.And then,I want to make some improvements.This is original matlab code:https://zenodo.org/record/4379503.The key code are:swarming_core/cl_init.m and mpc_functions/swarming_model_max_neig.m

Hello @simple,

Did you manage to get your code running? and did you find the error?