Get error using python for NMPC

User Questions
1

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 casadi as cs
import numpy as np
from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
import random
import math

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

    self.numAgent = 10
    self.max_neig = 3
    self.maxA = 1
    self.collisionRadiusAgent = 0.1
    self.collisionRadiusObstacles = 0.2

    # 邻域个体
    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

    self.acadosOcpSolver = self.optimizer()

    # 设置位置速度输入
    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):
    systemDirection, systemNavigation = list
    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)
        systemNavigation[index] = velocity*velocity-self.vReference^2
    expr_y = cs.vertcat(systemDirection, systemNavigation)
    expr_y_e = cs.vertcat(systemDirection, systemNavigation)
    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 = AcadosModel()
    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 = AcadosOcp()
    acados_source_path = os.environ['ACADOS_SOURCE_DIR']
    ocp.acados_include_path = acados_source_path + '/include'
    ocp.acados_lib_path = acados_source_path + '/lib'
    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_navigation = 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_navigation*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),
                                        w_navigation * 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)
    lhAgentColl = 4*self.collisionRadiusAgent*self.collisionRadiusAgent*np.ones(int(self.numAgent*(self.numAgent-1)/2))
    uhAgentColl = 300*300*np.ones(int(self.numAgent*(self.numAgent-1)/2))
    lhObstacleColl = np.array([self.collisionRadiusObstacles*self.collisionRadiusObstacles]*self.numAgent)
    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_adj = 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

    return AcadosOcpSolver(ocp)
    
# 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)
    self.acadosOcpSolver.set(0, 'lbx', x_init)
    self.acadosOcpSolver.set(0, 'ubx', x_init)
    # self.acadosOcpSolver.set(0, 'x', x_init)
    # self.acadosOcpSolver.set(0, 'u', u_init)
    self.acadosOcpSolver.solve()

    uOptAcados = np.ndarray((self.node, 30))
    xOptAcados = np.ndarray((self.node, 60))
    for i in range(self.node):
        xOptAcados[i, :] = self.acadosOcpSolver.get(i, "x")
        uOptAcados[i, :] = self.acadosOcpSolver.get(i, "u")

    uOptAcados = np.reshape(uOptAcados, (-1))

    return uOptAcados

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

Are there anybody could help me?

Cheers,

simple

2

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

3

Hello @simple,

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

4

Helle,thanks for your reply.My question has been solved.

5

I’m trying to implement something similar but still stuck with the same error. Could you provide any hints? or the code?