graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/EGPlanner/simAnnPlanner.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: simAnnPlanner.cpp,v 1.16 2009/05/07 19:57:26 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "simAnnPlanner.h"
00027 
00028 #include "searchState.h"
00029 #include "searchEnergy.h"
00030 #include "simAnn.h"
00031 
00032 //#define GRASPITDBG
00033 #include "debug.h"
00034 
00036 #define BEST_LIST_SIZE 20
00037 
00038 #define DISTANCE_THRESHOLD 0.3
00039 
00040 SimAnnPlanner::SimAnnPlanner(Hand *h)
00041 {
00042         mHand = h;
00043         init();
00044         mEnergyCalculator = new SearchEnergy();
00045         mSimAnn = new SimAnn();
00046         //mSimAnn->writeResults(true);
00047 }
00048 
00049 SimAnnPlanner::~SimAnnPlanner()
00050 {
00051         if (mSimAnn) delete mSimAnn;
00052 }
00053 
00054 void
00055 SimAnnPlanner::setAnnealingParameters(AnnealingType y) {
00056         if (isActive()) {
00057                 DBGA("Stop planner before setting ann parameters");
00058                 return;
00059         }
00060         mSimAnn->setParameters(y);
00061 }
00062 
00063 void
00064 SimAnnPlanner::resetParameters()
00065 {
00066         EGPlanner::resetParameters();
00067         mSimAnn->reset();
00068         mCurrentStep = mSimAnn->getCurrentStep();
00069         mCurrentState->setEnergy(1.0e8);
00070 }
00071 
00072 bool
00073 SimAnnPlanner::initialized()
00074 {
00075         if (!mCurrentState) return false;
00076         return true;
00077 }
00078 
00079 void
00080 SimAnnPlanner::setModelState(const GraspPlanningState *modelState)
00081 {
00082         if (isActive()) {
00083                 DBGA("Can not change model state while planner is running");
00084                 return;
00085         }
00086 
00087         if (mCurrentState) delete mCurrentState;
00088         mCurrentState = new GraspPlanningState(modelState);
00089         mCurrentState->setEnergy(1.0e5);
00090         //my hand might be a clone
00091         mCurrentState->changeHand(mHand, true);
00092 
00093         if (mTargetState && (mTargetState->readPosition()->getType() != mCurrentState->readPosition()->getType() ||
00094                                                  mTargetState->readPosture()->getType() != mCurrentState->readPosture()->getType() ) ) {
00095                 delete mTargetState; mTargetState = NULL;
00096     }
00097         if (!mTargetState) {
00098                 mTargetState = new GraspPlanningState(mCurrentState);
00099                 mTargetState->reset();
00100                 mInputType = INPUT_NONE;
00101         }
00102         invalidateReset();
00103 }
00104 
00105 void
00106 SimAnnPlanner::mainLoop()
00107 {
00108         GraspPlanningState *input = NULL;
00109         if ( processInput() ) {
00110                 input = mTargetState;
00111         }
00112 
00113         //call sim ann
00114         SimAnn::Result result = mSimAnn->iterate(mCurrentState, mEnergyCalculator, input);
00115         if ( result == SimAnn::FAIL) {
00116                 DBGP("Sim ann failed");
00117                 return;
00118         }
00119         DBGP("Sim Ann success");
00120 
00121         //put result in list if there's room or it's better than the worst solution so far
00122         double worstEnergy;
00123         if ((int)mBestList.size() < BEST_LIST_SIZE) worstEnergy = 1.0e5;
00124         else worstEnergy = mBestList.back()->getEnergy();
00125         if (result == SimAnn::JUMP && mCurrentState->getEnergy() < worstEnergy) {
00126                 GraspPlanningState *insertState = new GraspPlanningState(mCurrentState);
00127                 //but check if a similar solution is already in there
00128                 if (!addToListOfUniqueSolutions(insertState,&mBestList,0.2)) {
00129                         delete insertState;
00130                 } else {
00131                         mBestList.sort(GraspPlanningState::compareStates);
00132                         while ((int)mBestList.size() > BEST_LIST_SIZE) {
00133                                 delete(mBestList.back());
00134                                 mBestList.pop_back();
00135                         }
00136                 }
00137         }
00138         render();
00139         mCurrentStep = mSimAnn->getCurrentStep();
00140         if (mCurrentStep % 100 == 0 && !mMultiThread) emit update();
00141         if (mMaxSteps == 200) {DBGP("Child at " << mCurrentStep << " steps");}
00142 }