Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef SRDF_MODEL_
00038 #define SRDF_MODEL_
00039
00040 #include <map>
00041 #include <string>
00042 #include <vector>
00043 #include <utility>
00044 #include <urdf_model/model.h>
00045 #include <tinyxml.h>
00046
00048 namespace srdf
00049 {
00050
00052 class Model
00053 {
00054 public:
00055
00056 Model()
00057 {
00058 }
00059
00060 ~Model()
00061 {
00062 }
00063
00065 bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml);
00067 bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml);
00069 bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename);
00071 bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring);
00072
00077 struct Group
00078 {
00080 std::string name_;
00081
00087 std::vector<std::string> joints_;
00088
00093 std::vector<std::string> links_;
00094
00099 std::vector<std::pair<std::string, std::string> > chains_;
00100
00104 std::vector<std::string> subgroups_;
00105 };
00106
00111 struct VirtualJoint
00112 {
00114 std::string name_;
00115
00117 std::string type_;
00118
00120 std::string parent_frame_;
00121
00123 std::string child_link_;
00124 };
00125
00127 struct EndEffector
00128 {
00130 std::string name_;
00131
00133 std::string parent_link_;
00134
00137 std::string parent_group_;
00138
00140 std::string component_group_;
00141 };
00142
00144 struct GroupState
00145 {
00147 std::string name_;
00148
00150 std::string group_;
00151
00153 std::map<std::string, std::vector<double> > joint_values_;
00154 };
00155
00157 struct Sphere
00158 {
00160 double center_x_;
00161 double center_y_;
00162 double center_z_;
00163
00165 double radius_;
00166 };
00167
00169 struct LinkSpheres
00170 {
00172 std::string link_;
00173
00175 std::vector<Sphere> spheres_;
00176 };
00177
00179 struct DisabledCollision
00180 {
00182 std::string link1_;
00183
00185 std::string link2_;
00186
00188 std::string reason_;
00189 };
00190
00191
00192 struct PassiveJoint
00193 {
00195 std::string name_;
00196 };
00197
00199 const std::string& getName() const
00200 {
00201 return name_;
00202 }
00203
00205 const std::vector<DisabledCollision>& getDisabledCollisionPairs() const
00206 {
00207 return disabled_collisions_;
00208 }
00209
00211 __attribute__ ((deprecated))
00212 std::vector<std::pair<std::string, std::string> > getDisabledCollisions() const;
00213
00215 const std::vector<Group>& getGroups() const
00216 {
00217 return groups_;
00218 }
00219
00221 const std::vector<VirtualJoint>& getVirtualJoints() const
00222 {
00223 return virtual_joints_;
00224 }
00225
00227 const std::vector<EndEffector>& getEndEffectors() const
00228 {
00229 return end_effectors_;
00230 }
00231
00233 const std::vector<GroupState>& getGroupStates() const
00234 {
00235 return group_states_;
00236 }
00237
00239 const std::vector<PassiveJoint>& getPassiveJoints() const
00240 {
00241 return passive_joints_;
00242 }
00243
00245 const std::vector<LinkSpheres>& getLinkSphereApproximations() const
00246 {
00247 return link_sphere_approximations_;
00248 }
00249
00251 void clear();
00252
00253 private:
00254
00255 void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00256 void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00257 void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00258 void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00259 void loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00260 void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00261 void loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00262
00263 std::string name_;
00264 std::vector<Group> groups_;
00265 std::vector<GroupState> group_states_;
00266 std::vector<VirtualJoint> virtual_joints_;
00267 std::vector<EndEffector> end_effectors_;
00268 std::vector<LinkSpheres> link_sphere_approximations_;
00269 std::vector<DisabledCollision> disabled_collisions_;
00270 std::vector<PassiveJoint> passive_joints_;
00271 };
00272
00273 }
00274 #endif