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