bool GuidedGraspPlanningTask::saveGrasp(const GraspPlanningState *pre_gps, const GraspPlanningState *final_gps)
{
GraspitDBModel* dbModel= mObject->getDBModel();
assert(dbModel);
db_planner::Grasp* grasp = new db_planner::Grasp;
std::vector<double> contacts = grasp->GetContacts();
grasp->SetSourceModel( *(static_cast<db_planner::Model*>(dbModel)) );
grasp->SetHandName(GraspitDBGrasp::getHandDBName(mHand).toStdString());
grasp->SetEpsilonQuality(final_gps->getEpsilonQuality());
grasp->SetVolumeQuality(final_gps->getVolume());
grasp->SetEnergy(final_gps->getEnergy());
grasp->SetClearance(0.0);
grasp->SetClusterRep(false);
grasp->SetCompliantCopy(false);
grasp->SetSource("EIGENGRASPS");
std::vector<double> tempArray;
//the final grasp
GraspPlanningState *sol = new GraspPlanningState(final_gps);
//make sure you pass it sticky=true
sol->setPositionType(SPACE_COMPLETE,true);
sol->setPostureType(POSE_DOF,true);
tempArray.clear();
for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){
tempArray.push_back(sol->readPosture()->readVariable(i));
}
grasp->SetFinalgraspJoints(tempArray);
tempArray.clear();
for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){
tempArray.push_back(sol->readPosition()->readVariable(i));
}
grasp->SetFinalgraspPosition(tempArray);
delete sol;
//the pre-grasp
sol = new GraspPlanningState(pre_gps);
sol->setPositionType(SPACE_COMPLETE,true);
sol->setPostureType(POSE_DOF,true);
tempArray.clear();
for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){
tempArray.push_back(sol->readPosture()->readVariable(i));
}
grasp->SetPregraspJoints(tempArray);
tempArray.clear();
for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){
tempArray.push_back(sol->readPosition()->readVariable(i));
}
grasp->SetPregraspPosition(tempArray);
delete sol;
//contacts
//for some reason, the grasp's contact vector gets initialized to a mess!
tempArray.clear();
grasp->SetContacts(tempArray);
std::vector<db_planner::Grasp*> graspList;
graspList.push_back(grasp);
bool result = mDBMgr->SaveGrasps(graspList);
delete grasp;
return result;
}
