46 std::vector<collision_detection::CollisionSphere>
49 std::vector<collision_detection::CollisionSphere> css;
50 if (body->
getType() == shapes::ShapeType::SPHERE)
65 unsigned int num_points = ceil(cyl.
length / (cyl.
radius / 2.0));
66 double spacing = cyl.
length / ((num_points * 1.0) - 1.0);
67 relative_transform = cyl.
pose;
69 for (
unsigned int i = 1; i < num_points - 1; i++)
84 double maximum_value,
bool stop_at_first_collision)
88 bool in_collision =
false;
89 for (
unsigned int i = 0; i < sphere_list.size(); i++)
94 double dist = this->
getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
95 if (!in_bounds && grad.norm() > 0)
101 if (dist < maximum_value)
105 dist -= sphere_list[i].radius_;
112 dist = std::abs(dist);
116 if (sphere_list[i].radius_ - dist >
tolerance)
129 gradient.
types[i] = type;
135 if (stop_at_first_collision && in_collision)
144 const std::vector<CollisionSphere>& sphere_list,
148 bool subtract_radii,
double maximum_value,
149 bool stop_at_first_collision)
153 bool in_collision =
false;
154 for (
unsigned int i = 0; i < sphere_list.size(); i++)
159 double dist =
distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
160 if (!in_bounds && grad.norm() >
EPSILON)
162 ROS_DEBUG(
"Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
166 if (dist < maximum_value)
170 dist -= sphere_list[i].radius_;
179 if (sphere_list[i].radius_ - dist >
tolerance)
192 gradient.
types[i] = type;
198 if (stop_at_first_collision && in_collision)
207 const std::vector<CollisionSphere>& sphere_list,
209 double maximum_value,
double tolerance)
211 for (
unsigned int i = 0; i < sphere_list.size(); i++)
215 bool in_bounds =
true;
216 double dist =
distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
218 if (!in_bounds && grad.norm() > 0)
220 ROS_DEBUG(
"Collision sphere point is out of bounds");
224 if ((maximum_value > dist) && (sphere_list[i].radius_ - dist >
tolerance))
234 const std::vector<CollisionSphere>& sphere_list,
236 double maximum_value,
double tolerance,
unsigned int num_coll,
237 std::vector<unsigned int>& colls)
240 for (
unsigned int i = 0; i < sphere_list.size(); i++)
244 bool in_bounds =
true;
245 double dist =
distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
246 if (!in_bounds && (grad.norm() > 0))
248 ROS_DEBUG(
"Collision sphere point is out of bounds");
251 if (maximum_value > dist && (sphere_list[i].radius_ - dist >
tolerance))
259 if (colls.size() >= num_coll)
266 return !colls.empty();
276 std::vector<shapes::ShapeConstPtr>
shapes;
295 for (
unsigned int i = 0; i <
shapes.size(); i++)
297 bodies_.addBody(
shapes[i].
get(), poses[i], padding);
301 collision_spheres_.clear();
302 relative_collision_points_.clear();
303 std::vector<CollisionSphere> body_spheres;
305 for (
unsigned int i = 0; i < bodies_.getCount(); i++)
307 body_spheres.clear();
308 body_collision_points.clear();
311 collision_spheres_.insert(collision_spheres_.end(), body_spheres.begin(), body_spheres.end());
314 relative_collision_points_.insert(relative_collision_points_.end(), body_collision_points.begin(),
315 body_collision_points.end());
318 sphere_radii_.resize(collision_spheres_.size());
319 for (
unsigned int i = 0; i < collision_spheres_.size(); i++)
321 sphere_radii_[i] = collision_spheres_[i].radius_;
325 std::vector<bodies::BoundingSphere> bounding_spheres(bodies_.getCount());
326 for (
unsigned int i = 0; i < bodies_.getCount(); i++)
328 bodies_.getBody(i)->computeBoundingSphere(bounding_spheres[i]);
332 ROS_DEBUG_STREAM(
"BodyDecomposition generated " << collision_spheres_.size() <<
" collision spheres out of "
333 <<
shapes.size() <<
" shapes");
342 const BodyDecompositionConstPtr& body_decomposition)
343 : body_decomposition_(body_decomposition)
349 const BodyDecompositionConstPtr& body_decomposition,
const Eigen::Isometry3d& trans)
350 : body_decomposition_(body_decomposition)
356 const std::shared_ptr<const octomap::OcTree>& octree)
357 : body_decomposition_()
359 int num_nodes = octree->getNumLeafNodes();
361 for (octomap::OcTree::tree_iterator tree_iter = octree->begin_tree(); tree_iter != octree->end_tree(); ++tree_iter)
370 if (body_decomposition_)
372 posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
374 for (
unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); i++)
376 posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
382 const BodyDecompositionConstPtr& body_decomposition)
383 : body_decomposition_(body_decomposition)
393 posed_bounding_sphere_center_ = trans * body_decomposition_->getRelativeBoundingSphere().center;
394 for (
unsigned int i = 0; i < body_decomposition_->getCollisionSpheres().size(); i++)
396 sphere_centers_[i] = trans * body_decomposition_->getCollisionSpheres()[i].relative_vec_;
400 if (!body_decomposition_->getCollisionPoints().empty())
402 posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
403 for (
unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); i++)
405 posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
411 const PosedBodySphereDecompositionConstPtr& p2)
415 double p1_radius = p1->getBoundingSphereRadius();
416 double p2_radius = p2->getBoundingSphereRadius();
418 double dist = (p1_sphere_center - p2_sphere_center).squaredNorm();
419 return dist < (p1_radius + p2_radius);
423 const std_msgs::ColorRGBA& color,
const std::string& frame_id,
const std::string& ns,
const ros::Duration& dur,
424 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions, visualization_msgs::MarkerArray& arr)
426 unsigned int count = 0;
427 for (
const PosedBodySphereDecompositionPtr& posed_decomposition : posed_decompositions)
429 if (posed_decomposition)
431 for (
unsigned int j = 0; j < posed_decomposition->getCollisionSpheres().size(); j++)
433 visualization_msgs::Marker sphere;
434 sphere.type = visualization_msgs::Marker::SPHERE;
436 sphere.header.frame_id = frame_id;
439 sphere.lifetime = dur;
440 sphere.color = color;
441 sphere.scale.x = sphere.scale.y = sphere.scale.z = posed_decomposition->getCollisionSpheres()[j].radius_ * 2.0;
442 sphere.pose.position.x = posed_decomposition->getSphereCenters()[j].x();
443 sphere.pose.position.y = posed_decomposition->getSphereCenters()[j].y();
444 sphere.pose.position.z = posed_decomposition->getSphereCenters()[j].z();
445 arr.markers.push_back(sphere);
452 const std::string& frame_id,
const std::string& ns,
const ros::Duration& ,
453 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
454 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
455 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
457 if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
459 ROS_WARN_NAMED(
"collision_distance_field",
"Size mismatch between gradients %u and decompositions %u",
460 (
unsigned int)gradients.size(),
461 (
unsigned int)(posed_decompositions.size() + posed_vector_decompositions.size()));
464 for (
unsigned int i = 0; i < gradients.size(); i++)
466 for (
unsigned int j = 0; j < gradients[i].distances.size(); j++)
468 visualization_msgs::Marker arrow_mark;
469 arrow_mark.header.frame_id = frame_id;
473 arrow_mark.ns =
"self_coll_gradients";
479 arrow_mark.id = i * 1000 + j;
483 if (gradients[i].distances[j] > 0.0 && gradients[i].distances[j] != DBL_MAX)
485 if (gradients[i].gradients[j].norm() > 0.0)
487 xscale = gradients[i].gradients[j].x() / gradients[i].gradients[j].norm();
488 yscale = gradients[i].gradients[j].y() / gradients[i].gradients[j].norm();
489 zscale = gradients[i].gradients[j].z() / gradients[i].gradients[j].norm();
493 ROS_DEBUG_NAMED(
"collision_distance_field",
"Negative length for %u %d %lf", i, arrow_mark.id,
494 gradients[i].gradients[j].norm());
499 ROS_DEBUG_NAMED(
"collision_distance_field",
"Negative dist %lf for %u %d", gradients[i].distances[j], i,
502 arrow_mark.points.resize(2);
503 if (i < posed_decompositions.size())
505 arrow_mark.points[1].x = posed_decompositions[i]->getSphereCenters()[j].x();
506 arrow_mark.points[1].y = posed_decompositions[i]->getSphereCenters()[j].y();
507 arrow_mark.points[1].z = posed_decompositions[i]->getSphereCenters()[j].z();
511 arrow_mark.points[1].x =
512 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
513 arrow_mark.points[1].y =
514 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
515 arrow_mark.points[1].z =
516 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
518 arrow_mark.points[0] = arrow_mark.points[1];
519 arrow_mark.points[0].x -= xscale * gradients[i].distances[j];
520 arrow_mark.points[0].y -= yscale * gradients[i].distances[j];
521 arrow_mark.points[0].z -= zscale * gradients[i].distances[j];
522 arrow_mark.scale.x = 0.01;
523 arrow_mark.scale.y = 0.03;
524 arrow_mark.color.a = 1.0;
527 arrow_mark.color.r = 1.0;
528 arrow_mark.color.g = 0.2;
529 arrow_mark.color.b = .5;
533 arrow_mark.color.r = .2;
534 arrow_mark.color.g = 1.0;
535 arrow_mark.color.b = .5;
539 arrow_mark.color.r = .2;
540 arrow_mark.color.g = .5;
541 arrow_mark.color.b = 1.0;
545 arrow_mark.color.r = 1.0;
546 arrow_mark.color.g = .2;
547 arrow_mark.color.b = 1.0;
549 arr.markers.push_back(arrow_mark);
555 const std::string& frame_id,
const std::string& ns,
const ros::Duration& ,
556 const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
557 const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
558 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
560 if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
562 ROS_WARN_NAMED(
"collision_distance_field",
"Size mismatch between gradients %zu and decompositions %zu",
563 gradients.size(), posed_decompositions.size() + posed_vector_decompositions.size());
566 for (
unsigned int i = 0; i < gradients.size(); i++)
568 for (
unsigned int j = 0; j < gradients[i].types.size(); j++)
570 visualization_msgs::Marker sphere_mark;
571 sphere_mark.type = visualization_msgs::Marker::SPHERE;
572 sphere_mark.header.frame_id = frame_id;
576 sphere_mark.ns =
"distance_collisions";
582 sphere_mark.id = i * 1000 + j;
583 if (i < posed_decompositions.size())
585 sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
586 posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
587 sphere_mark.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
588 sphere_mark.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
589 sphere_mark.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
593 sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
594 posed_vector_decompositions[i - posed_decompositions.size()]->getCollisionSpheres()[j].radius_ * 2.0;
595 sphere_mark.pose.position.x =
596 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
597 sphere_mark.pose.position.y =
598 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
599 sphere_mark.pose.position.z =
600 posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
602 sphere_mark.pose.orientation.w = 1.0;
603 sphere_mark.color.a = 1.0;
606 sphere_mark.color.r = 1.0;
607 sphere_mark.color.g = 0.2;
608 sphere_mark.color.b = .5;
612 sphere_mark.color.r = .2;
613 sphere_mark.color.g = 1.0;
614 sphere_mark.color.b = .5;
618 sphere_mark.color.r = .2;
619 sphere_mark.color.g = .5;
620 sphere_mark.color.b = 1.0;
624 sphere_mark.color.r = 1.0;
625 sphere_mark.color.g = .2;
626 sphere_mark.color.b = 1.0;
628 arr.markers.push_back(sphere_mark);