collision_distance_field_types.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: E. Gil Jones */
36 
41 #include <ros/console.h>
42 #include <memory>
43 
44 const static double RESOLUTION_SCALE = 1.0;
45 const static double EPSILON = 0.0001;
46 
47 std::vector<collision_detection::CollisionSphere>
48 collision_detection::determineCollisionSpheres(const bodies::Body* body, Eigen::Affine3d& relative_transform)
49 {
50  std::vector<collision_detection::CollisionSphere> css;
51 
53  body->computeBoundingCylinder(cyl);
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;
57 
58  for (unsigned int i = 1; i < num_points - 1; i++)
59  {
61  relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius);
62  css.push_back(cs);
63  }
64 
65  return css;
66 }
67 
69  const std::vector<CollisionSphere>& sphere_list, const EigenSTL::vector_Vector3d& sphere_centers,
70  GradientInfo& gradient, const collision_detection::CollisionType& type, double tolerance, bool subtract_radii,
71  double maximum_value, bool stop_at_first_collision)
72 {
73  // assumes gradient is properly initialized
74 
75  bool in_collision = false;
76  for (unsigned int i = 0; i < sphere_list.size(); i++)
77  {
78  Eigen::Vector3d p = sphere_centers[i];
79  Eigen::Vector3d grad(0, 0, 0);
80  bool in_bounds;
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)
83  {
84  // out of bounds
85  return true;
86  }
87 
88  if (dist < maximum_value)
89  {
90  if (subtract_radii)
91  {
92  dist -= sphere_list[i].radius_;
93 
94  if ((dist < 0) && (-dist >= tolerance))
95  {
96  in_collision = true;
97  }
98 
99  dist = std::abs(dist);
100  }
101  else
102  {
103  if (sphere_list[i].radius_ - dist > tolerance)
104  {
105  in_collision = true;
106  }
107  }
108 
109  if (dist < gradient.closest_distance)
110  {
111  gradient.closest_distance = dist;
112  }
113 
114  if (dist < gradient.distances[i])
115  {
116  gradient.types[i] = type;
117  gradient.distances[i] = dist;
118  gradient.gradients[i] = grad;
119  }
120  }
121 
122  if (stop_at_first_collision && in_collision)
123  {
124  return true;
125  }
126  }
127  return in_collision;
128 }
129 
131  const std::vector<CollisionSphere>& sphere_list,
132  const EigenSTL::vector_Vector3d& sphere_centers,
133  GradientInfo& gradient,
134  const collision_detection::CollisionType& type, double tolerance,
135  bool subtract_radii, double maximum_value,
136  bool stop_at_first_collision)
137 {
138  // assumes gradient is properly initialized
139 
140  bool in_collision = false;
141  for (unsigned int i = 0; i < sphere_list.size(); i++)
142  {
143  Eigen::Vector3d p = sphere_centers[i];
144  Eigen::Vector3d grad;
145  bool in_bounds;
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)
148  {
149  ROS_DEBUG("Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
150  return true;
151  }
152 
153  if (dist < maximum_value)
154  {
155  if (subtract_radii)
156  {
157  dist -= sphere_list[i].radius_;
158 
159  if ((dist < 0) && (-dist >= tolerance))
160  {
161  in_collision = true;
162  }
163  }
164  else
165  {
166  if (sphere_list[i].radius_ - dist > tolerance)
167  {
168  in_collision = true;
169  }
170  }
171 
172  if (dist < gradient.closest_distance)
173  {
174  gradient.closest_distance = dist;
175  }
176 
177  if (dist < gradient.distances[i])
178  {
179  gradient.types[i] = type;
180  gradient.distances[i] = dist;
181  gradient.gradients[i] = grad;
182  }
183  }
184 
185  if (stop_at_first_collision && in_collision)
186  {
187  return true;
188  }
189  }
190  return in_collision;
191 }
192 
194  const std::vector<CollisionSphere>& sphere_list,
195  const EigenSTL::vector_Vector3d& sphere_centers,
196  double maximum_value, double tolerance)
197 {
198  for (unsigned int i = 0; i < sphere_list.size(); i++)
199  {
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);
204 
205  if (!in_bounds && grad.norm() > 0)
206  {
207  ROS_DEBUG("Collision sphere point is out of bounds");
208  return true;
209  }
210 
211  if ((maximum_value > dist) && (sphere_list[i].radius_ - dist > tolerance))
212  {
213  return true;
214  }
215  }
216 
217  return false;
218 }
219 
221  const std::vector<CollisionSphere>& sphere_list,
222  const EigenSTL::vector_Vector3d& sphere_centers,
223  double maximum_value, double tolerance, unsigned int num_coll,
224  std::vector<unsigned int>& colls)
225 {
226  colls.clear();
227  for (unsigned int i = 0; i < sphere_list.size(); i++)
228  {
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))
234  {
235  ROS_DEBUG("Collision sphere point is out of bounds");
236  return true;
237  }
238  if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance))
239  {
240  if (num_coll == 0)
241  {
242  return true;
243  }
244 
245  colls.push_back(i);
246  if (colls.size() >= num_coll)
247  {
248  return true;
249  }
250  }
251  }
252 
253  return colls.size() > 0;
254 }
255 
259 
261  double padding)
262 {
263  std::vector<shapes::ShapeConstPtr> shapes;
264  EigenSTL::vector_Affine3d poses(1, Eigen::Affine3d::Identity());
265 
266  shapes.push_back(shape);
267  init(shapes, poses, resolution, padding);
268 }
269 
270 collision_detection::BodyDecomposition::BodyDecomposition(const std::vector<shapes::ShapeConstPtr>& shapes,
271  const EigenSTL::vector_Affine3d& poses, double resolution,
272  double padding)
273 {
274  init(shapes, poses, resolution, padding);
275 }
276 
277 void collision_detection::BodyDecomposition::init(const std::vector<shapes::ShapeConstPtr>& shapes,
278  const EigenSTL::vector_Affine3d& poses, double resolution,
279  double padding)
280 {
281  bodies_.clear();
282  for (unsigned int i = 0; i < shapes.size(); i++)
283  {
284  bodies_.addBody(shapes[i]->clone(), poses[i], padding);
285  }
286 
287  // collecting collision spheres
288  collision_spheres_.clear();
289  relative_collision_points_.clear();
290  std::vector<CollisionSphere> body_spheres;
291  EigenSTL::vector_Vector3d body_collision_points;
292  for (unsigned int i = 0; i < bodies_.getCount(); i++)
293  {
294  body_spheres.clear();
295  body_collision_points.clear();
296 
297  body_spheres = determineCollisionSpheres(bodies_.getBody(i), relative_cylinder_pose_);
298  collision_spheres_.insert(collision_spheres_.end(), body_spheres.begin(), body_spheres.end());
299 
300  distance_field::findInternalPointsConvex(*bodies_.getBody(i), resolution, body_collision_points);
301  relative_collision_points_.insert(relative_collision_points_.end(), body_collision_points.begin(),
302  body_collision_points.end());
303  }
304 
305  sphere_radii_.resize(collision_spheres_.size());
306  for (unsigned int i = 0; i < collision_spheres_.size(); i++)
307  {
308  sphere_radii_[i] = collision_spheres_[i].radius_;
309  }
310 
311  // computing bounding sphere
312  std::vector<bodies::BoundingSphere> bounding_spheres(bodies_.getCount());
313  for (unsigned int i = 0; i < bodies_.getCount(); i++)
314  {
315  bodies_.getBody(i)->computeBoundingSphere(bounding_spheres[i]);
316  }
317  bodies::mergeBoundingSpheres(bounding_spheres, relative_bounding_sphere_);
318 
319  ROS_DEBUG_STREAM("BodyDecomposition generated " << collision_spheres_.size() << " collision spheres out of "
320  << shapes.size() << " shapes");
321 }
322 
324 {
325  bodies_.clear();
326 }
327 
329  const BodyDecompositionConstPtr& body_decomposition)
330  : body_decomposition_(body_decomposition)
331 {
332  posed_collision_points_ = body_decomposition_->getCollisionPoints();
333 }
334 
336  const BodyDecompositionConstPtr& body_decomposition, const Eigen::Affine3d& trans)
337  : body_decomposition_(body_decomposition)
338 {
339  updatePose(trans);
340 }
341 
343  std::shared_ptr<const octomap::OcTree> octree)
345 {
346  int num_nodes = octree->getNumLeafNodes();
347  posed_collision_points_.reserve(num_nodes);
348  for (octomap::OcTree::tree_iterator tree_iter = octree->begin_tree(); tree_iter != octree->end_tree(); ++tree_iter)
349  {
350  Eigen::Vector3d p = Eigen::Vector3d(tree_iter.getX(), tree_iter.getY(), tree_iter.getZ());
351  posed_collision_points_.push_back(p);
352  }
353 }
354 
356 {
358  {
359  posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
360 
361  for (unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); i++)
362  {
363  posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
364  }
365  }
366 }
367 
369  const BodyDecompositionConstPtr& body_decomposition)
370  : body_decomposition_(body_decomposition)
371 {
372  posed_bounding_sphere_center_ = body_decomposition_->getRelativeBoundingSphere().center;
373  sphere_centers_.resize(body_decomposition_->getCollisionSpheres().size());
374  updatePose(Eigen::Affine3d::Identity());
375 }
376 
378 {
379  // updating sphere centers
380  posed_bounding_sphere_center_ = trans * body_decomposition_->getRelativeBoundingSphere().center;
381  for (unsigned int i = 0; i < body_decomposition_->getCollisionSpheres().size(); i++)
382  {
383  sphere_centers_[i] = trans * body_decomposition_->getCollisionSpheres()[i].relative_vec_;
384  }
385 
386  // updating collision points
387  if (!body_decomposition_->getCollisionPoints().empty())
388  {
389  posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
390  for (unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); i++)
391  {
392  posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
393  }
394  }
395 }
396 
397 bool collision_detection::doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1,
398  const PosedBodySphereDecompositionConstPtr& p2)
399 {
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();
404 
405  double dist = (p1_sphere_center - p2_sphere_center).squaredNorm();
406  if (dist < (p1_radius + p2_radius))
407  {
408  return true;
409  }
410  return false;
411 }
412 
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)
416 {
417  unsigned int count = 0;
418  for (unsigned int i = 0; i < posed_decompositions.size(); i++)
419  {
420  if (posed_decompositions[i])
421  {
422  for (unsigned int j = 0; j < posed_decompositions[i]->getCollisionSpheres().size(); j++)
423  {
424  visualization_msgs::Marker sphere;
425  sphere.type = visualization_msgs::Marker::SPHERE;
426  sphere.header.stamp = ros::Time::now();
427  sphere.header.frame_id = frame_id;
428  sphere.ns = ns;
429  sphere.id = count++;
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);
438  }
439  }
440  }
441 }
442 
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)
448 {
449  if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
450  {
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()));
454  return;
455  }
456  for (unsigned int i = 0; i < gradients.size(); i++)
457  {
458  for (unsigned int j = 0; j < gradients[i].distances.size(); j++)
459  {
460  visualization_msgs::Marker arrow_mark;
461  arrow_mark.header.frame_id = frame_id;
462  arrow_mark.header.stamp = ros::Time::now();
463  if (ns.empty())
464  {
465  arrow_mark.ns = "self_coll_gradients";
466  }
467  else
468  {
469  arrow_mark.ns = ns;
470  }
471  arrow_mark.id = i * 1000 + j;
472  double xscale = 0.0;
473  double yscale = 0.0;
474  double zscale = 0.0;
475  if (gradients[i].distances[j] > 0.0 && gradients[i].distances[j] != DBL_MAX)
476  {
477  if (gradients[i].gradients[j].norm() > 0.0)
478  {
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();
482  }
483  else
484  {
485  ROS_DEBUG_NAMED("collision_distance_field", "Negative length for %u %d %lf", i, arrow_mark.id,
486  gradients[i].gradients[j].norm());
487  }
488  }
489  else
490  {
491  ROS_DEBUG_NAMED("collision_distance_field", "Negative dist %lf for %u %d", gradients[i].distances[j], i,
492  arrow_mark.id);
493  }
494  arrow_mark.points.resize(2);
495  if (i < posed_decompositions.size())
496  {
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();
500  }
501  else
502  {
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();
509  }
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;
517  if (gradients[i].types[j] == collision_detection::SELF)
518  {
519  arrow_mark.color.r = 1.0;
520  arrow_mark.color.g = 0.2;
521  arrow_mark.color.b = .5;
522  }
523  else if (gradients[i].types[j] == collision_detection::INTRA)
524  {
525  arrow_mark.color.r = .2;
526  arrow_mark.color.g = 1.0;
527  arrow_mark.color.b = .5;
528  }
529  else if (gradients[i].types[j] == collision_detection::ENVIRONMENT)
530  {
531  arrow_mark.color.r = .2;
532  arrow_mark.color.g = .5;
533  arrow_mark.color.b = 1.0;
534  }
535  else if (gradients[i].types[j] == collision_detection::NONE)
536  {
537  arrow_mark.color.r = 1.0;
538  arrow_mark.color.g = .2;
539  arrow_mark.color.b = 1.0;
540  }
541  arr.markers.push_back(arrow_mark);
542  }
543  }
544 }
545 
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)
551 {
552  if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
553  {
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());
556  return;
557  }
558  for (unsigned int i = 0; i < gradients.size(); i++)
559  {
560  for (unsigned int j = 0; j < gradients[i].types.size(); j++)
561  {
562  visualization_msgs::Marker sphere_mark;
563  sphere_mark.type = visualization_msgs::Marker::SPHERE;
564  sphere_mark.header.frame_id = frame_id;
565  sphere_mark.header.stamp = ros::Time::now();
566  if (ns.empty())
567  {
568  sphere_mark.ns = "distance_collisions";
569  }
570  else
571  {
572  sphere_mark.ns = ns;
573  }
574  sphere_mark.id = i * 1000 + j;
575  if (i < posed_decompositions.size())
576  {
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();
582  }
583  else
584  {
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();
593  }
594  sphere_mark.pose.orientation.w = 1.0;
595  sphere_mark.color.a = 1.0;
596  if (gradients[i].types[j] == collision_detection::SELF)
597  {
598  sphere_mark.color.r = 1.0;
599  sphere_mark.color.g = 0.2;
600  sphere_mark.color.b = .5;
601  }
602  else if (gradients[i].types[j] == collision_detection::INTRA)
603  {
604  sphere_mark.color.r = .2;
605  sphere_mark.color.g = 1.0;
606  sphere_mark.color.b = .5;
607  }
608  else if (gradients[i].types[j] == collision_detection::ENVIRONMENT)
609  {
610  sphere_mark.color.r = .2;
611  sphere_mark.color.g = .5;
612  sphere_mark.color.b = 1.0;
613  }
614  else
615  {
616  sphere_mark.color.r = 1.0;
617  sphere_mark.color.g = .2;
618  sphere_mark.color.b = 1.0;
619  }
620  arr.markers.push_back(sphere_mark);
621  }
622  }
623 }
const Eigen::Affine3d & getPose() const
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)
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...
void init(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &poses, double resolution, double padding)
Eigen::Affine3d pose
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...
#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
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)
#define ROS_DEBUG_STREAM(args)
Namespace for holding classes that generate distance fields.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecomposition(const 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
static Time now()
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)
#define ROS_DEBUG(...)
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Aug 12 2019 03:54:06