test_example5.cpp

Go to the documentation of this file.

/*
 *      This file is part of qpOASES.
 *
 *      qpOASES -- An Implementation of the Online Active Set Strategy.
 *      Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
 *      Christian Kirches et al. All rights reserved.
 *
 *      qpOASES is free software; you can redistribute it and/or
 *      modify it under the terms of the GNU Lesser General Public
 *      License as published by the Free Software Foundation; either
 *      version 2.1 of the License, or (at your option) any later version.
 *
 *      qpOASES is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *      See the GNU Lesser General Public License for more details.
 *
 *      You should have received a copy of the GNU Lesser General Public
 *      License along with qpOASES; if not, write to the Free Software
 *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */


#include <stdlib.h>

#include <qpOASES.hpp>
#include "../../examples/example4CP.cpp"
#include <qpOASES/UnitTesting.hpp>


int main( )
{
        USING_NAMESPACE_QPOASES

        int_t i,j,jj;
        real_t d = 0.0;

        /* Setup data of first QP... */
        real_t H[7*7];
        real_t A[50*7];
        real_t g[7];
        real_t lbA[50];

        /*          ( 1.0 0.5 |                    )
         *          ( 0.5 2.0 |                    )
         *          ( --------+------------------- )
         *      H = (         | 1e-6               )
         *          (         |      1e-6          )
         *          (         |           ...      )
         *          (         |               1e-6 ) */
        for( i=0; i<7*7; ++i )
                H[i] = 0.0;
        for( i=2; i<7; ++i )
                H[i*7+i] = 1.0e-6;
        H[0] = 1.0;
        H[1] = 0.5;
        H[7] = 0.5;
        H[8] = 2.0;

        /*          ( x.x x.x | 1.0             )
         *          ( x.x x.x | ...             )
         *          ( x.x x.x | 1.0             )
         *          ( x.x x.x |     1.0         )
         *      A = ( x.x x.x |     ...         )
         *          ( x.x x.x |     1.0         )
         *          ( x.x x.x |         ...     )
         *          ( x.x x.x |             1.0 )
         *          ( x.x x.x |             ... )
         *          ( x.x x.x |             1.0 ) */
        for( i=0; i<50*7; ++i )
                A[i] = 0.0;
        for( i=0; i<50; ++i )
        {
                for( j=0; j<2; ++j )
                        A[i*7+j] = (real_t)rand() / (real_t)RAND_MAX;

                A[i*7 + (i/10)+2] = 1.0;
        }

        /*          ( -1.0 )
         *          ( -0.5 )
         *          ( ---- )
         *      g = (      )
         *          (      )
         *          (      )
         *          (      ) */
        for( i=0; i<7; ++i )
                g[i] = 0.0;
        g[0] = -1.0;
        g[1] = -0.5;

        for( i=0; i<50; ++i )
                lbA[i] = 1.0;

        /* ... and setting up user-defined constraint product function. */
        MyConstraintProduct myCP( 7,50,A );


        /* Setting up QProblem object and set construct product function. */
        QProblem example( 7,50 );
        example.setConstraintProduct( &myCP );


        /* Solve first QP. */
        real_t cputime = 1.0;
        int_t nWSR = 100;
        example.init( H,g,A,0,0,lbA,0, nWSR,&cputime );

        /* Get and print solution of QP. */
        real_t xOpt[7], yOpt[7+50];
        example.getPrimalSolution( xOpt );
        example.getDualSolution( yOpt );


        /* Compute local linear feedback law */
        const int_t n_rhs = 7+7+50;
        real_t g_in[7*n_rhs];
        real_t b_in[7*n_rhs];
        real_t bA_in[50*n_rhs];
        real_t x_out[7*n_rhs];
        real_t y_out[(7+50)*n_rhs];

        int_t ii;
        memset (g_in, 0, sizeof (g_in));
        memset (b_in, 0, sizeof (b_in));
        memset (bA_in, 0, sizeof (bA_in));

        for ( ii = 0; ii < 7; ++ii )
                g_in[ii*7 + ii] = 1.0;
        for ( ii = 0; ii < 7; ++ii )
                b_in[(ii+7)*7 + ii] = 1.0;
        for ( ii = 0; ii < 50; ++ii )
                bA_in[(ii+14)*50 + ii] = 1.0;

        example.solveCurrentEQP ( n_rhs, g_in, b_in, b_in, bA_in, bA_in, x_out, y_out );

        /* Verify validity of local feedback law by perturbation and hot starts */
        real_t perturb = 1.0e-6;
        real_t nrm = 0.0;
        for ( ii = 0; ii < n_rhs; ++ii )
        {
                for ( jj = 0; jj < 7; ++jj )
                        g_in[ii*7 + jj] = g[jj] + g_in[ii*7+jj]*perturb;
                for ( jj = 0; jj < 50; ++jj )
                        bA_in[ii*50 + jj] = lbA[jj] + bA_in[ii*50+jj]*perturb;

                nWSR = 100;
                example.hotstart( &g_in[ii*7],0,0,&bA_in[ii*50],0, nWSR, 0 );

                real_t xPer[7], yPer[7+50];
                example.getPrimalSolution( xPer );
                example.getDualSolution( yPer );

                for ( jj = 0; jj < 7; ++jj )
                {
                        d = getAbs (x_out[ii*7+jj]*perturb - (xPer[jj]-xOpt[jj]) );
                        if (nrm < d) nrm=d;
                }
                for ( jj = 0; jj < 7+50; ++jj )
                {
                        d = getAbs (y_out[ii*(7+50)+jj]*perturb - (yPer[jj]-yOpt[jj]) );
                        if (nrm < d) nrm=d;
                }
        }
        printf ("Maximum perturbation over all directions: %e\n", nrm);

        QPOASES_TEST_FOR_TOL( nrm,1e-15 );


        /* // print feedback matrix
        for (ii = 0; ii < n_rhs; ++ii)
        {
                printf ("x: ");
                for (jj = 0; jj < 7; ++jj )
                        printf ("%8.2e ", x_out[ii*7+jj]);
                printf (" y: ");
                for (jj = 0; jj < 7+50; ++jj )
                        printf ("%8.2e ", y_out[ii*(7+50)+jj]);
                printf("\n");
        }
*/

        return TEST_PASSED;
}


/*
 *      end of file
 */