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 EPSILON = 0.0001;
45 
46 std::vector<collision_detection::CollisionSphere>
47 collision_detection::determineCollisionSpheres(const bodies::Body* body, Eigen::Isometry3d& relative_transform)
48 {
49  std::vector<collision_detection::CollisionSphere> css;
50 
52  body->computeBoundingCylinder(cyl);
53  unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0));
54  double spacing = cyl.length / ((num_points * 1.0) - 1.0);
55  relative_transform = body->getPose().inverse() * cyl.pose;
56 
57  for (unsigned int i = 1; i < num_points - 1; i++)
58  {
60  relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius);
61  css.push_back(cs);
62  }
63 
64  return css;
65 }
66 
68  const std::vector<CollisionSphere>& sphere_list, const EigenSTL::vector_Vector3d& sphere_centers,
69  GradientInfo& gradient, const collision_detection::CollisionType& type, double tolerance, bool subtract_radii,
70  double maximum_value, bool stop_at_first_collision)
71 {
72  // assumes gradient is properly initialized
73 
74  bool in_collision = false;
75  for (unsigned int i = 0; i < sphere_list.size(); i++)
76  {
77  Eigen::Vector3d p = sphere_centers[i];
78  Eigen::Vector3d grad(0, 0, 0);
79  bool in_bounds;
80  double dist = this->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
81  if (!in_bounds && grad.norm() > 0)
82  {
83  // out of bounds
84  return true;
85  }
86 
87  if (dist < maximum_value)
88  {
89  if (subtract_radii)
90  {
91  dist -= sphere_list[i].radius_;
92 
93  if ((dist < 0) && (-dist >= tolerance))
94  {
95  in_collision = true;
96  }
97 
98  dist = std::abs(dist);
99  }
100  else
101  {
102  if (sphere_list[i].radius_ - dist > tolerance)
103  {
104  in_collision = true;
105  }
106  }
107 
108  if (dist < gradient.closest_distance)
109  {
110  gradient.closest_distance = dist;
111  }
112 
113  if (dist < gradient.distances[i])
114  {
115  gradient.types[i] = type;
116  gradient.distances[i] = dist;
117  gradient.gradients[i] = grad;
118  }
119  }
120 
121  if (stop_at_first_collision && in_collision)
122  {
123  return true;
124  }
125  }
126  return in_collision;
127 }
128 
130  const std::vector<CollisionSphere>& sphere_list,
131  const EigenSTL::vector_Vector3d& sphere_centers,
132  GradientInfo& gradient,
133  const collision_detection::CollisionType& type, double tolerance,
134  bool subtract_radii, double maximum_value,
135  bool stop_at_first_collision)
136 {
137  // assumes gradient is properly initialized
138 
139  bool in_collision = false;
140  for (unsigned int i = 0; i < sphere_list.size(); i++)
141  {
142  Eigen::Vector3d p = sphere_centers[i];
143  Eigen::Vector3d grad;
144  bool in_bounds;
145  double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
146  if (!in_bounds && grad.norm() > EPSILON)
147  {
148  ROS_DEBUG("Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
149  return true;
150  }
151 
152  if (dist < maximum_value)
153  {
154  if (subtract_radii)
155  {
156  dist -= sphere_list[i].radius_;
157 
158  if ((dist < 0) && (-dist >= tolerance))
159  {
160  in_collision = true;
161  }
162  }
163  else
164  {
165  if (sphere_list[i].radius_ - dist > tolerance)
166  {
167  in_collision = true;
168  }
169  }
170 
171  if (dist < gradient.closest_distance)
172  {
173  gradient.closest_distance = dist;
174  }
175 
176  if (dist < gradient.distances[i])
177  {
178  gradient.types[i] = type;
179  gradient.distances[i] = dist;
180  gradient.gradients[i] = grad;
181  }
182  }
183 
184  if (stop_at_first_collision && in_collision)
185  {
186  return true;
187  }
188  }
189  return in_collision;
190 }
191 
193  const std::vector<CollisionSphere>& sphere_list,
194  const EigenSTL::vector_Vector3d& sphere_centers,
195  double maximum_value, double tolerance)
196 {
197  for (unsigned int i = 0; i < sphere_list.size(); i++)
198  {
199  Eigen::Vector3d p = sphere_centers[i];
200  Eigen::Vector3d grad;
201  bool in_bounds = true;
202  double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
203 
204  if (!in_bounds && grad.norm() > 0)
205  {
206  ROS_DEBUG("Collision sphere point is out of bounds");
207  return true;
208  }
209 
210  if ((maximum_value > dist) && (sphere_list[i].radius_ - dist > tolerance))
211  {
212  return true;
213  }
214  }
215 
216  return false;
217 }
218 
220  const std::vector<CollisionSphere>& sphere_list,
221  const EigenSTL::vector_Vector3d& sphere_centers,
222  double maximum_value, double tolerance, unsigned int num_coll,
223  std::vector<unsigned int>& colls)
224 {
225  colls.clear();
226  for (unsigned int i = 0; i < sphere_list.size(); i++)
227  {
228  Eigen::Vector3d p = sphere_centers[i];
229  Eigen::Vector3d grad;
230  bool in_bounds = true;
231  double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
232  if (!in_bounds && (grad.norm() > 0))
233  {
234  ROS_DEBUG("Collision sphere point is out of bounds");
235  return true;
236  }
237  if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance))
238  {
239  if (num_coll == 0)
240  {
241  return true;
242  }
243 
244  colls.push_back(i);
245  if (colls.size() >= num_coll)
246  {
247  return true;
248  }
249  }
250  }
251 
252  return !colls.empty();
253 }
254 
258 
260  double padding)
261 {
262  std::vector<shapes::ShapeConstPtr> shapes;
263  EigenSTL::vector_Isometry3d poses(1, Eigen::Isometry3d::Identity());
264 
265  shapes.push_back(shape);
266  init(shapes, poses, resolution, padding);
267 }
268 
269 collision_detection::BodyDecomposition::BodyDecomposition(const std::vector<shapes::ShapeConstPtr>& shapes,
270  const EigenSTL::vector_Isometry3d& poses, double resolution,
271  double padding)
272 {
273  init(shapes, poses, resolution, padding);
274 }
275 
276 void collision_detection::BodyDecomposition::init(const std::vector<shapes::ShapeConstPtr>& shapes,
277  const EigenSTL::vector_Isometry3d& poses, double resolution,
278  double padding)
279 {
280  bodies_.clear();
281  for (unsigned int i = 0; i < shapes.size(); i++)
282  {
283  bodies_.addBody(shapes[i].get(), poses[i], padding);
284  }
285 
286  // collecting collision spheres
287  collision_spheres_.clear();
288  relative_collision_points_.clear();
289  std::vector<CollisionSphere> body_spheres;
290  EigenSTL::vector_Vector3d body_collision_points;
291  for (unsigned int i = 0; i < bodies_.getCount(); i++)
292  {
293  body_spheres.clear();
294  body_collision_points.clear();
295 
296  body_spheres = determineCollisionSpheres(bodies_.getBody(i), relative_cylinder_pose_);
297  collision_spheres_.insert(collision_spheres_.end(), body_spheres.begin(), body_spheres.end());
298 
299  distance_field::findInternalPointsConvex(*bodies_.getBody(i), resolution, body_collision_points);
300  relative_collision_points_.insert(relative_collision_points_.end(), body_collision_points.begin(),
301  body_collision_points.end());
302  }
303 
304  sphere_radii_.resize(collision_spheres_.size());
305  for (unsigned int i = 0; i < collision_spheres_.size(); i++)
306  {
307  sphere_radii_[i] = collision_spheres_[i].radius_;
308  }
309 
310  // computing bounding sphere
311  std::vector<bodies::BoundingSphere> bounding_spheres(bodies_.getCount());
312  for (unsigned int i = 0; i < bodies_.getCount(); i++)
313  {
314  bodies_.getBody(i)->computeBoundingSphere(bounding_spheres[i]);
315  }
316  bodies::mergeBoundingSpheres(bounding_spheres, relative_bounding_sphere_);
317 
318  ROS_DEBUG_STREAM("BodyDecomposition generated " << collision_spheres_.size() << " collision spheres out of "
319  << shapes.size() << " shapes");
320 }
321 
323 {
324  bodies_.clear();
325 }
326 
328  const BodyDecompositionConstPtr& body_decomposition)
329  : body_decomposition_(body_decomposition)
330 {
331  posed_collision_points_ = body_decomposition_->getCollisionPoints();
332 }
333 
335  const BodyDecompositionConstPtr& body_decomposition, const Eigen::Isometry3d& trans)
336  : body_decomposition_(body_decomposition)
337 {
338  updatePose(trans);
339 }
340 
342  const std::shared_ptr<const octomap::OcTree>& octree)
343  : body_decomposition_()
344 {
345  int num_nodes = octree->getNumLeafNodes();
346  posed_collision_points_.reserve(num_nodes);
347  for (octomap::OcTree::tree_iterator tree_iter = octree->begin_tree(); tree_iter != octree->end_tree(); ++tree_iter)
348  {
349  Eigen::Vector3d p = Eigen::Vector3d(tree_iter.getX(), tree_iter.getY(), tree_iter.getZ());
350  posed_collision_points_.push_back(p);
351  }
352 }
353 
355 {
356  if (body_decomposition_)
357  {
358  posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
359 
360  for (unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); i++)
361  {
362  posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
363  }
364  }
365 }
366 
368  const BodyDecompositionConstPtr& body_decomposition)
369  : body_decomposition_(body_decomposition)
370 {
371  posed_bounding_sphere_center_ = body_decomposition_->getRelativeBoundingSphere().center;
372  sphere_centers_.resize(body_decomposition_->getCollisionSpheres().size());
373  updatePose(Eigen::Isometry3d::Identity());
374 }
375 
377 {
378  // updating sphere centers
379  posed_bounding_sphere_center_ = trans * body_decomposition_->getRelativeBoundingSphere().center;
380  for (unsigned int i = 0; i < body_decomposition_->getCollisionSpheres().size(); i++)
381  {
382  sphere_centers_[i] = trans * body_decomposition_->getCollisionSpheres()[i].relative_vec_;
383  }
384 
385  // updating collision points
386  if (!body_decomposition_->getCollisionPoints().empty())
387  {
388  posed_collision_points_.resize(body_decomposition_->getCollisionPoints().size());
389  for (unsigned int i = 0; i < body_decomposition_->getCollisionPoints().size(); i++)
390  {
391  posed_collision_points_[i] = trans * body_decomposition_->getCollisionPoints()[i];
392  }
393  }
394 }
395 
396 bool collision_detection::doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1,
397  const PosedBodySphereDecompositionConstPtr& p2)
398 {
399  Eigen::Vector3d p1_sphere_center = p1->getBoundingSphereCenter();
400  Eigen::Vector3d p2_sphere_center = p2->getBoundingSphereCenter();
401  double p1_radius = p1->getBoundingSphereRadius();
402  double p2_radius = p2->getBoundingSphereRadius();
403 
404  double dist = (p1_sphere_center - p2_sphere_center).squaredNorm();
405  return dist < (p1_radius + p2_radius);
406 }
407 
409  const std_msgs::ColorRGBA& color, const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
410  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions, visualization_msgs::MarkerArray& arr)
411 {
412  unsigned int count = 0;
413  for (const PosedBodySphereDecompositionPtr& posed_decomposition : posed_decompositions)
414  {
415  if (posed_decomposition)
416  {
417  for (unsigned int j = 0; j < posed_decomposition->getCollisionSpheres().size(); j++)
418  {
419  visualization_msgs::Marker sphere;
420  sphere.type = visualization_msgs::Marker::SPHERE;
421  sphere.header.stamp = ros::Time::now();
422  sphere.header.frame_id = frame_id;
423  sphere.ns = ns;
424  sphere.id = count++;
425  sphere.lifetime = dur;
426  sphere.color = color;
427  sphere.scale.x = sphere.scale.y = sphere.scale.z = posed_decomposition->getCollisionSpheres()[j].radius_ * 2.0;
428  sphere.pose.position.x = posed_decomposition->getSphereCenters()[j].x();
429  sphere.pose.position.y = posed_decomposition->getSphereCenters()[j].y();
430  sphere.pose.position.z = posed_decomposition->getSphereCenters()[j].z();
431  arr.markers.push_back(sphere);
432  }
433  }
434  }
435 }
436 
438  const std::string& frame_id, const std::string& ns, const ros::Duration& /*dur*/,
439  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
440  const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
441  const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
442 {
443  if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
444  {
445  ROS_WARN_NAMED("collision_distance_field", "Size mismatch between gradients %u and decompositions %u",
446  (unsigned int)gradients.size(),
447  (unsigned int)(posed_decompositions.size() + posed_vector_decompositions.size()));
448  return;
449  }
450  for (unsigned int i = 0; i < gradients.size(); i++)
451  {
452  for (unsigned int j = 0; j < gradients[i].distances.size(); j++)
453  {
454  visualization_msgs::Marker arrow_mark;
455  arrow_mark.header.frame_id = frame_id;
456  arrow_mark.header.stamp = ros::Time::now();
457  if (ns.empty())
458  {
459  arrow_mark.ns = "self_coll_gradients";
460  }
461  else
462  {
463  arrow_mark.ns = ns;
464  }
465  arrow_mark.id = i * 1000 + j;
466  double xscale = 0.0;
467  double yscale = 0.0;
468  double zscale = 0.0;
469  if (gradients[i].distances[j] > 0.0 && gradients[i].distances[j] != DBL_MAX)
470  {
471  if (gradients[i].gradients[j].norm() > 0.0)
472  {
473  xscale = gradients[i].gradients[j].x() / gradients[i].gradients[j].norm();
474  yscale = gradients[i].gradients[j].y() / gradients[i].gradients[j].norm();
475  zscale = gradients[i].gradients[j].z() / gradients[i].gradients[j].norm();
476  }
477  else
478  {
479  ROS_DEBUG_NAMED("collision_distance_field", "Negative length for %u %d %lf", i, arrow_mark.id,
480  gradients[i].gradients[j].norm());
481  }
482  }
483  else
484  {
485  ROS_DEBUG_NAMED("collision_distance_field", "Negative dist %lf for %u %d", gradients[i].distances[j], i,
486  arrow_mark.id);
487  }
488  arrow_mark.points.resize(2);
489  if (i < posed_decompositions.size())
490  {
491  arrow_mark.points[1].x = posed_decompositions[i]->getSphereCenters()[j].x();
492  arrow_mark.points[1].y = posed_decompositions[i]->getSphereCenters()[j].y();
493  arrow_mark.points[1].z = posed_decompositions[i]->getSphereCenters()[j].z();
494  }
495  else
496  {
497  arrow_mark.points[1].x =
498  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
499  arrow_mark.points[1].y =
500  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
501  arrow_mark.points[1].z =
502  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
503  }
504  arrow_mark.points[0] = arrow_mark.points[1];
505  arrow_mark.points[0].x -= xscale * gradients[i].distances[j];
506  arrow_mark.points[0].y -= yscale * gradients[i].distances[j];
507  arrow_mark.points[0].z -= zscale * gradients[i].distances[j];
508  arrow_mark.scale.x = 0.01;
509  arrow_mark.scale.y = 0.03;
510  arrow_mark.color.a = 1.0;
511  if (gradients[i].types[j] == collision_detection::SELF)
512  {
513  arrow_mark.color.r = 1.0;
514  arrow_mark.color.g = 0.2;
515  arrow_mark.color.b = .5;
516  }
517  else if (gradients[i].types[j] == collision_detection::INTRA)
518  {
519  arrow_mark.color.r = .2;
520  arrow_mark.color.g = 1.0;
521  arrow_mark.color.b = .5;
522  }
523  else if (gradients[i].types[j] == collision_detection::ENVIRONMENT)
524  {
525  arrow_mark.color.r = .2;
526  arrow_mark.color.g = .5;
527  arrow_mark.color.b = 1.0;
528  }
529  else if (gradients[i].types[j] == collision_detection::NONE)
530  {
531  arrow_mark.color.r = 1.0;
532  arrow_mark.color.g = .2;
533  arrow_mark.color.b = 1.0;
534  }
535  arr.markers.push_back(arrow_mark);
536  }
537  }
538 }
539 
541  const std::string& frame_id, const std::string& ns, const ros::Duration& /*dur*/,
542  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
543  const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
544  const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
545 {
546  if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
547  {
548  ROS_WARN_NAMED("collision_distance_field", "Size mismatch between gradients %zu and decompositions %zu",
549  gradients.size(), posed_decompositions.size() + posed_vector_decompositions.size());
550  return;
551  }
552  for (unsigned int i = 0; i < gradients.size(); i++)
553  {
554  for (unsigned int j = 0; j < gradients[i].types.size(); j++)
555  {
556  visualization_msgs::Marker sphere_mark;
557  sphere_mark.type = visualization_msgs::Marker::SPHERE;
558  sphere_mark.header.frame_id = frame_id;
559  sphere_mark.header.stamp = ros::Time::now();
560  if (ns.empty())
561  {
562  sphere_mark.ns = "distance_collisions";
563  }
564  else
565  {
566  sphere_mark.ns = ns;
567  }
568  sphere_mark.id = i * 1000 + j;
569  if (i < posed_decompositions.size())
570  {
571  sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
572  posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
573  sphere_mark.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
574  sphere_mark.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
575  sphere_mark.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
576  }
577  else
578  {
579  sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
580  posed_vector_decompositions[i - posed_decompositions.size()]->getCollisionSpheres()[j].radius_ * 2.0;
581  sphere_mark.pose.position.x =
582  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
583  sphere_mark.pose.position.y =
584  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
585  sphere_mark.pose.position.z =
586  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
587  }
588  sphere_mark.pose.orientation.w = 1.0;
589  sphere_mark.color.a = 1.0;
590  if (gradients[i].types[j] == collision_detection::SELF)
591  {
592  sphere_mark.color.r = 1.0;
593  sphere_mark.color.g = 0.2;
594  sphere_mark.color.b = .5;
595  }
596  else if (gradients[i].types[j] == collision_detection::INTRA)
597  {
598  sphere_mark.color.r = .2;
599  sphere_mark.color.g = 1.0;
600  sphere_mark.color.b = .5;
601  }
602  else if (gradients[i].types[j] == collision_detection::ENVIRONMENT)
603  {
604  sphere_mark.color.r = .2;
605  sphere_mark.color.g = .5;
606  sphere_mark.color.b = 1.0;
607  }
608  else
609  {
610  sphere_mark.color.r = 1.0;
611  sphere_mark.color.g = .2;
612  sphere_mark.color.b = 1.0;
613  }
614  arr.markers.push_back(sphere_mark);
615  }
616  }
617 }
collision_detection::PosedBodyPointDecomposition::body_decomposition_
BodyDecompositionConstPtr body_decomposition_
Definition: collision_distance_field_types.h:391
collision_detection::INTRA
@ INTRA
Definition: collision_distance_field_types.h:125
collision_detection::PosedDistanceField::getCollisionSphereGradients
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)
Definition: collision_distance_field_types.cpp:67
shapes
collision_detection::CollisionSphere
Definition: collision_distance_field_types.h:97
collision_detection::GradientInfo::types
std::vector< CollisionType > types
Definition: collision_distance_field_types.h:123
collision_detection::determineCollisionSpheres
std::vector< CollisionSphere > determineCollisionSpheres(const bodies::Body *body, Eigen::Isometry3d &relativeTransform)
Definition: collision_distance_field_types.cpp:47
collision_detection::GradientInfo::gradients
EigenSTL::vector_Vector3d gradients
Definition: collision_distance_field_types.h:122
get
ROSCPP_DECL bool get(const std::string &key, bool &b)
collision_distance_field_types.h
collision_detection::BodyDecomposition::~BodyDecomposition
~BodyDecomposition()
Definition: collision_distance_field_types.cpp:322
collision_detection::PosedDistanceField::getDistanceGradient
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...
Definition: collision_distance_field_types.h:188
ROS_DEBUG
#define ROS_DEBUG(...)
collision_detection::CollisionType
CollisionType
Definition: collision_distance_field_types.h:89
collision_detection::SELF
@ SELF
Definition: collision_distance_field_types.h:124
collision_detection::PosedBodySphereDecomposition::posed_bounding_sphere_center_
Eigen::Vector3d posed_bounding_sphere_center_
Definition: collision_distance_field_types.h:367
collision_detection::getProximityGradientMarkers
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)
Definition: collision_distance_field_types.cpp:437
bodies::Body::computeBoundingCylinder
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const=0
bodies::BoundingCylinder::radius
double radius
bodies::Body
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
collision_detection::doBoundingSpheresIntersect
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
Definition: collision_distance_field_types.cpp:396
collision_detection::PosedBodyPointDecomposition::PosedBodyPointDecomposition
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecomposition(const BodyDecompositionConstPtr &body_decomposition)
Definition: collision_distance_field_types.cpp:327
collision_detection::PosedBodyPointDecomposition::updatePose
void updatePose(const Eigen::Isometry3d &linkTransform)
Definition: collision_distance_field_types.cpp:354
collision_detection::PosedBodyPointDecomposition::posed_collision_points_
EigenSTL::vector_Vector3d posed_collision_points_
Definition: collision_distance_field_types.h:392
console.h
distance_field::DistanceField
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points....
Definition: distance_field.h:91
collision_detection::GradientInfo
Definition: collision_distance_field_types.h:110
collision_detection::getCollisionSphereMarkers
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)
Definition: collision_distance_field_types.cpp:408
collision_detection::getCollisionMarkers
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)
Definition: collision_distance_field_types.cpp:540
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
bodies::BoundingCylinder::length
double length
bodies::BoundingCylinder
collision_detection::getCollisionSphereGradients
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)
Definition: collision_distance_field_types.cpp:129
distance_field::findInternalPointsConvex
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....
Definition: find_internal_points.cpp:39
collision_detection::PosedBodySphereDecomposition::updatePose
void updatePose(const Eigen::Isometry3d &linkTransform)
Definition: collision_distance_field_types.cpp:376
collision_detection::NONE
@ NONE
Definition: collision_distance_field_types.h:123
bodies::BoundingCylinder::pose
Eigen::Isometry3d pose
bodies::Body::getPose
const Eigen::Isometry3d & getPose() const
body_operations.h
collision_detection::BodyDecomposition::init
void init(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &poses, double resolution, double padding)
Definition: collision_distance_field_types.cpp:276
collision_detection::PosedBodySphereDecomposition::PosedBodySphereDecomposition
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecomposition(const BodyDecompositionConstPtr &body_decomposition)
Definition: collision_distance_field_types.cpp:367
collision_detection::ENVIRONMENT
@ ENVIRONMENT
Definition: collision_distance_field_types.h:126
find_internal_points.h
distance_field.h
distance_field
Namespace for holding classes that generate distance fields.
Definition: distance_field.h:64
EPSILON
const static double EPSILON
Definition: collision_distance_field_types.cpp:44
collision_detection::PosedBodySphereDecomposition::body_decomposition_
BodyDecompositionConstPtr body_decomposition_
Definition: collision_distance_field_types.h:366
collision_detection::getCollisionSphereCollision
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)
Definition: collision_distance_field_types.cpp:192
collision_detection::BodyDecomposition::BodyDecomposition
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BodyDecomposition(const shapes::ShapeConstPtr &shape, double resolution, double padding=0.01)
Definition: collision_distance_field_types.cpp:259
collision_detection::PosedBodySphereDecomposition::sphere_centers_
EigenSTL::vector_Vector3d sphere_centers_
Definition: collision_distance_field_types.h:369
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
bodies::mergeBoundingSpheres
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
tolerance
S tolerance()
init
void init(const M_string &remappings)
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
ros::Duration
collision_detection::GradientInfo::closest_distance
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double closest_distance
Definition: collision_distance_field_types.h:118
collision_detection::GradientInfo::distances
std::vector< double > distances
Definition: collision_distance_field_types.h:121
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
ros::Time::now
static Time now()


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Nov 21 2021 03:25:56