kinematic_state.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_STATE_
00038 #define PLANNING_MODELS_KINEMATIC_STATE_
00039 
00040 #include "kinematic_model.h"
00041 
00043 namespace planning_models
00044 {
00045 
00049 class KinematicState 
00050 {
00051 
00052 public:
00053   
00054   class JointState; ;
00055   
00056   class LinkState; 
00058   class AttachedBodyState; 
00060   class JointGroupState; 
00062   class JointState
00063   {
00064   public: 
00065     
00067     JointState(const KinematicModel::JointModel* jm);
00068 
00069     ~JointState()
00070     {
00071     }
00072 
00074     bool setJointStateValues(const std::map<std::string, double>& joint_value_map);
00075 
00077     bool setJointStateValues(const std::map<std::string, double>& joint_value_map,
00078                              std::vector<std::string>& missing_states);
00079     
00081     bool setJointStateValues(const std::vector<double>& joint_value_vector);
00082     
00084     bool setJointStateValues(const tf::Transform& transform);
00085 
00088     bool allJointStateValuesAreDefined(const std::map<std::string, double>& joint_value_map) const;
00089 
00092     bool getJointValueBounds(const std::string& value_name, double& low, double& high) const;
00093     
00095     const std::map<std::string, std::pair<double, double> >& getAllJointValueBounds() const;
00096 
00098     bool areJointStateValuesWithinBounds() const;
00099 
00100     const std::string& getName() const 
00101     {
00102       return joint_model_->getName();
00103     }
00104     
00106     unsigned int getDimension() 
00107     {
00108       return joint_state_values_.size();
00109     }
00110 
00112     const std::vector<double>& getJointStateValues() const;
00113 
00115     const std::vector<std::string>& getJointStateNameOrder() const;
00116 
00118     const tf::Transform& getVariableTransform() const 
00119     {
00120       return variable_transform_;
00121     }
00122     
00124     const KinematicModel::JointModel* getJointModel() const
00125     {
00126       return joint_model_;
00127     }
00128 
00129     const std::map<std::string, unsigned int>& getJointStateIndexMap() const
00130     {
00131       return joint_state_index_map_;
00132     }
00133 
00134     const std::string& getParentFrameId() const
00135     {
00136       return joint_model_->getParentFrameId();
00137     }
00138 
00139     const std::string& getChildFrameId() const
00140     {
00141       return joint_model_->getChildFrameId();
00142     }
00143 
00144   private:
00145     
00146     const KinematicModel::JointModel* joint_model_;
00147     
00149     tf::Transform variable_transform_; 
00150       
00151     std::map<std::string, unsigned int> joint_state_index_map_;
00152 
00153     std::vector<std::string> joint_state_name_order_;
00154 
00155     std::vector<double> joint_state_values_;
00156   };
00157 
00158   class LinkState 
00159   {
00160   public:
00161     LinkState(const KinematicModel::LinkModel* lm);
00162 
00163     ~LinkState();
00164 
00165     const std::string& getName() const 
00166     {
00167       return link_model_->getName();
00168     }
00169 
00170     void setParentJointState(const JointState* js) 
00171     {
00172       parent_joint_state_ = js;
00173     }
00174     
00175     void setParentLinkState(const LinkState* ls)
00176     {
00177       parent_link_state_ = ls;
00178     }
00179 
00180     void updateGivenGlobalLinkTransform(const tf::Transform& transform)
00181     {
00182       global_link_transform_ = transform;
00183       global_collision_body_transform_.mult(global_link_transform_, link_model_->getCollisionOriginTransform());
00184       updateAttachedBodies();
00185     }
00186     
00188     void computeTransform(void);
00189     
00190     //updates all attached bodies given set link transforms
00191     void updateAttachedBodies();
00192 
00193     const KinematicModel::LinkModel* getLinkModel() const 
00194     {
00195       return link_model_;
00196     }
00197     
00198     const JointState* getParentJointState() const
00199     {
00200       return parent_joint_state_;
00201     }
00202 
00203     const LinkState* getParentLinkState() const
00204     {
00205       return parent_link_state_;
00206     }
00207 
00208     const std::vector<AttachedBodyState*>& getAttachedBodyStateVector() const
00209     {
00210       return attached_body_state_vector_;
00211     }
00212 
00213     const tf::Transform& getGlobalLinkTransform() const 
00214     {
00215       return global_link_transform_;
00216     }
00217 
00218     const tf::Transform& getGlobalCollisionBodyTransform() const
00219     {
00220       return global_collision_body_transform_;
00221     }
00222 
00223   private:
00224     
00225     const KinematicModel::LinkModel* link_model_;
00226 
00227     const JointState* parent_joint_state_;
00228 
00229     const LinkState* parent_link_state_;
00230 
00231     std::vector<AttachedBodyState*> attached_body_state_vector_;
00232 
00234     tf::Transform global_link_transform_;
00235     
00237     tf::Transform global_collision_body_transform_;    
00238   };
00239 
00240   class AttachedBodyState 
00241   {
00242   public:
00243     AttachedBodyState(const KinematicModel::AttachedBodyModel* abm, const LinkState* parent_link_state);
00244     
00245     ~AttachedBodyState()
00246     {
00247     }
00248 
00249     const std::string& getName() const
00250     {
00251       return attached_body_model_->getName();
00252     }
00253 
00254     const std::string& getAttachedLinkName() const
00255     {
00256       return attached_body_model_->getAttachedLinkModel()->getName();
00257     }
00258     
00259     const KinematicModel::AttachedBodyModel* getAttachedBodyModel() const
00260     {
00261       return attached_body_model_;
00262     }
00263 
00265     void computeTransform(void);
00266 
00267     const std::vector<tf::Transform>& getGlobalCollisionBodyTransforms() const
00268     {
00269       return global_collision_body_transforms_;
00270     }
00271 
00272   private:
00273     const KinematicModel::AttachedBodyModel* attached_body_model_;
00274 
00275     const LinkState* parent_link_state_;
00276 
00278     std::vector<tf::Transform> global_collision_body_transforms_;
00279     
00280   };
00281 
00282   class JointStateGroup
00283   {
00284   public:
00285             
00286     JointStateGroup(const KinematicModel::JointModelGroup* jmg, const KinematicState* owner);
00287 
00288     //JointGroupState(const JointGroupState* jgs)
00289 
00290     ~JointStateGroup(void)
00291     {
00292     }
00293 
00294     const KinematicModel::JointModelGroup* getJointModelGroup() {
00295       return joint_model_group_;
00296     }
00297 
00298     const std::string& getName() const 
00299     {
00300       return joint_model_group_->getName();
00301     }
00302 
00303     unsigned int getDimension() const
00304     {
00305       return dimension_;
00306     }
00307 
00312     bool setKinematicState(const std::vector<double>& joint_state_values);
00313     
00318     bool setKinematicState(const std::map<std::string, double>& joint_state_map);
00319 
00321     void updateKinematicLinks();        
00322 
00324     bool hasJointState(const std::string &joint) const;
00325 
00327     bool updatesLinkState(const std::string& joint) const;
00328 
00330     JointState* getJointState(const std::string &joint) const;
00331 
00332     void getKinematicStateValues(std::vector<double>& joint_state_values) const;
00333     
00334     void getKinematicStateValues(std::map<std::string, double>& joint_state_values) const;
00335    
00339     void setKinematicStateToDefault();
00340 
00341     const std::vector<JointState*>& getJointRoots() const 
00342     {
00343       return joint_roots_;
00344     }
00345 
00346     const std::map<std::string, unsigned int>& getKinematicStateIndexMap() const
00347     {
00348       return kinematic_state_index_map_;
00349     }
00350     
00351     const std::vector<std::string>& getJointNames() const
00352     {
00353       return joint_names_;
00354     }
00355 
00356     const std::vector<JointState*>& getJointStateVector() const
00357     {
00358       return joint_state_vector_;
00359     }
00360             
00361   private:
00362     
00364     boost::shared_ptr<KinematicModel> kinematic_model_;     
00365 
00366     const KinematicModel::JointModelGroup* joint_model_group_;
00367     
00368     unsigned int dimension_;
00369     std::map<std::string, unsigned int> kinematic_state_index_map_;
00370 
00372     std::vector<std::string> joint_names_;
00373 
00375     std::vector<JointState*> joint_state_vector_;
00376 
00378     std::map<std::string, JointState*> joint_state_map_;
00379 
00381     std::vector<JointState*> joint_roots_;
00382 
00384     std::vector<LinkState*> updated_links_;         
00385   };
00386 
00387   KinematicState(const KinematicModel* kinematic_model);
00388   
00389   KinematicState(const KinematicState& state);
00390 
00391   ~KinematicState(void);
00392 
00393   bool setKinematicState(const std::vector<double>& joint_state_values);
00394   
00395   bool setKinematicState(const std::map<std::string, double>& joint_state_map);
00396 
00397   bool setKinematicState(const std::map<std::string, double>& joint_state_map,
00398                          std::vector<std::string>& missing_links);
00399 
00400   void getKinematicStateValues(std::vector<double>& joint_state_values) const;
00401 
00402   void getKinematicStateValues(std::map<std::string, double>& joint_state_values) const;
00403 
00404   void updateKinematicLinks();
00405 
00406   bool updateKinematicStateWithLinkAt(const std::string& link_name, const tf::Transform& transform);
00407 
00408   const KinematicModel* getKinematicModel() const 
00409   {
00410     return kinematic_model_;
00411   } 
00412 
00413   unsigned int getDimension() const {
00414     return dimension_;
00415   }
00416   
00417   std::vector<LinkState*> getChildLinkStates(const std::string& link_name) const;
00418 
00419   void setKinematicStateToDefault();
00420 
00421   bool areJointsWithinBounds(const std::vector<std::string>& joints) const;
00422 
00423   bool isJointWithinBounds(const std::string& joint) const;
00424 
00426   const JointStateGroup* getJointStateGroup(const std::string &name) const;
00427 
00429   JointStateGroup* getJointStateGroup(const std::string &name);
00430         
00432   bool hasJointStateGroup(const std::string &name) const;
00433 
00435   bool hasJointState(const std::string &joint) const;
00436   
00438   bool hasLinkState(const std::string& joint) const;
00439 
00441   JointState* getJointState(const std::string &joint) const;
00442 
00444   LinkState* getLinkState(const std::string &link) const;
00445 
00447   AttachedBodyState* getAttachedBodyState(const std::string &attached) const;
00448 
00449   const std::vector<JointState*>& getJointStateVector() const 
00450   {
00451     return joint_state_vector_;
00452   }
00453 
00454   std::vector<JointState*>& getJointStateVector()
00455   {
00456     return joint_state_vector_;
00457   }
00458 
00459   const std::vector<LinkState*>& getLinkStateVector() const 
00460   {
00461     return link_state_vector_;
00462   } 
00463   
00464   const std::vector<const AttachedBodyState*>& getAttachedBodyStateVector() const
00465   {
00466     return attached_body_state_vector_;
00467   }
00468   
00469   const std::map<std::string, JointStateGroup*>& getJointStateGroupMap() const
00470   {
00471     return joint_state_group_map_;
00472   }
00473 
00474   void getJointStateGroupNames(std::vector<std::string>& names) const;
00475                 
00476   const std::map<std::string, unsigned int> getKinematicStateIndexMap() const
00477   {
00478     return kinematic_state_index_map_;
00479   }
00480 
00482   void printStateInfo(std::ostream &out = std::cout) const;
00483 
00485   void printTransforms(std::ostream &out = std::cout) const;
00486 
00487   void printTransform(const std::string &st, const tf::Transform &t, std::ostream &out = std::cout) const;
00488 
00489   const tf::Transform& getRootTransform() const;
00490   
00491 private:
00492 
00493   void setLinkStatesParents();
00494 
00495   const KinematicModel* kinematic_model_;
00496 
00497   unsigned int dimension_;
00498   std::map<std::string, unsigned int> kinematic_state_index_map_;
00499 
00500   std::vector<JointState*> joint_state_vector_;
00501   std::map<std::string, JointState*> joint_state_map_;
00502 
00503   std::vector<LinkState*> link_state_vector_;
00504   std::map<std::string, LinkState*> link_state_map_;
00505 
00506   //vector of bodies, owned by states
00507   std::vector<const AttachedBodyState*> attached_body_state_vector_;
00508   
00509   std::map<std::string, JointStateGroup*> joint_state_group_map_;
00510 };
00511 
00512 }
00513 
00514 #endif


planning_models
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Mon Dec 2 2013 12:33:37