kinematic_model.h
Go to the documentation of this file.
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 
00037 #ifndef PLANNING_MODELS_KINEMATIC_MODEL_
00038 #define PLANNING_MODELS_KINEMATIC_MODEL_
00039 
00040 #include <geometric_shapes/shapes.h>
00041 
00042 #include <urdf/model.h>
00043 #include <tf/LinearMath/Transform.h>
00044 #include <boost/thread/shared_mutex.hpp>
00045 
00046 #include <iostream>
00047 #include <vector>
00048 #include <string>
00049 #include <map>
00050 #include <boost/bimap.hpp>
00051 
00053 namespace planning_models
00054 {
00055  
00058 class KinematicModel
00059 {
00060 public: 
00062   class JointModel;
00063         
00065   class LinkModel;
00066 
00068   class AttachedBodyModel;
00069         
00070   struct MultiDofConfig
00071   {
00072     MultiDofConfig(std::string n) : name(n) {
00073     }
00074     
00075     ~MultiDofConfig() {
00076     }
00077     
00079     std::string name;
00080     
00082     std::string type;
00083     
00085     std::string parent_frame_id;
00086 
00088     std::string child_frame_id;
00089 
00091     std::map<std::string, std::string> name_equivalents;
00092   };
00093 
00094   struct GroupConfig
00095   {
00096     GroupConfig() { 
00097     }
00098 
00099     GroupConfig(std::string name, 
00100                 std::string base_link,
00101                 std::string tip_link) 
00102       : name_(name), base_link_(base_link), tip_link_(tip_link)
00103     {
00104     }
00105 
00106     GroupConfig(std::string name,
00107                 std::vector<std::string> joints,
00108                 std::vector<std::string> subgroups) 
00109       : name_(name)
00110     {
00111       joints_ = joints;
00112       subgroups_ = subgroups;
00113     }
00114 
00115     std::string name_;
00116     std::string base_link_;
00117     std::string tip_link_;
00118     std::vector<std::string> joints_;
00119     std::vector<std::string> subgroups_;
00120   };
00121   
00123   class JointModel
00124   {
00125     friend class KinematicModel;
00126   public:
00127     JointModel(const std::string& name);
00128 
00129     JointModel(const JointModel* joint);
00130 
00131     void initialize(const std::vector<std::string>& local_names,
00132                     const MultiDofConfig* multi_dof_config = NULL);
00133 
00134     virtual ~JointModel(void);
00135 
00136     typedef boost::bimap< std::string, std::string > js_type;
00137 
00138     const std::string& getName() const 
00139     {
00140       return name_;
00141     }
00142 
00143     const LinkModel* getParentLinkModel() const 
00144     {
00145       return parent_link_model_;
00146     }
00147     
00148     const LinkModel* getChildLinkModel() const
00149     {
00150       return child_link_model_;
00151     }
00152     
00153     const std::string& getParentFrameId() const
00154     {
00155       return parent_frame_id_;
00156     }
00157 
00158     const std::string& getChildFrameId() const
00159     {
00160       return child_frame_id_;
00161     }
00162 
00163     const js_type& getJointStateEquivalents() const
00164     {
00165       return joint_state_equivalents_;
00166     }
00167 
00168     const std::map<unsigned int, std::string>& getComputatationOrderMapIndex() const
00169     {
00170       return computation_order_map_index_;
00171     }
00173     std::string getEquiv(const std::string& name) const;
00174 
00176     bool getVariableBounds(const std::string& variable, std::pair<double, double>& bounds) const;
00177    
00179     bool setVariableBounds(const std::string& variable, double low, double high);
00180 
00184     virtual void getVariableDefaultValuesGivenBounds(std::map<std::string, double>& ret_map) const;
00185     
00186     virtual bool isValueWithinVariableBounds(const std::string& variable, const double& value, bool& within_bounds) const; 
00187 
00188     const std::map<std::string, std::pair<double, double> >& getAllVariableBounds() const {
00189       return joint_state_bounds_;
00190     }
00191 
00192     bool hasVariable(const std::string var) const
00193     {
00194       return(joint_state_equivalents_.right.find(var) != joint_state_equivalents_.right.end());
00195     }
00196    
00197     virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const = 0;
00198     
00199     virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const = 0;
00200 
00201   private:
00202 
00204     std::string name_;
00205     
00207     LinkModel *parent_link_model_;
00208     
00210     LinkModel *child_link_model_;
00211 
00212     //local names on the left, config names on the right
00213     js_type joint_state_equivalents_;
00214 
00215     //map for high and low bounds
00216     std::map<std::string, std::pair<double, double> > joint_state_bounds_;
00217     
00218     //correspondance between index into computation array and external name
00219     std::map<unsigned int, std::string> computation_order_map_index_;
00220    
00222     std::string parent_frame_id_;
00223     
00225     std::string child_frame_id_;
00226   };
00227 
00229   class FixedJointModel : public JointModel
00230   {
00231   public:
00232             
00233     FixedJointModel(const std::string name, const MultiDofConfig* multi_dof_config) :
00234       JointModel(name)
00235     {
00236       std::vector<std::string> local_names;
00237       initialize(local_names, multi_dof_config);
00238     }
00239             
00240     FixedJointModel(const FixedJointModel* joint): JointModel(joint)
00241     {
00242     }
00243 
00244     virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const {
00245       tf::Transform ident;
00246       ident.setIdentity();
00247       return ident;
00248     }
00249     
00250     virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const {
00251       std::vector<double> ret;
00252       return ret;
00253     }
00254 
00255   };
00256 
00258   class PlanarJointModel : public JointModel
00259   {
00260   public:
00261             
00262     PlanarJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
00263             
00264     PlanarJointModel(const PlanarJointModel* joint): JointModel(joint)
00265     {
00266     }
00267 
00268     virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
00269     
00270     virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
00271 
00272   };
00273 
00275   class FloatingJointModel : public JointModel
00276   {
00277   public:
00278             
00279     FloatingJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
00280 
00281     FloatingJointModel(const FloatingJointModel* joint) : JointModel(joint)
00282     {
00283     }
00284 
00285     virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
00286     
00287     virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
00288 
00289     virtual void getVariableDefaultValuesGivenBounds(std::map<std::string, double>& ret_map) const;
00290 
00291   };
00292 
00294   class PrismaticJointModel : public JointModel
00295   {
00296   public:
00297             
00298     PrismaticJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
00299     
00300     PrismaticJointModel(const PrismaticJointModel* joint) : JointModel(joint){
00301       axis_ = joint->axis_;
00302     }
00303             
00304     tf::Vector3 axis_;
00305     
00306     virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
00307     
00308     virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
00309     
00310   };
00311         
00313   class RevoluteJointModel : public JointModel
00314   {
00315   public:
00316             
00317     RevoluteJointModel(const std::string& name, const MultiDofConfig* multi_dof_config);
00318 
00319     RevoluteJointModel(const RevoluteJointModel* joint) : JointModel(joint){
00320       axis_ = joint->axis_;
00321       continuous_ = joint->continuous_;
00322     }
00323                     
00324     tf::Vector3 axis_;
00325     bool      continuous_;
00326 
00327     virtual tf::Transform computeTransform(const std::vector<double>& joint_values) const;
00328     
00329     virtual std::vector<double> computeJointStateValues(const tf::Transform& transform) const;
00330     
00331     //so we can return true for continuous joints
00332     virtual bool isValueWithinVariableBounds(const std::string& variable, const double& value, bool& within_bounds) const;
00333   };
00334         
00336   class LinkModel
00337   {
00338     friend class KinematicModel;
00339   public:
00340     
00341     LinkModel(const KinematicModel* kinematic_model);
00342     
00343     LinkModel(const LinkModel* link_model);
00344 
00345     ~LinkModel(void);
00346 
00347     const std::string& getName() const {
00348       return name_;
00349     }
00350     
00351     const JointModel* getParentJointModel() const {
00352       return parent_joint_model_;
00353     }
00354 
00355     const std::vector<JointModel*>& getChildJointModels() const {
00356       return child_joint_models_;
00357     }
00358 
00359     const tf::Transform& getJointOriginTransform() const {
00360       return joint_origin_transform_;
00361     }
00362 
00363     const tf::Transform& getCollisionOriginTransform() const {
00364       return collision_origin_transform_;
00365     }
00366 
00367     const shapes::Shape* getLinkShape() const {
00368       return shape_;
00369     }
00370     
00371     const std::vector<AttachedBodyModel*>& getAttachedBodyModels() const {
00372       return attached_body_models_;
00373     }
00374 
00375   private:
00376      
00378     void clearAttachedBodyModels();
00379     
00383     void replaceAttachedBodyModels(std::vector<AttachedBodyModel*>& attached_body_vector);
00384    
00385     void clearLinkAttachedBodyModel(const std::string& att_name);
00386 
00387     void addAttachedBodyModel(AttachedBodyModel* attached_body_model);
00388 
00390     std::string name_;
00391     
00393     const KinematicModel* kinematic_model_;
00394 
00396     JointModel *parent_joint_model_;
00397     
00399     std::vector<JointModel*> child_joint_models_;
00400     
00402     tf::Transform joint_origin_transform_;
00403     
00405     tf::Transform  collision_origin_transform_;
00406     
00408     shapes::Shape *shape_;
00409     
00411     std::vector<AttachedBodyModel*> attached_body_models_;              
00412   };
00413   
00417   class AttachedBodyModel
00418   {
00419   public:
00420     
00421     AttachedBodyModel(const LinkModel *link, 
00422                       const std::string& id,
00423                       const std::vector<tf::Transform>& attach_trans,
00424                       const std::vector<std::string>& touch_links,
00425                       std::vector<shapes::Shape*>& shapes);
00426 
00427     ~AttachedBodyModel(void);
00428     
00429     const std::string& getName() const 
00430     {
00431       return id_;
00432     }
00433     
00434     const LinkModel* getAttachedLinkModel() const 
00435     {
00436       return attached_link_model_;
00437     }
00438 
00439     const std::vector<shapes::Shape*>& getShapes() const 
00440     {
00441       return shapes_;
00442     }
00443 
00444     const std::vector<tf::Transform>& getAttachedBodyFixedTransforms() const
00445     {
00446       return attach_trans_;
00447     }
00448 
00449     const std::vector<std::string>& getTouchLinks() const
00450     {
00451       return touch_links_;
00452     }
00453     
00454   private:
00455 
00457     const LinkModel *attached_link_model_;
00458     
00460     std::vector<shapes::Shape*> shapes_;
00461     
00463     std::vector<tf::Transform> attach_trans_;
00464     
00466     std::vector<std::string> touch_links_;
00467     
00469     std::string id_;
00470   };
00471 
00472   class JointModelGroup
00473   {
00474     friend class KinematicModel;
00475   public:
00476             
00477     JointModelGroup(const std::string& name,
00478                     const std::vector<const JointModel*>& joint_vector,
00479                     const std::vector<const JointModel*>& fixed_joint_vector,
00480                     const KinematicModel* parent_model);
00481     
00482     ~JointModelGroup(void);
00483 
00484     const std::string& getName() const
00485     {
00486       return name_;
00487     }
00488             
00490     bool hasJointModel(const std::string &joint) const;
00491 
00493     const JointModel* getJointModel(const std::string &joint);
00494 
00495     const std::vector<const JointModel*>& getJointModels() const
00496     {
00497       return joint_model_vector_;
00498     }
00499 
00500     const std::vector<const JointModel*>& getFixedJointModels() const
00501     {
00502       return fixed_joint_model_vector_;
00503     }
00504     
00505     const std::vector<std::string>& getJointModelNames() const
00506     {
00507       return joint_model_name_vector_;
00508     }
00509 
00510     const std::vector<const JointModel*>& getJointRoots() const
00511     {
00512       return joint_roots_;
00513     }
00514 
00515     const std::vector<const LinkModel*>& getGroupLinkModels() const
00516     {
00517       return group_link_model_vector_;
00518     }
00519 
00520     std::vector<std::string> getGroupLinkNames() const
00521     {
00522       std::vector<std::string> ret_vec;
00523       for(unsigned int i = 0; i < group_link_model_vector_.size(); i++) {
00524         ret_vec.push_back(group_link_model_vector_[i]->getName());
00525       }
00526       return ret_vec;
00527     }
00528 
00529     const std::vector<const LinkModel*>& getUpdatedLinkModels() const
00530     {
00531       return updated_link_model_vector_;
00532     }
00533 
00534     std::vector<std::string> getUpdatedLinkModelNames() const
00535     {
00536       std::vector<std::string> ret_vec;
00537       for(unsigned int i = 0; i < updated_link_model_vector_.size(); i++) {
00538         ret_vec.push_back(updated_link_model_vector_[i]->getName());
00539       }
00540       return ret_vec;
00541     }    
00542 
00543   private:
00544 
00545     bool is_valid_;
00546 
00548     std::string name_;
00549 
00551     std::vector<std::string> joint_model_name_vector_;
00552 
00554     std::vector<const JointModel*> joint_model_vector_;
00555 
00557     std::vector<const JointModel*> fixed_joint_model_vector_;
00558 
00560     std::map<std::string, const JointModel*> joint_model_map_;
00561 
00563     std::vector<const JointModel*> joint_roots_;
00564 
00568     std::vector<const LinkModel*> group_link_model_vector_;
00569 
00571     std::vector<const LinkModel*> updated_link_model_vector_;
00572 
00573   };
00574 
00576   KinematicModel(const KinematicModel &source);
00577 
00579   KinematicModel(const urdf::Model &model, 
00580                  const std::vector<GroupConfig>& group_configs,
00581                  const std::vector<MultiDofConfig>& multi_dof_configs);
00582         
00584   ~KinematicModel(void);
00585 
00586   void copyFrom(const KinematicModel &source);
00587 
00589   const std::string& getName(void) const;
00590 
00592   const LinkModel* getLinkModel(const std::string &link) const;
00593 
00595   bool hasLinkModel(const std::string &name) const;
00596 
00598   void getLinkModelNames(std::vector<std::string> &links) const;
00599 
00601   void getChildLinkModels(const LinkModel* parent, std::vector<const LinkModel*> &links) const;
00602 
00604   void getChildLinkModels(const JointModel* parent, std::vector<const LinkModel*> &links) const;
00605 
00607   void getChildJointModels(const LinkModel* parent, std::vector<const JointModel*> &links) const;
00608 
00610   void getChildJointModels(const JointModel* parent, std::vector<const JointModel*> &links) const;
00611   
00613   std::vector<std::string> getChildLinkModelNames(const LinkModel* parent) const;
00614 
00616   std::vector<std::string> getChildJointModelNames(const LinkModel* parent) const;
00617 
00619   std::vector<std::string> getChildJointModelNames(const JointModel* parent) const;
00620 
00622   std::vector<std::string> getChildLinkModelNames(const JointModel* parent) const;
00623 
00625   const JointModel* getJointModel(const std::string &joint) const;
00626 
00628   bool hasJointModel(const std::string &name) const;
00629         
00632   const std::vector<JointModel*>& getJointModels() const
00633   {
00634     return joint_model_vector_;
00635   }
00638   const std::vector<LinkModel*>& getLinkModels() const
00639   {
00640     return link_model_vector_;
00641   }
00642 
00643   const std::vector<LinkModel*>& getLinkModelsWithCollisionGeometry() const
00644   {
00645     return link_models_with_collision_geometry_vector_;
00646   }
00647         
00650   void getJointModelNames(std::vector<std::string> &joints) const;
00651 
00653   const JointModel* getRoot(void) const;
00654         
00656   void exclusiveLock(void) const;
00657         
00659   void exclusiveUnlock(void) const;
00660 
00662   void sharedLock(void) const;
00663   
00665   void sharedUnlock(void) const;
00666 
00668   void printModelInfo(std::ostream &out = std::cout) const;
00669 
00670   bool hasModelGroup(const std::string& group) const;
00671 
00672   bool addModelGroup(const GroupConfig& group);
00673 
00674   void removeModelGroup(const std::string& group);
00675 
00676   const JointModelGroup* getModelGroup(const std::string& name) const
00677   {
00678     if(!hasModelGroup(name)) return NULL;
00679     return joint_model_group_map_.find(name)->second;
00680   }
00681 
00682   const std::map<std::string, JointModelGroup*>& getJointModelGroupMap() const
00683   {
00684     return joint_model_group_map_;
00685   }
00686 
00687   const std::map<std::string, GroupConfig>& getJointModelGroupConfigMap() const {
00688     return joint_model_group_config_map_;
00689   }
00690 
00691   void getModelGroupNames(std::vector<std::string>& getModelGroupNames) const;
00692 
00693   void clearAllAttachedBodyModels();
00694 
00695   void clearLinkAttachedBodyModels(const std::string& link_name);
00696 
00697   void clearLinkAttachedBodyModel(const std::string& link_name, const std::string& att_name);
00698 
00699   void replaceAttachedBodyModels(const std::string& link_name, std::vector<AttachedBodyModel*>& attached_body_vector);
00700 
00701   void addAttachedBodyModel(const std::string& link_name, AttachedBodyModel* att_body_model);
00702 
00703   std::vector<const AttachedBodyModel*> getAttachedBodyModels() const;
00704 
00705   std::string getRobotName() const {
00706     return model_name_;
00707   }
00708         
00709 private:
00710         
00712   mutable boost::shared_mutex lock_;
00713 
00715   std::string model_name_;      
00716 
00718   std::map<std::string, LinkModel*> link_model_map_;
00719 
00721   std::map<std::string, JointModel*> joint_model_map_;
00722 
00724   std::vector<JointModel*> joint_model_vector_;
00725         
00727   std::vector<LinkModel*> link_model_vector_;   
00728         
00730   std::vector<LinkModel*> link_models_with_collision_geometry_vector_;
00731 
00733   JointModel *root_;
00734         
00735   std::map<std::string, JointModelGroup*> joint_model_group_map_;
00736   std::map<std::string, GroupConfig> joint_model_group_config_map_;
00737 
00738   void buildGroups(const std::vector<GroupConfig>&);
00739   JointModel* buildRecursive(LinkModel *parent, const urdf::Link *link, 
00740                              const std::vector<MultiDofConfig>& multi_dof_configs);
00741   JointModel* constructJointModel(const urdf::Joint *urdfJointModel,  const urdf::Link *child_link,
00742                         const std::vector<MultiDofConfig>& multi_dof_configs);
00743   LinkModel* constructLinkModel(const urdf::Link *urdfLink);
00744   shapes::Shape* constructShape(const urdf::Geometry *geom);
00745 
00746   JointModel* copyJointModel(const JointModel *joint);
00747   JointModel* copyRecursive(LinkModel *parent, const LinkModel *link);
00748         
00749 };
00750 
00751 }
00752 
00753 #endif


planning_models
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Thu Dec 12 2013 11:09:02