i occur this problem as follow:
[MPC_SLOVER]
MPC Executing time: 0.003225s
MPC status: 0
State_x = 4.000000 State_y = 1.000000 State_angle = 1.570796
Refernce_x = 4.500000 Refernce_y = 2.000000 Refernce_angle = 0.000000
Short_x = 4.500000 Short_y = 1.000000 Short_angle = 0.000000
Linear_X = 0.306735 Angular_Z = -0.500000
Distance to short = 0.500000
Distance to target = 1.118034
Theta difference = 1.570796
**QP solver returned error status 3 in SQP iteration 2, QP iteration 4.**
Error status: 4
[MPC_SLOVER]
MPC Executing time: 0.001433s
MPC status: 4
State_x = 4.064686 State_y = 1.271739 State_angle = 1.111437
Refernce_x = 4.500000 Refernce_y = 2.250000 Refernce_angle = 0.000000
Short_x = 4.500000 Short_y = 1.250000 Short_angle = 0.000000
Linear_X = 0.000000 Angular_Z = 0.000000
Distance to short = 0.435856
Distance to target = 1.070744
Theta difference = 1.111437
and this my ocp.py
#!/usr/bin/env python3
from model import export_spot_model
from casadi import *
from acados_template import AcadosOcp, AcadosOcpSolver, ocp_get_default_cmake_builder
import numpy as np
import scipy.linalg
import math
def gen_model(prediction_steps=10, horizon_duration=1,
max_v=0.6, min_v=-0.6,
max_w=0.5, min_w=-0.5,
angle_threshold=0.05, distance_threshold=0.03):
model = export_spot_model(horizon_duration/prediction_steps)
ocp = AcadosOcp()
ocp.model = model
ocp.dims.N = prediction_steps
ocp.solver_options.tf = horizon_duration
# 狀態變數
robot_x = ocp.model.x[0]
robot_y = ocp.model.x[1]
robot_th = ocp.model.x[2]
# 目標點變數
ref_x = ocp.model.p[0]
ref_y = ocp.model.p[1]
ref_th = ocp.model.p[2]
# 直線方程參數
shortest_x = ocp.model.p[3]
shortest_y = ocp.model.p[4]
shortest_th = ocp.model.p[5]
# 計算與目標點的誤差
error_x = ref_x - robot_x
error_y = ref_y - robot_y
error_th = ref_th - robot_th
# 修正角度誤差到 [-pi, pi] 範圍內
# error_th = atan2(sin(error_th), cos(error_th))
# error_th = pi * ((sin(error_th))**2) # 這是你提到的部分的修正
error_th = math.pi * ((1 - cos(error_th))/2)
# 將誤差整合到一個向量中
error = vertcat(error_x, error_y, error_th)
Q = np.array([[0.5, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) # State weight
J_error = error.T@Q@error
R = np.array([[0.5, 0.0], [0.0, 0.5]]) # Control weight
J_control = ocp.model.u.T@R@ocp.model.u
# W = scipy.linalg.block_diag(Q, R)
J_cost = (J_error + J_control)/2
J_cost_e = (J_error)/2
ocp.cost.cost_type = 'EXTERNAL'
ocp.model.cost_expr_ext_cost = J_cost
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost_e = J_cost_e
# set constraints
ocp.constraints.lbu = np.array([min_v, min_w])
ocp.constraints.ubu = np.array([max_v, max_w])
ocp.constraints.idxbu = np.array([0, 1])
# ocp.constraints.constr_exp_h = model.con_h_expr
# ocp.constraints.lh = np.array([0.0])
# ocp.constraints.uh = np.array([999.0])
# ocp.constraints.lh_e = np.array([0.0])
# ocp.constraints.uh_e = np.array([999.0])
# ocp.constraints.uphi = np.array([999])
# ocp.constraints.lphi = np.array([0.0])
ocp.constraints.x0 = np.array([0, 0, 0]) # must set so you can set initial state
ocp.parameter_values = np.array([0, 0, 0, 0, 0, 0])
# solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'EXACT'
ocp.solver_options.integrator_type = 'DISCRETE'
ocp.solver_options.print_level = 0
ocp.solver_options.nlp_solver_max_iter = 100
ocp.solver_options.nlp_solver_type = 'SQP'
# ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
# ocp_get_default_cmake_builder()
AcadosOcpSolver(ocp, json_file = ocp.model.name + '_acados_ocp.json')
if __name__ == '__main__':
gen_model()
anyone can help me ?