QpOASES inscrutable Errors

[ElliotWinterbottom]

Hiya all.
I'm using the DQ robotics QpOASES interface to try and implement model predictive control. However, I keep getting errors thrown by QpOASES itself. Googling these errors doesn't seem to help at all as:

1. Any googling of these errors leads straight to the class reference for the error handling portion of QpOASES
2. The github page has largely been unresponsive for a few years now.

This is the error I'm getting when I run my code.

ERROR:  All bounds are active, no further bound can be added
->ERROR:  Failed to setup working set for auxiliary QP
  ->ERROR:  Failed to setup auxiliary QP for initialised homotopy
    ->ERROR:  Failed to setup auxiliary QP for initialised homotopy
terminate called after throwing an instance of 'std::runtime_error'
  what():  DQ_QPOASESSolver::solve_quadratic_program(): Unable to solve quadratic program.

However, I don't know what this error means and do not know how I should change my approach to fix it. Any suggestions are on how I could approach this are welcome including but not limited to:

• Other better supported solvers I could use.
• Documentation for the errors shown that do not just repeat the text of the error.
• Insight into this specific error and how I might solve it.

I am relatively new to a large portion of what I am doing here so if I have made an obvious mistake or have posted this to the wrong place please let me know and I'll do my best to fix it.
Thanks,
Elliot.

Edit: @mmmarinho Fixed the formatting of the output.

[bvadorno]

Hi @ElliotWinterbottom,

Welcome to the DQ Robotics community! The place is adequate, but more information is needed. Don't worry, our community will help you get up to speed. @juanjqo, could you please help @ElliotWinterbottom?

Kind regards,
Bruno

[ElliotWinterbottom]

Hiya @mmmarinho
You are correct about the -+ bounds. I'm using a slightly older version of the interface which meant that I couldn't use lb, ub, and Alb directly so I made do and haven't updated it (for lack of time and fear of breaking something else). That last part of $u$ is not a joint velocity but a trajectory scaling factor which reduces the required speed as to avoid deformation of the trajectory. 1 is no scaling and 0 is no movement. To keep things from fully halting unrecoverably I've set it to just a little above zero hence the $0.1$.
Here is the paper I'm loosely working from if it helps:
Predictive_Inverse_Kinematics_for_Redundant_Manipulators_With_Task_Scaling_and_Kinematic_Constraints.pdf
Thanks again,
Elliot.

[ElliotWinterbottom]

To add a little:
the optimisation variable I'm using looks something like:
$\underline{x} = [\underline{u}^T,s] = [u_1,u_2,u_3,u_4,s]^T $
What makes this problem even more frustrating is that it has no issues when run on a 2DoF planar example.

[bvadorno]

Thanks for the additional information, @ElliotWinterbottom.

The lower bound for $s$ cannot be 0.1. It must be 0. Feasibility is only ensured when the solution $[\dot{\boldsymbol{q}}^T , s]^T = \boldsymbol{0}$ is admissible.

Kind regards,
Bruno

[ElliotWinterbottom]

Thanks @bvadorno .
After making that change I'm getting the following error on the next iteration

ERROR:  Projected Hessian matrix not positive definite
terminate called after throwing an instance of 'std::runtime_error'
  what():  DQ_QPOASESSolver::solve_quadratic_program(): Unable to solve quadratic program.

Is this likely to mean my objective function is still incorrect?
Thanks again,
Elliot.

[bvadorno]

Hi, @ElliotWinterbottom.

Is this likely to mean my objective function is still incorrect?

It can be, but not necessarily. If the problem is ill-conditioned for whatever reason, it might generate very large control inputs, which in turn might affect further iterations. For the moment, it's hard to know because I don't know what the output of the first iteration is, nor what the Hessian in the second iteration looks like.

How many iterations would the solver run successfully when the lower bound of $s$ was 0.1?

Kind regards,
Bruno

[ElliotWinterbottom]

Hiya @bvadorno
When the lower bound is zero it crashes on the the 4th iteration.
When the lower bound is 0.1 it crashes on the 3rd iteration.
In both cases the value of the control input gets smaller over the number of iterations.
I can provide exact hessians and previous control inputs if necessary.
Thanks again,
Elliot.

[bvadorno]

Please provide the control inputs and the eigenvalues of the Hessians at each iteration.

Many thanks,
Bruno

[ElliotWinterbottom]

Hiya @bvadorno.
I've actually managed to get a little further by having a look at my objective function and changing a 1 to a -1. I'm now getting to iteration 6 at which the following error is produced:

ERROR:  Division by zero
->ERROR:  Abnormal termination due to TQ factorisation
  ->ERROR:  Determination of step direction failed
    ->ERROR:  Abnormal termination due to TQ factorisation
terminate called after throwing an instance of 'std::runtime_error'
  what():  DQ_QPOASESSolver::solve_quadratic_program(): Unable to solve quadratic program

The Hessians and control inputs are available here along with some other hopefully useful data
hessain_and_control_input.txt

Thanks again,
Elliot.

[bvadorno]

I'm sorry, @ElliotWinterbottom, but you'll need to provide data in a way that is easily parseable; otherwise, it's unlikely you'll get useful help (or people who are willing to help at all).

I asked you to provide the control signal and the eigenvalues of the Hessian matrices at each iteration. The file you sent is not quickly parseable and doesn't contain the eigenvalues of the Hessian matrices. You need to calculate it yourself (numpy certainly has available functions for it).

I've actually managed to get a little further by having a look at my objective function and changing a 1 to a -1. I'm now getting to iteration 6 at which the following error is produced:

Would that mean that you found an error in your implementation?

Kind regards,
Bruno

[juanjqo]

Hi @ElliotWinterbottom,

It looks like the problem is non-convex (the matrix H you are using is not positive definite).

I tried to solve the problem using quadprog in Matlab.

minimal example:

clear all
close all
clc

A = [eye(5,5); -eye(5,5)];
b = [148,148,148,148,1,148,148,148,148,-0.1]';

f= [0,0,0, 0,2000]';

H = [1.03859e+06,682318 ,248958,-185255, -4.8028e+07;
        682318,576044,370956, 101739,7.36292e+06;
        248958,      370956,      415868,     373893, 3.15979e+07;
        -185255,     101739,     373893,      581917, 1.96213e+07;
        -4.8028e+07, 7.36292e+06, 3.15979e+07, 1.96213e+07, 3.57663e+10];

options = optimoptions('quadprog','Display','iter', 'Algorithm','interior-point-convex');


[x,fval,exitflag,output,lambda] = quadprog(H, f, A, b, [], [], [], [],[], options)

Output

Solved 0 variables, 0 equality, and 10 inequality constraints during the presolve.

The problem is non-convex.


x =

   -0.0010
    0.0010
    0.0006
   -0.0008
    1.5800


fval =

   4.4644e+10


exitflag =

    -6


output = 

  struct with fields:

            message: 'The problem is non-convex.'
          algorithm: 'interior-point-convex'
      firstorderopt: 5.6511e+10
    constrviolation: 0.5800
         iterations: 0
       linearsolver: 'dense'
       cgiterations: []


lambda = 

  struct with fields:

    ineqlin: [10×1 double]
      eqlin: [0×1 double]
      lower: [5×1 double]
      upper: [5×1 double]

Checking the matrix H

source

>> chol(H)

Error using chol
Matrix must be positive definite.

If you are trying to solve a non-convex problem, maybe proxsuite could be an alternative. For instance,

Caution

The class DQ_PROXQPSolver.h is not an official DQ Robotics class and is experimental.

#include <iostream>

#include <dqrobotics/DQ.h>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
#include <dqrobotics/solvers/DQ_PROXQPSolver.h>
#include <memory>

using namespace DQ_robotics;


/**
 * @brief solve_and_show_data solve using the solver list
 * @param solver_names The list of the solver names to show
 * @param solvers the list of the solvers
 * @param H the n x n matrix of the quadratic coeficitients of the decision variables.
 * @param f the n x 1 vector of the linear coeficients of the decision variables.
 * @param A the m x n matrix of inequality constraints.
 * @param b the m x 1 value for the inequality constraints.
 * @param Aeq the m x n matrix of equality constraints.
 * @param beq the m x 1 value for the inequality constraints.
 */
void solve_and_show_data(const std::vector<std::string>& solver_names,
                         const std::vector<std::shared_ptr<DQ_QuadraticProgrammingSolver>>& solvers,
                         const Eigen::MatrixXd& H,
                         const Eigen::VectorXd& f,
                         const Eigen::MatrixXd& A,
                         const Eigen::VectorXd& b,
                         const Eigen::MatrixXd& Aeq,
                         const Eigen::VectorXd& beq);

int main()
{
    Eigen::Matrix<double,5,5> H;
    Eigen::Vector<double,5> f;
    Eigen::Matrix<double,10,5> A;
    Eigen::Vector<double,10> b;
    Eigen::MatrixXd Aeq; // empty equality constraints
    Eigen::VectorXd beq;

    A << MatrixXd::Identity(5,5), -MatrixXd::Identity(5,5);
    b << 148,148,148,148,1,148,148,148,148,-0.1;

    std::vector<std::string> solver_names = {"ProxQP", "qpOASES"};

    std::vector<std::shared_ptr<DQ_QuadraticProgrammingSolver>>
        solvers = {std::make_shared<DQ_PROXQPSolver>(), std::make_shared<DQ_QPOASESSolver>()};


    H<<1.03859e+06,682318 ,248958,-185255, -4.8028e+07,
        682318,576044,370956, 101739,7.36292e+06,
        248958,      370956,      415868,     373893, 3.15979e+07,
        -185255,     101739,     373893,      581917, 1.96213e+07,
        -4.8028e+07, 7.36292e+06, 3.15979e+07, 1.96213e+07, 3.57663e+10;
    f << 0,0,0, 0,2000;

    solve_and_show_data(solver_names,solvers,H,f,A,b,Aeq,beq);

    H<< 1.00041e+06     ,  701754     ,  270534     , -151405     ,   -4.74162e+07,
        701754            ,  653099     ,  410488     ,  115582     ,   1.73432e+07,
        270534            ,  410488     ,  417877     ,  360139     ,   3.09511e+07,
        -151405            ,  115582     ,  360139     ,  552401     ,   1.76042e+07,
        -4.74162e+07      ,  1.73432e+07,  3.09511e+07,  1.76042e+07,   3.4999e+10;

    solve_and_show_data(solver_names,solvers,H,f,A,b,Aeq,beq);

    H <<       994130  ,     703431  ,     273671 ,     -145626, -4.74491e+07,
        703431  ,     662732  ,     416530 ,      117741,  1.80003e+07,
        273671  ,     416530  ,     420327 ,      358725,  3.09832e+07,
        -145626 ,      117741 ,      358725,       547122,  1.72701e+07,
        -4.74491e+07,  1.80003e+07,  3.09832e+07,  1.72701e+07,  3.43859e+10;

    solve_and_show_data(solver_names,solvers,H,f,A,b,Aeq,beq);

    return 0;
}


void solve_and_show_data(const std::vector<std::string>& solver_names,
                         const std::vector<std::shared_ptr<DQ_QuadraticProgrammingSolver>>& solvers,
                         const Eigen::MatrixXd& H,
                         const Eigen::VectorXd& f,
                         const Eigen::MatrixXd& A,
                         const Eigen::VectorXd& b,
                         const Eigen::MatrixXd& Aeq,
                         const Eigen::VectorXd& beq)
{
    std::cout<<"--------------------"<<std::endl;
    int i = 0;
    for (auto& solver : solvers)
    {
        try {
            std::cout<<solver_names.at(i)<<": "<<solver->solve_quadratic_program(H, f,A,b,Aeq,beq).transpose()<<std::endl;
        } catch (const std::exception& e) {
            std::cout<<"Failed to solve! "<<e.what()<<std::endl;
        }
        i++;
    }
    std::cout<<"--------------------"<<std::endl;
}

output:

--------------------
ProxQP:   91.9142      -148    54.116   16.9945 0.0999905
qpOASES: 91.9179    -148 54.1071 17.0011     0.1
--------------------
--------------------
ProxQP:  37.9425  2.38489 -91.6269  66.4193 0.100962
qpOASES:  37.5809  2.36216 -90.7537  65.7864      0.1
--------------------
--------------------
ProxQP:   50.098  -46.171  -26.011  37.1316 0.101162
qpOASES: Failed to solve! DQ_QPOASESSolver::solve_quadratic_program(): Unable to solve quadratic program.
--------------------

Kind regards,

Juan

[ElliotWinterbottom]

Thank you both @juanjqo and @bvadorno and apologies Dr Adorno.
Here is a clearer output with the Hessian, it's Eigenvalues, and the previous control action at each iteration
hessain_and_control_input (1).txt
I had found an error in my implementation. In the QP in form $x^THx + f^Tx$ I had instead asked for $x^THx - f^Tx$ and was a result getting an ill posed problem. The error I'm getting now this one as mentioned above:

ERROR:  Division by zero
->ERROR:  Abnormal termination due to TQ factorisation
  ->ERROR:  Determination of step direction failed
    ->ERROR:  Abnormal termination due to TQ factorisation
terminate called after throwing an instance of 'std::runtime_error'
  what():  DQ_QPOASESSolver::solve_quadratic_program(): Unable to solve quadratic program

The only mention of this error I can find elsewhere is from this blog post which is having similar issues and solved it by setting some of the settings of the solver. Having followed these steps I am still getting the error.
Looking at the eigenvalues Hessian above there are some negative values but not in the step before it crashes. Is this still an issue?
I've also noticed that changing the gain on the positional error term of the objective function changes which iteration it crashes on
Thanks again,
Elliot.

[bvadorno]

Thanks, @ElliotWinterbottom.

Looking at the eigenvalues Hessian above there are some negative values but not in the step before it crashes. Is this still an issue?

It is. They are very small, which might pass through the solver's thresholds, but it's an indicative that something is ill-conditioned at best.

I'm intrigued by two things. First, if your robot has 4-DoF, your extended Jacobian (i.e., the one related to your augmented vector $[\dot{\boldsymbol{q}}^T , s]^T$ will be $n_t \times 5$, where $n_t$ is the dimension of your task vector (e.g., 8 for dual quaternions, 4 for orientation quaternions, 3 for position quaternions). If your predictive horizon is composed of $p$ steps, your Hessian should have a dimension $(5p \times 5p)$. Therefore, It seems to me that you are trying to solve for one step only. Is that correct?

The second point is that the Frobenius norm of your Hessian matrix seems very large, which is strange because your matrix is bounded, as each coefficient of the Jacobian matrices is given by combinations of $\sin$ and $\cos$ functions with the $a$ and $d$ DH parameters, which are relatively small. This also suggests that there are other incorrect calculations.

Kind regards,
Bruno

[ElliotWinterbottom]

Thanks @bvadorno,

Therefore, It seems to me that you are trying to solve for one step only. Is that correct?

Yes that is correct. For the first minimal reproducible version of the problem I reduced the prediction horizon to a single prediction as it makes the maths much easier to look at and verifies if the problem is with multiple predictions. Since I'm getting the same problem regardless of the number of predictions.

This also suggests that there are other incorrect calculations.

I don't think the matrix is entirely bounded in this way. There are also the additions to the Hessian which are made by the scaling factor terms which usually come out to around 1 which are then scaled up by 1000 ( this comes from the weighted approach to multiple tasks). Other than that there's also the gain on the on the positional error which is currently around 100. However increasing or decreasing this gain results in a fewer number of iterations before this error is thrown. I'll have a check through some of my other calculations to see if I can find anything that looks out of place. However, if you have any other suggestions on what I could try next I'd be very happy to try them.
Thanks,
Elliot.

[bvadorno]

don't think the matrix is entirely bounded in this way. There are also the additions to the Hessian which are made by the scaling factor terms which usually come out to around 1 which are then scaled up by 1000 ( this comes from the weighted approach to multiple tasks). Other than that there's also the gain on the on the positional error which is currently around 100.

Thanks for the further explanation, @ElliotWinterbottom. Ah, I forgot you had another term related to the scaling factor. If $s$ is close to one, then it makes sense to have terms up to $10^6$. The closed-loop gain won't be dominant because it'll add terms up to $10^4$. So, what is really influencing the large values in the Hessian matrix is the weight related to the scaling factor.

One thing to note. Given the sampling period $T$ and the closed-loop gain $\lambda$, you must ensure that $T\lambda < 1$ (this is to guarantee that the discretization won't destabilize the system or introduce undue oscillatory behavior). For example, if $T=0.001$, which is a reasonable value (you shouldn't choose any value greater than 0.02), then it's sensible to have $\lambda = 100$ so that $T\lambda = 0.1 \leq 1$.

I also would suggest choosing the weight for the scaling factor (i.e., for the "secondary" objective $w_2$) no greater than 50.

Kind regards,
Bruno

[ElliotWinterbottom]

Hiya all,
Sorry for the late reply, completing this work wiped my out a bit.
The issue was I think one with the scaling factors and the closed loop gain together making the number so large as to be numerically unstable within QP oases.
Thank you all so much for your help at such short notice :)
Elliot.