//core routine that shows the i-th loaded grasp
void DBaseDlg::showGrasp(int i)
{
if (mGraspList.empty()) return;
assert( i>=0 && i < (int)mGraspList.size() );
//put the model in correct place
mCurrentLoadedModel->getGraspableBody()->setTran(transf::IDENTITY);
//show the pregrasp or final grasp
if(showPreGraspRadioButton->isChecked()){
if(!static_cast<GraspitDBGrasp*>(mGraspList[i])->getPreGraspPlanningState())//NULL grasp, return
return;
static_cast<GraspitDBGrasp*>(mGraspList[i])->getPreGraspPlanningState()->execute();
}
else{
if(!static_cast<GraspitDBGrasp*>(mGraspList[i])->getFinalGraspPlanningState())//NULL grasp, return
return;
static_cast<GraspitDBGrasp*>(mGraspList[i])->getFinalGraspPlanningState()->execute();
if(graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->isA("Barrett")){
graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->autoGrasp(true);
}
}
//update the world and grasp information
DBGA("Find contacts");
graspItGUI->getIVmgr()->getWorld()->findAllContacts();
DBGA("Update grasps");
graspItGUI->getIVmgr()->getWorld()->updateGrasps();
mCurrentFrame = i;
updateGraspInfo();
DBGA("Show grasp done");
}
//core routine that shows the i-th loaded grasp
void DBaseDlg::showGrasp(int i)
{
if (mGraspList.empty()) return;
assert( i>=0 && i < (int)mGraspList.size() );
//put the model in correct place
mCurrentLoadedModel->getGraspableBody()->setTran(transf::IDENTITY);
//show the pregrasp or final grasp
if(showPreGraspRadioButton->isChecked()){
if(!static_cast<GraspitDBGrasp*>(mGraspList[i])->getPreGraspPlanningState())//NULL grasp, return
return;
static_cast<GraspitDBGrasp*>(mGraspList[i])->getPreGraspPlanningState()->execute();
}
else{
if(!static_cast<GraspitDBGrasp*>(mGraspList[i])->getFinalGraspPlanningState())//NULL grasp, return
return;
static_cast<GraspitDBGrasp*>(mGraspList[i])->getFinalGraspPlanningState()->execute();
if(graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->isA("Barrett")){
graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->autoGrasp(true);
}
}
/*
std::cerr << "Original tran: " << static_cast<GraspitDBGrasp*>(mGraspList[i])
->getFinalGraspPlanningState()->getPosition()->getCoreTran() << "\n";
GraspPlanningState *state = new GraspPlanningState(static_cast<GraspitDBGrasp*>(mGraspList[i])
->getFinalGraspPlanningState() );
state->setPositionType(SPACE_AXIS_ANGLE, true);
std::cerr << " New tran: " << state->getPosition()->getCoreTran() << "\n";
*/
//update the world and grasp information
graspItGUI->getIVmgr()->getWorld()->findAllContacts();
graspItGUI->getIVmgr()->getWorld()->updateGrasps();
mCurrentFrame = i;
updateGraspInfo();
}
