// test current grasp
bool GraspitDBPlanner::testCurrentGrasp(TestType t, DynamicCode* c){
// test current grasp in coresponding test type - static/dynamic
if(t == STATIC)
return testGraspStatic();
else{
if(!c)
return false;
return testGraspDynamic(c);
}
}
/*! Every grasp in graspList will be tested and stored in testedGraspList
even if their test scores, epsilon quality and volume quality, are
not positive, which means they are not stable grasps.
*/
bool GraspitDBPlanner::testGrasps(TestType t,
std::vector<db_planner::Grasp *> graspList,
std::vector<db_planner::Grasp *> *testedGraspList) {
mTestedGrasps.clear();
if (testedGraspList) {
testedGraspList->clear();
}
// test each of the grasps in graspList
for (int i = 0; i < ((int)graspList.size()); i ++)
{
GraspitDBGrasp *tempGrasp = new GraspitDBGrasp(*static_cast<GraspitDBGrasp *>(graspList[i]));
// load this grasp into the world
static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->setTran(transf::IDENTITY);
GraspPlanningState *tempState = new GraspPlanningState(tempGrasp->getPreGraspPlanningState());
tempState->setRefTran(transf::IDENTITY);
tempGrasp->setPreGraspPlanningState(tempState);
float elmts[16];
if (mAligner->Align(tempGrasp->SourceModel(), *mObject, elmts)) {
tempGrasp->Transform(elmts);
}
mHand->getGrasp()->setObject(static_cast<GraspitDBModel *>(mObject)->getGraspableBody());
// apply it to the hand
tempGrasp->getPreGraspPlanningState()->execute();
bool testResult;
DynamicCode dynCode;
// test this grasp
if (t == STATIC) {
testResult = testGraspStatic();
dynCode = NO_DYNAMICS;
} else if (t == DYNAMIC) {
testResult = testGraspDynamic(&dynCode);
//DBGA("Result: " << testResult << "; dynamics code: " << dynCode);
}
float eq = -1.0, vq;
// decide whether to record
if (testResult) {
computeQuality(eq, vq);
if (!static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getNumContacts()) {
dynCode = DYNAMIC_OBJECT_EJECTED;
} else if (eq <= 0.0) {
dynCode = DYNAMIC_NO_FC;
}
} else { return false; }
if (eq < 0) { continue; }
//we now save all grasps in mTestedGrasps
GraspPlanningState *newGrasp = new GraspPlanningState(mHand);
newGrasp->setPositionType(SPACE_COMPLETE, false);
newGrasp->setPostureType(POSE_DOF, false);
//it is possible the object moved after dynamics, so we need to re-set the reference posture
newGrasp->setRefTran(static_cast<GraspitDBModel *>(mObject)->getGraspableBody()->getTran());
newGrasp->saveCurrentHandState();
newGrasp->setEpsilonQuality(eq);
newGrasp->setVolume(vq);
newGrasp->setIndex(i);
//this is a hack; just gives us a field to save the dynamics code
newGrasp->setDistance((double)dynCode);
mTestedGrasps.push_back(newGrasp);
//std::cout << "grasp tested, quality: " << eq << std::endl;
if (!testedGraspList) {
continue;
}
// record this to synthesize the output
db_planner::Grasp *recordGrasp = static_cast<db_planner::Grasp *>(new GraspitDBGrasp(*tempGrasp));
tempState = new GraspPlanningState(static_cast<GraspitDBGrasp *>(recordGrasp)->getPreGraspPlanningState());
tempState->copyFrom(newGrasp);
static_cast<GraspitDBGrasp *>(recordGrasp)->setPreGraspPlanningState(tempState);
testedGraspList->push_back(recordGrasp);
delete tempGrasp;
}
if (mTestedGrasps.size() != 0) {
return true;
}
return false;
}