Jeremy Crowley - 2019-06-27

I have a pretty simple model and am minimizing a Mayer term of the final time parameter T. The solver gives the following warning but then proceeds to solve to problem.

[ACADO] Warning: QP needs to be relaxed due to infeasibility
  Code: (RET_RELAXING_QP)
  File: /cygdrive/c/Users/e32761/Documents/acado/acado/conic_solver/condensing_based_cp_solver.cpp
  Line: 494

The resulting states satisfy the initial and final state constraints so I'm not sure exactlty what is happening. When simulating, the dynamics seem to hold. The solver give the following output.

sqp it | qp its |       kkt tol |       obj val |     merit val |      ls param |
     1 |     63 |  1.919688e+09 |  5.495000e+00 |  2.303625e+09 |  1.000000e+00 |
     2 |    187 |  3.385004e+09 |  6.099231e+00 |  4.062005e+09 |  5.000000e-01 |
     3 |    114 |  5.309980e+08 |  6.636570e+00 |  6.371976e+08 |  5.000000e-01 |
     4 |     39 |  1.635070e+07 |  7.138134e+00 |  1.962084e+07 |  1.000000e+00 |
     5 |     34 |  5.290140e+04 |  7.135472e+00 |  6.348882e+04 |  1.000000e+00 |
     6 |     14 |  2.924902e-01 |  7.136399e+00 |  7.486274e+00 |  1.000000e+00 |
     7 |      8 |  2.965236e-07 |  7.136399e+00 |  7.136399e+00 |  1.000000e+00 |

Covergence achieved. Demanded KKT tolerance is 1.000000e-04.

So clearly is it acheiving convergence. How is it still able to find a solution after concluding the QP constraints need to be relaxed?

Thanks. Entire code is shown below.

#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>
#include <math.h>
#include <iostream>
using namespace std;

int main( ){

    // include all acado functionality
    USING_NAMESPACE_ACADO


    // introduce variables
    const double t_start = 0.0;
    const double t_end = 10.0;
    const double dt = 0.05;
    const double m = (t_end - t_start)/dt;

    const int numStates = 5;
    const int numInputs = 2;


    // states
    DifferentialState x[numStates];
    // x[0] - x position
    // x[1] - x velocity
    // x[2] - y position
    // x[3] - y velocity
    // x[4] - theta

    // inputs
    Control u[numInputs];
    // u[0] - forward acceleration
    // u[1] - theta ang velocity


    // final time param
    Parameter T;


    // diff eq
    DifferentialEquation f(t_start, T);


    // define optimal control problem
    OCP ocp(t_start, T, m); 


    // optimal control problem
    ocp.minimizeMayerTerm(T);

    // define continuous time system
    f << dot(x[0]) == x[1];
    f << dot(x[1]) == u[0]*cos(x[4]);
    f << dot(x[2]) == x[3];
    f << dot(x[3]) == u[0]*sin(x[4]);
    f << dot(x[4]) == u[1];

    ocp.subjectTo(f);


    // set initial condition
    ocp.subjectTo(AT_START, x[0] == -10.0);
    ocp.subjectTo(AT_START, x[1] == 0.0);
    ocp.subjectTo(AT_START, x[2] == 0.0);
    ocp.subjectTo(AT_START, x[3] == 0.0);
    ocp.subjectTo(AT_START, x[4] == 1.0);


    // set final condition constraints
    // final velocity of zero
    ocp.subjectTo(AT_END, x[0] == 0.0);
    ocp.subjectTo(AT_END, x[1] == 0.0);
    ocp.subjectTo(AT_END, x[2] == 0.0);
    ocp.subjectTo(AT_END, x[3] == 0.0);
    ocp.subjectTo(AT_END, x[4] == 0.0);


    // constrain inputs
    ocp.subjectTo(-1.0 <= u[0] <= 1.0);
    ocp.subjectTo(-1.0 <= u[1] <= 1.0);
    ocp.subjectTo(1.0 <= T <= 10.0);


    // set up optimization time alg
    OptimizationAlgorithm algorithm(ocp);

    algorithm.set(INTEGRATOR_TYPE, INT_RK78);
    algorithm.set(INTEGRATOR_TOLERANCE, 1e-8);
    algorithm.set(DISCRETIZATION_TYPE, SINGLE_SHOOTING);
    algorithm.set(KKT_TOLERANCE, 1e-4);


    // solve optimal control problem
    algorithm.solve();


    // log states and inputs
    algorithm.getDifferentialStates("states.txt");
    algorithm.getParameters("parameters.txt");
    algorithm.getControls("controls.txt");

    return 0;
}
 

Last edit: Jeremy Crowley 2019-06-27