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

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

#include <math.h>
#include <iostream>

#include "grasp_preshape.h"

preshape::preshape(){
    pType = PR_None;
    updateAngles();
}

preshape::preshape(preshapeType p){
    pType = p;
    updateAngles();
}

preshape::preshape(const preshape& p){
    pType = p.get_preshapeType();
    p.get_preshape(a, f1, f2, f3);
    updateAngles();
}

void 
preshape::set_preshapeType(preshapeType p){
    pType = p;
    updateAngles();
}

void 
preshape::set_preshape(double aa, double ff1, double ff2, double ff3){

  pType = PR_None;
  a=aa; f1 = ff1; f2 = ff2; f3=ff3;
}

preshapeType
preshape::get_preshapeType() const{
    return pType;
}

void 
preshape::get_preshape(double& aa, double& ff1, double& ff2, double& ff3) const{
    aa = a;
    ff1 = f1;
    ff2 = f2;
    ff3 = f3;
    return;
}

double 
preshape::distanceTo(preshape p) const{
    double aa,ff1,ff2,ff3;
    p.get_preshape(aa,ff1,ff2,ff3);
    if((aa*aa+ff1*ff1+ff2*ff2+ff3*ff3) == 0.0 &&
       (a*a  +f1*f1  +f2*f2  +f3*f3) == 0.0)
        return 0.0;
//      return (sqrt((aa-a)*(aa-a)+
//               (ff1-f1)*(ff1-f1)+
//               (ff2-f2)*(ff2-f2)+
//               (ff3-f3)*(ff3-f3)) / 
//      ((aa*aa+ff1*ff1+ff2*ff2+ff3*ff3) >
//       (a*a  +f1*f1  +f2*f2  +f3*f3)   ?
//       (aa*aa+ff1*ff1+ff2*ff2+ff3*ff3) :
//       (a*a  +f1*f1  +f2*f2  +f3*f3)));
    /* Should be a-aa/max here, but max doesnt exist in this class 
       as well as the hand and I'm too lazy to implement that
       now... */
    return fabs((a-aa)/(a>aa?a:aa));
}

void
preshape::updateAngles(){
    if (pType != PR_None){
        switch (pType) {
            case PR_circle:
                a  = PR_circle_Angle;
                f1 = PR_circle_F1;
                f2 = PR_circle_F2;
                f3 = PR_circle_F3;
                break;
            case PR_two_opp_one:
                a =  PR_two_opp_one_Angle;
                f1 = PR_two_opp_one_F1;
                f2 = PR_two_opp_one_F2;
                f3 = PR_two_opp_one_F3;
                break;
            case PR_three_opp_palm:
                a =  PR_three_opp_palm_Angle;
                f1 = PR_three_opp_palm_F1;
                f2 = PR_three_opp_palm_F2;
                f3 = PR_three_opp_palm_F3;
                break;
            case PR_t_shape:
                a =  PR_t_shape_Angle;
                f1 = PR_t_shape_F1;
                f2 = PR_t_shape_F2;
                f3 = PR_t_shape_F3;
                break;
            default:
                std::cout << "Preshape not defined or implemented." << std::endl;
                break;
        }
    }
}

---

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