model.h
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 #ifndef SRDF_MODEL_
00038 #define SRDF_MODEL_
00039 
00040 #include <map>
00041 #include <string>
00042 #include <vector>
00043 #include <utility>
00044 #include <urdf/model.h> // TODO: replace with urdf_model/types.h in Lunar
00045 #include <boost/shared_ptr.hpp>
00046 #include <tinyxml.h>
00047 
00048 namespace urdf
00049 {
00050 typedef boost::shared_ptr<const ::urdf::Link> LinkConstSharedPtr;
00051 }
00052 
00054 namespace srdf
00055 {
00056 
00058 class Model
00059 {
00060 public:
00061   
00062   Model()
00063   {
00064   }
00065   
00066   ~Model()
00067   {
00068   }
00069   
00071   bool initXml(const urdf::ModelInterface &urdf_model, TiXmlElement *xml);
00073   bool initXml(const urdf::ModelInterface &urdf_model, TiXmlDocument *xml);
00075   bool initFile(const urdf::ModelInterface &urdf_model, const std::string& filename);
00077   bool initString(const urdf::ModelInterface &urdf_model, const std::string& xmlstring);
00078   
00083   struct Group
00084   {
00086     std::string                                       name_;
00087     
00093     std::vector<std::string>                          joints_;
00094     
00099     std::vector<std::string>                          links_;
00100     
00105     std::vector<std::pair<std::string, std::string> > chains_;
00106     
00110     std::vector<std::string>                          subgroups_;
00111   };
00112   
00117   struct VirtualJoint
00118   {
00120     std::string name_;
00121     
00123     std::string type_;
00124     
00126     std::string parent_frame_;
00127     
00129     std::string child_link_;
00130   };
00131   
00133   struct EndEffector
00134   {
00136     std::string name_;
00137     
00139     std::string parent_link_;
00140     
00143     std::string parent_group_;
00144     
00146     std::string component_group_;
00147   };
00148   
00150   struct GroupState
00151   {
00153     std::string                                 name_;
00154     
00156     std::string                                 group_;
00157     
00159     std::map<std::string, std::vector<double> > joint_values_;
00160   };
00161   
00163   struct Sphere
00164   {
00166     double center_x_;
00167     double center_y_;
00168     double center_z_;
00169 
00171     double radius_;
00172   };
00173 
00175   struct LinkSpheres
00176   {
00178     std::string link_;
00179 
00181     std::vector<Sphere> spheres_;
00182   };
00183   
00185   struct DisabledCollision
00186   {
00188     std::string link1_;
00189 
00191     std::string link2_;
00192 
00194     std::string reason_;
00195   };
00196   
00197   // Some joints can be passive (not actuated). This structure specifies information about such joints
00198   struct PassiveJoint
00199   {   
00201     std::string name_;
00202   };
00203       
00205   const std::string& getName() const
00206   {
00207     return name_;
00208   }
00209   
00211   const std::vector<DisabledCollision>& getDisabledCollisionPairs() const
00212   {
00213     return disabled_collisions_;
00214   }
00215   
00217   __attribute__ ((deprecated)) 
00218   std::vector<std::pair<std::string, std::string> > getDisabledCollisions() const;
00219   
00221   const std::vector<Group>& getGroups() const
00222   {
00223     return groups_;
00224   }
00225   
00227   const std::vector<VirtualJoint>& getVirtualJoints() const
00228   {
00229     return virtual_joints_;
00230   }
00231   
00233   const std::vector<EndEffector>& getEndEffectors() const
00234   {
00235     return end_effectors_;
00236   }
00237   
00239   const std::vector<GroupState>& getGroupStates() const
00240   {
00241     return group_states_;
00242   }
00243   
00245   const std::vector<PassiveJoint>& getPassiveJoints() const
00246   {
00247     return passive_joints_;
00248   }
00249   
00251   const std::vector<LinkSpheres>& getLinkSphereApproximations() const
00252   {
00253     return link_sphere_approximations_;
00254   }
00255   
00257   void clear();
00258   
00259 private:
00260   
00261   void loadVirtualJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00262   void loadGroups(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00263   void loadGroupStates(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00264   void loadEndEffectors(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00265   void loadLinkSphereApproximations(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00266   void loadDisabledCollisions(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00267   void loadPassiveJoints(const urdf::ModelInterface &urdf_model, TiXmlElement *robot_xml);
00268   
00269   std::string                    name_;
00270   std::vector<Group>             groups_;
00271   std::vector<GroupState>        group_states_;
00272   std::vector<VirtualJoint>      virtual_joints_;
00273   std::vector<EndEffector>       end_effectors_;
00274   std::vector<LinkSpheres>       link_sphere_approximations_;
00275   std::vector<DisabledCollision> disabled_collisions_;
00276   std::vector<PassiveJoint>      passive_joints_;
00277 };
00278 typedef boost::shared_ptr<Model> ModelSharedPtr;
00279 typedef boost::shared_ptr<const Model> ModelConstSharedPtr;
00280 
00281 
00282 }
00283 #endif


srdfdom
Author(s): Ioan Sucan , Guillaume Walck
autogenerated on Thu Feb 9 2017 03:50:57