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

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

/* 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/SoRotation.h>
#include <Inventor/nodes/SoCamera.h>
#include <Inventor/nodes/SoTransformSeparator.h>
#include <Inventor/nodes/SoDirectionalLight.h>
#include <Inventor/sensors/SoIdleSensor.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/SbColor.h>
#include <Inventor/nodes/SoTransform.h>


#include "body.h"
#include "matvec3D.h"
#include "grasp_coordinates.h"
#include "grasp_directions.h"
#include "grasp_preshape.h"
#include "grasp_visualization.h"
#include "grasp_grasps.h"



plannedGrasp::plannedGrasp(){
    myGraspDir = cartesianGraspDirection();
    myPreshape = PR_None;
    finalGPos = finalGraspPosition();
    myQuality = -1.;
    myCP = NULL;
}

plannedGrasp::plannedGrasp(const plannedGrasp& in){
    set_graspDirection(in.get_graspDirection());
    set_fixedFingerDirection(in.get_fixedFingerDirection());
    myQuality = in.get_quality();
    myPreshape = in.get_preshape();
    myGraspableBody = in.get_graspableBody();
    finalGPos = in.get_finalGraspPosition();
    myCP = in.get_graspRepresentation();
}

plannedGrasp::plannedGrasp(cartesianGraspDirection in){
    set_graspDirection(in);
    myPreshape = PR_None;
    myQuality = -1.;
    myCP = NULL;
}

plannedGrasp::~plannedGrasp(){
    if (myCP != NULL)
        delete myCP;
}

double
plannedGrasp::distanceTo(plannedGrasp to) const {
    return (DIST_DIR_WEIGHT   * myGraspDir.distanceTo(to.get_graspDirection()) +                 
            DIST_FFD_WEIGHT   * fixedFingerDirection.distanceTo(to.get_fixedFingerDirection()) + 
            DIST_PRESH_WEIGHT * myPreshape.distanceTo(to.get_preshape())) /                      
        (DIST_DIR_WEIGHT + DIST_FFD_WEIGHT + DIST_PRESH_WEIGHT);
}


cartesianGraspDirection 
plannedGrasp::get_graspDirection()const{
    return myGraspDir;
}

void           
plannedGrasp::set_graspDirection(cartesianGraspDirection in){
    myGraspDir.set_point(in.get_point());
    myGraspDir.set_dir(in.get_dir());
    myGraspDir.set_empty(in.get_empty());
    myGraspDir.set_gdType(in.get_gdType());
    return;
}

cartesian_coordinates 
plannedGrasp::get_fixedFingerDirection()const{
    return fixedFingerDirection;
}

void           
plannedGrasp::set_fixedFingerDirection(cartesian_coordinates in){
    fixedFingerDirection = in;
    return;
}

finalGraspPosition
plannedGrasp::get_finalGraspPosition()const{
    return finalGPos;
}

void           
plannedGrasp::set_finalGraspPosition(finalGraspPosition in){
    finalGPos = in;
    return;
}

//  bool           
//  plannedGrasp::get_isReachable()const{
//      return isReachable;
//  }

//  void           
//  plannedGrasp::set_isReachable(bool in){
//      isReachable = in;
//      return;
//  }

double         
plannedGrasp::get_quality()const{
    return myQuality;
}

void           
plannedGrasp::set_quality(double in){
    myQuality = in;
    return;
}

preshape
plannedGrasp::get_preshape()const{
    return myPreshape;
}

void           
plannedGrasp::set_preshape(preshape in){
    myPreshape = in;
}

GraspableBody* 
plannedGrasp::get_graspableBody()const{
    return myGraspableBody;
}

void           
plannedGrasp::set_graspableBody(GraspableBody* gb){
    myGraspableBody = gb;
    return;
}

grasp_representation*
plannedGrasp::get_graspRepresentation()const{
    return myCP;
}

void           
plannedGrasp::set_graspRepresentation(grasp_representation* gr){
    myCP = gr;
    return;
}

void 
plannedGrasp::remove_graspRepresentation(){
    delete myCP;
    myCP = NULL;
    return;
}

#ifdef WIN32
bool
compareGraspQM::operator()(plannedGrasp* &first, plannedGrasp* &second) const
{
    if (first->get_quality() > second->get_quality())
        return true;
    return false;
}
#else
bool
compareGraspQM::operator()(plannedGrasp* &first, plannedGrasp* &second) const
{
    if (first->get_quality() < second->get_quality())
        return true;
    return false;
}
#endif

/*****************/

finalGraspPosition::finalGraspPosition(){
}

finalGraspPosition::finalGraspPosition(const finalGraspPosition& in){
    finalTran = in.get_finalTran();
    dof = in.get_dof();
}

finalGraspPosition::~finalGraspPosition(){
    dof.clear();
}

transf 
finalGraspPosition::get_finalTran()const{
    return finalTran;
}

void   
finalGraspPosition::set_finalTran(transf in){
    finalTran = in;
    return;
}

std::list<double>
finalGraspPosition::get_dof()   const{
    return dof;
}

void
finalGraspPosition::add_dof(double in){
    dof.push_back(in);
}

bool   
finalGraspPosition::change_dof(unsigned int nr, double in){
    if (nr<dof.size()){
        std::list<double>::iterator it = dof.begin();
        for (unsigned int i=0; i<nr; i++)
            it++;
        (*it) = in;
        return true;
    }
    else return false;
}
 

/******************
   Local Variables:
   mode:c++
   End:
******************/

---

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