00001 00030 #ifndef COLLISION_DETECTION_COLLISION_COMMON_H_ 00031 #define COLLISION_DETECTION_COLLISION_COMMON_H_ 00032 00033 #include <moveit/collision_detection/world.h> 00034 #include <moveit/collision_detection/collision_world.h> 00035 #include <fcl/broadphase/broadphase.h> 00036 #include <fcl/collision.h> 00037 #include <fcl/distance.h> 00038 #include <set> 00039 00040 namespace collision_detection 00041 { 00042 00043 00044 struct DistanceRequest 00045 { 00046 DistanceRequest(): detailed(false), 00047 global(true), 00048 active_components_only(NULL), 00049 acm(NULL), 00050 distance_threshold(std::numeric_limits<double>::max()), 00051 verbose(false), 00052 gradient(false) {} 00053 00054 DistanceRequest(bool detailed, 00055 bool global, 00056 const std::set<const robot_model::LinkModel*> *active_components_only, 00057 const collision_detection::AllowedCollisionMatrix *acm, 00058 double distance_threshold = std::numeric_limits<double>::max()): detailed(detailed), 00059 global(global), 00060 active_components_only(active_components_only), 00061 acm(acm), 00062 distance_threshold(distance_threshold), 00063 verbose(false), 00064 gradient(false) {} 00065 DistanceRequest(bool detailed, 00066 bool global, 00067 const std::set<const robot_model::LinkModel*> &active_components_only, 00068 const collision_detection::AllowedCollisionMatrix &acm, 00069 double distance_threshold = std::numeric_limits<double>::max()): detailed(detailed), 00070 global(global), 00071 active_components_only(&active_components_only), 00072 acm(&acm), 00073 distance_threshold(distance_threshold), 00074 verbose(false), 00075 gradient(false) {} 00076 DistanceRequest(bool detailed, 00077 bool global, 00078 const std::string group_name, 00079 const collision_detection::AllowedCollisionMatrix *acm, 00080 double distance_threshold = std::numeric_limits<double>::max()): detailed(detailed), 00081 global(global), 00082 group_name(group_name), 00083 active_components_only(NULL), 00084 acm(acm), 00085 distance_threshold(distance_threshold), 00086 verbose(false), 00087 gradient(false) {} 00088 DistanceRequest(bool detailed, 00089 bool global, 00090 const std::string group_name, 00091 const collision_detection::AllowedCollisionMatrix &acm, 00092 double distance_threshold = std::numeric_limits<double>::max()): detailed(detailed), 00093 global(global), 00094 group_name(group_name), 00095 active_components_only(NULL), 00096 acm(&acm), 00097 distance_threshold(distance_threshold), 00098 verbose(false), 00099 gradient(false) {} 00100 00101 virtual ~DistanceRequest() {} 00102 00104 void enableGroup(const robot_model::RobotModelConstPtr &kmodel); 00105 00106 bool detailed; 00107 00108 bool global; 00109 00110 std::string group_name; 00111 00112 const std::set<const robot_model::LinkModel*> *active_components_only; 00113 00114 const collision_detection::AllowedCollisionMatrix *acm; 00115 00116 double distance_threshold; 00117 00118 bool verbose; 00119 00120 bool gradient; 00121 00122 }; 00123 00124 struct DistanceResultsData 00125 { 00126 DistanceResultsData() 00127 { 00128 clear(); 00129 } 00130 00132 double min_distance; 00133 00135 Eigen::Vector3d nearest_points[2]; 00136 00138 std::string link_name[2]; 00139 00141 Eigen::Vector3d gradient; 00142 00143 bool hasGradient; 00144 00145 bool hasNearestPoints; 00146 00147 void clear() 00148 { 00149 min_distance = std::numeric_limits<double>::max(); 00150 nearest_points[0].setZero(); 00151 nearest_points[1].setZero(); 00152 link_name[0] = ""; 00153 link_name[1] = ""; 00154 gradient.setZero(); 00155 hasGradient = false; 00156 hasNearestPoints = false; 00157 } 00158 00159 void update(const DistanceResultsData &results) 00160 { 00161 min_distance = results.min_distance; 00162 nearest_points[0] = results.nearest_points[0]; 00163 nearest_points[1] = results.nearest_points[1]; 00164 link_name[0] = results.link_name[0]; 00165 link_name[1] = results.link_name[1]; 00166 gradient = results.gradient; 00167 hasGradient = results.hasGradient; 00168 hasNearestPoints = results.hasNearestPoints; 00169 } 00170 }; 00171 00172 typedef std::map<std::string, DistanceResultsData> DistanceMap; 00173 00174 struct DistanceResult 00175 { 00176 DistanceResult(): collision(false) {} 00177 virtual ~DistanceResult() {} 00178 00179 bool collision; 00180 00181 DistanceResultsData minimum_distance; 00182 00183 DistanceMap distance; 00184 00185 void clear() 00186 { 00187 collision = false; 00188 minimum_distance.clear(); 00189 distance.clear(); 00190 } 00191 }; 00192 00193 struct DistanceData 00194 { 00195 DistanceData(const DistanceRequest *req, DistanceResult *res): req(req), res(res), done(false) {} 00196 virtual ~DistanceData() {} 00197 00198 const DistanceRequest *req; 00199 00200 DistanceResult *res; 00201 00202 bool done; 00203 00204 }; 00205 00206 bool distanceDetailedCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void *data, double& min_dist); 00207 00209 struct DistanceInfo 00210 { 00211 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00212 std::string nearest_obsticle; 00213 Eigen::Vector3d link_point; 00214 Eigen::Vector3d obsticle_point; 00215 Eigen::Vector3d avoidance_vector; 00216 double distance; 00217 }; 00218 typedef std::map<std::string, DistanceInfo> DistanceInfoMap; 00219 00227 bool getDistanceInfo(const DistanceMap &distance_detailed, DistanceInfoMap &distance_info_map, const Eigen::Affine3d &tf); 00228 00235 bool getDistanceInfo(const DistanceMap &distance_detailed, DistanceInfoMap &distance_info_map); 00236 } 00237 00238 #endif