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; }