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 #include <moveit/collision_distance_field/collision_distance_field_types.h>
00038 #include <geometric_shapes/body_operations.h>
00039 #include <moveit/distance_field/distance_field.h>
00040 #include <moveit/distance_field/find_internal_points.h>
00041 #include <ros/console.h>
00042
00043 const static double RESOLUTION_SCALE = 1.0;
00044 const static double EPSILON = 0.0001;
00045
00046 std::vector<collision_detection::CollisionSphere>
00047 collision_detection::determineCollisionSpheres(const bodies::Body* body, Eigen::Affine3d& relative_transform)
00048 {
00049 std::vector<collision_detection::CollisionSphere> css;
00050
00051 bodies::BoundingCylinder cyl;
00052 body->computeBoundingCylinder(cyl);
00053 unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0));
00054 double spacing = cyl.length / ((num_points * 1.0) - 1.0);
00055 relative_transform = body->getPose().inverse() * cyl.pose;
00056
00057 for (unsigned int i = 1; i < num_points - 1; i++)
00058 {
00059 collision_detection::CollisionSphere cs(
00060 relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius);
00061 css.push_back(cs);
00062 }
00063
00064 return css;
00065 }
00066
00067 bool collision_detection::PosedDistanceField::getCollisionSphereGradients(
00068 const std::vector<CollisionSphere>& sphere_list, const EigenSTL::vector_Vector3d& sphere_centers,
00069 GradientInfo& gradient, const collision_detection::CollisionType& type, double tolerance, bool subtract_radii,
00070 double maximum_value, bool stop_at_first_collision)
00071 {
00072
00073
00074 bool in_collision = false;
00075 for (unsigned int i = 0; i < sphere_list.size(); i++)
00076 {
00077 Eigen::Vector3d p = sphere_centers[i];
00078 Eigen::Vector3d grad(0, 0, 0);
00079 bool in_bounds;
00080 double dist = this->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
00081 if (!in_bounds && grad.norm() > 0)
00082 {
00083
00084 return true;
00085 }
00086
00087 if (dist < maximum_value)
00088 {
00089 if (subtract_radii)
00090 {
00091 dist -= sphere_list[i].radius_;
00092
00093 if ((dist < 0) && (-dist >= tolerance))
00094 {
00095 in_collision = true;
00096 }
00097
00098 dist = std::abs(dist);
00099 }
00100 else
00101 {
00102 if (sphere_list[i].radius_ - dist > tolerance)
00103 {
00104 in_collision = true;
00105 }
00106 }
00107
00108 if (dist < gradient.closest_distance)
00109 {
00110 gradient.closest_distance = dist;
00111 }
00112
00113 if (dist < gradient.distances[i])
00114 {
00115 gradient.types[i] = type;
00116 gradient.distances[i] = dist;
00117 gradient.gradients[i] = grad;
00118 }
00119 }
00120
00121 if (stop_at_first_collision && in_collision)
00122 {
00123 return true;
00124 }
00125 }
00126 return in_collision;
00127 }
00128
00129 bool collision_detection::getCollisionSphereGradients(const distance_field::DistanceField* distance_field,
00130 const std::vector<CollisionSphere>& sphere_list,
00131 const EigenSTL::vector_Vector3d& sphere_centers,
00132 GradientInfo& gradient,
00133 const collision_detection::CollisionType& type, double tolerance,
00134 bool subtract_radii, double maximum_value,
00135 bool stop_at_first_collision)
00136 {
00137
00138
00139 bool in_collision = false;
00140 for (unsigned int i = 0; i < sphere_list.size(); i++)
00141 {
00142 Eigen::Vector3d p = sphere_centers[i];
00143 Eigen::Vector3d grad;
00144 bool in_bounds;
00145 double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
00146 if (!in_bounds && grad.norm() > EPSILON)
00147 {
00148 ROS_DEBUG("Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
00149 return true;
00150 }
00151
00152 if (dist < maximum_value)
00153 {
00154 if (subtract_radii)
00155 {
00156 dist -= sphere_list[i].radius_;
00157
00158 if ((dist < 0) && (-dist >= tolerance))
00159 {
00160 in_collision = true;
00161 }
00162 }
00163 else
00164 {
00165 if (sphere_list[i].radius_ - dist > tolerance)
00166 {
00167 in_collision = true;
00168 }
00169 }
00170
00171 if (dist < gradient.closest_distance)
00172 {
00173 gradient.closest_distance = dist;
00174 }
00175
00176 if (dist < gradient.distances[i])
00177 {
00178 gradient.types[i] = type;
00179 gradient.distances[i] = dist;
00180 gradient.gradients[i] = grad;
00181 }
00182 }
00183
00184 if (stop_at_first_collision && in_collision)
00185 {
00186 return true;
00187 }
00188 }
00189 return in_collision;
00190 }
00191
00192 bool collision_detection::getCollisionSphereCollision(const distance_field::DistanceField* distance_field,
00193 const std::vector<CollisionSphere>& sphere_list,
00194 const EigenSTL::vector_Vector3d& sphere_centers,
00195 double maximum_value, double tolerance)
00196 {
00197 for (unsigned int i = 0; i < sphere_list.size(); i++)
00198 {
00199 Eigen::Vector3d p = sphere_centers[i];
00200 Eigen::Vector3d grad;
00201 bool in_bounds = true;
00202 double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
00203
00204 if (!in_bounds && grad.norm() > 0)
00205 {
00206 ROS_DEBUG("Collision sphere point is out of bounds");
00207 return true;
00208 }
00209
00210 if ((maximum_value > dist) && (sphere_list[i].radius_ - dist > tolerance))
00211 {
00212 return true;
00213 }
00214 }
00215
00216 return false;
00217 }
00218
00219 bool collision_detection::getCollisionSphereCollision(const distance_field::DistanceField* distance_field,
00220 const std::vector<CollisionSphere>& sphere_list,
00221 const EigenSTL::vector_Vector3d& sphere_centers,
00222 double maximum_value, double tolerance, unsigned int num_coll,
00223 std::vector<unsigned int>& colls)
00224 {
00225 colls.clear();
00226 for (unsigned int i = 0; i < sphere_list.size(); i++)
00227 {
00228 Eigen::Vector3d p = sphere_centers[i];
00229 Eigen::Vector3d grad;
00230 bool in_bounds = true;
00231 double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
00232 if (!in_bounds && (grad.norm() > 0))
00233 {
00234 ROS_DEBUG("Collision sphere point is out of bounds");
00235 return true;
00236 }
00237 if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance))
00238 {
00239 if (num_coll == 0)
00240 {
00241 return true;
00242 }
00243
00244 colls.push_back(i);
00245 if (colls.size() >= num_coll)
00246 {
00247 return true;
00248 }
00249 }
00250 }
00251
00252 return colls.size() > 0;
00253 }
00254
00258
00259 collision_detection::BodyDecomposition::BodyDecomposition(const shapes::ShapeConstPtr& shape, double resolution,
00260 double padding)
00261 {
00262 std::vector<shapes::ShapeConstPtr> shapes;
00263 EigenSTL::vector_Affine3d poses(1, Eigen::Affine3d::Identity());
00264
00265 shapes.push_back(shape);
00266 init(shapes, poses, resolution, padding);
00267 }
00268
00269 collision_detection::BodyDecomposition::BodyDecomposition(const std::vector<shapes::ShapeConstPtr>& shapes,
00270 const EigenSTL::vector_Affine3d& poses, double resolution,
00271 double padding)
00272 {
00273 init(shapes, poses, resolution, padding);
00274 }
00275
00276 void collision_detection::BodyDecomposition::init(const std::vector<shapes::ShapeConstPtr>& shapes,
00277 const EigenSTL::vector_Affine3d& poses, double resolution,
00278 double padding)
00279 {
00280 bodies_.clear();
00281 for (unsigned int i = 0; i < shapes.size(); i++)
00282 {
00283 bodies_.addBody(shapes[i]->clone(), poses[i], padding);
00284 }
00285
00286
00287 collision_spheres_.clear();
00288 relative_collision_points_.clear();
00289 std::vector<CollisionSphere> body_spheres;
00290 EigenSTL::vector_Vector3d body_collision_points;
00291 double radius = RESOLUTION_SCALE * resolution;
00292 for (unsigned int i = 0; i < bodies_.getCount(); i++)
00293 {
00294 body_spheres.clear();
00295 body_collision_points.clear();
00296
00297 body_spheres = determineCollisionSpheres(bodies_.getBody(i), relative_cylinder_pose_);
00298 collision_spheres_.insert(collision_spheres_.end(), body_spheres.begin(), body_spheres.end());
00299
00300 distance_field::findInternalPointsConvex(*bodies_.getBody(i), resolution, body_collision_points);
00301 relative_collision_points_.insert(relative_collision_points_.end(), body_collision_points.begin(),
00302 body_collision_points.end());
00303 }
00304
00305 sphere_radii_.resize(collision_spheres_.size());
00306 for (unsigned int i = 0; i < collision_spheres_.size(); i++)
00307 {
00308 sphere_radii_[i] = collision_spheres_[i].radius_;
00309 }
00310
00311
00312 std::vector<bodies::BoundingSphere> bounding_spheres(bodies_.getCount());
00313 for (unsigned int i = 0; i < bodies_.getCount(); i++)
00314 {
00315 bodies_.getBody(i)->computeBoundingSphere(bounding_spheres[i]);
00316 }
00317 bodies::mergeBoundingSpheres(bounding_spheres, relative_bounding_sphere_);
00318
00319 ROS_DEBUG_STREAM("BodyDecomposition generated " << collision_spheres_.size() << " collision spheres out of "
00320 << shapes.size() << " shapes");
00321 }
00322
00323 collision_detection::BodyDecomposition::~BodyDecomposition()
00324 {
00325 bodies_.clear();
00326 }
00327
00328 collision_detection::PosedBodyPointDecomposition::PosedBodyPointDecomposition(
00329 const BodyDecompositionConstPtr& body_decomposition)
00330 : body_decomposition_(body_decomposition)
00331 {
00332 posed_collision_points_ = body_decomposition_->getCollisionPoints();
00333 }
00334
00335 collision_detection::PosedBodyPointDecomposition::PosedBodyPointDecomposition(
00336 const BodyDecompositionConstPtr& body_decomposition, const Eigen::Affine3d& trans)
00337 : body_decomposition_(body_decomposition)
00338 {
00339 updatePose(trans);
00340 }
00341
00342 collision_detection::PosedBodyPointDecomposition::PosedBodyPointDecomposition(
00343 boost::shared_ptr<const octomap::OcTree> octree)
00344 : body_decomposition_()
00345 {
00346 int num_nodes = octree->getNumLeafNodes();
00347 posed_collision_points_.reserve(num_nodes);
00348 for (octomap::OcTree::tree_iterator tree_iter = octree->begin_tree(); tree_iter != octree->end_tree(); ++tree_iter)
00349 {
00350 Eigen::Vector3d p = Eigen::Vector3d(tree_iter.getX(), tree_iter.getY(), tree_iter.getZ());
00351 posed_collision_points_.push_back(p);
00352 }
00353 }
00354
00355 void collision_detection::PosedBodyPointDecomposition::updatePose(const Eigen::Affine3d& trans)
00356 {
00357 if (body_decomposition_)
00358 {
00359 posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
00360
00361 for (unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); i++)
00362 {
00363 posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
00364 }
00365 }
00366 }
00367
00368 collision_detection::PosedBodySphereDecomposition::PosedBodySphereDecomposition(
00369 const BodyDecompositionConstPtr& body_decomposition)
00370 : body_decomposition_(body_decomposition)
00371 {
00372 posed_bounding_sphere_center_ = body_decomposition_->getRelativeBoundingSphere().center;
00373 sphere_centers_.resize(body_decomposition_->getCollisionSpheres().size());
00374 updatePose(Eigen::Affine3d::Identity());
00375 }
00376
00377 void collision_detection::PosedBodySphereDecomposition::updatePose(const Eigen::Affine3d& trans)
00378 {
00379
00380 posed_bounding_sphere_center_ = trans * body_decomposition_->getRelativeBoundingSphere().center;
00381 for (unsigned int i = 0; i < body_decomposition_->getCollisionSpheres().size(); i++)
00382 {
00383 sphere_centers_[i] = trans * body_decomposition_->getCollisionSpheres()[i].relative_vec_;
00384 }
00385
00386
00387 if (!body_decomposition_->getCollisionPoints().empty())
00388 {
00389 posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
00390 for (unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); i++)
00391 {
00392 posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
00393 }
00394 }
00395 }
00396
00397 bool collision_detection::doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1,
00398 const PosedBodySphereDecompositionConstPtr& p2)
00399 {
00400 Eigen::Vector3d p1_sphere_center = p1->getBoundingSphereCenter();
00401 Eigen::Vector3d p2_sphere_center = p2->getBoundingSphereCenter();
00402 double p1_radius = p1->getBoundingSphereRadius();
00403 double p2_radius = p2->getBoundingSphereRadius();
00404
00405 double dist = (p1_sphere_center - p2_sphere_center).squaredNorm();
00406 if (dist < (p1_radius + p2_radius))
00407 {
00408 return true;
00409 }
00410 return false;
00411 }
00412
00413 void collision_detection::getCollisionSphereMarkers(
00414 const std_msgs::ColorRGBA& color, const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
00415 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions, visualization_msgs::MarkerArray& arr)
00416 {
00417 unsigned int count = 0;
00418 for (unsigned int i = 0; i < posed_decompositions.size(); i++)
00419 {
00420 if (posed_decompositions[i])
00421 {
00422 for (unsigned int j = 0; j < posed_decompositions[i]->getCollisionSpheres().size(); j++)
00423 {
00424 visualization_msgs::Marker sphere;
00425 sphere.type = visualization_msgs::Marker::SPHERE;
00426 sphere.header.stamp = ros::Time::now();
00427 sphere.header.frame_id = frame_id;
00428 sphere.ns = ns;
00429 sphere.id = count++;
00430 sphere.lifetime = dur;
00431 sphere.color = color;
00432 sphere.scale.x = sphere.scale.y = sphere.scale.z =
00433 posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
00434 sphere.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
00435 sphere.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
00436 sphere.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
00437 arr.markers.push_back(sphere);
00438 }
00439 }
00440 }
00441 }
00442
00443 void collision_detection::getProximityGradientMarkers(
00444 const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
00445 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
00446 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
00447 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
00448 {
00449 if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
00450 {
00451 logWarn("Size mismatch between gradients %u and decompositions %u", (unsigned int)gradients.size(),
00452 (unsigned int)(posed_decompositions.size() + posed_vector_decompositions.size()));
00453 return;
00454 }
00455 for (unsigned int i = 0; i < gradients.size(); i++)
00456 {
00457 for (unsigned int j = 0; j < gradients[i].distances.size(); j++)
00458 {
00459 visualization_msgs::Marker arrow_mark;
00460 arrow_mark.header.frame_id = frame_id;
00461 arrow_mark.header.stamp = ros::Time::now();
00462 if (ns.empty())
00463 {
00464 arrow_mark.ns = "self_coll_gradients";
00465 }
00466 else
00467 {
00468 arrow_mark.ns = ns;
00469 }
00470 arrow_mark.id = i * 1000 + j;
00471 double xscale = 0.0;
00472 double yscale = 0.0;
00473 double zscale = 0.0;
00474 if (gradients[i].distances[j] > 0.0 && gradients[i].distances[j] != DBL_MAX)
00475 {
00476 if (gradients[i].gradients[j].norm() > 0.0)
00477 {
00478 xscale = gradients[i].gradients[j].x() / gradients[i].gradients[j].norm();
00479 yscale = gradients[i].gradients[j].y() / gradients[i].gradients[j].norm();
00480 zscale = gradients[i].gradients[j].z() / gradients[i].gradients[j].norm();
00481 }
00482 else
00483 {
00484 logDebug("Negative length for %u %d %lf", i, arrow_mark.id, gradients[i].gradients[j].norm());
00485 }
00486 }
00487 else
00488 {
00489 logDebug("Negative dist %lf for %u %d", gradients[i].distances[j], i, arrow_mark.id);
00490 }
00491 arrow_mark.points.resize(2);
00492 if (i < posed_decompositions.size())
00493 {
00494 arrow_mark.points[1].x = posed_decompositions[i]->getSphereCenters()[j].x();
00495 arrow_mark.points[1].y = posed_decompositions[i]->getSphereCenters()[j].y();
00496 arrow_mark.points[1].z = posed_decompositions[i]->getSphereCenters()[j].z();
00497 }
00498 else
00499 {
00500 arrow_mark.points[1].x =
00501 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
00502 arrow_mark.points[1].y =
00503 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
00504 arrow_mark.points[1].z =
00505 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
00506 }
00507 arrow_mark.points[0] = arrow_mark.points[1];
00508 arrow_mark.points[0].x -= xscale * gradients[i].distances[j];
00509 arrow_mark.points[0].y -= yscale * gradients[i].distances[j];
00510 arrow_mark.points[0].z -= zscale * gradients[i].distances[j];
00511 arrow_mark.scale.x = 0.01;
00512 arrow_mark.scale.y = 0.03;
00513 arrow_mark.color.a = 1.0;
00514 if (gradients[i].types[j] == collision_detection::SELF)
00515 {
00516 arrow_mark.color.r = 1.0;
00517 arrow_mark.color.g = 0.2;
00518 arrow_mark.color.b = .5;
00519 }
00520 else if (gradients[i].types[j] == collision_detection::INTRA)
00521 {
00522 arrow_mark.color.r = .2;
00523 arrow_mark.color.g = 1.0;
00524 arrow_mark.color.b = .5;
00525 }
00526 else if (gradients[i].types[j] == collision_detection::ENVIRONMENT)
00527 {
00528 arrow_mark.color.r = .2;
00529 arrow_mark.color.g = .5;
00530 arrow_mark.color.b = 1.0;
00531 }
00532 else if (gradients[i].types[j] == collision_detection::NONE)
00533 {
00534 arrow_mark.color.r = 1.0;
00535 arrow_mark.color.g = .2;
00536 arrow_mark.color.b = 1.0;
00537 }
00538 arr.markers.push_back(arrow_mark);
00539 }
00540 }
00541 }
00542
00543 void collision_detection::getCollisionMarkers(
00544 const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
00545 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
00546 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
00547 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
00548 {
00549 if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
00550 {
00551 logWarn("Size mismatch between gradients %u and decompositions ", (unsigned int)gradients.size(),
00552 (unsigned int)(posed_decompositions.size() + posed_vector_decompositions.size()));
00553 return;
00554 }
00555 for (unsigned int i = 0; i < gradients.size(); i++)
00556 {
00557 for (unsigned int j = 0; j < gradients[i].types.size(); j++)
00558 {
00559 visualization_msgs::Marker sphere_mark;
00560 sphere_mark.type = visualization_msgs::Marker::SPHERE;
00561 sphere_mark.header.frame_id = frame_id;
00562 sphere_mark.header.stamp = ros::Time::now();
00563 if (ns.empty())
00564 {
00565 sphere_mark.ns = "distance_collisions";
00566 }
00567 else
00568 {
00569 sphere_mark.ns = ns;
00570 }
00571 sphere_mark.id = i * 1000 + j;
00572 if (i < posed_decompositions.size())
00573 {
00574 sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
00575 posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
00576 sphere_mark.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
00577 sphere_mark.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
00578 sphere_mark.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
00579 }
00580 else
00581 {
00582 sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
00583 posed_vector_decompositions[i - posed_decompositions.size()]->getCollisionSpheres()[j].radius_ * 2.0;
00584 sphere_mark.pose.position.x =
00585 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
00586 sphere_mark.pose.position.y =
00587 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
00588 sphere_mark.pose.position.z =
00589 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
00590 }
00591 sphere_mark.pose.orientation.w = 1.0;
00592 sphere_mark.color.a = 1.0;
00593 if (gradients[i].types[j] == collision_detection::SELF)
00594 {
00595 sphere_mark.color.r = 1.0;
00596 sphere_mark.color.g = 0.2;
00597 sphere_mark.color.b = .5;
00598 }
00599 else if (gradients[i].types[j] == collision_detection::INTRA)
00600 {
00601 sphere_mark.color.r = .2;
00602 sphere_mark.color.g = 1.0;
00603 sphere_mark.color.b = .5;
00604 }
00605 else if (gradients[i].types[j] == collision_detection::ENVIRONMENT)
00606 {
00607 sphere_mark.color.r = .2;
00608 sphere_mark.color.g = .5;
00609 sphere_mark.color.b = 1.0;
00610 }
00611 else
00612 {
00613 sphere_mark.color.r = 1.0;
00614 sphere_mark.color.g = .2;
00615 sphere_mark.color.b = 1.0;
00616 }
00617 arr.markers.push_back(sphere_mark);
00618 }
00619 }
00620 }