graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Matei T. Ciocarlie
00021 //
00022 // $Id: sensorInputDlg.cpp,v 1.5 2009/05/12 14:16:15 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "sensorInputDlg.h"
00027 
00028 #include <assert.h>
00029 #include <Inventor/sensors/SoTimerSensor.h>
00030 
00031 #include "robot.h"
00032 #include "world.h"
00033 #include "graspitGUI.h"
00034 #include "ivmgr.h"
00035 #include "body.h"
00036 #include "barrett.h"
00037 #include "Flock.h"
00038 #include "CyberGlove.h"
00039 #include "gloveInterface.h"
00040 #include "collisionStructures.h"
00041 
00042 //#define GRASPITDBG
00043 #include "debug.h"
00044 
00048 void 
00049 SensorInputDlg::init(World *w)
00050 {
00051         mWorld = w;
00052         mGloveRunning = false;
00053         mFlockRunning = false;
00054         mFlock = NULL;
00055         mGlove = NULL;
00056         mTimerSensor = new SoTimerSensor(timerStaticCB, this);
00057         //timer interval, in seconds
00058         mTimerSensor->setInterval( SbTime(0.05) );
00059         mNumBirds = 1;
00060         flockModeBox->insertItem("Relative");
00061         flockModeBox->insertItem("Camera");
00062         flockModeBox->insertItem("Absolute");
00063         masterFlockBox->setMinimum(1);
00064         masterFlockBox->setMaximum(9);
00065         masterFlockBox->setValue(1);
00066         mMasterBird = 1;
00067         mFlockMode = FLOCK_RELATIVE;
00068 }
00069 
00070 void 
00071 SensorInputDlg::timerStaticCB(void *data, SoSensor*)
00072 {
00073         SensorInputDlg *dlg = static_cast<SensorInputDlg*>(data);
00074         dlg->timerInternalCB();
00075 }
00076 
00082 void 
00083 SensorInputDlg::timerInternalCB()
00084 {
00085         bool needed = false;
00086         if (mGloveRunning) {
00087                 if (!mGlove->instantRead()) {
00088                         DBGA("Error reading CyberGlove\n");
00089                         gloveStartButton_clicked();
00090                 } else {
00091                         if (!mFlockRunning) processGlove();
00092                         needed = true;
00093                 }
00094         }
00095         if (mFlockRunning) {
00096                 bool process = true;
00097                 // If the Flock is attached to a real Barrett, we ignore readings
00098                 // while the motors of the Barrett are active, as they corrupt the bird.
00099                 if ( mWorld->getCurrentHand() && mWorld->getCurrentHand()->isA("Barrett") ) {
00100                         if ( ((Barrett*)mWorld->getCurrentHand())->isBusy() ) process = false;
00101                 }
00102                 if (process) {
00103                         //read in transforms from all the birds in the system
00104                         std::vector<transf> birdTransf(mNumBirds+1);
00105                         for (int i=1; i<=mNumBirds; i++) {
00106                                 birdTransf[i] = getBirdTran(i);
00107                         }
00108                         processFlockCamera(birdTransf);
00109                         processFlockBodies(birdTransf);
00110                         if (!mGloveRunning) {
00111                                 processFlockRobots(birdTransf);
00112                         } else {
00113                                 flockAndGloveFuzzyGrasp(birdTransf);
00114                         }
00115                 }
00116                 needed = true;
00117         }
00118         if (!needed) {
00119                 mTimerSensor->unschedule();
00120         }
00121 }
00122 
00126 bool
00127 SensorInputDlg::initFlock()
00128 {
00129         mFlock = new Flock(1);
00130         return true;
00131 }
00132 
00138 bool
00139 SensorInputDlg::initGlove()
00140 {
00141         mGlove = new CyberGlove();
00142         if (!mGlove->testGlove()) {
00143                 delete mGlove; mGlove = NULL;
00144                 return false;
00145         }
00146         for ( int i=0; i<mWorld->getNumRobots(); i++) {
00147                 if (mWorld->getRobot(i)->useCyberGlove()) {
00148                         mWorld->getRobot(i)->setGlove(mGlove);
00149                 }
00150         }       
00151         return true;
00152 }
00153 
00154 void 
00155 SensorInputDlg::exitButton_clicked()
00156 {
00157         if (mFlock) delete mFlock;
00158         if (mGlove) delete mGlove;
00159         delete mTimerSensor;
00160         QDialog::accept();
00161 }
00162 
00166 void 
00167 SensorInputDlg::gloveStartButton_clicked()
00168 {
00169         if (!mGloveRunning) {
00170                 if (!mGlove) {
00171                         if (!initGlove()) {
00172                                 DBGA("Glove init failed");
00173                                 return;
00174                         }
00175                 }
00176                 mGloveRunning = true;
00177                 if (!mTimerSensor->isScheduled()) {
00178                         mTimerSensor->schedule();
00179                 }
00180                 gloveStartButton->setText("Stop");
00181         } else {
00182                 mGloveRunning = false;
00183                 gloveStartButton->setText("Start");
00184         }
00185 }
00186 
00194 void 
00195 SensorInputDlg::flockStartButton_clicked()
00196 {
00197         if (!mFlockRunning) {
00198                 if (!mFlock) {
00199                         if (!initFlock()) {
00200                                 DBGA("Flock init failed");
00201                                 return;
00202                         }
00203                         //on first execution we also reset the flock which sets the reference
00204                         //transforms for all the bodies in the world
00205                         resetFlock();
00206                 }
00207                 mFlockRunning = true;
00208                 if (!mTimerSensor->isScheduled()) {
00209                         mTimerSensor->schedule();
00210                 }
00211                 flockStartButton->setText("Stop");
00212                 flockModeBox->setEnabled(FALSE);
00213                 masterFlockBox->setEnabled(FALSE);
00214                 resetFlockButton->setEnabled(FALSE);
00215         } else {
00216                 mFlockRunning = false;
00217                 flockStartButton->setText("Start");
00218                 flockModeBox->setEnabled(TRUE);
00219                 masterFlockBox->setEnabled(TRUE);
00220                 resetFlockButton->setEnabled(TRUE);
00221         }
00222 }
00223 
00224 transf 
00225 SensorInputDlg::getBirdTran(int b)
00226 {
00227         if (!mFlock->instantRead(b) ) {
00228                 DBGA("Error reading Flock!");
00229                 mFlockRunning = false;
00230                 flockStartButton->setText("Start");
00231                 return transf::IDENTITY;
00232         }
00233         
00234         double r[9], t[3];
00235         mFlock->getRotationMatrix(r);
00236         mFlock->getPosition(t);
00237 
00238         transf birdTran;
00239         birdTran.set(mat3(r), vec3(t));
00240         return birdTran;
00241 }
00242 
00251 void
00252 SensorInputDlg::resetFlock()
00253 {
00254         assert(mFlock);
00255 
00256         //get the current transform of the master bird
00257         mMasterBird = masterFlockBox->value();
00258         if (flockModeBox->currentText()=="Relative") {
00259                 mFlockMode = FLOCK_RELATIVE;
00260         } else if (flockModeBox->currentText()=="Camera") {
00261                 mFlockMode = FLOCK_CAMERA;
00262         } else if (flockModeBox->currentText()=="Absolute") {
00263                 mFlockMode = FLOCK_ABSOLUTE;
00264         };
00265 
00266         transf masterFlockTran = getBirdTran(mMasterBird);
00267 
00268         //for each body, set the base transform that it will be relative to
00269         if (mFlockMode == FLOCK_CAMERA) {
00270                 //everything gets rearranged relative to the camera
00271                 transf mount(mat3( vec3(0,1,0), vec3(0,0,-1), vec3(-1,0,0) ), vec3(0,0,0));
00272                 mCameraFlockTran.setMount(mount.inverse());
00273                 //the base is set based on current camera position
00274                 transf cameraBaseTran = graspItGUI->getIVmgr()->getCameraTransf();
00275                 mCameraFlockTran.setFlockBase(masterFlockTran);
00276                 mCameraFlockTran.setObjectBase(cameraBaseTran);
00277                 cameraBaseTran = mount.inverse() * cameraBaseTran;
00278                 //process the bodies
00279                 for (int i=0; i<mWorld->getNumBodies(); i++) {
00280                         if (!mWorld->getBody(i)->usesFlock() || 
00281                                 mWorld->getBody(i)->getOwner() != mWorld->getBody(i) ) continue;
00282                         //each body will be re-arranged relative to the camera transform
00283                         mWorld->getBody(i)->getFlockTran()->setFlockBase( masterFlockTran );
00284                         mWorld->getBody(i)->getFlockTran()->setObjectBase( cameraBaseTran );
00285                 }
00286                 //process the robots
00287                 for (int i=0; i<mWorld->getNumRobots(); i++) {
00288                         if ( !mWorld->getRobot(i)->usesFlock() ) continue;
00289                         mWorld->getRobot(i)->getFlockTran()->setFlockBase( masterFlockTran );
00290                         mWorld->getRobot(i)->getFlockTran()->setObjectBase( cameraBaseTran );
00291                 }
00292         } else if (mFlockMode == FLOCK_RELATIVE) {
00293                 //bodies run by the master bird remain relative to themselves
00294                 //those that are not, will be relative to one of the bodies run by the master bird
00295                 //which one of the bodies run by the master bird? for now, we just pick one at random
00296                 //first, do the ones that are run by the master bird
00297                 transf objectBaseTran = transf::IDENTITY;
00298                 bool baseSet = false;
00299                 //set the bodies
00300                 for (int i=0; i<mWorld->getNumBodies(); i++) {
00301                         if (!mWorld->getBody(i)->usesFlock() || 
00302                                 mWorld->getBody(i)->getOwner() != mWorld->getBody(i) ) continue;
00303                         if (mWorld->getBody(i)->getBirdNumber()!=mMasterBird) continue;
00304                         //pick the first body run by the master as a reference
00305                         if (!baseSet) {
00306                                 objectBaseTran = mWorld->getBody(i)->getTran();
00307                                 baseSet = true;
00308                         }
00309                         //body will be relative to itself
00310                         mWorld->getBody(i)->getFlockTran()->setFlockBase( masterFlockTran );
00311                         mWorld->getBody(i)->getFlockTran()->setObjectBase( mWorld->getBody(i)->getTran() );
00312                 }
00313                 //set the robots
00314                 for (int i=0; i<mWorld->getNumRobots(); i++) {
00315                         if ( !mWorld->getRobot(i)->usesFlock() ) continue;
00316                         if ( mWorld->getRobot(i)->getBirdNumber() != mMasterBird ) continue;
00317                         if (!baseSet) {
00318                                 objectBaseTran = mWorld->getRobot(i)->getTran();
00319                                 baseSet = true;
00320                         }
00321                         mWorld->getRobot(i)->getFlockTran()->setFlockBase( masterFlockTran );
00322                         mWorld->getRobot(i)->getFlockTran()->setObjectBase( mWorld->getRobot(i)->getTran() );
00323                 }
00324                 if (!baseSet) {
00325                         DBGA("WARNING: nobody is run by master bird!");
00326                 }
00327                 //now do all those that are not run by the master bird
00328                 for (int i=0; i<mWorld->getNumBodies(); i++) {
00329                         if (!mWorld->getBody(i)->usesFlock() || 
00330                                 mWorld->getBody(i)->getOwner() != mWorld->getBody(i) ) continue;
00331                         if (mWorld->getBody(i)->getBirdNumber()==mMasterBird) continue;
00332                         mWorld->getBody(i)->getFlockTran()->setFlockBase( masterFlockTran );
00333                         mWorld->getBody(i)->getFlockTran()->setObjectBase( objectBaseTran );
00334                 }
00335                 for (int i=0; i<mWorld->getNumRobots(); i++) {
00336                         if ( !mWorld->getRobot(i)->usesFlock() ) continue;
00337                         if ( !mWorld->getRobot(i)->getBirdNumber() == mMasterBird ) continue;
00338                         mWorld->getRobot(i)->getFlockTran()->setFlockBase( masterFlockTran );
00339                         mWorld->getRobot(i)->getFlockTran()->setObjectBase( objectBaseTran );
00340                 }
00341         } else if (mFlockMode == FLOCK_ABSOLUTE) {
00342                 //we have nothing to do here; each body will get the bird tran in absolute terms
00343         } else {
00344                 DBGA("Unknown Flock mode requested!");
00345                 assert(0);
00346         }
00347 }
00348 
00349 void
00350 SensorInputDlg::processFlockCamera(std::vector<transf> &birdTransf)
00351 {
00352         if (mFlockMode == FLOCK_CAMERA) {
00353                 graspItGUI->getIVmgr()->setCameraTransf( mCameraFlockTran.get(birdTransf[mMasterBird]) );
00354         }
00355 }
00356 
00362 void
00363 SensorInputDlg::processFlockBodies(std::vector<transf> &birdTransf)
00364 {
00365         for (int i=0; i<mWorld->getNumBodies(); i++) {
00366                 if (mWorld->getBody(i)->usesFlock() && 
00367                         mWorld->getBody(i)->getOwner() == mWorld->getBody(i) ) {
00368                         transf bodyTran;
00369                         transf birdTran = birdTransf[mWorld->getBody(i)->getBirdNumber()];
00370                         if (mFlockMode == FLOCK_ABSOLUTE) {     
00371                                 bodyTran = mWorld->getBody(i)->getFlockTran()->getAbsolute(birdTran);
00372                         } else {
00373                                 bodyTran = mWorld->getBody(i)->getFlockTran()->get(birdTran);
00374                         }
00375                         mWorld->getBody(i)->moveTo(bodyTran , WorldElement::ONE_STEP, 
00376                                                                            WorldElement::ONE_STEP );
00377                 }
00378         }
00379 }
00380 
00381 void
00382 SensorInputDlg::processFlockRobots(std::vector<transf> &birdTransf)
00383 {
00384         for (int i=0; i<mWorld->getNumRobots(); i++) {
00385                 if ( mWorld->getRobot(i)->usesFlock() ) {
00386                         transf bodyTran;
00387                         transf birdTran = birdTransf[mWorld->getRobot(i)->getBirdNumber()]; 
00388                         if (mFlockMode == FLOCK_ABSOLUTE) {
00389                                 bodyTran = mWorld->getRobot(i)->getFlockTran()->getAbsolute( birdTran );
00390                         } else {
00391                                 bodyTran = mWorld->getRobot(i)->getFlockTran()->get( birdTran );
00392                         }
00393                         mWorld->getRobot(i)->moveTo( bodyTran, WorldElement::ONE_STEP, 
00394                                                                                  WorldElement::ONE_STEP );
00395                 }
00396         }
00397 }
00398 
00399 void
00400 SensorInputDlg::processGlove()
00401 {
00402         for ( int i=0; i<mWorld->getNumRobots(); i++) {
00403                 if (mWorld->getRobot(i)->useCyberGlove()) {
00404                         mWorld->getRobot(i)->processCyberGlove();
00405                 }               
00406         }
00407 }
00408 
00409 void
00410 SensorInputDlg::flockAndGloveFuzzyGrasp(std::vector<transf> &birdTransf) 
00411 {
00412         for (int i=0; i<mWorld->getNumRobots(); i++) {
00413                 Robot *robot = mWorld->getRobot(i);
00414                 if (!robot->usesFlock() || !robot->useCyberGlove()) continue;
00415                 //compute robot transform, same as in processFlockRobots
00416                 transf robotTran;
00417                 transf birdTran = birdTransf[mWorld->getRobot(i)->getBirdNumber()]; 
00418                 if (mFlockMode == FLOCK_ABSOLUTE) {
00419                         robotTran = mWorld->getRobot(i)->getFlockTran()->getAbsolute( birdTran );
00420                 } else {
00421                         robotTran = mWorld->getRobot(i)->getFlockTran()->get( birdTran );
00422                 }
00423                 //force hand in position
00424                 robot->setTran(robotTran);
00425                 //compute collisions only for the palm
00426                 CollisionReport colReport, originalColReport;
00427                 std::vector<Body*> interestList;
00428                 interestList.push_back(robot->getBase());
00429                 //if palm collision, back up until out of it
00430                 transf collisionTran;
00431                 bool collision = false;
00432                 int steps = 0, maxSteps = 3;
00433                 //we need to remember the original collision report for later when we interpolate
00434                 int numCollisions = mWorld->getCollisionReport(&originalColReport, &interestList);
00435                 while ( numCollisions && steps < maxSteps) {
00436                         steps++;
00437                         collision = true;
00438                         collisionTran = robot->getTran();
00439                         transf newTran = translate_transf(vec3(0,0,-10.0) * 
00440                                                          robot->getApproachTran()) * robot->getTran();
00441                         robot->setTran(newTran);
00442                         numCollisions = mWorld->getCollisionReport(&colReport, &interestList);
00443                 }
00444                 bool palmSuccess;
00445                 if (steps == maxSteps) {
00446                         //we have not managed to resolve the collision
00447                         palmSuccess = false;
00448                         robot->setTran(robotTran);
00449                         DBGP("Can not resolve collision");
00450                 } else if (collision) {
00451                         //if we have resolved a collision, go back in until contact
00452                         if (!robot->interpolateTo(robot->getTran(), collisionTran, originalColReport)) {
00453                                 DBGA("Palm interpolation failed");
00454                                 palmSuccess = false;
00455                         } else {
00456                                 mWorld->findContacts(originalColReport);
00457                                 palmSuccess = true;
00458                         }
00459                 } else {
00460                         //no collision from the beginning
00461                         palmSuccess = true;
00462                 }
00463 
00464                 //read and prepare the glove dof values
00465                 std::vector<double> dofVals(robot->getNumDOF(), 0.0);
00466                 robot->getDOFVals(&dofVals[0]);
00467                 for (int i=0; i<robot->getNumDOF(); i++) {
00468                         if ( robot->getGloveInterface()->isDOFControlled(i) ) {
00469                                 dofVals[i] = robot->getGloveInterface()->getDOFValue(i);
00470                         } 
00471                 }
00472                 robot->checkSetDOFVals(&dofVals[0]);
00473                 //force the required dof values, no collision check
00474                 robot->forceDOFVals(&dofVals[0]);
00475 
00476                 if (!palmSuccess) {
00477                         //we have failed with the palm; leave the chains like this and we are done
00478                         DBGA("Palm failure");
00479                         //mark the palm in red
00480                         continue;
00481                 }
00482                 
00483                 numCollisions = mWorld->getCollisionReport(&colReport);
00484                 //for each chain, if collision snap to contacts
00485                 bool success = true;
00486                 for (int c=0; c<robot->getNumChains(); c++) {
00487                         success = success && robot->snapChainToContacts(c, colReport);
00488                 }
00489                 if (success) {
00490                         mWorld->updateGrasps();
00491                 } else {
00492                         //mark colliding bodies
00493                         DBGP("Chain failure");
00494                 }
00495         }
00496 }
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/ui/sensorInputDlg.cpp
