/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/Planner/grasp_manager.cpp

Go to the documentation of this file.

//######################################################################
//
// 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 <http://www.gnu.org/licenses/>.
//
// Authors: Steffen Knoop
//          Andrew T. Miller 
//
// $Id: grasp_manager.cpp,v 1.8 2009/06/25 20:26:39 cmatei Exp $
//
//######################################################################

/***************************************************************************/
/*      FILE:     grasp_manager.cc                                         */
/*      AUTHOR:   Steffen Knoop                                            */
/*      DATE:     03-08-2002                                               */
/***************************************************************************/

/* standard c,c++ includes */
#include <stdio.h>
#include <iostream>

#include <QFile>
#include <QTextStream>

/* inventor includes */
#include <Inventor/actions/SoGetBoundingBoxAction.h>
#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoCone.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/SoLists.h>
#include "SoArrow.h"
#include "SoTorquePointer.h"
#include "SoComplexShape.h"
#include <Inventor/fields/SoSFFloat.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/SbLinear.h>
#include <Inventor/nodes/SoSelection.h>

#ifdef Q_WS_X11
  #include <unistd.h>
#endif

/* graspit includes */
#include "graspitGUI.h"
#include "contact.h"
#include "world.h"
#include "ivmgr.h"
#include "robot.h"
#include "body.h"
#include "grasp.h"

/* STL includes */
#include <list>

/* include grasp planner class */
#include "matvecIO.h"
#include "grasp_coordinates.h"
#include "grasp_directions.h"
#include "grasp_preshape.h"
#include "grasp_visualization.h"
#include "grasp_grasps.h"
#include "grasp_planner.h"
#include "grasp_tester.h"
#include "grasp_presenter.h"
 
#include "grasp_manager.h"

//#define GRASPITDBG


/* globals */
grasp_tester *myTester;

grasp_manager::grasp_manager(){
    graspsNotShown = 0;
    renderIt = false;
    doIteration = false;
    itQualThresh = ITERATION_QUALITY_THRESH;
    maxdist = MAX_GRASP_DISTANCE_IN_ITERATION;

    myPlanner = new grasp_planner();
    myTester = new grasp_tester();
    myPresent = new grasp_presenter();

#ifdef GRASPITDBG
    std::cout << "PL_OUT: Grasp Manager built." << std::endl;
#endif
}

grasp_manager::~grasp_manager(){
  std::list<plannedGrasp *>::iterator gptr;

        if (!graspList.empty())
          for (gptr = graspList.begin();gptr!=graspList.end();gptr++)
                delete *gptr;

    delete myPlanner;
    delete myTester;
    delete myPresent;

#ifdef GRASPITDBG
    std::cout << "PL_OUT: Grasp manager destroyed." << std::endl;
#endif
}


void
grasp_manager::loadPrimitives()
{
 
  SoInput myInput;
  char prDir[256];
  QString directory = QString(getenv("GRASPIT"))+
    QString("/models/objects/primitives/");
  QString filename = my_body->getFilename().section('/',-1);
  //make sure the extension is iv, as this is how primitives
  //are stored for now
  filename = filename.section('.',-2,-2) + ".iv";
  QString path = directory + filename;

  printf("Loading primitive %s.\n",path.latin1());

  if (!(myInput.openFile(path.latin1()))) {
    pr_error("could not open primitives file!");
    primitives = my_body->getIVGeomRoot();
    printf ("%s\n",prDir);
    printf("Setting primitive root node to original object.\n");
  }
  else {
      primitives = SoDB::readAll(&myInput);
          myInput.closeFile();
      if (primitives == NULL) {
          printf("Load Primitive didnt work, although file seems to exist.\n");
          printf("Setting primitive root node to original object.\n");
          primitives = my_body->getIVGeomRoot();
      }
      else {
        primitives->ref();
      }
  }

}

int
grasp_manager::readCandidateGraspsFile(const QString& filename)
{
  cartesian_coordinates pt,dir,thumbdir;
  cartesianGraspDirection gd;
  plannedGrasp *pg;
  double spreadAngle;
  std::list<plannedGrasp *>::iterator gptr;


  QFile file(filename);

  if (!file.open(QIODevice::ReadOnly))
    return 1;

  QTextStream stream( &file );

  my_body = graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->getGrasp()->getObject();

  if (my_body == NULL){
    int whichObject = 0;
    if (graspItGUI->getIVmgr()->getWorld()->getNumGB())
      my_body = graspItGUI->getIVmgr()->getWorld()->getGB(whichObject);
    else{
#ifdef GRASPITDBG
      std::cout << "PL_OUT: Nothing selected and no graspable bodies exist. Stop." << std::endl;
#endif
      return 1;
    }
  }
    
  preshape compPreshape;
  if (!graspList.empty())
    for (gptr = graspList.begin();gptr!=graspList.end();gptr++)
      delete *gptr;
  
  graspList.clear();
  
  while (!stream.atEnd()) {
    stream >> pt >> dir >> thumbdir>>spreadAngle; stream.readLine();
#ifdef GRASPITDBG
    std::cout << "read " << pt << dir << thumbdir<<spreadAngle<<std::endl;
#endif
    gd.set_point(pt);
    gd.set_dir(dir);
    pg = new plannedGrasp(gd);
    pg->set_fixedFingerDirection(thumbdir);
    pg->set_graspableBody(my_body);
    compPreshape.set_preshape(spreadAngle,0,0,0);
    pg->set_preshape(compPreshape);
 
    graspList.push_back(pg);
  }
  
#ifdef GRASPITDBG
  std::cout << "PL_OUT: "<<graspList.size()<<" grasps read."<<std::endl;
#endif

  nrOfPlannedGrasps = graspList.size();

  //use original body as primitive since grasps were pregenerated
  primitives = my_body->getIVGeomRoot();
  
  myTester->setupGraspVisWindow( my_body,primitives);
  myTester->visualizePlannedGrasps(graspList);
  
  return 0;
}

void
grasp_manager::generateGrasps(){
    
    std::list<plannedGrasp*> tmpList;

    my_body = graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->getGrasp()->getObject();

    if (my_body == NULL){
        int whichObject = 0;
        if (graspItGUI->getIVmgr()->getWorld()->getNumGB())
            my_body = graspItGUI->getIVmgr()->getWorld()->getGB(whichObject);
        else{
#ifdef GRASPITDBG
            std::cout << "PL_OUT: Nothing selected and no graspable bodies exist. Stop." << std::endl;
#endif
            return;
        }
    }
    
    loadPrimitives();

    /* change accuracy of planner */
    //changePlanningParameters(nrOfPlanTestIteration);

    tmpList = std::list<plannedGrasp*>(graspList);

    /* get directions and positions */
    graspList = myPlanner->planIt(my_body,primitives);

    /* only keep grasps that are near the good ones from last step, if doIteration is checked */
    if (doIteration)
      compareGraspListsByDist(graspList, tmpList);
    nrOfPlannedGrasps = graspList.size();

    myTester->setupGraspVisWindow( my_body,primitives);
    myTester->visualizePlannedGrasps(graspList);
}

void
grasp_manager::testGrasps(){
    /* test all generated directions */
    myTester->callTestIt(graspList, renderIt);
    graspsNotShown=1;
}

void
grasp_manager::compareGraspListsByDist(std::list<plannedGrasp*>& newList, std::list<plannedGrasp*> criteriaList){
    std::list<plannedGrasp*>::iterator it = newList.begin();
    std::list<plannedGrasp*>::iterator it_tmp;
    std::list<plannedGrasp*>::iterator cr_l_end = criteriaList.end();
    
    bool doErase;
    int size = newList.size();

    if (criteriaList.empty() || newList.empty())
      return;

    for(int i=0; i<size; i++){
      
      doErase = true;
      
      for (std::list<plannedGrasp*>::iterator cr_it = criteriaList.begin(); cr_it != cr_l_end; cr_it++){
        if((*cr_it)->get_quality() >= itQualThresh){
          double dist = double((*cr_it)->distanceTo(**it));
#ifdef GRASPITDBG
          std::cout << "PL_OUT: Dist is " << dist << std::endl;
#endif
          if (dist <= maxdist){
            doErase = false;
            break;
          }
        }
      }
      
      if (doErase){
        it_tmp = it;
        if (it!=newList.end())
          it++;
        delete (*it_tmp);
        newList.erase(it_tmp);// delete grasp from list
      }
      else if (it!=newList.end())
        it++;
      else break;
    }
    return;
}

//  void 
//  grasp_manager::changePlanningParameters(int step){
    
//      if(step == 0)
//      return;

//      int nr_of_360_deg_steps_in, 
//      nr_of_parallel_planes_in,
//      nr_of_180_deg_grasps_in,
//      nr_of_grasp_rotations_in;
//      myPlanner->get_planningParameters(nr_of_360_deg_steps_in, 
//                                    nr_of_parallel_planes_in,
//                                    nr_of_180_deg_grasps_in,
//                                    nr_of_grasp_rotations_in);
//      myPlanner->set_planningParameters(nr_of_360_deg_steps_in + DEG_STEPS_STEP, 
//                                    nr_of_parallel_planes_in + PAR_PLANES_STEP,
//                                    DEG_180_STEP,
//                                    nr_of_grasp_rotations_in + ROTATIONS_STEP);
//      if(maxdist > 0.0)
//      if (maxdist > 5.0)
//          maxdist-=2.0;
//      else if (maxdist > 3.0)
//          maxdist-=1.0;
//      else if (maxdist > 1.0)
//          maxdist-=0.5;
//      std::cout << "PL_OUT: MaxDist " << maxdist << std::endl;
//  }

void 
grasp_manager::showGrasps(int next)
{

  if (!graspList.empty()){
    
    if (graspsNotShown){
      
      /* hand the list over to the presenter */
      myPresent->takeList(graspList);
      graspsNotShown=0;
      nrOfStableGrasps = graspList.size();
      //            computingTime = myTester->get_computingTime();
#ifdef GRASPITDBG
      std::cout << "PL_OUT: " << std::endl << "PL_OUT: " << std::endl;
      std::cout << "PL_OUT: Result grasp generation:" << std::endl;
      std::cout << "PL_OUT: Nr of tested grasps " << nrOfPlannedGrasps << std::endl;
      std::cout << "PL_OUT: Nr of found stable grasps " << nrOfStableGrasps << std::endl;
      
      /* std::cout << "PL_OUT: Testing time: " 
         << (double)computingTime.tv_sec+(double)computingTime.tv_usec/1000000.0 
         << " seconds." << std::endl;
         if (nrOfPlannedGrasps)
         std::cout << "PL_OUT: That is " 
         << ((double)computingTime.tv_sec+(double)computingTime.tv_usec/1000000.0)/(double)nrOfPlannedGrasps 
         << " seconds per planned grasp." << std::endl;
         if (nrOfStableGrasps)
         std::cout << "PL_OUT: And " 
         << ((double)computingTime.tv_sec+(double)computingTime.tv_usec/1000000.0)/(double)nrOfStableGrasps 
         << " seconds per stable grasp." << std::endl;
      */
      std::cout << "PL_OUT: " << std::endl;
      std::cout << "PL_OUT: " << std::endl;
#endif
        }

    /* now present the best ones to the user */
    myPresent->showGrasp(next, renderIt);
  }

#ifdef GRASPITDBG
  else std::cout << "PL_OUT: No grasps planned yet. Nothing to show." << std::endl;
#endif
}

void 
grasp_manager::chooseGrasp(){
    myPresent->chooseGrasp();
}

void 
grasp_manager::set_render(bool in){
    renderIt = in;
}

bool 
grasp_manager::get_render()const{
    return renderIt;
}

void 
grasp_manager::set_doIteration(bool in){
    doIteration = in;
}

bool 
grasp_manager::get_doIteration()const{
    return doIteration;
}

bool 
grasp_manager::set_iterationParameters(double a,double b){
    if(a>1.0 || a<0.0 || b>1.0 || b<0.0)
        return false;
    itQualThresh = a;
    maxdist = b;
    return true;
}

void 
grasp_manager::get_iterationParameters(double& a,double& b)const{
    a = itQualThresh;
    b = maxdist;
    return;
}

grasp_planner* 
grasp_manager::get_graspPlanner()const{
    return myPlanner;
}

grasp_tester*  
grasp_manager::get_graspTester()const{
    return myTester;
}

---

graspit
Author(s):
autogenerated on Wed Jan 25 10:58:53 2012