/*! Keeps the list of solutions sorted according to some metric */ void OnLinePlanner::updateSolutionList() { transf stateTran, currentHandTran = mRefHand->getTran(); std::list<GraspPlanningState *>::iterator it; //re-compute distance between current hand position and solutions. for (it = mBestList.begin(); it != mBestList.end(); it++) { stateTran = (*it)->getTotalTran(); //compute distance between each solution and current hand position double dist = distanceOutsideApproach(stateTran, currentHandTran); if (dist < 0) { dist = -dist; } (*it)->setDistance(dist); if (mMarkSolutions) { if (dist < 1) { (*it)->setIVMarkerColor(1 - dist, dist, 0); } else { (*it)->setIVMarkerColor(0 , 1, 1); } } } //sort list according to distance from current hand position mBestList.sort(GraspPlanningState::compareStatesDistances); //keep only best in list while (mBestList.size() > SOLUTION_BUFFER_SIZE) { delete mBestList.back(); mBestList.pop_back(); } }
/*! Keeps the list of solutions sorted according to some metric */ void OnLinePlanner::updateSolutionList() { QMutexLocker lock(&mListAttributeMutex); transf stateTran, currentHandTran = mRefHand->getTran(); std::list<GraspPlanningState*>::iterator it; //re-compute distance between current hand position and solutions. for ( it = mBestList.begin(); it != mBestList.end(); it++ ) { stateTran = (*it)->getTotalTran(); //compute distance between each solution and current hand position double dist = distanceOutsideApproach(stateTran, currentHandTran, false); if (dist < 0) dist = -dist; dist += 1000 * (1-(*it)->getAttribute("testResult")); (*it)->setDistance(dist); if (mMarkSolutions) { if (dist<1) (*it)->setIVMarkerColor(1-dist, dist, 0); else (*it)->setIVMarkerColor(0 , 1, 1); } } //sort list according to distance from current hand position mBestList.sort(GraspPlanningState::compareStatesDistances); //keep only best in list std::list<GraspPlanningState *>::iterator it2 = mBestList.begin(); for(int i = 0; it2 != mBestList.end();) { if ((*it2)->getAttribute("testResult") <= 0) break; if(i >= SOLUTION_BUFFER_SIZE) { delete *it2; std::list<GraspPlanningState *>::iterator it3 = it2; ++it2; mBestList.erase(it3); } else ++it2; ++i; } while (mBestList.size() > 2*SOLUTION_BUFFER_SIZE) { delete mBestList.back(); mBestList.pop_back(); } }
double OnLinePlanner::stateDistance(const GraspPlanningState *s1, const GraspPlanningState *s2) { return distanceOutsideApproach(s1->getTotalTran(), s2->getTotalTran()); }