graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/EGPlanner/searchStateImpl.cpp Source File

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: searchStateImpl.cpp,v 1.12 2010/01/14 01:21:09 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "searchStateImpl.h"
00027 #include "robot.h"
00028 #include "eigenGrasp.h"
00029 
00030 #include "debug.h"
00031 
00032 void PostureStateDOF::createVariables()
00033 {
00034         QString name("DOF ");
00035         QString num;
00036         for (int i=0; i<mHand->getNumDOF(); i++) {
00037                 num.setNum(i);
00038                 mVariables.push_back( new SearchVariable(name+num, 
00039                                                          mHand->getDOF(i)->getMin(), mHand->getDOF(i)->getMax(),
00040                                                          0.5 * (mHand->getDOF(i)->getMax() 
00041                                                                 - mHand->getDOF(i)->getMin()) ));
00042         }
00043 }
00044 void PostureStateDOF::getHandDOF(double *dof) const
00045 {
00046         for (int i=0; i<mHand->getNumDOF(); i++) {
00047                 dof[i] = readVariable(i);
00048         }
00049 }
00050 void PostureStateDOF::storeHandDOF(const double *dof)
00051 {
00052         for (int i=0; i<mHand->getNumDOF(); i++) {
00053                 getVariable(i)->setValue(dof[i]);
00054         }
00055 }
00056 
00057 void PostureStateEigen::createVariables()
00058 {
00059         QString name("EG ");
00060         QString num;
00061         float min,max,jump;
00062 
00063         //this is a horrible hack to adapt EG range to the characteristics of the hand
00064         //should really compute these based on DOF max / min, but for some reason
00065         //I've never been able to do that properly
00066         if ( mHand->isA("Pr2Gripper") ) {
00067                 min = -0.6f;
00068                 max = 0.6f;
00069                 jump = (max-min) / 4.0;
00070         } else  if ( mHand->isA("Pr2Gripper2010") ) {
00071                 min = -0.45f;
00072                 max = 0.45f;
00073                 jump = (max-min) / 4.0;
00074         } else {
00075                 min = -4.0f;
00076                 max = 4.0f;
00077                 jump = (max-min) / 4.0;
00078         }
00079 
00080         for (int i=0; i<mHand->getEigenGrasps()->getSize(); i++) {
00081                 num.setNum(i);
00082                 mVariables.push_back( new SearchVariable(name+num, min, max, jump) );
00083         }
00084 }
00085 void PostureStateEigen::getHandDOF(double *dof) const
00086 {
00087         double *eg = new double[ mHand->getEigenGrasps()->getSize() ];
00088         for (int i=0; i < mHand->getEigenGrasps()->getSize(); i++) {
00089                 eg[i] = readVariable(i);
00090         }
00091         bool r = mHand->getEigenGrasps()->isRigid();
00092         mHand->getEigenGrasps()->setRigid(true);
00093         mHand->getEigenGrasps()->getDOF( eg, dof );
00094         mHand->checkSetDOFVals(dof);
00095         mHand->getEigenGrasps()->setRigid(r);
00096         delete [] eg;
00097 }
00098 void PostureStateEigen::storeHandDOF(const double *dof)
00099 {
00100         double *eg = new double[ mHand->getEigenGrasps()->getSize() ];
00101         mHand->getEigenGrasps()->getAmp(eg, dof);
00102         for (int i=0; i < mHand->getEigenGrasps()->getSize(); i++) {
00103                 getVariable(i)->setValue(eg[i]);
00104         }
00105         delete [] eg;
00106 }
00107 
00108 // ------------------------------------ POSITION STATES -------------------------------
00109 
00110 void PositionStateComplete::createVariables()
00111 {
00112         mVariables.push_back( new SearchVariable("Tx",-250,250,100) );
00113         mVariables.push_back( new SearchVariable("Ty",-250,250,100) );
00114         mVariables.push_back( new SearchVariable("Tz",-250,250,100) );
00115         mVariables.push_back( new SearchVariable("Qw",-5, 5, 1) );
00116         mVariables.push_back( new SearchVariable("Qx",-5, 5, 1) );
00117         mVariables.push_back( new SearchVariable("Qy",-5, 5, 1) );
00118         mVariables.push_back( new SearchVariable("Qz",-5, 5, 1) );
00119 }
00120 
00127 transf PositionStateComplete::getCoreTran() const
00128 {
00129         double tx = readVariable("Tx");
00130         double ty = readVariable("Ty");
00131         double tz = readVariable("Tz");
00132         double qw = readVariable("Qw");
00133         double qx = readVariable("Qx");
00134         double qy = readVariable("Qy");
00135         double qz = readVariable("Qz");
00136         return transf(Quaternion(qw,qx,qy,qz), vec3(tx,ty,tz)); 
00137 }
00138 void PositionStateComplete::setTran(const transf &t)
00139 {
00140         getVariable("Tx")->setValue( t.translation().x() );
00141         getVariable("Ty")->setValue( t.translation().y() );
00142         getVariable("Tz")->setValue( t.translation().z() );
00143         getVariable("Qw")->setValue( t.rotation().w );
00144         getVariable("Qx")->setValue( t.rotation().x );
00145         getVariable("Qy")->setValue( t.rotation().y );
00146         getVariable("Qz")->setValue( t.rotation().z );
00147 }
00148 
00149 void PositionStateAA::createVariables()
00150 {
00151         mVariables.push_back( new SearchVariable("Tx",-250,250,150) );
00152         mVariables.push_back( new SearchVariable("Ty",-250,250,150) );
00153         //variable limit hacked for search with Pr2Gripper2010 for tall objects
00154         mVariables.push_back( new SearchVariable("Tz",-250,350,150) );
00155         mVariables.push_back( new SearchVariable("theta", 0, M_PI, M_PI/5) );
00156         mVariables.push_back( new SearchVariable("phi", -M_PI, M_PI, M_PI/2, true) );
00157         mVariables.push_back( new SearchVariable("alpha",0, M_PI, M_PI/2) );
00158 }
00159 transf PositionStateAA::getCoreTran() const
00160 {
00161         double tx = readVariable("Tx");
00162         double ty = readVariable("Ty");
00163         double tz = readVariable("Tz");
00164         double theta = readVariable("theta");
00165         double phi = readVariable("phi");
00166         double alpha = readVariable("alpha");
00167         transf coreTran = rotate_transf(alpha, vec3( sin(theta)*cos(phi) , sin(theta)*sin(phi) , cos(theta) )) *
00168                    translate_transf(vec3(tx,ty,tz));
00169         //transform now returned relative to hand approach transform
00170         return mHand->getApproachTran().inverse() * coreTran;
00171 
00172 }
00173 void PositionStateAA::setTran(const transf &t)
00174 {
00175   transf rt = mHand->getApproachTran() * t;
00176 
00177         //translation is direct
00178         getVariable("Tx")->setValue( rt.translation().x() );
00179         getVariable("Ty")->setValue( rt.translation().y() );
00180         getVariable("Tz")->setValue( rt.translation().z() );
00181 
00182         vec3 axis; double angle;
00183         rt.rotation().ToAngleAxis(angle,axis);
00184         if (angle < 0) {
00185           angle = -angle;
00186           axis = -1.0 * axis;
00187         }
00188         if (angle > M_PI) {
00189           angle = 2 * M_PI - angle;
00190           axis = -1.0 * axis;
00191         }
00192         assert(0<=angle);
00193         assert(angle <= M_PI);
00194         //the angle
00195         getVariable("alpha")->setValue(angle);
00196         //the axis
00197         if ( fabs(axis.x()) > 1.0e-7 || fabs(axis.y()) > 1.0e-7) {
00198           getVariable("phi")->setValue( atan2(axis.y(), axis.x()) );
00199           double xy_len = sqrt( axis.x()*axis.x() + axis.y()*axis.y() );
00200           getVariable("theta")->setValue( atan2(xy_len, axis.z()) );
00201         } else {
00202           getVariable("phi")->setValue(0.0);
00203           getVariable("theta")->setValue(M_PI / 2.0);
00204         }
00205 }
00206 
00207 void PositionStateEllipsoid::createVariables()
00208 {
00209         mVariables.push_back( new SearchVariable("beta",-M_PI/2.0, M_PI/2.0, M_PI/2.0) );
00210         mVariables.push_back( new SearchVariable("gamma",-M_PI, M_PI, M_PI, true) );
00211         mVariables.push_back( new SearchVariable("tau",-M_PI, M_PI, M_PI, true) );
00212         mVariables.push_back( new SearchVariable("dist",-50, 100, 50) );
00213 
00214         //ellipsoid scaling parameters
00215         mParameters.push_back(SearchParameter("a",80));
00216         mParameters.push_back(SearchParameter("b",80));
00217         mParameters.push_back(SearchParameter("c",160));
00218 }
00219 transf PositionStateEllipsoid::getCoreTran() const
00220 {
00221         double a = getParameter("a");
00222         double b = getParameter("b");
00223         double c = getParameter("c");
00224         double beta = readVariable("beta");
00225         double gamma = readVariable("gamma");
00226         double tau = readVariable("tau");
00227         double distance = readVariable("dist");
00228         double px,py,pz;
00229         px =  a * cos(beta) * cos(gamma);
00230         py =  b * cos(beta) * sin(gamma);
00231         pz =  c * sin(beta);
00232         
00233         //compute normal direction - for some reason this always points INSIDE the ellipsoid
00234         vec3 n1(  -a*sin(beta)*cos(gamma), -b*sin(beta)*sin(gamma), c*cos(beta) );
00235         vec3 n2(  -a*cos(beta)*sin(gamma),  b*cos(beta)*cos(gamma), 0 );
00236         vec3 normal = normalise(n1) * normalise(n2);
00237 
00238         vec3 xdir(1,0,0);
00239         vec3 ydir = normal * normalise(xdir);
00240         xdir = ydir * normal;
00241         mat3 r(xdir,ydir,normal);
00242 
00243         transf handTran = transf(r, vec3(px,py,pz) - distance * normal);
00244         Quaternion zrot(tau, vec3(0,0,1));
00245         handTran = transf(zrot, vec3(0,0,0) ) * handTran;
00246         return mHand->getApproachTran().inverse() * handTran;
00247         //So: hand tranform is: move onto the ellipsoid and rotate z axis in normal direction
00248         //        --> hand approach transform inverted to get to hand origin
00249 }
00250 void PositionStateEllipsoid::setTran(const transf &t)
00251 {
00252         //not yet implemented
00253         (void*)&t; //kill the warning
00254         assert(0);
00255 }
00256 
00257 void PositionStateApproach::createVariables()
00258 {
00259         mVariables.push_back( new SearchVariable("dist",-30, 200, 100) );
00260         mVariables.push_back( new SearchVariable("wrist 1",-M_PI/3.0, M_PI/3.0, M_PI/6.0) );
00261         mVariables.push_back( new SearchVariable("wrist 2",-M_PI/3.0, M_PI/3.0, M_PI/6.0) );
00262 }
00263 transf PositionStateApproach::getCoreTran() const
00264 {
00265         double dist = readVariable("dist");
00266         double rx = readVariable("wrist 1");
00267         double ry = readVariable("wrist 2");
00268         transf handTran = transf(Quaternion::IDENTITY, vec3(0,0,dist));
00269         handTran = handTran * rotate_transf(rx, vec3(1,0,0) ) * rotate_transf( ry, vec3(0,1,0) );
00270         return mHand->getApproachTran().inverse() * handTran * mHand->getApproachTran();
00271 }
00272 void PositionStateApproach::setTran(const transf &t)
00273 {
00274         //not yet implemented
00275         (void*)&t; //kill the warning
00276         assert(0);
00277 }