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> // TODO: replace with urdf_model/types.h in Lunar
45 #include <boost/shared_ptr.hpp>
46 #include <tinyxml.h>
47 
49 namespace srdf
50 {
51 
53 class Model
54 {
55 public:
56 
58  {
59  }
60 
62  {
63  }
64 
66  bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml);
68  bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml);
70  bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename);
72  bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring);
73 
78  struct Group
79  {
81  std::string name_;
82 
88  std::vector<std::string> joints_;
89 
94  std::vector<std::string> links_;
95 
100  std::vector<std::pair<std::string, std::string> > chains_;
101 
105  std::vector<std::string> subgroups_;
106  };
107 
113  {
115  std::string name_;
116 
118  std::string type_;
119 
121  std::string parent_frame_;
122 
124  std::string child_link_;
125  };
126 
128  struct EndEffector
129  {
131  std::string name_;
132 
134  std::string parent_link_;
135 
138  std::string parent_group_;
139 
141  std::string component_group_;
142  };
143 
145  struct GroupState
146  {
148  std::string name_;
149 
151  std::string group_;
152 
154  std::map<std::string, std::vector<double> > joint_values_;
155  };
156 
158  struct Sphere
159  {
161  double center_x_;
162  double center_y_;
163  double center_z_;
164 
166  double radius_;
167  };
168 
170  struct LinkSpheres
171  {
173  std::string link_;
174 
176  std::vector<Sphere> spheres_;
177  };
178 
181  {
183  std::string link1_;
184 
186  std::string link2_;
187 
189  std::string reason_;
190  };
191 
192  // Some joints can be passive (not actuated). This structure specifies information about such joints
194  {
196  std::string name_;
197  };
198 
200  const std::string& getName() const
201  {
202  return name_;
203  }
204 
206  const std::vector<DisabledCollision>& getDisabledCollisionPairs() const
207  {
208  return disabled_collisions_;
209  }
210 
212  __attribute__ ((deprecated))
213  std::vector<std::pair<std::string, std::string> > getDisabledCollisions() const;
214 
216  const std::vector<Group>& getGroups() const
217  {
218  return groups_;
219  }
220 
222  const std::vector<VirtualJoint>& getVirtualJoints() const
223  {
224  return virtual_joints_;
225  }
226 
228  const std::vector<EndEffector>& getEndEffectors() const
229  {
230  return end_effectors_;
231  }
232 
234  const std::vector<GroupState>& getGroupStates() const
235  {
236  return group_states_;
237  }
238 
240  const std::vector<PassiveJoint>& getPassiveJoints() const
241  {
242  return passive_joints_;
243  }
244 
246  const std::vector<LinkSpheres>& getLinkSphereApproximations() const
247  {
249  }
250 
252  void clear();
253 
254 private:
255 
256  void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
257  void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
258  void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
259  void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
260  void loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
261  void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
262  void loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
263 
264  std::string name_;
265  std::vector<Group> groups_;
266  std::vector<GroupState> group_states_;
267  std::vector<VirtualJoint> virtual_joints_;
268  std::vector<EndEffector> end_effectors_;
269  std::vector<LinkSpheres> link_sphere_approximations_;
270  std::vector<DisabledCollision> disabled_collisions_;
271  std::vector<PassiveJoint> passive_joints_;
272 };
275 
276 
277 }
278 #endif
void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
Definition: model.cpp:47
std::vector< std::string > subgroups_
Definition: model.h:105
std::vector< VirtualJoint > virtual_joints_
Definition: model.h:267
The definition of a disabled collision between two links.
Definition: model.h:180
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:118
std::string link1_
The name of the first link (as in URDF) of the disabled collision.
Definition: model.h:183
Representation of semantic information about the robot.
Definition: model.h:53
double center_y_
Definition: model.h:162
std::string link2_
The name of the second link (as in URDF) of the disabled collision.
Definition: model.h:186
std::map< std::string, std::vector< double > > joint_values_
The values of joints for this state. Each joint can have a value. We use a vector for the &#39;value&#39; to ...
Definition: model.h:154
const std::vector< VirtualJoint > & getVirtualJoints() const
Get the list of virtual joints defined for this model.
Definition: model.h:222
std::vector< std::pair< std::string, std::string > > chains_
Definition: model.h:100
bool initFile(const urdf::ModelInterface &urdf_model, const std::string &filename)
Load Model given a filename.
Definition: model.cpp:596
Model()
Definition: model.h:57
std::vector< EndEffector > end_effectors_
Definition: model.h:268
void loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
Definition: model.cpp:525
double center_z_
Definition: model.h:163
~Model()
Definition: model.h:61
std::string name_
The name of the end effector.
Definition: model.h:131
A named state for a particular group.
Definition: model.h:145
std::vector< GroupState > group_states_
Definition: model.h:266
The definition of a list of spheres for a link.
Definition: model.h:170
double center_x_
The center of the sphere in the link collision frame.
Definition: model.h:161
void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
Definition: model.cpp:494
std::string name_
The name of the state.
Definition: model.h:148
std::string parent_link_
The name of the link this end effector connects to.
Definition: model.h:134
__attribute__((deprecated)) std const std::vector< Group > & getGroups() const
Get the list of groups defined for this model.
Definition: model.h:216
std::vector< Group > groups_
Definition: model.h:265
const std::vector< DisabledCollision > & getDisabledCollisionPairs() const
Get the list of pairs of links that need not be checked for collisions (because they can never touch ...
Definition: model.h:206
const std::string & getName() const
Get the name of this model.
Definition: model.h:200
void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
Definition: model.cpp:268
std::string reason_
The reason why the collision check was disabled.
Definition: model.h:189
bool initString(const urdf::ModelInterface &urdf_model, const std::string &xmlstring)
Load Model from a XML-string.
Definition: model.cpp:619
std::string parent_frame_
The transform applied by this joint to the robot model brings that model to a particular frame...
Definition: model.h:121
std::string name_
Definition: model.h:264
The definition of a sphere.
Definition: model.h:158
double radius_
The radius of the sphere.
Definition: model.h:166
Representation of an end effector.
Definition: model.h:128
std::string link_
The name of the link (as in URDF).
Definition: model.h:173
boost::shared_ptr< Model > ModelSharedPtr
Definition: model.h:273
std::string name_
The name of the group.
Definition: model.h:81
std::vector< DisabledCollision > disabled_collisions_
Definition: model.h:270
Main namespace.
Definition: model.h:49
A group consists of a set of joints and the corresponding descendant links. There are multiple ways t...
Definition: model.h:78
void loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
Definition: model.cpp:407
std::vector< std::string > links_
Definition: model.h:94
void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
Definition: model.cpp:355
std::string child_link_
The link this joint applies to.
Definition: model.h:124
const std::vector< LinkSpheres > & getLinkSphereApproximations() const
Get the collision spheres list.
Definition: model.h:246
const std::vector< PassiveJoint > & getPassiveJoints() const
Get the list of known passive joints.
Definition: model.h:240
std::vector< Sphere > spheres_
The spheres for the link.
Definition: model.h:176
std::vector< std::string > joints_
Definition: model.h:88
bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml)
Load Model from TiXMLElement.
Definition: model.cpp:553
std::vector< PassiveJoint > passive_joints_
Definition: model.h:271
std::string name_
The name of the new joint.
Definition: model.h:196
boost::shared_ptr< const Model > ModelConstSharedPtr
Definition: model.h:274
const std::vector< GroupState > & getGroupStates() const
Get the list of group states defined for this model.
Definition: model.h:234
std::string component_group_
The name of the group that includes the joints & links this end effector consists of...
Definition: model.h:141
std::string name_
The name of the new joint.
Definition: model.h:115
void clear()
Clear the model.
Definition: model.cpp:627
void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
Definition: model.cpp:95
const std::vector< EndEffector > & getEndEffectors() const
Get the list of end effectors defined for this model.
Definition: model.h:228
std::string parent_group_
Definition: model.h:138
std::string group_
The name of the group this state is specified for.
Definition: model.h:151
std::vector< LinkSpheres > link_sphere_approximations_
Definition: model.h:269


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Mon Jun 10 2019 15:06:23