GraspTable.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #include <ros/ros.h>
00019 #include <GraspTable.h>
00020 
00021 void GraspTable::ReadDoubleValue(TiXmlElement* xml, const char * tag, double * value)
00022 {
00023   TiXmlHandle handle(xml);
00024   TiXmlElement* element=handle.FirstChild(tag).Element();
00025   sscanf(element->GetText(), "%lf", value);
00026 }
00027 
00028 void GraspTable::ReadJoint(TiXmlElement* xml, const char * tag, std::vector<double> & values)
00029 {
00030   TiXmlHandle handle(xml);
00031   TiXmlElement* element=handle.FirstChild(tag).Element();
00032   values.resize(16);
00033   ReadDoubleValue(element, "ThumbAngle",           &(values.data()[0]));
00034   ReadDoubleValue(element, "ThumbBase",            &(values.data()[1]));
00035   ReadDoubleValue(element, "ThumbProximal",        &(values.data()[2]));
00036   ReadDoubleValue(element, "ThumbDistal",          &(values.data()[3]));
00037   ReadDoubleValue(element, "FirstFingerBase",      &(values.data()[4]));
00038   ReadDoubleValue(element, "FirstFingerProximal",  &(values.data()[5]));
00039   ReadDoubleValue(element, "FirstFingerDistal",    &(values.data()[6]));
00040   ReadDoubleValue(element, "MiddleFingerBase",     &(values.data()[7]));
00041   ReadDoubleValue(element, "MiddleFingerProximal", &(values.data()[8]));
00042   ReadDoubleValue(element, "MiddleFingerDistal",   &(values.data()[9]));
00043   ReadDoubleValue(element, "RingFingerBase",       &(values.data()[10]));
00044   ReadDoubleValue(element, "RingFingerProximal",   &(values.data()[11]));
00045   ReadDoubleValue(element, "RingFingerDistal",     &(values.data()[12]));
00046   ReadDoubleValue(element, "LittleFingerBase",     &(values.data()[13]));
00047   ReadDoubleValue(element, "LittleFingerProximal", &(values.data()[14]));
00048   ReadDoubleValue(element, "LittleFingerDistal",   &(values.data()[15]));
00049 }
00050 
00051 void GraspTable::ReadPose(TiXmlElement* xml, const char * tag, std::vector<double> & values)
00052 {
00053   TiXmlHandle handle(xml);
00054   TiXmlElement* element=handle.FirstChild(tag).Element();
00055   values.resize(6);
00056   ReadDoubleValue(element, "PositionX", &(values.data()[0]));
00057   ReadDoubleValue(element, "PositionY", &(values.data()[1]));
00058   ReadDoubleValue(element, "PositionZ", &(values.data()[2]));
00059   ReadDoubleValue(element, "Roll",      &(values.data()[3]));
00060   ReadDoubleValue(element, "Pitch",     &(values.data()[4]));
00061   ReadDoubleValue(element, "Yaw",       &(values.data()[5]));
00062 }
00063 
00064 int GraspTable::ReadFromFile(const char * filename, GraspTableObject* graspTableObject)
00065 {
00066   TiXmlDocument doc(filename);
00067   if (graspTableObject == NULL)
00068   {
00069     printf("GraspTable::ReadFromFile:Error,  argument error%s\n",filename);
00070     return -3;
00071   }
00072   if (!doc.LoadFile())
00073   {
00074     printf("GraspTable::ReadFromFile:Error, could not read %s\n",filename);
00075     return -1;
00076   }
00077   //printf ("Readig file %s\n",filename);
00078   TiXmlHandle root_handle(&doc);
00079   TiXmlHandle grasp_list_handle=root_handle.FirstChild("GraspList");
00080   TiXmlElement* number_element=grasp_list_handle.FirstChildElement("Number").Element();
00081   // total number of grasps in this file
00082   int number;
00083   sscanf(number_element->GetText(), "%d", &number);
00084 
00085   if (graspTableObject->Init(number) != 0)
00086   {
00087     printf("GraspTable::ReadFromFile:Error, could not allocate GraspTableObject\n");
00088     return -2;
00089   }
00090   for (int i=0; i<number; i++)
00091   {
00092     TiXmlHandle grasp_handle=grasp_list_handle.ChildElement("Grasp", i);
00093     TiXmlElement* grasp_element=grasp_handle.Element();
00094     double quality;
00095     grasp_element->QueryDoubleAttribute("Quality", &quality);
00096 
00097     Grasp * newGrasp = new Grasp();
00098     newGrasp->SetGraspId(i);
00099 
00100     std::vector<double> values;
00101 
00102     ReadPose(grasp_element, "ApproachPose", values);
00103     newGrasp->SetTCPPreGraspPose(values);
00104 
00105     ReadPose(grasp_element, "GraspPose", values);
00106     newGrasp->SetTCPGraspPose(values);
00107 
00108     ReadJoint(grasp_element, "ApproachJoint", values);
00109     newGrasp->SetHandPreGraspConfig(values);
00110 
00111     ReadJoint(grasp_element, "GraspJoint", values);
00112     newGrasp->SetHandGraspConfig(values);
00113 
00114     ReadJoint(grasp_element, "GraspOptimalJoint", values);
00115     newGrasp->SetHandOptimalGraspConfig(values);
00116 
00117     graspTableObject->AddGrasp(newGrasp);
00118   }
00119 
00120   return 0;
00121 }
00122 
00123 
00124 int GraspTable::Init(char* object_table_file,unsigned int table_size)
00125 {
00126   FILE* f = fopen(object_table_file,"r");
00127   int numberOfObjects = 0;
00128 
00129   if (f==NULL)
00130   {
00131     printf("GraspTable::Error, Object Table File not found :%s\n",object_table_file);
00132     return -1;
00133   }
00134   fscanf(f,"%d\n",&numberOfObjects);
00135   m_GraspTable.resize(table_size); //range of DESIRE class ids
00136   for (unsigned int i=0;i < table_size;i++)
00137   {
00138     m_GraspTable[i] = NULL;
00139   }
00140   for (int obj=0; obj <numberOfObjects; obj++)
00141   {
00142     char GraspTableFileName[500];
00143     int objectClassId = 0;
00144     fscanf(f,"%d, %s\n",&objectClassId,GraspTableFileName);
00145 //~ #####################################################################################################################
00146     std::string object_table_file_str=object_table_file;
00147     unsigned found = object_table_file_str.find_last_of("/");
00148     std::string filepath = object_table_file_str.substr(0, found);
00149     std::string grasp_table_file_str = filepath + '/' + GraspTableFileName;
00150     strncpy(GraspTableFileName, grasp_table_file_str.c_str(), sizeof(GraspTableFileName));
00151     GraspTableFileName[sizeof(GraspTableFileName) - 1] = 0;
00152 //~ #####################################################################################################################
00153     printf("GraspTable::Init: Trying to read grasp table for object %d from file %s ...\n",objectClassId,GraspTableFileName);
00154     GraspTableObject * graspTableObject = new GraspTableObject();
00155     graspTableObject->SetObjectClassId(objectClassId);
00156     if (ReadFromFile(GraspTableFileName,graspTableObject)==0)
00157     {
00158       printf("successful\n");
00159       AddGraspTableObject(graspTableObject);
00160     }
00161     else
00162     {
00163       printf("failed\n");
00164     }
00165   }
00166   return 5;
00167 }
00168 
00169 void GraspTable::AddGraspTableObject(GraspTableObject* object)
00170 {
00171   unsigned int objectClassId = object->GetObjectClassId();
00172   if (objectClassId < m_GraspTable.size())
00173   {
00174     m_GraspTable[objectClassId]=object;
00175   }
00176   else
00177   {
00178     printf("GraspTable::AddGraspTableObject: Error, class id larger than table size!\n");
00179   }
00180 }
00181 
00182 
00183 Grasp* GraspTable::GetNextGrasp(unsigned int objectClassId)
00184 {
00185   Grasp* retVal = NULL;
00186   if (objectClassId < m_GraspTable.size() && m_GraspTable[objectClassId] != NULL)
00187   {
00188     if ((objectClassId != m_lastObjectClassId))
00189     {
00190       m_lastObjectClassId=objectClassId;
00191       m_GraspTable[objectClassId]->ResetGraspReadPtr();
00192     }
00193     retVal =  m_GraspTable[objectClassId]->GetNextGrasp();
00194   }
00195   return retVal;
00196 }
00197 
00198 void GraspTable::ResetReadPtr(unsigned int object_class_id)
00199 {
00200   if (object_class_id < m_GraspTable.size())
00201   {
00202     m_GraspTable[object_class_id]->ResetGraspReadPtr();
00203   }
00204 }
00205 
00206 Grasp * GraspTable::GetGrasp(unsigned int objectClassId, unsigned int & grasp_id)
00207 {
00208   Grasp* retVal = NULL;
00209   if (objectClassId < m_GraspTable.size() && m_GraspTable[objectClassId] != NULL)
00210   {
00211     retVal=m_GraspTable[objectClassId]->GetGrasp(grasp_id);
00212   }
00213   return retVal;
00214 }


cob_pick_place_action
Author(s): Felix Messmer
autogenerated on Thu Jun 6 2019 21:23:15