void DBaseDlg::sortButton_clicked()
{
if (mGraspList.empty()) return;
VisualQualityFunctor func;
if (sortBox->currentText()== "Energy") {
std::sort(mGraspList.begin(), mGraspList.end(), db_planner::Grasp::CompareEnergy);
func.mType = VisualQualityFunctor::ENERGY;
} else if (sortBox->currentText()== "Epsilon") {
std::sort(mGraspList.begin(), mGraspList.end(), db_planner::Grasp::CompareEpsilon);
func.mType = VisualQualityFunctor::EPSILON;
} else if (sortBox->currentText()== "Volume") {
std::sort(mGraspList.begin(), mGraspList.end(), db_planner::Grasp::CompareVolume);
func.mType = VisualQualityFunctor::VOLUME;
} else {
assert(0);
}
mCurrentFrame = 0;
showGrasp(mCurrentFrame);
if (showMarkersBox->isChecked()) {
double min = func(mGraspList.front());
double max = func(mGraspList.back());
if (max==min) max=min+1.0;
std::vector<db_planner::Grasp*>::iterator it;
for (it=mGraspList.begin(); it!=mGraspList.end(); it++) {
double r = (func(*it) - min) / (max - min);
double g = 1-r;
double b = 0.0;
GraspPlanningState *state;
if(showPreGraspRadioButton->isChecked()) {
state = static_cast<GraspitDBGrasp*>(*it)->getPreGraspPlanningState();
} else {
state = static_cast<GraspitDBGrasp*>(*it)->getFinalGraspPlanningState();
}
state->setIVMarkerColor(r,g,b);
}
}
}
void
OnLinePlanner::mainLoop()
{
static clock_t lastCheck = clock();
clock_t time = clock();
double secs = (float)(time - lastCheck) / CLOCKS_PER_SEC;
if (secs < 0.2) {
//perform grasp planning all the time
graspLoop();
return;
}
lastCheck = time;
//every 0.2 seconds, perform the management part:
//set as a reference transform for the search the transform of the reference hand (presumably controlled by
//the user via a flock of birds)
mCurrentState->setRefTran(mRefHand->getTran(), false);
//this is to ensure this (potentially) illegal state does not make it into the best list
mCurrentState->setLegal(false);
//re-set the legal search range along the approach direction, so we don't search pointlessly inside the object
if (mCurrentState->getVariable("dist")) {
Body *obj = mCurrentState->getObject();
double maxDist = 200;
mObjectDistance = mRefHand->getApproachDistance(obj, maxDist);
if (mObjectDistance > maxDist) { mObjectDistance = maxDist; }
mCurrentState->getPosition()->getVariable("dist")->setRange(-30, mObjectDistance);
//make sure the current value is within range; otherwise simm ann can hang...
mCurrentState->getPosition()->getVariable("dist")->setValue(mObjectDistance / 2);
mCurrentState->getPosition()->getVariable("dist")->setJump(0.33);
}
//is the planning part has produced new candidates, send them to the grasp tester
std::list<GraspPlanningState *>::iterator it = mCandidateList.begin();
while (it != mCandidateList.end()) {
//while there is space
if (mGraspTester->postCandidate(*it)) {
DBGP("Candidate posted");
it = mCandidateList.erase(it);
} else {
DBGP("Tester thread buffer is full");
break;
}
}
//retrieve solutions from the tester
GraspPlanningState *s;
while ((s = mGraspTester->popSolution()) != NULL) {
//hack - this is not ideal, but so far I don't have a better solution of how to keep track
//of what hand is being used at what time
s->changeHand(mRefHand, true);
mBestList.push_back(s);
if (mMarkSolutions) {
mHand->getWorld()->getIVRoot()->addChild(s->getIVRoot());
}
}
updateSolutionList();
//now shape the real hand.
s = mInterface->updateHand(&mBestList);
if (s) {
if (mSolutionClone) { s->execute(mSolutionClone); }
if (mMarkSolutions) { s->setIVMarkerColor(1, 1, 0); }
}
DBGP("On-line main loop done");
}
