collision_proximity: collision_proximity: collision_proximity

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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_PROXIMITY_SPACE_
00038 #define COLLISION_PROXIMITY_SPACE_
00039 
00040 #include <vector>
00041 #include <string>
00042 #include <algorithm>
00043 #include <sstream>
00044 
00045 #include <ros/ros.h>
00046 
00047 #include <planning_models/kinematic_model.h>
00048 #include <planning_models/kinematic_state.h>
00049 #include <planning_environment/models/collision_models_interface.h>
00050 
00051 #include <collision_proximity/collision_proximity_types.h>
00052 
00053 namespace collision_proximity
00054 {
00055 //A class for implementation of proximity queries and proximity-based
00056 //collision queries
00057 
00058 class CollisionProximitySpace
00059 {
00060 
00061 public:
00062 
00063     enum TrajectoryPointType
00064     {
00065       StartCollision, EndCollision, Middle, None
00066     };
00067 
00068   CollisionProximitySpace(const std::string& robot_description_name, bool register_with_environment_server = true, bool use_signed_environment_field = false, bool use_signed_self_field = false);
00069   ~CollisionProximitySpace();
00070 
00071   //this function sets up the collision proximity space for making a series of 
00072   //proximity collision or gradient queries for the indicated group
00073   void setupForGroupQueries(const std::string& group_name,
00074                             const arm_navigation_msgs::RobotState& rob_state,
00075                             std::vector<std::string>& link_names,
00076                             std::vector<std::string>& attached_body_names);
00077 
00078   //returns the updating objects lock and destroys the current kinematic state
00079   void revertAfterGroupQueries();
00080 
00081   // sets the current group given the kinematic state
00082   void setCurrentGroupState(const planning_models::KinematicState& state);
00083 
00084   // returns true if the current group is in collision in the indicated state.
00085   // This doesn't affect the distance field or other robot links not in the group
00086   bool isStateInCollision() const;
00087 
00088   // returns the full set of collision information for each group link
00089   bool getStateCollisions(bool& in_collision, 
00090                           std::vector<CollisionType>& collisions) const;
00091   
00092   // returns the full gradient information for each group_link
00093   bool getStateGradients(std::vector<GradientInfo>& gradients, 
00094                          bool subtract_radii = false) const;
00095 
00096   bool getIntraGroupCollisions(std::vector<bool>& collisions,
00097                                bool stop_at_first = false) const;
00098   
00099   bool getIntraGroupProximityGradients(std::vector<GradientInfo>& gradients,
00100                                        bool subtract_radii = false) const;
00101 
00102   bool getSelfCollisions(std::vector<bool>& collisions,
00103                                bool stop_at_first = false) const;
00104   
00105   bool getSelfProximityGradients(std::vector<GradientInfo>& gradients,
00106                                        bool subtract_radii = false) const;
00107 
00108   bool getEnvironmentCollisions(std::vector<bool>& collisions,
00109                                 bool stop_at_first = false) const;
00110   
00111   bool getEnvironmentProximityGradients(std::vector<GradientInfo>& gradients,
00112                                         bool subtract_radii = false) const;
00113 
00114   bool isTrajectorySafe(const trajectory_msgs::JointTrajectory& trajectory,
00115                         const arm_navigation_msgs::Constraints& goal_constraints,
00116                         const arm_navigation_msgs::Constraints& path_constraints,
00117                         const std::string& groupName);
00118 
00119   // returns true if current setup is in environment collision
00120   bool isEnvironmentCollision() const;
00121 
00122   // returns true if current setup is in intra-group collision
00123   bool isIntraGroupCollision() const;
00124 
00125   // returns true if current setup is in self collision
00126   bool isSelfCollision() const;
00127 
00128   // returns the single closest proximity for the group previously configured
00129   //bool getEnvironmentProximity(ProximityInfo& prox) const;
00130   
00131   // returns true or false for environment collisions for the group that's been configured
00132   //bool getEnvironmentCollision() const;
00133 
00134   //
00135   //visualization functions
00136   //
00137   btTransform getInverseWorldTransform(const planning_models::KinematicState& state) const;
00138 
00139   void getProximityGradientMarkers(const std::vector<std::string>& link_names, 
00140                                    const std::vector<std::string>& attached_body_names,
00141                                    const std::vector<GradientInfo>& gradients,
00142                                    const std::string& ns, 
00143                                    visualization_msgs::MarkerArray& arr) const;
00144 
00145   void visualizeDistanceField(distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field) const;
00146 
00147   void visualizeDistanceFieldPlane(distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field) const;
00148   //void visualizeClosestCollisionSpheres(const std::vector<std::string>& link_names) const;
00149 
00150   void visualizeCollisions(const std::vector<std::string>& link_names, 
00151                            const std::vector<std::string>& attached_body_names, 
00152                            const std::vector<CollisionType> collisions) const;
00153 
00154   void visualizeObjectVoxels(const std::vector<std::string>& object_names) const;
00155 
00156   void visualizeObjectSpheres(const std::vector<std::string>& object_names) const;
00157 
00158   void visualizeBoundingCylinders(const std::vector<std::string>& object_names) const;
00159 
00160   planning_environment::CollisionModelsInterface* getCollisionModelsInterface() const {
00161     return collision_models_interface_;
00162   }
00163 
00164   bool setPlanningScene(const arm_navigation_msgs::PlanningScene& planning_scene);
00165 
00166   const std::string& getCurrentGroupName() const {
00167     return current_group_name_;
00168   }
00169 
00170   std::vector<std::string> getCurrentLinkNames() const
00171   {
00172     return current_link_names_;
00173   }
00174 
00175   std::vector<std::string> getCurrentAttachedBodyNames() const
00176   {
00177     return current_attached_body_names_;
00178   }
00179 
00180   void setCollisionTolerance(double tol) {
00181     tolerance_ = tol;
00182   }
00183 
00184   // Set to public to allow user to manually call these if callback overriden
00185   void setPlanningSceneCallback(const arm_navigation_msgs::PlanningScene& scene);
00186   void revertPlanningSceneCallback();
00187 
00188 private:
00189 
00190   // updates the current state of the spheres in the gradient
00191   bool updateSphereLocations(const std::vector<std::string>& link_names,
00192                              const std::vector<std::string>& attached_body_names, 
00193                              std::vector<GradientInfo>& gradients);
00194 
00195 
00196 
00197   void deleteAllStaticObjectDecompositions();
00198   void deleteAllAttachedObjectDecompositions();
00199 
00200   // sets the poses of the body to those held in the kinematic state
00201   void setBodyPosesToCurrent();
00202 
00203   // sets the body poses given the indicated kinematic state
00204   void setBodyPosesGivenKinematicState(const planning_models::KinematicState& state);
00205 
00206   void setDistanceFieldForGroupQueries(const std::string& group_name,
00207                                        const planning_models::KinematicState& state);
00208 
00209   bool getGroupLinkAndAttachedBodyNames(const std::string& group_name,
00210                                         const planning_models::KinematicState& state,
00211                                         std::vector<std::string>& link_names,
00212                                         std::vector<unsigned int>& link_indices,
00213                                         std::vector<std::string>& attached_body_names,
00214                                         std::vector<unsigned int>& attached_body_link_indices) const; 
00215   
00216   bool setupGradientStructures(const std::vector<std::string>& link_names,
00217                                const std::vector<std::string>& attached_body_names, 
00218                                std::vector<GradientInfo>& gradients) const;
00219 
00220   void prepareEnvironmentDistanceField(const planning_models::KinematicState& state);
00221 
00222   void prepareSelfDistanceField(const std::vector<std::string>& link_names, 
00223                                 const planning_models::KinematicState& state);
00224 
00225 
00226   //double getCollisionSphereProximity(const std::vector<CollisionSphere>& sphere_list, 
00227   //                                  unsigned int& closest, btVector3& grad) const;
00228 
00229   void syncObjectsWithCollisionSpace(const planning_models::KinematicState& state);
00230 
00231   //configuration convenience functions
00232   void loadRobotBodyDecompositions();
00233   void loadDefaultCollisionOperations();
00234 
00235   mutable std::vector<std::vector<double> > colors_;
00236 
00237   distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* environment_distance_field_;
00238   distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* self_distance_field_;
00239 
00240   planning_environment::CollisionModelsInterface* collision_models_interface_;
00241 
00242   ros::NodeHandle root_handle_, priv_handle_;
00243 
00244   ros::Publisher vis_marker_publisher_;
00245   ros::Publisher vis_marker_array_publisher_;
00246 
00247   mutable boost::recursive_mutex group_queries_lock_;
00248 
00249   std::map<std::string, BodyDecomposition*> body_decomposition_map_;
00250   std::map<std::string, BodyDecompositionVector*> static_object_map_;
00251   std::map<std::string, BodyDecompositionVector*> attached_object_map_;
00252 
00253   std::map<std::string, std::map<std::string, bool> > enabled_self_collision_links_;
00254   std::map<std::string, std::map<std::string, bool> > intra_group_collision_links_;
00255   std::map<std::string, std::map<std::string, bool> > attached_object_collision_links_;
00256   std::map<std::string, bool> self_excludes_;
00257 
00258   //current entries to avoid map lookups during collision checks
00259   std::string current_group_name_;
00260   std::vector<std::string> current_link_names_;
00261   std::vector<std::string> current_attached_body_names_;
00262   std::vector<unsigned int> current_link_indices_;
00263   std::vector<unsigned int> current_attached_body_indices_;
00264   std::vector<BodyDecomposition*> current_link_body_decompositions_;
00265   std::vector<BodyDecompositionVector*> current_attached_body_decompositions_;
00266   std::vector<std::vector<bool> > current_intra_group_collision_links_;
00267   std::vector<bool> current_self_excludes_;
00268 
00269   //just for initializing input
00270   std::vector<GradientInfo> current_gradients_;
00271   
00272   //distance field configuration
00273   double size_x_, size_y_, size_z_;
00274   double origin_x_, origin_y_, origin_z_;
00275   double resolution_, tolerance_;
00276 
00277   double max_environment_distance_;
00278   double max_self_distance_;
00279   double undefined_distance_;
00280 
00281 };
00282 
00283 }
00284 #endif
collision_proximity_space.h
