giaf / hpmpc

Library for High-Performance implementation of solvers for MPC.
GNU Lesser General Public License v2.1
53 stars 34 forks source link

Unsolvable QP (0 iterations, return code -1) #9

Closed mkatliar closed 8 years ago

mkatliar commented 8 years ago

The QP data passed to c_order_d_ip_ocp_hard_tv() are in the failed_qp.m file (see qp.zip). The following code converts the QP to CasADi nlp and solves it in MATLAB:

clear;
failed_qp;
[nlp, b, x0] = hpmpc_to_casadi(qp);
options.ipopt.linear_solver = 'ma86';
solver = casadi.nlpsol('NLPSolver', 'ipopt', nlp, options);
sol = solver('lbx', b.lbx, 'ubx', b.ubx, 'lbg', b.lbg, 'ubg', b.ubg)

Output:

This is Ipopt version 3.12.4, running with linear solver ma86.

Number of nonzeros in equality constraint Jacobian...:      104
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:       64

Total number of variables............................:       15
                     variables with only lower bounds:        0
                variables with lower and upper bounds:       15
                     variables with only upper bounds:        0
Total number of equality constraints.................:       13
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:        0

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
   0 -1.5718507e-06 1.05e-05 8.05e-05  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
   1 -7.1895982e-05 2.26e-09 1.34e-06  -1.0 1.84e-01    -  9.90e-01 1.00e+00f  1
   2 -6.4388769e-03 7.41e-10 1.97e-05  -1.0 1.62e+01    -  9.70e-01 1.00e+00f  1
   3 -3.3171558e-01 1.56e-08 1.35e-03  -1.0 1.13e+03    -  2.65e-01 1.00e+00f  1
   4 -5.2942863e-01 1.46e-08 1.18e-04  -1.0 6.99e+02    -  7.43e-01 1.00e+00f  1
   5 -7.4800557e-01 2.26e-09 8.37e-05  -1.7 1.13e+03    -  7.48e-01 1.00e+00f  1
   6 -8.6505679e-01 3.26e-09 9.71e-05  -2.5 1.17e+03    -  7.99e-01 1.00e+00f  1
   7 -8.7635189e-01 3.62e-09 1.74e-16  -2.5 4.42e+02    -  1.00e+00 1.00e+00f  1
   8 -8.7658396e-01 3.68e-09 2.93e-16  -3.8 7.23e+01    -  1.00e+00 1.00e+00f  1
   9 -8.7658433e-01 3.68e-09 3.82e-16  -5.7 3.14e+00    -  1.00e+00 1.00e+00h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  10 -8.7658433e-01 3.68e-09 3.30e-16  -8.6 5.19e-03    -  1.00e+00 1.00e+00h  1

Number of Iterations....: 10

                                   (scaled)                 (unscaled)
Objective...............:  -8.7658432596688318e-01   -8.7658432596688318e-01
Dual infeasibility......:   3.2992956828165456e-16    3.2992956828165456e-16
Constraint violation....:   3.6843180616129157e-09    3.6843180616129157e-09
Complementarity.........:   2.5105616195963237e-09    2.5105616195963237e-09
Overall NLP error.......:   3.6843180616129157e-09    3.6843180616129157e-09

Number of objective function evaluations             = 11
Number of objective gradient evaluations             = 11
Number of equality constraint evaluations            = 11
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 11
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 10
Total CPU secs in IPOPT (w/o function evaluations)   =      0.080
Total CPU secs in NLP function evaluations           =      0.012

EXIT: Optimal Solution Found.
                   proc           wall      num           mean             mean
                   time           time     evals       proc time        wall time
        nlp_f     0.001 [s]      0.000 [s]    11       0.06 [ms]        0.01 [ms]
        nlp_g     0.000 [s]      0.000 [s]    11       0.04 [ms]        0.01 [ms]
   nlp_grad_f     0.001 [s]      0.000 [s]    12       0.08 [ms]        0.02 [ms]
    nlp_jac_g     0.001 [s]      0.000 [s]    12       0.06 [ms]        0.02 [ms]
   nlp_hess_l     0.006 [s]      0.002 [s]    10       0.60 [ms]        0.15 [ms]
 all previous     0.009 [s]      0.002 [s]
callback_prep     0.000 [s]      0.000 [s]    11       0.04 [ms]        0.01 [ms]
       solver     0.098 [s]      0.026 [s]
     mainloop     0.107 [s]      0.028 [s]

sol = 

        f: [1x1 casadi.DM]
        g: [13x1 casadi.DM]
    lam_g: [13x1 casadi.DM]
    lam_p: [0x0 casadi.DM]
    lam_x: [21x1 casadi.DM]
        x: [21x1 casadi.DM]

On the same problem c_order_d_ip_ocp_hard_tv() returns -1 and *kk == 0.

To my best knowledge, the problem is feasible and the data are well-formed.

mkatliar commented 8 years ago

Before exiting, hpmpc prints:

5178.71808   0.00013   0.10000      -nan 

What are these numbers and why is a nan there?

giaf commented 8 years ago

The solver worked with the AVX target, and failed with the SSE3 and C99 targets. Now the bug is solved.

For next time, please specify the target you are using. Also please report the solution you get form other solvers as reference, the above output from IPOPT gives me no useful information beside the fact that it converged.

Finally, please send me the test problem in a format I can run directly (i.e. a test problem in C instead of a bunch of matrices from Matlab), instead of spending a lot of time in converting between different languages, with the risk of solving something different.

[The numbers in your second comment are from a print of the infinity norm of residuals that we added together for debugging in the wrapper, you can remove them.]