void CompliantGraspCopyTask::start() { //get the details of the planning task itself if (!mDBMgr->GetPlanningTaskRecord(mPlanningTask.taskId, &mPlanningTask)) { DBGA("Failed to get planning record for task"); mStatus = FAILED; return; } loadHand(); if (mStatus == FAILED) { return; } if (!mHand->isA("Pr2Gripper2010")) { DBGA("Compliant copy task only works on the PR2 gripper"); mStatus = FAILED; return; } Pr2Gripper2010 *gripper = static_cast<Pr2Gripper2010 *>(mHand); loadObject(); if (mStatus == FAILED) { return; } //load all the grasps std::vector<db_planner::Grasp *> graspList; if (!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)) { DBGA("Load grasps failed"); mStatus = FAILED; emptyGraspList(graspList); return; } bool success = true; std::vector<db_planner::Grasp *>::iterator it; for (it = graspList.begin(); it != graspList.end(); it++) { GraspPlanningState *graspState = static_cast<GraspitDBGrasp *>(*it)->getFinalGraspPlanningState(); gripper->setCompliance(Pr2Gripper2010::NONE); graspState->execute(); DBGA("Compliant copy around finger 0"); if (!compliantCopy(*it, Pr2Gripper2010::FINGER0)) { success = false; break; } gripper->setCompliance(Pr2Gripper2010::NONE); graspState->execute(); DBGA("Compliant copy around finger 1"); if (!compliantCopy(*it, Pr2Gripper2010::FINGER1)) { success = false; break; } } gripper->setCompliance(Pr2Gripper2010::NONE); emptyGraspList(graspList); if (success) { mStatus = DONE; } else { mStatus = FAILED; } }
void ListPlanner::showState(int index) { GraspPlanningState *state = getState(index); if (!state) return; state->execute(); }
double TableCheckTask::getTableClearance(db_planner::Grasp *grasp) { //place the hand in position GraspPlanningState *graspState = static_cast<GraspitDBGrasp *>(grasp)->getFinalGraspPlanningState(); graspState->execute(); //check distance for grasp World *world = graspitCore->getWorld(); double distance = world->getDist(mHand, mTable); if (distance < 0) { DBGA(" Grasp is in collision with table"); return distance; } DBGA(" Grasp clearance: " << distance); //check if pre-grasp is feasible if (!computePreGrasp(grasp)) { DBGA(" Pre-grasp is in collision with table"); return -1.0; } double pre_distance = world->getDist(mHand, mTable); DBGA(" Pre-grasp clearance: " << pre_distance); return std::min(distance, pre_distance); }
bool PreGraspCheckTask::computePreGrasp(db_planner::Grasp *grasp) { //place the hand in position GraspPlanningState *graspState = static_cast<GraspitDBGrasp *>(grasp)->getFinalGraspPlanningState(); graspState->execute(); //check the pre-grasp return preGraspCheck(mHand); }
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 OnLinePlanner::mainLoop() { static clock_t lastCheck = clock(); clock_t time = clock(); double secs = (float)(time - lastCheck) / CLOCKS_PER_SEC; if (secs < 0.2) { //perform grasp planning all the time graspLoop(); return; } lastCheck = time; //every 0.2 seconds, perform the management part: //set as a reference transform for the search the transform of the reference hand (presumably controlled by //the user via a flock of birds) mCurrentState->setRefTran(mRefHand->getTran(), false); //this is to ensure this (potentially) illegal state does not make it into the best list mCurrentState->setLegal(false); //re-set the legal search range along the approach direction, so we don't search pointlessly inside the object if (mCurrentState->getVariable("dist")) { Body *obj = mCurrentState->getObject(); double maxDist = 200; mObjectDistance = mRefHand->getApproachDistance(obj, maxDist); if (mObjectDistance > maxDist) { mObjectDistance = maxDist; } mCurrentState->getPosition()->getVariable("dist")->setRange(-30, mObjectDistance); //make sure the current value is within range; otherwise simm ann can hang... mCurrentState->getPosition()->getVariable("dist")->setValue(mObjectDistance / 2); mCurrentState->getPosition()->getVariable("dist")->setJump(0.33); } //is the planning part has produced new candidates, send them to the grasp tester std::list<GraspPlanningState *>::iterator it = mCandidateList.begin(); while (it != mCandidateList.end()) { //while there is space if (mGraspTester->postCandidate(*it)) { DBGP("Candidate posted"); it = mCandidateList.erase(it); } else { DBGP("Tester thread buffer is full"); break; } } //retrieve solutions from the tester GraspPlanningState *s; while ((s = mGraspTester->popSolution()) != NULL) { //hack - this is not ideal, but so far I don't have a better solution of how to keep track //of what hand is being used at what time s->changeHand(mRefHand, true); mBestList.push_back(s); if (mMarkSolutions) { mHand->getWorld()->getIVRoot()->addChild(s->getIVRoot()); } } updateSolutionList(); //now shape the real hand. s = mInterface->updateHand(&mBestList); if (s) { if (mSolutionClone) { s->execute(mSolutionClone); } if (mMarkSolutions) { s->setIVMarkerColor(1, 1, 0); } } DBGP("On-line main loop done"); }