model.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author Ioan Sucan */
00036 
00037 #include "srdfdom/model.h"
00038 #include <console_bridge/console.h>
00039 #include <boost/lexical_cast.hpp>
00040 #include <boost/algorithm/string/trim.hpp>
00041 #include <algorithm>
00042 #include <fstream>
00043 #include <sstream>
00044 #include <set>
00045 #include <limits>
00046 
00047 void srdf::Model::loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
00048 {
00049   for (TiXmlElement* vj_xml = robot_xml->FirstChildElement("virtual_joint"); vj_xml; vj_xml = vj_xml->NextSiblingElement("virtual_joint"))
00050   {
00051     const char *jname = vj_xml->Attribute("name");
00052     const char *child = vj_xml->Attribute("child_link");
00053     const char *parent = vj_xml->Attribute("parent_frame");
00054     const char *type = vj_xml->Attribute("type");
00055     if (!jname)
00056     {
00057       logError("Name of virtual joint is not specified");
00058       continue;
00059     }
00060     if (!child)
00061     {
00062       logError("Child link of virtual joint is not specified");
00063       continue;
00064     }
00065     if (!urdf_model.getLink(boost::trim_copy(std::string(child))))
00066     {
00067       logError("Virtual joint does not attach to a link on the robot (link '%s' is not known)", child);
00068       continue;
00069     }
00070     if (!parent)
00071     {
00072       logError("Parent frame of virtual joint is not specified");
00073       continue;
00074     }
00075     if (!type)
00076     {
00077       logError("Type of virtual joint is not specified");
00078       continue;
00079     }
00080     VirtualJoint vj;
00081     vj.type_ = std::string(type); boost::trim(vj.type_);
00082     std::transform(vj.type_.begin(), vj.type_.end(), vj.type_.begin(), ::tolower);
00083     if (vj.type_ != "planar" && vj.type_ != "floating" && vj.type_ != "fixed")
00084     {
00085       logError("Unknown type of joint: '%s'. Assuming 'fixed' instead. Other known types are 'planar' and 'floating'.", type);
00086       vj.type_ = "fixed";
00087     }
00088     vj.name_ = std::string(jname); boost::trim(vj.name_);        
00089     vj.child_link_ = std::string(child); boost::trim(vj.child_link_);
00090     vj.parent_frame_ = std::string(parent); boost::trim(vj.parent_frame_);
00091     virtual_joints_.push_back(vj);
00092   }
00093 }
00094 
00095 void srdf::Model::loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
00096 {
00097   for (TiXmlElement* group_xml = robot_xml->FirstChildElement("group"); group_xml; group_xml = group_xml->NextSiblingElement("group"))
00098   {
00099     const char *gname = group_xml->Attribute("name");
00100     if (!gname)
00101     {
00102       logError("Group name not specified");
00103       continue;
00104     }
00105     Group g;
00106     g.name_ = std::string(gname); boost::trim(g.name_);
00107     
00108     // get the links in the groups
00109     for (TiXmlElement* link_xml = group_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
00110     {
00111       const char *lname = link_xml->Attribute("name");
00112       if (!lname)
00113       {
00114         logError("Link name not specified");
00115         continue;
00116       }
00117       std::string lname_str = boost::trim_copy(std::string(lname));
00118       if (!urdf_model.getLink(lname_str))
00119       {
00120         logError("Link '%s' declared as part of group '%s' is not known to the URDF", lname, gname);
00121         continue;
00122       }
00123       g.links_.push_back(lname_str);
00124     }
00125     
00126     // get the joints in the groups
00127     for (TiXmlElement* joint_xml = group_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
00128     {
00129       const char *jname = joint_xml->Attribute("name");
00130       if (!jname)
00131       {
00132         logError("Joint name not specified");
00133         continue;
00134       }
00135       std::string jname_str = boost::trim_copy(std::string(jname));
00136       if (!urdf_model.getJoint(jname_str))
00137       {
00138         bool missing = true;
00139         for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
00140           if (virtual_joints_[k].name_ == jname_str)
00141           {
00142             missing = false;
00143             break;
00144           }
00145         if (missing)
00146         {
00147           logError("Joint '%s' declared as part of group '%s' is not known to the URDF", jname, gname);
00148           continue;
00149         }
00150       }
00151       g.joints_.push_back(jname_str);
00152     }
00153     
00154     // get the chains in the groups
00155     for (TiXmlElement* chain_xml = group_xml->FirstChildElement("chain"); chain_xml; chain_xml = chain_xml->NextSiblingElement("chain"))
00156     {
00157       const char *base = chain_xml->Attribute("base_link");
00158       const char *tip = chain_xml->Attribute("tip_link");
00159       if (!base)
00160       {
00161         logError("Base link name not specified for chain");
00162         continue;
00163       }
00164       if (!tip)
00165       {
00166         logError("Tip link name not specified for chain");
00167         continue;
00168       }
00169       std::string base_str = boost::trim_copy(std::string(base));
00170       std::string tip_str = boost::trim_copy(std::string(tip));
00171       if (!urdf_model.getLink(base_str))
00172       {
00173         logError("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", base, gname);
00174         continue;
00175       }
00176       if (!urdf_model.getLink(tip_str))
00177       {
00178         logError("Link '%s' declared as part of a chain in group '%s' is not known to the URDF", tip, gname);
00179         continue;
00180       }
00181       bool found = false;
00182       urdf::LinkConstSharedPtr l = urdf_model.getLink(tip_str);
00183       std::set<std::string> seen;
00184       while (!found && l)
00185       {
00186         seen.insert(l->name);
00187         if (l->name == base_str)
00188           found = true;
00189         else
00190           l = l->getParent();
00191       }
00192       if (!found)
00193       {
00194         l = urdf_model.getLink(base_str);
00195         while (!found && l)
00196         {
00197           if (seen.find(l->name) != seen.end())
00198             found = true;
00199           else
00200             l = l->getParent();
00201         }
00202       }
00203       if (found)
00204         g.chains_.push_back(std::make_pair(base_str, tip_str));
00205       else
00206         logError("Links '%s' and '%s' do not form a chain. Not included in group '%s'", base, tip, gname);
00207     }
00208     
00209     // get the subgroups in the groups
00210     for (TiXmlElement* subg_xml = group_xml->FirstChildElement("group"); subg_xml; subg_xml = subg_xml->NextSiblingElement("group"))
00211     {
00212       const char *sub = subg_xml->Attribute("name");
00213       if (!sub)
00214       {
00215         logError("Group name not specified when included as subgroup");
00216         continue;
00217       }
00218       g.subgroups_.push_back(boost::trim_copy(std::string(sub)));
00219     }
00220     if (g.links_.empty() && g.joints_.empty() && g.chains_.empty() && g.subgroups_.empty())
00221       logWarn("Group '%s' is empty.", gname);
00222     groups_.push_back(g);
00223   }
00224   
00225   // check the subgroups
00226   std::set<std::string> known_groups;
00227   bool update = true;
00228   while (update)
00229   {
00230     update = false;
00231     for (std::size_t i = 0 ; i < groups_.size() ; ++i)
00232     {
00233       if (known_groups.find(groups_[i].name_) != known_groups.end())
00234         continue;
00235       if (groups_[i].subgroups_.empty())
00236       {
00237         known_groups.insert(groups_[i].name_);
00238         update = true;
00239       }
00240       else
00241       {
00242         bool ok = true;
00243         for (std::size_t j = 0 ; ok && j < groups_[i].subgroups_.size() ; ++j)
00244           if (known_groups.find(groups_[i].subgroups_[j]) == known_groups.end())
00245             ok = false;
00246         if (ok)
00247         {
00248           known_groups.insert(groups_[i].name_);
00249           update = true;
00250         }
00251       }
00252     }
00253   }
00254   
00255   // if there are erroneous groups, keep only the valid ones
00256   if (known_groups.size() != groups_.size())
00257   {
00258     std::vector<Group> correct;
00259     for (std::size_t i = 0 ; i < groups_.size() ; ++i)
00260       if (known_groups.find(groups_[i].name_) != known_groups.end())
00261         correct.push_back(groups_[i]);
00262       else
00263         logError("Group '%s' has unsatisfied subgroups", groups_[i].name_.c_str());
00264     groups_.swap(correct);
00265   }
00266 }
00267 
00268 void srdf::Model::loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
00269 {
00270   for (TiXmlElement* gstate_xml = robot_xml->FirstChildElement("group_state"); gstate_xml; gstate_xml = gstate_xml->NextSiblingElement("group_state"))
00271   {
00272     const char *sname = gstate_xml->Attribute("name");
00273     const char *gname = gstate_xml->Attribute("group");
00274     if (!sname)
00275     {
00276       logError("Name of group state is not specified");
00277       continue;
00278     }
00279     if (!gname)
00280     {
00281       logError("Name of group for state '%s' is not specified", sname);
00282       continue;
00283     }
00284     
00285     GroupState gs;
00286     gs.name_ = boost::trim_copy(std::string(sname));
00287     gs.group_ = boost::trim_copy(std::string(gname));
00288     
00289     bool found = false;
00290     for (std::size_t k = 0 ; k < groups_.size() ; ++k)
00291       if (groups_[k].name_ == gs.group_)
00292       {
00293         found = true;
00294         break;
00295       }
00296     if (!found)
00297     {
00298       logError("Group state '%s' specified for group '%s', but that group is not known", sname, gname);
00299       continue;
00300     }
00301     
00302     // get the joint values in the group state
00303     for (TiXmlElement* joint_xml = gstate_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
00304     {
00305       const char *jname = joint_xml->Attribute("name");
00306       const char *jval = joint_xml->Attribute("value");
00307       if (!jname)
00308       {
00309         logError("Joint name not specified in group state '%s'", sname);
00310         continue;
00311       }
00312       if (!jval)
00313       {
00314         logError("Joint name not specified for joint '%s' in group state '%s'", jname, sname);
00315         continue;
00316       }
00317       std::string jname_str = boost::trim_copy(std::string(jname));
00318       if (!urdf_model.getJoint(jname_str))
00319       {
00320         bool missing = true;
00321         for (std::size_t k = 0 ; k < virtual_joints_.size() ; ++k)
00322           if (virtual_joints_[k].name_ == jname_str)
00323           {
00324             missing = false;
00325             break;
00326           }
00327         if (missing)
00328         {
00329           logError("Joint '%s' declared as part of group state '%s' is not known to the URDF", jname, sname);
00330           continue;
00331         }
00332       }
00333       try
00334       {
00335         std::string jval_str = std::string(jval);
00336         std::stringstream ss(jval_str);
00337         while (ss.good() && !ss.eof())
00338         {
00339           std::string val; ss >> val >> std::ws;
00340           gs.joint_values_[jname_str].push_back(boost::lexical_cast<double>(val));
00341         }
00342       }
00343       catch (boost::bad_lexical_cast &e)
00344       {
00345         logError("Unable to parse joint value '%s'", jval);
00346       }
00347       
00348       if (gs.joint_values_.empty())
00349         logError("Unable to parse joint value ('%s') for joint '%s' in group state '%s'", jval, jname, sname);
00350     }
00351     group_states_.push_back(gs);
00352   }
00353 }
00354 
00355 void srdf::Model::loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
00356 {
00357   for (TiXmlElement* eef_xml = robot_xml->FirstChildElement("end_effector"); eef_xml; eef_xml = eef_xml->NextSiblingElement("end_effector"))
00358   {
00359     const char *ename = eef_xml->Attribute("name");
00360     const char *gname = eef_xml->Attribute("group");
00361     const char *parent = eef_xml->Attribute("parent_link");
00362     const char *parent_group = eef_xml->Attribute("parent_group");
00363     if (!ename)
00364     {
00365       logError("Name of end effector is not specified");
00366       continue;
00367     }
00368     if (!gname)
00369     {
00370       logError("Group not specified for end effector '%s'", ename);
00371       continue;
00372     }
00373     EndEffector e;
00374     e.name_ = std::string(ename); boost::trim(e.name_);
00375     e.component_group_ = std::string(gname); boost::trim(e.component_group_);
00376     bool found = false;
00377     for (std::size_t k = 0 ; k < groups_.size() ; ++k)
00378       if (groups_[k].name_ == e.component_group_)
00379       {
00380         found = true;
00381         break;
00382       }
00383     if (!found)
00384     {
00385       logError("End effector '%s' specified for group '%s', but that group is not known", ename, gname);
00386       continue;
00387     }
00388     if (!parent)
00389     {
00390       logError("Parent link not specified for end effector '%s'", ename);
00391       continue;
00392     }
00393     e.parent_link_ = std::string(parent); boost::trim(e.parent_link_);
00394     if (!urdf_model.getLink(e.parent_link_))
00395     {
00396       logError("Link '%s' specified as parent for end effector '%s' is not known to the URDF", parent, ename);
00397       continue;
00398     }
00399     if (parent_group)
00400     {
00401       e.parent_group_ = std::string(parent_group); boost::trim(e.parent_group_);
00402     }
00403     end_effectors_.push_back(e);
00404   }
00405 }
00406 
00407 void srdf::Model::loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
00408 {
00409   for (TiXmlElement* cslink_xml = robot_xml->FirstChildElement("link_sphere_approximation"); cslink_xml; cslink_xml = cslink_xml->NextSiblingElement("link_sphere_approximation"))
00410   {
00411     int non_0_radius_sphere_cnt = 0;
00412     const char *link_name = cslink_xml->Attribute("link");
00413     if (!link_name)
00414     {
00415       logError("Name of link is not specified in link_collision_spheres");
00416       continue;
00417     }
00418     
00419     LinkSpheres link_spheres;
00420     link_spheres.link_ = boost::trim_copy(std::string(link_name));
00421     if (!urdf_model.getLink(link_spheres.link_))
00422     {
00423       logError("Link '%s' is not known to URDF.", link_name);
00424       continue;
00425     }
00426     
00427 
00428     // get the spheres for this link
00429     int cnt = 0;
00430     for (TiXmlElement* sphere_xml = cslink_xml->FirstChildElement("sphere"); sphere_xml; sphere_xml = sphere_xml->NextSiblingElement("sphere"), cnt++)
00431     {
00432       const char *s_center = sphere_xml->Attribute("center");
00433       const char *s_r = sphere_xml->Attribute("radius");
00434       if (!s_center || !s_r)
00435       {
00436         logError("Link collision sphere %d for link '%s' does not have both center and radius.", cnt, link_name);
00437         continue;
00438       }
00439 
00440       Sphere sphere;
00441       try
00442       {
00443         std::stringstream center(s_center);
00444         center.exceptions(std::stringstream::failbit | std::stringstream::badbit);
00445         center >> sphere.center_x_ >> sphere.center_y_ >> sphere.center_z_;
00446         sphere.radius_ = boost::lexical_cast<double>(s_r);
00447       }
00448       catch (std::stringstream::failure &e)
00449       {
00450         logError("Link collision sphere %d for link '%s' has bad center attribute value.", cnt, link_name);
00451         continue;
00452       }
00453       catch (boost::bad_lexical_cast &e)
00454       {
00455         logError("Link collision sphere %d for link '%s' has bad radius attribute value.", cnt, link_name);
00456         continue;
00457       }
00458 
00459       // ignore radius==0 spheres unless there is only 1 of them
00460       //
00461       // NOTE:
00462       //  - If a link has no sphere_approximation then one will be generated
00463       //     automatically containing a single sphere which encloses the entire
00464       //     collision geometry.  Internally this is represented by not having
00465       //     a link_sphere_approximations_ entry for this link.
00466       //  - If a link has only spheres with radius 0 then it will not be
00467       //     considered for collision detection.  In this case the internal
00468       //     representation is a single radius=0 sphere.
00469       //  - If a link has at least one sphere with radius>0 then those spheres
00470       //     are used.  Any radius=0 spheres are eliminated.
00471       if (sphere.radius_ > std::numeric_limits<double>::epsilon())
00472       {
00473         if (non_0_radius_sphere_cnt == 0)
00474           link_spheres.spheres_.clear();
00475         link_spheres.spheres_.push_back(sphere);
00476         non_0_radius_sphere_cnt++;
00477       }
00478       else if (non_0_radius_sphere_cnt == 0)
00479       {
00480         sphere.center_x_ = 0.0;
00481         sphere.center_y_ = 0.0;
00482         sphere.center_z_ = 0.0;
00483         sphere.radius_ = 0.0;
00484         link_spheres.spheres_.clear();
00485         link_spheres.spheres_.push_back(sphere);
00486       }
00487     }
00488 
00489     if (!link_spheres.spheres_.empty())
00490       link_sphere_approximations_.push_back(link_spheres);
00491   }
00492 }
00493 
00494 void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
00495 {
00496   for (TiXmlElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml; c_xml = c_xml->NextSiblingElement("disable_collisions"))
00497   {
00498     const char *link1 = c_xml->Attribute("link1");
00499     const char *link2 = c_xml->Attribute("link2");
00500     if (!link1 || !link2)
00501     {
00502       logError("A pair of links needs to be specified to disable collisions");
00503       continue;
00504     }
00505     DisabledCollision dc;
00506     dc.link1_ = boost::trim_copy(std::string(link1));
00507     dc.link2_ = boost::trim_copy(std::string(link2));
00508     if (!urdf_model.getLink(dc.link1_))
00509     {
00510       logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link1);
00511       continue;
00512     }
00513     if (!urdf_model.getLink(dc.link2_))
00514     {
00515       logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link2);
00516       continue;
00517     }
00518     const char *reason = c_xml->Attribute("reason");
00519     if (reason)
00520       dc.reason_ = std::string(reason);
00521     disabled_collisions_.push_back(dc);
00522   }
00523 }
00524 
00525 void srdf::Model::loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
00526 {  
00527   for (TiXmlElement* c_xml = robot_xml->FirstChildElement("passive_joint"); c_xml; c_xml = c_xml->NextSiblingElement("passive_joint"))
00528   {
00529     const char *name = c_xml->Attribute("name");
00530     if (!name)
00531     {
00532       logError("No name specified for passive joint. Ignoring.");
00533       continue;
00534     }
00535     PassiveJoint pj;
00536     pj.name_ = boost::trim_copy(std::string(name));
00537 
00538     // see if a virtual joint was marked as passive
00539     bool vjoint = false;
00540     for (std::size_t i = 0 ; !vjoint && i < virtual_joints_.size() ; ++i)
00541       if (virtual_joints_[i].name_ == pj.name_)
00542         vjoint = true;
00543     
00544     if (!vjoint && !urdf_model.getJoint(pj.name_))
00545     {
00546       logError("Joint '%s' marked as passive is not known to the URDF. Ignoring.", name);
00547       continue;
00548     }
00549     passive_joints_.push_back(pj);
00550   }
00551 }
00552 
00553 bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml)
00554 {
00555   clear();
00556   if (!robot_xml || robot_xml->ValueStr() != "robot")
00557   {
00558     logError("Could not find the 'robot' element in the xml file");
00559     return false;
00560   }
00561   
00562   // get the robot name
00563   const char *name = robot_xml->Attribute("name");
00564   if (!name)
00565     logError("No name given for the robot.");
00566   else
00567   {
00568     name_ = std::string(name); boost::trim(name_);
00569     if (name_ != urdf_model.getName())
00570       logError("Semantic description is not specified for the same robot as the URDF");
00571   }
00572   
00573   loadVirtualJoints(urdf_model, robot_xml);
00574   loadGroups(urdf_model, robot_xml);
00575   loadGroupStates(urdf_model, robot_xml);
00576   loadEndEffectors(urdf_model, robot_xml); 
00577   loadLinkSphereApproximations(urdf_model, robot_xml);
00578   loadDisabledCollisions(urdf_model, robot_xml);
00579   loadPassiveJoints(urdf_model, robot_xml);
00580   
00581   return true;
00582 }
00583 
00584 bool srdf::Model::initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml)
00585 {
00586   TiXmlElement *robot_xml = xml ? xml->FirstChildElement("robot") : NULL;
00587   if (!robot_xml)
00588   {
00589     logError("Could not find the 'robot' element in the xml file");
00590     return false;
00591   }
00592   return initXml(urdf_model, robot_xml);
00593 }
00594 
00595 
00596 bool srdf::Model::initFile(const urdf::ModelInterface &urdf_model, const std::string& filename)
00597 {
00598   // get the entire file
00599   std::string xml_string;
00600   std::fstream xml_file(filename.c_str(), std::fstream::in);
00601   if (xml_file.is_open())
00602   {
00603     while (xml_file.good())
00604     {
00605       std::string line;
00606       std::getline(xml_file, line);
00607       xml_string += (line + "\n");
00608     }
00609     xml_file.close();
00610     return initString(urdf_model, xml_string);
00611   }
00612   else
00613   {
00614     logError("Could not open file [%s] for parsing.", filename.c_str());
00615     return false;
00616   }
00617 }
00618 
00619 bool srdf::Model::initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring)
00620 {
00621   TiXmlDocument xml_doc;
00622   xml_doc.Parse(xmlstring.c_str());
00623   return initXml(urdf_model, &xml_doc);
00624 }
00625 
00626 
00627 void srdf::Model::clear()
00628 {
00629   name_ = "";
00630   groups_.clear();
00631   group_states_.clear();
00632   virtual_joints_.clear();
00633   end_effectors_.clear();
00634   link_sphere_approximations_.clear();
00635   disabled_collisions_.clear();
00636   passive_joints_.clear();
00637 }
00638 
00639 std::vector<std::pair<std::string, std::string> > srdf::Model::getDisabledCollisions() const
00640 {
00641   std::vector<std::pair<std::string, std::string> > result;
00642   for (std::size_t i = 0 ; i < disabled_collisions_.size() ; ++i)
00643     result.push_back(std::make_pair(disabled_collisions_[i].link1_, disabled_collisions_[i].link2_));
00644   return result;
00645 }


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Wed Feb 8 2017 04:01:15