model.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author Ioan Sucan */
36 
37 #ifndef SRDF_MODEL_
38 #define SRDF_MODEL_
39 
40 #include <map>
41 #include <string>
42 #include <vector>
43 #include <utility>
44 #include <urdf/model.h>
45 #include <memory>
46 #include <tinyxml2.h>
47 
49 namespace srdf
50 {
52 class Model
53 {
54 public:
55  Model()
56  {
57  }
58 
59  ~Model()
60  {
61  }
62 
64  bool initXml(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* xml);
66  bool initXml(const urdf::ModelInterface& urdf_model, tinyxml2::XMLDocument* xml);
68  bool initFile(const urdf::ModelInterface& urdf_model, const std::string& filename);
70  bool initString(const urdf::ModelInterface& urdf_model, const std::string& xmlstring);
71 
76  struct Group
77  {
79  std::string name_;
80 
86  std::vector<std::string> joints_;
87 
92  std::vector<std::string> links_;
93 
98  std::vector<std::pair<std::string, std::string> > chains_;
99 
103  std::vector<std::string> subgroups_;
104  };
105 
110  struct VirtualJoint
111  {
113  std::string name_;
114 
116  std::string type_;
117 
119  std::string parent_frame_;
120 
122  std::string child_link_;
123  };
124 
126  struct EndEffector
127  {
129  std::string name_;
130 
132  std::string parent_link_;
133 
136  std::string parent_group_;
137 
139  std::string component_group_;
140  };
141 
143  struct GroupState
144  {
146  std::string name_;
147 
149  std::string group_;
150 
153  std::map<std::string, std::vector<double> > joint_values_;
154  };
155 
157  struct Sphere
158  {
160  double center_x_;
161  double center_y_;
162  double center_z_;
163 
165  double radius_;
166  };
167 
169  struct LinkSpheres
170  {
172  std::string link_;
173 
175  std::vector<Sphere> spheres_;
176  };
177 
179  struct CollisionPair
180  {
182  std::string link1_;
183 
185  std::string link2_;
186 
188  std::string reason_;
189  };
190 
191  // Some joints can be passive (not actuated). This structure specifies information about such joints
192  struct PassiveJoint
193  {
195  std::string name_;
196  };
197 
199  const std::string& getName() const
200  {
201  return name_;
202  }
203 
205  const std::vector<std::string>& getNoDefaultCollisionLinks() const
206  {
208  }
209 
211  const std::vector<CollisionPair>& getEnabledCollisionPairs() const
212  {
214  }
215 
217  const std::vector<CollisionPair>& getDisabledCollisionPairs() const
218  {
220  }
221 
223  const std::vector<Group>& getGroups() const
224  {
225  return groups_;
226  }
227 
229  const std::vector<VirtualJoint>& getVirtualJoints() const
230  {
231  return virtual_joints_;
232  }
233 
235  const std::vector<EndEffector>& getEndEffectors() const
236  {
237  return end_effectors_;
238  }
239 
241  const std::vector<GroupState>& getGroupStates() const
242  {
244  }
245 
247  const std::vector<PassiveJoint>& getPassiveJoints() const
248  {
250  }
251 
253  const std::vector<LinkSpheres>& getLinkSphereApproximations() const
254  {
256  }
257 
259  void clear();
260 
261 private:
262  void loadVirtualJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
263  void loadGroups(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
264  void loadGroupStates(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
265  void loadEndEffectors(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
266  void loadLinkSphereApproximations(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
267  void loadCollisionDefaults(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
268  void loadCollisionPairs(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml, const char* tag_name,
269  std::vector<CollisionPair>& pairs);
270  void loadPassiveJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);
271 
272  std::string name_;
273  std::vector<Group> groups_;
274  std::vector<GroupState> group_states_;
275  std::vector<VirtualJoint> virtual_joints_;
276  std::vector<EndEffector> end_effectors_;
277  std::vector<LinkSpheres> link_sphere_approximations_;
278  std::vector<std::string> no_default_collision_links_;
279  std::vector<CollisionPair> enabled_collision_pairs_;
280  std::vector<CollisionPair> disabled_collision_pairs_;
281  std::vector<PassiveJoint> passive_joints_;
282 };
283 typedef std::shared_ptr<Model> ModelSharedPtr;
284 typedef std::shared_ptr<const Model> ModelConstSharedPtr;
285 } // namespace srdf
286 #endif
srdf::Model::getEnabledCollisionPairs
const std::vector< CollisionPair > & getEnabledCollisionPairs() const
Get the list of pairs of links for which we explicitly re-enable collision (after having disabled it ...
Definition: model.h:275
srdf::Model::EndEffector::component_group_
std::string component_group_
The name of the group that includes the joints & links this end effector consists of.
Definition: model.h:203
srdf::Model::EndEffector::name_
std::string name_
The name of the end effector.
Definition: model.h:193
srdf::Model::passive_joints_
std::vector< PassiveJoint > passive_joints_
Definition: model.h:345
srdf::Model::VirtualJoint::name_
std::string name_
The name of the new joint.
Definition: model.h:177
srdf::Model::virtual_joints_
std::vector< VirtualJoint > virtual_joints_
Definition: model.h:339
srdf::Model::Group::chains_
std::vector< std::pair< std::string, std::string > > chains_
Definition: model.h:162
srdf::Model::loadGroups
void loadGroups(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *robot_xml)
Definition: model.cpp:103
srdf::Model::getPassiveJoints
const std::vector< PassiveJoint > & getPassiveJoints() const
Get the list of known passive joints.
Definition: model.h:311
srdf::ModelSharedPtr
std::shared_ptr< Model > ModelSharedPtr
Definition: model.h:315
srdf::Model::link_sphere_approximations_
std::vector< LinkSpheres > link_sphere_approximations_
Definition: model.h:341
srdf::Model::~Model
~Model()
Definition: model.h:123
srdf::Model::loadGroupStates
void loadGroupStates(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *robot_xml)
Definition: model.cpp:284
srdf::Model::getNoDefaultCollisionLinks
const std::vector< std::string > & getNoDefaultCollisionLinks() const
Get the list of links that should have collision checking disabled by default (and only selectively e...
Definition: model.h:269
srdf::Model::initXml
bool initXml(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *xml)
Load Model from XMLElement.
srdf::Model::LinkSpheres
The definition of a list of spheres for a link.
Definition: model.h:233
srdf::Model::disabled_collision_pairs_
std::vector< CollisionPair > disabled_collision_pairs_
Definition: model.h:344
srdf::Model::PassiveJoint::name_
std::string name_
The name of the new joint.
Definition: model.h:259
srdf::Model::name_
std::string name_
Definition: model.h:336
srdf::Model::clear
void clear()
Clear the model.
Definition: model.cpp:689
srdf::Model::loadEndEffectors
void loadEndEffectors(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *robot_xml)
Definition: model.cpp:380
srdf::Model::VirtualJoint::parent_frame_
std::string parent_frame_
The transform applied by this joint to the robot model brings that model to a particular frame.
Definition: model.h:183
srdf::Model::CollisionPair::link2_
std::string link2_
The name of the second link (as in URDF) of the disabled collision.
Definition: model.h:249
srdf::Model::initString
bool initString(const urdf::ModelInterface &urdf_model, const std::string &xmlstring)
Load Model from a XML-string.
Definition: model.cpp:678
srdf::Model::Sphere::center_z_
double center_z_
Definition: model.h:226
srdf::Model::group_states_
std::vector< GroupState > group_states_
Definition: model.h:338
srdf::Model::getGroupStates
const std::vector< GroupState > & getGroupStates() const
Get the list of group states defined for this model.
Definition: model.h:305
srdf::Model::Group::name_
std::string name_
The name of the group.
Definition: model.h:143
srdf::Model::GroupState::group_
std::string group_
The name of the group this state is specified for.
Definition: model.h:213
srdf::Model::EndEffector::parent_link_
std::string parent_link_
The name of the link this end effector connects to.
Definition: model.h:196
srdf::Model::loadCollisionPairs
void loadCollisionPairs(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *robot_xml, const char *tag_name, std::vector< CollisionPair > &pairs)
Definition: model.cpp:556
srdf::Model::loadPassiveJoints
void loadPassiveJoints(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *robot_xml)
Definition: model.cpp:586
model.h
srdf::ModelConstSharedPtr
std::shared_ptr< const Model > ModelConstSharedPtr
Definition: model.h:316
srdf::Model::Group::links_
std::vector< std::string > links_
Definition: model.h:156
srdf::Model::initFile
bool initFile(const urdf::ModelInterface &urdf_model, const std::string &filename)
Load Model given a filename.
Definition: model.cpp:655
srdf::Model::end_effectors_
std::vector< EndEffector > end_effectors_
Definition: model.h:340
srdf::Model::Sphere::center_y_
double center_y_
Definition: model.h:225
srdf::Model::loadCollisionDefaults
void loadCollisionDefaults(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *robot_xml)
Definition: model.cpp:535
srdf::Model::getGroups
const std::vector< Group > & getGroups() const
Get the list of groups defined for this model.
Definition: model.h:287
srdf::Model::loadLinkSphereApproximations
void loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *robot_xml)
Definition: model.cpp:438
srdf::Model::no_default_collision_links_
std::vector< std::string > no_default_collision_links_
Definition: model.h:342
srdf::Model::LinkSpheres::spheres_
std::vector< Sphere > spheres_
The spheres for the link.
Definition: model.h:239
srdf::Model::Model
Model()
Definition: model.h:119
srdf::Model::PassiveJoint
Definition: model.h:256
srdf::Model::GroupState::joint_values_
std::map< std::string, std::vector< double > > joint_values_
Definition: model.h:217
srdf::Model::groups_
std::vector< Group > groups_
Definition: model.h:337
srdf::Model::GroupState
A named state for a particular group.
Definition: model.h:207
srdf::Model::Sphere
The definition of a sphere.
Definition: model.h:221
srdf::Model::enabled_collision_pairs_
std::vector< CollisionPair > enabled_collision_pairs_
Definition: model.h:343
srdf::Model::Sphere::center_x_
double center_x_
The center of the sphere in the link collision frame.
Definition: model.h:224
srdf::Model::getDisabledCollisionPairs
const std::vector< CollisionPair > & getDisabledCollisionPairs() const
Get the list of pairs of links for which we explicitly disable collision.
Definition: model.h:281
srdf
Main namespace.
Definition: model.h:49
srdf::Model::VirtualJoint::child_link_
std::string child_link_
The link this joint applies to.
Definition: model.h:186
srdf::Model::EndEffector::parent_group_
std::string parent_group_
Definition: model.h:200
srdf::Model::getEndEffectors
const std::vector< EndEffector > & getEndEffectors() const
Get the list of end effectors defined for this model.
Definition: model.h:299
srdf::Model::GroupState::name_
std::string name_
The name of the state.
Definition: model.h:210
srdf::Model::LinkSpheres::link_
std::string link_
The name of the link (as in URDF).
Definition: model.h:236
srdf::Model::Group::subgroups_
std::vector< std::string > subgroups_
Definition: model.h:167
srdf::Model::loadVirtualJoints
void loadVirtualJoints(const urdf::ModelInterface &urdf_model, tinyxml2::XMLElement *robot_xml)
Definition: model.cpp:48
srdf::Model::getLinkSphereApproximations
const std::vector< LinkSpheres > & getLinkSphereApproximations() const
Get the collision spheres list.
Definition: model.h:317
srdf::Model::Sphere::radius_
double radius_
The radius of the sphere.
Definition: model.h:229
srdf::Model::CollisionPair
The definition of a disabled/enabled collision between two links.
Definition: model.h:243
srdf::Model::getName
const std::string & getName() const
Get the name of this model.
Definition: model.h:263
srdf::Model::Group::joints_
std::vector< std::string > joints_
Definition: model.h:150
srdf::Model::CollisionPair::reason_
std::string reason_
The reason why the collision check was disabled/enabled.
Definition: model.h:252
srdf::Model::getVirtualJoints
const std::vector< VirtualJoint > & getVirtualJoints() const
Get the list of virtual joints defined for this model.
Definition: model.h:293
srdf::Model::CollisionPair::link1_
std::string link1_
The name of the first link (as in URDF) of the disabled collision.
Definition: model.h:246
srdf::Model::VirtualJoint::type_
std::string type_
The type of this new joint. This can be "fixed" (0 DOF), "planar" (3 DOF: x,y,yaw) or "floating" (6DO...
Definition: model.h:180
srdf::Model
Representation of semantic information about the robot.
Definition: model.h:84


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Wed Mar 2 2022 01:03:43