1 /********************************************************************* 2 * Software License Agreement (BSD License) 3 * 4 * Copyright (c) 2008, 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: Wim Meeussen */ 36 37 #ifndef URDF_INTERFACE_MODEL_H 38 #define URDF_INTERFACE_MODEL_H 39 40 #include <string> 41 #include <map> 42 #include <urdf_model/link.h> 43 #include <urdf_model/types.h> 44 #include <urdf_exception/exception.h> 45 46 namespace urdf { 47 48 class ModelInterface 49 { 50 public: getRoot(void)51 LinkConstSharedPtr getRoot(void) const{return this->root_link_;}; getLink(const std::string & name)52 LinkConstSharedPtr getLink(const std::string& name) const 53 { 54 LinkConstSharedPtr ptr; 55 if (this->links_.find(name) == this->links_.end()) 56 ptr.reset(); 57 else 58 ptr = this->links_.find(name)->second; 59 return ptr; 60 }; 61 getJoint(const std::string & name)62 JointConstSharedPtr getJoint(const std::string& name) const 63 { 64 JointConstSharedPtr ptr; 65 if (this->joints_.find(name) == this->joints_.end()) 66 ptr.reset(); 67 else 68 ptr = this->joints_.find(name)->second; 69 return ptr; 70 }; 71 72 getName()73 const std::string& getName() const {return name_;}; getLinks(std::vector<LinkSharedPtr> & links)74 void getLinks(std::vector<LinkSharedPtr >& links) const 75 { 76 for (std::map<std::string,LinkSharedPtr>::const_iterator link = this->links_.begin();link != this->links_.end(); link++) 77 { 78 links.push_back(link->second); 79 } 80 }; 81 clear()82 void clear() 83 { 84 name_.clear(); 85 this->links_.clear(); 86 this->joints_.clear(); 87 this->materials_.clear(); 88 this->root_link_.reset(); 89 }; 90 91 /// non-const getLink() getLink(const std::string & name,LinkSharedPtr & link)92 void getLink(const std::string& name, LinkSharedPtr &link) const 93 { 94 LinkSharedPtr ptr; 95 if (this->links_.find(name) == this->links_.end()) 96 ptr.reset(); 97 else 98 ptr = this->links_.find(name)->second; 99 link = ptr; 100 }; 101 102 /// non-const getMaterial() getMaterial(const std::string & name)103 MaterialSharedPtr getMaterial(const std::string& name) const 104 { 105 MaterialSharedPtr ptr; 106 if (this->materials_.find(name) == this->materials_.end()) 107 ptr.reset(); 108 else 109 ptr = this->materials_.find(name)->second; 110 return ptr; 111 }; 112 initTree(std::map<std::string,std::string> & parent_link_tree)113 void initTree(std::map<std::string, std::string> &parent_link_tree) 114 { 115 // loop through all joints, for every link, assign children links and children joints 116 for (std::map<std::string, JointSharedPtr>::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) 117 { 118 std::string parent_link_name = joint->second->parent_link_name; 119 std::string child_link_name = joint->second->child_link_name; 120 121 if (parent_link_name.empty() || child_link_name.empty()) 122 { 123 throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification."); 124 } 125 else 126 { 127 // find child and parent links 128 LinkSharedPtr child_link, parent_link; 129 this->getLink(child_link_name, child_link); 130 if (!child_link) 131 { 132 throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found"); 133 } 134 this->getLink(parent_link_name, parent_link); 135 if (!parent_link) 136 { 137 throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found. This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file."); 138 } 139 140 //set parent link for child link 141 child_link->setParent(parent_link); 142 143 //set parent joint for child link 144 child_link->parent_joint = joint->second; 145 146 //set child joint for parent link 147 parent_link->child_joints.push_back(joint->second); 148 149 //set child link for parent link 150 parent_link->child_links.push_back(child_link); 151 152 // fill in child/parent string map 153 parent_link_tree[child_link->name] = parent_link_name; 154 } 155 } 156 } 157 initRoot(const std::map<std::string,std::string> & parent_link_tree)158 void initRoot(const std::map<std::string, std::string> &parent_link_tree) 159 { 160 this->root_link_.reset(); 161 162 // find the links that have no parent in the tree 163 for (std::map<std::string, LinkSharedPtr>::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++) 164 { 165 std::map<std::string, std::string >::const_iterator parent = parent_link_tree.find(l->first); 166 if (parent == parent_link_tree.end()) 167 { 168 // store root link 169 if (!this->root_link_) 170 { 171 getLink(l->first, this->root_link_); 172 } 173 // we already found a root link 174 else 175 { 176 throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]"); 177 } 178 } 179 } 180 if (!this->root_link_) 181 { 182 throw ParseError("No root link found. The robot xml is not a valid tree."); 183 } 184 } 185 186 187 /// \brief complete list of Links 188 std::map<std::string, LinkSharedPtr> links_; 189 /// \brief complete list of Joints 190 std::map<std::string, JointSharedPtr> joints_; 191 /// \brief complete list of Materials 192 std::map<std::string, MaterialSharedPtr> materials_; 193 194 /// \brief The name of the robot model 195 std::string name_; 196 197 /// \brief The root is always a link (the parent of the tree describing the robot) 198 LinkSharedPtr root_link_; 199 200 201 202 }; 203 204 } 205 206 #endif 207