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 #include <collision_proximity/collision_proximity_types.h>
00038
00039 std::vector<collision_proximity::CollisionSphere> collision_proximity::determineCollisionSpheres(const bodies::Body* body, tf::Transform& relativeTransform)
00040 {
00041 std::vector<collision_proximity::CollisionSphere> css;
00042
00043 bodies::BoundingCylinder cyl;
00044 body->computeBoundingCylinder(cyl);
00045 unsigned int num_points = ceil(cyl.length/(cyl.radius/2.0));
00046 double spacing = cyl.length/((num_points*1.0)-1.0);
00047 tf::Vector3 vec(0.0,0.0,0.0);
00048 for(unsigned int i = 1; i < num_points-1; i++) {
00049 vec.setZ((-cyl.length/2.0)+i*spacing);
00050 tf::Vector3 p = cyl.pose*vec;
00051 collision_proximity::CollisionSphere cs(vec,cyl.radius);
00052 css.push_back(cs);
00053 }
00054 relativeTransform = body->getPose().inverse() * cyl.pose;
00055 return css;
00056 }
00057
00058 std::vector<tf::Vector3> collision_proximity::determineCollisionPoints(const bodies::Body* body, double resolution)
00059 {
00060 std::vector<tf::Vector3> ret_vec;
00061 bodies::BoundingSphere sphere;
00062 body->computeBoundingSphere(sphere);
00063
00064
00065 for(double xval = sphere.center.x()-sphere.radius-resolution; xval < sphere.center.x()+sphere.radius+resolution; xval += resolution) {
00066 for(double yval = sphere.center.y()-sphere.radius-resolution; yval < sphere.center.y()+sphere.radius+resolution; yval += resolution) {
00067 for(double zval = sphere.center.z()-sphere.radius-resolution; zval < sphere.center.z()+sphere.radius+resolution; zval += resolution) {
00068 tf::Vector3 rel_vec(xval, yval, zval);
00069 if(body->containsPoint(body->getPose()*rel_vec)) {
00070 ret_vec.push_back(rel_vec);
00071 }
00072 }
00073 }
00074 }
00075 return ret_vec;
00076 }
00077
00078 bool collision_proximity::getCollisionSphereGradients(const distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field,
00079 const std::vector<CollisionSphere>& sphere_list,
00080 GradientInfo& gradient,
00081 double tolerance,
00082 bool subtract_radii,
00083 double maximum_value,
00084 bool stop_at_first_collision) {
00085
00086 bool in_collision = false;
00087 for(unsigned int i = 0; i < sphere_list.size(); i++) {
00088 tf::Vector3 p = sphere_list[i].center_;
00089 double gx, gy, gz;
00090 double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), gx, gy, gz);
00091 if(dist < maximum_value && subtract_radii) {
00092 dist -= sphere_list[i].radius_;
00093 if(dist <= tolerance) {
00094 if(stop_at_first_collision) {
00095 return true;
00096 }
00097 in_collision = true;
00098 }
00099 }
00100 if(dist < gradient.closest_distance) {
00101 gradient.closest_distance = dist;
00102 }
00103 gradient.distances[i] = dist;
00104 gradient.gradients[i] = tf::Vector3(gx,gy,gz);
00105 }
00106 return in_collision;
00107 }
00108
00109 bool collision_proximity::getCollisionSphereCollision(const distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field,
00110 const std::vector<CollisionSphere>& sphere_list,
00111 double tolerance)
00112 {
00113 for(unsigned int i = 0; i < sphere_list.size(); i++) {
00114 tf::Vector3 p = sphere_list[i].center_;
00115 double gx, gy, gz;
00116 double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), gx, gy, gz);
00117 if(dist - sphere_list[i].radius_ < tolerance) {
00118 return true;
00119 }
00120 }
00121 return false;
00122
00123 }
00124
00128
00129 collision_proximity::BodyDecomposition::BodyDecomposition(const std::string& object_name, const shapes::Shape* shape, double resolution, double padding) :
00130 object_name_(object_name)
00131 {
00132 body_ = bodies::createBodyFromShape(shape);
00133 tf::Transform ident;
00134 ident.setIdentity();
00135 body_->setPose(ident);
00136 body_->setPadding(padding);
00137 collision_spheres_ = determineCollisionSpheres(body_, relative_cylinder_pose_);
00138 relative_collision_points_ = determineCollisionPoints(body_, resolution);
00139 posed_collision_points_ = relative_collision_points_;
00140 ROS_DEBUG_STREAM("Object " << object_name << " has " << relative_collision_points_.size() << " collision points");
00141 }
00142
00143 collision_proximity::BodyDecomposition::~BodyDecomposition()
00144 {
00145 delete body_;
00146 }
00147
00148 void collision_proximity::BodyDecomposition::updateSpheresPose(const tf::Transform& trans)
00149 {
00150
00151 tf::Transform cylTransform = trans * relative_cylinder_pose_;
00152 for(unsigned int i = 0; i < collision_spheres_.size(); i++) {
00153 collision_spheres_[i].center_ = cylTransform*collision_spheres_[i].relative_vec_;
00154 }
00155 }
00156
00157 void collision_proximity::BodyDecomposition::updatePointsPose(const tf::Transform& trans) {
00158
00159 posed_collision_points_.clear();
00160 posed_collision_points_.resize(relative_collision_points_.size());
00161 for(unsigned int i = 0; i < relative_collision_points_.size(); i++) {
00162 posed_collision_points_[i] = trans*relative_collision_points_[i];
00163 }
00164 }
00165
00166
00167 void collision_proximity::BodyDecomposition::updatePose(const tf::Transform& trans)
00168 {
00169 updateSpheresPose(trans);
00170 updatePointsPose(trans);
00171 }