$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #include "dbase_grasp_planner/grasp_planning_task.h" 00036 00037 #include <QString> 00038 00039 #include "mytools.h" 00040 #include "graspitGUI.h" 00041 #include "ivmgr.h" 00042 #include "world.h" 00043 #include "robot.h" 00044 #include "body.h" 00045 #include "EGPlanner/searchState.h" 00046 #include "EGPlanner/loopPlanner.h" 00047 #include "DBPlanner/db_manager.h" 00048 00049 #include "graspit_db_grasp.h" 00050 #include "graspit_db_model.h" 00051 00052 #include "debug.h" 00053 00054 namespace dbase_grasp_planner { 00055 00056 GraspPlanningTask::GraspPlanningTask(graspit_dbase_tasks::DBTaskDispatcher *disp, 00057 db_planner::DatabaseManager *mgr, 00058 db_planner::TaskRecord rec) : 00059 graspit_dbase_tasks::DBTask(disp, mgr, rec), 00060 mObject(NULL), 00061 mPlanner(NULL) 00062 { 00063 } 00064 00065 GraspPlanningTask::~GraspPlanningTask() 00066 { 00067 //remove the planning object from the world, but do not delete it 00068 if (mObject) { 00069 mObject->getWorld()->destroyElement(mObject, false); 00070 //clean up the loaded geometry 00071 //the model itself is left around. we don't have a good solution for that yet 00072 static_cast<GraspitDBModel*>(mPlanningTask.model)->unload(); 00073 } 00074 delete mPlanner; 00075 } 00076 00077 void GraspPlanningTask::start() 00078 { 00079 //get the details of the planning task itself 00080 if (!mDBMgr->GetPlanningTaskRecord(mRecord.taskId, &mPlanningTask)) { 00081 DBGA("Failed to get planning record for task"); 00082 mStatus = ERROR; 00083 return; 00084 } 00085 00086 World *world = graspItGUI->getIVmgr()->getWorld(); 00087 00088 //check if the currently selected hand is the same as the one we need 00089 //if not, load the hand we need 00090 if (world->getCurrentHand() && 00091 world->getCurrentHand()->getDBName() == QString(mPlanningTask.handName.c_str())) { 00092 DBGA("Grasp Planning Task: using currently loaded hand"); 00093 mHand = world->getCurrentHand(); 00094 } else { 00095 QString handPath = mDBMgr->getHandGraspitPath(QString(mPlanningTask.handName.c_str())); 00096 handPath = QString(getenv("GRASPIT")) + handPath; 00097 DBGA("Grasp Planning Task: loading hand from " << handPath.latin1()); 00098 mHand = static_cast<Hand*>(world->importRobot(handPath)); 00099 if ( !mHand ) { 00100 DBGA("Failed to load hand"); 00101 mStatus = ERROR; 00102 return; 00103 } 00104 } 00105 //check for virtual contacts 00106 if (mHand->getNumVirtualContacts()==0) { 00107 DBGA("Specified hand does not have virtual contacts defined"); 00108 mStatus = ERROR; 00109 return; 00110 } 00111 00112 //load the object 00113 GraspitDBModel *model = static_cast<GraspitDBModel*>(mPlanningTask.model); 00114 if (model->load(world) != SUCCESS) { 00115 DBGA("Grasp Planning Task: failed to load model"); 00116 mStatus = ERROR; 00117 return; 00118 } 00119 mObject = model->getGraspableBody(); 00120 mObject->addToIvc(); 00121 world->addBody(mObject); 00122 00123 //initialize the planner 00124 GraspPlanningState seed(mHand); 00125 seed.setObject(mObject); 00126 seed.setPositionType(SPACE_AXIS_ANGLE); 00127 seed.setPostureType(POSE_EIGEN); 00128 seed.setRefTran(mObject->getTran()); 00129 seed.reset(); 00130 00131 mPlanner = new LoopPlanner(mHand); 00132 QObject::connect(mPlanner, SIGNAL(loopUpdate()), this, SLOT(plannerLoopUpdate())); 00133 QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete())); 00134 00135 mPlanner->setEnergyType(ENERGY_CONTACT); 00136 mPlanner->setContactType(CONTACT_PRESET); 00137 mPlanner->setMaxSteps(65000); 00138 static_cast<LoopPlanner*>(mPlanner)->setSaveThreshold(10.0); 00139 mPlanner->setRepeat(true); 00140 //max time set from database record 00141 if (mPlanningTask.taskTime >= 0){ 00142 mPlanner->setMaxTime(mPlanningTask.taskTime); 00143 } else { 00144 mPlanner->setMaxTime(-1); 00145 } 00146 static_cast<SimAnnPlanner*>(mPlanner)->setModelState(&seed); 00147 00148 if (!mPlanner->resetPlanner()) { 00149 DBGA("Grasp Planning Task: failed to reset planner"); 00150 mStatus = ERROR; 00151 return ; 00152 } 00153 00154 //load all already known grasps so that we avoid them in current searches 00155 mDBMgr->SetGraspAllocator(new GraspitDBGraspAllocator(mHand)); 00156 std::vector<db_planner::Grasp*> graspList; 00157 if(!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)){ 00158 //for now, we don't know if this means "no grasps found" or "error" so we assume the first 00159 DBGA("No grasps found in database for model " << mPlanningTask.model->ModelName()); 00160 } 00161 //and pass them on to the planner 00162 for (size_t i=0; i<graspList.size(); i++) { 00163 GraspPlanningState *state = new GraspPlanningState(static_cast<GraspitDBGrasp*>(graspList[i]) 00164 ->getFinalGraspPlanningState() ); 00165 state->setObject(mObject); 00166 state->setPositionType(SPACE_AXIS_ANGLE, true); 00167 //careful here - is it the same posture in both spaces? 00168 state->setPostureType(POSE_EIGEN, true); 00169 static_cast<LoopPlanner*>(mPlanner)->addToAvoidList(state); 00170 } 00171 while (!graspList.empty()) { 00172 delete graspList.back(); 00173 graspList.pop_back(); 00174 } 00175 mLastSolution = mPlanner->getListSize(); 00176 DBGA("Planner starting off with " << mLastSolution << " solutions"); 00177 mPlanner->startPlanner(); 00178 mStatus = RUNNING; 00179 } 00180 00181 void GraspPlanningTask::plannerComplete() 00182 { 00183 //save solutions that have accumulated in last loop 00184 plannerLoopUpdate(); 00185 //finish 00186 mStatus = DONE; 00187 } 00188 00189 void GraspPlanningTask::plannerLoopUpdate() 00190 { 00191 if (mStatus != RUNNING) return; 00192 //save all new solutions to database 00193 for(int i=mLastSolution; i<mPlanner->getListSize(); i++) { 00194 //copy the solution so we can change it 00195 GraspPlanningState *final_gps = new GraspPlanningState(mPlanner->getGrasp(i)); 00196 GraspPlanningState *pre_gps; 00197 if (!computePreGrasp(final_gps, &pre_gps)) continue; 00198 if (!saveGrasp(pre_gps, final_gps)) { 00199 DBGA("Error saving grasp"); 00200 mStatus = ERROR; 00201 break; 00202 } 00203 } 00204 if (mStatus == ERROR) { 00205 // this is a bit of a hack, but ensures that the planner will stop 00206 // as soon as it attempts to take another step. If we specifically start 00207 // the planner from in here, it causes problem, as this is called from inside 00208 // the planner callback 00209 mPlanner->setMaxSteps(0); 00210 } else { 00211 DBGA(mPlanner->getListSize() - mLastSolution << " solutions saved to database"); 00212 } 00213 mLastSolution = mPlanner->getListSize(); 00214 } 00215 00216 bool GraspPlanningTask::computePreGrasp(const GraspPlanningState *final_gps, 00217 GraspPlanningState **pre_gps) 00218 { 00219 if (!setPreGrasp(final_gps)) { 00220 DBGA("Pre-grasp creation fails"); 00221 return false; 00222 } 00223 00224 //sanity check 00225 if (!mHand->getWorld()->noCollision()) { 00226 DBGA("Collision detected for pre-grasp!"); 00227 return false; 00228 } 00229 00230 //create pre-grasp 00231 *pre_gps = new GraspPlanningState(mHand); 00232 (*pre_gps)->setPostureType(POSE_DOF, false); 00233 (*pre_gps)->setPositionType(SPACE_COMPLETE, false); 00234 (*pre_gps)->setRefTran(mObject->getTran(), false); 00235 (*pre_gps)->saveCurrentHandState(); 00236 00237 return true; 00238 } 00239 00240 bool GraspPlanningTask::setPreGrasp(const GraspPlanningState *graspState) 00241 { 00242 //go back 10cm 00243 double RETREAT_BY = -100; 00244 00245 //place the mHand in position 00246 graspState->execute(); 00247 00248 mHand->autoGrasp(false, -1.0); 00249 //check if all DOF's are at the min or max of their respective range of travel 00250 //recall that we are opening, i.e. going against the default velocity 00251 for (int d=0; d<mHand->getNumDOF(); d++) { 00252 if (mHand->getDOF(d)->getDefaultVelocity() > 0 && 00253 fabs( mHand->getDOF(d)->getVal() - mHand->getDOF(d)->getMin() ) > 1.0e-5 ) 00254 { 00255 DBGP(" dof " << d << " not at min"); 00256 return false; 00257 } 00258 else if (mHand->getDOF(d)->getDefaultVelocity() < 0 && 00259 fabs( mHand->getDOF(d)->getVal() - mHand->getDOF(d)->getMax() ) > 1.0e-5 ) 00260 { 00261 DBGP(" dof " << d << " not at max"); 00262 return false; 00263 } 00264 } 00265 00266 //retreat along approach direction 00267 if (mHand->approachToContact(RETREAT_BY, false)) { 00268 //we have hit something 00269 DBGA(" retreat fails"); 00270 return false; 00271 } 00272 00273 return true; 00274 } 00275 00276 bool GraspPlanningTask::saveGrasp(const GraspPlanningState *pre_gps, 00277 const GraspPlanningState *final_gps) 00278 { 00279 GraspitDBModel* dbModel= mObject->getDBModel(); 00280 assert(dbModel); 00281 00282 db_planner::Grasp* grasp = new db_planner::Grasp; 00283 00284 std::vector<double> contacts = grasp->GetContacts(); 00285 00286 grasp->SetSourceModel( *(static_cast<db_planner::Model*>(dbModel)) ); 00287 grasp->SetHandName(mHand->getDBName().toStdString()); 00288 grasp->SetEpsilonQuality(final_gps->getEpsilonQuality()); 00289 grasp->SetVolumeQuality(final_gps->getVolume()); 00290 grasp->SetEnergy(final_gps->getEnergy()); 00291 grasp->SetClearance(0.0); 00292 grasp->SetTableClearance(0.0); 00293 grasp->SetClusterRep(false); 00294 grasp->SetCompliantCopy(false); 00295 grasp->SetSource("EIGENGRASPS"); 00296 00297 std::vector<double> tempArray; 00298 00299 //the final grasp 00300 GraspPlanningState *sol = new GraspPlanningState(final_gps); 00301 //make sure you pass it sticky=true 00302 sol->setPositionType(SPACE_COMPLETE,true); 00303 sol->setPostureType(POSE_DOF,true); 00304 tempArray.clear(); 00305 for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){ 00306 tempArray.push_back(sol->readPosture()->readVariable(i)); 00307 } 00308 grasp->SetFinalgraspJoints(tempArray); 00309 tempArray.clear(); 00310 for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){ 00311 tempArray.push_back(sol->readPosition()->readVariable(i)); 00312 } 00313 grasp->SetFinalgraspPosition(tempArray); 00314 delete sol; 00315 00316 //the pre-grasp 00317 sol = new GraspPlanningState(pre_gps); 00318 sol->setPositionType(SPACE_COMPLETE,true); 00319 sol->setPostureType(POSE_DOF,true); 00320 tempArray.clear(); 00321 for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){ 00322 tempArray.push_back(sol->readPosture()->readVariable(i)); 00323 } 00324 grasp->SetPregraspJoints(tempArray); 00325 tempArray.clear(); 00326 for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){ 00327 tempArray.push_back(sol->readPosition()->readVariable(i)); 00328 } 00329 grasp->SetPregraspPosition(tempArray); 00330 delete sol; 00331 00332 //contacts 00333 //for some reason, the grasp's contact vector gets initialized to a mess! 00334 tempArray.clear(); 00335 grasp->SetContacts(tempArray); 00336 00337 std::vector<db_planner::Grasp*> graspList; 00338 graspList.push_back(grasp); 00339 00340 bool result = mDBMgr->SaveGrasps(graspList); 00341 delete grasp; 00342 return result; 00343 } 00344 00345 }