graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/EGPlanner/graspTesterThread.cpp Source 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: graspTesterThread.cpp,v 1.15 2009/05/07 19:57:26 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "world.h"
00027 #include "body.h"
00028 #include "robot.h"
00029 #include "graspTesterThread.h"
00030 #include "searchState.h"
00031 #include "searchEnergy.h"
00032 
00033 //#define GRASPITDBG
00034 #include "debug.h"
00035 
00036 GraspTester::GraspTester(Hand *h)
00037 {
00038         mHand = h;
00039         init();
00040         mEnergyCalculator = new SearchEnergy();
00041         mEnergyCalculator->setType(ENERGY_STRICT_AUTOGRASP);
00042         mCurrentStep = 0; mMaxSteps = 1; //run forever
00043         mMaxCandidates = 20; mNumCandidates = 0;
00044 }
00045 
00046 GraspTester::~GraspTester()
00047 {
00048         clearBuffers();
00049 }
00050 
00051 void
00052 GraspTester::setEnergyType(SearchEnergyType)
00053 {
00054         DBGA("Grasp tester only uses STRICT AUTPGRASP energy");
00055 }
00056 
00057 bool
00058 GraspTester::resetPlanner()
00059 {
00060         if (!EGPlanner::resetPlanner()) return false;
00061         clearBuffers();
00062         return true;
00063 }
00064 
00065 void
00066 GraspTester::mainLoop()
00067 {
00068         GraspPlanningState *s = popCandidate();
00069 
00070         if (!s) {
00071                 DBGP("Empty buffer for tester");
00072                 msleep(100);
00073                 return;
00074         }
00075         s->changeHand(mHand,true);
00076         testGrasp(s);
00077         DBGP("TESTER: candidate has energy " << s->getEnergy());
00078         mHand->breakContacts();
00079         if (s->isLegal() && s->getEnergy() < -1.2) {
00080                 //save the final grasping position that has resulted from autograsp
00081                 s->setPositionType(SPACE_COMPLETE);
00082                 s->setPostureType(POSE_DOF);
00083                 //save the current transform in absolute terms
00084                 s->setRefTran(transf::IDENTITY);
00085                 s->saveCurrentHandState();
00086                 postSolution(s);
00087                 DBGP("Tester posting a solution at iteration " << s->getItNumber());
00088         } else {
00089                 DBGP("Tester removing candidate");
00090                 delete s;
00091         }
00092 }
00093 
00094 GraspPlanningState*
00095 GraspTester::popSolution()
00096 {
00097         GraspPlanningState *s = NULL;
00098         mListMutex.lock();
00099         if ( !mSolutionList.empty() ) {
00100                 s = mSolutionList.front();
00101                 mSolutionList.pop_front();
00102         }
00103         mListMutex.unlock();
00104         return s;
00105 }
00106 
00107 void
00108 GraspTester::postSolution(GraspPlanningState *s)
00109 {
00110         mListMutex.lock();
00111         mSolutionList.push_back(s);
00112         mListMutex.unlock();
00113 }
00114 
00115 GraspPlanningState*
00116 GraspTester::popCandidate()
00117 {
00118         GraspPlanningState *s = NULL;
00119         mListMutex.lock();
00120         if ( !mCandidateList.empty() ) {
00121                 s = mCandidateList.front();
00122                 mCandidateList.pop_front();
00123         }
00124         mNumCandidates = (int)mCandidateList.size();
00125         mListMutex.unlock();
00126         return s;
00127 }
00128 
00129 bool
00130 GraspTester::postCandidate(GraspPlanningState *s)
00131 {
00132         mListMutex.lock();
00133         if ( (int)mCandidateList.size() > mMaxCandidates ) {
00134                 DBGP("Tester buffer size: " << mCandidateList.size());
00135                 mListMutex.unlock();
00136                 return false;
00137         }
00138         mCandidateList.push_back(s);
00139         mNumCandidates = (int)mCandidateList.size();
00140         mListMutex.unlock();
00141         return true;
00142 }
00143 
00144 void
00145 GraspTester::clearBuffers()
00146 {
00147         std::list<GraspPlanningState*>::iterator it;
00148         mListMutex.lock();
00149 
00150         for (it = mCandidateList.begin(); it!= mCandidateList.end(); it++) {
00151                 delete (*it);
00152         }
00153         mCandidateList.clear();
00154         mNumCandidates = 0;
00155         for (it = mSolutionList.begin(); it!= mSolutionList.end(); it++) {
00156                 delete (*it);
00157         }
00158         mSolutionList.clear();
00159         mListMutex.unlock();
00160 }
00161 
00162 void
00163 GraspTester::testGrasp(GraspPlanningState *s)
00164 {
00165         bool legal; double energy;
00166         //test will leave the hand in the tested state, if it is legal!
00167         mEnergyCalculator->analyzeState(legal, energy, s, false);
00168         if (!legal) {
00169                 DBGA("Illegal state in tester thread!");
00170                 s->setLegal(false);
00171                 return;
00172         }
00173         s->setEnergy(energy);
00174 }