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.h>
00045 #include <boost/shared_ptr.hpp>
00046 #include <tinyxml.h>
00047
00048 namespace urdf
00049 {
00050 typedef boost::shared_ptr<const ::urdf::Link> LinkConstSharedPtr;
00051 }
00052
00054 namespace srdf
00055 {
00056
00058 class Model
00059 {
00060 public:
00061
00062 Model()
00063 {
00064 }
00065
00066 ~Model()
00067 {
00068 }
00069
00071 bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml);
00073 bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml);
00075 bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename);
00077 bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring);
00078
00083 struct Group
00084 {
00086 std::string name_;
00087
00093 std::vector<std::string> joints_;
00094
00099 std::vector<std::string> links_;
00100
00105 std::vector<std::pair<std::string, std::string> > chains_;
00106
00110 std::vector<std::string> subgroups_;
00111 };
00112
00117 struct VirtualJoint
00118 {
00120 std::string name_;
00121
00123 std::string type_;
00124
00126 std::string parent_frame_;
00127
00129 std::string child_link_;
00130 };
00131
00133 struct EndEffector
00134 {
00136 std::string name_;
00137
00139 std::string parent_link_;
00140
00143 std::string parent_group_;
00144
00146 std::string component_group_;
00147 };
00148
00150 struct GroupState
00151 {
00153 std::string name_;
00154
00156 std::string group_;
00157
00159 std::map<std::string, std::vector<double> > joint_values_;
00160 };
00161
00163 struct Sphere
00164 {
00166 double center_x_;
00167 double center_y_;
00168 double center_z_;
00169
00171 double radius_;
00172 };
00173
00175 struct LinkSpheres
00176 {
00178 std::string link_;
00179
00181 std::vector<Sphere> spheres_;
00182 };
00183
00185 struct DisabledCollision
00186 {
00188 std::string link1_;
00189
00191 std::string link2_;
00192
00194 std::string reason_;
00195 };
00196
00197
00198 struct PassiveJoint
00199 {
00201 std::string name_;
00202 };
00203
00205 const std::string& getName() const
00206 {
00207 return name_;
00208 }
00209
00211 const std::vector<DisabledCollision>& getDisabledCollisionPairs() const
00212 {
00213 return disabled_collisions_;
00214 }
00215
00217 __attribute__ ((deprecated))
00218 std::vector<std::pair<std::string, std::string> > getDisabledCollisions() const;
00219
00221 const std::vector<Group>& getGroups() const
00222 {
00223 return groups_;
00224 }
00225
00227 const std::vector<VirtualJoint>& getVirtualJoints() const
00228 {
00229 return virtual_joints_;
00230 }
00231
00233 const std::vector<EndEffector>& getEndEffectors() const
00234 {
00235 return end_effectors_;
00236 }
00237
00239 const std::vector<GroupState>& getGroupStates() const
00240 {
00241 return group_states_;
00242 }
00243
00245 const std::vector<PassiveJoint>& getPassiveJoints() const
00246 {
00247 return passive_joints_;
00248 }
00249
00251 const std::vector<LinkSpheres>& getLinkSphereApproximations() const
00252 {
00253 return link_sphere_approximations_;
00254 }
00255
00257 void clear();
00258
00259 private:
00260
00261 void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00262 void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00263 void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00264 void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00265 void loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00266 void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00267 void loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00268
00269 std::string name_;
00270 std::vector<Group> groups_;
00271 std::vector<GroupState> group_states_;
00272 std::vector<VirtualJoint> virtual_joints_;
00273 std::vector<EndEffector> end_effectors_;
00274 std::vector<LinkSpheres> link_sphere_approximations_;
00275 std::vector<DisabledCollision> disabled_collisions_;
00276 std::vector<PassiveJoint> passive_joints_;
00277 };
00278 typedef boost::shared_ptr<Model> ModelSharedPtr;
00279 typedef boost::shared_ptr<const Model> ModelConstSharedPtr;
00280
00281
00282 }
00283 #endif