robot_model.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Ioan A. Sucan
00005  *  Copyright (c) 2008-2013, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 /* Author: Ioan Sucan */
00037 
00038 #ifndef MOVEIT_CORE_ROBOT_MODEL_
00039 #define MOVEIT_CORE_ROBOT_MODEL_
00040 
00041 #include <moveit/macros/class_forward.h>
00042 #include <moveit/exceptions/exceptions.h>
00043 #include <console_bridge/console.h>
00044 #include <urdf_model/model.h>
00045 #include <srdfdom/model.h>
00046 
00047 // joint types
00048 #include <moveit/robot_model/joint_model_group.h>
00049 #include <moveit/robot_model/fixed_joint_model.h>
00050 #include <moveit/robot_model/floating_joint_model.h>
00051 #include <moveit/robot_model/planar_joint_model.h>
00052 #include <moveit/robot_model/revolute_joint_model.h>
00053 #include <moveit/robot_model/prismatic_joint_model.h>
00054 
00055 #include <Eigen/Geometry>
00056 #include <iostream>
00057 
00059 namespace moveit
00060 {
00062 namespace core
00063 {
00064 
00067 class RobotModel
00068 {
00069 public:
00070   
00072   RobotModel(const boost::shared_ptr<const urdf::ModelInterface> &urdf_model,
00073              const boost::shared_ptr<const srdf::Model> &srdf_model);
00074   
00076   ~RobotModel();
00077   
00079   const std::string& getName() const
00080   {
00081     return model_name_;
00082   }
00083   
00088   const std::string& getModelFrame() const
00089   {
00090     return model_frame_;
00091   }
00092 
00094   bool isEmpty() const
00095   {
00096     return root_link_ == NULL;
00097   }
00098   
00100   const boost::shared_ptr<const urdf::ModelInterface>& getURDF() const
00101   {
00102     return urdf_;
00103   }
00104 
00106   const boost::shared_ptr<const srdf::Model>& getSRDF() const
00107   {
00108     return srdf_;
00109   }
00110   
00112   void printModelInfo(std::ostream &out) const;
00113 
00122   const JointModel* getRootJoint() const;
00123   
00125   const std::string& getRootJointName() const
00126   {
00127     return getRootJoint()->getName();
00128   }
00129   
00131   bool hasJointModel(const std::string &name) const;
00132 
00134   const JointModel* getJointModel(const std::string &joint) const;
00135 
00137   const JointModel* getJointModel(int index) const;
00138 
00140   JointModel* getJointModel(const std::string &joint);
00141   
00144   const std::vector<const JointModel*>& getJointModels() const
00145   {
00146     return joint_model_vector_const_;
00147   }
00148   
00152   const std::vector<JointModel*>& getJointModels()
00153   {
00154     return joint_model_vector_;
00155   }
00156 
00158   const std::vector<std::string>& getJointModelNames() const
00159   {
00160     return joint_model_names_vector_;
00161   }
00162   
00164   const std::vector<const JointModel*>& getActiveJointModels() const
00165   {
00166     return active_joint_model_vector_const_;
00167   }
00168 
00170   const std::vector<JointModel*>& getActiveJointModels()
00171   {
00172     return active_joint_model_vector_;
00173   }
00174 
00176   const std::vector<const JointModel*>& getSingleDOFJointModels() const
00177   {
00178     return single_dof_joints_;
00179   }
00180 
00182   const std::vector<const JointModel*>& getMultiDOFJointModels() const
00183   {
00184     return multi_dof_joints_;
00185   }  
00186 
00189   const std::vector<const JointModel*>& getContinuousJointModels() const
00190   {
00191     return continuous_joint_model_vector_;
00192   }
00193 
00196   const std::vector<const JointModel*>& getMimicJointModels() const
00197   {
00198     return mimic_joints_;
00199   }
00200   
00201   const JointModel* getJointOfVariable(int variable_index) const
00202   {
00203     return joints_of_variable_[variable_index];
00204   }
00205 
00206   const JointModel* getJointOfVariable(const std::string &variable) const
00207   {
00208     return joints_of_variable_[getVariableIndex(variable)];
00209   }
00210 
00211   std::size_t getJointModelCount() const
00212   {
00213     return joint_model_vector_.size();
00214   }
00215   
00223   const LinkModel* getRootLink() const;
00224 
00226   const std::string& getRootLinkName() const
00227   {
00228     return getRootLink()->getName();
00229   }
00230   
00232   bool hasLinkModel(const std::string &name) const;
00233 
00235   const LinkModel* getLinkModel(const std::string &link) const;
00236 
00238   const LinkModel* getLinkModel(int index) const;
00239 
00241   LinkModel* getLinkModel(const std::string &link);
00242 
00244   const std::vector<const LinkModel*>& getLinkModels() const
00245   {
00246     return link_model_vector_const_;
00247   }
00248 
00250   const std::vector<LinkModel*>& getLinkModels()
00251   {
00252     return link_model_vector_;
00253   }
00254 
00256   const std::vector<std::string>& getLinkModelNames() const
00257   {
00258     return link_model_names_vector_;
00259   }
00260 
00262   const std::vector<const LinkModel*>& getLinkModelsWithCollisionGeometry() const
00263   {
00264     return link_models_with_collision_geometry_vector_;
00265   }
00266 
00268   const std::vector<std::string>& getLinkModelNamesWithCollisionGeometry() const
00269   {
00270     return link_model_names_with_collision_geometry_vector_;
00271   }
00272 
00273   std::size_t getLinkModelCount() const
00274   {
00275     return link_model_vector_.size();
00276   }
00277   
00278   std::size_t getLinkGeometryCount() const
00279   {
00280     return link_geometry_count_;
00281   }
00282   
00287   void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, double *values) const;
00288 
00290   void getVariableDefaultPositions(double *values) const;
00291 
00293   void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::vector<double> &values) const
00294   {
00295     values.resize(variable_count_);
00296     getVariableRandomPositions(rng, &values[0]);
00297   }
00298   
00300   void getVariableDefaultPositions(std::vector<double> &values) const
00301   {
00302     values.resize(variable_count_);
00303     getVariableDefaultPositions(&values[0]);
00304   }
00305   
00307   void getVariableRandomPositions(random_numbers::RandomNumberGenerator &rng, std::map<std::string, double> &values) const;
00308 
00310   void getVariableDefaultPositions(std::map<std::string, double> &values) const;
00311 
00312   bool enforcePositionBounds(double *state) const
00313   {
00314     return enforcePositionBounds(state, active_joint_models_bounds_);
00315   }
00316   bool enforcePositionBounds(double *state, const JointBoundsVector &active_joint_bounds) const;
00317   bool satisfiesPositionBounds(const double *state, double margin = 0.0) const
00318   {
00319     return satisfiesPositionBounds(state, active_joint_models_bounds_, margin);
00320   }
00321   bool satisfiesPositionBounds(const double *state, const JointBoundsVector &active_joint_bounds, double margin = 0.0) const;
00322   double getMaximumExtent() const
00323   {
00324     return getMaximumExtent(active_joint_models_bounds_);
00325   }
00326   double getMaximumExtent(const JointBoundsVector &active_joint_bounds) const;
00327 
00328   double distance(const double *state1, const double *state2) const;  
00329   void interpolate(const double *from, const double *to, double t, double *state) const;
00330 
00336   bool hasJointModelGroup(const std::string& group) const;
00337   
00339   const JointModelGroup* getJointModelGroup(const std::string& name) const;
00340   
00342   JointModelGroup* getJointModelGroup(const std::string& name);
00343   
00345   const std::vector<const JointModelGroup*>& getJointModelGroups() const
00346   {
00347     return joint_model_groups_const_;
00348   }
00349 
00351   const std::vector<JointModelGroup*>& getJointModelGroups()
00352   {
00353     return joint_model_groups_;
00354   }
00355   
00357   const std::vector<std::string>& getJointModelGroupNames() const
00358   {
00359     return joint_model_group_names_;
00360   }
00361 
00363   bool hasEndEffector(const std::string& eef) const;
00364 
00366   const JointModelGroup* getEndEffector(const std::string& name) const;
00367 
00369   JointModelGroup* getEndEffector(const std::string& name);
00370 
00372   const std::vector<const JointModelGroup*>& getEndEffectors() const
00373   {
00374     return end_effectors_;
00375   }
00376 
00380   std::size_t getVariableCount() const
00381   {
00382     return variable_count_;
00383   }
00384 
00387   const std::vector<std::string>& getVariableNames() const
00388   {
00389     return variable_names_;
00390   }
00391 
00393   const VariableBounds& getVariableBounds(const std::string& variable) const
00394   {
00395     return getJointOfVariable(variable)->getVariableBounds(variable);
00396   }  
00397 
00399   const JointBoundsVector& getActiveJointModelsBounds() const
00400   {
00401     return active_joint_models_bounds_;
00402   }
00403   
00404   void getMissingVariableNames(const std::vector<std::string> &variables, std::vector<std::string> &missing_variables) const;
00405   
00407   int getVariableIndex(const std::string &variable) const;
00408   
00410   const JointModel* getCommonRoot(const JointModel *a, const JointModel *b) const
00411   {
00412     if (!a)
00413       return b;
00414     if (!b)
00415       return a;
00416     return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() + b->getJointIndex()]];
00417   }
00418   
00420   void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn> &allocators);
00421 
00422 protected:
00423 
00424   void computeFixedTransforms(const LinkModel *link, const Eigen::Affine3d &transform, LinkTransformMap &associated_transforms);
00425 
00427   const JointModel* computeCommonRoot(const JointModel *a, const JointModel *b) const;
00428 
00430   void updateMimicJoints(double *values) const;
00431   
00432   // GENERIC INFO
00433   
00435   std::string                                   model_name_;
00436 
00438   std::string                                   model_frame_;
00439 
00440   boost::shared_ptr<const srdf::Model>          srdf_;
00441 
00442   boost::shared_ptr<const urdf::ModelInterface> urdf_;
00443 
00444 
00445   // LINKS
00446 
00448   const LinkModel                              *root_link_;
00449 
00451   LinkModelMap                                  link_model_map_;
00452 
00454   std::vector<LinkModel*>                       link_model_vector_;
00455 
00457   std::vector<const LinkModel*>                 link_model_vector_const_;
00458 
00460   std::vector<std::string>                      link_model_names_vector_;
00461 
00463   std::vector<const LinkModel*>                 link_models_with_collision_geometry_vector_;
00464 
00466   std::vector<std::string>                      link_model_names_with_collision_geometry_vector_;
00467 
00469   std::size_t                                   link_geometry_count_;
00470   
00471   // JOINTS
00472 
00474   const JointModel                             *root_joint_;
00475 
00477   JointModelMap                                 joint_model_map_;
00478 
00480   std::vector<JointModel*>                      joint_model_vector_;
00481 
00483   std::vector<const JointModel*>                joint_model_vector_const_;
00484 
00486   std::vector<std::string>                      joint_model_names_vector_;
00487 
00489   std::vector<JointModel*>                      active_joint_model_vector_;
00490 
00492   std::vector<const JointModel*>                active_joint_model_vector_const_;
00493 
00495   std::vector<const JointModel*>                continuous_joint_model_vector_;
00496 
00498   std::vector<const JointModel*>                mimic_joints_;
00499 
00500   std::vector<const JointModel*>                single_dof_joints_;
00501  
00502   std::vector<const JointModel*>                multi_dof_joints_;
00503 
00511   std::vector<int>                              common_joint_roots_;
00512 
00513   // INDEXING
00514 
00516   std::vector<std::string>                      variable_names_;
00517 
00519   std::size_t                                   variable_count_;
00520 
00524   VariableIndexMap                              joint_variables_index_map_;
00525 
00526   std::vector<int>                              active_joint_model_start_index_;
00527   
00529   JointBoundsVector                             active_joint_models_bounds_;
00530 
00532   std::vector<const JointModel*>                joints_of_variable_;
00533   
00534   // GROUPS
00535 
00537   JointModelGroupMap                            joint_model_group_map_;
00538 
00540   JointModelGroupMap                            end_effectors_map_;
00541 
00543   std::vector<JointModelGroup*>                 joint_model_groups_;
00544   
00546   std::vector<const JointModelGroup*>           joint_model_groups_const_;
00547   
00549   std::vector<std::string>                      joint_model_group_names_;
00550 
00552   std::vector<const JointModelGroup*>           end_effectors_;
00553   
00555   void buildModel(const urdf::ModelInterface &urdf_model, const srdf::Model &srdf_model);
00556 
00558   void buildGroups(const srdf::Model &srdf_model);
00559 
00561   void buildGroupsInfo_Subgroups(const srdf::Model &srdf_model);
00562 
00564   void buildGroupsInfo_EndEffectors(const srdf::Model &srdf_model);
00565 
00567   void buildMimic(const urdf::ModelInterface &urdf_model);
00568 
00570   void buildGroupStates(const srdf::Model &srdf_model);
00571 
00573   void buildJointInfo();
00574 
00576   void computeDescendants();
00577 
00579   void computeCommonRoots();
00580   
00583   JointModel* buildRecursive(LinkModel *parent, const urdf::Link *link, const srdf::Model &srdf_model);
00584 
00586   bool addJointModelGroup(const srdf::Model::Group& group);
00587 
00590   JointModel* constructJointModel(const urdf::Joint *urdf_joint_model, const urdf::Link *child_link, const srdf::Model &srdf_model);
00591 
00593   LinkModel* constructLinkModel(const urdf::Link *urdf_link);
00594 
00596   shapes::ShapePtr constructShape(const urdf::Geometry *geom);
00597 };
00598 
00599 MOVEIT_CLASS_FORWARD(RobotModel);
00600 
00601 }
00602 }
00603 
00604 namespace robot_model=moveit::core;
00605 namespace robot_state=moveit::core;
00606 
00607 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53