/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/Planner/grasp_presenter.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_presenter.cpp,v 1.3 2009/03/25 22:10:05 cmatei Exp $
//
//######################################################################

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

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


#include <qstring.h>

/* inventor includes */
#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
#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/SoRotation.h>
#include <Inventor/nodes/SoCamera.h>
#include <Inventor/nodes/SoTransformSeparator.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoDirectionalLight.h>

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

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

/* STL includes */
#include <list>

/* include grasp planner class */
#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_presenter.h"

/* externs; defined in main.cc */
extern IVmgr *ivmgr;

grasp_presenter::grasp_presenter(){
    processing = -1;
    my_hand = NULL;
#ifdef GRASPITDBG
    std::cout << "PL_OUT: Presenter created." << std::endl;
#endif
}

grasp_presenter::~grasp_presenter(){
#ifdef GRASPITDBG
    std::cout << "PL_OUT: Presenter destroyed." << std::endl;
#endif
}


void
grasp_presenter::updateGlobals(){
    ivmgr    = graspItGUI->getIVmgr();
    myViewer = ivmgr->getViewer();
    my_world = ivmgr->getWorld();
    my_hand  = my_world->getCurrentHand();
}

void
grasp_presenter::takeList(std::list<plannedGrasp*> graspList_in){
    graspList = graspList_in;
    processing = -1;
}

void
grasp_presenter::showGrasp(int next, bool render){

    if (!graspList.empty()){
        updateGlobals();

        /* Here, a separate window should be opened with copies of
           the hand, the robot, the object and all obstacles */


        /* Show all grasps beginning with the best */
        if (processing == -1){
            it_gr = graspList.begin();
            processing = 0;
        }

        if (next >=0) {
#ifdef GRASPITDBG
          std::cout << "PL_OUT: Showing next grasp." << std::endl;
        }
        else {
          std::cout<<"PL_OUT: Previous not implemented. Showing next instead." << std::endl;
#endif
        }

        if ((*it_gr)->get_quality() > 0.0){
            putHand((*it_gr)->get_finalGraspPosition(), render);
#ifdef GRASPITDBG
            std::cout << "PL_OUT: Grasp Nr " << processing << std::endl;
            std::cout << "PL_OUT: Quality: " << (*it_gr)->get_quality() << std::endl;
#endif
            myViewer->render();
        }
        it_gr++;
        processing++;
        if (it_gr == graspList.end()){
            it_gr = graspList.begin();
            processing = 0;
        }
    }
#ifdef GRASPITDBG
    else std::cout << "PL_OUT: No grasps planned yet. There's nothin to show." << std::endl;
#endif
}

void
grasp_presenter::chooseGrasp(){
#ifdef GRASPITDBG
    std::cout << "PL_OUT: YEAH, actual grasp chosen. Congratulations!" << std::endl;
#endif
}

void 
grasp_presenter::putHand(finalGraspPosition fgp, bool render){

    std::list<double> tmp = fgp.get_dof();
    std::list<double>::iterator it = tmp.begin();

    my_hand->setTran(fgp.get_finalTran());

    for (int i=0; i<my_hand->getNumDOF(); i++){
        my_hand->forceDOFVal(i,*it);
        if ((my_hand->getName() == "Barrett") && i>0)
            my_hand->forceDOFVal(i,my_hand->getDOF(i)->getMin());
        if (it != tmp.end())
            it++;
    }
    if (my_hand->getName() == "Barrett") {
        my_hand->autoGrasp(render,50);
    } 

    my_world->findAllContacts();
    my_world->updateGrasps();
        
}

---

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