Quadcopter MPC with External Cost, HPIPM Returns NaN

User Questions
1

I keep getting SQP_RTI: QP solver returned error status 3 QP iteration 1 when using ocpSolver.solve(), after setting the initial state value and the reference by calling ocpSolver.set(i, 'p', y[:, i]) and ocpSolver.set(0, 'x', x).

I think something must be wrong but I just have trouble finding the spot. Thanks in advance for pointing out errors.

To reproduce:

  1. clone the repo: GitHub - ErcBunny/learn_acados
  2. export acados path and run python3 simplequad.py
  3. the expected output is
model summary
m = 1
J = @1=1, @2=0, 
[[@1, @2, @2], 
 [@2, @1, @2], 
 [@2, @2, @1]]
x = [p_0, p_1, p_2, q_0, q_1, q_2, q_3, v_0, v_1, v_2, w_0, w_1, w_2]
u = [f_0, f_1, f_2, t_0, t_1, t_2]
p = [p_ref_0, p_ref_1, p_ref_2, q_ref_0, q_ref_1, q_ref_2, q_ref_3, v_ref_0, v_ref_1, v_ref_2, w_ref_0, w_ref_1, w_ref_2]
================= generate ocp solver c code =================
*
================= generate sim solver c code =================
*
================= test sim solver =================
t = 0.1
[ 0.          0.         -0.005       0.99999688  0.          0.          0.0025      0.          0.         -0.1         0.          0.          0.1       ]
t = 1.0
[ 0.          0.         -0.5         0.96891242  0.          0.          0.24740396  0.          0.         -1.          0.          0.          1.        ]
t = 10.0
[  0.           0.         -50.           0.99120279   0.           0.          -0.13235159   0.           0.         -10.           0.           0.          10.        ]
================= test ocp solver =================
x = [0 0 0 1 0 0 0 0 0 0 0 0 0]
ref =
[[ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [ 0.         -0.00615583 -0.02447174 -0.05449674 -0.0954915  -0.14644661 -0.20610737 -0.27300475 -0.3454915  -0.42178277 -0.5        -0.57821723 -0.6545085  -0.72699525 -0.79389263 -0.85355339 -0.9045085  -0.94550326 -0.97552826 -0.99384417 -1.        ]
 [ 1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.          1.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [-0.         -0.24572668 -0.48540276 -0.71312661 -0.92329092 -1.11072073 -1.27080092 -1.39958978 -1.49391608 -1.55145722 -1.57079633 -1.55145722 -1.49391608 -1.39958978 -1.27080092 -1.11072073 -0.92329092 -0.71312661 -0.48540276 -0.24572668 -0.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]
 [ 0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.          0.        ]]

SQP_RTI: QP solver returned error status 3 QP iteration 1.
OCP_SOL: ocp_status = 4 , exiting

Things I’ve tried but not working:

  • tweaking the initial value
  • tweaking the cost matrices
  • changing the reference trajectory
  • changing solver settings like ERK/IRK and the condensing method
2

Hi,

I had a brief look at your code and saw that you don’t specify an initial state.

Note that this:
ocpSolver.set(0, 'x', x) (learn_acados/simplequad.py at 5ad362d50103147e86aa32fa246b2038799c1ae7 · ErcBunny/learn_acados · GitHub)

Does NOT set the initial state constraint, but the initialization of x at shooting node 0.

Your code works when adding:

ocp.constraints.x0 = np.array([0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0])

You can update the initial state constraint like here: acados/minimal_example_closed_loop.py at master · acados/acados · GitHub

Here is what I did:

Cheers!

4

Wow, thanks!
Will try it out!
Is there an advantage of partial condensing over full condensing? What about IRK and ERK?