qp_lib.cpp

Go to the documentation of this file.

#include <iostream>
#include <Eigen/Dense>

// hack
#include <ros/common.h>
#if ROS_VERSION_GE(ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH, 1, 11, 10)
namespace Eigen 
{
  namespace internal
  {
    float abs(float a) { return std::abs(a); }
    float sqrt(float v) { return ::sqrt(v); }
  }
}
#endif

//#define Eigen::interal::abs std::abs

#include "eiquadprog.hpp"

using namespace Eigen;

#define print(var)  \
  std::cout<<#var"= "<<std::endl<<var<<std::endl

int flag = -1 ;

extern "C" {
int get_constraints_check_flag(){
        return flag ;
}
}

extern "C" {
int check_constraints(double* CE, double* ce0, double* CI, double* ci0,
                double* x, int x_len, int ce_len, int ci_len, double eqthre, double* ce_err,
                double* ci_err) {
        int ret = ce_len + ci_len ;
        for (int i = 0; i < ce_len; i++) {
                for (int j = 0; j < x_len; j++) {
                        ce_err[i] += CE[i * x_len + j] * x[j];
                }
                ce_err[i] += ce0[i];
                if ( ce_err[i] < eqthre && ce_err[i] > -eqthre ) ret-- ;
        }
        for (int i = 0; i < ci_len; i++) {
                for (int j = 0; j < x_len; j++) {
                        ci_err[i] += CI[i * x_len + j] * x[j];
                }
                ci_err[i] += ci0[i];
                if ( ci_err[i] > -eqthre ) ret-- ;
        }
        return ret;
}
}


extern "C" {
double* solve_eiquadprog(double* G, double* g0, double* CE, double* ce0, double* CI,
                double* ci0, double* x,
                int x_len, int ce_len, int ci_len,
                double eqthre,
                int debug,
                double* ret_buf, double* ce_err, double* ci_err) {
        MatrixXd G_buf(x_len, x_len);
        Eigen::VectorXd g0_buf(x_len);
        MatrixXd CE_buf(x_len, ce_len);
        Eigen::VectorXd ce0_buf(ce_len);
        MatrixXd CI_buf(x_len, ci_len);
        Eigen::VectorXd ci0_buf(ci_len);
        Eigen::VectorXd x_buf(x_len);

        for ( int i=0 ; i<x_len ; i++ ){
                for ( int j=0 ; j<x_len ; j++ ){
                        G_buf(i,j) = G[i+ x_len*j] ;
//                      std::cout << "(" << i << "," << j << ")" ;
//                      std::cout << " -" << i+x_len*j << "-" ;
//                      std::cout << " -> " << G[i+ x_len*j] << std::endl ;
                        if ( i == 0 ){
                                g0_buf(j) = g0[j] ;
                                x_buf(j) = x[j] ;
                        }
                }
                for ( int j=0 ; j<ce_len ; j++ ){
                        CE_buf(i,j) = CE[i+ x_len*j] ;
                        if ( i == 0 ){
                                ce0_buf(j) = ce0[j] ;
                        }
                }
                for ( int j=0 ; j<ci_len ; j++ ){
                        CI_buf(i,j) = CI[i+ x_len*j] ;
                        if ( i == 0 ){
                                ci0_buf(j) = ci0[j] ;
                        }
                }
        }


        if (debug>1) {
                print(G_buf);
                print(g0_buf);
                print(CE_buf);
                print(ce0_buf);
                print(CI_buf);
                print(ci0_buf);
        }

        ret_buf[0] = solve_quadprog(G_buf, g0_buf, CE_buf, ce0_buf, CI_buf,
                                                        ci0_buf, x_buf) ;
        for (int i = 0; i < x_buf.size(); i++)
                                x[i] = x_buf(i) ;

        flag = check_constraints(CE, ce0, CI, ci0, x, x_len, ce_len, ci_len, eqthre, ce_err, ci_err);
        //flag = check_constraints(CE, ce0, CI, ci0, x, x_len, eqthre, ce_len, ci_len, ce_err, ci_err);
        if (debug>0) {
                std::cout << "[eus-eiquadprog]" << std::endl;
                std::cout << "  :minimized-object "
                                << ret_buf[0] << std::endl;
                std::cout << "  :optimal-state [";
                for (int i = 0; i < x_buf.size(); i++)
                        std::cout << x[i]  << ' ';
                std::cout << "]" << std::endl;

                std::cout << "  :eq-constraint || " ;
                for ( int i=0 ; i <ce_len ; i++ ) std::cout << ce_err[i] << " " ;
                std::cout << "|| < " << eqthre ;
                std::cout << std::endl ;
                std::cout << "  :iq-constraint [" ;
                for ( int i=0 ; i <ci_len ; i++ ) std::cout << ci_err[i] << " " ;
                std::cout << "] > " << -eqthre ;
                std::cout << std::endl ;
                std::cout << "  :constraint-check " ;
                std::cout << flag  ;
                std::cout << std::endl ;
        }

        return x;
}
}