Go to the documentation of this file.00001
00035 #ifndef __GRASP_TABLE_H__
00036 #define __GRASP_TABLE_H__
00037
00038 #include <tinyxml.h>
00039 #include <iostream>
00040 #include <vector>
00041
00042 #define MAX_NO_OF_OBJECTS 200
00043
00044
00045 class Grasp
00046 {
00047 public:
00048 Grasp(){;}
00049
00050 std::vector<double> GetTCPPreGraspPose(){return m_TCPPreGraspPose;}
00051 void SetTCPPreGraspPose(std::vector<double> TCPPreGraspPose){m_TCPPreGraspPose = TCPPreGraspPose;}
00052
00053 std::vector<double> GetTCPGraspPose(){return m_TCPGraspPose;}
00054 void SetTCPGraspPose(std::vector<double> TCPGraspPose){m_TCPGraspPose = TCPGraspPose;}
00055
00057 std::vector<double> GetHandPreGraspConfig(){return m_HandPreGraspConfig;}
00058 void SetHandPreGraspConfig(std::vector<double> HandPreGraspConfig){m_HandPreGraspConfig=HandPreGraspConfig;}
00059
00060 std::vector<double> GetHandGraspConfig(){return m_HandGraspConfig;}
00061 void SetHandGraspConfig(std::vector<double> HandGraspConfig){m_HandGraspConfig=HandGraspConfig;}
00062
00063 std::vector<double> GetHandOptimalGraspConfig(){return m_HandOptimalGraspConfig;}
00064 void SetHandOptimalGraspConfig(std::vector<double> HandOptimalGraspConfig){m_HandOptimalGraspConfig=HandOptimalGraspConfig;}
00065
00066 void SetGraspId(int graspId){m_GraspId = graspId;}
00067 int GetGraspId(){return m_GraspId;}
00068
00069 private:
00070 std::vector<double> m_TCPGraspPose;
00071 std::vector<double> m_TCPPreGraspPose;
00072 std::vector<double> m_HandPreGraspConfig;
00073 std::vector<double> m_HandGraspConfig;
00074 std::vector<double> m_HandOptimalGraspConfig;
00075
00076 int m_GraspId;
00077 };
00078
00079 class GraspTableObject
00080 {
00081 public:
00082 GraspTableObject():m_GraspReadPtr(0),m_GraspWritePtr(0),m_ObjectClassId(0)
00083 {m_GraspTableObject.clear();}
00084
00085 int Init(int size)
00086 {m_GraspTableObject.resize(size);return 0;}
00087
00088
00090 std::vector<Grasp*>& Get()
00091 {return m_GraspTableObject;}
00092
00093 Grasp * GetNextGrasp()
00094 {
00095 if (m_GraspReadPtr <m_GraspWritePtr)
00096 return m_GraspTableObject[m_GraspReadPtr++];
00097 else
00098 return NULL;
00099 }
00100
00101 Grasp * GetGrasp(unsigned int graspId)
00102 {
00103 if (graspId < m_GraspTableObject.size())
00104 return m_GraspTableObject[graspId];
00105 else
00106 return NULL;
00107 }
00108
00109 void AddGrasp(Grasp * grasp)
00110 {if (m_GraspWritePtr < m_GraspTableObject.size()) m_GraspTableObject[m_GraspWritePtr++]=grasp;}
00111
00112 unsigned int GetObjectClassId(){return m_ObjectClassId;}
00113 void SetObjectClassId(unsigned int ObjectClassId){m_ObjectClassId = ObjectClassId;}
00114
00115 void ResetGraspReadPtr(){m_GraspReadPtr=0;}
00116
00117 private:
00118
00119 unsigned int m_GraspReadPtr;
00120 unsigned int m_GraspWritePtr;
00121 unsigned int m_ObjectClassId;
00122 std::vector<Grasp*> m_GraspTableObject;
00123
00124
00125 };
00126
00127 class GraspTable
00128 {
00129 public:
00130 GraspTable(){;}
00131 int Init(char * iniFile, unsigned int table_size=MAX_NO_OF_OBJECTS);
00132 void AddGraspTableObject(GraspTableObject * graspTableObject);
00133
00134 Grasp * GetNextGrasp(unsigned int object_class_id);
00135 Grasp * GetGrasp(unsigned int object_class_id, unsigned int & grasp_id);
00136 void ResetReadPtr(unsigned int object_class_id);
00137
00138 private:
00139 void ReadDoubleValue(TiXmlElement* xml, const char * tag, double * value);
00140 void ReadJoint(TiXmlElement* xml, const char * tag, std::vector<double> & values);
00141 void ReadPose(TiXmlElement* xml, const char * tag, std::vector<double> & values);
00142 int ReadFromFile(const char * filename, GraspTableObject * tableObject);
00143
00144 std::vector<GraspTableObject*> m_GraspTable;
00145 unsigned int m_lastObjectClassId;
00146 };
00147
00148 #endif
00149
00150
00151