environment.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 * 
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 * 
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 * 
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 * 
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_
#define COLLISION_SPACE_ENVIRONMENT_MODEL_

#include "collision_space/environment_objects.h"
#include <planning_models/kinematic_model.h>
#include <planning_models/kinematic_state.h>
#include <geometric_shapes/bodies.h>
#include <tf/LinearMath/Vector3.h>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>
#include <string>


namespace collision_space
{
    
class EnvironmentModel
{
public:
        
  enum BodyType {
    LINK,
    ATTACHED,
    OBJECT
  };

  struct Contact
  {
    tf::Vector3 pos;     
    tf::Vector3 normal;  
    double depth;

    std::string body_name_1;
    BodyType body_type_1;

    std::string body_name_2;
    BodyType body_type_2;
  };

  struct AllowedContact
  {
    boost::shared_ptr<bodies::Body> bound;
    
    std::string body_name_1;
    std::string body_name_2;

    double depth;
  };

  typedef std::map<std::string, std::map<std::string, std::vector<AllowedContact> > > AllowedContactMap;

  /* False means that no collisions are allowed, true means ok */
  class AllowedCollisionMatrix
  {
  public:
    
    AllowedCollisionMatrix(){
      valid_ = true;
    }

    AllowedCollisionMatrix(const std::vector<std::string>& names,
                           bool allowed = false);

    AllowedCollisionMatrix(const std::vector<std::vector<bool> >& all_coll_vectors,
                           const std::map<std::string, unsigned int>& all_coll_indices);
    
    AllowedCollisionMatrix(const AllowedCollisionMatrix& acm);

    bool getAllowedCollision(const std::string& name1, const std::string& name2,
                             bool& allowed_collision) const;

    bool getAllowedCollision(unsigned int i, unsigned int j,
                             bool& allowed_collision) const;

    bool hasEntry(const std::string& name) const;

    bool getEntryIndex(const std::string& name,
                               unsigned int& index) const;
    
    bool getEntryName(const unsigned int ind,
                      std::string& name) const;

    bool removeEntry(const std::string& name);

    bool addEntry(const std::string& name, bool allowed);

    bool changeEntry(bool allowed);
    
    bool changeEntry(const std::string& name, bool allowed);

    bool changeEntry(const std::string& name1,
                     const std::string& name2,
                     bool allowed);

    bool changeEntry(const unsigned int ind_1,
                     const unsigned int ind_2,
                     bool allowed);

    bool changeEntry(const std::string& name, 
                     const std::vector<std::string>& change_names,
                     bool allowed);

    bool changeEntry(const std::vector<std::string>& change_names_1,
                     const std::vector<std::string>& change_names_2,
                     bool allowed);

    void getAllEntryNames(std::vector<std::string>& names) const;

    bool getValid() const {
      return valid_;
    };

    unsigned int getSize() const {
      return allowed_entries_.size();
    }

    typedef boost::bimap<std::string, unsigned int> entry_type;

    const entry_type& getEntriesBimap() const {
      return allowed_entries_bimap_;
    }

    void print(std::ostream& out) const;
    
  private:

    bool valid_;
    std::vector<std::vector<bool> > allowed_entries_;

    entry_type allowed_entries_bimap_;
  };

  EnvironmentModel(void)
  {
    verbose_ = false;
    objects_ = new EnvironmentObjects();
    use_altered_collision_matrix_ = false;
  }
        
  virtual ~EnvironmentModel(void)
  {
    if (objects_)
      delete objects_;
  }

  /**********************************************************************/
  /* Collision Environment Configuration                                */
  /**********************************************************************/
        
  virtual void setRobotModel(const planning_models::KinematicModel* model, 
                             const AllowedCollisionMatrix& acm,
                             const std::map<std::string, double>& link_padding_map,
                             double default_padding = 0.0,
                             double scale = 1.0);

  double getRobotScale(void) const;
        
  double getRobotPadding(void) const;
        
  virtual void updateRobotModel(const planning_models::KinematicState* state) = 0;

  virtual void updateAttachedBodies() = 0;
                
  const planning_models::KinematicModel* getRobotModel(void) const;
        
  /**********************************************************************/
  /* Collision Checking Routines                                        */
  /**********************************************************************/
        

  virtual bool isCollision(void) const = 0;
        
  virtual bool isSelfCollision(void) const = 0;

  virtual bool isEnvironmentCollision(void) const = 0;

  virtual bool isObjectRobotCollision(const std::string& object_name) const = 0;        

  virtual bool isObjectObjectCollision(const std::string& object1_name, 
                                       const std::string& object2_name) const = 0;

  virtual bool isObjectInEnvironmentCollision(const std::string& object_name) const = 0;

  virtual bool getAllObjectEnvironmentCollisionContacts (const std::string& object_name, 
                                                         std::vector<Contact> &contacts,
                                                         unsigned int num_contacts_per_pair) const = 0;
    
  virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_total = 1, unsigned int max_per_pair = 1) const = 0;

  virtual bool getAllCollisionContacts(std::vector<Contact> &contacts, unsigned int num_per_contact = 1) const = 0;

  /**********************************************************************/
  /* Collision Bodies                                                   */
  /**********************************************************************/
        
  virtual void clearObjects(void) = 0;
        
  virtual void clearObjects(const std::string &ns) = 0;

  virtual bool hasObject(const std::string& ns) const = 0;
        
  virtual void addObject(const std::string &ns, shapes::StaticShape *shape) = 0;

  virtual void addObject(const std::string &ns, shapes::Shape* shape, const tf::Transform &pose) = 0;

  virtual void addObjects(const std::string &ns, const std::vector<shapes::Shape*> &shapes, const std::vector<tf::Transform> &poses) = 0;

  virtual void getAttachedBodyPoses(std::map<std::string, std::vector<tf::Transform> >& pose_map) const = 0;

  virtual void setAlteredLinkPadding(const std::map<std::string, double>& link_padding_map);

  virtual void revertAlteredLinkPadding();

  const std::map<std::string,double>& getDefaultLinkPaddingMap() const;

  std::map<std::string,double> getCurrentLinkPaddingMap() const;

  double getCurrentLinkPadding(std::string name) const;
                
  const EnvironmentObjects* getObjects(void) const;
  
  const AllowedCollisionMatrix& getDefaultAllowedCollisionMatrix() const;
  
  const AllowedCollisionMatrix& getCurrentAllowedCollisionMatrix() const;

  virtual void setAlteredCollisionMatrix(const AllowedCollisionMatrix& acm);

  virtual void revertAlteredCollisionMatrix();

  virtual void setAllowedContacts(const std::vector<AllowedContact>& allowed_contacts);

  const std::vector<AllowedContact>& getAllowedContacts() const;
  
  void clearAllowedContacts() {
    allowed_contact_map_.clear();
    allowed_contacts_.clear();
  }

  /**********************************************************************/
  /* Miscellaneous Routines                                             */
  /**********************************************************************/

  void lock(void) const;
        
  void unlock(void) const;

  void setVerbose(bool verbose);
        
  bool getVerbose(void) const;
        
  virtual EnvironmentModel* clone(void) const = 0;
        
protected:
        
  mutable boost::recursive_mutex lock_;

  bool verbose_;

  const planning_models::KinematicModel* robot_model_;

  EnvironmentObjects *objects_;
        
  double robot_scale_;

  double default_robot_padding_;        

  AllowedCollisionMatrix default_collision_matrix_;
  AllowedCollisionMatrix altered_collision_matrix_;

  bool use_altered_collision_matrix_;

  std::map<std::string, double> default_link_padding_map_;
  std::map<std::string, double> altered_link_padding_map_;

  bool use_altered_link_padding_map_;

  AllowedContactMap allowed_contact_map_;
  std::vector<AllowedContact> allowed_contacts_;
        
};
}

#endif