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; } }
/*! Stops a child, and waits for the child thread to finish. After that, it retrieves the solutions found by the child, and places them in the list of solutions and also in the list of states to be avoided in the future. Solutions are also marked visually. Note that, for each solution recovered from a child, this function also reconstructs the final grasp (remember that solutions are usually pre-grasps) and saves BOTH in the list of solutions. */ void GuidedPlanner::stopChild(SimAnnPlanner *pl) { DBGA("Child has finished!"); //this sets the state to DONE which in turn waits for the thread to stop spinning pl->stopPlanner(); DBGP("Thread has stopped."); int j = pl->getListSize(); if (j) { //place a copy of the best state (which is the pre-grasp) in my best list GraspPlanningState *s = new GraspPlanningState( pl->getGrasp(0) ); s->printState(); //recall that child used its own cloned hand for planning s->changeHand(mHand, true); mBestList.push_back(s); //use this part to also save the final grasp from the child planner, //not just the pre-grasp pl->showGrasp(0); GraspPlanningState *finalGrasp = new GraspPlanningState(pl->getGrasp(0)); finalGrasp->setPositionType(SPACE_COMPLETE); //the final grasp is not in eigengrasp space, so we must save it in DOF space finalGrasp->setPostureType(POSE_DOF); finalGrasp->saveCurrentHandState(); //change the hand finalGrasp->changeHand(mHand, true); //and save it mBestList.push_back(finalGrasp); //place a copy of the pre-grasp in the avoid list and mark it red GraspPlanningState *s2 = new GraspPlanningState(s); mAvoidList.push_back(s2); mHand->getWorld()->getIVRoot()->addChild( s2->getIVRoot() ); if (s2->getEnergy() < 10.0) { s2->setIVMarkerColor(1,0,0); } else { s2->setIVMarkerColor(0.1,0.1,0.1); } DBGA("Enrgy from child: " << s2->getEnergy()); //the avoid list gets emptied at cleanup } else { DBGA("Child has no solutions"); } }
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 );//CHANGED! to mSolutionClone from mRefHand mBestList.push_back(s); //graspItGUI->getIVmgr()->emitAnalyzeGrasp(s); if (mMarkSolutions) { mHand->getWorld()->getIVRoot()->addChild( s->getIVRoot() ); } } updateSolutionList(); //now shape the real hand. //s = mInterface->updateHand(&mBestList);//this used to update the current hand to solutions that are found, no longer does this // if (mBestList.size() != 0) showGrasp(0);//CHANGED! this replaces line above //CHANGED! stuff below was taken out // if (s) { // if (mSolutionClone) s->execute(mSolutionClone); // if (mMarkSolutions) s->setIVMarkerColor(1,1,0); // } DBGP("On-line main loop done"); }