47 std::vector<collision_detection::CollisionSphere>
50 std::vector<collision_detection::CollisionSphere> css;
54 unsigned int num_points = ceil(cyl.
length / (cyl.
radius / 2.0));
55 double spacing = cyl.
length / ((num_points * 1.0) - 1.0);
56 relative_transform = body->
getPose().inverse(Eigen::Isometry) * cyl.
pose;
58 for (
unsigned int i = 1; i < num_points - 1; i++)
61 relative_transform * Eigen::Vector3d(0, 0, (-cyl.
length / 2.0) + i * spacing), cyl.
radius);
71 double maximum_value,
bool stop_at_first_collision)
75 bool in_collision =
false;
76 for (
unsigned int i = 0; i < sphere_list.size(); i++)
78 Eigen::Vector3d p = sphere_centers[i];
79 Eigen::Vector3d grad(0, 0, 0);
81 double dist = this->
getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
82 if (!in_bounds && grad.norm() > 0)
88 if (dist < maximum_value)
92 dist -= sphere_list[i].radius_;
94 if ((dist < 0) && (-dist >= tolerance))
99 dist = std::abs(dist);
103 if (sphere_list[i].radius_ - dist > tolerance)
116 gradient.
types[i] = type;
122 if (stop_at_first_collision && in_collision)
131 const std::vector<CollisionSphere>& sphere_list,
135 bool subtract_radii,
double maximum_value,
136 bool stop_at_first_collision)
140 bool in_collision =
false;
141 for (
unsigned int i = 0; i < sphere_list.size(); i++)
143 Eigen::Vector3d p = sphere_centers[i];
144 Eigen::Vector3d grad;
146 double dist = distance_field->
getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
147 if (!in_bounds && grad.norm() >
EPSILON)
149 ROS_DEBUG(
"Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
153 if (dist < maximum_value)
157 dist -= sphere_list[i].radius_;
159 if ((dist < 0) && (-dist >= tolerance))
166 if (sphere_list[i].radius_ - dist > tolerance)
179 gradient.
types[i] = type;
185 if (stop_at_first_collision && in_collision)
194 const std::vector<CollisionSphere>& sphere_list,
196 double maximum_value,
double tolerance)
198 for (
unsigned int i = 0; i < sphere_list.size(); i++)
200 Eigen::Vector3d p = sphere_centers[i];
201 Eigen::Vector3d grad;
202 bool in_bounds =
true;
203 double dist = distance_field->
getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
205 if (!in_bounds && grad.norm() > 0)
207 ROS_DEBUG(
"Collision sphere point is out of bounds");
211 if ((maximum_value > dist) && (sphere_list[i].radius_ - dist > tolerance))
221 const std::vector<CollisionSphere>& sphere_list,
223 double maximum_value,
double tolerance,
unsigned int num_coll,
224 std::vector<unsigned int>& colls)
227 for (
unsigned int i = 0; i < sphere_list.size(); i++)
229 Eigen::Vector3d p = sphere_centers[i];
230 Eigen::Vector3d grad;
231 bool in_bounds =
true;
232 double dist = distance_field->
getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
233 if (!in_bounds && (grad.norm() > 0))
235 ROS_DEBUG(
"Collision sphere point is out of bounds");
238 if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance))
246 if (colls.size() >= num_coll)
253 return colls.size() > 0;
263 std::vector<shapes::ShapeConstPtr>
shapes;
266 shapes.push_back(shape);
267 init(shapes, poses, resolution, padding);
274 init(shapes, poses, resolution, padding);
282 for (
unsigned int i = 0; i < shapes.size(); i++)
284 bodies_.addBody(shapes[i]->clone(), poses[i], padding);
288 collision_spheres_.clear();
289 relative_collision_points_.clear();
290 std::vector<CollisionSphere> body_spheres;
292 for (
unsigned int i = 0; i < bodies_.getCount(); i++)
294 body_spheres.clear();
295 body_collision_points.clear();
298 collision_spheres_.insert(collision_spheres_.end(), body_spheres.begin(), body_spheres.end());
301 relative_collision_points_.insert(relative_collision_points_.end(), body_collision_points.begin(),
302 body_collision_points.end());
305 sphere_radii_.resize(collision_spheres_.size());
306 for (
unsigned int i = 0; i < collision_spheres_.size(); i++)
308 sphere_radii_[i] = collision_spheres_[i].radius_;
312 std::vector<bodies::BoundingSphere> bounding_spheres(bodies_.getCount());
313 for (
unsigned int i = 0; i < bodies_.getCount(); i++)
315 bodies_.getBody(i)->computeBoundingSphere(bounding_spheres[i]);
319 ROS_DEBUG_STREAM(
"BodyDecomposition generated " << collision_spheres_.size() <<
" collision spheres out of " 320 << shapes.size() <<
" shapes");
329 const BodyDecompositionConstPtr& body_decomposition)
330 : body_decomposition_(body_decomposition)
336 const BodyDecompositionConstPtr& body_decomposition,
const Eigen::Affine3d& trans)
343 std::shared_ptr<const octomap::OcTree> octree)
346 int num_nodes = octree->getNumLeafNodes();
348 for (octomap::OcTree::tree_iterator tree_iter = octree->begin_tree(); tree_iter != octree->end_tree(); ++tree_iter)
350 Eigen::Vector3d p = Eigen::Vector3d(tree_iter.getX(), tree_iter.getY(), tree_iter.getZ());
369 const BodyDecompositionConstPtr& body_decomposition)
398 const PosedBodySphereDecompositionConstPtr& p2)
400 Eigen::Vector3d p1_sphere_center = p1->getBoundingSphereCenter();
401 Eigen::Vector3d p2_sphere_center = p2->getBoundingSphereCenter();
402 double p1_radius = p1->getBoundingSphereRadius();
403 double p2_radius = p2->getBoundingSphereRadius();
405 double dist = (p1_sphere_center - p2_sphere_center).squaredNorm();
406 if (dist < (p1_radius + p2_radius))
414 const std_msgs::ColorRGBA& color,
const std::string& frame_id,
const std::string& ns,
const ros::Duration& dur,
415 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions, visualization_msgs::MarkerArray& arr)
417 unsigned int count = 0;
418 for (
unsigned int i = 0; i < posed_decompositions.size(); i++)
420 if (posed_decompositions[i])
422 for (
unsigned int j = 0; j < posed_decompositions[i]->getCollisionSpheres().size(); j++)
424 visualization_msgs::Marker sphere;
425 sphere.type = visualization_msgs::Marker::SPHERE;
427 sphere.header.frame_id = frame_id;
430 sphere.lifetime = dur;
431 sphere.color = color;
432 sphere.scale.x = sphere.scale.y = sphere.scale.z =
433 posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
434 sphere.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
435 sphere.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
436 sphere.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
437 arr.markers.push_back(sphere);
444 const std::string& frame_id,
const std::string& ns,
const ros::Duration& dur,
445 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
446 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
447 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
449 if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
451 ROS_WARN_NAMED(
"collision_distance_field",
"Size mismatch between gradients %u and decompositions %u",
452 (
unsigned int)gradients.size(),
453 (
unsigned int)(posed_decompositions.size() + posed_vector_decompositions.size()));
456 for (
unsigned int i = 0; i < gradients.size(); i++)
458 for (
unsigned int j = 0; j < gradients[i].distances.size(); j++)
460 visualization_msgs::Marker arrow_mark;
461 arrow_mark.header.frame_id = frame_id;
465 arrow_mark.ns =
"self_coll_gradients";
471 arrow_mark.id = i * 1000 + j;
475 if (gradients[i].distances[j] > 0.0 && gradients[i].distances[j] != DBL_MAX)
477 if (gradients[i].gradients[j].norm() > 0.0)
479 xscale = gradients[i].gradients[j].x() / gradients[i].gradients[j].norm();
480 yscale = gradients[i].gradients[j].y() / gradients[i].gradients[j].norm();
481 zscale = gradients[i].gradients[j].z() / gradients[i].gradients[j].norm();
485 ROS_DEBUG_NAMED(
"collision_distance_field",
"Negative length for %u %d %lf", i, arrow_mark.id,
486 gradients[i].gradients[j].norm());
491 ROS_DEBUG_NAMED(
"collision_distance_field",
"Negative dist %lf for %u %d", gradients[i].distances[j], i,
494 arrow_mark.points.resize(2);
495 if (i < posed_decompositions.size())
497 arrow_mark.points[1].x = posed_decompositions[i]->getSphereCenters()[j].x();
498 arrow_mark.points[1].y = posed_decompositions[i]->getSphereCenters()[j].y();
499 arrow_mark.points[1].z = posed_decompositions[i]->getSphereCenters()[j].z();
503 arrow_mark.points[1].x =
504 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
505 arrow_mark.points[1].y =
506 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
507 arrow_mark.points[1].z =
508 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
510 arrow_mark.points[0] = arrow_mark.points[1];
511 arrow_mark.points[0].x -= xscale * gradients[i].distances[j];
512 arrow_mark.points[0].y -= yscale * gradients[i].distances[j];
513 arrow_mark.points[0].z -= zscale * gradients[i].distances[j];
514 arrow_mark.scale.x = 0.01;
515 arrow_mark.scale.y = 0.03;
516 arrow_mark.color.a = 1.0;
519 arrow_mark.color.r = 1.0;
520 arrow_mark.color.g = 0.2;
521 arrow_mark.color.b = .5;
525 arrow_mark.color.r = .2;
526 arrow_mark.color.g = 1.0;
527 arrow_mark.color.b = .5;
531 arrow_mark.color.r = .2;
532 arrow_mark.color.g = .5;
533 arrow_mark.color.b = 1.0;
537 arrow_mark.color.r = 1.0;
538 arrow_mark.color.g = .2;
539 arrow_mark.color.b = 1.0;
541 arr.markers.push_back(arrow_mark);
547 const std::string& frame_id,
const std::string& ns,
const ros::Duration& dur,
548 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
549 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
550 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
552 if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
554 ROS_WARN_NAMED(
"collision_distance_field",
"Size mismatch between gradients %zu and decompositions %zu",
555 gradients.size(), posed_decompositions.size() + posed_vector_decompositions.size());
558 for (
unsigned int i = 0; i < gradients.size(); i++)
560 for (
unsigned int j = 0; j < gradients[i].types.size(); j++)
562 visualization_msgs::Marker sphere_mark;
563 sphere_mark.type = visualization_msgs::Marker::SPHERE;
564 sphere_mark.header.frame_id = frame_id;
568 sphere_mark.ns =
"distance_collisions";
574 sphere_mark.id = i * 1000 + j;
575 if (i < posed_decompositions.size())
577 sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
578 posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
579 sphere_mark.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
580 sphere_mark.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
581 sphere_mark.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
585 sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
586 posed_vector_decompositions[i - posed_decompositions.size()]->getCollisionSpheres()[j].radius_ * 2.0;
587 sphere_mark.pose.position.x =
588 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
589 sphere_mark.pose.position.y =
590 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
591 sphere_mark.pose.position.z =
592 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
594 sphere_mark.pose.orientation.w = 1.0;
595 sphere_mark.color.a = 1.0;
598 sphere_mark.color.r = 1.0;
599 sphere_mark.color.g = 0.2;
600 sphere_mark.color.b = .5;
604 sphere_mark.color.r = .2;
605 sphere_mark.color.g = 1.0;
606 sphere_mark.color.b = .5;
610 sphere_mark.color.r = .2;
611 sphere_mark.color.g = .5;
612 sphere_mark.color.b = 1.0;
616 sphere_mark.color.r = 1.0;
617 sphere_mark.color.g = .2;
618 sphere_mark.color.b = 1.0;
620 arr.markers.push_back(sphere_mark);
const Eigen::Affine3d & getPose() const
void updatePose(const Eigen::Affine3d &linkTransform)
void getProximityGradientMarkers(const std::string &frame_id, const std::string &ns, const ros::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, const std::vector< PosedBodySphereDecompositionVectorPtr > &posed_vector_decompositions, const std::vector< GradientInfo > &gradients, visualization_msgs::MarkerArray &arr)
std::vector< CollisionSphere > determineCollisionSpheres(const bodies::Body *body, Eigen::Affine3d &relativeTransform)
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const =0
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecomposition(const BodyDecompositionConstPtr &body_decomposition)
#define ROS_WARN_NAMED(name,...)
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bool getCollisionSphereGradients(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
void init(const M_string &remappings)
EigenSTL::vector_Vector3d sphere_centers_
BodyDecompositionConstPtr body_decomposition_
double getDistanceGradient(double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z, bool &in_bounds) const
Gets not only the distance to the nearest cell but the gradient direction. The gradient is computed a...
EigenSTL::vector_Vector3d posed_collision_points_
EigenSTL::vector_Vector3d gradients
void init(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &poses, double resolution, double padding)
double getDistanceGradient(double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z, bool &in_bounds) const
Gets not only the distance to the nearest cell but the gradient direction. The point defined by the x...
std::vector< CollisionType > types
#define ROS_DEBUG_NAMED(name,...)
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points...
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double closest_distance
static const double resolution
std::vector< double > distances
void getCollisionSphereMarkers(const std_msgs::ColorRGBA &color, const std::string &frame_id, const std::string &ns, const ros::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, visualization_msgs::MarkerArray &arr)
void updatePose(const Eigen::Affine3d &linkTransform)
#define ROS_DEBUG_STREAM(args)
Namespace for holding classes that generate distance fields.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecomposition(const BodyDecompositionConstPtr &body_decomposition)
EigenSTL::vector_Vector3d posed_collision_points_
Eigen::Vector3d posed_bounding_sphere_center_
BodyDecompositionConstPtr body_decomposition_
static const double EPSILON
bool getCollisionSphereGradients(const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
static const double RESOLUTION_SCALE
void findInternalPointsConvex(const bodies::Body &body, double resolution, EigenSTL::vector_Vector3d &points)
Find all points on a regular grid that are internal to the body, assuming the body is a convex shape...
bool getCollisionSphereCollision(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, double maximum_value, double tolerance)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BodyDecomposition(const shapes::ShapeConstPtr &shape, double resolution, double padding=0.01)
void getCollisionMarkers(const std::string &frame_id, const std::string &ns, const ros::Duration &dur, const std::vector< PosedBodySphereDecompositionPtr > &posed_decompositions, const std::vector< PosedBodySphereDecompositionVectorPtr > &posed_vector_decompositions, const std::vector< GradientInfo > &gradients, visualization_msgs::MarkerArray &arr)
std::shared_ptr< const Shape > ShapeConstPtr