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]->clone(), 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)
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 {
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 (unsigned int i = 0; i < posed_decompositions.size(); i++)
414  {
415  if (posed_decompositions[i])
416  {
417  for (unsigned int j = 0; j < posed_decompositions[i]->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 =
428  posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
429  sphere.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
430  sphere.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
431  sphere.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
432  arr.markers.push_back(sphere);
433  }
434  }
435  }
436 }
437 
439  const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
440  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
441  const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
442  const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
443 {
444  if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
445  {
446  ROS_WARN_NAMED("collision_distance_field", "Size mismatch between gradients %u and decompositions %u",
447  (unsigned int)gradients.size(),
448  (unsigned int)(posed_decompositions.size() + posed_vector_decompositions.size()));
449  return;
450  }
451  for (unsigned int i = 0; i < gradients.size(); i++)
452  {
453  for (unsigned int j = 0; j < gradients[i].distances.size(); j++)
454  {
455  visualization_msgs::Marker arrow_mark;
456  arrow_mark.header.frame_id = frame_id;
457  arrow_mark.header.stamp = ros::Time::now();
458  if (ns.empty())
459  {
460  arrow_mark.ns = "self_coll_gradients";
461  }
462  else
463  {
464  arrow_mark.ns = ns;
465  }
466  arrow_mark.id = i * 1000 + j;
467  double xscale = 0.0;
468  double yscale = 0.0;
469  double zscale = 0.0;
470  if (gradients[i].distances[j] > 0.0 && gradients[i].distances[j] != DBL_MAX)
471  {
472  if (gradients[i].gradients[j].norm() > 0.0)
473  {
474  xscale = gradients[i].gradients[j].x() / gradients[i].gradients[j].norm();
475  yscale = gradients[i].gradients[j].y() / gradients[i].gradients[j].norm();
476  zscale = gradients[i].gradients[j].z() / gradients[i].gradients[j].norm();
477  }
478  else
479  {
480  ROS_DEBUG_NAMED("collision_distance_field", "Negative length for %u %d %lf", i, arrow_mark.id,
481  gradients[i].gradients[j].norm());
482  }
483  }
484  else
485  {
486  ROS_DEBUG_NAMED("collision_distance_field", "Negative dist %lf for %u %d", gradients[i].distances[j], i,
487  arrow_mark.id);
488  }
489  arrow_mark.points.resize(2);
490  if (i < posed_decompositions.size())
491  {
492  arrow_mark.points[1].x = posed_decompositions[i]->getSphereCenters()[j].x();
493  arrow_mark.points[1].y = posed_decompositions[i]->getSphereCenters()[j].y();
494  arrow_mark.points[1].z = posed_decompositions[i]->getSphereCenters()[j].z();
495  }
496  else
497  {
498  arrow_mark.points[1].x =
499  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
500  arrow_mark.points[1].y =
501  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
502  arrow_mark.points[1].z =
503  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
504  }
505  arrow_mark.points[0] = arrow_mark.points[1];
506  arrow_mark.points[0].x -= xscale * gradients[i].distances[j];
507  arrow_mark.points[0].y -= yscale * gradients[i].distances[j];
508  arrow_mark.points[0].z -= zscale * gradients[i].distances[j];
509  arrow_mark.scale.x = 0.01;
510  arrow_mark.scale.y = 0.03;
511  arrow_mark.color.a = 1.0;
512  if (gradients[i].types[j] == collision_detection::SELF)
513  {
514  arrow_mark.color.r = 1.0;
515  arrow_mark.color.g = 0.2;
516  arrow_mark.color.b = .5;
517  }
518  else if (gradients[i].types[j] == collision_detection::INTRA)
519  {
520  arrow_mark.color.r = .2;
521  arrow_mark.color.g = 1.0;
522  arrow_mark.color.b = .5;
523  }
524  else if (gradients[i].types[j] == collision_detection::ENVIRONMENT)
525  {
526  arrow_mark.color.r = .2;
527  arrow_mark.color.g = .5;
528  arrow_mark.color.b = 1.0;
529  }
530  else if (gradients[i].types[j] == collision_detection::NONE)
531  {
532  arrow_mark.color.r = 1.0;
533  arrow_mark.color.g = .2;
534  arrow_mark.color.b = 1.0;
535  }
536  arr.markers.push_back(arrow_mark);
537  }
538  }
539 }
540 
542  const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
543  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
544  const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
545  const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr)
546 {
547  if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
548  {
549  ROS_WARN_NAMED("collision_distance_field", "Size mismatch between gradients %zu and decompositions %zu",
550  gradients.size(), posed_decompositions.size() + posed_vector_decompositions.size());
551  return;
552  }
553  for (unsigned int i = 0; i < gradients.size(); i++)
554  {
555  for (unsigned int j = 0; j < gradients[i].types.size(); j++)
556  {
557  visualization_msgs::Marker sphere_mark;
558  sphere_mark.type = visualization_msgs::Marker::SPHERE;
559  sphere_mark.header.frame_id = frame_id;
560  sphere_mark.header.stamp = ros::Time::now();
561  if (ns.empty())
562  {
563  sphere_mark.ns = "distance_collisions";
564  }
565  else
566  {
567  sphere_mark.ns = ns;
568  }
569  sphere_mark.id = i * 1000 + j;
570  if (i < posed_decompositions.size())
571  {
572  sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
573  posed_decompositions[i]->getCollisionSpheres()[j].radius_ * 2.0;
574  sphere_mark.pose.position.x = posed_decompositions[i]->getSphereCenters()[j].x();
575  sphere_mark.pose.position.y = posed_decompositions[i]->getSphereCenters()[j].y();
576  sphere_mark.pose.position.z = posed_decompositions[i]->getSphereCenters()[j].z();
577  }
578  else
579  {
580  sphere_mark.scale.x = sphere_mark.scale.y = sphere_mark.scale.z =
581  posed_vector_decompositions[i - posed_decompositions.size()]->getCollisionSpheres()[j].radius_ * 2.0;
582  sphere_mark.pose.position.x =
583  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].x();
584  sphere_mark.pose.position.y =
585  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].y();
586  sphere_mark.pose.position.z =
587  posed_vector_decompositions[i - posed_decompositions.size()]->getSphereCenters()[j].z();
588  }
589  sphere_mark.pose.orientation.w = 1.0;
590  sphere_mark.color.a = 1.0;
591  if (gradients[i].types[j] == collision_detection::SELF)
592  {
593  sphere_mark.color.r = 1.0;
594  sphere_mark.color.g = 0.2;
595  sphere_mark.color.b = .5;
596  }
597  else if (gradients[i].types[j] == collision_detection::INTRA)
598  {
599  sphere_mark.color.r = .2;
600  sphere_mark.color.g = 1.0;
601  sphere_mark.color.b = .5;
602  }
603  else if (gradients[i].types[j] == collision_detection::ENVIRONMENT)
604  {
605  sphere_mark.color.r = .2;
606  sphere_mark.color.g = .5;
607  sphere_mark.color.b = 1.0;
608  }
609  else
610  {
611  sphere_mark.color.r = 1.0;
612  sphere_mark.color.g = .2;
613  sphere_mark.color.b = 1.0;
614  }
615  arr.markers.push_back(sphere_mark);
616  }
617  }
618 }
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)
void updatePose(const Eigen::Isometry3d &linkTransform)
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const=0
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecomposition(const BodyDecompositionConstPtr &body_decomposition)
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
#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)
#define ROS_DEBUG(...)
#define ROS_DEBUG_NAMED(name,...)
std::vector< CollisionSphere > determineCollisionSpheres(const bodies::Body *body, Eigen::Isometry3d &relativeTransform)
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
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 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)
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...
void updatePose(const Eigen::Isometry3d &linkTransform)
#define ROS_DEBUG_STREAM(args)
Namespace for holding classes that generate distance fields.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecomposition(const BodyDecompositionConstPtr &body_decomposition)
void init(const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &poses, double resolution, double padding)
Eigen::Isometry3d pose
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 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...
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
const Eigen::Isometry3d & getPose() const
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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Nov 11 2019 04:01:08