/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/EGPlanner/loopPlanner.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: loopPlanner.cpp,v 1.7 2009/10/08 16:20:31 cmatei Exp $ 00023 // 00024 //###################################################################### 00025 00026 #include "loopPlanner.h" 00027 00028 #include "robot.h" 00029 #include "world.h" 00030 #include "simAnn.h" 00031 #include "searchEnergy.h" 00032 #include "searchState.h" 00033 00034 //#define GRASPITDBG 00035 #include "debug.h" 00036 00037 LoopPlanner::LoopPlanner(Hand *h) 00038 { 00039 mHand = h; 00040 init(); 00041 mEnergyCalculator = new ClosureSearchEnergy(); 00042 mEnergyCalculator->setType(ENERGY_CONTACT_QUALITY); 00043 ((ClosureSearchEnergy*)mEnergyCalculator)->setAvoidList( &mAvoidList ); 00044 00045 mSimAnn = new SimAnn(); 00046 mSimAnn->setParameters(ANNEAL_LOOP); 00047 mRepeat = true; 00048 00049 mDistanceThreshold = 0.1f; 00050 ((ClosureSearchEnergy*)mEnergyCalculator)->setThreshold(mDistanceThreshold); 00051 } 00052 00053 LoopPlanner::~LoopPlanner() 00054 { 00055 } 00056 00060 void LoopPlanner::resetParameters() 00061 { 00062 while (!mBestList.empty()) { 00063 GraspPlanningState *s = mBestList.front(); 00064 mBestList.pop_front(); 00065 if (s->getEnergy() > 10.0) { 00066 delete s; 00067 } else { 00068 mAvoidList.push_back( s ); 00069 } 00070 } 00071 SimAnnPlanner::resetParameters(); 00072 emit loopUpdate(); 00073 } 00074 00075 const GraspPlanningState* 00076 LoopPlanner::getGrasp(int i) 00077 { 00078 DBGP("Loop get grasp"); 00079 assert (i>=0 && i<(int)mAvoidList.size()); 00080 std::list<GraspPlanningState*>::iterator it = mAvoidList.begin(); 00081 for (int k=0; k<i; k++) { 00082 it++; 00083 } 00084 return (*it); 00085 } 00086 00087 void 00088 LoopPlanner::clearSolutions() 00089 { 00090 SimAnnPlanner::clearSolutions(); 00091 while ( !mAvoidList.empty() ) { 00092 delete(mAvoidList.back()); 00093 mAvoidList.pop_back(); 00094 } 00095 }