/*! 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. */ 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++; emit update(); PRINT_STAT(mOut, std::endl); }
/*! The caller passes it a HandObjectState and a calculator that can be used to compute the quality (or in annealing terms "energy") of a HandObjectState. This function computes the next state in the annealing schedule. See SimAnn::Result declaration for possible return values. */ SimAnn::Result SimAnn::iterate(GraspPlanningState *currentState, SearchEnergy *energyCalculator, GraspPlanningState *targetState) { //using different cooling constants for probs and neighbors double T = cooling(mT0, mParams.YC, mCurrentStep, mParams.YDIMS); //attempt to compute a neighbor of the current state GraspPlanningState *newState; double energy; bool legal = false; int attempts = 0; int maxAttempts = 10; DBGP("Ngbr gen loop"); while (!legal && attempts <= maxAttempts) { newState = stateNeighbor(currentState, T * mParams.NBR_ADJ, targetState); DBGP("Analyze state..."); energyCalculator->analyzeState(legal, energy, newState); DBGP("Analysis done."); if (!legal) { delete newState; } attempts++; } if (!legal) { DBGP("Failed to compute a legal neighbor"); //we have failed to compute a legal neighbor. //weather the SimAnn should advance a step and cool down the temperature even when it fails to compute a //legal neighbor is debatable. Might be more interactive (especially for the online planner) if it does. //mCurrentStep += 1; return FAIL; } //we have a neighbor. Now we decide if we jump to it. DBGP("Legal neighbor computed; energy: " << energy) newState->setEnergy(energy); newState->setLegal(true); newState->setItNumber(mCurrentStep); //using different cooling constants for probs and neighbors T = cooling(mT0, mParams.HC, mCurrentStep, mParams.HDIMS); double P = prob(mParams.ERR_ADJ * currentState->getEnergy(), mParams.ERR_ADJ * newState->getEnergy(), T); double U = ((double)rand()) / RAND_MAX; Result r = KEEP; if (P > U) { DBGP("Jump performed"); currentState->copyFrom(newState); r = JUMP; } else { DBGP("Jump rejected"); } mCurrentStep += 1; mTotalSteps += 1; DBGP("Main iteration done.") delete newState; if (mWriteResults && mCurrentStep % 2 == 0) { assert(mFile); fprintf(mFile, "%ld %d %f %f %f %f\n", mCurrentStep, mTotalSteps, T, currentState->getEnergy(), currentState->readPosition()->readVariable("Tx"), targetState->readPosition()->readVariable("Tx")); //currentState->writeToFile(mFile); } return r; }
int EvalPlugin::mainLoop() { //save grasps and reset planner if(plannerStarted && plannerFinished && (!evaluatingGrasps)) { std::cout << "FINISHED Planning\n" ; SearchEnergy *mEnergyCalculator = new SearchEnergy(); mEnergyCalculator->setType(ENERGY_CONTACT_QUALITY); mEnergyCalculator->setContactType(CONTACT_PRESET); int num_grasps = mPlanner->getListSize(); std::cout << "Found " << num_grasps << " Grasps. " << std::endl; for(int i=0; i < num_grasps; i++) { GraspPlanningState gps = mPlanner->getGrasp(i); gps.execute(mHand); mHand->autoGrasp(render_it, 1.0, false); bool is_legal; double new_planned_energy; mEnergyCalculator->analyzeCurrentPosture(mHand,graspItGUI->getMainWorld()->getGB(0),is_legal,new_planned_energy,false ); gps.setEnergy(new_planned_energy); } //remove completed body std::cout << "About to Remove Body\n" ; graspItGUI->getMainWorld()->destroyElement(graspItGUI->getMainWorld()->getGB(0)); std::cout << "Removed Body\n" ; //add gt body std::cout << "About to add GT Body: " << gt_mesh_filepath.toStdString().c_str() << "\n" ; graspItGUI->getMainWorld()->importBody("GraspableBody", gt_mesh_filepath); graspItGUI->getMainWorld()->getGB(0)->setGeometryScaling(scale,scale,scale); std::cout << "Added GT Body\n" ; evaluatingGrasps = true; std::ofstream outfile; outfile.open(result_filepath.toStdString().c_str(), std::ios_base::app); outfile << "Planned Meshfile,"; outfile << "Ground Truth Meshfile,"; outfile << "Grasp Legal,"; outfile << "Planned Quality,"; outfile << "Evaluated Quality,"; outfile << "Planned Joint Values,"; outfile << "Evaluated Joint Values,"; outfile << "Planned Pose Values,"; outfile << "Evaluated Pose Values,"; outfile << "Jaccard" << std::endl; for(int i=0; i < num_grasps; i++) { GraspPlanningState gps = mPlanner->getGrasp(i); gps.setObject(graspItGUI->getMainWorld()->getGB(0)); double desiredVals [mHand->getNumDOF()]; double desiredSteps [mHand->getNumDOF()]; desiredSteps[0] = 20.0; desiredSteps[1] = 20.0; desiredSteps[2] = 20.0; desiredSteps[3] = 20.0; bool legal; double new_energy; double old_energy = gps.getEnergy(); //mEnergyCalculator->analyzeState(legal,new_energy, &gps); //disable collisions graspItGUI->getMainWorld()->toggleAllCollisions(false); usleep(10000); gps.execute(mHand); mHand->getDOFVals(desiredVals); double initial_joint_values [mHand->getNumJoints()]; mHand->getJointValues(initial_joint_values); transf initial_tran = mHand->getPalm()->getTran(); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } //mHand->quickOpen(); mHand->autoGrasp(true, -10.0, false); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } mHand->approachToContact(-200.0); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } graspItGUI->getMainWorld()->toggleAllCollisions(true); mHand->approachToContact(200); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } //gps.execute(mHand); //possible //mHand->moveDOFToContacts(); //mHand->checkSetDOFVals(); //moveDOFToContacts //mHand->jumpDOFToContact(); //mHand->moveDOFToContacts(double *desiredVals, double *desiredSteps, bool stopAtContact, bool renderIt); mHand->moveDOFToContacts( desiredVals, desiredSteps, false, true); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } mHand->autoGrasp(render_it, 1.0, false); double final_joint_values [mHand->getNumJoints()]; mHand->getJointValues(final_joint_values); transf final_tran = mHand->getPalm()->getTran(); mEnergyCalculator->analyzeCurrentPosture(mHand,graspItGUI->getMainWorld()->getGB(0),legal,new_energy,false ); // outfile << "Planned Meshfile,"; outfile << completed_mesh_filepath.toStdString().c_str() << ","; // outfile << "Ground Truth Meshfile,"; outfile << gt_mesh_filepath.toStdString().c_str() << ","; // outfile << "Grasp Legal,"; outfile << legal << ","; // outfile << "Planned Quality,"; outfile << old_energy << ","; // outfile << "Evaluated Quality,"; outfile << new_energy << ","; // outfile << "Planned Joint Values,"; for (int j =0; j < mHand->getNumJoints(); j++) { outfile << initial_joint_values[j] << ";" ; } outfile << ","; // outfile << "Evaluated Joint Values,"; for (int j =0; j < mHand->getNumJoints(); j++) { outfile << final_joint_values[j] << ";" ; } outfile << ","; // outfile << "Planned Pose Values,"; outfile << initial_tran.translation().x() << ";"; outfile << initial_tran.translation().y() << ";"; outfile << initial_tran.translation().z() << ";"; outfile << initial_tran.rotation().w << ";"; outfile << initial_tran.rotation().x << ";"; outfile << initial_tran.rotation().y << ";"; outfile << initial_tran.rotation().z << ";"; outfile << ","; // outfile << "Evaluated Pose Values" << std::endl; outfile << final_tran.translation().x() << ";"; outfile << final_tran.translation().y() << ";"; outfile << final_tran.translation().z() << ";"; outfile << final_tran.rotation().w << ";"; outfile << final_tran.rotation().x << ";"; outfile << final_tran.rotation().y << ";"; outfile << final_tran.rotation().z << ";"; outfile << ","; //mEnergyCalculator->analyzeState(l2, new_energy, &gps); outfile << jaccard; outfile << std::endl; } outfile.close(); assert(false); } //start planner else if (!plannerStarted) { std::cout << "Starting Planner\n" ; graspItGUI->getMainWorld()->importBody("GraspableBody", completed_mesh_filepath); graspItGUI->getMainWorld()->getGB(0)->setGeometryScaling(scale,scale,scale); mObject = graspItGUI->getMainWorld()->getGB(0); mHand = graspItGUI->getMainWorld()->getCurrentHand(); mHand->getGrasp()->setObjectNoUpdate(mObject); mHand->getGrasp()->setGravity(false); mHandObjectState = new GraspPlanningState(mHand); mHandObjectState->setObject(mObject); mHandObjectState->setPositionType(SPACE_AXIS_ANGLE); mHandObjectState->setRefTran(mObject->getTran()); mHandObjectState->reset(); mPlanner = new SimAnnPlanner(mHand); ((SimAnnPlanner*)mPlanner)->setModelState(mHandObjectState); mPlanner->setEnergyType(ENERGY_CONTACT_QUALITY); mPlanner->setContactType(CONTACT_PRESET); mPlanner->setMaxSteps(num_steps); mPlanner->resetPlanner(); mPlanner->startPlanner(); plannerStarted = true; } //let planner run. else if( (plannerStarted) && !plannerFinished ) { if ( mPlanner->getCurrentStep() >= num_steps) { mPlanner->stopPlanner(); plannerFinished=true; } } return 0; }