$search
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_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 <LinearMath/btTransform.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(btVector3 rel, double radius) 00074 { 00075 relative_vec_ = rel; 00076 radius_ = radius; 00077 } 00078 00079 btVector3 center_; 00080 btVector3 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<btVector3> sphere_locations; 00095 std::vector<double> distances; 00096 std::vector<btVector3> 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 //determines set of collision spheres given a posed body 00110 std::vector<CollisionSphere> determineCollisionSpheres(const bodies::Body* body, btTransform& relativeTransform); 00111 00112 //determines a set of points at the indicated resolution that are inside the supplied body 00113 std::vector<btVector3> determineCollisionPoints(const bodies::Body* body, double resolution); 00114 00115 //determines a set of gradients of the given collision spheres in the distance field 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 //forward declaration required for friending apparently 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 btTransform relative_cylinder_pose_; 00142 00143 //assumed to be in reference frame, updates the pose of the body, 00144 //the collision spheres, and the posed collision points 00145 void updatePose(const btTransform& linkTransform); 00146 00147 void updateSpheresPose(const btTransform& linkTransform); 00148 void updatePointsPose(const btTransform& linkTransform); 00149 00150 const std::vector<CollisionSphere>& getCollisionSpheres() const 00151 { 00152 return collision_spheres_; 00153 } 00154 00155 const std::vector<btVector3>& 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<btVector3> relative_collision_points_; 00173 std::vector<btVector3> 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<btVector3>& getCollisionPoints() const 00191 { 00192 return collision_points_; 00193 } 00194 00195 const std::vector<CollisionSphere>& getCollisionSpheres() const 00196 { 00197 return collision_spheres_; 00198 } 00199 00200 // the decomposition vector keeps its own copies of the points 00201 // for efficiency reasons 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 btTransform& 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<btVector3>& 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 btTransform& 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<btVector3> 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 btVector3 closest_point; 00269 btVector3 closest_gradient; 00270 }; 00271 00272 } 00273 00274 #endif