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


collision_proximity
Author(s): Gil Jones
autogenerated on Thu Dec 12 2013 11:08:47