graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/ui/EGPlanner/egPlannerDlg.cpp Source File

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Matei T. Ciocarlie
00021 //
00022 // $Id: egPlannerDlg.cpp,v 1.9 2009/07/02 21:04:05 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "egPlannerDlg.h"
00027 
00028 #include <QGridLayout>
00029 #include <QCheckBox>
00030 #include <QVBoxLayout>
00031 #include <QCheckBox>
00032 #include <QLabel>
00033 #include <QHBoxLayout>
00034 #include <QSlider>
00035 #include <QLabel>
00036 #include <QValidator>
00037 #include <QFileDialog>
00038 
00039 #include "search.h"
00040 #include "searchState.h"
00041 #include "body.h"
00042 #include "robot.h"
00043 #include "egPlanner.h"
00044 #include "eigenGrasp.h"
00045 #include "grasp.h"
00046 #include "world.h"
00047 #include "graspitGUI.h"
00048 #include "ivmgr.h"
00049 #include "contactExaminerDlg.h"
00050 #include "onLinePlanner.h"
00051 #include "timeTest.h"
00052 #include "guidedPlanner.h"
00053 #include "loopPlanner.h"
00054 
00055 
00056 //#define GRASPITDBG
00057 #include "debug.h"
00058 
00059 void EigenGraspPlannerDlg::exitButton_clicked()
00060 {
00061  QDialog::accept();
00062 }
00063 
00064 void EigenGraspPlannerDlg::init()
00065 { 
00066  energyBox->insertItem("Hand Contacts");
00067  energyBox->insertItem("Potential Quality");
00068  energyBox->insertItem("Contacts AND Quality");
00069  energyBox->insertItem("Autograsp Quality");
00070  energyBox->insertItem("Guided Autograsp");
00071  energyBox->setCurrentItem(0);
00072 
00073  plannerTypeBox->insertItem("Sim. Ann.");
00074  plannerTypeBox->insertItem("Loop");
00075  plannerTypeBox->insertItem("Multi-Threaded");
00076  plannerTypeBox->insertItem("Online");
00077  plannerTypeBox->insertItem("Time Test");
00078  plannerTypeBox->setCurrentItem(0);
00079  
00080  plannerInitButton->setEnabled(TRUE);
00081  plannerResetButton->setEnabled(FALSE);
00082  plannerStartButton->setEnabled(FALSE);
00083  instantEnergyButton->setEnabled(FALSE);
00084 
00085  useVirtualHandBox->setChecked(true);
00086  onlineDetailsGroup->setEnabled(FALSE);
00087 
00088  QString n;
00089  QIntValidator* vAnnSteps = new QIntValidator(1,500000,this);
00090  annStepsEdit->setValidator(vAnnSteps);
00091  n.setNum(70000);
00092  annStepsEdit->setText(n);
00093 
00094  spaceSearchBox->insertItem("Complete");
00095  spaceSearchBox->insertItem("Axis-angle");
00096  spaceSearchBox->insertItem("Ellipsoid");
00097  spaceSearchBox->insertItem("Approach");
00098  spaceSearchBox->setCurrentItem(1);
00099 
00100  prevGraspButton->setEnabled(FALSE);
00101  nextGraspButton->setEnabled(FALSE);
00102  bestGraspButton->setEnabled(FALSE);
00103 
00104  variableBox->setColumnLayout(0, Qt::Vertical);
00105 
00106  varGridLayout = new QGridLayout( variableBox->layout(),1,5 );
00107  varGridLayout->setSpacing(5);
00108  varGridLayout->setAlignment(Qt::AlignTop);
00109  varGridLayout->addMultiCellWidget(spaceSearchLabel,0,0,0,1);
00110  varGridLayout->addMultiCellWidget(spaceSearchBox,0,0,2,4);
00111 
00112  varGridLayout->addWidget( new QLabel("On", variableBox),1,0 );
00113  varGridLayout->addWidget( new QLabel("Name", variableBox),1,1 );
00114  varGridLayout->addWidget( new QLabel("Input", variableBox),1,2 );
00115  varGridLayout->addWidget( new QLabel("Target", variableBox),1,3 );
00116  varGridLayout->addWidget( new QLabel("Confidence", variableBox),1,4 );
00117 
00118  inputGloveBox->setEnabled(FALSE);
00119  inputLoadButton->setEnabled(FALSE);
00120 
00121  fprintf(stderr,"INIT DONE \n");
00122 }
00123 
00124 void EigenGraspPlannerDlg::destroy()
00125 {
00126  delete mHandObjectState;
00127  if (mPlanner) delete mPlanner;
00128 
00129  //cleanup
00130  for (unsigned int i=0; i<varNames.size(); i++) {
00131 //  varMainLayout->removeItem(varLayouts[i]);
00132 
00133   varGridLayout->remove(varNames[i]);
00134   varGridLayout->remove(varCheck[i]);
00135   varGridLayout->remove(varInput[i]);
00136   varGridLayout->remove(varTarget[i]);
00137   varGridLayout->remove(varConfidence[i]);
00138 
00139   delete varNames[i];
00140   delete varCheck[i];
00141   delete varInput[i];
00142   delete varTarget[i];
00143   delete varConfidence[i];
00144 
00145  }
00146  varNames.clear();
00147  varCheck.clear();
00148  varInput.clear();
00149  varConfidence.clear();
00150  varTarget.clear();
00151  varLayouts.clear();
00152 }
00153 
00154 void EigenGraspPlannerDlg::setMembers( Hand *h, GraspableBody *b )
00155 {
00156  mPlanner = NULL;
00157  mHand = h;
00158  mObject = b;
00159  mHand->getGrasp()->setObjectNoUpdate(mObject);
00160 // mHand->getGrasp()->setGravity(true);
00161  mHand->getGrasp()->setGravity(false);
00162 
00163  mHandObjectState = new GraspPlanningState(mHand);
00164  mHandObjectState->setObject(mObject);
00165  mHandObjectState->setPositionType(SPACE_AXIS_ANGLE);
00166  mHandObjectState->setRefTran(mObject->getTran());
00167  mHandObjectState->reset();
00168  setVariableLayout();
00169 
00170  if (mHand->getNumVirtualContacts() > 0) {
00171          setContactsBox->setChecked(TRUE);
00172  }
00173 
00174  updateVariableLayout();
00175  updateInputLayout();
00176 }
00177 
00178 // ----------------------------------- Search State and variable layout management -------------------------------
00179 
00180 void EigenGraspPlannerDlg::setVariableLayout()
00181 {
00182  //cleanup
00183  for (unsigned int i=0; i<varNames.size(); i++) {
00184   varGridLayout->remove(varNames[i]);
00185   varGridLayout->remove(varCheck[i]);
00186   varGridLayout->remove(varInput[i]);
00187   varGridLayout->remove(varTarget[i]);
00188   varGridLayout->remove(varConfidence[i]);
00189   delete varNames[i];
00190   delete varCheck[i];
00191   delete varInput[i];
00192   delete varTarget[i];
00193   delete varConfidence[i];
00194  }
00195  varNames.clear();
00196  varCheck.clear();
00197  varInput.clear();
00198  varTarget.clear();
00199  varConfidence.clear();
00200  varLayouts.clear();
00201 
00202  int size = mHand->getEigenGrasps()->getSize() + 7;
00203 
00204  for (int i=0; i<size; i++) {
00205   QLabel *name = new QLabel("foo",variableBox);
00206   QCheckBox *check = new QCheckBox(variableBox);
00207   connect(check, SIGNAL(clicked()), this, SLOT(variableCheckBoxChanged()));
00208   QCheckBox *inputCheck = new QCheckBox(variableBox);
00209   connect(inputCheck, SIGNAL(clicked()), this, SLOT(variableInputChanged()));
00210   QLabel *target = new QLabel("N/A",variableBox);
00211   QSlider *slider = new QSlider(0,100,10,0,Qt::Horizontal,variableBox);
00212   connect(slider, SIGNAL(sliderReleased()), this, SLOT(variableInputChanged()));
00213 
00214   slider->setLineStep(1);
00215   slider->setMaximumWidth(50);
00216   varGridLayout->addWidget(check,2+i,0);
00217   varGridLayout->addWidget(name,2+i,1);
00218   varGridLayout->addWidget(inputCheck,2+i,2);
00219   varGridLayout->addWidget(target,2+i,3);
00220   varGridLayout->addWidget(slider,2+i,4);
00221   
00222 
00223   varCheck.push_back( check );
00224   varNames.push_back( name );
00225   varInput.push_back( inputCheck );
00226   varTarget.push_back( target );
00227   varConfidence.push_back( slider );
00228  }
00229 }
00230 
00231 void EigenGraspPlannerDlg::updateVariableLayout()
00232 {
00233         int i;
00234  for (i=0; i<mHandObjectState->getNumVariables(); i++) {
00235   varNames[i]->setEnabled(TRUE);
00236   varNames[i]->setText( mHandObjectState->getVariable(i)->getName() );
00237   varCheck[i]->setEnabled(TRUE);
00238   if (mHandObjectState->getVariable(i)->isFixed()) varCheck[i]->setChecked(false);
00239   else varCheck[i]->setChecked(true);
00240  }
00241 
00242  for (i=mHandObjectState->getNumVariables(); i < mHand->getEigenGrasps()->getSize() + 7; i++) {
00243   varNames[i]->setEnabled(FALSE);
00244   varNames[i]->setText( "n/a" );
00245   varCheck[i]->setChecked(false);
00246   varCheck[i]->setEnabled(FALSE);
00247  }
00248 }
00249 
00250 void EigenGraspPlannerDlg::updateInputLayout()
00251 {
00252         int i;
00253  for (i=0; i<mHandObjectState->getNumVariables(); i++) {
00254    if ( !mPlanner || !(mPlanner->isReady() || mPlanner->isActive()) ) {
00255    varInput[i]->setEnabled(FALSE);
00256    varInput[i]->setChecked(false);
00257    varTarget[i]->setText("N/A");
00258    varTarget[i]->setEnabled(FALSE);
00259    varConfidence[i]->setValue( 0 );
00260    varConfidence[i]->setEnabled(FALSE);
00261   } else {
00262    GraspPlanningState *t = mPlanner->getTargetState();
00263    varInput[i]->setEnabled(TRUE);
00264    QString n;
00265    n.setNum(t->getVariable(i)->getValue(),'f',3);
00266    varTarget[i]->setText(n);
00267    varConfidence[i]->setValue( t->getVariable(i)->getConfidence() * 100 );
00268    if ( t->getVariable(i)->isFixed() ) {
00269     varInput[i]->setChecked(TRUE);
00270     varTarget[i]->setEnabled(TRUE);
00271     varConfidence[i]->setEnabled(TRUE);
00272    } else {
00273     varInput[i]->setChecked(FALSE);
00274     varTarget[i]->setEnabled(FALSE);
00275     varConfidence[i]->setEnabled(FALSE);
00276    }
00277   }
00278  }
00279 
00280  for (i=mHandObjectState->getNumVariables(); i < mHand->getEigenGrasps()->getSize() + 7; i++) {
00281   varInput[i]->setEnabled(FALSE);
00282   varInput[i]->setChecked(false);
00283   varTarget[i]->setText("N/A");
00284   varTarget[i]->setEnabled(FALSE);
00285   varConfidence[i]->setValue( 0 );
00286   varConfidence[i]->setEnabled(FALSE);
00287  }
00288 }
00289 
00290 void EigenGraspPlannerDlg::variableInputChanged()
00291 {
00292         assert(mPlanner);
00293         GraspPlanningState *t = mPlanner->getTargetState();
00294         assert(t);
00295         for (int i=0; i<mHandObjectState->getNumVariables(); i++) {
00296                 if (varInput[i]->isChecked()) {
00297                         varTarget[i]->setEnabled(TRUE);
00298                         varConfidence[i]->setEnabled(TRUE);
00299                         t->getVariable(i)->setFixed(true);
00300                         t->getVariable(i)->setConfidence( ((double)varConfidence[i]->value()) / 100.0);
00301                         DBGP("Conf: " << ((double)varConfidence[i]->value()) / 100.0);
00302                 }
00303                 else {
00304                         varTarget[i]->setEnabled(FALSE);
00305                         t->getVariable(i)->setFixed(false);
00306                         t->getVariable(i)->setConfidence(0);
00307                         varConfidence[i]->setValue(0);
00308                         varConfidence[i]->setEnabled(FALSE);
00309                 }
00310         }
00311 }
00312 
00313 void EigenGraspPlannerDlg::variableCheckBoxChanged()
00314 {
00315  for (int i=0; i<mHandObjectState->getNumVariables(); i++) {
00316   if (varCheck[i]->isChecked()) mHandObjectState->getVariable(i)->setFixed(false);
00317   else mHandObjectState->getVariable(i)->setFixed(true);
00318  }
00319  //force a reset of the planner
00320  plannerStartButton->setEnabled(FALSE);
00321 }
00322 
00323 void EigenGraspPlannerDlg::spaceSearchBox_activated( const QString &s )
00324 {
00325  if ( s==QString("Complete") ) {
00326   mHandObjectState->setPositionType(SPACE_COMPLETE);
00327   mHandObjectState->setRefTran( mObject->getTran() );
00328  }  else if ( s==QString("Axis-angle") ) {
00329   mHandObjectState->setPositionType(SPACE_AXIS_ANGLE);
00330   mHandObjectState->setRefTran( mObject->getTran() );
00331  } else if ( s==QString("Ellipsoid") ) {
00332   mHandObjectState->setPositionType(SPACE_ELLIPSOID);
00333   mHandObjectState->setRefTran( mObject->getTran() );
00334  } else if ( s==QString("Approach") ) {
00335   mHandObjectState->setPositionType(SPACE_APPROACH);
00336   mHandObjectState->setRefTran( mHand->getTran() );
00337  } else {
00338   fprintf(stderr,"WRONG SEARCH TYPE IN DROP BOX!\n");
00339  }
00340  mHandObjectState->reset();
00341  updateVariableLayout();
00342  //force a reset of the planner
00343  if (mPlanner) mPlanner->invalidateReset();
00344  updateStatus();
00345 }
00346 
00347 //------------------------------------- Show Results stuff ---------------------------------
00348 
00349 void EigenGraspPlannerDlg::prevGraspButton_clicked()
00350 {
00351  mDisplayState--;
00352  updateResults(true);
00353 }
00354 
00355 void EigenGraspPlannerDlg::bestGraspButton_clicked()
00356 {
00357  if (!mPlanner) return;
00358  mDisplayState = 0;
00359  updateResults(true);
00360 }
00361 
00362 void EigenGraspPlannerDlg::nextGraspButton_clicked()
00363 {
00364  mDisplayState++;
00365  updateResults(true);
00366 }
00367 
00368 void EigenGraspPlannerDlg::plannerUpdate()
00369 {
00370  assert(mPlanner);
00371  mDisplayState = 0;
00372  updateResults(false);
00373  //if we are using the CyberGlove for input this updates the target values
00374  if (inputGloveBox->isChecked()) {
00375          updateInputLayout();
00376  }
00377 }
00378 
00379 void EigenGraspPlannerDlg::updateResults(bool render)
00380 {
00381  assert(mPlanner);
00382 
00383  QString nStr;
00384  nStr.setNum(mPlanner->getCurrentStep());
00385  currentStepLabel->setText(QString("Current step: ") + nStr);
00386 
00387  nStr.setNum(mPlanner->getRunningTime(),'f',0);
00388  timeLabel->setText(QString("Time used: ") + nStr + QString(" sec."));
00389 
00390  int d = mPlanner->getListSize();
00391  int rank, size, iteration; double energy;
00392 
00393  if (d==0) {
00394   mDisplayState = rank = size = energy = iteration = 0; render = false;
00395  } else if (mDisplayState < 0){
00396   mDisplayState = 0;
00397  } else if ( mDisplayState >= d) {
00398   mDisplayState = d-1;
00399  } 
00400  
00401  if ( d!=0 ){
00402   const GraspPlanningState *s = mPlanner->getGrasp( mDisplayState);
00403   rank = mDisplayState+1;
00404   size = d;
00405   iteration = s->getItNumber();
00406   energy = s->getEnergy();
00407  }
00408 
00409  /*
00410  FILE *f = fopen("foo.txt","w");
00411  for (int i=0; i<mPlanner->getListSize(); i++) {
00412          for(int j=i+1; j<mPlanner->getListSize(); j++) {
00413                  float d = mPlanner->getGrasp(i)->distance( mPlanner->getGrasp(j) );
00414                  fprintf(stderr,"%d -- %d: %f\n",i+1,j+1,d);
00415          }
00416          fprintf(stderr,"\n");
00417          mPlanner->getGrasp(i)->writeToFile(f);
00418  }
00419  fclose(f);
00420  */
00421 
00422  QString n1,n2;
00423  n1.setNum(rank);
00424  n2.setNum(size);
00425  rankLabel->setText("Rank: " + n1 + "/" + n2);
00426  n1.setNum(iteration);
00427  iterationLabel->setText("Iteration: " + n1);
00428  n1.setNum(energy,'f',3);
00429  energyLabel->setText("Energy: " + n1);
00430 
00431  if (render) {
00432   mPlanner->showGrasp(mDisplayState);
00433   //mHand->getWorld()->findAllContacts();
00434   //mHand->getGrasp()->update();
00435   mPlanner->getGrasp(mDisplayState)->printState();
00436   //mHand->autoGrasp(true);
00437  }
00438 }
00439 
00440 // ----------------------------- Settings management ---------------------------
00441 
00442 
00443 void EigenGraspPlannerDlg::updateStatus()
00444 {
00445         PlannerState s = DONE;
00446         if (mPlanner) s = mPlanner->getState();
00447         DBGP("Update Layout");
00448         switch(s) {
00449                 case INIT:
00450                         plannerInitButton->setEnabled(FALSE);
00451                         plannerResetButton->setEnabled(TRUE);
00452                         plannerStartButton->setEnabled(FALSE);
00453                         plannerStartButton->setText(">");
00454 
00455                         prevGraspButton->setEnabled(FALSE);
00456                         nextGraspButton->setEnabled(FALSE);
00457                         bestGraspButton->setEnabled(FALSE);
00458                         mObject->showFrictionCones(true);
00459 
00460                         inputGloveBox->setEnabled(FALSE);
00461                         inputLoadButton->setEnabled(FALSE);
00462                         onlineDetailsGroup->setEnabled(FALSE);
00463                         break;
00464                 case READY:
00465                         {
00466                         plannerInitButton->setEnabled(FALSE);
00467                         plannerResetButton->setEnabled(TRUE);
00468                         plannerStartButton->setEnabled(TRUE);
00469                         plannerStartButton->setText(">");
00470 
00471                         prevGraspButton->setEnabled(TRUE);
00472                         nextGraspButton->setEnabled(TRUE);
00473                         bestGraspButton->setEnabled(TRUE);
00474                         mObject->showFrictionCones(true);
00475 
00476                         inputGloveBox->setEnabled(TRUE);
00477                         inputLoadButton->setEnabled(TRUE);
00478                         if (mPlanner->getType() == PLANNER_ONLINE) onlineDetailsGroup->setEnabled(TRUE);
00479                         else onlineDetailsGroup->setEnabled(FALSE);
00480                         }
00481                         break;
00482                 case RUNNING:
00483                         plannerInitButton->setEnabled(FALSE);
00484                         plannerResetButton->setEnabled(FALSE);
00485                         plannerStartButton->setEnabled(TRUE);
00486                         plannerStartButton->setText("||");
00487 
00488                         prevGraspButton->setEnabled(FALSE);
00489                         nextGraspButton->setEnabled(FALSE);
00490                         bestGraspButton->setEnabled(FALSE);
00491                         mObject->showFrictionCones(false);
00492                         break;
00493                 default:
00494                         plannerInitButton->setEnabled(TRUE);
00495                         plannerResetButton->setEnabled(FALSE);
00496                         plannerStartButton->setEnabled(FALSE);
00497                         plannerStartButton->setText(">");
00498 
00499                         prevGraspButton->setEnabled(FALSE);
00500                         nextGraspButton->setEnabled(FALSE);
00501                         bestGraspButton->setEnabled(FALSE);
00502                         mObject->showFrictionCones(true);
00503 
00504                         inputGloveBox->setEnabled(FALSE);
00505                         inputLoadButton->setEnabled(FALSE);
00506                         onlineDetailsGroup->setEnabled(FALSE);
00507                         break;
00508         }
00509         updateInputLayout();
00510 }
00511 
00512 void EigenGraspPlannerDlg::energyBox_activated( const QString & )
00513 {
00514  //force a reset of the planner
00515         if (mPlanner) mPlanner->invalidateReset();
00516         updateStatus();
00517 }
00518 
00519 void EigenGraspPlannerDlg::setContactsBox_toggled( bool checked)
00520 {
00521  if (checked) {
00522   if ( mHand->getNumVirtualContacts() == 0 ) {
00523    //if we are asking to use pre-set contacts, but none are available, pop up the dialog
00524    //for loading virtual contacts
00525    ContactExaminerDlg dlg(this);
00526    dlg.exec();
00527   }
00528   if (mHand->getNumVirtualContacts() == 0) {
00529    //if we still have no virtual contacts, un-check the box
00530    setContactsBox->setChecked(false);
00531   }
00532  }
00533  if (mPlanner) mPlanner->invalidateReset();
00534  updateStatus();
00535 }
00536 
00537 void EigenGraspPlannerDlg::readPlannerSettings()
00538 {
00539  assert(mPlanner);
00540  //energy type
00541  QString s = energyBox->currentText();
00542  if ( s == QString("Hand Contacts") ) {
00543   mPlanner->setEnergyType(ENERGY_CONTACT);
00544  } else if ( s == QString("Potential Quality") ) {
00545   mPlanner->setEnergyType(ENERGY_POTENTIAL_QUALITY);
00546  } else if ( s == QString("Autograsp Quality") ) {
00547   mPlanner->setEnergyType(ENERGY_AUTOGRASP_QUALITY);
00548  } else if ( s == QString("Contacts AND Quality") ) {
00549   mPlanner->setEnergyType(ENERGY_CONTACT_QUALITY);
00550  } else if ( s == QString("Guided Autograsp") ) {
00551   mPlanner->setEnergyType(ENERGY_GUIDED_AUTOGRASP);
00552  } else {
00553   fprintf(stderr,"WRONG ENERGY TYPE IN DROP BOX!\n");
00554  }
00555 
00556  //contact type
00557  if ( setContactsBox->isChecked() ) {
00558   mPlanner->setContactType(CONTACT_PRESET);
00559  } else {
00560   mPlanner->setContactType(CONTACT_LIVE);
00561  }
00562 
00563  //steps
00564  int steps = annStepsEdit->text().toInt();
00565  mPlanner->setMaxSteps(steps);
00566 }
00567 
00568 void EigenGraspPlannerDlg::plannerComplete()
00569 {
00570  updateStatus();
00571  bestGraspButton_clicked();
00572 }
00573 //----------------------------------- Planner start / stop control stuff ---------------------------
00574 
00575 void EigenGraspPlannerDlg::plannerInit_clicked()
00576 {
00577  QString s = plannerTypeBox->currentText();
00578  if (s == QString("Sim. Ann.")) {
00579   if (mPlanner) delete mPlanner;
00580   mPlanner = new SimAnnPlanner(mHand);
00581   ((SimAnnPlanner*)mPlanner)->setModelState(mHandObjectState);
00582   energyBox->setEnabled(TRUE);
00583  } else if (s == QString("Loop")) {
00584   if (mPlanner) delete mPlanner;
00585   mPlanner = new LoopPlanner(mHand);
00586   ((LoopPlanner*)mPlanner)->setModelState(mHandObjectState);
00587   energyBox->setEnabled(TRUE);
00588  } else if (s == QString("Multi-Threaded")) {
00589   if (mPlanner) delete mPlanner;
00590   mPlanner = new GuidedPlanner(mHand);
00591   ((GuidedPlanner*)mPlanner)->setModelState(mHandObjectState);
00592   energyBox->setCurrentItem(2);
00593   energyBox->setEnabled(FALSE);
00594  } else if (s == QString("Online") ) {
00595   if (mPlanner) delete mPlanner;
00596   mPlanner = new OnLinePlanner(mHand);
00597   ((OnLinePlanner*)mPlanner)->setModelState(mHandObjectState);
00598   energyBox->setEnabled(TRUE);
00599   energyBox->setCurrentItem(2);
00600   QString n;
00601   n.setNum(2000);
00602   annStepsEdit->setText(n);
00603   QObject::connect(mPlanner,SIGNAL(update()),this,SLOT(onlinePlannerUpdate())); 
00604  } else if ( s == QString("Time Test") ) {
00605   if (mPlanner) delete mPlanner;
00606   mPlanner = new MTTester(mHand);
00607  } else {
00608   fprintf(stderr,"Unknown planner type requested\n");
00609   return;
00610  }
00611 
00612  QObject::connect(mPlanner,SIGNAL(update()),this,SLOT(plannerUpdate()));
00613  QObject::connect(mPlanner,SIGNAL(complete()),this,SLOT(plannerComplete()));
00614 
00615  updateStatus();
00616  plannerReset_clicked();
00617 }
00618 
00619 void EigenGraspPlannerDlg::plannerReset_clicked() 
00620 {
00621         assert(mPlanner);
00622         readPlannerSettings();
00623         mPlanner->resetPlanner();
00624         updateStatus();
00625 }
00626 
00627 void EigenGraspPlannerDlg::startPlanner()
00628 {
00629         mPlanner->startPlanner();
00630         updateStatus();
00631 }
00632 
00633 void EigenGraspPlannerDlg::stopPlanner()
00634 {
00635         mPlanner->pausePlanner();
00636         updateStatus();
00637 }
00638 
00639 void EigenGraspPlannerDlg::plannerStart_clicked()
00640 {       
00641         if (!mPlanner->isActive()){
00642                 startPlanner();
00643         } else {
00644                 stopPlanner();
00645         }
00646 }
00647 
00648 void EigenGraspPlannerDlg::plannerTypeBox_activated( const QString & )
00649 {
00650         if (mPlanner) {
00651                 delete mPlanner;
00652                 mPlanner = NULL;
00653         }
00654         updateStatus();
00655 }
00656 
00657 //----------------------------------- Dedicated on-line planner control ---------------------------
00658 
00659 void EigenGraspPlannerDlg::autoGraspBox_clicked()
00660 {
00661 
00662 }
00663 
00664 //this slot does the updating that's specific to the online planner
00665 void EigenGraspPlannerDlg::onlinePlannerUpdate()
00666 {
00667  assert( mPlanner->getType() == PLANNER_ONLINE);
00668  OnLinePlanner *op = (OnLinePlanner*)mPlanner;
00669  QString num;
00670 
00671  double objDist = op->getObjectDistance();
00672  num.setNum(objDist,'f',0);
00673  objDistLabel->setText("Object distance: "  + num);
00674 /*
00675  double solDist = op->getSolutionDistance();
00676  if (solDist >= 0) num.setNum(solDist,'f',0);
00677  else num.setAscii("N/A");
00678  solDistLabel->setText("Solution distance: " + num);
00679 */
00680  ActionType s = op->getAction();
00681  switch (s) {
00682          case ACTION_PLAN:
00683                  num.setAscii("PLANNING");
00684                  break;
00685          case ACTION_GRASP:
00686                  num.setAscii("GRASPING");
00687                  break;
00688          case ACTION_OPEN:
00689                  num.setAscii("OPEN");
00690                  break;
00691          default:
00692                  num.setAscii("N/A");
00693                  break;
00694  }
00695  onlineStatusLabel->setText("Status: " + num);
00696 
00697  num.setNum( op->getSABufferSize() );
00698  saBufferLabel->setText("SimAnn buffer: " + num);
00699  num.setNum( op->getFCBufferSize() );
00700  fcBufferLabel->setText("FC Thread buffer: " + num); 
00701 }
00702 
00703 void EigenGraspPlannerDlg::onlineGraspButton_clicked()
00704 {
00705  assert( mPlanner->getType() == PLANNER_ONLINE);
00706  OnLinePlanner *op = (OnLinePlanner*)mPlanner;
00707  op->action(ACTION_GRASP);
00708  onlinePlannerUpdate();
00709 }
00710 
00711 void EigenGraspPlannerDlg::onlineReleaseButton_clicked()
00712 {
00713  assert( mPlanner->getType() == PLANNER_ONLINE);
00714  OnLinePlanner *op = (OnLinePlanner*)mPlanner;
00715  op->action(ACTION_OPEN);
00716  onlinePlannerUpdate();
00717 }
00718 
00719 
00720 void EigenGraspPlannerDlg::onlinePlanButton_clicked()
00721 {
00722  assert( mPlanner->getType() == PLANNER_ONLINE);
00723  OnLinePlanner *op = (OnLinePlanner*)mPlanner;
00724  op->action(ACTION_PLAN);
00725  onlinePlannerUpdate(); 
00726 }
00727 
00728 
00729 void EigenGraspPlannerDlg::instantEnergyButton_clicked()
00730 {
00731  assert(mPlanner);
00732 // mPlanner->instantEnergy();
00733 }
00734 
00735 
00736 void EigenGraspPlannerDlg::showCloneBox_toggled( bool c)
00737 {
00738  assert( mPlanner->getType() == PLANNER_ONLINE);
00739  OnLinePlanner *op = (OnLinePlanner*)mPlanner;
00740  op->showClone(c);  
00741 }
00742 
00743 
00744 void EigenGraspPlannerDlg::showSolutionBox_toggled( bool c)
00745 {       
00746  assert( mPlanner->getType() == PLANNER_ONLINE);
00747  OnLinePlanner *op = (OnLinePlanner*)mPlanner;
00748  op->showSolutionClone(c);
00749 }
00750 
00751 void EigenGraspPlannerDlg::useVirtualHandBox_clicked()
00752 {
00753         /*
00754  assert( mPlanner->getType() == PLANNER_ONLINE);
00755  OnLinePlanner *op = (OnLinePlanner*)mPlanner;
00756  bool c = useVirtualHandBox->isChecked();
00757  op->controlVirtualHand(c);
00758  */
00759 
00760 }
00761 
00762 void EigenGraspPlannerDlg::useRealBarrettBox_toggled( bool s)
00763 {
00764  assert( mPlanner->getType() == PLANNER_ONLINE);
00765  OnLinePlanner *op = (OnLinePlanner*)mPlanner;
00766  op->useRealBarrettHand(s);
00767 }
00768 
00769 //---------------------------------------- Input selection -----------------------------------------
00770 
00771 void EigenGraspPlannerDlg::inputGloveBox_toggled( bool on)
00772 {
00773         assert(mPlanner);
00774         mPlanner->setInput(INPUT_GLOVE, on);
00775 }
00776 
00777 void EigenGraspPlannerDlg::inputLoadButton_clicked()
00778 {
00779         assert(mPlanner);
00780         QString fn = QFileDialog::getOpenFileName(this, QString(),  QString(getenv("GRASPIT"))+QString("/models/grasps"),
00781                 "Grasp Files (*.txt)" );
00782 
00783         if ( fn.isEmpty() ) {
00784                 return;
00785         }
00786 
00787         FILE *fp = fopen(fn.latin1(), "r");
00788         bool success = true;
00789         if (!fp) {
00790                 DBGA("Failed to open input file!");
00791                 success = false;
00792         }else if (!mPlanner->getTargetState()->readFromFile(fp)) {
00793                 DBGA("Failed to read target from input file!");
00794                 success = false;
00795         } else {
00796                 DBGA("Target values loaded successfully");
00797         }
00798         fclose(fp);
00799         mPlanner->setInput(INPUT_FILE, success);
00800         updateInputLayout();
00801 }