GCOP
1.0
|
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, 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: Wim Meeussen */ 00036 00037 #ifndef URDF_INTERFACE_MODEL_H 00038 #define URDF_INTERFACE_MODEL_H 00039 00040 #include <string> 00041 #include <map> 00042 #include <boost/function.hpp> 00043 #include "urdflink.h" 00044 00045 00046 namespace gcop_urdf { 00047 00048 class ModelInterface 00049 { 00050 public: 00051 boost::shared_ptr<const Link> getRoot(void) const{return this->root_link_;}; 00052 boost::shared_ptr<const Link> getLink(const std::string& name) const 00053 { 00054 boost::shared_ptr<const Link> ptr; 00055 if (this->links_.find(name) == this->links_.end()) 00056 ptr.reset(); 00057 else 00058 ptr = this->links_.find(name)->second; 00059 return ptr; 00060 }; 00061 00062 boost::shared_ptr<const Joint> getJoint(const std::string& name) const 00063 { 00064 boost::shared_ptr<const Joint> ptr; 00065 if (this->joints_.find(name) == this->joints_.end()) 00066 ptr.reset(); 00067 else 00068 ptr = this->joints_.find(name)->second; 00069 return ptr; 00070 }; 00071 00072 00073 const std::string& getName() const {return name_;}; 00074 void getLinks(std::vector<boost::shared_ptr<Link> >& links) const 00075 { 00076 for (std::map<std::string,boost::shared_ptr<Link> >::const_iterator link = this->links_.begin();link != this->links_.end(); link++) 00077 { 00078 links.push_back(link->second); 00079 } 00080 }; 00081 00082 void clear() 00083 { 00084 name_.clear(); 00085 this->links_.clear(); 00086 this->joints_.clear(); 00087 this->materials_.clear(); 00088 this->root_link_.reset(); 00089 }; 00090 00092 void getLink(const std::string& name,boost::shared_ptr<Link> &link) const 00093 { 00094 boost::shared_ptr<Link> ptr; 00095 if (this->links_.find(name) == this->links_.end()) 00096 ptr.reset(); 00097 else 00098 ptr = this->links_.find(name)->second; 00099 link = ptr; 00100 }; 00101 00103 boost::shared_ptr<Material> getMaterial(const std::string& name) const 00104 { 00105 boost::shared_ptr<Material> ptr; 00106 if (this->materials_.find(name) == this->materials_.end()) 00107 ptr.reset(); 00108 else 00109 ptr = this->materials_.find(name)->second; 00110 return ptr; 00111 }; 00112 00115 bool initTree(std::map<std::string, std::string> &parent_link_tree) 00116 { 00117 // loop through all joints, for every link, assign children links and children joints 00118 for (std::map<std::string,boost::shared_ptr<Joint> >::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++) 00119 { 00120 std::string parent_link_name = joint->second->parent_link_name; 00121 std::string child_link_name = joint->second->child_link_name; 00122 00123 //printf("build tree: joint: '%s' has parent link '%s' and child link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str()); 00124 if (parent_link_name.empty() || child_link_name.empty()) 00125 { 00126 //printf(" Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str()); 00127 return false; 00128 } 00129 else 00130 { 00131 // find child and parent links 00132 boost::shared_ptr<Link> child_link, parent_link; 00133 this->getLink(child_link_name, child_link); 00134 if (!child_link) 00135 { 00136 //printf(" child link '%s' of joint '%s' not found", child_link_name.c_str(), joint->first.c_str() ); 00137 return false; 00138 } 00139 this->getLink(parent_link_name, parent_link); 00140 if (!parent_link) 00141 { 00142 //printf(" parent link '%s' of joint '%s' not found. The Boxturtle urdf parser used to automatically add this link for you, but 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 from your urdf file, or add \"<link name=\"%s\" />\" to your urdf file.", parent_link_name.c_str(), joint->first.c_str(), parent_link_name.c_str() ); 00143 return false; 00144 } 00145 00146 //set parent link for child link 00147 child_link->setParent(parent_link); 00148 00149 //set parent joint for child link 00150 child_link->setParentJoint(joint->second); 00151 00152 //set child joint for parent link 00153 parent_link->addChildJoint(joint->second); 00154 00155 //set child link for parent link 00156 parent_link->addChild(child_link); 00157 00158 // fill in child/parent string map 00159 parent_link_tree[child_link->name] = parent_link_name; 00160 00161 //printf(" now Link '%s' has %i children ", parent_link->name.c_str(), (int)parent_link->child_links.size()); 00162 } 00163 } 00164 00165 return true; 00166 }; 00167 00168 00171 bool initRoot(std::map<std::string, std::string> &parent_link_tree) 00172 { 00173 this->root_link_.reset(); 00174 00175 // find the links that have no parent in the tree 00176 for (std::map<std::string, boost::shared_ptr<Link> >::iterator l=this->links_.begin(); l!=this->links_.end(); l++) 00177 { 00178 std::map<std::string, std::string >::iterator parent = parent_link_tree.find(l->first); 00179 if (parent == parent_link_tree.end()) 00180 { 00181 // store root link 00182 if (!this->root_link_) 00183 { 00184 getLink(l->first, this->root_link_); 00185 } 00186 // we already found a root link 00187 else{ 00188 //printf("Two root links found: '%s' and '%s'", this->root_link_->name.c_str(), l->first.c_str()); 00189 return false; 00190 } 00191 } 00192 } 00193 if (!this->root_link_) 00194 { 00195 //printf("No root link found. The robot xml is not a valid tree."); 00196 return false; 00197 } 00198 //printf("Link '%s' is the root link", this->root_link_->name.c_str()); 00199 00200 return true; 00201 }; 00202 00204 std::map<std::string, boost::shared_ptr<Link> > links_; 00206 std::map<std::string, boost::shared_ptr<Joint> > joints_; 00208 std::map<std::string, boost::shared_ptr<Material> > materials_; 00209 00210 std::string name_; 00211 00216 boost::shared_ptr<Link> root_link_; 00217 00218 00219 00220 }; 00221 00222 } 00223 00224 #endif