/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/pincer.cpp
Go to the documentation of this file.00001 //###################################################################### 00002 // 00003 // GraspIt! 00004 // Copyright (C) 2002-2004 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: Andrew T. Miller (amiller@cs.columbia.edu) 00021 // 00022 // $Id: pincer.cpp,v 1.2 2009/07/23 21:32:18 cmatei Exp $ 00023 // 00024 //###################################################################### 00025 00030 #include "pincer.h" 00031 #include "world.h" 00032 #include "grasp.h" 00033 00040 void 00041 Pincer::DOFController(double timeStep) 00042 { 00043 int d,c,j,l; 00044 Link *prevLink,*nextLink; 00045 position jointPos; 00046 vec3 jointAxis; 00047 transf jointTran; 00048 bool dofDone[numDOF]; 00049 int numContacts; 00050 00051 #ifdef GRASPITDBG 00052 std::cout << " in pincer controller"<<std::endl; 00053 #endif 00054 Robot::DOFController(timeStep); 00055 00056 // base->SetFixedTran(setPoint); 00057 00058 grasp->CollectContacts(); 00059 numContacts = grasp->getNumContacts(); 00060 00061 if (numContacts > 0) { 00062 grasp->setGripForce(20.0); 00063 grasp->BuildGraspMap(); 00064 grasp->BuildJacobian(); 00065 // if (grasp->FindOptimalGraspForce()) 00066 if (grasp->FindGripAxis()==SUCCESS) { 00067 for (d=0;d<numDOF;d++) { 00068 #ifdef GRASPITDBG 00069 printf("setting dof effort %d to: %lf\n",d,grasp->getOptDOFEfforts()[d]); 00070 #endif 00071 dofVec[d]->SetForce(1.0e+9*grasp->getOptDOFEfforts()[d]); 00072 } 00073 } 00074 } 00075 00076 }