environment.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 COLLISION_SPACE_ENVIRONMENT_MODEL_
00038 #define COLLISION_SPACE_ENVIRONMENT_MODEL_
00039 
00040 #include "collision_space/environment_objects.h"
00041 #include <planning_models/kinematic_model.h>
00042 #include <planning_models/kinematic_state.h>
00043 #include <geometric_shapes/bodies.h>
00044 #include <tf/LinearMath/Vector3.h>
00045 #include <boost/thread/recursive_mutex.hpp>
00046 #include <boost/shared_ptr.hpp>
00047 #include <vector>
00048 #include <string>
00049 
00050 
00052 namespace collision_space
00053 {
00054     
00061 class EnvironmentModel
00062 {
00063 public:
00064         
00065   enum BodyType {
00066     LINK,
00067     ATTACHED,
00068     OBJECT
00069   };
00070 
00072   struct Contact
00073   {
00075     tf::Vector3 pos;     
00077     tf::Vector3 normal;  
00079     double depth;
00080 
00082     std::string body_name_1;
00083     BodyType body_type_1;
00084 
00086     std::string body_name_2;
00087     BodyType body_type_2;
00088   };
00089 
00091   struct AllowedContact
00092   {
00094     boost::shared_ptr<bodies::Body> bound;
00095     
00096     std::string body_name_1;
00097     std::string body_name_2;
00098 
00100     double depth;
00101   };
00102 
00103   typedef std::map<std::string, std::map<std::string, std::vector<AllowedContact> > > AllowedContactMap;
00104 
00106   /* False means that no collisions are allowed, true means ok */
00107   class AllowedCollisionMatrix
00108   {
00109   public:
00110     
00111     AllowedCollisionMatrix(){
00112       valid_ = true;
00113     }
00114 
00115     AllowedCollisionMatrix(const std::vector<std::string>& names,
00116                            bool allowed = false);
00117 
00118     AllowedCollisionMatrix(const std::vector<std::vector<bool> >& all_coll_vectors,
00119                            const std::map<std::string, unsigned int>& all_coll_indices);
00120     
00121     AllowedCollisionMatrix(const AllowedCollisionMatrix& acm);
00122 
00123     bool getAllowedCollision(const std::string& name1, const std::string& name2,
00124                              bool& allowed_collision) const;
00125 
00126     bool getAllowedCollision(unsigned int i, unsigned int j,
00127                              bool& allowed_collision) const;
00128 
00129     bool hasEntry(const std::string& name) const;
00130 
00131     bool getEntryIndex(const std::string& name,
00132                                unsigned int& index) const;
00133     
00134     bool getEntryName(const unsigned int ind,
00135                       std::string& name) const;
00136 
00137     bool removeEntry(const std::string& name);
00138 
00139     bool addEntry(const std::string& name, bool allowed);
00140 
00141     bool changeEntry(bool allowed);
00142     
00143     bool changeEntry(const std::string& name, bool allowed);
00144 
00145     bool changeEntry(const std::string& name1,
00146                      const std::string& name2,
00147                      bool allowed);
00148 
00149     bool changeEntry(const unsigned int ind_1,
00150                      const unsigned int ind_2,
00151                      bool allowed);
00152 
00153     bool changeEntry(const std::string& name, 
00154                      const std::vector<std::string>& change_names,
00155                      bool allowed);
00156 
00157     bool changeEntry(const std::vector<std::string>& change_names_1,
00158                      const std::vector<std::string>& change_names_2,
00159                      bool allowed);
00160 
00161     void getAllEntryNames(std::vector<std::string>& names) const;
00162 
00163     bool getValid() const {
00164       return valid_;
00165     };
00166 
00167     unsigned int getSize() const {
00168       return allowed_entries_.size();
00169     }
00170 
00171     typedef boost::bimap<std::string, unsigned int> entry_type;
00172 
00173     const entry_type& getEntriesBimap() const {
00174       return allowed_entries_bimap_;
00175     }
00176 
00177     void print(std::ostream& out) const;
00178     
00179   private:
00180 
00181     bool valid_;
00182     std::vector<std::vector<bool> > allowed_entries_;
00183 
00184     entry_type allowed_entries_bimap_;
00185   };
00186 
00187   EnvironmentModel(void)
00188   {
00189     verbose_ = false;
00190     objects_ = new EnvironmentObjects();
00191     use_altered_collision_matrix_ = false;
00192   }
00193         
00194   virtual ~EnvironmentModel(void)
00195   {
00196     if (objects_)
00197       delete objects_;
00198   }
00199 
00200   /**********************************************************************/
00201   /* Collision Environment Configuration                                */
00202   /**********************************************************************/
00203         
00210   virtual void setRobotModel(const planning_models::KinematicModel* model, 
00211                              const AllowedCollisionMatrix& acm,
00212                              const std::map<std::string, double>& link_padding_map,
00213                              double default_padding = 0.0,
00214                              double scale = 1.0);
00215 
00217   double getRobotScale(void) const;
00218         
00220   double getRobotPadding(void) const;
00221         
00223   virtual void updateRobotModel(const planning_models::KinematicState* state) = 0;
00224 
00226   virtual void updateAttachedBodies() = 0;
00227                 
00229   const planning_models::KinematicModel* getRobotModel(void) const;
00230         
00231   /**********************************************************************/
00232   /* Collision Checking Routines                                        */
00233   /**********************************************************************/
00234         
00235 
00237   virtual bool isCollision(void) const = 0;
00238         
00240   virtual bool isSelfCollision(void) const = 0;
00241 
00243   virtual bool isEnvironmentCollision(void) const = 0;
00244 
00246   virtual bool isObjectRobotCollision(const std::string& object_name) const = 0;        
00247 
00249   virtual bool isObjectObjectCollision(const std::string& object1_name, 
00250                                        const std::string& object2_name) const = 0;
00251 
00253   virtual bool isObjectInEnvironmentCollision(const std::string& object_name) const = 0;
00254 
00255   virtual bool getAllObjectEnvironmentCollisionContacts (const std::string& object_name, 
00256                                                          std::vector<Contact> &contacts,
00257                                                          unsigned int num_contacts_per_pair) const = 0;
00258     
00260   virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const = 0;
00261 
00263   virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const = 0;
00264 
00265   /**********************************************************************/
00266   /* Collision Bodies                                                   */
00267   /**********************************************************************/
00268         
00270   virtual void clearObjects(void) = 0;
00271         
00273   virtual void clearObjects(const std::string &ns) = 0;
00274 
00276   virtual bool hasObject(const std::string& ns) const = 0;
00277         
00279   virtual void addObject(const std::string &ns, shapes::StaticShape *shape) = 0;
00280 
00282   virtual void addObject(const std::string &ns, shapes::Shape* shape, const tf::Transform &pose) = 0;
00283 
00285   virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<tf::Transform> &poses) = 0;
00286 
00287   virtual void getAttachedBodyPoses(std::map<std::string, std::vector<tf::Transform> >& pose_map) const = 0;
00288 
00290   virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);
00291 
00293   virtual void revertAlteredLinkPadding();
00294 
00295   const std::map<std::string,double>& getDefaultLinkPaddingMap() const;
00296 
00297   std::map<std::string,double> getCurrentLinkPaddingMap() const;
00298 
00299   double getCurrentLinkPadding(std::string name) const;
00300                 
00302   const EnvironmentObjects* getObjects(void) const;
00303   
00304   const AllowedCollisionMatrix& getDefaultAllowedCollisionMatrix() const;
00305   
00306   const AllowedCollisionMatrix& getCurrentAllowedCollisionMatrix() const;
00307 
00309   virtual void setAlteredCollisionMatrix(const AllowedCollisionMatrix& acm);
00310 
00312   virtual void revertAlteredCollisionMatrix();
00313 
00315   virtual void setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts);
00316 
00318   const std::vector<AllowedContact>& getAllowedContacts() const;
00319   
00321   void clearAllowedContacts() {
00322     allowed_contact_map_.clear();
00323     allowed_contacts_.clear();
00324   }
00325 
00326   /**********************************************************************/
00327   /* Miscellaneous Routines                                             */
00328   /**********************************************************************/
00329 
00331   void lock(void) const;
00332         
00334   void unlock(void) const;
00335 
00337   void setVerbose(bool verbose);
00338         
00340   bool getVerbose(void) const;
00341         
00343   virtual EnvironmentModel* clone(void) const = 0;
00344         
00345 protected:
00346         
00348   mutable boost::recursive_mutex lock_;
00349 
00351   bool verbose_;
00352 
00354   const planning_models::KinematicModel* robot_model_;
00355 
00357   EnvironmentObjects *objects_;
00358         
00360   double robot_scale_;
00361 
00363   double default_robot_padding_;        
00364 
00365   AllowedCollisionMatrix default_collision_matrix_;
00366   AllowedCollisionMatrix altered_collision_matrix_;
00367 
00368   bool use_altered_collision_matrix_;
00369 
00370   std::map<std::string, double> default_link_padding_map_;
00371   std::map<std::string, double> altered_link_padding_map_;
00372 
00373   bool use_altered_link_padding_map_;
00374 
00375   AllowedContactMap allowed_contact_map_;
00376   std::vector<AllowedContact> allowed_contacts_;
00377         
00378 };
00379 }
00380 
00381 #endif
00382 


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