model.cpp
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 #include "srdfdom/model.h"
38 #include <console_bridge/console.h>
39 #include <boost/lexical_cast.hpp>
40 #include <boost/algorithm/string/trim.hpp>
41 #include <algorithm>
42 #include <fstream>
43 #include <sstream>
44 #include <set>
45 #include <limits>
46 
47 void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
48 {
49  for (TiXmlElement* vj_xml = robot_xml->FirstChildElement("virtual_joint"); vj_xml; vj_xml = vj_xml->NextSiblingElement("virtual_joint"))
50  {
51  const char *jname = vj_xml->Attribute("name");
52  const char *child = vj_xml->Attribute("child_link");
53  const char *parent = vj_xml->Attribute("parent_frame");
54  const char *type = vj_xml->Attribute("type");
55  if (!jname)
56  {
57  CONSOLE_BRIDGE_logError("Name of virtual joint is not specified");
58  continue;
59  }
60  if (!child)
61  {
62  CONSOLE_BRIDGE_logError("Child link of virtual joint is not specified");
63  continue;
64  }
65  if (!urdf_model.getLink(boost::trim_copy(std::string(child))))
66  {
67  CONSOLE_BRIDGE_logError("Virtual joint does not attach to a link on the robot (link '%s' is not known)", child);
68  continue;
69  }
70  if (!parent)
71  {
72  CONSOLE_BRIDGE_logError("Parent frame of virtual joint is not specified");
73  continue;
74  }
75  if (!type)
76  {
77  CONSOLE_BRIDGE_logError("Type of virtual joint is not specified");
78  continue;
79  }
80  VirtualJoint vj;
81  vj.type_ = std::string(type); boost::trim(vj.type_);
82  std::transform(vj.type_.begin(), vj.type_.end(), vj.type_.begin(), ::tolower);
83  if (vj.type_ != "planar" && vj.type_ != "floating" && vj.type_ != "fixed")
84  {
85  CONSOLE_BRIDGE_logError("Unknown type of joint: '%s'. Assuming 'fixed' instead. Other known types are 'planar' and 'floating'.", type);
86  vj.type_ = "fixed";
87  }
88  vj.name_ = std::string(jname); boost::trim(vj.name_);
89  vj.child_link_ = std::string(child); boost::trim(vj.child_link_);
90  vj.parent_frame_ = std::string(parent); boost::trim(vj.parent_frame_);
91  virtual_joints_.push_back(vj);
92  }
93 }
94 
95 void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
96 {
97  for (TiXmlElement* group_xml = robot_xml->FirstChildElement("group"); group_xml; group_xml = group_xml->NextSiblingElement("group"))
98  {
99  const char *gname = group_xml->Attribute("name");
100  if (!gname)
101  {
102  CONSOLE_BRIDGE_logError("Group name not specified");
103  continue;
104  }
105  Group g;
106  g.name_ = std::string(gname); boost::trim(g.name_);
107 
108  // get the links in the groups
109  for (TiXmlElement* link_xml = group_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
110  {
111  const char *lname = link_xml->Attribute("name");
112  if (!lname)
113  {
114  CONSOLE_BRIDGE_logError("Link name not specified");
115  continue;
116  }
117  std::string lname_str = boost::trim_copy(std::string(lname));
118  if (!urdf_model.getLink(lname_str))
119  {
120  CONSOLE_BRIDGE_logError("Link '%s' declared as part of group '%s' is not known to the URDF", lname, gname);
121  continue;
122  }
123  g.links_.push_back(lname_str);
124  }
125 
126  // get the joints in the groups
127  for (TiXmlElement* joint_xml = group_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
128  {
129  const char *jname = joint_xml->Attribute("name");
130  if (!jname)
131  {
132  CONSOLE_BRIDGE_logError("Joint name not specified");
133  continue;
134  }
135  std::string jname_str = boost::trim_copy(std::string(jname));
136  if (!urdf_model.getJoint(jname_str))
137  {
138  bool missing = true;
139  for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
140  if (virtual_joints_[k].name_ == jname_str)
141  {
142  missing = false;
143  break;
144  }
145  if (missing)
146  {
147  CONSOLE_BRIDGE_logError("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname);
148  continue;
149  }
150  }
151  g.joints_.push_back(jname_str);
152  }
153 
154  // get the chains in the groups
155  for (TiXmlElement* chain_xml = group_xml->FirstChildElement("chain"); chain_xml; chain_xml = chain_xml->NextSiblingElement("chain"))
156  {
157  const char *base = chain_xml->Attribute("base_link");
158  const char *tip = chain_xml->Attribute("tip_link");
159  if (!base)
160  {
161  CONSOLE_BRIDGE_logError("Base link name not specified for chain");
162  continue;
163  }
164  if (!tip)
165  {
166  CONSOLE_BRIDGE_logError("Tip link name not specified for chain");
167  continue;
168  }
169  std::string base_str = boost::trim_copy(std::string(base));
170  std::string tip_str = boost::trim_copy(std::string(tip));
171  if (!urdf_model.getLink(base_str))
172  {
173  CONSOLE_BRIDGE_logError("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", base, gname);
174  continue;
175  }
176  if (!urdf_model.getLink(tip_str))
177  {
178  CONSOLE_BRIDGE_logError("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", tip, gname);
179  continue;
180  }
181  bool found = false;
182  urdf::LinkConstSharedPtr l = urdf_model.getLink(tip_str);
183  std::set<std::string> seen;
184  while (!found && l)
185  {
186  seen.insert(l->name);
187  if (l->name == base_str)
188  found = true;
189  else
190  l = l->getParent();
191  }
192  if (!found)
193  {
194  l = urdf_model.getLink(base_str);
195  while (!found && l)
196  {
197  if (seen.find(l->name) != seen.end())
198  found = true;
199  else
200  l = l->getParent();
201  }
202  }
203  if (found)
204  g.chains_.push_back(std::make_pair(base_str, tip_str));
205  else
206  CONSOLE_BRIDGE_logError("Links '%s' and '%s' do not form a chain. Not included in group '%s'", base, tip, gname);
207  }
208 
209  // get the subgroups in the groups
210  for (TiXmlElement* subg_xml = group_xml->FirstChildElement("group"); subg_xml; subg_xml = subg_xml->NextSiblingElement("group"))
211  {
212  const char *sub = subg_xml->Attribute("name");
213  if (!sub)
214  {
215  CONSOLE_BRIDGE_logError("Group name not specified when included as subgroup");
216  continue;
217  }
218  g.subgroups_.push_back(boost::trim_copy(std::string(sub)));
219  }
220  if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty())
221  CONSOLE_BRIDGE_logWarn("Group '%s' is empty.", gname);
222  groups_.push_back(g);
223  }
224 
225  // check the subgroups
226  std::set<std::string> known_groups;
227  bool update = true;
228  while (update)
229  {
230  update = false;
231  for (std::size_t i = 0 ; i < groups_.size() ; ++i)
232  {
233  if (known_groups.find(groups_[i].name_) != known_groups.end())
234  continue;
235  if (groups_[i].subgroups_.empty())
236  {
237  known_groups.insert(groups_[i].name_);
238  update = true;
239  }
240  else
241  {
242  bool ok = true;
243  for (std::size_t j = 0 ; ok && j < groups_[i].subgroups_.size() ; ++j)
244  if (known_groups.find(groups_[i].subgroups_[j]) == known_groups.end())
245  ok = false;
246  if (ok)
247  {
248  known_groups.insert(groups_[i].name_);
249  update = true;
250  }
251  }
252  }
253  }
254 
255  // if there are erroneous groups, keep only the valid ones
256  if (known_groups.size() != groups_.size())
257  {
258  std::vector<Group> correct;
259  for (std::size_t i = 0 ; i < groups_.size() ; ++i)
260  if (known_groups.find(groups_[i].name_) != known_groups.end())
261  correct.push_back(groups_[i]);
262  else
263  CONSOLE_BRIDGE_logError("Group '%s' has unsatisfied subgroups", groups_[i].name_.c_str());
264  groups_.swap(correct);
265  }
266 }
267 
268 void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
269 {
270  for (TiXmlElement* gstate_xml = robot_xml->FirstChildElement("group_state"); gstate_xml; gstate_xml = gstate_xml->NextSiblingElement("group_state"))
271  {
272  const char *sname = gstate_xml->Attribute("name");
273  const char *gname = gstate_xml->Attribute("group");
274  if (!sname)
275  {
276  CONSOLE_BRIDGE_logError("Name of group state is not specified");
277  continue;
278  }
279  if (!gname)
280  {
281  CONSOLE_BRIDGE_logError("Name of group for state '%s' is not specified", sname);
282  continue;
283  }
284 
285  GroupState gs;
286  gs.name_ = boost::trim_copy(std::string(sname));
287  gs.group_ = boost::trim_copy(std::string(gname));
288 
289  bool found = false;
290  for (std::size_t k = 0 ; k < groups_.size() ; ++k)
291  if (groups_[k].name_ == gs.group_)
292  {
293  found = true;
294  break;
295  }
296  if (!found)
297  {
298  CONSOLE_BRIDGE_logError("Group state '%s' specified for group '%s', but that group is not known", sname, gname);
299  continue;
300  }
301 
302  // get the joint values in the group state
303  for (TiXmlElement* joint_xml = gstate_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
304  {
305  const char *jname = joint_xml->Attribute("name");
306  const char *jval = joint_xml->Attribute("value");
307  if (!jname)
308  {
309  CONSOLE_BRIDGE_logError("Joint name not specified in group state '%s'", sname);
310  continue;
311  }
312  if (!jval)
313  {
314  CONSOLE_BRIDGE_logError("Joint name not specified for joint '%s' in group state '%s'", jname, sname);
315  continue;
316  }
317  std::string jname_str = boost::trim_copy(std::string(jname));
318  if (!urdf_model.getJoint(jname_str))
319  {
320  bool missing = true;
321  for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
322  if (virtual_joints_[k].name_ == jname_str)
323  {
324  missing = false;
325  break;
326  }
327  if (missing)
328  {
329  CONSOLE_BRIDGE_logError("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, sname);
330  continue;
331  }
332  }
333  try
334  {
335  std::string jval_str = std::string(jval);
336  std::stringstream ss(jval_str);
337  while (ss.good() && !ss.eof())
338  {
339  std::string val; ss >> val >> std::ws;
340  gs.joint_values_[jname_str].push_back(boost::lexical_cast<double>(val));
341  }
342  }
343  catch (boost::bad_lexical_cast &e)
344  {
345  CONSOLE_BRIDGE_logError("Unable to parse joint value '%s'", jval);
346  }
347 
348  if (gs.joint_values_.empty())
349  CONSOLE_BRIDGE_logError("Unable to parse joint value ('%s') for joint '%s' in group state '%s'", jval, jname, sname);
350  }
351  group_states_.push_back(gs);
352  }
353 }
354 
355 void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
356 {
357  for (TiXmlElement* eef_xml = robot_xml->FirstChildElement("end_effector"); eef_xml; eef_xml = eef_xml->NextSiblingElement("end_effector"))
358  {
359  const char *ename = eef_xml->Attribute("name");
360  const char *gname = eef_xml->Attribute("group");
361  const char *parent = eef_xml->Attribute("parent_link");
362  const char *parent_group = eef_xml->Attribute("parent_group");
363  if (!ename)
364  {
365  CONSOLE_BRIDGE_logError("Name of end effector is not specified");
366  continue;
367  }
368  if (!gname)
369  {
370  CONSOLE_BRIDGE_logError("Group not specified for end effector '%s'", ename);
371  continue;
372  }
373  EndEffector e;
374  e.name_ = std::string(ename); boost::trim(e.name_);
375  e.component_group_ = std::string(gname); boost::trim(e.component_group_);
376  bool found = false;
377  for (std::size_t k = 0 ; k < groups_.size() ; ++k)
378  if (groups_[k].name_ == e.component_group_)
379  {
380  found = true;
381  break;
382  }
383  if (!found)
384  {
385  CONSOLE_BRIDGE_logError("End effector '%s' specified for group '%s', but that group is not known", ename, gname);
386  continue;
387  }
388  if (!parent)
389  {
390  CONSOLE_BRIDGE_logError("Parent link not specified for end effector '%s'", ename);
391  continue;
392  }
393  e.parent_link_ = std::string(parent); boost::trim(e.parent_link_);
394  if (!urdf_model.getLink(e.parent_link_))
395  {
396  CONSOLE_BRIDGE_logError("Link '%s' specified as parent for end effector '%s' is not known to the URDF", parent, ename);
397  continue;
398  }
399  if (parent_group)
400  {
401  e.parent_group_ = std::string(parent_group); boost::trim(e.parent_group_);
402  }
403  end_effectors_.push_back(e);
404  }
405 }
406 
407 void srdf::Model::loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
408 {
409  for (TiXmlElement* cslink_xml = robot_xml->FirstChildElement("link_sphere_approximation"); cslink_xml; cslink_xml = cslink_xml->NextSiblingElement("link_sphere_approximation"))
410  {
411  int non_0_radius_sphere_cnt = 0;
412  const char *link_name = cslink_xml->Attribute("link");
413  if (!link_name)
414  {
415  CONSOLE_BRIDGE_logError("Name of link is not specified in link_collision_spheres");
416  continue;
417  }
418 
419  LinkSpheres link_spheres;
420  link_spheres.link_ = boost::trim_copy(std::string(link_name));
421  if (!urdf_model.getLink(link_spheres.link_))
422  {
423  CONSOLE_BRIDGE_logError("Link '%s' is not known to URDF.", link_name);
424  continue;
425  }
426 
427 
428  // get the spheres for this link
429  int cnt = 0;
430  for (TiXmlElement* sphere_xml = cslink_xml->FirstChildElement("sphere"); sphere_xml; sphere_xml = sphere_xml->NextSiblingElement("sphere"), cnt++)
431  {
432  const char *s_center = sphere_xml->Attribute("center");
433  const char *s_r = sphere_xml->Attribute("radius");
434  if (!s_center || !s_r)
435  {
436  CONSOLE_BRIDGE_logError("Link collision sphere %d for link '%s' does not have both center and radius.", cnt, link_name);
437  continue;
438  }
439 
440  Sphere sphere;
441  try
442  {
443  std::stringstream center(s_center);
444  center.exceptions(std::stringstream::failbit | std::stringstream::badbit);
445  center >> sphere.center_x_ >> sphere.center_y_ >> sphere.center_z_;
446  sphere.radius_ = boost::lexical_cast<double>(s_r);
447  }
448  catch (std::stringstream::failure &e)
449  {
450  CONSOLE_BRIDGE_logError("Link collision sphere %d for link '%s' has bad center attribute value.", cnt, link_name);
451  continue;
452  }
453  catch (boost::bad_lexical_cast &e)
454  {
455  CONSOLE_BRIDGE_logError("Link collision sphere %d for link '%s' has bad radius attribute value.", cnt, link_name);
456  continue;
457  }
458 
459  // ignore radius==0 spheres unless there is only 1 of them
460  //
461  // NOTE:
462  // - If a link has no sphere_approximation then one will be generated
463  // automatically containing a single sphere which encloses the entire
464  // collision geometry. Internally this is represented by not having
465  // a link_sphere_approximations_ entry for this link.
466  // - If a link has only spheres with radius 0 then it will not be
467  // considered for collision detection. In this case the internal
468  // representation is a single radius=0 sphere.
469  // - If a link has at least one sphere with radius>0 then those spheres
470  // are used. Any radius=0 spheres are eliminated.
471  if (sphere.radius_ > std::numeric_limits<double>::epsilon())
472  {
473  if (non_0_radius_sphere_cnt == 0)
474  link_spheres.spheres_.clear();
475  link_spheres.spheres_.push_back(sphere);
476  non_0_radius_sphere_cnt++;
477  }
478  else if (non_0_radius_sphere_cnt == 0)
479  {
480  sphere.center_x_ = 0.0;
481  sphere.center_y_ = 0.0;
482  sphere.center_z_ = 0.0;
483  sphere.radius_ = 0.0;
484  link_spheres.spheres_.clear();
485  link_spheres.spheres_.push_back(sphere);
486  }
487  }
488 
489  if (!link_spheres.spheres_.empty())
490  link_sphere_approximations_.push_back(link_spheres);
491  }
492 }
493 
494 void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
495 {
496  for (TiXmlElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml; c_xml = c_xml->NextSiblingElement("disable_collisions"))
497  {
498  const char *link1 = c_xml->Attribute("link1");
499  const char *link2 = c_xml->Attribute("link2");
500  if (!link1 || !link2)
501  {
502  CONSOLE_BRIDGE_logError("A pair of links needs to be specified to disable collisions");
503  continue;
504  }
506  dc.link1_ = boost::trim_copy(std::string(link1));
507  dc.link2_ = boost::trim_copy(std::string(link2));
508  if (!urdf_model.getLink(dc.link1_))
509  {
510  CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
511  continue;
512  }
513  if (!urdf_model.getLink(dc.link2_))
514  {
515  CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
516  continue;
517  }
518  const char *reason = c_xml->Attribute("reason");
519  if (reason)
520  dc.reason_ = std::string(reason);
521  disabled_collisions_.push_back(dc);
522  }
523 }
524 
525 void srdf::Model::loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
526 {
527  for (TiXmlElement* c_xml = robot_xml->FirstChildElement("passive_joint"); c_xml; c_xml = c_xml->NextSiblingElement("passive_joint"))
528  {
529  const char *name = c_xml->Attribute("name");
530  if (!name)
531  {
532  CONSOLE_BRIDGE_logError("No name specified for passive joint. Ignoring.");
533  continue;
534  }
535  PassiveJoint pj;
536  pj.name_ = boost::trim_copy(std::string(name));
537 
538  // see if a virtual joint was marked as passive
539  bool vjoint = false;
540  for (std::size_t i = 0 ; !vjoint && i < virtual_joints_.size() ; ++i)
541  if (virtual_joints_[i].name_ == pj.name_)
542  vjoint = true;
543 
544  if (!vjoint && !urdf_model.getJoint(pj.name_))
545  {
546  CONSOLE_BRIDGE_logError("Joint '%s' marked as passive is not known to the URDF. Ignoring.", name);
547  continue;
548  }
549  passive_joints_.push_back(pj);
550  }
551 }
552 
553 bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
554 {
555  clear();
556  if (!robot_xml || robot_xml->ValueStr() != "robot")
557  {
558  CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file");
559  return false;
560  }
561 
562  // get the robot name
563  const char *name = robot_xml->Attribute("name");
564  if (!name)
565  CONSOLE_BRIDGE_logError("No name given for the robot.");
566  else
567  {
568  name_ = std::string(name); boost::trim(name_);
569  if (name_ != urdf_model.getName())
570  CONSOLE_BRIDGE_logError("Semantic description is not specified for the same robot as the URDF");
571  }
572 
573  loadVirtualJoints(urdf_model, robot_xml);
574  loadGroups(urdf_model, robot_xml);
575  loadGroupStates(urdf_model, robot_xml);
576  loadEndEffectors(urdf_model, robot_xml);
577  loadLinkSphereApproximations(urdf_model, robot_xml);
578  loadDisabledCollisions(urdf_model, robot_xml);
579  loadPassiveJoints(urdf_model, robot_xml);
580 
581  return true;
582 }
583 
584 bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml)
585 {
586  TiXmlElement *robot_xml = xml ? xml->FirstChildElement("robot") : NULL;
587  if (!robot_xml)
588  {
589  CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file");
590  return false;
591  }
592  return initXml(urdf_model, robot_xml);
593 }
594 
595 
596 bool srdf::Model::initFile(const urdf::ModelInterface &urdf_model, const std::string& filename)
597 {
598  // get the entire file
599  std::string xml_string;
600  std::fstream xml_file(filename.c_str(), std::fstream::in);
601  if (xml_file.is_open())
602  {
603  while (xml_file.good())
604  {
605  std::string line;
606  std::getline(xml_file, line);
607  xml_string += (line + "\n");
608  }
609  xml_file.close();
610  return initString(urdf_model, xml_string);
611  }
612  else
613  {
614  CONSOLE_BRIDGE_logError("Could not open file [%s] for parsing.", filename.c_str());
615  return false;
616  }
617 }
618 
619 bool srdf::Model::initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring)
620 {
621  TiXmlDocument xml_doc;
622  xml_doc.Parse(xmlstring.c_str());
623  return initXml(urdf_model, &xml_doc);
624 }
625 
626 
628 {
629  name_ = "";
630  groups_.clear();
631  group_states_.clear();
632  virtual_joints_.clear();
633  end_effectors_.clear();
635  disabled_collisions_.clear();
636  passive_joints_.clear();
637 }
638 
639 std::vector<std::pair<std::string, std::string> > srdf::Model::getDisabledCollisions() const
640 {
641  std::vector<std::pair<std::string, std::string> > result;
642  for (std::size_t i = 0 ; i < disabled_collisions_.size() ; ++i)
643  result.push_back(std::make_pair(disabled_collisions_[i].link1_, disabled_collisions_[i].link2_));
644  return result;
645 }
#define CONSOLE_BRIDGE_logWarn(fmt,...)
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
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
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
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
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
std::vector< Group > groups_
Definition: model.h:265
void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
Definition: model.cpp:268
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
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
ROSCPP_DECL bool ok()
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
std::string name_
The name of the group.
Definition: model.h:81
std::vector< DisabledCollision > disabled_collisions_
Definition: model.h:270
def xml_string(rootXml, addHeader=True)
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
#define CONSOLE_BRIDGE_logError(fmt,...)
std::string child_link_
The link this joint applies to.
Definition: model.h:124
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
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
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 Thu Jun 6 2019 19:59:13