collision_world_distance_field.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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 
00035 /* Author: E. Gil Jones */
00036 
00037 #ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_DISTANCE_FIELD_
00038 #define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_WORLD_DISTANCE_FIELD_
00039 
00040 #include <moveit/collision_detection/collision_world.h>
00041 #include <moveit/collision_distance_field/collision_distance_field_types.h>
00042 #include <moveit/collision_distance_field/collision_robot_distance_field.h>
00043 
00044 namespace collision_detection
00045 {
00046 class CollisionWorldDistanceField : public CollisionWorld
00047 {
00048 public:
00049   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00050   struct DistanceFieldCacheEntry
00051   {
00052     std::map<std::string, std::vector<PosedBodyPointDecompositionPtr> > posed_body_point_decompositions_;
00053     boost::shared_ptr<distance_field::DistanceField> distance_field_;
00054   };
00055 
00056   CollisionWorldDistanceField(Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z),
00057                               Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0),
00058                               bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD,
00059                               double resolution = DEFAULT_RESOLUTION,
00060                               double collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
00061                               double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE);
00062 
00063   explicit CollisionWorldDistanceField(
00064       const WorldPtr& world, Eigen::Vector3d size = Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z),
00065       Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0),
00066       bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution = DEFAULT_RESOLUTION,
00067       double collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
00068       double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE);
00069 
00070   CollisionWorldDistanceField(const CollisionWorldDistanceField& other, const WorldPtr& world);
00071 
00072   virtual ~CollisionWorldDistanceField();
00073 
00074   virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00075                               const robot_state::RobotState& state) const;
00076 
00077   virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00078                               const robot_state::RobotState& state,
00079                               boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00080 
00081   virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00082                               const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
00083 
00084   virtual void checkCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00085                               const robot_state::RobotState& state, const AllowedCollisionMatrix& acm,
00086                               boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00087 
00088   virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00089                                    const robot_state::RobotState& state) const;
00090 
00091   virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00092                                    const robot_state::RobotState& state,
00093                                    boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00094 
00095   virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00096                                    const robot_state::RobotState& state, const AllowedCollisionMatrix& acm) const;
00097 
00098   virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00099                                    const robot_state::RobotState& state, const AllowedCollisionMatrix& acm,
00100                                    boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00101 
00102   virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00103                                    const robot_state::RobotState& state1, const robot_state::RobotState& state2) const
00104   {
00105   }
00106   virtual void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00107                                    const robot_state::RobotState& state1, const robot_state::RobotState& state2,
00108                                    const AllowedCollisionMatrix& acm) const
00109   {
00110   }
00111   virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res,
00112                                    const CollisionWorld& other_world) const
00113   {
00114   }
00115   virtual void checkWorldCollision(const CollisionRequest& req, CollisionResult& res, const CollisionWorld& other_world,
00116                                    const AllowedCollisionMatrix& acm) const
00117   {
00118   }
00119 
00120   virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state) const
00121   {
00122     return 0.0;
00123   }
00124   virtual double distanceRobot(const CollisionRobot& robot, const robot_state::RobotState& state,
00125                                const AllowedCollisionMatrix& acm) const
00126   {
00127     return 0.0;
00128   }
00129   virtual double distanceWorld(const CollisionWorld& world) const
00130   {
00131     return 0.0;
00132   }
00133   virtual double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm) const
00134   {
00135     return 0.0;
00136   }
00137 
00138   virtual void setWorld(const WorldPtr& world);
00139 
00140   void generateEnvironmentDistanceField(bool redo = true);
00141 
00142   boost::shared_ptr<const distance_field::DistanceField> getDistanceField() const
00143   {
00144     return distance_field_cache_entry_->distance_field_;
00145   }
00146 
00147   boost::shared_ptr<const collision_detection::GroupStateRepresentation> getLastGroupStateRepresentation() const
00148   {
00149     return last_gsr_;
00150   }
00151 
00152   void getCollisionGradients(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00153                              const robot_state::RobotState& state, const AllowedCollisionMatrix* acm,
00154                              boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00155 
00156   void getAllCollisions(const CollisionRequest& req, CollisionResult& res, const CollisionRobot& robot,
00157                         const robot_state::RobotState& state, const AllowedCollisionMatrix* acm,
00158                         boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00159 
00160 protected:
00161   boost::shared_ptr<DistanceFieldCacheEntry> generateDistanceFieldCacheEntry();
00162 
00163   void updateDistanceObject(const std::string& id,
00164                             boost::shared_ptr<CollisionWorldDistanceField::DistanceFieldCacheEntry>& dfce,
00165                             EigenSTL::vector_Vector3d& add_points, EigenSTL::vector_Vector3d& subtract_points);
00166 
00167   bool getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res,
00168                                 const boost::shared_ptr<const distance_field::DistanceField>& env_distance_field,
00169                                 boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00170 
00171   bool
00172   getEnvironmentProximityGradients(const boost::shared_ptr<const distance_field::DistanceField>& env_distance_field,
00173                                    boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00174 
00175   static void notifyObjectChange(CollisionWorldDistanceField* self, const ObjectConstPtr& obj, World::Action action);
00176 
00177   Eigen::Vector3d size_;
00178   Eigen::Vector3d origin_;
00179   bool use_signed_distance_field_;
00180   double resolution_;
00181   double collision_tolerance_;
00182   double max_propogation_distance_;
00183 
00184   mutable boost::mutex update_cache_lock_;
00185   boost::shared_ptr<DistanceFieldCacheEntry> distance_field_cache_entry_;
00186   boost::shared_ptr<collision_detection::GroupStateRepresentation> last_gsr_;
00187   World::ObserverHandle observer_handle_;
00188 };
00189 }
00190 
00191 #endif


moveit_experimental
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley , Jorge Nicho
autogenerated on Wed Jun 19 2019 19:24:03