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