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 <LinearMath/btVector3.h>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/shared_ptr.hpp>
#include <vector>
#include <string>


namespace collision_space
{
    
class EnvironmentModel
{
public:
        
  struct Contact
  {
    btVector3                              pos;     
    btVector3                              normal;  
    double                                 depth;
    const planning_models::KinematicModel::LinkModel *link1; 

    unsigned int link1_attached_body_index;
    
    const planning_models::KinematicModel::LinkModel *link2; 

    unsigned int link2_attached_body_index;

    //if the contact is with an object, this will be non-empty, and link2 should be NULL
    std::string object_name; 
  };

  struct AllowedContact
  {
    boost::shared_ptr<bodies::Body> bound;
            
    std::vector<std::string>        links;

    double                          depth;
  };
    
  EnvironmentModel(void)
  {
    m_selfCollision = true;
    m_verbose = false;
    m_objects = new EnvironmentObjects();
    use_set_collision_matrix_ = false;
  }
        
  virtual ~EnvironmentModel(void)
  {
    if (m_objects)
      delete m_objects;
  }

  /**********************************************************************/
  /* Collision Environment Configuration                                */
  /**********************************************************************/
        
  void setSelfCollision(bool selfCollision);
        
  bool getSelfCollision(void) const;
                        
  virtual void addSelfCollisionGroup(const std::vector<std::string> &group1,
                                     const std::vector<std::string> &group2);

  virtual void removeSelfCollisionGroup(const std::vector<std::string> &group1,
                                        const std::vector<std::string> &group2);

  virtual int setCollisionCheck(const std::string &link, bool state) = 0;

  virtual void setCollisionCheckLinks(const std::vector<std::string> &links, bool state) = 0;

  virtual void setCollisionCheckOnlyLinks(const std::vector<std::string> &links, bool state) = 0;

  virtual void setCollisionCheckAll(bool state) = 0;

  virtual void setRobotModel(const boost::shared_ptr<const planning_models::KinematicModel> &model, 
                             const std::vector<std::string> &links,
                             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 boost::shared_ptr<const planning_models::KinematicModel>& getRobotModel(void) const;
        
  /**********************************************************************/
  /* Collision Checking Routines                                        */
  /**********************************************************************/
        

  virtual bool isCollision(void) = 0;
        
  virtual bool isSelfCollision(void) = 0;
        
  virtual bool getCollisionContacts(const std::vector<AllowedContact> &allowedContacts, std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
  bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
        
  /**********************************************************************/
  /* Collision Bodies                                                   */
  /**********************************************************************/
        
  virtual void clearObjects(void) = 0;
        
  virtual void clearObjects(const std::string &ns) = 0;
        
  virtual void addObject(const std::string &ns, shapes::StaticShape *shape) = 0;

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

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

  virtual void removeCollidingObjects(const shapes::StaticShape *shape) = 0;

  virtual void removeCollidingObjects(const shapes::Shape *shape, const btTransform &pose) = 0;

  virtual const std::vector<const planning_models::KinematicModel::AttachedBodyModel*> getAttachedBodies(void) const = 0;

  virtual const std::vector<const planning_models::KinematicModel::AttachedBodyModel*> getAttachedBodies(const std::string link_name) const = 0;

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

  virtual void revertRobotLinkPadding();

  double getCurrentLinkPadding(std::string name) const;
                
  const EnvironmentObjects* getObjects(void) const;
  
  virtual void getDefaultAllowedCollisionMatrix(std::vector<std::vector<bool> > &matrix,
                                                std::map<std::string, unsigned int> &ind) const = 0;

  virtual void getCurrentAllowedCollisionMatrix(std::vector<std::vector<bool> > &matrix,
                                                std::map<std::string, unsigned int> &ind) const;
  
  virtual void setAllowedCollisionMatrix(const std::vector<std::vector<bool> > &matrix,
                                 const std::map<std::string, unsigned int > &ind);

  virtual void revertAllowedCollisionMatrix();

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

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

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

  std::vector<std::string>                                 m_collisionLinks;

  std::map<std::string, unsigned int>                      m_collisionLinkIndex;

  /* true means they can collide */
  std::vector< std::vector<bool> >                         m_selfCollisionTest;
        
  bool                                                     m_selfCollision;
        
  bool                                                     m_verbose;

  boost::shared_ptr<const planning_models::KinematicModel>       m_robotModel;

  EnvironmentObjects                                      *m_objects;
        
  double                                                   m_robotScale;

  double                                                   m_robotPadd; 

  std::vector<std::vector<bool> > set_collision_matrix_;
  std::map<std::string, unsigned int> set_collision_ind_;

  bool use_set_collision_matrix_;

  std::map<std::string, double> link_padding_map_;
  std::map<std::string, double> altered_link_padding_;
        
};
}

#endif

---

collision_space
Author(s): Ioan Sucan/isucan@willowgarage.com
autogenerated on Fri Jan 11 10:01:24 2013