Max iter always reached on humanoid MPC problem

[stephane-caron]

I'm comparing QP solvers on a classical humanoid model predictive control problem.

• Code: model_predictive_control.py.
• QP matrices it produces: humanoid_mpc_matrices.zip (names are for a call to qpSWIFT.run(q, h, P, G, opts=opts))
• qpSWIFT's output: qpSWIFT-output.txt

Other solvers handle this problem fine, but qpSWIFT always fails with exit code 2 (MAXITER reached).

The default setting for this parameter is 100 iterations. I have tried setting it to its allowed maximum of 199, but the solver still fails. If we look at the last iterations, primal and dual step sizes oscillate:

It: 168 || pcost : 1.153087e+03 || rx:7.793081e+00   ||  ry:0.000000e+00 ||  rz:1.414214e+03 || mu:2.652561e-47
      || Primal Step Size : 0.503682 || Dual Step Size   : 0.000000
...
It: 190 || pcost : 2.567155e+03 || rx:2.370485e+01   ||  ry:0.000000e+00 ||  rz:1.414214e+03 || mu:4.891264e-44
      || Primal Step Size : 0.000263 || Dual Step Size   : 0.000263
...
It: 198 || pcost : 2.632743e+03 || rx:2.229404e+01   ||  ry:0.000000e+00 ||  rz:1.414214e+03 || mu:3.183775e-57
      || Primal Step Size : 0.059123 || Dual Step Size   : 0.060551

So, it seems qpSWIFT does not converge on this problem? Maybe the default tolerance settings are too tight? :thinking:

[abhishek-pandala]

I have noticed that the matrix G provided in the humanoid_mpc_matrices.zip file is not of full row-rank. G matrix has the first two rows entirely zero. This makes the LDL solver unstable. Removing the first two rows in the G matrix makes the problem solvable.

qpSWIFT does not perform input checking on the matrices provided. The inputs are assumed to be in the correct format and are well-conditioned. For example, P is symmetric positive definite (at least symmetric positive semi-definite), and matrices corresponding to the equality constraints are full row-rank. It is also not very advisable to have completely zero rows in the G matrix as this can lead to numerical instability issues. This decision was made intentionally to reduce the time needed to perform input checking, which can be critical when running real-time applications.

Thank you for pointing this out. We will update the documentation highlighting this point

[stephane-caron]

Thanks for taking a look. I've updated the doc of qpswift_solve_qp to mention the full-row-rank requirement, and the benchmark table now that we can measure qpSWIFT's performance on this example.

[stephane-caron]

and matrices corresponding to the equality and inequality constraints are full row-rank

According to your paper, the full row-rank requirement only applies to the equality-constraint matrix, right?

The full row-rank condition would not make sense here because the inequality matrix is 32×16 (30×16 after removing the two rows of zeros) and its rank is only 15 (its nullspace is the line directed by [0 0 ... 0 1]).

[abhishek-pandala]

qpSWIFT solves QP by forming a KKT matrix which is of the form K = [P A' G' A 0 0 G 0 -I]; P == cost function, A == equality constraint matrix, G == inequality constraint matrix

This system of equations is solved using a simple LDL solver that does not account for numerical instabilities that can arise when factorizing ill-conditioned matrices. This situation arises when the rank(A) < p (== rows of A) and rank([P A' G']) ~= n. For more insight, please refer to the second page on rank assumptions. In either case, having completely zero rows in the matrix A, G can be bad. We recommend avoiding these completely.

Commerical solvers and solvers embedded into scripting languages like matlab and python perform many error checkings before computing the iterations. Furthermore, solvers that use first-order optimization methods do not suffer from these issues as they only involve matrix-vector computations and not matrix inversions.

Suggestion: If you set the max-iteration of qpSWIFT to say 40 (based on experience), you will find that the solution obtained is very much usable in practical scenarios even if there are numerical instabilities, infeasibility or max-iteration issues. Refer to link for practical applications of qpSWIFT.

[stephane-caron]

Thank you for this pointer and the rank assumptions :ok_hand: I've updated the doc accordingly.

In this failure case (original G with rows of zeros) the failure does not seem related to rank. If we run:

    K = np.bmat([[P, G.T], [G, -np.eye(G.shape[0])]])
    print(f"n = {P.shape[1]}")
    print(f"rank([P G^T]) = {np.linalg.matrix_rank(np.hstack([P, G.T]))}")
    print(f"K.shape = {K.shape}")
    print(f"rank(K) = {np.linalg.matrix_rank(K)}")

We get:

n = 16
rank([P G^T]) = 16
K.shape = (48, 48)
rank(K) = 48

So the cause is likely something else stemming from the zero rows in G.

having completely zero rows in the matrix A, G can be bad. We recommend avoiding these completely.

Makes sense, but zero rows is probably just a particular case of a more general requirement of qpSWIFT's LDL solver. (I've tried scipy.linalg.ldl on K when G has rows of zeros and it factorizes it alright.) Can we figure out what this general requirement is?

It would be useful information for users, so that they can check that their problem satisfies it before throwing it at the solver, or debug their use case if they see the solver failing and want to figure out why.