$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_clustering_task.h" 00036 00037 #include <algorithm> 00038 00039 #include "world.h" 00040 #include "robot.h" 00041 #include "graspitGUI.h" 00042 #include "matvec3D.h" 00043 #include "EGPlanner/searchState.h" 00044 #include "DBPlanner/db_manager.h" 00045 #include "graspit_db_grasp.h" 00046 #include "debug.h" 00047 00048 namespace dbase_grasp_planner { 00049 00050 GraspClusteringTask::GraspClusteringTask(graspit_dbase_tasks::DBTaskDispatcher *disp, 00051 db_planner::DatabaseManager *mgr, 00052 db_planner::TaskRecord rec) : DBTask (disp, mgr, rec) 00053 { 00054 //nothing so far 00055 } 00056 00061 void GraspClusteringTask::start() 00062 { 00063 //get the details of the planning task itself 00064 if (!mDBMgr->GetPlanningTaskRecord(mRecord.taskId, &mPlanningTask)) { 00065 DBGA("Failed to get planning record for task"); 00066 mStatus = ERROR; 00067 return; 00068 } 00069 00070 World *world = graspItGUI->getIVmgr()->getWorld(); 00071 Hand *hand; 00072 00073 //check if the currently selected hand is the same as the one we need 00074 //if not, load the hand 00075 if (world->getCurrentHand() && world->getCurrentHand()->getDBName() == QString(mPlanningTask.handName.c_str())) { 00076 DBGA("Grasp Clustering Task: using currently loaded hand"); 00077 hand = world->getCurrentHand(); 00078 } else { 00079 QString handPath = mDBMgr->getHandGraspitPath(QString(mPlanningTask.handName.c_str())); 00080 handPath = QString(getenv("GRASPIT")) + handPath; 00081 DBGA("Grasp Clustering Task: loading hand from " << handPath.latin1()); 00082 hand = static_cast<Hand*>(world->importRobot(handPath)); 00083 if ( !hand ) { 00084 DBGA("Failed to load hand"); 00085 mStatus = ERROR; 00086 return; 00087 } 00088 } 00089 mDBMgr->SetGraspAllocator(new GraspitDBGraspAllocator(hand)); 00090 00091 //load all the grasps 00092 std::vector<db_planner::Grasp*> graspList; 00093 if(!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)){ 00094 DBGA("Load grasps failed"); 00095 mStatus = ERROR; 00096 while (!graspList.empty()) { 00097 delete graspList.back(); 00098 graspList.pop_back(); 00099 } 00100 return; 00101 } 00102 00103 //sort grasps by energy (hard-coded in, maybe later we'll allow other sorting) 00104 std::sort(graspList.begin(), graspList.end(), db_planner::Grasp::CompareEnergy); 00105 00106 //if all goes well, we are done 00107 mStatus = DONE; 00108 int clusters = 0; 00109 DBGA("Clustering " << graspList.size() << " grasps"); 00110 00111 while (!graspList.empty()) { 00112 //pop the front (best grasp) 00113 db_planner::Grasp* repGrasp = graspList.front(); 00114 graspList.erase(graspList.begin()); 00115 00116 //compliant_copy grasps are ignored by clustering tasks 00117 if (repGrasp->CompliantCopy()) { 00118 delete repGrasp; 00119 continue; 00120 } 00121 00122 //mark it as cluster center in the database 00123 if (!mDBMgr->SetGraspClusterRep(repGrasp, true)) { 00124 DBGA("Failed to mark cluster rep in database"); 00125 mStatus = ERROR; 00126 delete repGrasp; 00127 break; 00128 } 00129 clusters++; 00130 00131 //find other grasps in its cluster 00132 int cloud=0; 00133 std::vector<db_planner::Grasp*>::iterator it = graspList.begin(); 00134 while(it!=graspList.end()) { 00135 00136 //compliant_copy grasps are ignored by clustering tasks 00137 if ( !(*it)->CompliantCopy() && 00138 clusterGrasps(static_cast<GraspitDBGrasp*>(repGrasp), static_cast<GraspitDBGrasp*>(*it)) ) { 00139 (*it)->SetClusterRep(false); 00140 //mark it as non-center in the database 00141 if (!mDBMgr->SetGraspClusterRep(*it, false)) { 00142 DBGA("Failed to mark non-cluster rep in database"); 00143 mStatus = ERROR; 00144 break; 00145 } 00146 cloud++; 00147 delete *it; 00148 it = graspList.erase(it); 00149 } else { 00150 it++; 00151 } 00152 00153 } 00154 DBGA(" Marked cluster of size " << cloud); 00155 delete repGrasp; 00156 if (mStatus == ERROR) break; 00157 } 00158 while (!graspList.empty()) { 00159 delete graspList.back(); 00160 graspList.pop_back(); 00161 } 00162 DBGA("Successfully marked " << clusters << " clusters"); 00163 } 00164 00172 bool GraspClusteringTask::clusterGrasps(const GraspitDBGrasp *g1, const GraspitDBGrasp *g2) 00173 { 00174 //2 cm distance threshold 00175 double DISTANCE_THRESHOLD = 20; 00176 //30 degrees angular threshold 00177 double ANGULAR_THRESHOLD = 0.52; 00178 00179 transf t1 = g1->getHand()->getApproachTran() * g1->getFinalGraspPlanningState()->getTotalTran(); 00180 transf t2 = g2->getHand()->getApproachTran() * g2->getFinalGraspPlanningState()->getTotalTran(); 00181 00182 vec3 dvec = t1.translation() - t2.translation(); 00183 double d = dvec.len(); 00184 if (d > DISTANCE_THRESHOLD) return false; 00185 00186 Quaternion qvec = t1.rotation() * t2.rotation().inverse(); 00187 vec3 axis; double angle; 00188 qvec.ToAngleAxis(angle,axis); 00189 if (angle > M_PI) angle -= 2*M_PI; 00190 if (angle < -M_PI) angle += 2*M_PI; 00191 if (fabs(angle) > ANGULAR_THRESHOLD) return false; 00192 00193 return true; 00194 } 00195 00196 }