00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include "compliantPlannerDlg.h"
00027
00028 #include <iostream>
00029
00030 #include <Inventor/actions/SoGetBoundingBoxAction.h>
00031
00032 #include <QIntValidator>
00033
00034
00035 #include "debug.h"
00036
00037
00038 #include "profiling.h"
00039
00040 PROF_DECLARE(QS_TOTAL);
00041
00042
00043 #include "graspitGUI.h"
00044 #include "ivmgr.h"
00045
00046
00047 #include "world.h"
00048
00049 #include "robot.h"
00050 #include "body.h"
00051 #include "grasp.h"
00052 #include "listPlanner.h"
00053 #include "searchState.h"
00054
00055 void
00056 CompliantPlannerDlg::init()
00057 {
00058 mPlanner = new ListPlanner(mHand);
00059 energyTypeBox->insertItem("Quasistatic");
00060 energyTypeBox->insertItem("Dynamic");
00061 mPlanner->setRenderType(RENDER_ALWAYS);
00062 mPlanner->setStatStream(&std::cerr);
00063 mOut = NULL;
00064 mHand->getGrasp()->setObjectNoUpdate(mObject);
00065 mObjectRefTran = mObject->getTran();
00066 QObject::connect(mPlanner, SIGNAL(update()), this, SLOT(update()));
00067 QObject::connect(mPlanner, SIGNAL(finished()), this, SLOT(plannerFinished()));
00068 mNumCandidates = 0;
00069 currentLabel->setText("0/0");
00070 QIntValidator* val = new QIntValidator(2, 99, this);
00071 resolutionEdit->setValidator(val);
00072 resolutionEdit->setText("8");
00073 QIntValidator* val2 = new QIntValidator(this);
00074 testOneEdit->setValidator(val2);
00075 testOneEdit->setText("0");
00076 resultsBox->insertItem("console");
00077 resultsBox->insertItem("file:");
00078 resultsFileEdit->setText("comp_plan.txt");
00079
00080 QDoubleValidator* dVal = new QDoubleValidator(0.5, 1.5, 3, this);
00081 tFromEdit->setValidator(dVal);
00082 tToEdit->setValidator(dVal);
00083 tStepEdit->setValidator(dVal);
00084 sFromEdit->setValidator(dVal);
00085 sToEdit->setValidator(dVal);
00086 sStepEdit->setValidator(dVal);
00087
00088 tFromEdit->setText("0.5");
00089 tToEdit->setText("1.5");
00090 tStepEdit->setText("0.1");
00091 sFromEdit->setText("0.5");
00092 sToEdit->setText("1.5");
00093 sStepEdit->setText("0.1");
00094
00095 }
00096
00097 CompliantPlannerDlg::~CompliantPlannerDlg()
00098 {
00099 if (mOut) {
00100 mOut->close();
00101 delete mOut;
00102 }
00103 delete mPlanner;
00104 }
00105
00106 void
00107 CompliantPlannerDlg::addCartesianSamples(const GraspPlanningState &seed,
00108 std::list<GraspPlanningState*> *sampling,
00109 int samples, double x, double y, double z)
00110 {
00111
00112 double a = seed.readPosition()->getParameter("a");
00113 double b = seed.readPosition()->getParameter("b");
00114
00115
00116
00117
00118
00119
00120 double beta = asin(z / sqrt(x*x + y*y + z*z));
00121 double gamma = atan2(y/b, x/a);
00122 DBGP("x: " << x << "; y: " << y <<"; z: " << z);
00123 DBGP("gamma: " << gamma << "; beta: " << beta);
00124
00125 for (int m=0; m<samples; m++) {
00126
00127 double tau = M_PI * ((double)m) / samples;
00128 GraspPlanningState *newState = new GraspPlanningState(&seed);
00129 newState->getPosition()->getVariable("tau")->setValue(tau);
00130 newState->getPosition()->getVariable("gamma")->setValue(gamma);
00131 newState->getPosition()->getVariable("beta")->setValue(beta);
00132 sampling->push_back(newState);
00133 }
00134 }
00135
00139 void
00140 CompliantPlannerDlg::gridEllipsoidSampling(const GraspPlanningState &seed,
00141 std::list<GraspPlanningState*> *sampling,
00142 int samples)
00143 {
00144 double a = seed.readPosition()->getParameter("a");
00145 double aRes = 2.0 * a / samples;
00146 double b = seed.readPosition()->getParameter("b");
00147 double bRes = 2.0 * b / samples;
00148 double c = seed.readPosition()->getParameter("c");
00149 double cRes = 2.0 * c / samples;
00150 DBGP("a: " << a << "; b: " << b <<"; c: " << c);
00151
00152 for (double i=0.5; i<samples; i+=1.0) {
00153 for(double j=0.5; j<samples; j+=1.0) {
00154 addCartesianSamples(seed, sampling, samples, a, -b+i*bRes, -c+j*cRes);
00155 addCartesianSamples(seed, sampling, samples, -a, -b+i*bRes, -c+j*cRes);
00156 addCartesianSamples(seed, sampling, samples, -a+i*aRes, b , -c+j*cRes);
00157 addCartesianSamples(seed, sampling, samples, -a+i*aRes,-b , -c+j*cRes);
00158 addCartesianSamples(seed, sampling, samples, -a+i*aRes, -b+j*bRes , c);
00159 addCartesianSamples(seed, sampling, samples, -a+i*aRes, -b+j*bRes ,-c);
00160 }
00161 }
00162 }
00163
00164 void
00165 CompliantPlannerDlg::generateButtonClicked()
00166 {
00167 int resolution = resolutionEdit->text().toInt();
00168 if (resolution < 1) {
00169 DBGA("Resolution must be at least 1");
00170 return;
00171 }
00172
00173 SoGetBoundingBoxAction *bba =
00174 new SoGetBoundingBoxAction(graspItGUI->getIVmgr()->getViewer()->getViewportRegion());
00175 bba->apply(mObject->getIVGeomRoot());
00176 SbVec3f bbmin,bbmax;
00177 bba->getBoundingBox().getBounds(bbmin,bbmax);
00178 delete bba;
00179 double a = 0.5*(bbmax[0] - bbmin[0]);
00180 double b = 0.5*(bbmax[1] - bbmin[1]);
00181 double c = 0.5*(bbmax[2] - bbmin[2]);
00182
00183 boxSampling(a,b,c, resolution);
00184
00185 mPlanner->resetPlanner();
00186 if (visualMarkersBox->isChecked()) {
00187 visualMarkersBoxClicked();
00188 }
00189 update();
00190 }
00191
00192 void
00193 CompliantPlannerDlg::ellipsoidSampling(double a, double b, double c, double resolution)
00194 {
00195
00196 GraspPlanningState seed(mHand);
00197 seed.setObject(mObject);
00198
00199
00200 seed.setRefTran(mObject->getTran(), false);
00201 seed.setPostureType(POSE_DOF, false);
00202 seed.setPositionType(SPACE_ELLIPSOID, false);
00203 seed.reset();
00204
00205
00206 seed.getPosition()->setParameter("a", a);
00207 seed.getPosition()->setParameter("b", b);
00208 seed.getPosition()->setParameter("c", c);
00209
00210 seed.getPosition()->getVariable("dist")->setValue(0.0);
00211 seed.getPosition()->getVariable("dist")->setFixed(true);
00212
00213 std::list<GraspPlanningState*> sampling;
00214
00215
00216
00217 gridEllipsoidSampling(seed, &sampling, resolution);
00218 DBGA("Sampled " << sampling.size() << " states.");
00219 mNumCandidates = sampling.size();
00220
00221 mPlanner->setInput(sampling);
00222 }
00223
00224 void
00225 CompliantPlannerDlg::sampleFace(vec3 x, vec3 y, vec3 z,
00226 double sz1, double sz2, vec3 tln, double res,
00227 std::list<GraspPlanningState*> *sampling)
00228 {
00229 mat3 R(x, y, z);
00230 int rotSamples=2;
00231
00232 double m1 = (2.0*sz1 - floor(2.0*sz1 / res) * res)/2.0;
00233 while (m1 < 2*sz1){
00234 double m2 = (2.0*sz2 - floor(2.0*sz2 / res) * res)/2.0;
00235 while (m2 < 2*sz2) {
00236 vec3 myTln(tln);
00237 myTln = myTln + (m1 - sz1)* y;
00238 myTln = myTln + (m2 - sz2)* z;
00239 transf tr(R, myTln);
00240 for(int rot=0; rot < rotSamples; rot++) {
00241 double angle = M_PI * ((double)rot) / rotSamples;
00242 transf rotTran(Quaternion(angle, vec3(1,0,0)), vec3(0,0,0));
00243 tr = rotTran * tr;
00244 GraspPlanningState* seed = new GraspPlanningState(mHand);
00245 seed->setObject(mObject);
00246 seed->setRefTran(mObject->getTran(), false);
00247 seed->setPostureType(POSE_DOF, false);
00248 seed->setPositionType(SPACE_COMPLETE, false);
00249 seed->reset();
00250 seed->getPosition()->setTran(tr);
00251 sampling->push_back(seed);
00252 }
00253 m2+=res;
00254 }
00255 m1 += res;
00256 }
00257 }
00258
00259 void
00260 CompliantPlannerDlg::boxSampling(double a, double b, double c, double res)
00261 {
00262 std::list<GraspPlanningState*> sampling;
00263 res = 30;
00264 sampleFace( vec3(0, 1,0), vec3(-1,0,0), vec3(0,0,1) , a, c, vec3(0,-b,0), res, &sampling);
00265 sampleFace( vec3(0,-1,0), vec3( 1,0,0), vec3(0,0,1) , a, c, vec3(0, b,0), res, &sampling);
00266
00267 sampleFace( vec3(0,0, 1), vec3(0,1,0), vec3(-1,0,0) , b, a, vec3(0,0,-c), res, &sampling);
00268 sampleFace( vec3(0,0,-1), vec3(0,1,0), vec3( 1,0,0) , b, a, vec3(0,0, c), res, &sampling);
00269
00270 sampleFace( vec3( 1,0,0), vec3(0, 1,0), vec3(0,0,1) , b, c, vec3(-a,0,0), res, &sampling);
00271 sampleFace( vec3(-1,0,0), vec3(0,-1,0), vec3(0,0,1) , b, c, vec3( a,0,0), res, &sampling);
00272
00273 DBGA("Sampled " << sampling.size() << " states.");
00274 mNumCandidates = sampling.size();
00275 mPlanner->setInput(sampling);
00276 }
00277
00278 void
00279 CompliantPlannerDlg::testOneButtonClicked()
00280 {
00281 if (energyTypeBox->currentText() == "Quasistatic") {
00282 mPlanner->setEnergyType(ENERGY_COMPLIANT);
00283 } else if (energyTypeBox->currentText() == "Dynamic") {
00284 mPlanner->setEnergyType(ENERGY_DYNAMIC);
00285 } else {
00286 assert(0);
00287 }
00288 if (mPlanner->isActive()) {
00289 DBGA("Stop planner first!");
00290 return;
00291 }
00292 int num = testOneEdit->text().toInt();
00293 if (num < 0 || num >= mNumCandidates) {
00294 DBGA("Wrong test number selected");
00295 return;
00296 }
00297
00298 mPlanner->setStatStream(&std::cerr);
00299 DBGA("Testing pre-grasp #" << num);
00300 mPlanner->testState(num);
00301 mHand->getWorld()->updateGrasps();
00302 graspItGUI->getIVmgr()->drawDynamicForces();
00303 graspItGUI->getIVmgr()->drawUnbalancedForces();
00304 }
00305
00306 void
00307 CompliantPlannerDlg::startPlanner()
00308 {
00309 mPlanner->startPlanner();
00310 }
00311
00312 void
00313 CompliantPlannerDlg::testButtonClicked()
00314 {
00315 if (energyTypeBox->currentText() == "Quasistatic") {
00316 mPlanner->setEnergyType(ENERGY_COMPLIANT);
00317 } else if (energyTypeBox->currentText() == "Dynamic") {
00318 mPlanner->setEnergyType(ENERGY_DYNAMIC);
00319 } else {
00320 assert(0);
00321 }
00322
00323 if (mPlanner->isActive()) {
00324 DBGA("Pause:");
00325 mPlanner->pausePlanner();
00326 } else {
00327 PROF_RESET(QS_TOTAL);
00328 PROF_START_TIMER(QS_TOTAL);
00329 mBatch = false;
00330 if (mOut) {
00331 mPlanner->setStatStream(mOut);
00332 } else {
00333 mPlanner->setStatStream(&std::cerr);
00334 }
00335 startPlanner();
00336 }
00337 }
00338
00339 void
00340 CompliantPlannerDlg::update()
00341 {
00342 QString n1,n2;
00343 n1.setNum(mPlanner->getCurrentStep());
00344 n2.setNum(mNumCandidates);
00345 currentLabel->setText(n1 + "/" + n2);
00346 }
00347
00348 void
00349 CompliantPlannerDlg::showResult()
00350 {
00351 int d = mPlanner->getListSize();
00352 int rank, size, iteration; double energy;
00353 bool render = true;
00354
00355 if (d==0) {
00356 mBestGraspNum = rank = size = iteration = energy = 0; render = false;
00357 } else if (mBestGraspNum < 0){
00358 mBestGraspNum = 0;
00359 } else if ( mBestGraspNum >= d) {
00360 mBestGraspNum = d-1;
00361 }
00362
00363 if ( d!=0 ){
00364 const GraspPlanningState *s = mPlanner->getGrasp(mBestGraspNum);
00365 rank = mBestGraspNum+1;
00366 size = d;
00367 energy = s->getEnergy();
00368 iteration = s->getItNumber();
00369 }
00370
00371 QString n1,n2;
00372 n1.setNum(rank);
00373 n2.setNum(size);
00374 rankLabel->setText("Rank: " + n1 + "/" + n2);
00375 n1.setNum(energy,'f',3);
00376 energyLabel->setText("Energy: " + n1);
00377 n1.setNum(iteration);
00378 testOneEdit->setText(n1);
00379 showOneButtonClicked();
00380 iterationLabel->setText("Iteration: " + n1);
00381
00382 }
00383
00384 void
00385 CompliantPlannerDlg::prevButtonClicked()
00386 {
00387 mBestGraspNum--;
00388 showResult();
00389 }
00390 void
00391 CompliantPlannerDlg::nextButtonClicked()
00392 {
00393 mBestGraspNum++;
00394 showResult();
00395 }
00396 void
00397 CompliantPlannerDlg::bestButtonClicked()
00398 {
00399 mBestGraspNum=0;
00400 showResult();
00401 }
00402
00403 void
00404 CompliantPlannerDlg::showOneButtonClicked()
00405 {
00406 if (mPlanner->isActive()) {
00407 DBGA("Stop planner first!");
00408 return;
00409 }
00410 int num = testOneEdit->text().toInt();
00411 if (num < 0 || num >= mNumCandidates) {
00412 DBGA("Wrong test number selected");
00413 return;
00414 }
00415 DBGA("Testing pre-grasp #" << num);
00416 mPlanner->showState(num);
00417 }
00418
00419 void
00420 CompliantPlannerDlg::prepareOneButtonClicked()
00421 {
00422 if (mPlanner->isActive()) {
00423 DBGA("Stop planner first!");
00424 return;
00425 }
00426 int num = testOneEdit->text().toInt();
00427 if (num < 0 || num >= mNumCandidates) {
00428 DBGA("Wrong test number selected");
00429 return;
00430 }
00431 DBGA("Testing pre-grasp #" << num);
00432 mPlanner->prepareState(num);
00433 }
00434
00435 void
00436 CompliantPlannerDlg::visualMarkersBoxClicked()
00437 {
00438 mPlanner->showVisualMarkers( visualMarkersBox->isChecked());
00439 }
00440
00441 void
00442 CompliantPlannerDlg::resetObjectButtonClicked()
00443 {
00444 mObject->setTran(mObjectRefTran);
00445 }
00446
00447 void
00448 CompliantPlannerDlg::updateOut()
00449 {
00450 if (mOut) {
00451 mOut->close();
00452 delete mOut; mOut = NULL;
00453 }
00454 if (resultsBox->currentIndex()==0) {
00455 DBGA("Output to stderr");
00456 return;
00457 }
00458 QString filename("data\\" + resultsFileEdit->text());
00459 mOut = new std::fstream(filename.latin1(), std::fstream::out | std::fstream::app);
00460 if (mOut->fail()) {
00461 DBGA("Failed to open file " << filename.latin1());
00462 delete mOut; mOut = NULL;
00463 resultsBox->setCurrentItem(0);
00464 } else {
00465 DBGA("Output file opened: " << filename.latin1());
00466 }
00467 }
00468
00469 void
00470 CompliantPlannerDlg::designTestButtonClicked()
00471 {
00472 if (!mOut) {
00473 DBGA("Set output file first!!!!");
00474 return;
00475 }
00476 mTFrom = tFromEdit->text().toDouble();
00477 mTTo = tToEdit->text().toDouble();
00478 mTStep = tStepEdit->text().toDouble();
00479 mSFrom = sFromEdit->text().toDouble();
00480 mSTo = sToEdit->text().toDouble();
00481 mSStep = sStepEdit->text().toDouble();
00482 DBGA("Starting batch testing");
00483 mBatch = true;
00484 mTR = mTFrom;
00485 mSR = mSFrom;
00486 mPlanner->setEnergyType(ENERGY_COMPLIANT);
00487 mPlanner->setStatStream(NULL);
00488 startPlanner();
00489 }
00490
00491 void
00492 CompliantPlannerDlg::plannerFinished()
00493 {
00494 PROF_STOP_TIMER(QS_TOTAL);
00495 PROF_PRINT(QS_TOTAL);
00496 if (!mBatch) return;
00497
00498 DBGA("Test done: " << mTR << " and " << mSR);
00499 if (mOut) {
00500 *mOut << mTR << " " << mSR;
00501 *mOut << std::endl;
00502 }
00503 mSR += mSStep;
00504 if (mSR > mSTo + 1.0e-2) {
00505 mSR = mSFrom;
00506 mTR += mTStep;
00507 if (mTR > mTTo + 1.0e-2) {
00508 mBatch = false;
00509 return;
00510 }
00511 }
00512 mPlanner->resetPlanner();
00513 startPlanner();
00514 }
