00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
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
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
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);
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 }