void
CompliantPlannerDlg::addCartesianSamples(const GraspPlanningState &seed,
std::list<GraspPlanningState*> *sampling,
int samples, double x, double y, double z)
{
//redundant, but easier...
double a = seed.readPosition()->getParameter("a");
double b = seed.readPosition()->getParameter("b");
//double c = seed.readPosition()->getParameter("c");
//compute angular values
//from HandObjectStateImpl:
//x = a * cos(beta) * cos(gamma);
//y = b * cos(beta) * sin(gamma);
//z = c * sin(beta);
double beta = asin(z / sqrt(x*x + y*y + z*z));
double gamma = atan2(y/b, x/a);
DBGP("x: " << x << "; y: " << y <<"; z: " << z);
DBGP("gamma: " << gamma << "; beta: " << beta);
//sample roll angle as well
for (int m=0; m<samples; m++) {
//only sample from 0 to almost PI, as the HH is symmetric
double tau = M_PI * ((double)m) / samples;
GraspPlanningState *newState = new GraspPlanningState(&seed);
newState->getPosition()->getVariable("tau")->setValue(tau);
newState->getPosition()->getVariable("gamma")->setValue(gamma);
newState->getPosition()->getVariable("beta")->setValue(beta);
sampling->push_back(newState);
}
}
/*! Samples an ellipsoid by sampling uniformly a grid with the same aspect
ratio and projecting the resulting points on the ellipsoid. Not ideal,
but at least much better then sampling angular variables directly */
void
CompliantPlannerDlg::gridEllipsoidSampling(const GraspPlanningState &seed,
std::list<GraspPlanningState*> *sampling,
int samples)
{
double a = seed.readPosition()->getParameter("a");
double aRes = 2.0 * a / samples;
double b = seed.readPosition()->getParameter("b");
double bRes = 2.0 * b / samples;
double c = seed.readPosition()->getParameter("c");
double cRes = 2.0 * c / samples;
DBGP("a: " << a << "; b: " << b <<"; c: " << c);
for (double i=0.5; i<samples; i+=1.0) {
for(double j=0.5; j<samples; j+=1.0) {
addCartesianSamples(seed, sampling, samples, a, -b+i*bRes, -c+j*cRes);
addCartesianSamples(seed, sampling, samples, -a, -b+i*bRes, -c+j*cRes);
addCartesianSamples(seed, sampling, samples, -a+i*aRes, b , -c+j*cRes);
addCartesianSamples(seed, sampling, samples, -a+i*aRes,-b , -c+j*cRes);
addCartesianSamples(seed, sampling, samples, -a+i*aRes, -b+j*bRes , c);
addCartesianSamples(seed, sampling, samples, -a+i*aRes, -b+j*bRes ,-c);
}
}
}
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;
}
