moveit_experimental: collision_robot_distance

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_ROBOT_DISTANCE_FIELD_
00038 #define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_ROBOT_DISTANCE_FIELD_
00039 
00040 #include <moveit/collision_detection/collision_robot.h>
00041 #include <moveit/collision_distance_field/collision_distance_field_types.h>
00042 #include <moveit/collision_distance_field/collision_common_distance_field.h>
00043 #include <moveit/planning_scene/planning_scene.h>
00044 #include <boost/thread/mutex.hpp>
00045 
00046 namespace collision_detection
00047 {
00048 static const double DEFAULT_SIZE_X = 3.0;
00049 static const double DEFAULT_SIZE_Y = 3.0;
00050 static const double DEFAULT_SIZE_Z = 4.0;
00051 static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD = false;
00052 static const double DEFAULT_RESOLUTION = .02;
00053 static const double DEFAULT_COLLISION_TOLERANCE = 0.0;
00054 static const double DEFAULT_MAX_PROPOGATION_DISTANCE = .25;
00055 
00056 class CollisionRobotDistanceField : public CollisionRobot
00057 {
00058   friend class CollisionWorldDistanceField;
00059 
00060 public:
00061   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00062 
00063   CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& kmodel);
00064 
00065   CollisionRobotDistanceField(const CollisionRobot& col_robot, const Eigen::Vector3d& size,
00066                               const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution,
00067                               double collision_tolerance, double max_propogation_distance, double padding);
00068 
00069   CollisionRobotDistanceField(const robot_model::RobotModelConstPtr& kmodel,
00070                               const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions,
00071                               double size_x = DEFAULT_SIZE_X, double size_y = DEFAULT_SIZE_Y,
00072                               double size_z = DEFAULT_SIZE_Z,
00073                               bool use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD,
00074                               double resolution = DEFAULT_RESOLUTION,
00075                               double collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
00076                               double max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE, double padding = 0.0,
00077                               double scale = 1.0);
00078 
00079   CollisionRobotDistanceField(const CollisionRobotDistanceField& other);
00080 
00081   void initialize(const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions,
00082                   const Eigen::Vector3d& size, const Eigen::Vector3d& origin, bool use_signed_distance_field,
00083                   double resolution, double collision_tolerance, double max_propogation_distance);
00084 
00085   virtual void checkSelfCollision(const collision_detection::CollisionRequest& req,
00086                                   collision_detection::CollisionResult& res,
00087                                   const moveit::core::RobotState& state) const;
00088 
00089   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00090                           const moveit::core::RobotState& state,
00091                           boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00092 
00093   virtual void checkSelfCollision(const collision_detection::CollisionRequest& req,
00094                                   collision_detection::CollisionResult& res, const moveit::core::RobotState& state,
00095                                   const collision_detection::AllowedCollisionMatrix& acm) const;
00096 
00097   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00098                           const moveit::core::RobotState& state, const collision_detection::AllowedCollisionMatrix& acm,
00099                           boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00100 
00101   virtual void checkSelfCollision(const collision_detection::CollisionRequest& req,
00102                                   collision_detection::CollisionResult& res, const moveit::core::RobotState& state1,
00103                                   const moveit::core::RobotState& state2) const
00104   {
00105     logWarn("Not implemented");
00106   };
00107 
00108   virtual void checkSelfCollision(const collision_detection::CollisionRequest& req,
00109                                   collision_detection::CollisionResult& res, const moveit::core::RobotState& state1,
00110                                   const moveit::core::RobotState& state2,
00111                                   const collision_detection::AllowedCollisionMatrix& acm) const
00112   {
00113     logWarn("Not implemented");
00114   };
00115 
00116   virtual void checkOtherCollision(const collision_detection::CollisionRequest& req,
00117                                    collision_detection::CollisionResult& res, const moveit::core::RobotState& state,
00118                                    const CollisionRobot& other_robot, const moveit::core::RobotState& other_state) const
00119   {
00120     logWarn("Not implemented");
00121   };
00122 
00123   virtual void checkOtherCollision(const collision_detection::CollisionRequest& req,
00124                                    collision_detection::CollisionResult& res, const moveit::core::RobotState& state,
00125                                    const CollisionRobot& other_robot, const moveit::core::RobotState& other_state,
00126                                    const collision_detection::AllowedCollisionMatrix& acm) const
00127   {
00128     logWarn("Not implemented");
00129   };
00130 
00131   virtual void checkOtherCollision(const collision_detection::CollisionRequest& req,
00132                                    collision_detection::CollisionResult& res, const moveit::core::RobotState& state1,
00133                                    const moveit::core::RobotState& state2, const CollisionRobot& other_robot,
00134                                    const moveit::core::RobotState& other_state1,
00135                                    const moveit::core::RobotState& other_state2) const
00136   {
00137     logWarn("Not implemented");
00138   };
00139 
00140   virtual void checkOtherCollision(const collision_detection::CollisionRequest& req,
00141                                    collision_detection::CollisionResult& res, const moveit::core::RobotState& state1,
00142                                    const moveit::core::RobotState& state2, const CollisionRobot& other_robot,
00143                                    const moveit::core::RobotState& other_state1,
00144                                    const moveit::core::RobotState& other_state2,
00145                                    const collision_detection::AllowedCollisionMatrix& acm) const
00146   {
00147     logWarn("Not implemented");
00148   };
00149 
00150   void createCollisionModelMarker(const moveit::core::RobotState& state,
00151                                   visualization_msgs::MarkerArray& model_markers) const;
00152 
00153   virtual double distanceSelf(const moveit::core::RobotState& state) const
00154   {
00155     return 0.0;
00156   };
00157   virtual double distanceSelf(const moveit::core::RobotState& state,
00158                               const collision_detection::AllowedCollisionMatrix& acm) const
00159   {
00160     return 0.0;
00161   };
00162   virtual double distanceOther(const moveit::core::RobotState& state, const CollisionRobot& other_robot,
00163                                const moveit::core::RobotState& other_state) const
00164   {
00165     return 0.0;
00166   };
00167   virtual double distanceOther(const moveit::core::RobotState& state, const CollisionRobot& other_robot,
00168                                const moveit::core::RobotState& other_state,
00169                                const collision_detection::AllowedCollisionMatrix& acm) const
00170   {
00171     return 0.0;
00172   };
00173 
00174   boost::shared_ptr<const DistanceFieldCacheEntry> getLastDistanceFieldEntry() const
00175   {
00176     return distance_field_cache_entry_;
00177   }
00178 
00179   // void getSelfCollisionsGradients(const collision_detection::CollisionRequest
00180   // &req,
00181   //                                 collision_detection::CollisionResult &res,
00182   //                                 const moveit::core::RobotState &state,
00183   //                                 const
00184   //                                 collision_detection::AllowedCollisionMatrix
00185   //                                 &acm) const;
00186 protected:
00187   bool getSelfProximityGradients(boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00188 
00189   bool getIntraGroupProximityGradients(boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00190 
00191   bool getSelfCollisions(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
00192                          boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00193 
00194   bool getIntraGroupCollisions(const collision_detection::CollisionRequest& req,
00195                                collision_detection::CollisionResult& res,
00196                                boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00197 
00198   void checkSelfCollisionHelper(const collision_detection::CollisionRequest& req,
00199                                 collision_detection::CollisionResult& res, const moveit::core::RobotState& state,
00200                                 const collision_detection::AllowedCollisionMatrix* acm,
00201                                 boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00202 
00203   void updateGroupStateRepresentationState(const moveit::core::RobotState& state,
00204                                            boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00205 
00206   void generateCollisionCheckingStructures(const std::string& group_name, const moveit::core::RobotState& state,
00207                                            const collision_detection::AllowedCollisionMatrix* acm,
00208                                            boost::shared_ptr<GroupStateRepresentation>& gsr,
00209                                            bool generate_distance_field) const;
00210 
00211   boost::shared_ptr<const DistanceFieldCacheEntry>
00212   getDistanceFieldCacheEntry(const std::string& group_name, const moveit::core::RobotState& state,
00213                              const collision_detection::AllowedCollisionMatrix* acm) const;
00214 
00215   boost::shared_ptr<DistanceFieldCacheEntry> generateDistanceFieldCacheEntry(
00216       const std::string& group_name, const moveit::core::RobotState& state,
00217       const collision_detection::AllowedCollisionMatrix* acm, bool generate_distance_field) const;
00218 
00219   void addLinkBodyDecompositions(double resolution);
00220 
00221   void addLinkBodyDecompositions(double resolution,
00222                                  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions);
00223 
00224   PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel* ls,
00225                                                                       unsigned int ind) const;
00226 
00227   PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel* ls) const;
00228 
00229   void getGroupStateRepresentation(const boost::shared_ptr<const DistanceFieldCacheEntry>& dfce,
00230                                    const moveit::core::RobotState& state,
00231                                    boost::shared_ptr<GroupStateRepresentation>& gsr) const;
00232 
00233   bool compareCacheEntryToState(const boost::shared_ptr<const DistanceFieldCacheEntry>& dfce,
00234                                 const moveit::core::RobotState& state) const;
00235 
00236   bool compareCacheEntryToAllowedCollisionMatrix(const boost::shared_ptr<const DistanceFieldCacheEntry>& dfce,
00237                                                  const collision_detection::AllowedCollisionMatrix& acm) const;
00238 
00239   virtual void updatedPaddingOrScaling(const std::vector<std::string>& links){};
00240 
00241   Eigen::Vector3d size_;
00242   Eigen::Vector3d origin_;
00243   bool use_signed_distance_field_;
00244   double resolution_;
00245   double collision_tolerance_;
00246   double max_propogation_distance_;
00247 
00248   std::vector<BodyDecompositionConstPtr> link_body_decomposition_vector_;
00249   std::map<std::string, unsigned int> link_body_decomposition_index_map_;
00250 
00251   mutable boost::mutex update_cache_lock_;
00252   boost::shared_ptr<DistanceFieldCacheEntry> distance_field_cache_entry_;
00253   std::map<std::string, std::map<std::string, bool>> in_group_update_map_;
00254   std::map<std::string, boost::shared_ptr<GroupStateRepresentation>> pregenerated_group_state_representation_map_;
00255 
00256   planning_scene::PlanningScenePtr planning_scene_;
00257 };
00258 }
00259 
00260 #endif