00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "graspCaptureDlg.h"
00027
00028 #include <QFileDialog>
00029
00030 #include "world.h"
00031 #include "robot.h"
00032 #include "grasp.h"
00033 #include "quality.h"
00034 #include "searchState.h"
00035 #include "contact.h"
00036 #include "quality.h"
00037 #include "graspitGUI.h"
00038 #include "ivmgr.h"
00039 #include "mytools.h"
00040
00041 #include "debug.h"
00042
00043 #ifdef CGDB_ENABLED
00044 #include "DBase/DBPlanner/grasp.h"
00045 #include "DBase/graspit_db_grasp.h"
00046 #include "DBase/graspit_db_model.h"
00047 #include "DBase/DBPlanner/sql_database_manager.h"
00048 #endif
00049
00050 void
00051 GraspCaptureDlg::init(World *w) {
00052 mWorld = w;
00053 QObject::connect(mWorld, SIGNAL(graspsUpdated()), this, SLOT(updateQuality()));
00054 mIndicator = new QualityIndicator(this);
00055 mIndicator->setAttribute(Qt::WA_ShowModal, false);
00056 mIndicator->setAttribute(Qt::WA_DeleteOnClose, false);
00057 mIndicator->show();
00058 mQualEpsilon = NULL;
00059 mQualVolume = NULL;
00060 mCurrentHand = NULL;
00061 saveToDBaseButton->setEnabled(FALSE);
00062
00063 if (graspItGUI->getIVmgr()->getDBMgr()) {
00064 saveToDBaseButton->setEnabled(TRUE);
00065 } else {
00066 QTWARNING("DBase connection not found; only Save to File possible.");
00067 saveToDBaseButton->setEnabled(FALSE);
00068 }
00069 }
00070
00071 GraspCaptureDlg::~GraspCaptureDlg()
00072 {
00073 mIndicator->close();
00074 delete mIndicator;
00075 clearListButtonClicked();
00076 delete mQualEpsilon;
00077 delete mQualVolume;
00078 }
00079
00080 bool
00081 GraspCaptureDlg::checkHandSelection()
00082 {
00083 Hand *hand = mWorld->getCurrentHand();
00084 if (!hand) return false;
00085 if (mCurrentHand != hand) {
00086 mCurrentHand = hand;
00087 delete mQualEpsilon;
00088 mQualEpsilon = new QualEpsilon(hand->getGrasp(), QString("Grasp_recorder_qm"), "L1 Norm");
00089 delete mQualVolume;
00090 mQualVolume = new QualVolume(hand->getGrasp(), QString("Grasp_recorder_qm"), "L1 Norm");
00091 }
00092 GraspableBody *body = hand->getGrasp()->getObject();
00093 if (!body) return false;
00094 return true;
00095 }
00096
00097 void
00098 GraspCaptureDlg::captureButtonClicked()
00099 {
00100 if (!checkHandSelection()) return;
00101 if (!mWorld->noCollision()) {
00102 DBGA("COLLISION");
00103 return;
00104 }
00105 mWorld->findAllContacts();
00106 mWorld->updateGrasps();
00107 double quality = mQualEpsilon->evaluate();
00108 if (!allowNonFCBox->isChecked() && quality < 0.0) {
00109 DBGA("NON FORCE CLOSURE");
00110 return;
00111 }
00112 GraspPlanningState *newState = new GraspPlanningState(mCurrentHand);
00113 newState->setPostureType(POSE_DOF, false);
00114 GraspableBody *body = mCurrentHand->getGrasp()->getObject();
00115 newState->setRefTran(body->getTran());
00116 newState->setObject(body);
00117 newState->setEpsilonQuality( std::max(0.0, quality) );
00118 newState->setVolume( std::max(0.0, quality) );
00119 newState->saveCurrentHandState();
00120 for (int i=0; i<mCurrentHand->getGrasp()->getNumContacts(); i++) {
00121 newState->getContacts()->push_back( mCurrentHand->getGrasp()->getContact(i)->getPosition() );
00122 }
00123 mGrasps.push_back(newState);
00124 updateNumGrasps();
00125
00126 if (graspItGUI->getIVmgr()->getDBMgr()) {
00127 saveToDBaseButton->setEnabled(TRUE);
00128 } else {
00129 saveToDBaseButton->setEnabled(FALSE);
00130 }
00131 }
00132
00133 void
00134 GraspCaptureDlg::saveToFileButtonClicked()
00135 {
00136 DBGA("Foo");
00137 if (mGrasps.empty()) {
00138 DBGA("No recorded grasps to save");
00139 return;
00140 }
00141 QString fn( QFileDialog::getSaveFileName(this, QString(),
00142 QString(getenv("GRASPIT")), "Text Files (*.txt)") );
00143 if ( fn.isEmpty() ) return;
00144 if (fn.section('.',1).isEmpty()) fn.append(".txt");
00145 FILE *fp = fopen(fn.ascii(), "a");
00146 if (!fp) {
00147 DBGA("Failed to open save file " << fn.ascii());
00148 return;
00149 }
00150 std::list<GraspPlanningState*>::iterator it;
00151 for (it=mGrasps.begin(); it!=mGrasps.end(); it++) {
00152 (*it)->writeToFile(fp);
00153 }
00154 fclose(fp);
00155 DBGA("Grasps saved.");
00156 }
00157
00162 void
00163 GraspCaptureDlg::saveToDBaseButtonClicked()
00164 {
00165 #ifndef CGDB_ENABLED
00166 return;
00167 #else
00168 db_planner::DatabaseManager *dbMgr = graspItGUI->getIVmgr()->getDBMgr();
00169 if (!dbMgr) return;
00170 std::list<GraspPlanningState*>::iterator it;
00171 std::vector<db_planner::Grasp*> graspList;
00172 for (it=mGrasps.begin(); it!=mGrasps.end(); it++) {
00173 GraspitDBModel* dbModel= (*it)->getObject()->getDBModel();
00174 if (!dbModel) {
00175 DBGA("Model not from database!");
00176 continue;
00177 }
00178 db_planner::Grasp* grasp = new GraspitDBGrasp(mCurrentHand);
00179 grasp->SetSourceModel( *(static_cast<db_planner::Model*>(dbModel)) );
00180 grasp->SetHandName(GraspitDBGrasp::getHandDBName(mCurrentHand).toStdString());
00181 grasp->SetEpsilonQuality((*it)->getEpsilonQuality());
00182 grasp->SetVolumeQuality((*it)->getVolume());
00183 grasp->SetEnergy( - 30*(*it)->getEpsilonQuality() - 100*(*it)->getVolume() );
00184
00185
00186
00187 grasp->SetSource("HUMAN_REFINED");
00188
00189 std::vector<double> tempArray;
00190
00191 for(int i = 0; i < (*it)->getPosture()->getNumVariables(); ++i){
00192 tempArray.push_back((*it)->getPosture()->getVariable(i)->getValue());
00193 }
00194 grasp->SetPregraspJoints(tempArray);
00195 grasp->SetFinalgraspJoints(tempArray);
00196
00197
00198 tempArray.clear();
00199 for(int i = 0; i < (*it)->getPosition()->getNumVariables(); ++i){
00200 tempArray.push_back((*it)->getPosition()->getVariable(i)->getValue());
00201 }
00202 grasp->SetPregraspPosition(tempArray);
00203 grasp->SetFinalgraspPosition(tempArray);
00204
00205
00206 std::list<position> *contacts;
00207 tempArray.clear();
00208 contacts = (*it)->getContacts();
00209 std::list<position>::iterator itContact;
00210 for(itContact = contacts->begin(); itContact != contacts->end(); ++itContact){
00211 tempArray.push_back((*itContact).x());
00212 tempArray.push_back((*itContact).y());
00213 tempArray.push_back((*itContact).z());
00214 }
00215 grasp->SetContacts(tempArray);
00216
00217 graspList.push_back(grasp);
00218 }
00219 dbMgr->SaveGrasps(graspList);
00220 DBGA(graspList.size() << " grasps successfully saved to database");
00221 for(size_t i = 0; i < graspList.size(); ++i){
00222 delete graspList[i];
00223 }
00224
00225 clearListButtonClicked();
00226 #endif
00227 }
00228
00229 void
00230 GraspCaptureDlg::updateNumGrasps()
00231 {
00232 QString num;
00233 num.setNum((int)mGrasps.size());
00234 recordedGraspsLabel->setText("Recorded grasps: " + num);
00235 }
00236
00237 void
00238 GraspCaptureDlg::clearListButtonClicked()
00239 {
00240 while (!mGrasps.empty()) {
00241 delete mGrasps.back();
00242 mGrasps.pop_back();
00243 }
00244 updateNumGrasps();
00245 }
00246
00247 void
00248 GraspCaptureDlg::updateQuality()
00249 {
00250 double quality = 0.0;
00251 if (checkHandSelection()) {
00252 quality = mQualEpsilon->evaluate();
00253 }
00254 mIndicator->setBar( std::max(0.0, quality) );
00255 }
