void CompliantPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z, double sz1, double sz2, vec3 tln, double res, std::list<GraspPlanningState*> *sampling) { mat3 R(x, y, z); int rotSamples=2; double m1 = (2.0*sz1 - floor(2.0*sz1 / res) * res)/2.0; while (m1 < 2*sz1){ double m2 = (2.0*sz2 - floor(2.0*sz2 / res) * res)/2.0; while (m2 < 2*sz2) { vec3 myTln(tln); myTln = myTln + (m1 - sz1)* y; myTln = myTln + (m2 - sz2)* z; transf tr(R, myTln); for(int rot=0; rot < rotSamples; rot++) { double angle = M_PI * ((double)rot) / rotSamples; transf rotTran(Quaternion(angle, vec3(1,0,0)), vec3(0,0,0)); tr = rotTran * tr; GraspPlanningState* seed = new GraspPlanningState(mHand); seed->setObject(mObject); seed->setRefTran(mObject->getTran(), false); seed->setPostureType(POSE_DOF, false); seed->setPositionType(SPACE_COMPLETE, false); seed->reset(); seed->getPosition()->setTran(tr); sampling->push_back(seed); } m2+=res; } m1 += res; } }
OnLinePlanner * createDefaultPlanner(){ World * w = getWorld(); if(!w->getCurrentHand()) { DBGA("plannerTools::createDefaultPlanner::No current Hand!"); return NULL; } GraspPlanningState *mHandObjectState = new GraspPlanningState(w->getCurrentHand()); mHandObjectState->setPositionType(SPACE_AXIS_ANGLE); mHandObjectState->setObject(w->getGB(0)); mHandObjectState->setRefTran(w->getGB(0)->getTran()); mHandObjectState->reset(); OnLinePlanner * op = new OnLinePlanner(w->getCurrentHand()); op->setContactType(CONTACT_PRESET); op->setEnergyType(ENERGY_CONTACT_QUALITY); op->setMaxSteps(2000); op->setModelState(mHandObjectState); w->setCurrentPlanner(op); op->showSolutionClone(true); op->resetPlanner(); return op; }
void importGraspsFromDBMgr( OnLinePlanner * mPlanner, db_planner::DatabaseManager * mDbMgr) { Hand*mHand = mPlanner->getHand(); // Get corresponding model from database std::vector<db_planner::Model*> modelList; QString *objectFilename = new QString('/' + mHand->getGrasp()->getObject()->getFilename().split('/').back()); mDbMgr->ModelList(&modelList,db_planner::FilterList::USE_FILE_NAME, *objectFilename); if(modelList.empty()) { DBGA("No Models Found \n"); return; } // Using the found model, retrieve the grasps std::vector<db_planner::Grasp*> grasps; mDbMgr->GetGrasps(*modelList.back(), GraspitDBGrasp::getHandDBName(mHand).toStdString(), &grasps); mHand->saveState(); bool collisionState = getWorld()->collisionsAreOff(mPlanner->getHand()); getWorld()->toggleCollisions(true, mPlanner->getHand()); bool targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); // Load the grasps into the grasp planner list. unsigned int numGrasps = std::min<unsigned int>(grasps.size(), 10); for (unsigned int gNum = 0; gNum < numGrasps; ++gNum) { GraspPlanningState *s = new GraspPlanningState(static_cast<GraspitDBGrasp *> (grasps[gNum])->getFinalGraspPlanningState()); s->setObject(mHand->getGrasp()->getObject()); s->setRefTran(mHand->getGrasp()->getObject()->getTran(), false); targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); float testResult = -2*bci_experiment::planner_tools::testGraspCollisions(mHand, s); while(targetsOff) { getWorld()->toggleCollisions(true, mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); } s->addAttribute("graspId", gNum); s->addAttribute("testResult", testResult); s->addAttribute("testTime", 0); mPlanner->addSolution(s); } //reorders the solutions we have found. mPlanner->updateSolutionList(); targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); //needed to return hand to aligned with object, since it was used to testGraspCollisions getWorld()->toggleCollisions(!collisionState, mPlanner->getHand()); mHand->restoreState(); targetsOff = getWorld()->collisionsAreOff(mPlanner->getHand(), mPlanner->getHand()->getGrasp()->getObject()); }
int EvalPlugin::mainLoop() { //save grasps and reset planner if(plannerStarted && plannerFinished && (!evaluatingGrasps)) { std::cout << "FINISHED Planning\n" ; SearchEnergy *mEnergyCalculator = new SearchEnergy(); mEnergyCalculator->setType(ENERGY_CONTACT_QUALITY); mEnergyCalculator->setContactType(CONTACT_PRESET); int num_grasps = mPlanner->getListSize(); std::cout << "Found " << num_grasps << " Grasps. " << std::endl; for(int i=0; i < num_grasps; i++) { GraspPlanningState gps = mPlanner->getGrasp(i); gps.execute(mHand); mHand->autoGrasp(render_it, 1.0, false); bool is_legal; double new_planned_energy; mEnergyCalculator->analyzeCurrentPosture(mHand,graspItGUI->getMainWorld()->getGB(0),is_legal,new_planned_energy,false ); gps.setEnergy(new_planned_energy); } //remove completed body std::cout << "About to Remove Body\n" ; graspItGUI->getMainWorld()->destroyElement(graspItGUI->getMainWorld()->getGB(0)); std::cout << "Removed Body\n" ; //add gt body std::cout << "About to add GT Body: " << gt_mesh_filepath.toStdString().c_str() << "\n" ; graspItGUI->getMainWorld()->importBody("GraspableBody", gt_mesh_filepath); graspItGUI->getMainWorld()->getGB(0)->setGeometryScaling(scale,scale,scale); std::cout << "Added GT Body\n" ; evaluatingGrasps = true; std::ofstream outfile; outfile.open(result_filepath.toStdString().c_str(), std::ios_base::app); outfile << "Planned Meshfile,"; outfile << "Ground Truth Meshfile,"; outfile << "Grasp Legal,"; outfile << "Planned Quality,"; outfile << "Evaluated Quality,"; outfile << "Planned Joint Values,"; outfile << "Evaluated Joint Values,"; outfile << "Planned Pose Values,"; outfile << "Evaluated Pose Values,"; outfile << "Jaccard" << std::endl; for(int i=0; i < num_grasps; i++) { GraspPlanningState gps = mPlanner->getGrasp(i); gps.setObject(graspItGUI->getMainWorld()->getGB(0)); double desiredVals [mHand->getNumDOF()]; double desiredSteps [mHand->getNumDOF()]; desiredSteps[0] = 20.0; desiredSteps[1] = 20.0; desiredSteps[2] = 20.0; desiredSteps[3] = 20.0; bool legal; double new_energy; double old_energy = gps.getEnergy(); //mEnergyCalculator->analyzeState(legal,new_energy, &gps); //disable collisions graspItGUI->getMainWorld()->toggleAllCollisions(false); usleep(10000); gps.execute(mHand); mHand->getDOFVals(desiredVals); double initial_joint_values [mHand->getNumJoints()]; mHand->getJointValues(initial_joint_values); transf initial_tran = mHand->getPalm()->getTran(); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } //mHand->quickOpen(); mHand->autoGrasp(true, -10.0, false); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } mHand->approachToContact(-200.0); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } graspItGUI->getMainWorld()->toggleAllCollisions(true); mHand->approachToContact(200); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } //gps.execute(mHand); //possible //mHand->moveDOFToContacts(); //mHand->checkSetDOFVals(); //moveDOFToContacts //mHand->jumpDOFToContact(); //mHand->moveDOFToContacts(double *desiredVals, double *desiredSteps, bool stopAtContact, bool renderIt); mHand->moveDOFToContacts( desiredVals, desiredSteps, false, true); if(render_it) { graspItGUI->getIVmgr()->getViewer()->render(); usleep(1000000); } mHand->autoGrasp(render_it, 1.0, false); double final_joint_values [mHand->getNumJoints()]; mHand->getJointValues(final_joint_values); transf final_tran = mHand->getPalm()->getTran(); mEnergyCalculator->analyzeCurrentPosture(mHand,graspItGUI->getMainWorld()->getGB(0),legal,new_energy,false ); // outfile << "Planned Meshfile,"; outfile << completed_mesh_filepath.toStdString().c_str() << ","; // outfile << "Ground Truth Meshfile,"; outfile << gt_mesh_filepath.toStdString().c_str() << ","; // outfile << "Grasp Legal,"; outfile << legal << ","; // outfile << "Planned Quality,"; outfile << old_energy << ","; // outfile << "Evaluated Quality,"; outfile << new_energy << ","; // outfile << "Planned Joint Values,"; for (int j =0; j < mHand->getNumJoints(); j++) { outfile << initial_joint_values[j] << ";" ; } outfile << ","; // outfile << "Evaluated Joint Values,"; for (int j =0; j < mHand->getNumJoints(); j++) { outfile << final_joint_values[j] << ";" ; } outfile << ","; // outfile << "Planned Pose Values,"; outfile << initial_tran.translation().x() << ";"; outfile << initial_tran.translation().y() << ";"; outfile << initial_tran.translation().z() << ";"; outfile << initial_tran.rotation().w << ";"; outfile << initial_tran.rotation().x << ";"; outfile << initial_tran.rotation().y << ";"; outfile << initial_tran.rotation().z << ";"; outfile << ","; // outfile << "Evaluated Pose Values" << std::endl; outfile << final_tran.translation().x() << ";"; outfile << final_tran.translation().y() << ";"; outfile << final_tran.translation().z() << ";"; outfile << final_tran.rotation().w << ";"; outfile << final_tran.rotation().x << ";"; outfile << final_tran.rotation().y << ";"; outfile << final_tran.rotation().z << ";"; outfile << ","; //mEnergyCalculator->analyzeState(l2, new_energy, &gps); outfile << jaccard; outfile << std::endl; } outfile.close(); assert(false); } //start planner else if (!plannerStarted) { std::cout << "Starting Planner\n" ; graspItGUI->getMainWorld()->importBody("GraspableBody", completed_mesh_filepath); graspItGUI->getMainWorld()->getGB(0)->setGeometryScaling(scale,scale,scale); mObject = graspItGUI->getMainWorld()->getGB(0); mHand = graspItGUI->getMainWorld()->getCurrentHand(); mHand->getGrasp()->setObjectNoUpdate(mObject); mHand->getGrasp()->setGravity(false); mHandObjectState = new GraspPlanningState(mHand); mHandObjectState->setObject(mObject); mHandObjectState->setPositionType(SPACE_AXIS_ANGLE); mHandObjectState->setRefTran(mObject->getTran()); mHandObjectState->reset(); mPlanner = new SimAnnPlanner(mHand); ((SimAnnPlanner*)mPlanner)->setModelState(mHandObjectState); mPlanner->setEnergyType(ENERGY_CONTACT_QUALITY); mPlanner->setContactType(CONTACT_PRESET); mPlanner->setMaxSteps(num_steps); mPlanner->resetPlanner(); mPlanner->startPlanner(); plannerStarted = true; } //let planner run. else if( (plannerStarted) && !plannerFinished ) { if ( mPlanner->getCurrentStep() >= num_steps) { mPlanner->stopPlanner(); plannerFinished=true; } } return 0; }
void GraspPlanningTask::start() { //get the details of the planning task itself if (!mDBMgr->GetPlanningTaskRecord(mRecord.taskId, &mPlanningTask)) { DBGA("Failed to get planning record for task"); mStatus = FAILED; return; } World *world = graspitCore->getWorld(); //check if the currently selected hand is the same as the one we need //if not, load the hand we need if (world->getCurrentHand() && world->getCurrentHand()->getDBName() == QString(mPlanningTask.handName.c_str())) { DBGA("Grasp Planning Task: using currently loaded hand"); mHand = world->getCurrentHand(); } else { QString handPath = mDBMgr->getHandGraspitPath(QString(mPlanningTask.handName.c_str())); handPath = QString(getenv("GRASPIT")) + handPath; DBGA("Grasp Planning Task: loading hand from " << handPath.latin1()); mHand = static_cast<Hand*>(world->importRobot(handPath)); if ( !mHand ) { DBGA("Failed to load hand"); mStatus = FAILED; return; } } //check for virtual contacts if (mHand->getNumVirtualContacts()==0) { DBGA("Specified hand does not have virtual contacts defined"); mStatus = FAILED; return; } //load the object GraspitDBModel *model = static_cast<GraspitDBModel*>(mPlanningTask.model); if (model->load(world) != SUCCESS) { DBGA("Grasp Planning Task: failed to load model"); mStatus = FAILED; return; } mObject = model->getGraspableBody(); mObject->addToIvc(); world->addBody(mObject); //initialize the planner GraspPlanningState seed(mHand); seed.setObject(mObject); seed.setPositionType(SPACE_AXIS_ANGLE); seed.setPostureType(POSE_EIGEN); seed.setRefTran(mObject->getTran()); seed.reset(); mPlanner = new LoopPlanner(mHand); QObject::connect(mPlanner, SIGNAL(loopUpdate()), this, SLOT(plannerLoopUpdate())); QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete())); mPlanner->setEnergyType(ENERGY_CONTACT); mPlanner->setContactType(CONTACT_PRESET); mPlanner->setMaxSteps(65000); mPlanner->setRepeat(true); //max time set from database record if (mPlanningTask.taskTime >= 0){ mPlanner->setMaxTime(mPlanningTask.taskTime); } else { mPlanner->setMaxTime(-1); } static_cast<SimAnnPlanner*>(mPlanner)->setModelState(&seed); if (!mPlanner->resetPlanner()) { DBGA("Grasp Planning Task: failed to reset planner"); mStatus = FAILED; return ; } //load all already known grasps so that we avoid them in current searches mDBMgr->SetGraspAllocator(new GraspitDBGraspAllocator(mHand)); std::vector<db_planner::Grasp*> graspList; if(!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)){ //for now, we don't know if this means "no grasps found" or "error" so we assume the first DBGA("No grasps found in database for model " << mPlanningTask.model->ModelName()); } //and pass them on to the planner for (size_t i=0; i<graspList.size(); i++) { GraspPlanningState *state = new GraspPlanningState(static_cast<GraspitDBGrasp*>(graspList[i]) ->getFinalGraspPlanningState() ); state->setObject(mObject); state->setPositionType(SPACE_AXIS_ANGLE, true); //careful here - is it the same posture in both spaces? state->setPostureType(POSE_EIGEN, true); static_cast<LoopPlanner*>(mPlanner)->addToAvoidList(state); } while (!graspList.empty()) { delete graspList.back(); graspList.pop_back(); } mLastSolution = mPlanner->getListSize(); DBGA("Planner starting off with " << mLastSolution << " solutions"); mPlanner->startPlanner(); mStatus = RUNNING; }