/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/EGPlanner/timeTest.cpp
Go to the documentation of this file.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: timeTest.cpp,v 1.14 2009/12/01 18:54:03 cmatei Exp $ 00023 // 00024 //###################################################################### 00025 00026 #include "timeTest.h" 00027 00028 #include "searchState.h" 00029 #include "searchEnergy.h" 00030 #include "robot.h" 00031 00032 //#define GRASPITDBG 00033 #include "debug.h" 00034 00035 void 00036 TimeTester::startPlanner() 00037 { 00038 mCount = 0; 00039 mIllegalCount = 0; 00040 SimAnnPlanner::startPlanner(); 00041 } 00042 00043 void 00044 TimeTester::mainLoop() 00045 { 00046 GraspPlanningState *sn = new GraspPlanningState(mCurrentState); 00047 SearchVariable *var; 00048 double v; 00049 double r = ((float)rand()) / RAND_MAX - 0.5; 00050 for (int i=0; i<sn->getNumVariables(); i++) { 00051 var = sn->getVariable(i); 00052 if ( var->isFixed() ) continue; 00053 v = var->getValue() + r * ( var->mMaxJump ) * 0.1; 00054 var->setValue(v); 00055 } 00056 bool legal; double energy; 00057 mEnergyCalculator->analyzeState(legal, energy, sn); 00058 if (legal) mCount++; 00059 else mIllegalCount++; 00060 delete sn; 00061 } 00062 00063 TimeTester *MTTester::startChild() 00064 { 00065 TimeTester *child = new TimeTester(mHand); 00066 child->startThread(); 00067 child->setEnergyType(ENERGY_AUTOGRASP_QUALITY); 00068 child->setModelState(mCurrentState); 00069 child->resetPlanner(); 00070 return child; 00071 } 00072 00073 void 00074 MTTester::startPlanner() 00075 { 00076 int numChildren = 3; 00077 00078 assert(mCurrentState); 00079 mCurrentState->setRefTran(mHand->getTran(), false); 00080 00081 mChildren.clear(); 00082 for (int i=0; i<numChildren; i++) { 00083 TimeTester *child = startChild(); 00084 mChildren.push_back( child ); 00085 } 00086 DBGA("Children ready"); 00087 for (int i=0; i<(int)mChildren.size(); i++){ 00088 mChildren[i]->startPlanner(); 00089 } 00090 DBGA("Children started"); 00091 setState(RUNNING); 00092 } 00093 00094 void 00095 MTTester::pausePlanner() 00096 { 00097 for (int i=0; i<(int)mChildren.size(); i++){ 00098 mChildren[i]->stopPlanner(); 00099 } 00100 00101 int count = 0, illegal = 0; 00102 for (int i=0; i<(int)mChildren.size(); i++){ 00103 count += mChildren[i]->getCount(); 00104 illegal += mChildren[i]->getIllegal(); 00105 DBGA("Child " << i << ": " << mChildren[i]->getCount() << " grasps.") 00106 } 00107 00108 DBGA(count << " grasps."); 00109 DBGA("Illegal states: " << illegal); 00110 setState(READY); 00111 }