/* show the original pre-grasp, aligned it before shown to the screen,
this alignment does not influence the grasp by copying grasp to another
grasp g for the transformation
*/
void DBasePlannerDlg::showGrasp(db_planner::Grasp* grasp){
if(!mAligner){
DBGA("Aligner is not available\n");
return;
}
if (!grasp) return;
GraspitDBGrasp *g = new GraspitDBGrasp(*static_cast<GraspitDBGrasp*>(grasp));
if(!testedGraspRadioButton->isChecked()){
float elmts[16];
if(mAligner->Align(g->SourceModel(), *mPlanningModel, elmts))
g->Transform(elmts);
}
static_cast<GraspitDBModel*>(mPlanningModel)->getGraspableBody()->setTran(transf::IDENTITY);
g->getPreGraspPlanningState()->execute();
if(mHand->isA("Barrett") && testedGraspRadioButton->isChecked()){
graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->autoGrasp(true);
}
mHand->getWorld()->findAllContacts();
mHand->getWorld()->updateGrasps();
}
void DBaseDlg::showMarkers()
{
std::vector<db_planner::Grasp*>::iterator it;
for (it=mGraspList.begin(); it!=mGraspList.end(); it++) {
GraspitDBGrasp *grasp = static_cast<GraspitDBGrasp*>(*it);
if (showPreGraspRadioButton->isChecked()) {
grasp->getFinalGraspPlanningState()->hideVisualMarker();
if (showMarkersBox->isChecked()) {
grasp->getPreGraspPlanningState()->showVisualMarker();
//show markers in blue
if (grasp->ClusterRep()) {
grasp->getPreGraspPlanningState()->setIVMarkerColor(0,0,1);
} else if (grasp->CompliantCopy()) {
grasp->getPreGraspPlanningState()->setIVMarkerColor(1.0,1.0,0.0);
} else {
grasp->getPreGraspPlanningState()->setIVMarkerColor(0.5,0.5,0.5);
}
} else {
grasp->getPreGraspPlanningState()->hideVisualMarker();
}
} else {
grasp->getPreGraspPlanningState()->hideVisualMarker();
if (showMarkersBox->isChecked()) {
grasp->getFinalGraspPlanningState()->showVisualMarker();
//show markers in blue
if (grasp->ClusterRep()) {
grasp->getFinalGraspPlanningState()->setIVMarkerColor(0,0,1);
} else if (grasp->CompliantCopy()) {
grasp->getFinalGraspPlanningState()->setIVMarkerColor(1.0,1.0,0.0);
} else {
grasp->getFinalGraspPlanningState()->setIVMarkerColor(0.5,0.5,0.5);
}
} else {
grasp->getFinalGraspPlanningState()->hideVisualMarker();
}
}
}
}
/*! 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;
}
