adding list planner

CMakeLists.txt

@@ -526,7 +526,7 @@ set (GRASPIT_FORMS
     ${GSRC}/ui/settingsDlg.ui 
     ${GSRC}/ui/Planner/plannerdlg.ui 
     ${GSRC}/ui/EGPlanner/egPlannerDlg.ui 
-    ${GSRC}/ui/EGPlanner/compliantPlannerDlg.ui
+    ${GSRC}/ui/EGPlanner/listPlannerDlg.ui
 )
 
 set (GRASPIT_FORM_HEADERS 
@@ -544,7 +544,7 @@ set (GRASPIT_FORM_HEADERS
     ${GSRC}/ui/qmDlg.h 
     ${GSRC}/ui/Planner/plannerdlg.h 
     ${GSRC}/ui/EGPlanner/egPlannerDlg.h 
-    ${GSRC}/ui/EGPlanner/compliantPlannerDlg.h
+    ${GSRC}/ui/EGPlanner/listPlannerDlg.h
 )
 
 set (GRASPIT_FORM_SOURCES
@@ -562,7 +562,7 @@ set (GRASPIT_FORM_SOURCES
     ${GSRC}/ui/settingsDlg.cpp 
     ${GSRC}/ui/Planner/plannerdlg.cpp 
     ${GSRC}/ui/EGPlanner/egPlannerDlg.cpp 
-    ${GSRC}/ui/EGPlanner/compliantPlannerDlg.cpp
+    ${GSRC}/ui/EGPlanner/listPlannerDlg.cpp
 )
 
 set (MOC_HEADERS
@@ -572,7 +572,7 @@ set (MOC_HEADERS
     ${GSRC}/ui/bodyPropDlg.h
     ${GSRC}/ui/eigenGraspDlg.h
     ${GSRC}/ui/EGPlanner/egPlannerDlg.h
-    ${GSRC}/ui/EGPlanner/compliantPlannerDlg.h
+    ${GSRC}/ui/EGPlanner/listPlannerDlg.h
     ${GSRC}/ui/Planner/plannerdlg.h
     ${GSRC}/ui/graspCaptureDlg.h
     ${GSRC}/ui/contactExaminerDlg.h

include/EGPlanner/listPlanner.h

@@ -59,7 +59,11 @@ class ListPlanner : public EGPlanner
     virtual void mainLoop();
 
     //! Pulls the state at the given index from the input list
-    GraspPlanningState *getState(int index);
+    GraspPlanningState *getInitialState(int index);
+
+    //! Pulls the state at the given index from the final list
+    GraspPlanningState *getFinalState(int index);
+
   public:
     ListPlanner(Hand *h);
     ~ListPlanner();
@@ -79,7 +83,10 @@ class ListPlanner : public EGPlanner
     void testState(int index);
 
     //! Shows the unprocessed state at the given index in the input list
-    void showState(int index);
+    void showInitialState(int index);
+
+    //! Shows the state at the given index in the best list
+    void showFinalState(int index);
 
     //! A hack; shows the state and also does approachToContact as the SearchEnergy would
     void prepareState(int index);

src/EGPlanner/listPlanner.cpp

@@ -34,151 +34,179 @@
 
 //this should be a parameter of the EGPlanner
 //iros09
-#define BEST_LIST_SIZE 2000
+#define BEST_LIST_SIZE 20000
 
 ListPlanner::ListPlanner(Hand *h)
 {
-  mHand = h;
-  init();
-  mEnergyCalculator = SearchEnergy::getSearchEnergy(ENERGY_CONTACT);
-  mEnergyCalculator->disableRendering(false);
+	mHand = h;
+	init();
+    mEnergyCalculator = SearchEnergy::getSearchEnergy(ENERGY_CONTACT);
+	mEnergyCalculator->disableRendering(false);
 }
 
 /*! The destructor also eliminates the list of input grasps, therefore
-  care must be excercised when passing grasps to this class.
+	care must be excercised when passing grasps to this class. 
 */
 ListPlanner::~ListPlanner()
 {
-  while (!mInputList.empty()) {
-    delete mInputList.back();
-    mInputList.pop_back();
-  }
+	while (!mInputList.empty()){
+		delete mInputList.back();
+		mInputList.pop_back();
+	}
 }
 
-void
-ListPlanner::resetParameters() {
-  EGPlanner::resetParameters();
-  mPlanningIterator = mInputList.begin();
-  if (mCurrentState) { delete mCurrentState; }
-  mCurrentState = new GraspPlanningState(*mPlanningIterator);
+void 
+ListPlanner::resetParameters(){
+	EGPlanner::resetParameters();
+	mPlanningIterator = mInputList.begin();
+	if(mCurrentState) delete mCurrentState;
+	mCurrentState = new GraspPlanningState(*mPlanningIterator);
 }
 
-void
-ListPlanner::setInput(std::list<GraspPlanningState *> input)
+void 
+ListPlanner::setInput(std::list<GraspPlanningState*> input)
 {
-  if (isActive()) {
-    DBGA("Can not change input while planner is running");
-    return;
-  }
-  while (!mInputList.empty()) {
-    delete mInputList.back();
-    mInputList.pop_back();
-  }
-  mInputList = input;
-  mMaxSteps = input.size();
-  invalidateReset();
+	if (isActive()) {
+		DBGA("Can not change input while planner is running");
+		return;
+	}
+	while (!mInputList.empty()){
+		delete mInputList.back();
+		mInputList.pop_back();
+	}
+	mInputList = input;
+	mMaxSteps = input.size();
+	invalidateReset();
 }
 
-GraspPlanningState *
-ListPlanner::getState(int index)
+GraspPlanningState*
+ListPlanner::getInitialState(int index)
 {
-  std::list<GraspPlanningState *>::iterator it = mInputList.begin();
-  int count = 0;
-  while (it != mInputList.end() && count < index) {
-    count++; it++;
-  }
-  if (it == mInputList.end()) {
-    DBGA("Requested grasp not in list");
-    return NULL;
-  }
-  return *it;
+	std::list<GraspPlanningState*>::iterator it = mInputList.begin();
+	int count = 0;
+	while (it!=mInputList.end() && count < index) {
+		count++; it++;
+	}
+	if (it==mInputList.end()) {
+		DBGA("Requested grasp not in list");
+		return NULL;
+	}
+	return *it;
 }
 
-void
+GraspPlanningState*
+ListPlanner::getFinalState(int index)
+{
+    std::list<GraspPlanningState*>::iterator it = mBestList.begin();
+    int count = 0;
+    while (it!=mBestList.end() && count < index) {
+        count++; it++;
+    }
+    if (it==mBestList.end()) {
+        DBGA("Requested grasp not in list");
+        return NULL;
+    }
+    return *it;
+}
+
+void 
 ListPlanner::testState(int index)
 {
-  GraspPlanningState *state = getState(index);
-  if (!state) { return; }
-  bool legal; double energy;
-  mEnergyCalculator->analyzeState(legal, energy, state, false);
-  DBGA("Energy: " << energy);
+    GraspPlanningState *state = getInitialState(index);
+	if (!state) return;
+	bool legal; double energy;
+	mEnergyCalculator->analyzeState(legal, energy, state, false);
+	DBGA("Energy: " << energy);
 }
 
 void
-ListPlanner::showState(int index)
+ListPlanner::showInitialState(int index)
 {
-  GraspPlanningState *state = getState(index);
-  if (!state) { return; }
-  state->execute();
+    GraspPlanningState *state = getInitialState(index);
+	if (!state) return;
+	state->execute();
+}
+
+void
+ListPlanner::showFinalState(int index)
+{
+    GraspPlanningState *state = getFinalState(index);
+    if (!state) return;
+    state->execute();
 }
 
 void
 ListPlanner::prepareState(int index)
 {
-  showState(index);
-  mHand->findInitialContact(200);
+    showInitialState(index);
+    mHand->findInitialContact(200);
 }
 
 /*! The main planning function. Simply takes the next input grasp from the list,
-  tries it and then places is in the output depending on the quality.
+	tries it and then places is in the output depending on the quality.
 */
-void
+void 
 ListPlanner::mainLoop()
 {
-  //check if we are done
-  if (mPlanningIterator == mInputList.end()) {
-    mCurrentStep = mMaxSteps + 1;
-    return;
-  }
-
-  //analyze the current state
-  //we don't allow it to leave the hand in the analysis posture
-  //so that after dynamics object gets put back
-  bool legal; double energy;
-  PRINT_STAT(mOut, mCurrentStep);
-  mEnergyCalculator->analyzeState(legal, energy, *mPlanningIterator, true);
-
-  //for rendering purposes; will see later if it's needed
-  mCurrentState->copyFrom(*mPlanningIterator);
-  mCurrentState->setLegal(legal);
-
-  //put a copy of the result in list if it's legal and there's room or it's
-  //better than the worst solution so far
-  //this whole thing could go into a higher level fctn in EGPlanner
-  if (legal) {
-    double worstEnergy;
-    if ((int)mBestList.size() < BEST_LIST_SIZE) { worstEnergy = 1.0e5; }
-    else { worstEnergy = mBestList.back()->getEnergy(); }
-    if (energy < worstEnergy) {
-      GraspPlanningState *insertState = new GraspPlanningState(*mPlanningIterator);
-      insertState->setEnergy(energy);
-      insertState->setItNumber(mCurrentStep);
-      DBGP("Solution at step " << mCurrentStep);
-      mBestList.push_back(insertState);
-      mBestList.sort(GraspPlanningState::compareStates);
-      while ((int)mBestList.size() > BEST_LIST_SIZE) {
-        delete(mBestList.back());
-        mBestList.pop_back();
-      }
-    }
-  }
-
-  //advance the planning iterator
-  mPlanningIterator++;
-  mCurrentStep++;
-  Q_EMIT update();
-  PRINT_STAT(mOut, std::endl);
+	//check if we are done
+	if (mPlanningIterator==mInputList.end()) {
+		mCurrentStep = mMaxSteps+1;
+		return;
+	}
+
+	//analyze the current state
+	//we don't allow it to leave the hand in the analysis posture
+	//so that after dynamics object gets put back
+	bool legal; double energy;
+	PRINT_STAT(mOut, mCurrentStep);
+
+    (*mPlanningIterator)->execute();
+    mHand->findInitialContact(200);
+
+    mEnergyCalculator->analyzeState(legal, energy, *mPlanningIterator, false);
+
+	//for rendering purposes; will see later if it's needed
+	mCurrentState->copyFrom(*mPlanningIterator);
+	mCurrentState->setLegal(legal);
+
+	//put a copy of the result in list if it's legal and there's room or it's 
+	//better than the worst solution so far
+	//this whole thing could go into a higher level fctn in EGPlanner
+	if (legal) {
+		double worstEnergy;
+        if ((int)mBestList.size() < BEST_LIST_SIZE) worstEnergy = 1.0e5;
+		else worstEnergy = mBestList.back()->getEnergy();
+		if (energy < worstEnergy) {
+			GraspPlanningState *insertState = new GraspPlanningState(*mPlanningIterator);
+            insertState->saveCurrentHandState();
+			insertState->setEnergy(energy);
+			insertState->setItNumber(mCurrentStep);
+			DBGP("Solution at step " << mCurrentStep);
+			mBestList.push_back(insertState);
+			mBestList.sort(GraspPlanningState::compareStates);
+            while ((int)mBestList.size() > BEST_LIST_SIZE) {
+				delete(mBestList.back());
+				mBestList.pop_back();
+			}
+		}
+	}
+
+	//advance the planning iterator
+    mPlanningIterator++;
+	mCurrentStep++;
+	Q_EMIT update();
+	PRINT_STAT(mOut, std::endl);
 }
 
-void
+void 
 ListPlanner::showVisualMarkers(bool show)
 {
-  std::list<GraspPlanningState *>::iterator it;
-  for (it = mInputList.begin(); it != mInputList.end(); it++) {
-    if (show) {
-      (*it)->showVisualMarker();
-    } else {
-      (*it) ->hideVisualMarker();
-    }
-  }
+	std::list<GraspPlanningState*>::iterator it;
+	for (it = mInputList.begin(); it!=mInputList.end(); it++) {
+		if (show) {
+			(*it)->showVisualMarker();
+		} else {
+			(*it) ->hideVisualMarker();
+		}
+	}
 }