diff --git a/CMakeLists.txt b/CMakeLists.txt index ffc2d1f295..09950a9657 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -526,7 +526,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 @@ -544,7 +544,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 @@ -562,7 +562,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 @@ -572,7 +572,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/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 bd40fa8d0e..abe761515b 100644 --- a/src/EGPlanner/listPlanner.cpp +++ b/src/EGPlanner/listPlanner.cpp @@ -34,151 +34,179 @@ //this should be a parameter of the EGPlanner //iros09 -#define BEST_LIST_SIZE 2000 +#define BEST_LIST_SIZE 20000 ListPlanner::ListPlanner(Hand *h) { - mHand = h; - init(); - mEnergyCalculator = SearchEnergy::getSearchEnergy(ENERGY_CONTACT); - mEnergyCalculator->disableRendering(false); + mHand = h; + init(); + mEnergyCalculator = SearchEnergy::getSearchEnergy(ENERGY_CONTACT); + mEnergyCalculator->disableRendering(false); } /*! The destructor also eliminates the list of input grasps, therefore - care must be excercised when passing grasps to this class. + care must be excercised when passing grasps to this class. */ ListPlanner::~ListPlanner() { - while (!mInputList.empty()) { - delete mInputList.back(); - mInputList.pop_back(); - } + while (!mInputList.empty()){ + delete mInputList.back(); + mInputList.pop_back(); + } } -void -ListPlanner::resetParameters() { - EGPlanner::resetParameters(); - mPlanningIterator = mInputList.begin(); - if (mCurrentState) { delete mCurrentState; } - mCurrentState = new GraspPlanningState(*mPlanningIterator); +void +ListPlanner::resetParameters(){ + EGPlanner::resetParameters(); + mPlanningIterator = mInputList.begin(); + if(mCurrentState) delete mCurrentState; + mCurrentState = new GraspPlanningState(*mPlanningIterator); } -void -ListPlanner::setInput(std::list input) +void +ListPlanner::setInput(std::list input) { - if (isActive()) { - DBGA("Can not change input while planner is running"); - return; - } - while (!mInputList.empty()) { - delete mInputList.back(); - mInputList.pop_back(); - } - mInputList = input; - mMaxSteps = input.size(); - invalidateReset(); + if (isActive()) { + DBGA("Can not change input while planner is running"); + return; + } + while (!mInputList.empty()){ + delete mInputList.back(); + mInputList.pop_back(); + } + mInputList = input; + mMaxSteps = input.size(); + invalidateReset(); } -GraspPlanningState * -ListPlanner::getState(int index) +GraspPlanningState* +ListPlanner::getInitialState(int index) { - std::list::iterator it = mInputList.begin(); - int count = 0; - while (it != mInputList.end() && count < index) { - count++; it++; - } - if (it == mInputList.end()) { - DBGA("Requested grasp not in list"); - return NULL; - } - return *it; + std::list::iterator it = mInputList.begin(); + int count = 0; + while (it!=mInputList.end() && count < index) { + count++; it++; + } + if (it==mInputList.end()) { + DBGA("Requested grasp not in list"); + return NULL; + } + return *it; } -void +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); - if (!state) { return; } - bool legal; double energy; - mEnergyCalculator->analyzeState(legal, energy, state, false); - DBGA("Energy: " << energy); + GraspPlanningState *state = getInitialState(index); + if (!state) return; + bool legal; double energy; + mEnergyCalculator->analyzeState(legal, energy, state, false); + DBGA("Energy: " << energy); } void -ListPlanner::showState(int index) +ListPlanner::showInitialState(int index) { - GraspPlanningState *state = getState(index); - if (!state) { return; } - state->execute(); + 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); - mHand->findInitialContact(200); + showInitialState(index); + mHand->findInitialContact(200); } /*! The main planning function. Simply takes the next input grasp from the list, - tries it and then places is in the output depending on the quality. + tries it and then places is in the output depending on the quality. */ -void +void ListPlanner::mainLoop() { - //check if we are done - if (mPlanningIterator == mInputList.end()) { - mCurrentStep = mMaxSteps + 1; - return; - } - - //analyze the current state - //we don't allow it to leave the hand in the analysis posture - //so that after dynamics object gets put back - bool legal; double energy; - PRINT_STAT(mOut, mCurrentStep); - mEnergyCalculator->analyzeState(legal, energy, *mPlanningIterator, true); - - //for rendering purposes; will see later if it's needed - mCurrentState->copyFrom(*mPlanningIterator); - mCurrentState->setLegal(legal); - - //put a copy of the result in list if it's legal and there's room or it's - //better than the worst solution so far - //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; } - else { worstEnergy = mBestList.back()->getEnergy(); } - if (energy < worstEnergy) { - GraspPlanningState *insertState = new GraspPlanningState(*mPlanningIterator); - insertState->setEnergy(energy); - insertState->setItNumber(mCurrentStep); - DBGP("Solution at step " << mCurrentStep); - mBestList.push_back(insertState); - mBestList.sort(GraspPlanningState::compareStates); - while ((int)mBestList.size() > BEST_LIST_SIZE) { - delete(mBestList.back()); - mBestList.pop_back(); - } - } - } - - //advance the planning iterator - mPlanningIterator++; - mCurrentStep++; - Q_EMIT update(); - PRINT_STAT(mOut, std::endl); + //check if we are done + if (mPlanningIterator==mInputList.end()) { + mCurrentStep = mMaxSteps+1; + return; + } + + //analyze the current state + //we don't allow it to leave the hand in the analysis posture + //so that after dynamics object gets put back + bool legal; double energy; + PRINT_STAT(mOut, mCurrentStep); + + (*mPlanningIterator)->execute(); + mHand->findInitialContact(200); + + mEnergyCalculator->analyzeState(legal, energy, *mPlanningIterator, false); + + //for rendering purposes; will see later if it's needed + mCurrentState->copyFrom(*mPlanningIterator); + mCurrentState->setLegal(legal); + + //put a copy of the result in list if it's legal and there's room or it's + //better than the worst solution so far + //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; + 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); + mBestList.push_back(insertState); + mBestList.sort(GraspPlanningState::compareStates); + while ((int)mBestList.size() > BEST_LIST_SIZE) { + delete(mBestList.back()); + mBestList.pop_back(); + } + } + } + + //advance the planning iterator + mPlanningIterator++; + mCurrentStep++; + Q_EMIT update(); + PRINT_STAT(mOut, std::endl); } -void +void ListPlanner::showVisualMarkers(bool show) { - std::list::iterator it; - for (it = mInputList.begin(); it != mInputList.end(); it++) { - if (show) { - (*it)->showVisualMarker(); - } else { - (*it) ->hideVisualMarker(); - } - } + std::list::iterator it; + for (it = mInputList.begin(); it!=mInputList.end(); it++) { + if (show) { + (*it)->showVisualMarker(); + } else { + (*it) ->hideVisualMarker(); + } + } } diff --git a/ui/EGPlanner/compliantPlannerDlg.cpp b/ui/EGPlanner/compliantPlannerDlg.cpp deleted file mode 100644 index 699de75528..0000000000 --- a/ui/EGPlanner/compliantPlannerDlg.cpp +++ /dev/null @@ -1,518 +0,0 @@ -//###################################################################### -// -// GraspIt! -// Copyright (C) 2002-2009 Columbia University in the City of New York. -// All rights reserved. -// -// GraspIt! is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// GraspIt! is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with GraspIt!. If not, see . -// -// Author(s): Matei T. Ciocarlie -// -// $Id: compliantPlannerDlg.cpp,v 1.14 2009/07/02 21:04:05 cmatei Exp $ -// -//###################################################################### - -#include "compliantPlannerDlg.h" - -#include - -#include - -#include - -//#define GRASPITDBG -#include "debug.h" - -//#define PROF_ENABLED -#include "profiling.h" - -PROF_DECLARE(QS_TOTAL); - -//for the bounding box and drawing forces -#include "graspitCore.h" -#include "ivmgr.h" - -//for debug -#include "world.h" - -#include "robot.h" -#include "body.h" -#include "grasp.h" -#include "listPlanner.h" -#include "searchState.h" - -void -CompliantPlannerDlg::init() -{ - mPlanner = new ListPlanner(mHand); - energyTypeBox->insertItem("Quasistatic"); - energyTypeBox->insertItem("Dynamic"); - mPlanner->setRenderType(RENDER_ALWAYS); - mPlanner->setStatStream(&std::cerr); - mOut = NULL; - mHand->getGrasp()->setObjectNoUpdate(mObject); - mObjectRefTran = mObject->getTran(); - QObject::connect(mPlanner, SIGNAL(update()), this, SLOT(update())); - QObject::connect(mPlanner, SIGNAL(finished()), this, SLOT(plannerFinished())); - mNumCandidates = 0; - 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"); - 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() -{ - if (mOut) { - mOut->close(); - delete mOut; - } - delete mPlanner; -} - -void -CompliantPlannerDlg::addCartesianSamples(const GraspPlanningState &seed, - std::list *sampling, - int samples, double x, double y, double z) -{ - //redundant, but easier... - double a = seed.readPosition()->getParameter("a"); - double b = seed.readPosition()->getParameter("b"); - //double c = seed.readPosition()->getParameter("c"); - //compute angular values - //from HandObjectStateImpl: - //x = a * cos(beta) * cos(gamma); - //y = b * cos(beta) * sin(gamma); - //z = c * sin(beta); - double beta = asin(z / sqrt(x * x + y * y + z * z)); - double gamma = atan2(y / b, x / a); - DBGP("x: " << x << "; y: " << y << "; z: " << z); - DBGP("gamma: " << gamma << "; beta: " << beta); - //sample roll angle as well - for (int m = 0; m < samples; m++) { - //only sample from 0 to almost PI, as the HH is symmetric - double tau = M_PI * ((double)m) / samples; - GraspPlanningState *newState = new GraspPlanningState(&seed); - newState->getPosition()->getVariable("tau")->setValue(tau); - newState->getPosition()->getVariable("gamma")->setValue(gamma); - newState->getPosition()->getVariable("beta")->setValue(beta); - sampling->push_back(newState); - } -} - -/*! Samples an ellipsoid by sampling uniformly a grid with the same aspect - 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, - std::list *sampling, - int samples) -{ - double a = seed.readPosition()->getParameter("a"); - double aRes = 2.0 * a / samples; - double b = seed.readPosition()->getParameter("b"); - double bRes = 2.0 * b / samples; - double c = seed.readPosition()->getParameter("c"); - double cRes = 2.0 * c / samples; - DBGP("a: " << a << "; b: " << b << "; c: " << c); - - for (double i = 0.5; i < samples; i += 1.0) { - for (double j = 0.5; j < samples; j += 1.0) { - addCartesianSamples(seed, sampling, samples, a, -b + i * bRes, -c + j * cRes); - addCartesianSamples(seed, sampling, samples, -a, -b + i * bRes, -c + j * cRes); - addCartesianSamples(seed, sampling, samples, -a + i * aRes, b , -c + j * cRes); - addCartesianSamples(seed, sampling, samples, -a + i * aRes, -b , -c + j * cRes); - addCartesianSamples(seed, sampling, samples, -a + i * aRes, -b + j * bRes , c); - addCartesianSamples(seed, sampling, samples, -a + i * aRes, -b + j * bRes , -c); - } - } -} - -void -CompliantPlannerDlg::generateButtonClicked() -{ - int resolution = resolutionEdit->text().toInt(); - if (resolution < 1) { - DBGA("Resolution must be at least 1"); - return; - } - //get object bbox dimensions - SoGetBoundingBoxAction *bba = - new SoGetBoundingBoxAction(graspitCore->getIVmgr()->getViewer()->getViewportRegion()); - bba->apply(mObject->getIVGeomRoot()); - SbVec3f bbmin, bbmax; - bba->getBoundingBox().getBounds(bbmin, bbmax); - delete bba; - 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); - - mPlanner->resetPlanner(); - if (visualMarkersBox->isChecked()) { - visualMarkersBoxClicked(); - } - update(); -} - -void -CompliantPlannerDlg::ellipsoidSampling(double a, double b, double c, double resolution) -{ - //generate a list of grasps by sampling an ellipsoid around the object - GraspPlanningState seed(mHand); - seed.setObject(mObject); - //todo: should use bbox center as reference frame, not object origin - //which could be anything - seed.setRefTran(mObject->getTran(), false); - seed.setPostureType(POSE_DOF, false); - seed.setPositionType(SPACE_ELLIPSOID, false); - seed.reset(); - - //set ellipsoid parameters - seed.getPosition()->setParameter("a", a); - seed.getPosition()->setParameter("b", b); - seed.getPosition()->setParameter("c", c); - //we don't want to sample distance - seed.getPosition()->getVariable("dist")->setValue(0.0); - seed.getPosition()->getVariable("dist")->setFixed(true); - //create a list of earch states that sample the variables of the seed - std::list sampling; - //uniform sampling of variables. Creates a horrible sampling. - //createPositionSpaceSampling(seed, &sampling, resolution); - //grid based sampling. Does somewhat better. - gridEllipsoidSampling(seed, &sampling, resolution); - DBGA("Sampled " << sampling.size() << " states."); - mNumCandidates = sampling.size(); - //pass the list to the planner which will take ownership of it and destroy it - mPlanner->setInput(sampling); -} - -void -CompliantPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z, - double sz1, double sz2, vec3 tln, double res, - std::list *sampling) -{ - mat3 R; - R.col(0) = x; - R.col(1) = y; - R.col(2) = z; - int rotSamples = 2; - - double m1 = (2.0 * sz1 - floor(2.0 * sz1 / res) * res) / 2.0; - while (m1 < 2 * sz1) { - double m2 = (2.0 * sz2 - floor(2.0 * sz2 / res) * res) / 2.0; - while (m2 < 2 * sz2) { - vec3 myTln(tln); - myTln = myTln + (m1 - sz1) * y; - myTln = myTln + (m2 - sz2) * z; - transf tr(R, myTln); - for (int rot = 0; rot < rotSamples; rot++) { - double angle = M_PI * ((double)rot) / rotSamples; - Eigen::AngleAxisd aa = Eigen::AngleAxisd(angle, vec3(1, 0, 0)); - Quaternion q(aa); - transf rotTran(q, vec3(0, 0, 0)); - tr = tr % rotTran; - GraspPlanningState *seed = new GraspPlanningState(mHand); - seed->setObject(mObject); - seed->setRefTran(mObject->getTran(), false); - seed->setPostureType(POSE_DOF, false); - seed->setPositionType(SPACE_COMPLETE, false); - seed->reset(); - seed->getPosition()->setTran(tr); - sampling->push_back(seed); - } - m2 += res; - } - m1 += res; - } -} - -void -CompliantPlannerDlg::boxSampling(double a, double b, double c, double res) -{ - std::list sampling; - res = 30; - sampleFace(vec3(0, 1, 0), vec3(-1, 0, 0), vec3(0, 0, 1) , a, c, vec3(0, -b, 0), res, &sampling); - sampleFace(vec3(0, -1, 0), vec3(1, 0, 0), vec3(0, 0, 1) , a, c, vec3(0, b, 0), res, &sampling); - - sampleFace(vec3(0, 0, 1), vec3(0, 1, 0), vec3(-1, 0, 0) , b, a, vec3(0, 0, -c), res, &sampling); - sampleFace(vec3(0, 0, -1), vec3(0, 1, 0), vec3(1, 0, 0) , b, a, vec3(0, 0, c), res, &sampling); - - sampleFace(vec3(1, 0, 0), vec3(0, 1, 0), vec3(0, 0, 1) , b, c, vec3(-a, 0, 0), res, &sampling); - sampleFace(vec3(-1, 0, 0), vec3(0, -1, 0), vec3(0, 0, 1) , b, c, vec3(a, 0, 0), res, &sampling); - - DBGA("Sampled " << sampling.size() << " states."); - mNumCandidates = sampling.size(); - mPlanner->setInput(sampling); -} - -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() -{ - mPlanner->startPlanner(); -} - -void -CompliantPlannerDlg::testButtonClicked() -{ - if (energyTypeBox->currentText() == "Quasistatic") { - mPlanner->setEnergyType(ENERGY_COMPLIANT); - } else if (energyTypeBox->currentText() == "Dynamic") { - mPlanner->setEnergyType(ENERGY_DYNAMIC); - } else { - assert(0); - } - - if (mPlanner->isActive()) { - DBGA("Pause:"); - mPlanner->pausePlanner(); - } else { - PROF_RESET(QS_TOTAL); - PROF_START_TIMER(QS_TOTAL); - mBatch = false; - if (mOut) { - mPlanner->setStatStream(mOut); - } else { - mPlanner->setStatStream(&std::cerr); - } - startPlanner(); - } -} - -void -CompliantPlannerDlg::update() -{ - QString n1, n2; - n1.setNum(mPlanner->getCurrentStep()); - n2.setNum(mNumCandidates); - currentLabel->setText(n1 + "/" + n2); -} - -void -CompliantPlannerDlg::showResult() -{ - int d = mPlanner->getListSize(); - int rank, size, iteration; double energy; - - if (d == 0) { - mBestGraspNum = rank = size = iteration = energy = 0; - } else if (mBestGraspNum < 0) { - mBestGraspNum = 0; - } else if (mBestGraspNum >= d) { - mBestGraspNum = d - 1; - } - - if (d != 0) { - const GraspPlanningState *s = mPlanner->getGrasp(mBestGraspNum); - rank = mBestGraspNum + 1; - size = d; - energy = s->getEnergy(); - iteration = s->getItNumber(); - } - - QString n1, n2; - n1.setNum(rank); - n2.setNum(size); - rankLabel->setText("Rank: " + n1 + "/" + n2); - n1.setNum(energy, 'f', 3); - energyLabel->setText("Energy: " + n1); - n1.setNum(iteration); - testOneEdit->setText(n1); - showOneButtonClicked(); - iterationLabel->setText("Iteration: " + n1); - -} - -void -CompliantPlannerDlg::prevButtonClicked() -{ - mBestGraspNum--; - showResult(); -} -void -CompliantPlannerDlg::nextButtonClicked() -{ - mBestGraspNum++; - showResult(); -} -void -CompliantPlannerDlg::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() -{ - if (mOut) { - mOut->close(); - delete mOut; mOut = NULL; - } - if (resultsBox->currentIndex() == 0) { - DBGA("Output to stderr"); - return; - } - QString filename("data\\" + resultsFileEdit->text()); - mOut = new std::fstream(filename.latin1(), std::fstream::out | std::fstream::app); - if (mOut->fail()) { - DBGA("Failed to open file " << filename.latin1()); - delete mOut; mOut = NULL; - resultsBox->setCurrentItem(0); - } else { - DBGA("Output file opened: " << filename.latin1()); - } -} - -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() -{ - PROF_STOP_TIMER(QS_TOTAL); - PROF_PRINT(QS_TOTAL); - if (!mBatch) { return; } - //write results to file - DBGA("Test done: " << mTR << " and " << mSR); - if (mOut) { - *mOut << mTR << " " << mSR; - *mOut << std::endl; - } - mSR += mSStep; - if (mSR > mSTo + 1.0e-2) { - mSR = mSFrom; - mTR += mTStep; - if (mTR > mTTo + 1.0e-2) { - mBatch = false; - return; - } - } - mPlanner->resetPlanner(); - startPlanner(); -} diff --git a/ui/EGPlanner/compliantPlannerDlg.h b/ui/EGPlanner/compliantPlannerDlg.h deleted file mode 100644 index 06e8c8118f..0000000000 --- a/ui/EGPlanner/compliantPlannerDlg.h +++ /dev/null @@ -1,141 +0,0 @@ -//###################################################################### -// -// GraspIt! -// Copyright (C) 2002-2009 Columbia University in the City of New York. -// All rights reserved. -// -// GraspIt! is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// GraspIt! is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// You should have received a copy of the GNU General Public License -// along with GraspIt!. If not, see . -// -// Author(s): Matei T. Ciocarlie -// -// $Id: compliantPlannerDlg.h,v 1.8 2009/05/07 19:59:21 cmatei Exp $ -// -//###################################################################### - -#ifndef _compliantplannerdlg_h_ -#define _compliantplannerdlg_h_ - -#include -#include -#include - -#include "ui_compliantPlannerDlg.h" - -#include "matvec3D.h" - -class ListPlanner; -class Hand; -class GraspableBody; -class GraspPlanningState; - -//! Used for running tests with compliant hands -/*! 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 -{ - Q_OBJECT - private: - //! The planner the dialog is using - ListPlanner *mPlanner; - //! The hand we are planning on - Hand *mHand; - //! The object we are planning on - GraspableBody *mObject; - //! The number of candidate grasps that were sampled to be tried out - int mNumCandidates; - //! The current grasp displayed from the best list - int mBestGraspNum; - //! The reference position of the target object - transf mObjectRefTran; - //! The file (if any) that full results are written to - std::fstream *mOut; - - //! For batch testing for hand design - double mTFrom, mTTo, mTStep, mTR; - double mSFrom, mSTo, mSStep, mSR; - bool mBatch; - - //! - void ellipsoidSampling(double a, double b, double c, double resolution); - //! - void boxSampling(double a, double b, double c, double res); - //! - void sampleFace(vec3 x, vec3 y, vec3 z, double sz1, double sz2, vec3 tln, double res, - std::list *sampling); - - //! Samples an ellipsoid using a grid-based method to generate pre-grasps. - void gridEllipsoidSampling(const GraspPlanningState &seed, - std::list *sampling, int samples); - //! Helper function for the above - void addCartesianSamples(const GraspPlanningState &seed, std::list *sampling, - int samples, double x, double y, double z); - //! Initialization stuff - void init(); - //! Display one grasp from the result list - void showResult(); - void startPlanner(); - public: - CompliantPlannerDlg(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(); - - public Q_SLOTS: - void generateButtonClicked(); - void testButtonClicked(); - void testOneButtonClicked(); - void nextButtonClicked(); - void prevButtonClicked(); - void bestButtonClicked(); - void update(); - void plannerFinished(); - void showOneButtonClicked(); - void prepareOneButtonClicked(); - void visualMarkersBoxClicked(); - void resetObjectButtonClicked(); - void designTestButtonClicked(); - void updateOut(); -}; - -#endif 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/listPlannerDlg.cpp b/ui/EGPlanner/listPlannerDlg.cpp new file mode 100644 index 0000000000..a8fec84c4e --- /dev/null +++ b/ui/EGPlanner/listPlannerDlg.cpp @@ -0,0 +1,429 @@ +//###################################################################### +// +// GraspIt! +// Copyright (C) 2002-2009 Columbia University in the City of New York. +// All rights reserved. +// +// GraspIt! is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// GraspIt! is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with GraspIt!. If not, see . +// +// Author(s): Matei T. Ciocarlie +// +// $Id: compliantPlannerDlg.cpp,v 1.14 2009/07/02 21:04:05 cmatei Exp $ +// +//###################################################################### + +#include "listPlannerDlg.h" + +#include + +#include + +#include + +//#define GRASPITDBG +#include "debug.h" + +//#define PROF_ENABLED +#include "profiling.h" + +PROF_DECLARE(QS_TOTAL); + +//for the bounding box and drawing forces +#include "graspitCore.h" +#include "ivmgr.h" + +//for debug +#include "world.h" + +#include "robot.h" +#include "body.h" +#include "grasp.h" +#include "listPlanner.h" +#include "searchState.h" + +void +ListPlannerDlg::init() +{ + mPlanner = new ListPlanner(mHand); + energyTypeBox->insertItem("AutoGrasp"); + energyTypeBox->insertItem("Quasistatic"); + samplingTypeBox->insertItem("Ellipsoid"); + samplingTypeBox->insertItem("Grid"); + mPlanner->setRenderType(RENDER_ALWAYS); + mPlanner->setStatStream(&std::cerr); + mOut = NULL; + mHand->getGrasp()->setObjectNoUpdate(mObject); + mObjectRefTran = mObject->getTran(); + QObject::connect(mPlanner, SIGNAL(update()), this, SLOT(update())); + QObject::connect(mPlanner, SIGNAL(finished()), this, SLOT(plannerFinished())); + mNumCandidates = 0; + currentLabel->setText("0/0"); + QIntValidator* val = new QIntValidator(2, 99, this); + resolutionEdit->setValidator(val); + resolutionEdit->setText("3");; + resultsBox->insertItem("console"); + resultsBox->insertItem("file:"); + resultsFileEdit->setText("comp_plan.txt"); + +} + +ListPlannerDlg::~ListPlannerDlg() +{ + if (mOut) { + mOut->close(); + delete mOut; + } + delete mPlanner; +} + +void +ListPlannerDlg::addCartesianSamples(const GraspPlanningState &seed, + std::list *sampling, + int samples, double x, double y, double z) +{ + //redundant, but easier... + double a = seed.readPosition()->getParameter("a"); + double b = seed.readPosition()->getParameter("b"); + //double c = seed.readPosition()->getParameter("c"); + //compute angular values + //from HandObjectStateImpl: + //x = a * cos(beta) * cos(gamma); + //y = b * cos(beta) * sin(gamma); + //z = c * sin(beta); + double beta = asin(z / sqrt(x*x + y*y + z*z)); + double gamma = atan2(y/b, x/a); + DBGP("x: " << x << "; y: " << y <<"; z: " << z); + DBGP("gamma: " << gamma << "; beta: " << beta); + //sample roll angle as well + for (int m=0; mgetPosition()->getVariable("tau")->setValue(tau); + newState->getPosition()->getVariable("gamma")->setValue(gamma); + newState->getPosition()->getVariable("beta")->setValue(beta); + sampling->push_back(newState); + } +} + +/*! Samples an ellipsoid by sampling uniformly a grid with the same aspect + ratio and projecting the resulting points on the ellipsoid. Not ideal, + but at least much better then sampling angular variables directly */ +void +ListPlannerDlg::gridEllipsoidSampling(const GraspPlanningState &seed, + std::list *sampling, + int samples) +{ + double a = seed.readPosition()->getParameter("a"); + double aRes = 2.0 * a / samples; + double b = seed.readPosition()->getParameter("b"); + double bRes = 2.0 * b / samples; + double c = seed.readPosition()->getParameter("c"); + double cRes = 2.0 * c / samples; + DBGP("a: " << a << "; b: " << b <<"; c: " << c); + + for (double i=0.5; itext().toInt(); + if (resolution < 1) { + DBGA("Resolution must be at least 1"); + return; + } + //get object bbox dimensions + SoGetBoundingBoxAction *bba = + new SoGetBoundingBoxAction(graspitCore->getIVmgr()->getViewer()->getViewportRegion()); + bba->apply(mObject->getIVGeomRoot()); + SbVec3f bbmin,bbmax; + bba->getBoundingBox().getBounds(bbmin,bbmax); + delete bba; + double a = 0.5*(bbmax[0] - bbmin[0]); + double b = 0.5*(bbmax[1] - bbmin[1]); + double c = 0.5*(bbmax[2] - bbmin[2]); + + 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(); + + update(); +} + +void +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); + seed.setObject(mObject); + //todo: should use bbox center as reference frame, not object origin + //which could be anything + seed.setRefTran(mObject->getTran(), false); + seed.setPostureType(POSE_DOF, false); + seed.setPositionType(SPACE_ELLIPSOID, false); + seed.reset(); + + //set ellipsoid parameters + seed.getPosition()->setParameter("a", a); + seed.getPosition()->setParameter("b", b); + seed.getPosition()->setParameter("c", c); + //we don't want to sample distance + seed.getPosition()->getVariable("dist")->setValue(0.0); + seed.getPosition()->getVariable("dist")->setFixed(true); + //create a list of earch states that sample the variables of the seed + std::list sampling; + //uniform sampling of variables. Creates a horrible sampling. + //createPositionSpaceSampling(seed, &sampling, resolution); + //grid based sampling. Does somewhat better. + gridEllipsoidSampling(seed, &sampling, resolution); + DBGA("Sampled " << sampling.size() << " states."); + mNumCandidates = sampling.size(); + //pass the list to the planner which will take ownership of it and destroy it + mPlanner->setInput(sampling); +} + +void +ListPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z, + double sz1, double sz2, vec3 tln, double res, + std::list *sampling) +{ + mat3 R; + R.col(0) = x; + R.col(0) = y; + R.col(0) = z; + int rotSamples=2; + + double m1 = (2.0*sz1 - floor(2.0*sz1 / res) * res)/2.0; + while (m1 < 2*sz1){ + double m2 = (2.0*sz2 - floor(2.0*sz2 / res) * res)/2.0; + while (m2 < 2*sz2) { + vec3 myTln(tln); + myTln = myTln + (m1 - sz1)* y; + myTln = myTln + (m2 - sz2)* z; + transf tr(R, myTln); + for(int rot=0; rot < rotSamples; rot++) { + double angle = M_PI * ((double)rot) / rotSamples; + transf rotTran = transf::AXIS_ANGLE_ROTATION(angle, vec3(1,0,0)); + tr = tr % rotTran; + GraspPlanningState* seed = new GraspPlanningState(mHand); + seed->setObject(mObject); + seed->setRefTran(mObject->getTran(), false); + seed->setPostureType(POSE_DOF, false); + seed->setPositionType(SPACE_COMPLETE, false); + seed->reset(); + seed->getPosition()->setTran(tr); + sampling->push_back(seed); + } + m2+=res; + } + m1 += res; + } +} + +void +ListPlannerDlg::boxSampling(double a, double b, double c, double res) +{ + std::list sampling; + res = 30; + sampleFace( vec3(0, 1,0), vec3(-1,0,0), vec3(0,0,1) , a, c, vec3(0,-b,0), res, &sampling); + sampleFace( vec3(0,-1,0), vec3( 1,0,0), vec3(0,0,1) , a, c, vec3(0, b,0), res, &sampling); + + sampleFace( vec3(0,0, 1), vec3(0,1,0), vec3(-1,0,0) , b, a, vec3(0,0,-c), res, &sampling); + sampleFace( vec3(0,0,-1), vec3(0,1,0), vec3( 1,0,0) , b, a, vec3(0,0, c), res, &sampling); + + sampleFace( vec3( 1,0,0), vec3(0, 1,0), vec3(0,0,1) , b, c, vec3(-a,0,0), res, &sampling); + sampleFace( vec3(-1,0,0), vec3(0,-1,0), vec3(0,0,1) , b, c, vec3( a,0,0), res, &sampling); + + DBGA("Sampled " << sampling.size() << " states."); + mNumCandidates = sampling.size(); + mPlanner->setInput(sampling); +} + +void +ListPlannerDlg::startPlanner() +{ + mPlanner->startPlanner(); +} + +void +ListPlannerDlg::testButtonClicked() +{ + if (energyTypeBox->currentText() == "Quasistatic") { + mPlanner->setEnergyType(ENERGY_COMPLIANT); + } else if (energyTypeBox->currentText() == "AutoGrasp") { + mPlanner->setEnergyType(ENERGY_STRICT_AUTOGRASP); + } else { + assert(0); + } + + if (mPlanner->isActive()) { + DBGA("Pause:"); + mPlanner->pausePlanner(); + } else { + PROF_RESET(QS_TOTAL); + PROF_START_TIMER(QS_TOTAL); + mBatch = false; + if (mOut) { + mPlanner->setStatStream(mOut); + } else { + mPlanner->setStatStream(&std::cerr); + } + startPlanner(); + } +} + +void +ListPlannerDlg::update() +{ + QString n1,n2; + n1.setNum(mPlanner->getCurrentStep()); + n2.setNum(mNumCandidates); + currentLabel->setText(n1 + "/" + n2); +} + +void +ListPlannerDlg::showResult() +{ + int d = mPlanner->getListSize(); + int rank, size, iteration; double energy; + + if (d==0) { + mBestGraspNum = rank = size = iteration = energy = 0; + } else if (mBestGraspNum < 0){ + mBestGraspNum = 0; + } else if ( mBestGraspNum >= d) { + mBestGraspNum = d-1; + } + + if ( d!=0 ){ + const GraspPlanningState *s = mPlanner->getGrasp(mBestGraspNum); + mPlanner->showFinalState(mBestGraspNum); + rank = mBestGraspNum+1; + size = d; + energy = s->getEnergy(); + iteration = s->getItNumber(); + } + + QString n1,n2; + n1.setNum(rank); + n2.setNum(size); + rankLabel->setText("Rank: " + n1 + "/" + n2); + n1.setNum(energy,'f',3); + energyLabel->setText("Energy: " + n1); + n1.setNum(iteration); + showOne(); + iterationLabel->setText("Iteration: " + n1); + +} + +void +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("Showing Planned Grasp #" << num); + mPlanner->showFinalState(num); +} + +void +ListPlannerDlg::prevButtonClicked() +{ + mBestGraspNum--; + showResult(); +} +void +ListPlannerDlg::nextButtonClicked() +{ + mBestGraspNum++; + showResult(); +} +void +ListPlannerDlg::bestButtonClicked() +{ + mBestGraspNum=0; + showResult(); +} + + +void +ListPlannerDlg::updateOut() +{ + if (mOut) { + mOut->close(); + delete mOut; mOut = NULL; + } + if (resultsBox->currentIndex()==0) { + DBGA("Output to stderr"); + return; + } + QString filename("data\\" + resultsFileEdit->text()); + mOut = new std::fstream(filename.latin1(), std::fstream::out | std::fstream::app); + if (mOut->fail()) { + DBGA("Failed to open file " << filename.latin1()); + delete mOut; mOut = NULL; + resultsBox->setCurrentItem(0); + } else { + DBGA("Output file opened: " << filename.latin1()); + } +} + +void +ListPlannerDlg::plannerFinished() +{ + PROF_STOP_TIMER(QS_TOTAL); + PROF_PRINT(QS_TOTAL); + if (!mBatch) return; + //write results to file + DBGA("Test done: " << mTR << " and " << mSR); + if (mOut) { + *mOut << mTR << " " << mSR; + *mOut << std::endl; + } + mSR += mSStep; + if (mSR > mSTo + 1.0e-2) { + mSR = mSFrom; + mTR += mTStep; + if (mTR > mTTo + 1.0e-2) { + mBatch = false; + return; + } + } + mPlanner->resetPlanner(); + startPlanner(); +} diff --git a/ui/EGPlanner/listPlannerDlg.h b/ui/EGPlanner/listPlannerDlg.h new file mode 100644 index 0000000000..aee647a27a --- /dev/null +++ b/ui/EGPlanner/listPlannerDlg.h @@ -0,0 +1,124 @@ +//###################################################################### +// +// GraspIt! +// Copyright (C) 2002-2009 Columbia University in the City of New York. +// All rights reserved. +// +// GraspIt! is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// GraspIt! is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with GraspIt!. If not, see . +// +// Author(s): Matei T. Ciocarlie +// +// $Id: compliantPlannerDlg.h,v 1.8 2009/05/07 19:59:21 cmatei Exp $ +// +//###################################################################### + +#ifndef _compliantplannerdlg_h_ +#define _compliantplannerdlg_h_ + +#include +#include +#include + +#include "ui_listPlannerDlg.h" + +#include "matvec3D.h" + +class ListPlanner; +class Hand; +class GraspableBody; +class GraspPlanningState; + +//! 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 ListPlannerDlg : public QDialog, public Ui::ListPlannerDlgUI +{ + Q_OBJECT +private: + //! The planner the dialog is using + ListPlanner *mPlanner; + //! The hand we are planning on + Hand *mHand; + //! The object we are planning on + GraspableBody *mObject; + //! The number of candidate grasps that were sampled to be tried out + int mNumCandidates; + //! The current grasp displayed from the best list + int mBestGraspNum; + //! The reference position of the target object + transf mObjectRefTran; + //! The file (if any) that full results are written to + std::fstream *mOut; + + //! For batch testing for hand design + double mTFrom, mTTo, mTStep, mTR; + double mSFrom, mSTo, mSStep, mSR; + bool mBatch; + + //! + void ellipsoidSampling(double a, double b, double c, double resolution); + //! + void boxSampling(double a, double b, double c, double res); + //! + void sampleFace(vec3 x, vec3 y, vec3 z, double sz1, double sz2, vec3 tln, double res, + std::list *sampling); + + //! Samples an ellipsoid using a grid-based method to generate pre-grasps. + void gridEllipsoidSampling(const GraspPlanningState &seed, + std::list *sampling, int samples); + //! Helper function for the above + void addCartesianSamples(const GraspPlanningState &seed, std::list *sampling, + int samples, double x, double y, double z); + //! Initialization stuff + void init(); + //! Display one grasp from the result list + void showResult(); + void startPlanner(); +public: + 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(nextButton, SIGNAL(clicked()), + this, SLOT(nextButtonClicked())); + QObject::connect(prevButton, SIGNAL(clicked()), + this, SLOT(prevButtonClicked())); + QObject::connect(bestButton, SIGNAL(clicked()), + this, SLOT(bestButtonClicked())); + QObject::connect(resultsFileEdit, SIGNAL(editingFinished()), + this, SLOT(updateOut())); + QObject::connect(resultsBox, SIGNAL(activated(int)), + this, SLOT(updateOut())); + init(); + } + ~ListPlannerDlg(); + +public Q_SLOTS: + void generateButtonClicked(); + void testButtonClicked(); + void nextButtonClicked(); + void prevButtonClicked(); + void bestButtonClicked(); + void showOne(); + void update(); + void plannerFinished(); + void updateOut(); +}; + +#endif 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 33540e2a70..4554e35893 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 "gfoDlg.h" #include "contactExaminerDlg.h" #include "egPlannerDlg.h" @@ -109,6 +109,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())); @@ -123,6 +124,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())); @@ -577,7 +581,8 @@ void MainWindow::updateElementMenu() */ void MainWindow::updateGraspMenu() { - bool handFound = (world->getCurrentHand() != NULL); + + bool handFound = (world->getCurrentHand() !=NULL); bool bodyFound = (world->getNumBodies() != 0); mUI->graspAutoGraspAction->setEnabled(handFound); mUI->graspAuto_OpenAction->setEnabled(handFound); @@ -588,7 +593,8 @@ 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); @@ -693,15 +699,17 @@ 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(), - world->getGB(gb), mWindow); + + ListPlannerDlg *dlg =new ListPlannerDlg(world->getCurrentHand(), + world->getGB(gb), mWindow); dlg->setAttribute(Qt::WA_ShowModal, false); dlg->setAttribute(Qt::WA_DeleteOnClose, true); dlg->show(); diff --git a/ui/mainWindow.h b/ui/mainWindow.h index 1378f18ce9..85d13bc35d 100644 --- a/ui/mainWindow.h +++ b/ui/mainWindow.h @@ -24,20 +24,20 @@ //###################################################################### // -// $Header: +// $Header: // -/*! \file - \brief Implements the UI functions and Q_SLOTS for the main window. +/*! \file + \brief Implements the UI functions and Q_SLOTS for the main window. */ /*! \class MainWindow - \brief Main user interface, includes menus, tools, widget for viewer, etc. - + \brief Main user interface, includes menus, tools, widget for viewer, etc. + This class is derived from the QT mainWindow widget. It creates the main application window which includes the menu bar, tool bar, main - viewer window, quality measure result box, and the contacts list box. + viewer window, quality measure result box, and the contacts list box. */ @@ -54,103 +54,103 @@ class Grasp; class MainWindow : public QObject { - Q_OBJECT - private: + Q_OBJECT +private: World *world; QString fileName; int selectedContact; - void init(); - void destroy(); - void destroyChildren(); - public: - Ui::MainWindowUI *mUI; - Q3MainWindow *mWindow; + void init(); + void destroy(); + void destroyChildren(); +public: + Ui::MainWindowUI *mUI; + Q3MainWindow *mWindow; - MainWindow(QWidget *parent = 0); - virtual ~MainWindow() { - destroy(); - delete mWindow; - delete mUI; - } - void setMainWorld(World *w); - World *getMainWorld() {return world;} + MainWindow(QWidget *parent = 0); + virtual ~MainWindow(){ + destroy(); + delete mWindow; + delete mUI; + } + void setMainWorld( World *w ); + World* getMainWorld(){return world;} - public Q_SLOTS: - //------------- - void fileNew(); - void fileOpen(); - void fileSave(); - void fileSaveAs(); - void fileImportRobot(); - void fileImportObstacle(); - void fileImportObject(); - void fileEditSettings(); - void fileExit(); - void fileSaveImage(); - int saveAndContinue(const QString &action); - //------------- - void helpManual(); - void helpAbout(); - void helpAboutQT(); - //------------- - void setTool(QAction *a); - void elementTurnOffCollisions(); - void elementBodyProperties(); - void elementPrimitives(); - void updateElementMenu(); - //------------- - void graspAutoGrasp(); - void graspAutoOpen(); - void graspQualityMeasures(); - void graspCreateProjection(Grasp *g = NULL); - void graspForceOptimization(); - void graspPlanner(); - void updateGraspMenu(); - //------------- - void eigenGraspActivated(); - void graspContactExaminer_activated(); - void eigenGraspPlannerActivated(); - void graspCompliantPlanner(); - //------------- - void dbaseGUIAction_activated(); - void dbasePlannerAction_activated(); - void graspCapture(); - //------------- - void sensorsSensor_InputAction_activated(); - void sensorsBarrettHandAction(); - //------------- - void stereoOn(); - void stereoOff(); - void stereoFlip(); - //------------- - void archBuilder(); - //------------- - void updateContactsList(); - void contactSelected(int newSelection); - void clearContactsList(); - void updateQualityList(); - //-------------- - void updateTimeReadout(); - void toggleDynamics(); - void showDynamicsError(const char *errMsg); - void dynamicsPushState(); - void dynamicsPopState(); - //-------------- - void updateMaterialBoxList(); - void materialSelected(int whichMat); - void updateMaterialBox(); - void updateGraspBoxes(); - void handleHandSelectionChange(); - void selectGraspedBody(int sb); - void setCurrentHand(int sh); - void updateCollisionAction(bool state); - //--------------- - void updateTendonNamesBox(); - void handleTendonSelectionArea(); - void handleTendonDetailsArea(); - void TendonForceInput_valueChanged(int f); - void tendonNamesBoxActivated(int i); - void tendonVisibleCheckBox_toggled(bool vis); - void forcesVisibleCheckBox_toggled(bool vis); +public Q_SLOTS: + //------------- + void fileNew(); + void fileOpen(); + void fileSave(); + void fileSaveAs(); + void fileImportRobot(); + void fileImportObstacle(); + void fileImportObject(); + void fileEditSettings(); + void fileExit(); + void fileSaveImage(); + int saveAndContinue(const QString & action); + //------------- + void helpManual(); + void helpAbout(); + void helpAboutQT(); + //------------- + void setTool( QAction *a ); + void elementTurnOffCollisions(); + void elementBodyProperties(); + void elementPrimitives(); + void updateElementMenu(); + //------------- + void graspAutoGrasp(); + void graspAutoOpen(); + void graspQualityMeasures(); + void graspCreateProjection(Grasp* g = NULL); + void graspForceOptimization(); + void graspPlanner(); + void updateGraspMenu(); + //------------- + void eigenGraspActivated(); + void graspContactExaminer_activated(); + void eigenGraspPlannerActivated(); + void graspListPlannerActivated(); + //------------- + void dbaseGUIAction_activated(); + void dbasePlannerAction_activated(); + void graspCapture(); + //------------- + void sensorsSensor_InputAction_activated(); + void sensorsBarrettHandAction(); + //------------- + void stereoOn(); + void stereoOff(); + void stereoFlip(); + //------------- + void archBuilder(); + //------------- + void updateContactsList(); + void contactSelected(int newSelection); + void clearContactsList(); + void updateQualityList(); + //-------------- + void updateTimeReadout(); + void toggleDynamics(); + void showDynamicsError( const char *errMsg ); + void dynamicsPushState(); + void dynamicsPopState(); + //-------------- + void updateMaterialBoxList(); + void materialSelected( int whichMat ); + void updateMaterialBox(); + void updateGraspBoxes(); + void handleHandSelectionChange(); + void selectGraspedBody(int sb); + void setCurrentHand( int sh ); + void updateCollisionAction(bool state); + //--------------- + void updateTendonNamesBox(); + void handleTendonSelectionArea(); + void handleTendonDetailsArea(); + void TendonForceInput_valueChanged( int f); + void tendonNamesBoxActivated( int i); + void tendonVisibleCheckBox_toggled( bool vis); + void forcesVisibleCheckBox_toggled( bool vis); }; diff --git a/ui/mainWindow.ui b/ui/mainWindow.ui index 619ec047e3..a9fd23e678 100644 --- a/ui/mainWindow.ui +++ b/ui/mainWindow.ui @@ -434,6 +434,7 @@ + @@ -706,15 +707,15 @@ Grasp Force Optimization... - + - graspCompliantPlannerAction + graspListPlannerAction - Compliant Planner... + List Planner... - Compliant Planner... + List Planner...