void DBaseBatchPlanner::processSolution(const GraspPlanningState *s)
{
//we will write this solution to the result file
//if it's a poor solution don't even bother
if (s->getEnergy() > energyConstraint){
DBGAF(mLogStream,"Solution with energy to be thrown: " << s->getEnergy());
return;
}
//we need a SearchEnergy calculator in order to do the autoGrasp in the exact same way that the planner does it
static SearchEnergy *se = NULL; //don't create it each time
if (!se) {
//this is the same type used by the loop planner
switch(mType) {
case DEXTEROUS:
se = SearchEnergy::getSearchEnergy(ENERGY_STRICT_AUTOGRASP);
break;
case GRIPPER:
se = SearchEnergy::getSearchEnergy(ENERGY_CONTACT);
se->setContactType(CONTACT_PRESET);
break;
}
}
DBGAF(mLogStream,"Solution with energy: " << s->getEnergy());
//first, copy it to a new one so it's not const and we can modify it
GraspPlanningState *sol = new GraspPlanningState(s);
//convert it's tranform to the Quaternion__Translation format
//make sure you pass it sticky=true, otherwise information is lost in the conversion
sol->setPositionType(SPACE_COMPLETE,true);
//we will want to save exact DOF positions, not eigengrasp values
//again, make sure sticky=true
sol->setPostureType(POSE_DOF,true);
//we can write it to a file
fprintf(mResultFile,"pre-grasp\n");
sol->writeToFile(mResultFile);
//at this point, we are saving each DOF individually, but it should be an eigengrasp posture
if (mType == DEXTEROUS) {
//now close the fingers and perform the autograsp
bool legal; double energy;
//make sure to pass it noChange = false and it will leave the hand in the posture AFTER the autograsp
//otherwise, analyzeState will revert the state to what it was on its entry
se->analyzeState(legal,energy,sol,false);
if (!legal) {
DBGAF(mLogStream,"buru Illegal solution! This should not be!");
}
//store the hand posture resulting from the autograsp
//careful: this only works if sol is of the type SPACE_COMPLETE (as set above)
sol->saveCurrentHandState();
//write this one to the file as well
fprintf(mResultFile,"grasp\n");
sol->writeToFile(mResultFile);
//this posture is probably not in eigengrasp space
//write the contacts to a file
fprintf(mResultFile,"contacts\n");
writeContactsToFile(sol->getHand(), sol->getObject());
}
numOfGrasps++;
printf("\nup to now, %d out of %d grasps have been found\n",numOfGrasps, numOfGraspsGoal);
//add some codes here to limit the grasps
if(numOfGrasps == numOfGraspsGoal)
{
mMaxTime = 10;// this is a trick, maybe not good
printf("begin to leave...");
DBGAF(mLogStream, numOfGraspsGoal);
mPlanner->setMaxTime(mMaxTime);
}
//we are done
delete sol;
}
