graspit: 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   for (int i=0; i<mHand->getEigenGrasps()->getSize(); i++) {
00060     QString name("EG ");
00061     QString num;
00062     num.setNum(i);
00063     float min,max;
00064     if (mHand->getEigenGrasps()->getGrasp(i)->mPredefinedLimits) {
00065       min = mHand->getEigenGrasps()->getGrasp(i)->mMin;
00066       max = mHand->getEigenGrasps()->getGrasp(i)->mMax;
00067     } else if ( mHand->isA("Pr2Gripper") ) {
00068       min = -0.6f;
00069       max = 0.6f;
00070     } else if ( mHand->isA("Pr2Gripper2010") ) {
00071       min = -0.45f;
00072       max = 0.45f;
00073     } else {
00074       //if limits are not pre-defined in files, we use these hard-coded values
00075       //as we don't actually trust our abilities to compute them
00076       min = -0.4f;
00077       max = 0.4f;
00078     }
00079     float jump  = (max-min) / 4.0;
00080     mVariables.push_back( new SearchVariable(name+num, min, max, jump) );
00081   }
00082 }
00083 void PostureStateEigen::getHandDOF(double *dof) const
00084 {
00085         double *eg = new double[ mHand->getEigenGrasps()->getSize() ];
00086         for (int i=0; i < mHand->getEigenGrasps()->getSize(); i++) {
00087                 eg[i] = readVariable(i);
00088         }
00089         bool r = mHand->getEigenGrasps()->isRigid();
00090         mHand->getEigenGrasps()->setRigid(true);
00091         mHand->getEigenGrasps()->getDOF( eg, dof );
00092         mHand->checkSetDOFVals(dof);
00093         mHand->getEigenGrasps()->setRigid(r);
00094         delete [] eg;
00095 }
00096 void PostureStateEigen::storeHandDOF(const double *dof)
00097 {
00098         double *eg = new double[ mHand->getEigenGrasps()->getSize() ];
00099         mHand->getEigenGrasps()->getAmp(eg, dof);
00100         for (int i=0; i < mHand->getEigenGrasps()->getSize(); i++) {
00101                 getVariable(i)->setValue(eg[i]);
00102         }
00103         delete [] eg;
00104 }
00105 
00106 // ------------------------------------ POSITION STATES -------------------------------
00107 
00108 void PositionStateComplete::createVariables()
00109 {
00110         mVariables.push_back( new SearchVariable("Tx",-250,250,100) );
00111         mVariables.push_back( new SearchVariable("Ty",-250,250,100) );
00112         mVariables.push_back( new SearchVariable("Tz",-250,250,100) );
00113         mVariables.push_back( new SearchVariable("Qw",-5, 5, 1) );
00114         mVariables.push_back( new SearchVariable("Qx",-5, 5, 1) );
00115         mVariables.push_back( new SearchVariable("Qy",-5, 5, 1) );
00116         mVariables.push_back( new SearchVariable("Qz",-5, 5, 1) );
00117 }
00118 
00125 transf PositionStateComplete::getCoreTran() const
00126 {
00127         double tx = readVariable("Tx");
00128         double ty = readVariable("Ty");
00129         double tz = readVariable("Tz");
00130         double qw = readVariable("Qw");
00131         double qx = readVariable("Qx");
00132         double qy = readVariable("Qy");
00133         double qz = readVariable("Qz");
00134         return transf(Quaternion(qw,qx,qy,qz), vec3(tx,ty,tz)); 
00135 }
00136 void PositionStateComplete::setTran(const transf &t)
00137 {
00138         getVariable("Tx")->setValue( t.translation().x() );
00139         getVariable("Ty")->setValue( t.translation().y() );
00140         getVariable("Tz")->setValue( t.translation().z() );
00141         getVariable("Qw")->setValue( t.rotation().w );
00142         getVariable("Qx")->setValue( t.rotation().x );
00143         getVariable("Qy")->setValue( t.rotation().y );
00144         getVariable("Qz")->setValue( t.rotation().z );
00145 }
00146 
00147 void PositionStateAA::createVariables()
00148 {
00149         mVariables.push_back( new SearchVariable("Tx",-250,250,150) );
00150         mVariables.push_back( new SearchVariable("Ty",-250,250,150) );
00151         //variable limit hacked for search with Pr2Gripper2010 for tall objects
00152         mVariables.push_back( new SearchVariable("Tz",-250,350,150) );
00153         mVariables.push_back( new SearchVariable("theta", 0, M_PI, M_PI/5) );
00154         mVariables.push_back( new SearchVariable("phi", -M_PI, M_PI, M_PI/2, true) );
00155         mVariables.push_back( new SearchVariable("alpha",0, M_PI, M_PI/2) );
00156 }
00157 transf PositionStateAA::getCoreTran() const
00158 {
00159         double tx = readVariable("Tx");
00160         double ty = readVariable("Ty");
00161         double tz = readVariable("Tz");
00162         double theta = readVariable("theta");
00163         double phi = readVariable("phi");
00164         double alpha = readVariable("alpha");
00165         transf coreTran = rotate_transf(alpha, vec3( sin(theta)*cos(phi) , sin(theta)*sin(phi) , cos(theta) )) *
00166                    translate_transf(vec3(tx,ty,tz));
00167         //transform now returned relative to hand approach transform
00168         return mHand->getApproachTran().inverse() * coreTran;
00169 
00170 }
00171 void PositionStateAA::setTran(const transf &t)
00172 {
00173   transf rt = mHand->getApproachTran() * t;
00174 
00175         //translation is direct
00176         getVariable("Tx")->setValue( rt.translation().x() );
00177         getVariable("Ty")->setValue( rt.translation().y() );
00178         getVariable("Tz")->setValue( rt.translation().z() );
00179 
00180         vec3 axis; double angle;
00181         rt.rotation().ToAngleAxis(angle,axis);
00182         if (angle < 0) {
00183           angle = -angle;
00184           axis = -1.0 * axis;
00185         }
00186         if (angle > M_PI) {
00187           angle = 2 * M_PI - angle;
00188           axis = -1.0 * axis;
00189         }
00190         assert(0<=angle);
00191         assert(angle <= M_PI);
00192         //the angle
00193         getVariable("alpha")->setValue(angle);
00194         //the axis
00195         if ( fabs(axis.x()) > 1.0e-7 || fabs(axis.y()) > 1.0e-7) {
00196           getVariable("phi")->setValue( atan2(axis.y(), axis.x()) );
00197           double xy_len = sqrt( axis.x()*axis.x() + axis.y()*axis.y() );
00198           getVariable("theta")->setValue( atan2(xy_len, axis.z()) );
00199         } else {
00200           getVariable("phi")->setValue(0.0);
00201           getVariable("theta")->setValue(M_PI / 2.0);
00202         }
00203 }
00204 
00205 void PositionStateEllipsoid::createVariables()
00206 {
00207         mVariables.push_back( new SearchVariable("beta",-M_PI/2.0, M_PI/2.0, M_PI/2.0) );
00208         mVariables.push_back( new SearchVariable("gamma",-M_PI, M_PI, M_PI, true) );
00209         mVariables.push_back( new SearchVariable("tau",-M_PI, M_PI, M_PI, true) );
00210         mVariables.push_back( new SearchVariable("dist",-50, 100, 50) );
00211 
00212         //ellipsoid scaling parameters
00213         mParameters.push_back(SearchParameter("a",80));
00214         mParameters.push_back(SearchParameter("b",80));
00215         mParameters.push_back(SearchParameter("c",160));
00216 }
00217 transf PositionStateEllipsoid::getCoreTran() const
00218 {
00219         double a = getParameter("a");
00220         double b = getParameter("b");
00221         double c = getParameter("c");
00222         double beta = readVariable("beta");
00223         double gamma = readVariable("gamma");
00224         double tau = readVariable("tau");
00225         double distance = readVariable("dist");
00226         double px,py,pz;
00227         px =  a * cos(beta) * cos(gamma);
00228         py =  b * cos(beta) * sin(gamma);
00229         pz =  c * sin(beta);
00230         
00231         //compute normal direction - for some reason this always points INSIDE the ellipsoid
00232         vec3 n1(  -a*sin(beta)*cos(gamma), -b*sin(beta)*sin(gamma), c*cos(beta) );
00233         vec3 n2(  -a*cos(beta)*sin(gamma),  b*cos(beta)*cos(gamma), 0 );
00234         vec3 normal = normalise(n1) * normalise(n2);
00235 
00236         vec3 xdir(1,0,0);
00237         vec3 ydir = normal * normalise(xdir);
00238         xdir = ydir * normal;
00239         mat3 r(xdir,ydir,normal);
00240 
00241         transf handTran = transf(r, vec3(px,py,pz) - distance * normal);
00242         Quaternion zrot(tau, vec3(0,0,1));
00243         handTran = transf(zrot, vec3(0,0,0) ) * handTran;
00244         return mHand->getApproachTran().inverse() * handTran;
00245         //So: hand tranform is: move onto the ellipsoid and rotate z axis in normal direction
00246         //        --> hand approach transform inverted to get to hand origin
00247 }
00248 void PositionStateEllipsoid::setTran(const transf &t)
00249 {
00250         //not yet implemented
00251         const_cast<transf &>(t) = t; //kill the warning
00252         assert(0);
00253 }
00254 
00255 void PositionStateApproach::createVariables()
00256 {
00257         mVariables.push_back( new SearchVariable("dist",-30, 200, 100) );
00258         mVariables.push_back( new SearchVariable("wrist 1",-M_PI/3.0, M_PI/3.0, M_PI/6.0) );
00259         mVariables.push_back( new SearchVariable("wrist 2",-M_PI/3.0, M_PI/3.0, M_PI/6.0) );
00260 }
00261 transf PositionStateApproach::getCoreTran() const
00262 {
00263         double dist = readVariable("dist");
00264         double rx = readVariable("wrist 1");
00265         double ry = readVariable("wrist 2");
00266         transf handTran = transf(Quaternion::IDENTITY, vec3(0,0,dist));
00267         handTran = handTran * rotate_transf(rx, vec3(1,0,0) ) * rotate_transf( ry, vec3(0,1,0) );
00268         return mHand->getApproachTran().inverse() * handTran * mHand->getApproachTran();
00269 }
00270 void PositionStateApproach::setTran(const transf &t)
00271 {
00272         //not yet implemented
00273         const_cast<transf &>(t) = t; //kill the warning
00274         assert(0);
00275 }