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): Andrew T. Miller
00021 //
00022 // $Id: lmiOptimizer.cpp,v 1.1 2009/07/21 19:30:41 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "lmiOptimizer.h"
00027 
00028 #include "grasp.h"
00029 #include "matvec3D.h"
00030 #include "robot.h"
00031 #include "contact.h"
00032 
00033 #ifdef MKL
00034 #include "mkl_wrappers.h"
00035 #else
00036 #include "lapack_wrappers.h"
00037 #endif
00038 
00039 #include "maxdet.h"
00040 
00041 bool errorOccurred = false;
00042 
00043 double LMIOptimizer::GFO_WEIGHT_FACTOR = 1.0;
00044 
00045 //print out error message and exit
00046 void FatalErrMsg(char* s)
00047 {
00048   fprintf(stderr,"error: %s\n",s);
00049   exit(-1);
00050 }
00051 
00052 #define errMsg(S_)                 \
00053 {                                  \
00054   fprintf(stderr,"error: %s\n",S_);\
00055   errorOccurred = true;            \
00056   return;                          \
00057 } 
00058 
00062 double * block_Identity(int numBlocks, int * blockSizes, int pkSize)
00063 {
00064 
00065   int i, j, itemIndex, blockDim;
00066   double db0=0.0,*identity= (double*) malloc(pkSize*sizeof(double));
00067   
00068   dcopy(pkSize,&db0, 0, identity, 1);
00069   itemIndex=0;
00070   for(i=0;i<numBlocks; i++){
00071     blockDim=blockSizes[i];
00072     for(j=0;j<blockDim;j++){
00073       identity[itemIndex]=1.0;
00074       itemIndex+=blockDim-j;
00075     }
00076   }
00077   return(identity);
00078 }
00079 
00089 void
00090 LMIOptimizer::buildJacobian()
00091 {
00092         double db0=0.0;
00093         int i,f,l,blkOffset,rowOffset;
00094         transf Tec;
00095         double *Jee,*Tv_ec,*J;
00096         std::list<Contact *>::iterator cp;
00097         std::list<Contact *> contactList;
00098   
00099         numDOF = mGrasp->hand->getNumDOF();   // +6 for palm motion
00100 
00101         if (externalTorques) delete [] externalTorques;
00102         externalTorques = new double[numDOF];
00103         for (i=0;i<numDOF;i++) {
00104                 externalTorques[i] = mGrasp->hand->getDOF(i)->getExtForce();
00105         }
00106 
00107         if (Jacobian) delete [] Jacobian;
00108         Jacobian = new double[numDOF*numWrenches];
00109         dcopy(numDOF*numWrenches,&db0,0,Jacobian,1);
00110         blkOffset = 0;
00111 
00112         // J is an individual contact jacobian
00113         J = new double[6*numDOF];
00114 
00115         for (f=0;f<mGrasp->hand->getNumFingers();f++) {
00116                 for (l=0;l<mGrasp->hand->getFinger(f)->getNumLinks();l++) {
00117                         contactList = mGrasp->hand->getFinger(f)->getLink(l)->getContacts();
00118                         if (contactList.empty()) continue;
00119 
00120                         Jee = mGrasp->getLinkJacobian(f,l);
00121 
00122                         for (cp=contactList.begin();cp!=contactList.end();cp++) {
00123                                 if ((*cp)->getBody2() != mGrasp->object) continue;
00124                                 dcopy(6*numDOF,&db0,0,J,1);
00125 
00126                                 // Tec: tranform from link base to contact point
00127                                 Tec = rotate_transf(M_PI,vec3(1,0,0)) * (*cp)->getContactFrame();
00128                                 Tv_ec = new double[36];
00129                                 Tec.jacobian(Tv_ec);
00130                                 // J = Tv_ec * Jee;
00131                                 dgemm("N","N",6,numDOF,6,1.0,Tv_ec,6,Jee,6,0.0,J,6);
00132                                 delete [] Tv_ec;
00133 
00134                                 // we only want to include the portion of J that is admissible
00135                                 // by the contact type
00136             
00137                                 rowOffset = 0;
00138             
00139                                 if ((*cp)->getContactDim() >= 3) {
00140                                         // copy the first row of J (dx/dtheta)
00141                                         dcopy(numDOF,J,6,Jacobian+blkOffset+rowOffset,numWrenches);
00142                                         rowOffset++;
00143                                 // copy the second row of J (dy/dtheta)
00144                                         dcopy(numDOF,J+1,6,Jacobian+blkOffset+rowOffset,numWrenches);
00145                                         rowOffset++;
00146                                 }
00147    
00148                                 // copy the third row of J (dz/dtheta)
00149                                 dcopy(numDOF,J+2,6,Jacobian+blkOffset+rowOffset,numWrenches);
00150                                 rowOffset++;
00151     
00152                                 if ((*cp)->getContactDim() > 3) {
00153                                         // copy the sixth row of J (drz/dtheta)
00154                                         dcopy(numDOF,J+5,6,Jacobian+blkOffset+rowOffset,numWrenches);
00155                                         rowOffset++;
00156                                 }
00157                         blkOffset += rowOffset;
00158                         }
00159                         delete [] Jee;
00160                 }
00161         }  
00162         delete [] J;
00163 
00164 #ifdef GRASPDEBUG
00165         printf("JACOBIAN:\n");
00166         disp_mat(stdout,Jacobian,numWrenches,numDOF,0);
00167 #endif
00168 
00169 }
00170 
00171 
00178 void
00179 LMIOptimizer::buildGraspMap()
00180 {
00181   int i,curLoc = 0;
00182 
00183   if (graspMap) delete [] graspMap;
00184   graspMap = NULL;
00185 
00186   // count the total number of wrenches contributed by all the contacts
00187   numWrenches = 0;
00188   for(i=0;i<mGrasp->numContacts;i++)
00189     numWrenches += mGrasp->contactVec[i]->getContactDim();
00190 
00191   if (mGrasp->numContacts) {
00192     graspCounter++;
00193     graspMap = new double[6*numWrenches];
00194 
00195     /*
00196       This code has not been verified in a long time. Please check that the frictionEdges
00197       below indeed do what you expect them to
00198     */
00199     fprintf(stderr,"Unverified code! Check me first!\n");
00200     // add the basis wrenches of each contact to the grasp map
00201     for (i=0;i<mGrasp->numContacts;i++) {
00202       dcopy(6*mGrasp->contactVec[i]->numFrictionEdges,
00203             mGrasp->contactVec[i]->getMate()->frictionEdges,1,graspMap+curLoc,1);
00204           curLoc += 6 * mGrasp->contactVec[i]->getContactDim();
00205     }
00206 #ifdef GRASPDEBUG
00207     printf("Built GraspMap (6x%d):\n",numWrenches);
00208     disp_mat(stdout,graspMap,6,numWrenches,1);
00209 #endif
00210 
00211   }
00212 }
00213 
00218 void
00219 LMIOptimizer::computeNullSpace() {
00220 
00221   double *tempGraspMap, *work;
00222   double *leftSV, *singularValues, *rightSV, *checkNull, *checkRSV;
00223   double db0=0.0;
00224   int lwork, rank, info;
00225   int numGElements, numCheck, i;
00226   int row = 6, col = numWrenches;
00227   
00228   rank=row;
00229   nullDim=col-rank;
00230   if (nullDim <= 0) return;
00231 
00232   if (nullSpace) free(nullSpace);
00233 
00234   numGElements = row*col;
00235   tempGraspMap=(double*) malloc(numGElements*sizeof(double));
00236   dcopy(numGElements,graspMap,1, tempGraspMap,1);
00237   
00238   lwork=MAX(MAX(1,3*MIN(row,col)+ MAX(row,col)),5*MIN(row,col));  //-4
00239   work=(double*) malloc(lwork*sizeof(double));
00240   singularValues=(double*)malloc(MIN(row,col)*sizeof(double));
00241   leftSV=(double*)malloc(row*row*sizeof(double));
00242   rightSV=(double*)malloc(col*col*sizeof(double));
00243 
00244 #ifdef GRASPDEBUG  
00245   printf("--------- tempGraspMap ------------ \n");
00246   disp_mat(stdout,tempGraspMap,row,col,0);
00247 #endif
00248   
00249   dgesvd("A","A", row, col,tempGraspMap, row, singularValues, leftSV, row,
00250          rightSV, col, work, lwork, &info);
00251   if(info==0){
00252 
00253 #ifdef GRASPDEBUG  
00254       printf("--------- singular values ------------ \n");
00255       disp_mat(stdout,singularValues,1,MIN(row,col),0);
00256       
00257       printf("--------- Left Singular Vectors -------- \n");
00258       disp_mat(stdout, leftSV, row, row,0);
00259       printf("--------- Right Singular Vectors -------- \n");
00260       disp_mat(stdout, rightSV, col, col,0);
00261       
00262       printf("-------- GraspMap*RightSV ------------ \n");
00263 #endif
00264 
00265       checkRSV=(double*)malloc(col*col*sizeof(double));
00266       dcopy(numGElements,graspMap,1, tempGraspMap,1);
00267       dgemm("N","T",row,col,col,1.0,tempGraspMap,row,rightSV,col,0.0,
00268       checkRSV,col);
00269 #ifdef GRASPDEBUG  
00270       disp_mat(stdout, checkRSV, col, col,0);
00271 #endif
00272       free(checkRSV);
00273 
00274     nullSpace=(double *)malloc(col* nullDim *sizeof(double));
00275     for (i=row;i<col;i++)
00276       dcopy(col,rightSV+i,col,nullSpace+(i-row)*col,1);
00277     
00278 #ifdef GRASPDEBUG  
00279       printf("---------- Null Space -----------------\n");
00280       disp_mat(stdout, nullSpace, col, nullDim,0);
00281       
00282       printf("---------- check null space --------- \n");
00283 #endif
00284       numCheck=row*nullDim;
00285       checkNull=(double*)malloc(numCheck*sizeof(double));
00286       dcopy(numCheck, &db0, 0,checkNull,1);
00287       dcopy(numGElements,graspMap,1, tempGraspMap,1);
00288       dgemm("N","N", row, nullDim, col, 1.0, tempGraspMap, row,
00289       nullSpace,col, 0.0, checkNull,row);
00290 #ifdef GRASPDEBUG  
00291       disp_mat(stdout, checkNull, row, nullDim, 0);
00292 #endif
00293       free(checkNull);
00294 
00295     free(tempGraspMap);free(work); 
00296     free(leftSV); free(singularValues); free(rightSV);
00297   }
00298   else {
00299     free(tempGraspMap);free(work);
00300     free(leftSV); free(singularValues); free(rightSV);    
00301     errMsg("error in SVD of grasp map");
00302   }
00303 }
00304 
00317 void
00318 LMIOptimizer::minimalNorm()
00319 {
00320   double *tempGraspMap, *work, *incForce;
00321   int lwork, info;
00322   int numGElements, ldx0, i;
00323   int row = 6, col = numWrenches, nrhs = 1;
00324 
00325   if (minNormSln) free(minNormSln);
00326 
00327   incForce=(double*)malloc(row*sizeof(double));
00328   dcopy(row,mGrasp->object->getExtWrenchAcc(),1,incForce,1);
00329 
00330   // Grasp should generate the _negative_ object wrench
00331   dscal(row,-1.0,incForce,1);
00332 
00333 #ifdef GRASPDEBUG  
00334   printf("---------- Object Wrench --------------------- \n");
00335   disp_mat(stdout, mGrasp->object->getExtWrenchAcc(), row, 1,0);
00336 #endif
00337 
00338   minNormSln = (double*) malloc(MAX(row,col)*nrhs*sizeof(double));
00339   //lwork for dgels_ 
00340   lwork=MAX(1,MIN(row,col)+MAX(row, MAX(col,nrhs)));
00341   work= (double*)malloc(lwork*sizeof(double));
00342   
00343   numGElements = row*col;
00344   tempGraspMap=(double*) malloc(numGElements*sizeof(double));
00345   dcopy(numGElements,graspMap,1, tempGraspMap,1);
00346   
00347   ldx0=MAX(row,col);
00348   for (i=0; i<nrhs;i++)
00349     dcopy(row,incForce+i*row, 1, minNormSln+i*ldx0, 1);
00350   
00351   dgels("N", row, col, nrhs, tempGraspMap, row, minNormSln, ldx0, work,
00352          lwork, &info);
00353   
00354   if(info==0){
00355 #ifdef GRASPDEBUG
00356     printf("preset work length: %d, optimal length: %f \n", lwork, work[0]);
00357     printf("---------- Minimal Norm/Error Solution ---------- \n");
00358     disp_mat(stdout, minNormSln, nrhs, ldx0,0);
00359 #endif
00360     free(work); free(tempGraspMap); free(incForce);
00361   }
00362   else {
00363     free(work); free(tempGraspMap); free(minNormSln); free(incForce);
00364     minNormSln = NULL;
00365     errMsg(" no exact solution exists");
00366   }    
00367 }
00368 
00376 double *
00377 lmiContactLimits(int numForces, int nullDim,  double* nullSpace,
00378                          double * minNormSln, double* lowerBounds,
00379                          double* upperBounds)
00380 {
00381   double *LmiLinear, *tempLower, *tempUpper;
00382   int row, col, nullSpaceSize;
00383   int i;
00384   
00385   tempLower=(double*)malloc(numForces*sizeof(double));
00386   dcopy(numForces, lowerBounds, 1, tempLower, 1);
00387   daxpy(numForces, -1.0, minNormSln, 1, tempLower, 1);  //lower-minNormSln
00388   dscal(numForces,-1.0,tempLower, 1);                    //-(lower-minNormSln)
00389   
00390   tempUpper=(double*)malloc(numForces*sizeof(double));  
00391   dcopy(numForces, upperBounds, 1, tempUpper, 1);
00392   daxpy(numForces, -1.0, minNormSln, 1, tempUpper, 1);  // upper - minNormSln
00393   
00394   row=2*numForces;   col=nullDim+1; 
00395   LmiLinear=(double*)malloc(row*col*sizeof(double));
00396   dcopy(numForces, tempLower, 1, LmiLinear, 1);
00397   dcopy(numForces, tempUpper, 1, LmiLinear+numForces, 1);
00398   
00399   for(i=0;i<nullDim; i++)
00400     dcopy(numForces, nullSpace+i*numForces, 1, LmiLinear+(i+1)*row,1);
00401   nullSpaceSize=numForces*nullDim;
00402   dscal(nullSpaceSize,-1.0, nullSpace, 1);
00403   for(i=0;i<nullDim; i++)
00404     dcopy(numForces, nullSpace+i*numForces, 1, LmiLinear+(i+1)*row+numForces,1);
00405   
00406   dscal(nullSpaceSize,-1.0, nullSpace, 1);
00407   free(tempLower); free(tempUpper);
00408   return(LmiLinear);
00409 }
00410 
00414 double *
00415 LMIOptimizer::lmiTorqueLimits()
00416 {
00417   double  db0=0.0,*tildeJ, *tildeExternalTrq;
00418   int i,tildeJDim=numDOF*nullDim;
00419   double *lmi,*lowerBounds,*upperBounds;
00420 
00421   /*  L=2*numWrenches;
00422   F_blkszs= (int *) malloc(L*sizeof(int));
00423   for (int i=0;i<L;i++) F_blkszs[i]=1;
00424   */
00425 
00426   L=2*numDOF;
00427   F_blkszs= (int *) malloc(L*sizeof(int));
00428   for (i=0;i<L;i++) F_blkszs[i]=1;
00429 
00430   lowerBounds = (double*) malloc(numDOF*sizeof(double));
00431   upperBounds = (double*) malloc(numDOF*sizeof(double));
00432   for (i=0;i<mGrasp->hand->getNumDOF();i++) {
00433     upperBounds[i] = mGrasp->hand->getDOF(i)->getMaxForce()*1.0e-9;
00434     lowerBounds[i] = - mGrasp->hand->getDOF(i)->getMaxForce()*1.0e-9;
00435   }
00436 
00437   tildeJ=(double*) malloc(tildeJDim*sizeof(double));
00438   dcopy(tildeJDim, &db0, 0, tildeJ,1);
00439 
00440   dgemm("T","N", numDOF, nullDim, numWrenches, 1.0, Jacobian, numWrenches,
00441          nullSpace, numWrenches, 0.0, tildeJ, numDOF);
00442 
00443   tildeExternalTrq=(double*) malloc(numDOF*sizeof(double));
00444   dcopy(numDOF, externalTorques, 1, tildeExternalTrq, 1);
00445   
00446   dgemv("T", numWrenches, numDOF, 1.0, Jacobian, numWrenches, minNormSln, 1,
00447          1.0, tildeExternalTrq, 1);
00448 
00449 #ifdef GRASPDEBUG  
00450   printf("tildeExternalTrq:\n");
00451   disp_mat(stdout,tildeExternalTrq,1,numDOF,0);
00452 #endif
00453 
00454   lmi = lmiContactLimits(numDOF, nullDim, tildeJ, tildeExternalTrq,
00455                        lowerBounds, upperBounds);
00456 
00457   free(tildeJ); free(tildeExternalTrq);  free(lowerBounds); free(upperBounds);
00458   return(lmi);
00459 }
00460 
00461 void
00462 LMIOptimizer::lmiFL(double *lmi,int rowInit, int colInit, int totalRow)
00463 {
00464   int x3_post=colInit*totalRow+rowInit;
00465   lmi[x3_post]=1.0;
00466 }
00467 
00468 void
00469 LMIOptimizer::lmiPCWF(double cof, double *lmi,int rowInit, int colInit,
00470                     int totalRow)
00471 {
00472   int x1_post, x2_post, x3_post;
00473 
00474   x1_post=colInit*totalRow+rowInit+2;
00475   lmi[x1_post]=1.0;
00476   
00477   x2_post=(colInit+1)*totalRow+rowInit+4;
00478   lmi[x2_post]=1.0;
00479   
00480   x3_post=(colInit+2)*totalRow+rowInit;
00481   lmi[x3_post]=cof;
00482   lmi[x3_post+3]=cof;
00483   lmi[x3_post+5]=cof;
00484 }
00485 
00486 void
00487 LMIOptimizer::lmiSFCE(double cof, double cof_t,double *lmi,int rowInit,
00488                     int colInit, int totalRow)
00489 {
00490   int x1_post, x2_post, x3_post, x4_post;
00491   double alpha, beta;
00492   
00493   alpha=1.0/sqrt(cof);
00494   beta =1.0/sqrt(cof_t);
00495   
00496   x1_post=colInit*totalRow+rowInit+3;
00497   lmi[x1_post]=alpha;
00498   
00499   x2_post=(colInit+1)*totalRow+rowInit+6;
00500   lmi[x2_post]=alpha;
00501   
00502   x3_post=(colInit+2)*totalRow+rowInit;
00503   lmi[x3_post]=1.0;
00504   lmi[x3_post+4]=1.0;
00505   lmi[x3_post+7]=1.0;
00506   lmi[x3_post+9]=1.0;
00507 
00508   x4_post=(colInit+3)*totalRow+rowInit+8;
00509   lmi[x4_post]=beta;
00510 }
00511 
00512 void
00513 LMIOptimizer::lmiSFCL(double cof, double cof_t,double *lmi,int rowInit,
00514                     int colInit, int totalRow)
00515 {
00516   int colFirst;
00517   double ratio = cof/cof_t;
00518 
00519   // The 1 dimensional index for the first item of this LMI block
00520   // in the x_i column
00521 
00522   //x_1
00523   colFirst=colInit* totalRow+rowInit;
00524   lmi[colFirst+9]=1.0;
00525   lmi[colFirst+24]=1.0;
00526   
00527   //x_2
00528   colFirst=(colInit+1)*totalRow+rowInit;
00529   lmi[colFirst+14]=1.0;
00530   lmi[colFirst+26]=1.0;
00531     
00532   //x_3
00533   colFirst=(colInit+2)*totalRow+rowInit;
00534   lmi[colFirst]   =1.0;
00535   lmi[colFirst+7] =cof;
00536   lmi[colFirst+13]=cof;
00537   lmi[colFirst+18]=cof;
00538   lmi[colFirst+22]=cof;
00539   lmi[colFirst+25]=cof;
00540   lmi[colFirst+27]=cof;
00541   
00542   //x_4
00543   colFirst=(colInit+3)*totalRow+rowInit;
00544   lmi[colFirst+7] =ratio;
00545   lmi[colFirst+13]=ratio;
00546   lmi[colFirst+18]=ratio;
00547   lmi[colFirst+22]=-ratio;
00548   lmi[colFirst+25]=-ratio;
00549   lmi[colFirst+27]=-ratio;
00550 }
00551 
00552 // note that LMI is stored in a packed form where the whole lower triangle of
00553 // each P_i is stored as a single column (remember: column-major format)
00554 //
00555 double *
00556 LMIOptimizer::lmiFrictionCones()
00557 {
00558   double db0=0.0,*initLMI, *finalLMI;
00559   int lmiDim;
00560   int row=0, col=0, *blockRowIndex, *blockColIndex;
00561   int i, initSize, finalSize;
00562 
00563   blockRowIndex = new int[mGrasp->numContacts];
00564   blockColIndex = new int[mGrasp->numContacts];
00565 
00566   K=mGrasp->numContacts;
00567   G_blkszs=(int *)malloc(K*sizeof(int));
00568 
00569   for (i=0;i<mGrasp->numContacts;i++) {
00570     blockRowIndex[i]=row;
00571     blockColIndex[i]=col;
00572     lmiDim = mGrasp->contactVec[i]->getLmiDim();
00573     row += lmiDim*(lmiDim+1)/2;  // size of the lower triangle
00574     col += mGrasp->contactVec[i]->getContactDim();
00575     G_blkszs[i]=lmiDim;
00576   }
00577 
00578   initSize=row*col;
00579   initLMI=(double*)malloc(initSize*sizeof(double));
00580   dcopy(initSize,&db0, 0, initLMI, 1);
00581 
00582   for (i=0;i<mGrasp->numContacts;i++) {
00583     switch(mGrasp->contactVec[i]->getFrictionType()) {
00584       case FL:   lmiFL(initLMI,blockRowIndex[i],blockColIndex[i], row);
00585                  break;
00586       case PCWF: lmiPCWF(mGrasp->contactVec[i]->getCof(),initLMI,blockRowIndex[i],
00587                          blockColIndex[i], row);
00588                  break;
00589       case SFCE: assert(0); //not handled
00590                          //lmiSFCE(mGrasp->contactVec[i]->getCof(),
00591                          //mGrasp->contactVec[i]->getTorsionalCof(),initLMI,
00592                          //blockRowIndex[i], blockColIndex[i], row);
00593                  break;
00594       case SFCL: assert(0); //not handled
00595                      //lmiSFCL(mGrasp->contactVec[i]->getCof(),
00596                          //mGrasp->contactVec[i]->getTorsionalCof(),initLMI,
00597                          //blockRowIndex[i], blockColIndex[i], row);
00598     }
00599   }
00600 #ifdef GRASPDEBUG  
00601   printf("------------- Init LMI(%-dx%-d) ----------------------\n",row, col);
00602   disp_mat(stdout, initLMI, row, col,0);
00603 #endif
00604 
00605   finalSize=row*(nullDim+1);
00606   
00607   finalLMI = (double*) malloc(finalSize*sizeof(double));
00608   dcopy(finalSize,&db0, 0, finalLMI, 1);
00609   dgemm("N","N",row, nullDim, col, 1.0, initLMI, row, nullSpace, col, 
00610          0.0,finalLMI+row, row);
00611   dgemv("N",row,col, 1.0, initLMI, row, minNormSln, 1, 0.0, finalLMI, 1);
00612   free(initLMI);
00613   delete [] blockRowIndex;
00614   delete [] blockColIndex;
00615   GPkRow = row;
00616   return(finalLMI);
00617 }
00618 
00619 double *
00620 LMIOptimizer::weightVec()
00621 {
00622   double  db0=0.0,*initWeights, *finalWeights;
00623   int i,dim,blockIndex=0;
00624   
00625   initWeights=(double*)malloc(numWrenches*sizeof(double));
00626   for (i=0;i<mGrasp->numContacts;i++) {
00627     dim = mGrasp->contactVec[i]->getContactDim();
00628     dcopy(dim,&db0,1,initWeights+blockIndex,1);
00629 
00630     // temporary preset weights
00631     if (dim==1)
00632       *(initWeights+blockIndex) = LMIOptimizer::GFO_WEIGHT_FACTOR;
00633     else
00634       *(initWeights+blockIndex+2) = LMIOptimizer::GFO_WEIGHT_FACTOR;
00635     
00636     blockIndex += dim;
00637   }
00638 
00639 #ifdef GRASPDEBUG
00640   printf("------------ Init Weight Vector(%d) -------------\n",numWrenches);
00641   disp_mat(stdout, initWeights,1,numWrenches,0);
00642   
00643       printf("---------- Null Space -----------------\n");
00644       disp_mat(stdout, nullSpace, numWrenches, nullDim,0);
00645 #endif
00646       
00647   finalWeights=(double*)malloc(nullDim*sizeof(double));
00648   dcopy(nullDim,&db0,0,finalWeights,1);
00649   dgemv("T",numWrenches,nullDim,1.0,nullSpace,numWrenches,initWeights,1,
00650         0.0,finalWeights, 1);
00651   
00652   constOffset = ddot(numWrenches,initWeights,1,minNormSln,1);
00653 
00654 #ifdef GRASPDEBUG
00655   printf("------------ Final Weight Vector(%d) for new variables z & %lf -------------\n",nullDim, constOffset);
00656   disp_mat(stdout, finalWeights,1,nullDim,0);
00657 #endif
00658 
00659   free(initWeights);
00660   return(finalWeights);
00661 }
00662 
00668 double *
00669 combineLMIs(int numVariables, double* lmi1, int lmi1RowSz, double * lmi2,
00670             int lmi2RowSz) {
00671 
00672   double * combinedLMI;
00673   int numElements, totalRowSz;
00674   int i;
00675   
00676   totalRowSz=lmi1RowSz+lmi2RowSz;
00677   numElements=(numVariables+1)*totalRowSz;
00678   combinedLMI=(double*)malloc(numElements*sizeof(double));
00679   for(i=0;i<=numVariables;i++){
00680     dcopy(lmi1RowSz,lmi1+i*lmi1RowSz,1,combinedLMI+i*totalRowSz,1);
00681     dcopy(lmi2RowSz,lmi2+i*lmi2RowSz,1,combinedLMI+i*totalRowSz+lmi1RowSz,1);
00682   }
00683   return(combinedLMI);
00684 }
00685 
00690 double * maxdet_wrap(int m, int L, double *F, int *F_blkszs,
00691                    int K, double *G, int *G_blkszs,
00692                    double *c, double *z0, double *Z, double *W,
00693                    double gamma, double abstol, double reltol,
00694                    int * pNTiters, int negativeFlag, FILE* pRstFile)
00695 
00696 
00697 {
00698   register int i;
00699   int    n, l, max_n, max_l, F_sz, G_sz;  
00700   int    ptr_size;
00701   
00702   int *iwork, lwork;
00703   double db0=0.0,ul[2], *hist, *truncatedHist, *work;
00704   int m3, m2, sourceCol, destCol, destSize;
00705   int info, info2;
00706   
00707   
00708   //  struct tms before,after;
00709   
00710   // Gets rid of compiler warning
00711 #ifndef GRASPDBG
00712   pRstFile = NULL;
00713 #endif
00714   
00715   for (i=0; i<L; i++) {
00716     if (F_blkszs[i] <= 0) {
00717       pr_error("Elements of F_blkszs must be positive.\n");
00718       errorOccurred = true;
00719       return NULL;
00720     }
00721   }
00722   for (i=0; i<K; i++) {
00723     if (G_blkszs[i] <= 0) {
00724       pr_error("Elements of G_blkszs must be positive.\n");
00725       errorOccurred = true;
00726       return NULL;
00727     }
00728   }
00729   
00730   /* various dimensions
00731    * n, l: dimensions
00732    * F_sz, G_sz: length in packed storage
00733    * F_upsz, G_upsz: length in unpacked storage
00734    * max_n, max_l: max block size */
00735   for (i=0, n=0, F_sz=0, max_n=0; i<L; i++) {
00736     n += F_blkszs[i];
00737     F_sz += (F_blkszs[i]*(F_blkszs[i]+1))/2;
00738     max_n = MAX(max_n, F_blkszs[i]);
00739   }
00740   for (i=0, l=0, G_sz=0, max_l=0; i<K; i++) {
00741     l += G_blkszs[i];
00742     G_sz += (G_blkszs[i]*(G_blkszs[i]+1))/2;
00743     max_l = MAX(max_l, G_blkszs[i]);
00744   }
00745   
00746   /* hist (5th output argument) */
00747   ptr_size =(3+m)*(*pNTiters);
00748   //    ptr_size =(3+m)*(NTiters);
00749   hist=(double *) malloc(ptr_size*sizeof(double));
00750   dcopy(ptr_size,&db0, 0, hist, 1);
00751   
00752   /* allocate work space */
00753   lwork = (2*m+5)*(F_sz+G_sz) + 2*(n+l) +
00754     MAX(m+(F_sz+G_sz)*NB,MAX(3*(m+SQR(m)+MAX(G_sz,F_sz)),
00755                              MAX(3*(MAX(max_l,max_n)+MAX(G_sz,F_sz)),
00756                                  MAX(G_sz+3*max_l,F_sz+3*max_n))));
00757   work = (double *) malloc(lwork*sizeof(double));
00758   dcopy(lwork,&db0,0,work,1);
00759   iwork = (int *) malloc(10*m*sizeof(int));
00760   for(i=0;i<10*m;i++)   iwork[i]=0;
00761   
00762   //  times(&before);
00763   info2=maxdet(m,L, (double*)F,(int *) F_blkszs,K,(double *) G,
00764                (int *) G_blkszs,(double *)c,(double *)z0,(double *)Z,
00765                (double *)W, (double *)ul, (double*)hist,gamma,abstol,reltol,
00766                pNTiters,work,lwork,iwork,&info,negativeFlag);
00767   
00768   if (info2) {
00769       free(work); free(iwork); free(hist);
00770       errorOccurred = true;
00771       pr_error("Error in maxdet.\n");
00772       return NULL;
00773   }
00774   
00775   // times(&after);
00776   
00777   //#ifndef REAL_RUN
00778   //  fprintf(pRstFile,"times:  User time: %f seconds\n",(float) (after.tms_utime-before.tms_utime)/sysconf(CLK_TCK));
00779   //#endif
00780   
00781   // infostr 
00782 #ifdef GRASPDEBUG
00783   switch (info) {
00784   case 1:
00785     fprintf(pRstFile, "  maximum Newton iteration exceeded\n");
00786     break;
00787   case 2:
00788     fprintf(pRstFile, "  absolute tolerance reached\n");
00789     break;
00790   case 3:
00791     fprintf(pRstFile, "  relative tolerance reached\n");
00792     break;
00793   case 4:
00794     fprintf(pRstFile, "  negative objective value reached\n");
00795     break;
00796   default:
00797     printf("error occurred in maxdet\n"); 
00798     exit(-1);
00799   }  
00800 #endif
00801 
00802   //  truncate hist
00803   destCol=0;
00804   m3=m+3; m2=m3 -1;
00805   for(sourceCol=0;sourceCol<*pNTiters;sourceCol++) 
00806     if(hist[sourceCol*m3+m2]!=0.0){
00807       dcopy(m3,hist+sourceCol*m3,1,hist+destCol*m3,1);
00808       destCol++;
00809     }
00810   //        printf("init Col: %d  final Col: %d \n", sourceCol, destCol);
00811   destSize=destCol*m3;
00812   truncatedHist=(double *)malloc(destSize*sizeof(double));
00813   dcopy(destSize, hist, 1, truncatedHist, 1);
00814   *pNTiters=destCol;
00815   
00816   
00817   /* free matrices allocated */
00818   free(work); free(iwork); free(hist);
00819   
00820   return(truncatedHist);
00821 }
00822 
00827 void
00828 LMIOptimizer::feasibilityAnalysis()
00829 {
00830   double *eigF, minEigF;
00831   double *eigG, minEigG;
00832   double *work, *hist;
00833   
00834   double *identityF, *identityG;
00835   double *expandF, *expandG;
00836   double *newF, *newG;
00837   int    *newF_blkszs,  newG_blkszs;
00838   double db0=0.0,t0, *x0,*Z0, W0, *c;
00839   int    newM, newL, newK, newPkSz,initSize;
00840   
00841   int F_pksz=0, G_pksz=0;
00842   int F_dim=0, G_dim=0;
00843   int blkSize, maxBlockSize=0;
00844   int i,m,workSize;
00845   //int  negativeFlag=TRUE;
00846     
00847   eigF = new double[numDOF*2]; // max dimension of torque limit LMI
00848   eigG = new double[mGrasp->numContacts*7];    // max dimension of friction LMI
00849   
00850   m = nullDim;
00851   newM=m+1;
00852   newG=(double*)malloc((newM+1)*sizeof(double));
00853   newG[0]=1.0;
00854   dcopy(newM,&db0,0,newG+1,1);
00855   newK=1;
00856   newG_blkszs=1;
00857   W0=0.0;
00858     
00859   c=(double*)malloc(newM*sizeof(double));
00860   dcopy(m,&db0,0,c,1);
00861   c[m]=1.0;
00862   
00863   for(i=0;i<L;i++){
00864     blkSize=F_blkszs[i];
00865     F_dim+=blkSize;
00866     F_pksz+=(blkSize+1)*blkSize/2;
00867     maxBlockSize=MAX(maxBlockSize,blkSize);
00868   }
00869   workSize=F_pksz+3*maxBlockSize;
00870   work=(double*)malloc(workSize*sizeof(double));
00871   minEigF=eig_val(eigF,F,L,F_blkszs,F_pksz,work);
00872   free(work);
00873   
00874   maxBlockSize=0;
00875   for(i=0;i<K;i++){
00876     blkSize=G_blkszs[i];
00877     G_dim+=blkSize;
00878     G_pksz+=(blkSize+1)*blkSize/2;
00879     maxBlockSize=MAX(maxBlockSize,blkSize);
00880   }
00881   workSize=G_pksz+3*maxBlockSize;
00882   work=(double*)malloc(workSize*sizeof(double));
00883   minEigG=eig_val(eigG,G,K,G_blkszs,G_pksz,work);
00884   free(work);
00885   
00886 #ifdef GRASPDEBUG
00887   fprintf(stderr,"minEigF: %f  minEigG:%f \n",minEigF, minEigG);
00888   fprintf(stderr," ------- EigF(%d) ------ \n",F_dim);
00889   disp_mat(stderr,eigF,1,F_dim,0);
00890   fprintf(stderr," ------- EigG(%d) ------ \n",G_dim);
00891   disp_mat(stderr,eigG,1,G_dim,0);
00892 #endif
00893   
00894   t0=MAX(-1.1*MIN(minEigF,minEigG),1.0);
00895   x0=(double*)malloc(newM*sizeof(double));
00896   dcopy(m,&db0,0,x0,1);
00897   x0[m]=t0;
00898   
00899   identityF=(double*)block_Identity(L,F_blkszs,F_pksz);
00900   identityG=(double*)block_Identity(K,G_blkszs,G_pksz);
00901 
00902 #ifdef GRASPDEBUG
00903   fprintf(stderr,"------------- Identity_F ---------------- \n");
00904   disp_mat(stderr,identityF,1,F_pksz,0);
00905   fprintf(stderr,"------------- Identity_G ---------------- \n");
00906   disp_mat(stderr,identityG,1,G_pksz,0);
00907 #endif
00908   
00909   expandF=(double*)malloc((newM+1)*F_pksz*sizeof(double));
00910   initSize=(m+1)*F_pksz;
00911   dcopy(initSize,F,1,expandF,1);
00912   dcopy(F_pksz,identityF,1,expandF+initSize,1);
00913   free(identityF);
00914   
00915   expandG=(double*)malloc((newM+1)*G_pksz*sizeof(double));
00916   initSize=(m+1)*G_pksz;
00917   dcopy(initSize,G,1,expandG,1);
00918   dcopy(G_pksz,identityG,1,expandG+initSize,1);
00919   free(identityG);
00920   
00921   newF=(double*) combineLMIs(newM,expandF,F_pksz,expandG,G_pksz);
00922   free(expandF); free(expandG);
00923   
00924   newL=L+K;
00925   newF_blkszs=(int*)malloc(newL*sizeof(int));
00926   for(i=0;i<L;i++)  newF_blkszs[i]=F_blkszs[i];
00927   for(i=0;i<K;i++)  newF_blkszs[L+i]=G_blkszs[i];
00928   
00929   newPkSz=F_pksz+G_pksz;
00930   Z0=(double*)malloc(newPkSz*sizeof(double));
00931   dcopy(newPkSz,&db0,0,Z0,1);
00932   
00933 #ifdef GRASPDEBUG
00934   fprintf(stderr,"------------- Combined LMI (%d x %d) ---------------- \n", F_pksz+G_pksz, newM+1);
00935   disp_mat(stderr,newF,F_pksz+G_pksz, newM+1,0);
00936   fprintf(stderr,"------------- Combined LMI Block Sizes(%d) ---------------- \n", newL);
00937   disp_imat(stderr,newF_blkszs,1, newL,0);
00938 #endif
00939   
00940   fprintf(stderr,"Checking grasping force feasibility...\n");
00941   hist = maxdet_wrap(newM, newL, newF, newF_blkszs, newK, newG, &newG_blkszs,
00942                      c, x0,Z0, &W0, gamma, abstol,  reltol, &feasNTiters,
00943                      negativeFlag,pRstFile);
00944 
00945   if (errorOccurred) {
00946     free(newF); free(newF_blkszs); free(Z0);
00947     free(newG); free(c); free(x0);
00948     delete [] eigF; delete [] eigG;
00949     return;
00950   }
00951 
00952   if (initz0) free(initz0);
00953   initz0=(double *)malloc(nullDim*sizeof(double));
00954   dcopy(m,x0,1,initz0,1);
00955 
00956   if (feasZHistory) free(feasZHistory);
00957   feasZHistory=(double*)malloc((m+3)*(feasNTiters)*sizeof(double));
00958   for(i=0;i<feasNTiters;i++){
00959     dcopy(newM,hist+i*(newM+3),1,feasZHistory+i*(m+3),1);
00960     dcopy(2,hist+i*(newM+3)+newM+1, 1, feasZHistory+i*(m+3)+m+1,1);
00961   }
00962   if(x0[m]<0) {
00963     feasible=TRUE;
00964 #ifdef GRASPDEBUG
00965     printf("\n Feasible! min eig value: %f \n",-x0[m]);
00966 #endif
00967   }
00968   else feasible=FALSE;
00969   
00970   free(newF); free(newF_blkszs); free(Z0);
00971   free(newG); free(c); free(x0); free(hist);
00972   delete [] eigF; delete [] eigG;
00973 }
00974 
00978 void
00979 LMIOptimizer::optm_EffortBarrier()
00980 {
00981   int    newL, newK;
00982   double *newF, *newG;
00983   int    newF_blkszs,  *newG_blkszs;
00984   double db0=0.0,Z, *W;
00985   int F_pksz=0, G_pksz=0, newPkSz, i, blkSize;
00986   int m;
00987   
00988   
00989   m=nullDim;
00990   if (optmz0) free(optmz0);
00991   optmz0=(double*)malloc(m*sizeof(double));
00992   dcopy(m, initz0, 1, optmz0, 1);
00993 
00994   newL=1;
00995   newF_blkszs=1;
00996   newF=(double*)malloc((m+1)*sizeof(double));
00997   newF[0]=1.0;
00998   dcopy(m,&db0,0,newF+1,1);
00999   Z=db0;
01000   
01001   newK=L+K;
01002   newG_blkszs=(int*)malloc(newK*sizeof(int));
01003   for(i=0;i<L;i++){
01004     blkSize=F_blkszs[i];
01005     newG_blkszs[i]= blkSize;
01006     F_pksz+=(blkSize+1)*blkSize/2;
01007   }
01008   for(i=0;i<K;i++){
01009     blkSize=G_blkszs[i];
01010     newG_blkszs[L+i]= blkSize;
01011     G_pksz+=(blkSize+1)*blkSize/2;
01012   }
01013   newG = (double*) combineLMIs(m,F,F_pksz,G,G_pksz);
01014   newPkSz=F_pksz+G_pksz;
01015   W=(double*)malloc(newPkSz*sizeof(double));
01016   dcopy(newPkSz,&db0,0,W,1);
01017 
01018   if (optmZHistory) free(optmZHistory);
01019   fprintf(stderr,"Optimizing...\n");
01020   optmZHistory =  maxdet_wrap(m,newL, newF, &newF_blkszs, newK, newG,
01021                               newG_blkszs, c, optmz0, &Z, W, gamma, abstol,
01022                               reltol, &optmNTiters, FALSE, pRstFile);
01023 
01024   if (errorOccurred) {
01025     free(newF); free(newG_blkszs); free(W);
01026     free(newG);
01027     return;
01028   }
01029   
01030   // transform the optimal z value to the optimal x value 
01031   if (optmx0) free(optmx0);
01032   optmx0=(double*)malloc(numWrenches*sizeof(double));
01033   dgemv("N", numWrenches, nullDim, 1.0, nullSpace, numWrenches, 
01034         optmz0,1, 0.0, optmx0,1);
01035   daxpy(numWrenches, 1.0, minNormSln, 1, optmx0, 1);
01036 
01037   free(newF); 
01038   free(newG); free(newG_blkszs); free(W);
01039 }
01040 
01046 void
01047 LMIOptimizer::computeObjectives()
01048 {
01049   double *tempG, *zHistory,objective;
01050   int    zHistRowDim;
01051   
01052   double db0=0.0,minEigG, *eigG, *work;
01053   int workSize, maxBlockSize=0, lmiDim=0;
01054   int i,j;
01055 
01056   /* put the initial z value in the first row of the history, then
01057      copy z history after that */
01058   zHistRowDim=nullDim+3;
01059   if (extendOptmZHistory) free(extendOptmZHistory);
01060   extendOptmZHistory=(double*) malloc(zHistRowDim*(optmNTiters+1)*
01061                                            sizeof(double));
01062   dcopy(zHistRowDim,initz0,1,extendOptmZHistory,1);
01063   dcopy(zHistRowDim*optmNTiters,optmZHistory,1,extendOptmZHistory+
01064         zHistRowDim,1);
01065   
01066 
01067   zHistory = extendOptmZHistory;
01068 
01069   tempG=(double *)malloc(GPkRow*sizeof(double));
01070   
01071   for(i=0;i<K;i++){
01072     maxBlockSize=MAX(maxBlockSize, G_blkszs[i]);
01073     lmiDim+=G_blkszs[i];
01074   }
01075   workSize=GPkRow+3*maxBlockSize;
01076   work=(double *)malloc(workSize*sizeof(double));
01077   eigG=(double *)malloc(lmiDim*sizeof(double));
01078 
01079 
01080   for(i=0;i<optmNTiters+1;i++) {
01081     objective=ddot(nullDim,c,1,zHistory+i*zHistRowDim,1);
01082     objective+=constOffset;
01083     
01084     dcopy(GPkRow,G,1, tempG,1);
01085     dgemv("N", GPkRow, nullDim, 1.0, G+GPkRow, GPkRow,
01086            zHistory+i*zHistRowDim,1,1.0, tempG, 1);
01087     
01088     dcopy(workSize,&db0,0,work,1);
01089     dcopy(lmiDim,&db0,0,eigG,1);
01090     minEigG=eig_val(eigG,tempG,K,G_blkszs,GPkRow,work);
01091     if(minEigG<0)       errMsg("G(x) not positive definite");
01092     for(j=0;j<lmiDim; j++)
01093       objective -=log(eigG[j]);
01094     
01095     zHistory[i*zHistRowDim+nullDim]=objective;
01096   }
01097   free(tempG);  free(work); free(eigG);
01098 }
01099 
01100 
01104 double *
01105 LMIOptimizer::xzHistoryTransfrom(double *zHistory,int numIters)
01106 {
01107   double db0=0.0,*xHistory;
01108   int xHistSize, xHistRowSize, zHistRowSize, i;
01109 
01110 
01111   xHistRowSize=numWrenches+1;           // X+objective
01112   xHistSize=xHistRowSize*numIters;
01113   xHistory=(double*)malloc(xHistSize*sizeof(double));
01114   dcopy(xHistSize,&db0,0,xHistory, 1);
01115   
01116   zHistRowSize=nullDim+3;
01117 
01118   for(i=0;i<numIters;i++){
01119     dgemv("N", numWrenches, nullDim, 1.0, nullSpace, numWrenches, 
01120            zHistory+i*zHistRowSize,1, 0.0, xHistory+i*xHistRowSize,1);
01121     daxpy(numWrenches, 1.0, minNormSln, 1, xHistory+i*xHistRowSize, 1);
01122   }
01123   dcopy(numIters,zHistory+nullDim, zHistRowSize, xHistory+numWrenches,
01124         xHistRowSize);
01125 
01126   return(xHistory);
01127 }
01128 
01133 int
01134 LMIOptimizer::findOptimalGraspForce()
01135 {
01136   int i;
01137 
01138   feasible = FALSE;
01139 
01140   if (numWrenches < 6) {
01141     printf("More contacts are necessary before solving for the optimal grasp force\n");
01142     return FAILURE;
01143   }
01144 
01145 #ifdef GRASPDEBUG  
01146   char rstFile[40];
01147   sprintf(rstFile,"Grasp%02d.rst",graspCounter);
01148   if ((pRstFile=fopen(rstFile,"w"))==NULL)
01149     FatalErrMsg("failed to open result file");
01150 
01151   //  graspMap = ReadGraspMap("mytest.G",18);
01152   printf("---------------- Solve Grasping Force Equation ---------------- \n");
01153 #endif
01154 
01155   minimalNorm();
01156   // if (optmx0) {free(optmx0); optmx0=NULL;}
01157   if (errorOccurred) {errorOccurred = false; if (pRstFile) fclose(pRstFile);return FAILURE;}
01158 
01159   computeNullSpace();
01160   if (errorOccurred) {errorOccurred = false; if (pRstFile) fclose(pRstFile);return FAILURE;}
01161 
01162   if (nullDim == 0) {
01163     printf("The dimension of the null space is 0. Cannot solve for the optimal grasp force.\n");
01164 
01165     return FAILURE;
01166   }
01167  
01168 #ifdef GRASPDEBUG  
01169   printf("----------- Prepare LMIs for Torque/Contact Limits --------------- \n");
01170 #endif
01171 
01172   if (F) free(F);
01173   F = lmiTorqueLimits();
01174  
01175 #ifdef GRASPDEBUG
01176   printf("----------- Prepare LMIs for Friction Cones --------------- \n");
01177 #endif
01178   
01179   if (G) free(G);
01180   G = lmiFrictionCones();
01181 
01182 #ifdef GRASPDEBUG
01183   printf("--------- Prepare Weight Vectors in the Objective Function ------- \n");
01184 #endif
01185 
01186   if (c) free(c);
01187   c = weightVec();
01188   
01189 #ifdef GRAPSDEBUG
01190   printf("\n -------- The  Feasibility Phase ----------------------- \n");
01191   fprintf(pRstFile, "\n -------- The  Feasibility Phase ----------------------- \n");
01192 #endif
01193 
01194   feasNTiters=MAX_FEAS_LOOPS;  
01195   feasibilityAnalysis();
01196   if (errorOccurred) {errorOccurred = false; if (pRstFile) fclose(pRstFile); return FAILURE;}
01197 
01198   if (feasXHistory) free(feasXHistory);
01199   feasXHistory=xzHistoryTransfrom(feasZHistory, feasNTiters);
01200 
01201   if(feasible) {
01202 #ifdef GRASPDEBUG
01203     printf("\n -------- The  Optimization  Phase ----------------------- \n");
01204     fprintf(pRstFile, "\n -------- The  Optimization  Phase ----------------------- \n");
01205 #endif
01206 
01207     optmNTiters=MAX_OPTM_LOOPS;
01208     optm_EffortBarrier();
01209     if (errorOccurred) {feasible = FALSE; errorOccurred = false; if (pRstFile) fclose(pRstFile); return FAILURE;}
01210     
01211     computeObjectives();
01212     if (errorOccurred) {feasible = FALSE; errorOccurred = false; if (pRstFile) fclose(pRstFile); return FAILURE;}
01213 
01214     if (optmXHistory) free(optmXHistory);
01215     optmXHistory = xzHistoryTransfrom(extendOptmZHistory, optmNTiters+1);
01216     
01217     printf("OPTIMAL CONTACT FORCES:\n");
01218     disp_mat(stdout,optmx0,1,numWrenches,0);
01219 
01220     //    double *testOut = new double[6];
01221     //    dgemv("N",6,numWrenches,1.0,graspMap,6,optmx0,1,0.0,testOut,1);
01222     //    printf("CHECK:\n");
01223     //    for (i=0;i<6;i++)
01224     //      printf("%15.12le ",testOut[i]);
01225     //    printf("\n");
01226     //    delete [] testOut;
01227 
01228     if (optTorques) delete [] optTorques;
01229     optTorques = new double[numDOF];
01230 
01231     dcopy(numDOF,externalTorques,1,optTorques,1);
01232     dgemv("T", numWrenches, numDOF, 1.0, Jacobian, numWrenches,
01233           optmx0, 1,1.0, optTorques, 1);
01234 
01235     printf("OPTIMAL TORQUES:\n");
01236     disp_mat(stdout,optTorques,1,numDOF,0);
01237 
01238     int offset = 0;
01239     for (i=0;i<mGrasp->numContacts;i++) {
01240       mGrasp->contactVec[i]->getMate()->setContactForce(optmx0+offset);
01241       offset += mGrasp->contactVec[i]->getContactDim();
01242     }
01243   }
01244   else {
01245     // prompt that the current grasp force is infeasible.
01246     printf("not feasible.\n");
01247 #ifdef GRASPDEBUG
01248     fprintf(pRstFile,"\n --------  Problem Infeasible ----------------------- \n");
01249 #endif
01250     return FAILURE;
01251   }
01252 
01253 
01254 // save the problem parameters, the timing results and the feaibility as well as 
01255 // optimization results at the last simulation step to file argv[1].rst
01256 #ifdef GRASPDEBUG
01257   fprintf(pRstFile, "-------------- Jacobian (%-dx%-d) -------------- \n",numWrenches,numDOF);
01258   disp_mat(pRstFile, Jacobian, numWrenches, numDOF, 0);
01259 
01260   fprintf(pRstFile, "-------------- External Torques (%-d)------------------ \n",numDOF);
01261   disp_mat(pRstFile, externalTorques,1,numDOF,0);
01262   
01263   fprintf(pRstFile, "-------------- Grasp Map (6x%-d) -------------- \n", numWrenches);
01264   disp_mat(pRstFile, graspMap,6, numWrenches,0);
01265   
01266   fprintf(pRstFile, "-------------- Object Wrench (6) -------------- \n");
01267   disp_mat(pRstFile, mGrasp->object->getExtWrenchAcc(), 1, 6, 0);
01268   
01269   fprintf(pRstFile, "-------------- Minimal Norm Solution (%-d)------------------ \n",numWrenches);
01270   disp_mat(pRstFile, minNormSln,1,numWrenches,0);
01271   
01272   fprintf(pRstFile, "-------------- Admissible Null Space (%-dx%-d)------------------ \n",numWrenches, nullDim);
01273   disp_mat(pRstFile, nullSpace, numWrenches, nullDim,0);
01274   
01275   fprintf(pRstFile, "-------------- F blkszs (%d)------------- \n",L);
01276   disp_imat(pRstFile, F_blkszs, 1, L, 0);
01277   
01278   fprintf(pRstFile, "-------------- F (%-dx%-d) ------------- \n",L,nullDim+1);
01279   disp_mat(pRstFile, F, L, nullDim+1,0);
01280   
01281   fprintf(pRstFile, "-------------- G blkszs (%d)------------- \n",K);
01282   disp_imat(pRstFile, G_blkszs, 1, K,0);
01283   
01284   fprintf(pRstFile,"-------------- G (%-dx%-d) -------------- \n",GPkRow,
01285           nullDim+1);
01286   disp_mat(pRstFile, G, GPkRow, nullDim+1,0);
01287   
01288   fprintf(pRstFile,"-------- Final Weight Vector(%d) and Offset: %f -------\n",
01289           nullDim,constOffset);
01290   disp_mat(pRstFile, c,1,nullDim,0);
01291   
01292   fprintf(pRstFile,"------------- Initial Feasible z and History(%dx%d) -------------- \n",
01293           3+nullDim,feasNTiters);
01294   disp_mat(pRstFile, initz0,1,nullDim,0);
01295   disp_mat(pRstFile, feasZHistory, 3+nullDim, feasNTiters, 0);
01296   
01297   fprintf(pRstFile,"-------------  Feasible X History(%dx%d) ---------------- \n",
01298           numWrenches+1,feasNTiters);
01299   disp_mat(pRstFile, feasXHistory, numWrenches+1, feasNTiters, 0);
01300   
01301   if(feasible) {
01302     fprintf(pRstFile,"------------- Optimal z and History(%dx%d)------------------ \n",
01303             3+nullDim,optmNTiters+1);
01304     disp_mat(pRstFile, optmz0,1,nullDim,0);
01305     disp_mat(pRstFile, extendOptmZHistory, 3+nullDim, optmNTiters+1, 0);
01306     
01307     fprintf(pRstFile,"------------- Optimal X and History(%dx%d)------------------ \n",
01308             numWrenches+1,optmNTiters+1);
01309     disp_mat(pRstFile, optmXHistory, numWrenches+1, optmNTiters+1, 0);
01310   }
01311 
01312   fclose(pRstFile);  
01313 #endif
01314 
01315   return SUCCESS;
01316 
01317 }
01318
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/lmiOptimizer.cpp
