robot_model.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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 Willow Garage, Inc. 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, E. Gil Jones */
00036 
00037 #ifndef MOVEIT_ROBOT_MODEL_ROBOT_MODEL_
00038 #define MOVEIT_ROBOT_MODEL_ROBOT_MODEL_
00039 
00040 #include <urdf_model/model.h>
00041 #include <srdfdom/model.h>
00042 #include <geometric_shapes/shapes.h>
00043 #include <geometric_shapes/shape_messages.h>
00044 #include <random_numbers/random_numbers.h>
00045 #include <moveit/robot_model/joint_model_group.h>
00046 #include <moveit/robot_model/fixed_joint_model.h>
00047 #include <moveit/robot_model/floating_joint_model.h>
00048 #include <moveit/robot_model/planar_joint_model.h>
00049 #include <moveit/robot_model/revolute_joint_model.h>
00050 #include <moveit/robot_model/prismatic_joint_model.h>
00051 #include <boost/shared_ptr.hpp>
00052 #include <iostream>
00053 #include <set>
00054 
00055 #include <Eigen/Geometry>
00056 
00057 #include <console_bridge/console.h>
00058 
00060 namespace robot_model
00061 {
00062 
00065 class RobotModel
00066 {
00067 public:
00068 
00070   RobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00071              const boost::shared_ptr<const srdf::Model> &srdf_model);
00072 
00074   virtual ~RobotModel();
00075 
00077   const std::string& getName() const
00078   {
00079     return model_name_;
00080   }
00081 
00083   const boost::shared_ptr<const urdf::ModelInterface>& getURDF() const
00084   {
00085     return urdf_;
00086   }
00087 
00089   const boost::shared_ptr<const srdf::Model>& getSRDF() const
00090   {
00091     return srdf_;
00092   }
00093 
00095   bool hasLinkModel(const std::string &name) const;
00096 
00098   const LinkModel* getLinkModel(const std::string &link) const;
00099 
00101   bool hasJointModel(const std::string &name) const;
00102 
00104   const JointModel* getJointModel(const std::string &joint) const;
00105 
00107   void getChildLinkModels(const LinkModel* parent, std::vector<const LinkModel*> &links) const;
00108 
00110   void getChildLinkModels(const JointModel* parent, std::vector<const LinkModel*> &links) const;
00111 
00113   void getChildJointModels(const LinkModel* parent, std::vector<const JointModel*> &links) const;
00114 
00116   void getChildJointModels(const JointModel* parent, std::vector<const JointModel*> &links) const;
00117 
00119   std::vector<std::string> getChildLinkModelNames(const LinkModel* parent) const;
00120 
00122   std::vector<std::string> getChildJointModelNames(const LinkModel* parent) const;
00123 
00125   std::vector<std::string> getChildJointModelNames(const JointModel* parent) const;
00126 
00128   std::vector<std::string> getChildLinkModelNames(const JointModel* parent) const;
00129 
00132   const std::vector<const JointModel*>& getJointModels() const
00133   {
00134     return joint_model_vector_const_;
00135   }
00136 
00139   const std::vector<JointModel*>& getJointModels()
00140   {
00141     return joint_model_vector_;
00142   }
00143 
00145   const std::vector<std::string>& getJointModelNames() const
00146   {
00147     return joint_model_names_vector_;
00148   }
00149 
00152   const std::vector<const JointModel*>& getContinuousJointModels() const
00153   {
00154     return continuous_joint_model_vector_const_;
00155   }
00156 
00158   const std::vector<const LinkModel*>& getLinkModels() const
00159   {
00160     return link_model_vector_const_;
00161   }
00162 
00164   const std::vector<LinkModel*>& getLinkModels()
00165   {
00166     return link_model_vector_;
00167   }
00168 
00170   const std::vector<std::string>& getLinkModelNames() const
00171   {
00172     return link_model_names_vector_;
00173   }
00174 
00176   const std::vector<LinkModel*>& getLinkModelsWithCollisionGeometry() const
00177   {
00178     return link_models_with_collision_geometry_vector_;
00179   }
00180 
00182   const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
00183   {
00184     return link_model_names_with_collision_geometry_vector_;
00185   }
00186 
00190   const JointModel* getRoot() const
00191   {
00192     return root_joint_;
00193   }
00194 
00195   const std::string& getRootJointName() const;
00196 
00198   const LinkModel* getRootLink() const
00199   {
00200     return root_link_;
00201   }
00202 
00204   const std::string& getRootLinkName() const;
00205 
00210   const std::string& getModelFrame() const
00211   {
00212     return model_frame_;
00213   }
00214 
00216   void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const;
00217 
00219   void getVariableRandomValues(random_numbers::RandomNumberGenerator &rng, std::map<std::string, double> &values) const;
00220 
00222   void getVariableDefaultValues(std::vector<double> &values) const;
00223 
00225   void getVariableDefaultValues(std::map<std::string, double> &values) const;
00226 
00228   void printModelInfo(std::ostream &out = std::cout) const;
00229 
00231   bool hasJointModelGroup(const std::string& group) const;
00232 
00234   const JointModelGroup* getJointModelGroup(const std::string& name) const;
00235 
00237   JointModelGroup* getJointModelGroup(const std::string& name);
00238 
00240   const std::map<std::string, JointModelGroup*>& getJointModelGroupMap() const
00241   {
00242     return joint_model_group_map_;
00243   }
00244 
00246   const std::vector<std::string>& getJointModelGroupNames() const
00247   {
00248     return joint_model_group_names_;
00249   }
00250 
00252   const std::map<std::string, srdf::Model::Group>& getJointModelGroupConfigMap() const
00253   {
00254     return joint_model_group_config_map_;
00255   }
00256 
00258   bool hasEndEffector(const std::string& eef) const;
00259 
00261   const JointModelGroup* getEndEffector(const std::string& name) const;
00262 
00264   JointModelGroup* getEndEffector(const std::string& name);
00265 
00267   const std::map<std::string, JointModelGroup*>& getEndEffectorsMap() const
00268   {
00269     return end_effectors_;
00270   }
00271 
00273   unsigned int getVariableCount() const
00274   {
00275     return variable_count_;
00276   }
00277 
00281   const std::vector<std::string>& getVariableNames() const
00282   {
00283     return active_variable_names_;
00284   }
00285 
00287   const std::map<std::string, std::pair<double, double> >& getAllVariableBounds() const
00288   {
00289     return variable_bounds_;
00290   }
00291 
00296   const std::map<std::string, unsigned int>& getJointVariablesIndexMap() const
00297   {
00298     return joint_variables_index_map_;
00299   }
00300 
00302   void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn> &allocators);
00303 
00304 protected:
00305 
00306   typedef std::map<LinkModel*, Eigen::Affine3d, std::less<LinkModel*>,
00307                    Eigen::aligned_allocator<std::pair<const LinkModel*, Eigen::Affine3d> > > LinkModelToAffine3dMap;
00308 
00309   void computeFixedTransforms(LinkModel *link, const Eigen::Affine3d &transform, LinkModelToAffine3dMap &associated_transforms);
00310 
00312   std::string                                   model_name_;
00313 
00315   std::string                                   model_frame_;
00316 
00318   std::map<std::string, LinkModel*>             link_model_map_;
00319 
00321   std::vector<LinkModel*>                       link_model_vector_;
00322 
00324   std::vector<const LinkModel*>                 link_model_vector_const_;
00325 
00327   std::vector<std::string>                      link_model_names_vector_;
00328 
00330   std::vector<LinkModel*>                       link_models_with_collision_geometry_vector_;
00331 
00333   std::vector<std::string>                      link_model_names_with_collision_geometry_vector_;
00334 
00336   std::map<std::string, JointModel*>            joint_model_map_;
00337 
00339   std::vector<JointModel*>                      joint_model_vector_;
00340 
00342   std::vector<const JointModel*>                joint_model_vector_const_;
00343 
00345   std::vector<std::string>                      joint_model_names_vector_;
00346 
00348   std::vector<const JointModel*>                continuous_joint_model_vector_const_;
00349 
00351   std::vector<std::string>                      active_variable_names_;
00352 
00354   unsigned int                                  variable_count_;
00355 
00357   std::map<std::string,
00358            std::pair<double, double> >          variable_bounds_;
00359 
00363   std::map<std::string, unsigned int>           joint_variables_index_map_;
00364 
00366   JointModel                                   *root_joint_;
00367 
00369   LinkModel                                    *root_link_;
00370 
00372   std::map<std::string, JointModelGroup*>       joint_model_group_map_;
00373 
00375   std::vector<std::string>                      joint_model_group_names_;
00376 
00378   std::map<std::string, srdf::Model::Group>     joint_model_group_config_map_;
00379 
00381   std::map<std::string, JointModelGroup*>       end_effectors_;
00382 
00383   boost::shared_ptr<const srdf::Model>          srdf_;
00384 
00385   boost::shared_ptr<const urdf::ModelInterface> urdf_;
00386 
00388   void buildModel(const urdf::ModelInterface &urdf_model, const srdf::Model &srdf_model);
00389 
00391   void buildGroups(const srdf::Model &srdf_model);
00392 
00394   void buildGroupsInfo_Subgroups(const srdf::Model &srdf_model);
00395 
00397   void buildGroupsInfo_EndEffectors(const srdf::Model &srdf_model);
00398 
00400   void buildMimic(const urdf::ModelInterface &urdf_model);
00401 
00403   void buildGroupStates(const srdf::Model &srdf_model);
00404 
00406   void buildJointInfo();
00407 
00410   JointModel* buildRecursive(LinkModel *parent, const urdf::Link *link, const srdf::Model &srdf_model);
00411 
00413   bool addJointModelGroup(const srdf::Model::Group& group);
00414 
00417   JointModel* constructJointModel(const urdf::Joint *urdf_joint_model, const urdf::Link *child_link, const srdf::Model &srdf_model);
00418 
00420   LinkModel* constructLinkModel(const urdf::Link *urdf_link);
00421 
00423   shapes::ShapePtr constructShape(const urdf::Geometry *geom);
00424 };
00425 
00426 typedef boost::shared_ptr<RobotModel> RobotModelPtr;
00427 typedef boost::shared_ptr<const RobotModel> RobotModelConstPtr;
00428 
00429 }
00430 
00431 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:47