qp_lib.cpp
Go to the documentation of this file.
00001 
00002 #include <iostream>
00003 #include <Eigen/Dense>
00004 
00005 #include "eiquadprog.hpp"
00006 
00007 using namespace Eigen;
00008 
00009 #define print(var)  \
00010   std::cout<<#var"= "<<std::endl<<var<<std::endl
00011 
00012 int flag = -1 ;
00013 
00014 extern "C" {
00015 int get_constraints_check_flag(){
00016         return flag ;
00017 }
00018 }
00019 
00020 extern "C" {
00021 int check_constraints(double* CE, double* ce0, double* CI, double* ci0,
00022                 double* x, int x_len, int ce_len, int ci_len, double eqthre, double* ce_err,
00023                 double* ci_err) {
00024         int ret = ce_len + ci_len ;
00025         for (int i = 0; i < ce_len; i++) {
00026                 for (int j = 0; j < x_len; j++) {
00027                         ce_err[i] += CE[i * x_len + j] * x[j];
00028                 }
00029                 ce_err[i] += ce0[i];
00030                 if ( ce_err[i] < eqthre && ce_err[i] > -eqthre ) ret-- ;
00031         }
00032         for (int i = 0; i < ci_len; i++) {
00033                 for (int j = 0; j < x_len; j++) {
00034                         ci_err[i] += CI[i * x_len + j] * x[j];
00035                 }
00036                 ci_err[i] += ci0[i];
00037                 if ( ci_err[i] > -eqthre ) ret-- ;
00038         }
00039         return ret;
00040 }
00041 }
00042 
00043 
00044 extern "C" {
00045 double* solve_eiquadprog(double* G, double* g0, double* CE, double* ce0, double* CI,
00046                 double* ci0, double* x,
00047                 int x_len, int ce_len, int ci_len,
00048                 double eqthre,
00049                 int debug,
00050                 double* ret_buf, double* ce_err, double* ci_err) {
00051         MatrixXd G_buf(x_len, x_len);
00052         Eigen::VectorXd g0_buf(x_len);
00053         MatrixXd CE_buf(x_len, ce_len);
00054         Eigen::VectorXd ce0_buf(ce_len);
00055         MatrixXd CI_buf(x_len, ci_len);
00056         Eigen::VectorXd ci0_buf(ci_len);
00057         Eigen::VectorXd x_buf(x_len);
00058 
00059         for ( int i=0 ; i<x_len ; i++ ){
00060                 for ( int j=0 ; j<x_len ; j++ ){
00061                         G_buf(i,j) = G[i+ x_len*j] ;
00062 //                      std::cout << "(" << i << "," << j << ")" ;
00063 //                      std::cout << " -" << i+x_len*j << "-" ;
00064 //                      std::cout << " -> " << G[i+ x_len*j] << std::endl ;
00065                         if ( i == 0 ){
00066                                 g0_buf(j) = g0[j] ;
00067                                 x_buf(j) = x[j] ;
00068                         }
00069                 }
00070                 for ( int j=0 ; j<ce_len ; j++ ){
00071                         CE_buf(i,j) = CE[i+ x_len*j] ;
00072                         if ( i == 0 ){
00073                                 ce0_buf(j) = ce0[j] ;
00074                         }
00075                 }
00076                 for ( int j=0 ; j<ci_len ; j++ ){
00077                         CI_buf(i,j) = CI[i+ x_len*j] ;
00078                         if ( i == 0 ){
00079                                 ci0_buf(j) = ci0[j] ;
00080                         }
00081                 }
00082         }
00083 
00084 
00085         if (debug>1) {
00086                 print(G_buf);
00087                 print(g0_buf);
00088                 print(CE_buf);
00089                 print(ce0_buf);
00090                 print(CI_buf);
00091                 print(ci0_buf);
00092         }
00093 
00094         ret_buf[0] = solve_quadprog(G_buf, g0_buf, CE_buf, ce0_buf, CI_buf,
00095                                                         ci0_buf, x_buf) ;
00096         for (int i = 0; i < x_buf.size(); i++)
00097                                 x[i] = x_buf(i) ;
00098 
00099         flag = check_constraints(CE, ce0, CI, ci0, x, x_len, ce_len, ci_len, eqthre, ce_err, ci_err);
00100         //flag = check_constraints(CE, ce0, CI, ci0, x, x_len, eqthre, ce_len, ci_len, ce_err, ci_err);
00101         if (debug>0) {
00102                 std::cout << "[eus-eiquadprog]" << std::endl;
00103                 std::cout << "  :minimized-object "
00104                                 << ret_buf[0] << std::endl;
00105                 std::cout << "  :optimal-state [";
00106                 for (int i = 0; i < x_buf.size(); i++)
00107                         std::cout << x[i]  << ' ';
00108                 std::cout << "]" << std::endl;
00109 
00110                 std::cout << "  :eq-constraint || " ;
00111                 for ( int i=0 ; i <ce_len ; i++ ) std::cout << ce_err[i] << " " ;
00112                 std::cout << "|| < " << eqthre ;
00113                 std::cout << std::endl ;
00114                 std::cout << "  :iq-constraint [" ;
00115                 for ( int i=0 ; i <ci_len ; i++ ) std::cout << ci_err[i] << " " ;
00116                 std::cout << "] > " << -eqthre ;
00117                 std::cout << std::endl ;
00118                 std::cout << "  :constraint-check " ;
00119                 std::cout << flag  ;
00120                 std::cout << std::endl ;
00121         }
00122 
00123         return x;
00124 }
00125 }


eus_qp
Author(s):
autogenerated on Fri Apr 19 2019 03:45:20