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; }