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
00035
00036
00037 #ifndef MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_DISTANCE_FIELD_TYPES_
00038 #define MOVEIT_COLLISION_DISTANCE_FIELD_COLLISION_DISTANCE_FIELD_TYPES_
00039
00040 #include <vector>
00041 #include <string>
00042 #include <algorithm>
00043 #include <sstream>
00044 #include <float.h>
00045
00046 #include <geometric_shapes/shapes.h>
00047 #include <geometric_shapes/bodies.h>
00048 #include <octomap/OcTree.h>
00049
00050 #include <moveit/distance_field/distance_field.h>
00051 #include <moveit/distance_field/propagation_distance_field.h>
00052 #include <visualization_msgs/MarkerArray.h>
00053 #include <console_bridge/console.h>
00054
00055 namespace collision_detection
00056 {
00057 enum CollisionType
00058 {
00059 NONE = 0,
00060 SELF = 1,
00061 INTRA = 2,
00062 ENVIRONMENT = 3,
00063 };
00064
00065 struct CollisionSphere
00066 {
00067 CollisionSphere(const Eigen::Vector3d& rel, double radius)
00068 {
00069 relative_vec_ = rel;
00070 radius_ = radius;
00071 }
00072 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00073
00074 Eigen::Vector3d relative_vec_;
00075 double radius_;
00076 };
00077
00078 struct GradientInfo
00079 {
00080 GradientInfo() : closest_distance(DBL_MAX), collision(false)
00081 {
00082 }
00083
00084 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00085
00086 double closest_distance;
00087 bool collision;
00088 EigenSTL::vector_Vector3d sphere_locations;
00089 std::vector<double> distances;
00090 EigenSTL::vector_Vector3d gradients;
00091 std::vector<CollisionType> types;
00092 std::vector<double> sphere_radii;
00093 std::string joint_name;
00094
00095 void clear()
00096 {
00097 closest_distance = DBL_MAX;
00098 collision = false;
00099 sphere_locations.clear();
00100 distances.clear();
00101 gradients.clear();
00102 sphere_radii.clear();
00103 joint_name.clear();
00104 }
00105 };
00106
00107 class PosedDistanceField : public distance_field::PropagationDistanceField
00108 {
00109 public:
00110 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00111
00112 PosedDistanceField(const Eigen::Vector3d& size, const Eigen::Vector3d& origin, double resolution, double max_distance,
00113 bool propagate_negative_distances = false)
00114 : distance_field::PropagationDistanceField(size.x(), size.y(), size.z(), resolution, origin.x(), origin.y(),
00115 origin.z(), max_distance, propagate_negative_distances)
00116 , pose_(Eigen::Affine3d::Identity())
00117 {
00118 }
00119
00120 void updatePose(const Eigen::Affine3d& transform)
00121 {
00122 pose_ = transform;
00123 }
00124
00125 const Eigen::Affine3d& getPose() const
00126 {
00127 return pose_;
00128 }
00129
00149 double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z,
00150 bool& in_bounds) const
00151 {
00152 Eigen::Vector3d rel_pos = pose_.inverse() * Eigen::Vector3d(x, y, z);
00153 double gx, gy, gz;
00154 double res = distance_field::PropagationDistanceField::getDistanceGradient(rel_pos.x(), rel_pos.y(), rel_pos.z(),
00155 gx, gy, gz, in_bounds);
00156 Eigen::Vector3d grad = pose_ * Eigen::Vector3d(gx, gy, gz);
00157 gradient_x = grad.x();
00158 gradient_y = grad.y();
00159 gradient_z = grad.z();
00160 return res;
00161 }
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178 bool getCollisionSphereGradients(const std::vector<CollisionSphere>& sphere_list,
00179 const EigenSTL::vector_Vector3d& sphere_centers, GradientInfo& gradient,
00180 const CollisionType& type, double tolerance, bool subtract_radii,
00181 double maximum_value, bool stop_at_first_collision);
00182
00183 protected:
00184 Eigen::Affine3d pose_;
00185 };
00186
00187 typedef boost::shared_ptr<PosedDistanceField> PosedDistanceFieldPtr;
00188 typedef boost::shared_ptr<const PosedDistanceField> PosedDistanceFieldConstPtr;
00189
00190
00191
00192
00193 std::vector<CollisionSphere> determineCollisionSpheres(const bodies::Body* body, Eigen::Affine3d& relativeTransform);
00194
00195
00196
00197 bool getCollisionSphereGradients(const distance_field::DistanceField* distance_field,
00198 const std::vector<CollisionSphere>& sphere_list,
00199 const EigenSTL::vector_Vector3d& sphere_centers, GradientInfo& gradient,
00200 const CollisionType& type, double tolerance, bool subtract_radii, double maximum_value,
00201 bool stop_at_first_collision);
00202
00203 bool getCollisionSphereCollision(const distance_field::DistanceField* distance_field,
00204 const std::vector<CollisionSphere>& sphere_list,
00205 const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value,
00206 double tolerance);
00207
00208 bool getCollisionSphereCollision(const distance_field::DistanceField* distance_field,
00209 const std::vector<CollisionSphere>& sphere_list,
00210 const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value,
00211 double tolerance, unsigned int num_coll, std::vector<unsigned int>& colls);
00212
00213
00214 class BodyDecompositionVector;
00215
00216 class BodyDecomposition
00217 {
00218 friend class BodyDecompositionVector;
00219
00220 public:
00221 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00222
00223 BodyDecomposition(const shapes::ShapeConstPtr& shape, double resolution, double padding = 0.01);
00224
00225 BodyDecomposition(const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Affine3d& poses,
00226 double resolution, double padding);
00227
00228 ~BodyDecomposition();
00229
00230 Eigen::Affine3d relative_cylinder_pose_;
00231
00232 void replaceCollisionSpheres(const std::vector<CollisionSphere>& new_collision_spheres,
00233 const Eigen::Affine3d& new_relative_cylinder_pose)
00234 {
00235
00236
00237 collision_spheres_ = new_collision_spheres;
00238 relative_cylinder_pose_ = new_relative_cylinder_pose;
00239 }
00240
00241 const std::vector<CollisionSphere>& getCollisionSpheres() const
00242 {
00243 return collision_spheres_;
00244 }
00245
00246 const std::vector<double>& getSphereRadii() const
00247 {
00248 return sphere_radii_;
00249 }
00250
00251 const EigenSTL::vector_Vector3d& getCollisionPoints() const
00252 {
00253 return relative_collision_points_;
00254 }
00255
00256 const bodies::Body* getBody(unsigned int i) const
00257 {
00258 return bodies_.getBody(i);
00259 }
00260
00261 unsigned int getBodiesCount()
00262 {
00263 return bodies_.getCount();
00264 }
00265
00266 Eigen::Affine3d getRelativeCylinderPose() const
00267 {
00268 return relative_cylinder_pose_;
00269 }
00270
00271 const bodies::BoundingSphere& getRelativeBoundingSphere() const
00272 {
00273 return relative_bounding_sphere_;
00274 }
00275
00276 protected:
00277 void init(const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Affine3d& poses, double resolution,
00278 double padding);
00279
00280 protected:
00281 bodies::BodyVector bodies_;
00282
00283 bodies::BoundingSphere relative_bounding_sphere_;
00284 std::vector<double> sphere_radii_;
00285 std::vector<CollisionSphere> collision_spheres_;
00286 EigenSTL::vector_Vector3d relative_collision_points_;
00287 };
00288
00289 typedef boost::shared_ptr<BodyDecomposition> BodyDecompositionPtr;
00290 typedef boost::shared_ptr<const BodyDecomposition> BodyDecompositionConstPtr;
00291
00292 class PosedBodySphereDecomposition
00293 {
00294 public:
00295 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00296
00297 PosedBodySphereDecomposition(const BodyDecompositionConstPtr& body_decomposition);
00298
00299 const std::vector<CollisionSphere>& getCollisionSpheres() const
00300 {
00301 return body_decomposition_->getCollisionSpheres();
00302 }
00303
00304 const EigenSTL::vector_Vector3d& getSphereCenters() const
00305 {
00306 return sphere_centers_;
00307 }
00308
00309 const EigenSTL::vector_Vector3d& getCollisionPoints() const
00310 {
00311 return posed_collision_points_;
00312 }
00313
00314 const std::vector<double>& getSphereRadii() const
00315 {
00316 return body_decomposition_->getSphereRadii();
00317 }
00318 const Eigen::Vector3d& getBoundingSphereCenter() const
00319 {
00320 return posed_bounding_sphere_center_;
00321 }
00322
00323 double getBoundingSphereRadius() const
00324 {
00325 return body_decomposition_->getRelativeBoundingSphere().radius;
00326 }
00327
00328
00329
00330 void updatePose(const Eigen::Affine3d& linkTransform);
00331
00332 protected:
00333 BodyDecompositionConstPtr body_decomposition_;
00334 Eigen::Vector3d posed_bounding_sphere_center_;
00335 EigenSTL::vector_Vector3d posed_collision_points_;
00336 EigenSTL::vector_Vector3d sphere_centers_;
00337 };
00338
00339 class PosedBodyPointDecomposition
00340 {
00341 public:
00342 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00343
00344 PosedBodyPointDecomposition(const BodyDecompositionConstPtr& body_decomposition);
00345
00346 PosedBodyPointDecomposition(const BodyDecompositionConstPtr& body_decomposition, const Eigen::Affine3d& pose);
00347
00348 PosedBodyPointDecomposition(boost::shared_ptr<const octomap::OcTree> octree);
00349
00350 const EigenSTL::vector_Vector3d& getCollisionPoints() const
00351 {
00352 return posed_collision_points_;
00353 }
00354
00355 void updatePose(const Eigen::Affine3d& linkTransform);
00356
00357 protected:
00358 BodyDecompositionConstPtr body_decomposition_;
00359 EigenSTL::vector_Vector3d posed_collision_points_;
00360 };
00361
00362 typedef boost::shared_ptr<PosedBodyPointDecomposition> PosedBodyPointDecompositionPtr;
00363 typedef boost::shared_ptr<const PosedBodyPointDecomposition> PosedBodyPointDecompositionConstPtr;
00364 typedef boost::shared_ptr<PosedBodySphereDecomposition> PosedBodySphereDecompositionPtr;
00365 typedef boost::shared_ptr<const PosedBodySphereDecomposition> PosedBodySphereDecompositionConstPtr;
00366
00367 class PosedBodySphereDecompositionVector
00368 {
00369 public:
00370 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00371
00372 PosedBodySphereDecompositionVector()
00373 {
00374 }
00375
00376 const std::vector<CollisionSphere>& getCollisionSpheres() const
00377 {
00378 return collision_spheres_;
00379 }
00380
00381 const EigenSTL::vector_Vector3d& getSphereCenters() const
00382 {
00383 return posed_collision_spheres_;
00384 }
00385
00386 const std::vector<double>& getSphereRadii() const
00387 {
00388 return sphere_radii_;
00389 }
00390
00391 void addToVector(PosedBodySphereDecompositionPtr& bd)
00392 {
00393 sphere_index_map_[decomp_vector_.size()] = collision_spheres_.size();
00394 decomp_vector_.push_back(bd);
00395 collision_spheres_.insert(collision_spheres_.end(), bd->getCollisionSpheres().begin(),
00396 bd->getCollisionSpheres().end());
00397 posed_collision_spheres_.insert(posed_collision_spheres_.end(), bd->getSphereCenters().begin(),
00398 bd->getSphereCenters().end());
00399 sphere_radii_.insert(sphere_radii_.end(), bd->getSphereRadii().begin(), bd->getSphereRadii().end());
00400 }
00401
00402 unsigned int getSize() const
00403 {
00404 return decomp_vector_.size();
00405 }
00406
00407 PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const
00408 {
00409 if (i >= decomp_vector_.size())
00410 {
00411 logInform("No body decomposition");
00412 return empty_ptr_;
00413 }
00414 return decomp_vector_[i];
00415 }
00416
00417 void updatePose(unsigned int ind, const Eigen::Affine3d& pose)
00418 {
00419 if (ind >= decomp_vector_.size())
00420 {
00421 logWarn("Can't update pose");
00422 return;
00423 }
00424 decomp_vector_[ind]->updatePose(pose);
00425 for (unsigned int i = 0; i < decomp_vector_[ind]->getSphereCenters().size(); i++)
00426 {
00427 posed_collision_spheres_[sphere_index_map_[ind] + i] = decomp_vector_[ind]->getSphereCenters()[i];
00428 }
00429 }
00430
00431 private:
00432 PosedBodySphereDecompositionConstPtr empty_ptr_;
00433 std::vector<PosedBodySphereDecompositionPtr> decomp_vector_;
00434 std::vector<CollisionSphere> collision_spheres_;
00435 EigenSTL::vector_Vector3d posed_collision_spheres_;
00436 std::vector<double> sphere_radii_;
00437 std::map<unsigned int, unsigned int> sphere_index_map_;
00438 };
00439
00440 class PosedBodyPointDecompositionVector
00441 {
00442 public:
00443 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00444
00445 PosedBodyPointDecompositionVector()
00446 {
00447 }
00448
00449 EigenSTL::vector_Vector3d getCollisionPoints() const
00450 {
00451 EigenSTL::vector_Vector3d ret_points;
00452 for (unsigned int i = 0; i < decomp_vector_.size(); i++)
00453 {
00454 ret_points.insert(ret_points.end(), decomp_vector_[i]->getCollisionPoints().begin(),
00455 decomp_vector_[i]->getCollisionPoints().end());
00456 }
00457 return ret_points;
00458 }
00459
00460 void addToVector(PosedBodyPointDecompositionPtr& bd)
00461 {
00462 decomp_vector_.push_back(bd);
00463 }
00464
00465 unsigned int getSize() const
00466 {
00467 return decomp_vector_.size();
00468 }
00469
00470 PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const
00471 {
00472 if (i >= decomp_vector_.size())
00473 {
00474 logInform("No body decomposition");
00475 return empty_ptr_;
00476 }
00477 return decomp_vector_[i];
00478 }
00479
00480 void updatePose(unsigned int ind, const Eigen::Affine3d& pose)
00481 {
00482 if (ind < decomp_vector_.size())
00483 {
00484 decomp_vector_[ind]->updatePose(pose);
00485 }
00486 else
00487 {
00488 logWarn("Can't update pose");
00489 return;
00490 }
00491 }
00492
00493 private:
00494 PosedBodyPointDecompositionPtr empty_ptr_;
00495 std::vector<PosedBodyPointDecompositionPtr> decomp_vector_;
00496 };
00497
00498 typedef boost::shared_ptr<PosedBodySphereDecompositionVector> PosedBodySphereDecompositionVectorPtr;
00499 typedef boost::shared_ptr<const PosedBodySphereDecompositionVector> PosedBodySphereDecompositionVectorConstPtr;
00500 typedef boost::shared_ptr<PosedBodyPointDecompositionVector> PosedBodyPointDecompositionVectorPtr;
00501 typedef boost::shared_ptr<const PosedBodyPointDecompositionVector> PosedBodyPointDecompositionVectorConstPtr;
00502
00503 struct ProximityInfo
00504 {
00505 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00506
00507 std::string link_name;
00508 std::string attached_object_name;
00509 double proximity;
00510 unsigned int sphere_index;
00511 unsigned int att_index;
00512 Eigen::Vector3d closest_point;
00513 Eigen::Vector3d closest_gradient;
00514 };
00515
00516 bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1,
00517 const PosedBodySphereDecompositionConstPtr& p2);
00518
00519 void getCollisionSphereMarkers(const std_msgs::ColorRGBA& color, const std::string& frame_id, const std::string& ns,
00520 const ros::Duration& dur,
00521 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
00522 visualization_msgs::MarkerArray& arr);
00523
00524 void getProximityGradientMarkers(const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
00525 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
00526 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
00527 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
00528
00529 void getCollisionMarkers(const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
00530 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
00531 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
00532 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
00533 }
00534
00535 #endif