void GraspTester::mainLoop() { GraspPlanningState *s = popCandidate(); if (!s) { DBGP("Empty buffer for tester"); msleep(100); return; } s->changeHand(mHand, true); testGrasp(s); DBGP("TESTER: candidate has energy " << s->getEnergy()); mHand->breakContacts(); if (s->isLegal() && s->getEnergy() < -1.2) { //save the final grasping position that has resulted from autograsp s->setPositionType(SPACE_COMPLETE); s->setPostureType(POSE_DOF); //save the current transform in absolute terms s->setRefTran(transf::IDENTITY); s->saveCurrentHandState(); postSolution(s); DBGP("Tester posting a solution at iteration " << s->getItNumber()); } else { DBGP("Tester removing candidate"); delete s; } }
/** * @function sampleBox_valueChanged */ void CanonicalPlannerDlg::sampleBox_valueChanged(int _j ) { if( mPlanner == NULL ) { std::cout << "No planner yet. Load base grasps first."<< std::endl; return; } sampleBox->setRange(0, mPlanner->getNumSampleGrasps(baseBox->value()) - 1 ); mPlanner->showSampleGrasp( baseBox->value(), _j ); GraspPlanningState * gps = mPlanner->getSampleGrasp( baseBox->value(), _j ); double energy = gps->getEnergy(); int iter = gps->getItNumber(); std::cout << " Grasp ["<<baseBox->value()<<","<<_j<<"]: Energy: "<< energy<<" and iter: "<< iter <<std::endl; }
void OnLinePlanner::graspLoop() { //DBGP("Grasp loop started"); //prepare input GraspPlanningState *input = NULL; if ( processInput() ) { input = mTargetState; } //call simulated annealing SimAnn::Result r = mSimAnn->iterate(mCurrentState,mEnergyCalculator,input); mCurrentStep = mSimAnn->getCurrentStep(); if ( r == SimAnn::JUMP ) { assert(mCurrentState->isLegal()); //we have a new state from the SimAnn if (mCurrentState->getEnergy() < 0 || mCurrentState->getEnergy() < mCurrentBest->getEnergy()) { DBGP("New candidate"); GraspPlanningState *insertState = new GraspPlanningState(mCurrentState); //make solution independent of reference hand position insertState->setPositionType(SPACE_COMPLETE,true); insertState->setRefTran( mCurrentState->getObject()->getTran(), true); insertState->setItNumber( mCurrentStep ); if (insertState->getEnergy() < mCurrentBest->getEnergy()) { mCurrentBest->copyFrom( insertState ); } if (!addToListOfUniqueSolutions(insertState, &mCandidateList,0.4)) { DBGP("Similar to old candidate"); delete insertState; } else { //graspItGUI->getIVmgr()->emitAnalyzeGrasp(mCandidateList.back()); mCandidateList.sort(GraspPlanningState::compareStates);//CHANGED! was compareStates while (mCandidateList.size() > CANDIDATE_BUFFER_SIZE) { delete mCandidateList.back(); mCandidateList.pop_back(); } } DBGP("Added candidate"); } } if (mCurrentStep % 100 == 0) emit update(); render(); //DBGP("Grasp loop done"); }
/*! 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; }