/*! 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());
}
