Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #ifndef COLLISION_PROXIMITY_TYPES_
00038 #define COLLISION_PROXIMITY_TYPES_
00039
00040 #include <vector>
00041 #include <string>
00042 #include <algorithm>
00043 #include <sstream>
00044
00045 #include <geometric_shapes/shapes.h>
00046 #include <geometric_shapes/bodies.h>
00047 #include <tf/LinearMath/Transform.h>
00048
00049 #include <distance_field/distance_field.h>
00050 #include <distance_field/propagation_distance_field.h>
00051 #include <distance_field/pf_distance_field.h>
00052
00053 namespace collision_proximity
00054 {
00055
00056 struct CollisionType {
00057
00058 CollisionType() :
00059 none(true),
00060 self(false),
00061 intra(false),
00062 environment(false)
00063 {}
00064
00065 bool none;
00066 bool self;
00067 bool intra;
00068 bool environment;
00069 };
00070
00071 struct CollisionSphere {
00072
00073 CollisionSphere(tf::Vector3 rel, double radius)
00074 {
00075 relative_vec_ = rel;
00076 radius_ = radius;
00077 }
00078
00079 tf::Vector3 center_;
00080 tf::Vector3 relative_vec_;
00081 double radius_;
00082 };
00083
00084 struct GradientInfo
00085 {
00086 GradientInfo() :
00087 closest_distance(DBL_MAX),
00088 collision(false)
00089 {
00090 }
00091
00092 double closest_distance;
00093 bool collision;
00094 std::vector<tf::Vector3> sphere_locations;
00095 std::vector<double> distances;
00096 std::vector<tf::Vector3> gradients;
00097 std::vector<double> sphere_radii;
00098 std::string joint_name;
00099
00100 void clear() {
00101 closest_distance = DBL_MAX;
00102 collision = false;
00103 sphere_locations.clear();
00104 distances.clear();
00105 gradients.clear();
00106 }
00107 };
00108
00109
00110 std::vector<CollisionSphere> determineCollisionSpheres(const bodies::Body* body, tf::Transform& relativeTransform);
00111
00112
00113 std::vector<tf::Vector3> determineCollisionPoints(const bodies::Body* body, double resolution);
00114
00115
00116 bool getCollisionSphereGradients(const distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field,
00117 const std::vector<CollisionSphere>& sphere_list,
00118 GradientInfo& gradient,
00119 double tolerance,
00120 bool subtract_radii,
00121 double maximum_value,
00122 bool stop_at_first_collision);
00123
00124 bool getCollisionSphereCollision(const distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field,
00125 const std::vector<CollisionSphere>& sphere_list,
00126 double tolerance);
00127
00128
00129 class BodyDecompositionVector;
00130
00131 class BodyDecomposition {
00132
00133 friend class BodyDecompositionVector;
00134
00135 public:
00136
00137 BodyDecomposition(const std::string& object_name, const shapes::Shape* shape, double resolution, double padding = 0.01);
00138
00139 ~BodyDecomposition();
00140
00141 tf::Transform relative_cylinder_pose_;
00142
00143
00144
00145 void updatePose(const tf::Transform& linkTransform);
00146
00147 void updateSpheresPose(const tf::Transform& linkTransform);
00148 void updatePointsPose(const tf::Transform& linkTransform);
00149
00150 const std::vector<CollisionSphere>& getCollisionSpheres() const
00151 {
00152 return collision_spheres_;
00153 }
00154
00155 const std::vector<tf::Vector3>& getCollisionPoints() const
00156 {
00157 return posed_collision_points_;
00158 }
00159
00160 const bodies::Body* getBody() const
00161 {
00162 return body_;
00163 }
00164
00165 private:
00166
00167 std::string object_name_;
00168
00169 bodies::Body* body_;
00170
00171 std::vector<CollisionSphere> collision_spheres_;
00172 std::vector<tf::Vector3> relative_collision_points_;
00173 std::vector<tf::Vector3> posed_collision_points_;
00174
00175 };
00176
00177 class BodyDecompositionVector
00178 {
00179 public:
00180 BodyDecompositionVector()
00181 {}
00182
00183 ~BodyDecompositionVector(){
00184 for(unsigned int i = 0; i < decomp_vector_.size(); i++) {
00185 delete decomp_vector_[i];
00186 }
00187 decomp_vector_.clear();
00188 }
00189
00190 const std::vector<tf::Vector3>& getCollisionPoints() const
00191 {
00192 return collision_points_;
00193 }
00194
00195 const std::vector<CollisionSphere>& getCollisionSpheres() const
00196 {
00197 return collision_spheres_;
00198 }
00199
00200
00201
00202 void addToVector(BodyDecomposition* bd)
00203 {
00204 sphere_index_map_[decomp_vector_.size()] = collision_spheres_.size();
00205 point_index_map_[decomp_vector_.size()] = collision_points_.size();
00206 decomp_vector_.push_back(bd);
00207 collision_spheres_.insert(collision_spheres_.end(), bd->getCollisionSpheres().begin(), bd->getCollisionSpheres().end());
00208 collision_points_.insert(collision_points_.end(), bd->getCollisionPoints().begin(), bd->getCollisionPoints().end());
00209 }
00210
00211 unsigned int getSize() const {
00212 return decomp_vector_.size();
00213 }
00214
00215 const BodyDecomposition* getBodyDecomposition(unsigned int i) const {
00216 if(i >= decomp_vector_.size()) {
00217 ROS_INFO_STREAM("No body decomposition");
00218 return NULL;
00219 }
00220 return decomp_vector_[i];
00221 }
00222
00223 void updateBodyPose(unsigned int ind, const tf::Transform& pose) {
00224 if(ind < decomp_vector_.size()) {
00225 decomp_vector_[ind]->updatePose(pose);
00226 } else {
00227 ROS_WARN("Can't update pose");
00228 return;
00229 }
00230 const std::vector<CollisionSphere>& spheres = decomp_vector_[ind]->getCollisionSpheres();
00231 for(unsigned j = 0; j < spheres.size(); j++) {
00232 collision_spheres_[sphere_index_map_[ind]+j] = spheres[j];
00233 }
00234 const std::vector<tf::Vector3>& points = decomp_vector_[ind]->getCollisionPoints();
00235 for(unsigned j = 0; j < points.size(); j++) {
00236 collision_points_[point_index_map_[ind]+j] = points[j];
00237 }
00238 }
00239
00240 void updateSpheresPose(unsigned int ind, const tf::Transform& pose) {
00241 if(ind < decomp_vector_.size()) {
00242 decomp_vector_[ind]->updateSpheresPose(pose);
00243 } else {
00244 ROS_WARN("Can't update pose");
00245 return;
00246 }
00247 const std::vector<CollisionSphere>& spheres = decomp_vector_[ind]->getCollisionSpheres();
00248 for(unsigned j = 0; j < spheres.size(); j++) {
00249 collision_spheres_[sphere_index_map_[ind]+j] = spheres[j];
00250 }
00251 }
00252
00253 private:
00254 std::map<unsigned int, unsigned int> sphere_index_map_;
00255 std::map<unsigned int, unsigned int> point_index_map_;
00256 std::vector<BodyDecomposition*> decomp_vector_;
00257 std::vector<CollisionSphere> collision_spheres_;
00258 std::vector<tf::Vector3> collision_points_;
00259 };
00260
00261 struct ProximityInfo
00262 {
00263 std::string link_name;
00264 std::string attached_object_name;
00265 double proximity;
00266 unsigned int sphere_index;
00267 unsigned int att_index;
00268 tf::Vector3 closest_point;
00269 tf::Vector3 closest_gradient;
00270 };
00271
00272 }
00273
00274 #endif