eus_qpoases.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <qpOASES.hpp>
00003 
00004 using namespace qpOASES;
00005 extern "C" {
00006   double* solve_qpoases (double* ret,
00007                          double* eval_weight_matrix, double* eval_coeff_vector,
00008                          double* state_min_vector, double* state_max_vector,
00009                          double* inequality_matrix, double* inequality_min_vector, double* inequality_max_vector,
00010                          int state_len, int inequality_len, PrintLevel print_level, double* ret_status) {
00011     real_t* H = new real_t[state_len*state_len];
00012     real_t* A = new real_t[inequality_len*state_len];
00013     real_t* g = new real_t[state_len];
00014     real_t* ub = new real_t[state_len];
00015     real_t* lb = new real_t[state_len];
00016     real_t* ubA = new real_t[inequality_len];
00017     real_t* lbA = new real_t[inequality_len];
00018     for (int i = 0; i < state_len*state_len; i++) {
00019       H[i] = eval_weight_matrix[i];
00020     }
00021     for (int i = 0; i < state_len; i++) {
00022       ub[i] = state_max_vector[i];
00023     }
00024     for (int i = 0; i < state_len; i++) {
00025       lb[i] = state_min_vector[i];
00026     }
00027     for (int i = 0; i < state_len; i++) {
00028       g[i] = eval_coeff_vector[i];
00029     }
00030     for (int i = 0; i < inequality_len; i++) {
00031       ubA[i] = inequality_max_vector[i];
00032     }
00033     for (int i = 0; i < inequality_len; i++) {
00034       lbA[i] = inequality_min_vector[i];
00035     }
00036     for (int i = 0; i < inequality_len*state_len; i++) {
00037       A[i] = inequality_matrix[i];
00038     }
00039 
00040     QProblem example( state_len,inequality_len );
00041     Options options;
00042     options.printLevel = print_level;
00043     example.setOptions( options );
00044     /* Solve first QP. */
00045     int nWSR = 1000;
00046     returnValue status = example.init( H,g,A,lb,ub,lbA,ubA, nWSR );
00047     //printf("%d %d %d\n", print_level, PL_NONE, PL_MEDIUM);
00048     ret_status[0] = getSimpleStatus(status, (print_level != PL_NONE)? BT_TRUE:BT_FALSE);
00049     /* Get and print solution of second QP. */
00050     real_t* xOpt = new real_t[state_len];
00051     example.getPrimalSolution( xOpt );
00052     for (int i = 0; i < state_len; i++) {
00053       ret[i] = xOpt[i];
00054     }
00055     delete[] H;
00056     delete[] A;
00057     delete[] g;
00058     delete[] ub;
00059     delete[] lb;
00060     delete[] ubA;
00061     delete[] lbA;
00062     delete[] xOpt;
00063 
00064     return ret;
00065   }
00066 }


eus_qpoases
Author(s):
autogenerated on Mon Oct 6 2014 01:10:37