graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Matei T. Ciocarlie
00021 //
00022 // $Id:$
00023 //
00024 //######################################################################
00025 
00030 #include "qpoases.h"
00031 
00032 #include <QProblem.hpp>
00033 
00034 #include <limits>
00035 #include <math.h>
00036 
00037 #include "matrix.h"
00038 //#define GRASPITDBG
00039 #include "debug.h"
00040 
00044 void oasesFromMatrix(std::vector<double> &O, const Matrix &M, double scale = 1.0)
00045 {
00046   int i,j;
00047   double value;
00048   M.sequentialReset();
00049   O.resize(0);
00050   O.resize( M.rows()*M.cols(), 0.0 );
00051   while (M.nextSequentialElement(i, j, value)) {
00052     O.at( i*M.cols() + j) = value / scale;
00053   }
00054 }
00055 
00056 void printMatrix(const std::vector<double> &M, std::string name)
00057 {
00058   std::cerr << name << "[";
00059   for(size_t i=0; i<M.size(); i++) {
00060     if (i!=0) std::cerr << " ";
00061     std::cerr << M[i];
00062   }
00063   std::cerr << "]\n";
00064 }
00065 
00066 int QPOASESSolverWrapperQP(const Matrix &Q, 
00067                            const Matrix &Eq, const Matrix &b,
00068                            const Matrix &InEq, const Matrix &ib,
00069                            const Matrix &lowerBounds, const Matrix &upperBounds,
00070                            Matrix &sol, double *objVal)
00071 {
00072   // number of variables
00073   int nV = sol.rows();
00074   // number of constraints 
00075   int nC = Eq.rows() + InEq.rows();
00076   // set up QProblem object
00077   qpOASES::QProblem problem( nV, nC );
00078   problem.setPrintLevel(qpOASES::PL_LOW);
00079   
00080   //solvers are sensitive to numerical problems. Scale the problem down
00081   //we will use this value to scale down the right hand side of equality
00082   //and inequality constraints and lower and upper bounds
00083   //after solving, we must scale back up the solution and the value of the
00084   //objective
00085   double scale = b.absMax();
00086   if (scale < 1.0e2) {
00087     scale = 1.0;
00088   } else {
00089     DBGP("qpOASES solver: scaling problem down by " << scale);
00090   }
00091   
00092   // prepare the quadratic matrix
00093   std::vector<double> H;
00094   oasesFromMatrix(H, Q);
00095 
00096   //prepare the constraints matrix, first in Matrix form
00097   SparseMatrix GA(Matrix::BLOCKCOLUMN<SparseMatrix>(Eq, InEq));
00098   std::vector<double> A;
00099   oasesFromMatrix(A, GA);
00100   //upper bounds
00101   SparseMatrix GubA(Matrix::BLOCKCOLUMN<SparseMatrix>(b, ib));
00102   std::vector<double> ubA;
00103   oasesFromMatrix(ubA, GubA, scale);
00104   //lower bounds; min inf for inequalities
00105   SparseMatrix GlbA( Matrix::BLOCKCOLUMN<SparseMatrix>( b, Matrix::MIN_VECTOR(ib.rows()) ) );
00106   std::vector<double> lbA;
00107   oasesFromMatrix(lbA, GlbA, scale);
00108   
00109   //lower and upper bounds
00110   std::vector<double> lb;
00111   lowerBounds.getData(&lb);
00112   std::vector<double> ub;
00113   upperBounds.getData(&ub);
00114   for (size_t i=0; i<lb.size(); i++) {
00115     if (lb[i] != std::numeric_limits<double>::max() && lb[i] != -std::numeric_limits<double>::max() ) {
00116       lb[i] /= scale;
00117     }
00118     if (ub[i] != std::numeric_limits<double>::max() && ub[i] != -std::numeric_limits<double>::max() ) {
00119       ub[i] /= scale;
00120     }
00121   }
00122 
00123   //g vector, all zeroes
00124   std::vector<double> g;
00125   g.resize(sol.rows(), 0.0);
00126 
00127   // solve QP
00128   int nWSR = 1000;
00129   qpOASES::returnValue retVal = problem.init( &H[0], &g[0], &A[0], &lb[0], &ub[0], &lbA[0], &ubA[0], nWSR, 0 );
00130 
00131   if (retVal == qpOASES::RET_MAX_NWSR_REACHED) {
00132     DBGA("qpOASES QP:max iterations reached");
00133     return -1;
00134   } else if (retVal == qpOASES::RET_INIT_FAILED) {
00135     DBGA("qpOASES QP:init failed with generic RET_INIT_FAILED error code");
00136     return -1;
00137   } else if (retVal == qpOASES::RET_INIT_FAILED_INFEASIBILITY) {
00138     DBGA("qpOASES QP:init failed; problem infeasible");
00139     return 1;
00140   } else if (retVal == qpOASES::RET_INIT_FAILED_UNBOUNDEDNESS) {
00141     DBGA("qpOASES QP:init failed; problem unbounded");
00142     return 1;
00143   } else if (retVal != qpOASES::SUCCESSFUL_RETURN) {
00144     DBGA("qpOASES QP:init failed with error code " << retVal);
00145     return -1;
00146   }
00147   DBGP("qpOASES QP:problem reports success");
00148 
00149   // retrieve the solution
00150   std::vector<double> xOpt;
00151   xOpt.resize(sol.rows());
00152   retVal = problem.getPrimalSolution(&xOpt[0]);
00153   if (retVal != qpOASES::SUCCESSFUL_RETURN) {
00154     DBGA("qpOASES QP:failed to retrieve solution");
00155     return -1;
00156   }
00157   for (size_t i=0; i<xOpt.size(); i++) {
00158     sol.elem(i,0) = scale * xOpt[i];
00159   }
00160   //retrieve the objective
00161   *objVal = problem.getObjVal();
00162   if (*objVal == qpOASES::INFTY) {
00163     DBGA("qpOASES QP:objective is infinity");
00164     return -1;
00165   }
00166   *objVal *= scale * scale;
00167 
00168   return 0;
00169 }
00170 
00171 int QPOASESSolverWrapperLP(const Matrix &Q,
00172                            const Matrix &Eq, const Matrix &b,
00173                            const Matrix &InEq, const Matrix &ib,
00174                            const Matrix &lowerBounds, const Matrix &upperBounds,
00175                            Matrix &sol, double* objVal)
00176 {
00177   // number of variables
00178   int nV = sol.rows();
00179   // number of constraints 
00180   int nC = Eq.rows() + InEq.rows();
00181   // set up QProblem object, inform it of 0 Hessian (since we have an LP)
00182   qpOASES::QProblem problem( nV, nC, qpOASES::HST_ZERO );
00183   problem.setPrintLevel(qpOASES::PL_LOW);
00184   
00185   //solvers are sensitive to numerical problems. Scale the problem down
00186   //we will use this value to scale down the right hand side of equality
00187   //and inequality constraints and lower and upper bounds
00188   //after solving, we must scale back up the solution and the value of the
00189   //objective
00190   double scale = b.absMax();
00191   if (scale < 1.0e2) {
00192     scale = 1.0;
00193   } else {
00194     DBGP("qpOASES solver: scaling problem down by " << scale);
00195   }
00196   
00197   // prepare the objective matrix
00198   assert(Q.rows() == 1);
00199   std::vector<double> g;
00200   oasesFromMatrix(g, Q);
00201 
00202   //prepare the constraints matrix, first in Matrix form
00203   SparseMatrix GA(Matrix::BLOCKCOLUMN<SparseMatrix>(Eq, InEq));
00204   std::vector<double> A;
00205   oasesFromMatrix(A, GA);
00206   //upper bounds
00207   SparseMatrix GubA(Matrix::BLOCKCOLUMN<SparseMatrix>(b, ib));
00208   std::vector<double> ubA;
00209   oasesFromMatrix(ubA, GubA, scale);
00210   //lower bounds; min inf for inequalities
00211   SparseMatrix GlbA( Matrix::BLOCKCOLUMN<SparseMatrix>( b, Matrix::MIN_VECTOR(ib.rows()) ) );
00212   std::vector<double> lbA;
00213   oasesFromMatrix(lbA, GlbA, scale);
00214   
00215   //lower and upper bounds
00216   std::vector<double> lb;
00217   lowerBounds.getData(&lb);
00218   std::vector<double> ub;
00219   upperBounds.getData(&ub);
00220   for (size_t i=0; i<lb.size(); i++) {
00221     if (lb[i] != std::numeric_limits<double>::max() && lb[i] != -std::numeric_limits<double>::max() ) {
00222       lb[i] /= scale;
00223     }
00224     if (ub[i] != std::numeric_limits<double>::max() && ub[i] != -std::numeric_limits<double>::max() ) {
00225       ub[i] /= scale;
00226     }
00227   }
00228 
00229   // solve LP
00230   int nWSR = 1000;
00231   qpOASES::returnValue retVal = problem.init( NULL, &g[0], &A[0], &lb[0], &ub[0], &lbA[0], &ubA[0], nWSR, 0 );
00232 
00233   if (retVal == qpOASES::RET_MAX_NWSR_REACHED) {
00234     DBGA("qpOASES LP: max iterations reached");
00235     return -1;
00236   } else if (retVal == qpOASES::RET_INIT_FAILED) {
00237     DBGA("qpOASES LP: init failed with generic RET_INIT_FAILED error code");
00238     return -1;
00239   } else if (retVal == qpOASES::RET_INIT_FAILED_INFEASIBILITY) {
00240     DBGA("qpOASES LP: init failed; problem infeasible");
00241     return 1;
00242   } else if (retVal == qpOASES::RET_INIT_FAILED_UNBOUNDEDNESS) {
00243     DBGA("qpOASES LP: init failed; problem unbounded");
00244     return 1;
00245   } else if (retVal != qpOASES::SUCCESSFUL_RETURN) {
00246     DBGA("qpOASES LP: init failed with error code " << retVal);
00247     return -1;
00248   }
00249   DBGP("qpOASES LP: problem reports success");
00250 
00251   // retrieve the solution
00252   std::vector<double> xOpt;
00253   xOpt.resize(sol.rows());
00254   retVal = problem.getPrimalSolution(&xOpt[0]);
00255   if (retVal != qpOASES::SUCCESSFUL_RETURN) {
00256     DBGA("qpOASES LP: failed to retrieve solution");
00257     return -1;
00258   }
00259   for (size_t i=0; i<xOpt.size(); i++) {
00260     sol.elem(i,0) = scale * xOpt[i];
00261   }
00262   //retrieve the objective
00263   *objVal = problem.getObjVal();
00264   if (*objVal == qpOASES::INFTY) {
00265     DBGA("qpOASES LP: objective is infinity");
00266     return -1;
00267   }
00268   *objVal *= scale;
00269 
00270   return 0;
00271 }
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/math/qpoases.cpp
