$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/guided_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/guidedPlanner.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 GuidedGraspPlanningTask::GuidedGraspPlanningTask(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 GuidedGraspPlanningTask::~GuidedGraspPlanningTask() 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 GuidedGraspPlanningTask::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() && world->getCurrentHand()->getDBName() == QString(mPlanningTask.handName.c_str())) { 00091 DBGA("Guided Grasp Planning Task: using currently loaded hand"); 00092 mHand = world->getCurrentHand(); 00093 } else { 00094 if(world->getCurrentHand()) world->removeRobot(world->getCurrentHand()); 00095 QString handPath = mDBMgr->getHandGraspitPath(QString(mPlanningTask.handName.c_str())); 00096 handPath = QString(getenv("GRASPIT")) + handPath; 00097 DBGA("Guided 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 GuidedPlanner(mHand); 00132 mPlanner->setModelState(&seed); 00133 mPlanner->setContactType(CONTACT_PRESET); 00134 00135 if (mPlanningTask.taskTime >= 0) mPlanner->setMaxTime(mPlanningTask.taskTime); 00136 else mPlanner->setMaxTime(-1); 00137 00138 QObject::connect(mPlanner, SIGNAL(update()), this, SLOT(plannerUpdate())); 00139 QObject::connect(mPlanner, SIGNAL(complete()), this, SLOT(plannerComplete())); 00140 00141 if (!mPlanner->resetPlanner()) { 00142 DBGA("Grasp Planning Task: failed to reset planner"); 00143 mStatus = ERROR; 00144 return; 00145 } 00146 mLastSolution = 0; 00147 mPlanner->startPlanner(); 00148 mStatus = RUNNING; 00149 } 00150 00151 void GuidedGraspPlanningTask::plannerComplete() 00152 { 00153 mStatus = DONE; 00154 } 00155 00156 void GuidedGraspPlanningTask::plannerUpdate() 00157 { 00158 while (mLastSolution +1 < mPlanner->getListSize() ) 00159 { 00160 DBGA("New solution!"); 00161 if (mLastSolution + 2 > mPlanner->getListSize()) 00162 { 00163 DBGA("Error, expected even number of solutions"); 00164 mStatus = ERROR; 00165 return; 00166 } 00167 if ( !saveGrasp(mPlanner->getGrasp(mLastSolution), mPlanner->getGrasp(mLastSolution+1)) ) 00168 { 00169 DBGA("Error saving grasp"); 00170 mStatus = ERROR; 00171 return; 00172 } 00173 mLastSolution += 2; 00174 } 00175 } 00176 00177 bool GuidedGraspPlanningTask::saveGrasp(const GraspPlanningState *pre_gps, const GraspPlanningState *final_gps) 00178 { 00179 GraspitDBModel* dbModel= mObject->getDBModel(); 00180 assert(dbModel); 00181 00182 db_planner::Grasp* grasp = new db_planner::Grasp; 00183 00184 std::vector<double> contacts = grasp->GetContacts(); 00185 00186 grasp->SetSourceModel( *(static_cast<db_planner::Model*>(dbModel)) ); 00187 grasp->SetHandName(mHand->getDBName().toStdString()); 00188 grasp->SetEpsilonQuality(final_gps->getEpsilonQuality()); 00189 grasp->SetVolumeQuality(final_gps->getVolume()); 00190 grasp->SetEnergy(final_gps->getEnergy()); 00191 grasp->SetClearance(0.0); 00192 grasp->SetClusterRep(false); 00193 grasp->SetCompliantCopy(false); 00194 grasp->SetSource("EIGENGRASPS"); 00195 00196 std::vector<double> tempArray; 00197 00198 //the final grasp 00199 GraspPlanningState *sol = new GraspPlanningState(final_gps); 00200 //make sure you pass it sticky=true 00201 sol->setPositionType(SPACE_COMPLETE,true); 00202 sol->setPostureType(POSE_DOF,true); 00203 tempArray.clear(); 00204 for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){ 00205 tempArray.push_back(sol->readPosture()->readVariable(i)); 00206 } 00207 grasp->SetFinalgraspJoints(tempArray); 00208 tempArray.clear(); 00209 for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){ 00210 tempArray.push_back(sol->readPosition()->readVariable(i)); 00211 } 00212 grasp->SetFinalgraspPosition(tempArray); 00213 delete sol; 00214 00215 //the pre-grasp 00216 sol = new GraspPlanningState(pre_gps); 00217 sol->setPositionType(SPACE_COMPLETE,true); 00218 sol->setPostureType(POSE_DOF,true); 00219 tempArray.clear(); 00220 for(int i = 0; i < sol->readPosture()->getNumVariables(); ++i){ 00221 tempArray.push_back(sol->readPosture()->readVariable(i)); 00222 } 00223 grasp->SetPregraspJoints(tempArray); 00224 tempArray.clear(); 00225 for(int i = 0; i < sol->readPosition()->getNumVariables(); ++i){ 00226 tempArray.push_back(sol->readPosition()->readVariable(i)); 00227 } 00228 grasp->SetPregraspPosition(tempArray); 00229 delete sol; 00230 00231 //contacts 00232 //for some reason, the grasp's contact vector gets initialized to a mess! 00233 tempArray.clear(); 00234 grasp->SetContacts(tempArray); 00235 00236 std::vector<db_planner::Grasp*> graspList; 00237 graspList.push_back(grasp); 00238 00239 bool result = mDBMgr->SaveGrasps(graspList); 00240 delete grasp; 00241 return result; 00242 } 00243 00244 }