bench for test transition with sparse solver

CMakeLists.txt

@@ -47,6 +47,7 @@ ENDIF(BUILD_PYTHON_INTERFACE)
 
 
 find_package (centroidal-dynamics-lib REQUIRED)
+find_package (glpk REQUIRED)
 find_package (Spline REQUIRED)
 
 # Declare Headers

cmake2/Findglpk.cmake

+ ################################################################################
+ #    Copyright (C) 2014 GSI Helmholtzzentrum fuer Schwerionenforschung GmbH    #
+ #                                                                              #
+ #              This software is distributed under the terms of the             # 
+ #              GNU Lesser General Public Licence (LGPL) version 3,             #  
+ #                  copied verbatim in the file "LICENSE"                       #
+ ################################################################################
+# - Try to find GLPK instalation
+# Once done this will define
+#
+
+MESSAGE(STATUS "Looking for GLPK ...")
+
+FIND_PATH(GLPK_INCLUDE_DIR NAMES glpk.h PATHS
+  HINTS ${GLPK_INCLUDE_DIR} /usr/local/include
+  NO_DEFAULT_PATH
+)
+
+FIND_PATH(GLPK_LIB_DIR  NAMES libglpk.so PATHS
+  ${SIMPATH}/basics/glpk/lib
+  ${SIMPATH}/local/lib
+  ${SIMPATH}/local/lib
+  /usr/local/lib
+  /usr/lib/x86_64-linux-gnu/
+  NO_DEFAULT_PATH
+)
+
+find_library(GLPK_LIB NAMES libglpk.so
+             HINTS ${GLPK_LIB_DIR} /usr/lib/libglpk.so )
+
+if (GLPK_INCLUDE_DIR AND GLPK_LIB_DIR)
+   set(GLPK_FOUND TRUE)
+endif (GLPK_INCLUDE_DIR AND GLPK_LIB_DIR)
+
+if (GLPK_FOUND)
+  if (NOT GLPK_FOUND_QUIETLY)
+    MESSAGE(STATUS "Looking for GLPK... - found ${GLPK_LIB_DIR}")
+#    message(STATUS "Found PLUTO: ${PLUTO_LIBRARY_DIR}")
+    SET(LD_LIBRARY_PATH ${LD_LIBRARY_PATH} ${GLPK_LIB_DIR})
+  endif (NOT GLPK_FOUND_QUIETLY)
+else (GLPK_FOUND)
+  if (GLPK_FOUND_REQUIRED)
+    message(FATAL_ERROR "Looking for GLPK... - Not found")
+  endif (GLPK_FOUND_REQUIRED)
+endif (GLPK_FOUND)

include/solver/glpk-wrapper.hpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of tsid
+// tsid 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
+// 3 of the License, or (at your option) any later version.
+// tsid 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
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef GLPKWRAPPER_HH_
+#define GLPKWRAPPER_HH_
+
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+#define DEFAULT_MAX_ITER 1000
+
+
+namespace solvers
+{
+    typedef Eigen::MatrixXd MatrixXd;
+    typedef Eigen::VectorXd VectorXd;
+    typedef Eigen::VectorXi VectorXi;
+    enum glpk_status
+    {
+      glpk_OPTIMAL=0,
+      glpk_INFEASIBLE=1,
+      glpk_UNBOUNDED=2,
+      glpk_MAX_ITER_REACHED=3,
+    };
+
+    // min g'x
+    // st  CIx <= ci0
+    //     CEx  = ce0
+    glpk_status solve(const VectorXd & g0,
+                      const MatrixXd & CE,
+                      const VectorXd & ce0,
+                      const MatrixXd & CI,
+                      const VectorXd & ci0,
+                      VectorXd& x);
+
+} /* namespace solvers */
+
+#endif /* GLPKWRAPPER_HH_ */

src/CMakeLists.txt

@@ -6,6 +6,7 @@ INCLUDE_DIRECTORIES("${INCLUDE_DIR}")
 INCLUDE_DIRECTORIES("${EIGEN3_INCLUDE_DIR}")
 INCLUDE_DIRECTORIES("${SPLINE_INCLUDE_DIR}")
 INCLUDE_DIRECTORIES("${CDL_INCLUDE_DIR}")
+INCLUDE_DIRECTORIES("${GLPK_INCLUDE_DIR}")
 
 
 SET(LIBRARY_NAME ${PROJECT_NAME})
@@ -28,9 +29,11 @@ SET(${LIBRARY_NAME}_SOURCES
     ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
     ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
     ${INCLUDE_DIR}/solver/eiquadprog-fast.hpp
+    ${INCLUDE_DIR}/solver/glpk-wrapper.hpp
     common_solve_methods.cpp
     costfunction_definition.cpp
     eiquadprog-fast.cpp
+    glpk-wrapper.cpp
     computeCOMTraj.cpp
     solve_0_step.cpp
     utils.cpp
@@ -43,6 +46,7 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES})
 if ( MSVC )
 	SET(CMAKE_DEBUG_POSTFIX d)
 endif ( MSVC )
+TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${GLPK_LIB})
 TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CDL_LIBRARIES})
 
 

src/common_solve_methods.cpp

 
 #include <bezier-com-traj/common_solve_methods.hh>
 #include <solver/eiquadprog-fast.hpp>
+#include <solver/glpk-wrapper.hpp>
 #include <bezier-com-traj/waypoints/waypoints_definition.hh>
 
 namespace bezier_com_traj
@@ -156,7 +157,8 @@ typedef Eigen::SparseMatrix<double> SpMat;
 typedef Eigen::SparseVector<double> SpVec;
 typedef Eigen::SparseVector<int>    SpVeci;
 
-ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D, Cref_vectorX d, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess, const bool sparse)
+ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D, Cref_vectorX d, Cref_matrixXX H,
+                 Cref_vectorX g, Cref_vectorX initGuess, const bool sparse)
 {
     /*
    * solves the problem
@@ -174,27 +176,35 @@ ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D, Cref_vector
     assert (!(is_nan(initGuess)));
     assert (!(is_nan(H)));
 
+    ResultData res;
     MatrixXX CI = -A;
     MatrixXX CE = D;
     VectorX ce0  = -d;
     tsid::solvers::EiquadprogFast QPsolver = tsid::solvers::EiquadprogFast();
     VectorX x = initGuess;
 
-
-    //clock_t s0,e0;
-    //s0 = clock();
     tsid::solvers::EiquadprogFast_status status;
     if(sparse)
     {
+        clock_t s0,e0;
         SpMat Hsp = H.sparseView();
+        s0 = clock();
         status = QPsolver.solve_quadprog_sparse(Hsp,g,CE,ce0,CI,ci0,x);
+        e0 = clock();
+        std::cout<<"Time required with force formulation sparse : "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000<<" ms "<<std::endl;
+        s0 = clock();
+        solvers::glpk_status ret = solvers::solve(g,D,d,A,ci0,x);
+        e0 = clock();
+        res.success_ = (ret == solvers::glpk_OPTIMAL );
+        std::cout<<"Time required with force formulation LP : "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000<<" ms "<<std::endl;
+        //std::cout << "solvers agree ? " << ret << " " << status << std::endl;
+        //assert(((int)ret == 0 && (int)status == 0) || ((int)ret != 0 && (int)status != 0));
     }
     else
+    {
         status = QPsolver.solve_quadprog(H,g,CE,ce0,CI,ci0,x);
-    //e0 = clock();
-    //std::cout<<"Time required with force formulation normal : "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000<<" ms "<<std::endl;
-    ResultData res;
-    res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL );
+        res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL );
+    }
     res.x = x;
     if(res.success_)
     {

src/glpk-wrapper.cpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of tsid
+// tsid 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
+// 3 of the License, or (at your option) any later version.
+// tsid 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
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+
+#include "solver/glpk-wrapper.hpp"
+#include <glpk.h>
+#include <iostream>
+
+
+  namespace solvers
+  {
+
+  glpk_status solve(const VectorXd & g0,
+                    const MatrixXd & CE,
+                    const VectorXd & ce0,
+                    const MatrixXd & CI,
+                    const VectorXd & ci0,
+                    VectorXd& x)
+  {
+      glp_smcp opts; glp_init_smcp(&opts); opts.msg_lev = GLP_MSG_OFF;
+      glp_prob *lp;
+      int ia[1 + 20000];                    //Row indices of each element
+      int ja[1 + 20000];                    //column indices of each element
+      double ar[1 + 20000];                     //numerical values of corresponding elements
+      lp = glp_create_prob();                     //creates a problem object
+      glp_set_prob_name(lp, "sample");            //assigns a symbolic name to the problem object
+      glp_set_obj_dir(lp, GLP_MIN);               //calls the routine glp_set_obj_dir to set the
+                                                  //omptimization direction flag,
+                                                  //where GLP_MAX means maximization
+
+      //ROWS
+      // TODO SPECIFIC
+      const int numEqConstraints = (int)(CE.rows());
+      // for inequality, x.size()-3 last constraints are not relevant as they are the positivity constraints
+      const int numIneqConstraints =(int)(CI.rows() - (x.size() -3));
+      const int num_constraints_total (numEqConstraints + numIneqConstraints);
+      glp_add_rows(lp, num_constraints_total);
+      int idrow = 1;//adds three rows to the problem object
+      int idcol = 1;
+      int idConsMat = 1;
+      int xsize = (int)(x.size());
+      for (int i =0; i < numIneqConstraints; ++i, ++idrow )
+      {
+          glp_set_row_bnds(lp, idrow, GLP_UP, 0.0, ci0(i));
+          for (int j =0; j < xsize; ++j, ++idcol )
+          {
+              if(CI(i,j) != 0.)
+              {
+                  ia[idConsMat] = idrow, ja[idConsMat] = idcol, ar[idConsMat] = CI(i,j); /* a[1,1] = 1 */
+                  ++ idConsMat;
+              }
+          }
+          idcol = 1;
+      }
+      for (int i =0; i < numEqConstraints; ++i, ++idrow )
+      {
+          glp_set_row_bnds(lp, idrow, GLP_FX, ce0(i), ce0(i));
+          for (int j =0; j < xsize; ++j, ++idcol )
+          {
+              if(CE(i,j) != 0.)
+              {
+                  ia[idConsMat] = idrow, ja[idConsMat] = idcol, ar[idConsMat] = CE(i,j); /* a[1,1] = 1 */
+                  ++ idConsMat;
+              }
+          }
+          idcol = 1;
+      }
+      //assert(num_constraints_total == idConsMat -1);
+
+
+
+      //COLUMNS
+      glp_add_cols(lp, xsize); //adds three columns to the problem object
+      // TODO SPECIFIC
+      for (int i =0; i < 3; ++i, ++idcol )
+      {
+          glp_set_col_bnds(lp, idcol, GLP_FR, 0.0, 0.0);
+          glp_set_obj_coef(lp, idcol, g0(i));
+      }
+      for (int i =3; i < xsize; ++i, ++idcol )
+      {
+          glp_set_col_bnds(lp, idcol, GLP_LO, 0.0, 0.0);
+          glp_set_obj_coef(lp, idcol, g0(i));
+      }
+      /*for (int i =3; i < x.size(); ++i, ++idcol )
+          glp_set_col_bnds(lp, idrow, GLP_UP, 0.0, ci0(i));*/
+
+      // Constraint matrix
+      /*Eigen::MatrixXd constraints = Eigen::MatrixXd::Zero(1+num_constraints_total,1+x.size());
+      constraints.block(1,1,numIneqConstraints,idcol) = CI.block(0,0,numIneqConstraints,idcol);
+      constraints.block(1+numIneqConstraints,1,numEqConstraints,idcol) = CE;*/
+
+      glp_load_matrix(lp,idConsMat-1,ia,ja,ar);
+
+      //ia[1] = 1, ja[1] = 1, ar[1] = 1.0; /* a[1,1] = 1 */
+
+      int res = glp_simplex(lp, &opts);                      //calls the routine glp_simplex
+      glpk_status ret = glpk_INFEASIBLE;
+      if(res == 0)
+      {
+          ret = glpk_OPTIMAL;
+                                                  //to solve LP problem
+          //z = glp_get_obj_val(lp);                    //obtains a computed value of the objective function
+
+          idrow = 1;
+          for (int i =0; i < xsize; ++i, ++idrow)
+              x(i) = glp_get_col_prim(lp, idrow);
+
+
+          //printf("\nz = %g; x1 = %g; x2 = %g;\n", z, x1, x2); //writes out the optimal solution
+      }
+      glp_delete_prob(lp);                        //calls the routine glp_delete_prob, which frees all the memory
+      glp_free_env();
+      return ret;
+  }
+
+  } /* namespace solvers */

tests/test-transition.cc

@@ -70,6 +70,11 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,bool shoul
     if(continuous)
     {
         clock_t s0,e0;
+        std::cout<<"num contacts : "<< pData.contacts_[0].contactPhase_->m_G_centr.cols() / 16 << std::endl;
+        if(pData.representation_ == bezier_com_traj::FORCE)
+            std::cout<<"FORCE --------------------" <<std::endl;
+        else
+            std::cout<<"CONTINUOUS --------------------" <<std::endl;
         s0 = clock();
         res = bezier_com_traj::computeCOMTraj(pData,Ts);
         e0 = clock();
@@ -77,6 +82,7 @@ void check_transition(bezier_com_traj::ProblemData& pData, VectorX Ts,bool shoul
             std::cout<<"Time required with force formulation : "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000<<" ms "<<std::endl;
         else
             std::cout<<"Time required with Double Description: "<<((double)(e0-s0)/CLOCKS_PER_SEC)*1000<<" ms "<<std::endl;
+        std::cout<<"-------------------- \n" <<std::endl <<std::endl;
     }
     else
         res = bezier_com_traj::computeCOMTrajFixedSize(pData,Ts,pointsPerPhase);