From cbd6a85a12e1eef1face16b888906c377b961b64 Mon Sep 17 00:00:00 2001 From: Jake Varley Date: Tue, 4 Oct 2016 14:11:59 -0400 Subject: [PATCH 1/2] adding list planner --- CMakeLists.txt | 8 +- src/EGPlanner/listPlanner.cpp | 18 +- ui/EGPlanner/compliantPlannerDlg.ui | 498 ------------------ ...liantPlannerDlg.cpp => listPlannerDlg.cpp} | 193 ++----- ...compliantPlannerDlg.h => listPlannerDlg.h} | 29 +- ui/EGPlanner/listPlannerDlg.ui | 357 +++++++++++++ ui/mainWindow.cpp | 12 +- ui/mainWindow.h | 2 +- ui/mainWindow.ui | 9 +- 9 files changed, 446 insertions(+), 680 deletions(-) delete mode 100644 ui/EGPlanner/compliantPlannerDlg.ui rename ui/EGPlanner/{compliantPlannerDlg.cpp => listPlannerDlg.cpp} (72%) rename ui/EGPlanner/{compliantPlannerDlg.h => listPlannerDlg.h} (78%) create mode 100644 ui/EGPlanner/listPlannerDlg.ui diff --git a/CMakeLists.txt b/CMakeLists.txt index b1a2d26f1e..6f4e82bc5a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -522,7 +522,7 @@ set (GRASPIT_FORMS ${GSRC}/ui/settingsDlg.ui ${GSRC}/ui/Planner/plannerdlg.ui ${GSRC}/ui/EGPlanner/egPlannerDlg.ui - ${GSRC}/ui/EGPlanner/compliantPlannerDlg.ui + ${GSRC}/ui/EGPlanner/listPlannerDlg.ui ) set (GRASPIT_FORM_HEADERS @@ -540,7 +540,7 @@ set (GRASPIT_FORM_HEADERS ${GSRC}/ui/qmDlg.h ${GSRC}/ui/Planner/plannerdlg.h ${GSRC}/ui/EGPlanner/egPlannerDlg.h - ${GSRC}/ui/EGPlanner/compliantPlannerDlg.h + ${GSRC}/ui/EGPlanner/listPlannerDlg.h ) set (GRASPIT_FORM_SOURCES @@ -558,7 +558,7 @@ set (GRASPIT_FORM_SOURCES ${GSRC}/ui/settingsDlg.cpp ${GSRC}/ui/Planner/plannerdlg.cpp ${GSRC}/ui/EGPlanner/egPlannerDlg.cpp - ${GSRC}/ui/EGPlanner/compliantPlannerDlg.cpp + ${GSRC}/ui/EGPlanner/listPlannerDlg.cpp ) set (MOC_HEADERS @@ -568,7 +568,7 @@ set (MOC_HEADERS ${GSRC}/ui/bodyPropDlg.h ${GSRC}/ui/eigenGraspDlg.h ${GSRC}/ui/EGPlanner/egPlannerDlg.h - ${GSRC}/ui/EGPlanner/compliantPlannerDlg.h + ${GSRC}/ui/EGPlanner/listPlannerDlg.h ${GSRC}/ui/Planner/plannerdlg.h ${GSRC}/ui/graspCaptureDlg.h ${GSRC}/ui/contactExaminerDlg.h diff --git a/src/EGPlanner/listPlanner.cpp b/src/EGPlanner/listPlanner.cpp index c225a09251..8e907bdccc 100644 --- a/src/EGPlanner/listPlanner.cpp +++ b/src/EGPlanner/listPlanner.cpp @@ -34,7 +34,7 @@ //this should be a parameter of the EGPlanner //iros09 -#define BEST_LIST_SIZE 2000 +#define BEST_LIST_SIZE 20000 ListPlanner::ListPlanner(Hand *h) { @@ -116,7 +116,7 @@ void ListPlanner::prepareState(int index) { showState(index); - mHand->findInitialContact(200); + mHand->findInitialContact(200); } /*! The main planning function. Simply takes the next input grasp from the list, @@ -136,7 +136,13 @@ ListPlanner::mainLoop() //so that after dynamics object gets put back bool legal; double energy; PRINT_STAT(mOut, mCurrentStep); - mEnergyCalculator->analyzeState(legal, energy, *mPlanningIterator, true); + + +// (*mPlanningIterator)->execute(); +// mHand->findInitialContact(200); + + + mEnergyCalculator->analyzeState(legal, energy, *mPlanningIterator, false); //for rendering purposes; will see later if it's needed mCurrentState->copyFrom(*mPlanningIterator); @@ -147,7 +153,7 @@ ListPlanner::mainLoop() //this whole thing could go into a higher level fctn in EGPlanner if (legal) { double worstEnergy; - if ((int)mBestList.size() < BEST_LIST_SIZE) worstEnergy = 1.0e5; + if ((int)mBestList.size() < BEST_LIST_SIZE) worstEnergy = 1.0e5; else worstEnergy = mBestList.back()->getEnergy(); if (energy < worstEnergy) { GraspPlanningState *insertState = new GraspPlanningState(*mPlanningIterator); @@ -156,7 +162,7 @@ ListPlanner::mainLoop() DBGP("Solution at step " << mCurrentStep); mBestList.push_back(insertState); mBestList.sort(GraspPlanningState::compareStates); - while ((int)mBestList.size() > BEST_LIST_SIZE) { + while ((int)mBestList.size() > BEST_LIST_SIZE) { delete(mBestList.back()); mBestList.pop_back(); } @@ -164,7 +170,7 @@ ListPlanner::mainLoop() } //advance the planning iterator - mPlanningIterator++; + mPlanningIterator++; mCurrentStep++; Q_EMIT update(); PRINT_STAT(mOut, std::endl); diff --git a/ui/EGPlanner/compliantPlannerDlg.ui b/ui/EGPlanner/compliantPlannerDlg.ui deleted file mode 100644 index 3a0bb13edd..0000000000 --- a/ui/EGPlanner/compliantPlannerDlg.ui +++ /dev/null @@ -1,498 +0,0 @@ - - CompliantPlannerDlgUI - - - - 0 - 0 - 246 - 444 - - - - Compliant Planner - - - - - 40 - 10 - 61 - 25 - - - - Generate - - - - - - 90 - 140 - 21 - 25 - - - - > - - - - - - 40 - 140 - 21 - 25 - - - - < - - - - - - 60 - 140 - 31 - 25 - - - - Best - - - - - - 130 - 110 - 111 - 16 - - - - computation type - - - - - - 10 - 40 - 61 - 25 - - - - Test All - - - - - - 10 - 12 - 23 - 20 - - - - - - - - - - 120 - 166 - 101 - 16 - - - - Energy: 0.00 - - - - - - 120 - 145 - 91 - 16 - - - - Rank: 0/0 - - - - - - 60 - 70 - 31 - 25 - - - - Test - - - - - - 100 - 70 - 41 - 25 - - - - Show - - - - - - 150 - 70 - 41 - 25 - - - - Prep - - - - - - 10 - 72 - 41 - 20 - - - - - - - 77 - 46 - 101 - 16 - - - - TextLabel - - - - - - 108 - 14 - 101 - 18 - - - - Visual markers - - - - - - 10 - 110 - 111 - 22 - - - - - - - 120 - 190 - 101 - 16 - - - - Iteration: 0 - - - - - - 120 - 400 - 111 - 33 - - - - - 6 - - - 0 - - - - - Qt::Horizontal - - - - 131 - 31 - - - - - - - - Close - - - - - - - - - 10 - 374 - 61 - 16 - - - - Output to: - - - - - - 10 - 400 - 75 - 25 - - - - Reset Object - - - - - - 152 - 370 - 81 - 20 - - - - - - - 70 - 370 - 71 - 22 - - - - - - - 10 - 210 - 221 - 16 - - - - Qt::Horizontal - - - - - - 10 - 230 - 75 - 23 - - - - Design Test - - - - - - 110 - 260 - 31 - 20 - - - - - - - 110 - 290 - 31 - 20 - - - - - - - 110 - 320 - 31 - 20 - - - - - - - 150 - 260 - 31 - 20 - - - - - - - 150 - 290 - 31 - 20 - - - - - - - 150 - 320 - 31 - 20 - - - - - - - 80 - 260 - 31 - 16 - - - - From - - - - - - 90 - 290 - 16 - 16 - - - - To - - - - - - 80 - 320 - 31 - 16 - - - - Step - - - - - - 110 - 230 - 41 - 16 - - - - Torque - - - - - - 150 - 230 - 21 - 16 - - - - Stiff - - - - - - 10 - 340 - 221 - 16 - - - - Qt::Horizontal - - - - - - - okButton - clicked() - CompliantPlannerDlgUI - accept() - - - 278 - 253 - - - 96 - 254 - - - - - diff --git a/ui/EGPlanner/compliantPlannerDlg.cpp b/ui/EGPlanner/listPlannerDlg.cpp similarity index 72% rename from ui/EGPlanner/compliantPlannerDlg.cpp rename to ui/EGPlanner/listPlannerDlg.cpp index a1b19957f6..df8d303771 100644 --- a/ui/EGPlanner/compliantPlannerDlg.cpp +++ b/ui/EGPlanner/listPlannerDlg.cpp @@ -23,7 +23,7 @@ // //###################################################################### -#include "compliantPlannerDlg.h" +#include "listPlannerDlg.h" #include @@ -53,11 +53,13 @@ PROF_DECLARE(QS_TOTAL); #include "searchState.h" void -CompliantPlannerDlg::init() +ListPlannerDlg::init() { mPlanner = new ListPlanner(mHand); + energyTypeBox->insertItem("AutoGrasp"); energyTypeBox->insertItem("Quasistatic"); - energyTypeBox->insertItem("Dynamic"); + samplingTypeBox->insertItem("Ellipsoid"); + samplingTypeBox->insertItem("Grid"); mPlanner->setRenderType(RENDER_ALWAYS); mPlanner->setStatStream(&std::cerr); mOut = NULL; @@ -69,32 +71,14 @@ CompliantPlannerDlg::init() currentLabel->setText("0/0"); QIntValidator* val = new QIntValidator(2, 99, this); resolutionEdit->setValidator(val); - resolutionEdit->setText("8"); - QIntValidator* val2 = new QIntValidator(this); - testOneEdit->setValidator(val2); - testOneEdit->setText("0"); + resolutionEdit->setText("3");; resultsBox->insertItem("console"); resultsBox->insertItem("file:"); resultsFileEdit->setText("comp_plan.txt"); - QDoubleValidator* dVal = new QDoubleValidator(0.5, 1.5, 3, this); - tFromEdit->setValidator(dVal); - tToEdit->setValidator(dVal); - tStepEdit->setValidator(dVal); - sFromEdit->setValidator(dVal); - sToEdit->setValidator(dVal); - sStepEdit->setValidator(dVal); - - tFromEdit->setText("0.5"); - tToEdit->setText("1.5"); - tStepEdit->setText("0.1"); - sFromEdit->setText("0.5"); - sToEdit->setText("1.5"); - sStepEdit->setText("0.1"); - } -CompliantPlannerDlg::~CompliantPlannerDlg() +ListPlannerDlg::~ListPlannerDlg() { if (mOut) { mOut->close(); @@ -104,7 +88,7 @@ CompliantPlannerDlg::~CompliantPlannerDlg() } void -CompliantPlannerDlg::addCartesianSamples(const GraspPlanningState &seed, +ListPlannerDlg::addCartesianSamples(const GraspPlanningState &seed, std::list *sampling, int samples, double x, double y, double z) { @@ -137,7 +121,7 @@ CompliantPlannerDlg::addCartesianSamples(const GraspPlanningState &seed, ratio and projecting the resulting points on the ellipsoid. Not ideal, but at least much better then sampling angular variables directly */ void -CompliantPlannerDlg::gridEllipsoidSampling(const GraspPlanningState &seed, +ListPlannerDlg::gridEllipsoidSampling(const GraspPlanningState &seed, std::list *sampling, int samples) { @@ -162,7 +146,7 @@ CompliantPlannerDlg::gridEllipsoidSampling(const GraspPlanningState &seed, } void -CompliantPlannerDlg::generateButtonClicked() +ListPlannerDlg::generateButtonClicked() { int resolution = resolutionEdit->text().toInt(); if (resolution < 1) { @@ -179,18 +163,24 @@ CompliantPlannerDlg::generateButtonClicked() double a = 0.5*(bbmax[0] - bbmin[0]); double b = 0.5*(bbmax[1] - bbmin[1]); double c = 0.5*(bbmax[2] - bbmin[2]); - //ellipsoidSampling(a,b,c,resolution); - boxSampling(a,b,c, resolution); + + if (samplingTypeBox->currentText() == "Grid") { + boxSampling(a,b,c, resolution); + } else if (samplingTypeBox->currentText() == "Ellipsoid") { + ellipsoidSampling(a,b,c,resolution); + } else { + DBGA("Invalid Sampling Type"); + assert(0); + } + mPlanner->showVisualMarkers( true); mPlanner->resetPlanner(); - if (visualMarkersBox->isChecked()) { - visualMarkersBoxClicked(); - } + update(); } void -CompliantPlannerDlg::ellipsoidSampling(double a, double b, double c, double resolution) +ListPlannerDlg::ellipsoidSampling(double a, double b, double c, double resolution) { //generate a list of grasps by sampling an ellipsoid around the object GraspPlanningState seed(mHand); @@ -222,7 +212,7 @@ CompliantPlannerDlg::ellipsoidSampling(double a, double b, double c, double reso } void -CompliantPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z, +ListPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z, double sz1, double sz2, vec3 tln, double res, std::list *sampling) { @@ -257,7 +247,7 @@ CompliantPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z, } void -CompliantPlannerDlg::boxSampling(double a, double b, double c, double res) +ListPlannerDlg::boxSampling(double a, double b, double c, double res) { std::list sampling; res = 30; @@ -276,46 +266,18 @@ CompliantPlannerDlg::boxSampling(double a, double b, double c, double res) } void -CompliantPlannerDlg::testOneButtonClicked() -{ - if (energyTypeBox->currentText() == "Quasistatic") { - mPlanner->setEnergyType(ENERGY_COMPLIANT); - } else if (energyTypeBox->currentText() == "Dynamic") { - mPlanner->setEnergyType(ENERGY_DYNAMIC); - } else { - assert(0); - } - if (mPlanner->isActive()) { - DBGA("Stop planner first!"); - return; - } - int num = testOneEdit->text().toInt(); - if (num < 0 || num >= mNumCandidates) { - DBGA("Wrong test number selected"); - return; - } - //single tests are always printed out to console - mPlanner->setStatStream(&std::cerr); - DBGA("Testing pre-grasp #" << num); - mPlanner->testState(num); - mHand->getWorld()->updateGrasps(); - graspitCore->getIVmgr()->drawDynamicForces(); - graspitCore->getIVmgr()->drawUnbalancedForces(); -} - -void -CompliantPlannerDlg::startPlanner() +ListPlannerDlg::startPlanner() { mPlanner->startPlanner(); } void -CompliantPlannerDlg::testButtonClicked() +ListPlannerDlg::testButtonClicked() { if (energyTypeBox->currentText() == "Quasistatic") { mPlanner->setEnergyType(ENERGY_COMPLIANT); - } else if (energyTypeBox->currentText() == "Dynamic") { - mPlanner->setEnergyType(ENERGY_DYNAMIC); + } else if (energyTypeBox->currentText() == "AutoGrasp") { + mPlanner->setEnergyType(ENERGY_STRICT_AUTOGRASP); } else { assert(0); } @@ -337,7 +299,7 @@ CompliantPlannerDlg::testButtonClicked() } void -CompliantPlannerDlg::update() +ListPlannerDlg::update() { QString n1,n2; n1.setNum(mPlanner->getCurrentStep()); @@ -346,7 +308,7 @@ CompliantPlannerDlg::update() } void -CompliantPlannerDlg::showResult() +ListPlannerDlg::showResult() { int d = mPlanner->getListSize(); int rank, size, iteration; double energy; @@ -361,6 +323,7 @@ CompliantPlannerDlg::showResult() if ( d!=0 ){ const GraspPlanningState *s = mPlanner->getGrasp(mBestGraspNum); + mPlanner->showState(mBestGraspNum); rank = mBestGraspNum+1; size = d; energy = s->getEnergy(); @@ -374,77 +337,49 @@ CompliantPlannerDlg::showResult() n1.setNum(energy,'f',3); energyLabel->setText("Energy: " + n1); n1.setNum(iteration); - testOneEdit->setText(n1); - showOneButtonClicked(); + showOne(); iterationLabel->setText("Iteration: " + n1); } void -CompliantPlannerDlg::prevButtonClicked() +ListPlannerDlg::showOne() +{ + if (mPlanner->isActive()) { + DBGA("Stop planner first!"); + return; + } + int num = mBestGraspNum; + if (num < 0 || num >= mNumCandidates) { + DBGA("Wrong test number selected"); + return; + } + DBGA("Testing pre-grasp #" << num); + mPlanner->showState(num); +} + +void +ListPlannerDlg::prevButtonClicked() { mBestGraspNum--; showResult(); } void -CompliantPlannerDlg::nextButtonClicked() +ListPlannerDlg::nextButtonClicked() { mBestGraspNum++; showResult(); } void -CompliantPlannerDlg::bestButtonClicked() +ListPlannerDlg::bestButtonClicked() { mBestGraspNum=0; showResult(); } -void -CompliantPlannerDlg::showOneButtonClicked() -{ - if (mPlanner->isActive()) { - DBGA("Stop planner first!"); - return; - } - int num = testOneEdit->text().toInt(); - if (num < 0 || num >= mNumCandidates) { - DBGA("Wrong test number selected"); - return; - } - DBGA("Testing pre-grasp #" << num); - mPlanner->showState(num); -} - -void -CompliantPlannerDlg::prepareOneButtonClicked() -{ - if (mPlanner->isActive()) { - DBGA("Stop planner first!"); - return; - } - int num = testOneEdit->text().toInt(); - if (num < 0 || num >= mNumCandidates) { - DBGA("Wrong test number selected"); - return; - } - DBGA("Testing pre-grasp #" << num); - mPlanner->prepareState(num); -} - -void -CompliantPlannerDlg::visualMarkersBoxClicked() -{ - mPlanner->showVisualMarkers( visualMarkersBox->isChecked()); -} - -void -CompliantPlannerDlg::resetObjectButtonClicked() -{ - mObject->setTran(mObjectRefTran); -} void -CompliantPlannerDlg::updateOut() +ListPlannerDlg::updateOut() { if (mOut) { mOut->close(); @@ -465,30 +400,8 @@ CompliantPlannerDlg::updateOut() } } -void -CompliantPlannerDlg::designTestButtonClicked() -{ - if (!mOut) { - DBGA("Set output file first!!!!"); - return; - } - mTFrom = tFromEdit->text().toDouble(); - mTTo = tToEdit->text().toDouble(); - mTStep = tStepEdit->text().toDouble(); - mSFrom = sFromEdit->text().toDouble(); - mSTo = sToEdit->text().toDouble(); - mSStep = sStepEdit->text().toDouble(); - DBGA("Starting batch testing"); - mBatch = true; - mTR = mTFrom; - mSR = mSFrom; - mPlanner->setEnergyType(ENERGY_COMPLIANT); - mPlanner->setStatStream(NULL); - startPlanner(); -} - void -CompliantPlannerDlg::plannerFinished() +ListPlannerDlg::plannerFinished() { PROF_STOP_TIMER(QS_TOTAL); PROF_PRINT(QS_TOTAL); diff --git a/ui/EGPlanner/compliantPlannerDlg.h b/ui/EGPlanner/listPlannerDlg.h similarity index 78% rename from ui/EGPlanner/compliantPlannerDlg.h rename to ui/EGPlanner/listPlannerDlg.h index 8a0b3663cc..aee647a27a 100644 --- a/ui/EGPlanner/compliantPlannerDlg.h +++ b/ui/EGPlanner/listPlannerDlg.h @@ -30,7 +30,7 @@ #include #include -#include "ui_compliantPlannerDlg.h" +#include "ui_listPlannerDlg.h" #include "matvec3D.h" @@ -39,12 +39,12 @@ class Hand; class GraspableBody; class GraspPlanningState; -//! Used for running tests with compliant hands +//! Used for uniformly sampling approach directions around an object /*! This dialog is used for running various tests of the compliant hand solver framework. It is not finished, and currently not in a very well-engineered state. It is in bad need of a re-design. */ -class CompliantPlannerDlg : public QDialog, public Ui::CompliantPlannerDlgUI +class ListPlannerDlg : public QDialog, public Ui::ListPlannerDlgUI { Q_OBJECT private: @@ -88,53 +88,36 @@ class CompliantPlannerDlg : public QDialog, public Ui::CompliantPlannerDlgUI void showResult(); void startPlanner(); public: - CompliantPlannerDlg(Hand *h, GraspableBody *gb, QWidget *parent = 0) : + ListPlannerDlg(Hand *h, GraspableBody *gb, QWidget *parent = 0) : QDialog(parent), mHand(h), mObject(gb) { setupUi(this); QObject::connect(generateButton, SIGNAL(clicked()), this, SLOT(generateButtonClicked())); QObject::connect(testButton, SIGNAL(clicked()), this, SLOT(testButtonClicked())); - QObject::connect(testOneButton, SIGNAL(clicked()), - this, SLOT(testOneButtonClicked())); QObject::connect(nextButton, SIGNAL(clicked()), this, SLOT(nextButtonClicked())); QObject::connect(prevButton, SIGNAL(clicked()), this, SLOT(prevButtonClicked())); QObject::connect(bestButton, SIGNAL(clicked()), this, SLOT(bestButtonClicked())); - QObject::connect(designTestButton, SIGNAL(clicked()), - this, SLOT(designTestButtonClicked())); - QObject::connect(showOneButton, SIGNAL(clicked()), - this, SLOT(showOneButtonClicked())); - QObject::connect(prepareOneButton, SIGNAL(clicked()), - this, SLOT(prepareOneButtonClicked())); - QObject::connect(visualMarkersBox, SIGNAL(clicked()), - this, SLOT(visualMarkersBoxClicked())); - QObject::connect(resetObjectButton, SIGNAL(clicked()), - this, SLOT(resetObjectButtonClicked())); QObject::connect(resultsFileEdit, SIGNAL(editingFinished()), this, SLOT(updateOut())); QObject::connect(resultsBox, SIGNAL(activated(int)), this, SLOT(updateOut())); init(); } - ~CompliantPlannerDlg(); + ~ListPlannerDlg(); public Q_SLOTS: void generateButtonClicked(); void testButtonClicked(); - void testOneButtonClicked(); void nextButtonClicked(); void prevButtonClicked(); void bestButtonClicked(); + void showOne(); void update(); void plannerFinished(); - void showOneButtonClicked(); - void prepareOneButtonClicked(); - void visualMarkersBoxClicked(); - void resetObjectButtonClicked(); - void designTestButtonClicked(); void updateOut(); }; diff --git a/ui/EGPlanner/listPlannerDlg.ui b/ui/EGPlanner/listPlannerDlg.ui new file mode 100644 index 0000000000..23ca20dc84 --- /dev/null +++ b/ui/EGPlanner/listPlannerDlg.ui @@ -0,0 +1,357 @@ + + + ListPlannerDlgUI + + + + 0 + 0 + 404 + 499 + + + + List Planner + + + + + 80 + 80 + 221 + 25 + + + + Generate Approach Directions + + + + + + 160 + 280 + 21 + 25 + + + + > + + + + + + 80 + 280 + 21 + 25 + + + + < + + + + + + 110 + 280 + 41 + 25 + + + + Best + + + + + + 210 + 150 + 131 + 16 + + + + computation type + + + + + + 80 + 190 + 221 + 25 + + + + Test All Approach Dircections + + + + + + 20 + 15 + 31 + 21 + + + + + + + + + + 190 + 286 + 121 + 16 + + + + Energy: 0.00 + + + + + + 190 + 265 + 111 + 16 + + + + Rank: 0/0 + + + + + + 130 + 220 + 101 + 16 + + + + TextLabel + + + + + + 50 + 140 + 141 + 31 + + + + + + + 190 + 310 + 121 + 16 + + + + Iteration: 0 + + + + + + 280 + 450 + 111 + 33 + + + + + 6 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Close + + + + + + + Qt::Horizontal + + + + 131 + 31 + + + + + + + + + + 80 + 370 + 91 + 16 + + + + Output to: + + + + + + 70 + 400 + 201 + 21 + + + + + + + 170 + 361 + 111 + 31 + + + + + + + 60 + 20 + 161 + 16 + + + + Sampling Resolution + + + + + + 10 + 110 + 381 + 20 + + + + Qt::Horizontal + + + + + + 10 + 420 + 381 + 20 + + + + Qt::Horizontal + + + + + + 10 + 330 + 381 + 20 + + + + Qt::Horizontal + + + + + + 10 + 240 + 381 + 20 + + + + Qt::Horizontal + + + + + + 20 + 40 + 141 + 31 + + + + + + + 180 + 50 + 131 + 16 + + + + Sampling Method + + + + + + + okButton + clicked() + ListPlannerDlgUI + accept() + + + 278 + 253 + + + 96 + 254 + + + + + diff --git a/ui/mainWindow.cpp b/ui/mainWindow.cpp index bd3ccc7331..93e926c7e7 100644 --- a/ui/mainWindow.cpp +++ b/ui/mainWindow.cpp @@ -60,7 +60,7 @@ #include "gwsProjDlg.h" #include "plannerdlg.h" #include "eigenGraspDlg.h" -#include "compliantPlannerDlg.h" +#include "listPlannerDlg.h" #include "optimizerDlg.h" #include "gfoDlg.h" #include "contactExaminerDlg.h" @@ -119,6 +119,7 @@ MainWindow::MainWindow(QWidget *parent) this, SLOT(updateCollisionAction(bool))); QObject::connect(mUI->elementBodyPropertiesAction, SIGNAL(activated()), this, SLOT(elementBodyProperties())); + // -- grasp menu, part I QObject::connect(mUI->graspAutoGraspAction, SIGNAL(triggered()), this, SLOT(graspAutoGrasp())); QObject::connect(mUI->graspAuto_OpenAction, SIGNAL(triggered()), this, SLOT(graspAutoOpen())); @@ -133,6 +134,9 @@ MainWindow::MainWindow(QWidget *parent) this, SLOT(graspContactExaminer_activated())); QObject::connect(mUI->graspEigenGrasp_PlannerAction, SIGNAL(triggered()), this, SLOT(eigenGraspPlannerActivated())); + QObject::connect(mUI->graspListPlannerAction, SIGNAL(triggered()), + this, SLOT(graspListPlannerActivated())); + // -- dbase menu QObject::connect(mUI->dbaseGUIAction, SIGNAL(triggered()), this, SLOT(dbaseGUIAction_activated())); @@ -602,7 +606,7 @@ void MainWindow::updateGraspMenu() mUI->graspEigenGrasp_InterfaceAction->setEnabled(handFound); mUI->graspContact_ExaminerAction->setEnabled(bodyFound); mUI->graspEigenGrasp_PlannerAction->setEnabled(handFound); - mUI->graspCompliantPlannerAction->setEnabled(handFound); + mUI->graspListPlannerAction->setEnabled(handFound); #ifdef CGDB_ENABLED mUI->dbaseGUIAction->setEnabled(handFound); mUI->dbasePlannerAction->setEnabled(handFound); @@ -711,14 +715,14 @@ MainWindow::graspForceOptimization() this interface might be redesigned, to use the same frameworks as either the regular planner or the eigengrasp planner */ -void MainWindow::graspCompliantPlanner() +void MainWindow::graspListPlannerActivated() { int gb = mUI->graspedBodyBox->currentItem(); if ( gb < 0 || world->getNumGB() < gb+1 ) { fprintf(stderr,"No object selected\n"); return; } - CompliantPlannerDlg *dlg =new CompliantPlannerDlg(world->getCurrentHand(), + ListPlannerDlg *dlg =new ListPlannerDlg(world->getCurrentHand(), world->getGB(gb), mWindow); dlg->setAttribute(Qt::WA_ShowModal, false); dlg->setAttribute(Qt::WA_DeleteOnClose, true); diff --git a/ui/mainWindow.h b/ui/mainWindow.h index 6eb01d88c5..27018f9965 100644 --- a/ui/mainWindow.h +++ b/ui/mainWindow.h @@ -111,7 +111,7 @@ public Q_SLOTS: void eigenGraspActivated(); void graspContactExaminer_activated(); void eigenGraspPlannerActivated(); - void graspCompliantPlanner(); + void graspListPlannerActivated(); //------------- void dbaseGUIAction_activated(); void dbasePlannerAction_activated(); diff --git a/ui/mainWindow.ui b/ui/mainWindow.ui index d478804cdf..fc193418dc 100644 --- a/ui/mainWindow.ui +++ b/ui/mainWindow.ui @@ -434,6 +434,7 @@ + @@ -743,15 +744,15 @@ Grasp Force Optimization... - + - graspCompliantPlannerAction + graspListPlannerAction - Compliant Planner... + List Planner... - Compliant Planner... + List Planner... From bdd73e40a3b61fe9ffad7274608360e2c1da4c49 Mon Sep 17 00:00:00 2001 From: Jake Varley Date: Thu, 9 Feb 2017 16:05:43 -0500 Subject: [PATCH 2/2] still fixing list planner, grasps occasionally end up inside of the object --- include/EGPlanner/listPlanner.h | 11 +++++++++-- src/EGPlanner/listPlanner.cpp | 35 +++++++++++++++++++++++++++------ src/contact/contact.cpp | 2 +- src/transform.cpp | 2 +- ui/EGPlanner/listPlannerDlg.cpp | 8 ++++---- 5 files changed, 44 insertions(+), 14 deletions(-) diff --git a/include/EGPlanner/listPlanner.h b/include/EGPlanner/listPlanner.h index 6518f3e190..2587a8e0d7 100644 --- a/include/EGPlanner/listPlanner.h +++ b/include/EGPlanner/listPlanner.h @@ -59,7 +59,11 @@ class ListPlanner : public EGPlanner virtual void mainLoop(); //! Pulls the state at the given index from the input list - GraspPlanningState *getState(int index); + GraspPlanningState *getInitialState(int index); + + //! Pulls the state at the given index from the final list + GraspPlanningState *getFinalState(int index); + public: ListPlanner(Hand *h); ~ListPlanner(); @@ -79,7 +83,10 @@ class ListPlanner : public EGPlanner void testState(int index); //! Shows the unprocessed state at the given index in the input list - void showState(int index); + void showInitialState(int index); + + //! Shows the state at the given index in the best list + void showFinalState(int index); //! A hack; shows the state and also does approachToContact as the SearchEnergy would void prepareState(int index); diff --git a/src/EGPlanner/listPlanner.cpp b/src/EGPlanner/listPlanner.cpp index e4cbf3d95b..abe761515b 100644 --- a/src/EGPlanner/listPlanner.cpp +++ b/src/EGPlanner/listPlanner.cpp @@ -80,7 +80,7 @@ ListPlanner::setInput(std::list input) } GraspPlanningState* -ListPlanner::getState(int index) +ListPlanner::getInitialState(int index) { std::list::iterator it = mInputList.begin(); int count = 0; @@ -94,10 +94,25 @@ ListPlanner::getState(int index) return *it; } +GraspPlanningState* +ListPlanner::getFinalState(int index) +{ + std::list::iterator it = mBestList.begin(); + int count = 0; + while (it!=mBestList.end() && count < index) { + count++; it++; + } + if (it==mBestList.end()) { + DBGA("Requested grasp not in list"); + return NULL; + } + return *it; +} + void ListPlanner::testState(int index) { - GraspPlanningState *state = getState(index); + GraspPlanningState *state = getInitialState(index); if (!state) return; bool legal; double energy; mEnergyCalculator->analyzeState(legal, energy, state, false); @@ -105,17 +120,25 @@ ListPlanner::testState(int index) } void -ListPlanner::showState(int index) +ListPlanner::showInitialState(int index) { - GraspPlanningState *state = getState(index); + GraspPlanningState *state = getInitialState(index); if (!state) return; state->execute(); } +void +ListPlanner::showFinalState(int index) +{ + GraspPlanningState *state = getFinalState(index); + if (!state) return; + state->execute(); +} + void ListPlanner::prepareState(int index) { - showState(index); + showInitialState(index); mHand->findInitialContact(200); } @@ -137,7 +160,6 @@ ListPlanner::mainLoop() bool legal; double energy; PRINT_STAT(mOut, mCurrentStep); - (*mPlanningIterator)->execute(); mHand->findInitialContact(200); @@ -156,6 +178,7 @@ ListPlanner::mainLoop() else worstEnergy = mBestList.back()->getEnergy(); if (energy < worstEnergy) { GraspPlanningState *insertState = new GraspPlanningState(*mPlanningIterator); + insertState->saveCurrentHandState(); insertState->setEnergy(energy); insertState->setItNumber(mCurrentStep); DBGP("Solution at step " << mCurrentStep); diff --git a/src/contact/contact.cpp b/src/contact/contact.cpp index 44b2f013a0..811029ac30 100644 --- a/src/contact/contact.cpp +++ b/src/contact/contact.cpp @@ -389,7 +389,7 @@ Contact::localToWorldWrenchBlockMatrix(const std::list &contacts) */ bool Contact::preventsMotion(const transf &motion) const { - bool result = ((motion * loc) - loc).dot(normal) > MACHINE_ZERO; + bool result = ((motion * loc) - loc).dot(normal) < -MACHINE_ZERO; return result; } diff --git a/src/transform.cpp b/src/transform.cpp index f5751c0fd7..86e2702bf0 100644 --- a/src/transform.cpp +++ b/src/transform.cpp @@ -200,7 +200,7 @@ The jacobian is returned is in column-major format. void transf::jacobian(double jac[]) { - mat3 m = affine(); + mat3 m = affine().transpose(); vec3 p = translation(); // first 3 columns of jac diff --git a/ui/EGPlanner/listPlannerDlg.cpp b/ui/EGPlanner/listPlannerDlg.cpp index b1eb1ea15f..a8fec84c4e 100644 --- a/ui/EGPlanner/listPlannerDlg.cpp +++ b/ui/EGPlanner/listPlannerDlg.cpp @@ -326,7 +326,7 @@ ListPlannerDlg::showResult() if ( d!=0 ){ const GraspPlanningState *s = mPlanner->getGrasp(mBestGraspNum); - mPlanner->showState(mBestGraspNum); + mPlanner->showFinalState(mBestGraspNum); rank = mBestGraspNum+1; size = d; energy = s->getEnergy(); @@ -340,7 +340,7 @@ ListPlannerDlg::showResult() n1.setNum(energy,'f',3); energyLabel->setText("Energy: " + n1); n1.setNum(iteration); - showOne(); + showOne(); iterationLabel->setText("Iteration: " + n1); } @@ -357,8 +357,8 @@ ListPlannerDlg::showOne() DBGA("Wrong test number selected"); return; } - DBGA("Testing pre-grasp #" << num); - mPlanner->showState(num); + DBGA("Showing Planned Grasp #" << num); + mPlanner->showFinalState(num); } void