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 MOVEIT_CLASS_FORWARD(RobotModel);
00065 
00068 class RobotModel
00069 {
00070 public:
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 
00286   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values) const;
00287 
00289   void getVariableDefaultPositions(double* values) const;
00290 
00292   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, std::vector<double>& values) const
00293   {
00294     values.resize(variable_count_);
00295     getVariableRandomPositions(rng, &values[0]);
00296   }
00297 
00299   void getVariableDefaultPositions(std::vector<double>& values) const
00300   {
00301     values.resize(variable_count_);
00302     getVariableDefaultPositions(&values[0]);
00303   }
00304 
00306   void getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng,
00307                                   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,
00322                                double margin = 0.0) const;
00323   double getMaximumExtent() const
00324   {
00325     return getMaximumExtent(active_joint_models_bounds_);
00326   }
00327   double getMaximumExtent(const JointBoundsVector& active_joint_bounds) const;
00328 
00329   double distance(const double* state1, const double* state2) const;
00330   void interpolate(const double* from, const double* to, double t, double* state) const;
00331 
00337   bool hasJointModelGroup(const std::string& group) const;
00338 
00340   const JointModelGroup* getJointModelGroup(const std::string& name) const;
00341 
00343   JointModelGroup* getJointModelGroup(const std::string& name);
00344 
00346   const std::vector<const JointModelGroup*>& getJointModelGroups() const
00347   {
00348     return joint_model_groups_const_;
00349   }
00350 
00352   const std::vector<JointModelGroup*>& getJointModelGroups()
00353   {
00354     return joint_model_groups_;
00355   }
00356 
00358   const std::vector<std::string>& getJointModelGroupNames() const
00359   {
00360     return joint_model_group_names_;
00361   }
00362 
00364   bool hasEndEffector(const std::string& eef) const;
00365 
00367   const JointModelGroup* getEndEffector(const std::string& name) const;
00368 
00370   JointModelGroup* getEndEffector(const std::string& name);
00371 
00373   const std::vector<const JointModelGroup*>& getEndEffectors() const
00374   {
00375     return end_effectors_;
00376   }
00377 
00381   std::size_t getVariableCount() const
00382   {
00383     return variable_count_;
00384   }
00385 
00390   const std::vector<std::string>& getVariableNames() const
00391   {
00392     return variable_names_;
00393   }
00394 
00396   const VariableBounds& getVariableBounds(const std::string& variable) const
00397   {
00398     return getJointOfVariable(variable)->getVariableBounds(variable);
00399   }
00400 
00402   const JointBoundsVector& getActiveJointModelsBounds() const
00403   {
00404     return active_joint_models_bounds_;
00405   }
00406 
00407   void getMissingVariableNames(const std::vector<std::string>& variables,
00408                                std::vector<std::string>& missing_variables) const;
00409 
00411   int getVariableIndex(const std::string& variable) const;
00412 
00414   const JointModel* getCommonRoot(const JointModel* a, const JointModel* b) const
00415   {
00416     if (!a)
00417       return b;
00418     if (!b)
00419       return a;
00420     return joint_model_vector_[common_joint_roots_[a->getJointIndex() * joint_model_vector_.size() +
00421                                                    b->getJointIndex()]];
00422   }
00423 
00425   void setKinematicsAllocators(const std::map<std::string, SolverAllocatorFn>& allocators);
00426 
00427 protected:
00428   void computeFixedTransforms(const LinkModel* link, const Eigen::Affine3d& transform,
00429                               LinkTransformMap& associated_transforms);
00430 
00432   const JointModel* computeCommonRoot(const JointModel* a, const JointModel* b) const;
00433 
00435   void updateMimicJoints(double* values) const;
00436 
00437   // GENERIC INFO
00438 
00440   std::string model_name_;
00441 
00443   std::string model_frame_;
00444 
00445   boost::shared_ptr<const srdf::Model> srdf_;
00446 
00447   boost::shared_ptr<const urdf::ModelInterface> urdf_;
00448 
00449   // LINKS
00450 
00452   const LinkModel* root_link_;
00453 
00455   LinkModelMap link_model_map_;
00456 
00458   std::vector<LinkModel*> link_model_vector_;
00459 
00461   std::vector<const LinkModel*> link_model_vector_const_;
00462 
00464   std::vector<std::string> link_model_names_vector_;
00465 
00467   std::vector<const LinkModel*> link_models_with_collision_geometry_vector_;
00468 
00470   std::vector<std::string> link_model_names_with_collision_geometry_vector_;
00471 
00473   std::size_t link_geometry_count_;
00474 
00475   // JOINTS
00476 
00478   const JointModel* root_joint_;
00479 
00481   JointModelMap joint_model_map_;
00482 
00484   std::vector<JointModel*> joint_model_vector_;
00485 
00487   std::vector<const JointModel*> joint_model_vector_const_;
00488 
00490   std::vector<std::string> joint_model_names_vector_;
00491 
00493   std::vector<JointModel*> active_joint_model_vector_;
00494 
00496   std::vector<const JointModel*> active_joint_model_vector_const_;
00497 
00499   std::vector<const JointModel*> continuous_joint_model_vector_;
00500 
00502   std::vector<const JointModel*> mimic_joints_;
00503 
00504   std::vector<const JointModel*> single_dof_joints_;
00505 
00506   std::vector<const JointModel*> multi_dof_joints_;
00507 
00515   std::vector<int> common_joint_roots_;
00516 
00517   // INDEXING
00518 
00521   std::vector<std::string> variable_names_;
00522 
00524   std::size_t variable_count_;
00525 
00529   VariableIndexMap joint_variables_index_map_;
00530 
00531   std::vector<int> active_joint_model_start_index_;
00532 
00534   JointBoundsVector active_joint_models_bounds_;
00535 
00537   std::vector<const JointModel*> joints_of_variable_;
00538 
00539   // GROUPS
00540 
00542   JointModelGroupMap joint_model_group_map_;
00543 
00545   JointModelGroupMap end_effectors_map_;
00546 
00548   std::vector<JointModelGroup*> joint_model_groups_;
00549 
00551   std::vector<const JointModelGroup*> joint_model_groups_const_;
00552 
00554   std::vector<std::string> joint_model_group_names_;
00555 
00557   std::vector<const JointModelGroup*> end_effectors_;
00558 
00560   void buildModel(const urdf::ModelInterface& urdf_model, const srdf::Model& srdf_model);
00561 
00563   void buildGroups(const srdf::Model& srdf_model);
00564 
00566   void buildGroupsInfo_Subgroups(const srdf::Model& srdf_model);
00567 
00569   void buildGroupsInfo_EndEffectors(const srdf::Model& srdf_model);
00570 
00572   void buildMimic(const urdf::ModelInterface& urdf_model);
00573 
00575   void buildGroupStates(const srdf::Model& srdf_model);
00576 
00578   void buildJointInfo();
00579 
00581   void computeDescendants();
00582 
00584   void computeCommonRoots();
00585 
00588   JointModel* buildRecursive(LinkModel* parent, const urdf::Link* link, const srdf::Model& srdf_model);
00589 
00591   bool addJointModelGroup(const srdf::Model::Group& group);
00592 
00595   JointModel* constructJointModel(const urdf::Joint* urdf_joint_model, const urdf::Link* child_link,
00596                                   const srdf::Model& srdf_model);
00597 
00599   LinkModel* constructLinkModel(const urdf::Link* urdf_link);
00600 
00602   shapes::ShapePtr constructShape(const urdf::Geometry* geom);
00603 };
00604 }
00605 }
00606 
00607 namespace robot_model = moveit::core;
00608 namespace robot_state = moveit::core;
00609 
00610 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44