collision_distance_field_types.h
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 
37 #pragma once
38 
39 #include <vector>
40 #include <string>
41 #include <algorithm>
42 #include <memory>
43 #include <float.h>
44 
47 #include <octomap/OcTree.h>
48 
52 #include <visualization_msgs/MarkerArray.h>
53 #include <ros/console.h>
54 
55 namespace collision_detection
56 {
57 enum CollisionType
58 {
59  NONE = 0,
60  SELF = 1,
61  INTRA = 2,
62  ENVIRONMENT = 3,
63 };
64 
65 struct CollisionSphere
66 {
67  CollisionSphere(const Eigen::Vector3d& rel, double radius)
68  {
69  relative_vec_ = rel;
70  radius_ = radius;
71  }
73 
75  double radius_;
76 };
77 
78 struct GradientInfo
79 {
80  GradientInfo() : closest_distance(DBL_MAX), collision(false)
81  {
82  }
83 
85 
86  double closest_distance;
87  bool collision;
88  EigenSTL::vector_Vector3d sphere_locations;
89  std::vector<double> distances;
90  EigenSTL::vector_Vector3d gradients;
91  std::vector<CollisionType> types;
92  std::vector<double> sphere_radii;
93  std::string joint_name;
94 
95  void clear()
96  {
97  closest_distance = DBL_MAX;
98  collision = false;
100  distances.clear();
101  gradients.clear();
102  sphere_radii.clear();
103  joint_name.clear();
104  }
105 };
106 
108 MOVEIT_CLASS_FORWARD(BodyDecomposition); // Defines BodyDecompositionPtr, ConstPtr, WeakPtr... etc
113 
115 {
116 public:
118 
119  PosedDistanceField(const Eigen::Vector3d& size, const Eigen::Vector3d& origin, double resolution, double max_distance,
120  bool propagate_negative_distances = false)
121  : distance_field::PropagationDistanceField(size.x(), size.y(), size.z(), resolution, origin.x(), origin.y(),
122  origin.z(), max_distance, propagate_negative_distances)
123  , pose_(Eigen::Isometry3d::Identity())
124  {
125  }
126 
127  void updatePose(const Eigen::Isometry3d& transform)
128  {
129  pose_ = transform;
130  }
131 
132  const Eigen::Isometry3d& getPose() const
133  {
134  return pose_;
135  }
136 
156  double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z,
157  bool& in_bounds) const
158  {
159  Eigen::Vector3d rel_pos = pose_.inverse() * Eigen::Vector3d(x, y, z);
160  double gx, gy, gz;
161  double res = distance_field::PropagationDistanceField::getDistanceGradient(rel_pos.x(), rel_pos.y(), rel_pos.z(),
162  gx, gy, gz, in_bounds);
163  Eigen::Vector3d grad = pose_ * Eigen::Vector3d(gx, gy, gz);
164  gradient_x = grad.x();
165  gradient_y = grad.y();
166  gradient_z = grad.z();
167  return res;
168  }
169 
170  /*
171  * @brief determines a set of gradients of the given collision spheres in the
172  * distance field
173  * @param sphere_list vector of the spheres that approximate a links geometry
174  * @param sphere_centers vector of points which indicate the center of each
175  * sphere in sphere_list
176  * @param gradient output argument to be populated with the resulting gradient
177  * calculation
178  * @param tolerance
179  * @param subtract_radii distance to the sphere centers will be computed by
180  * substracting the sphere radius from the nearest point
181  * @param maximum_value
182  * @param stop_at_first_collision when true the computation is terminated when
183  * the first collision is found
184  */
185  bool getCollisionSphereGradients(const std::vector<CollisionSphere>& sphere_list,
186  const EigenSTL::vector_Vector3d& sphere_centers, GradientInfo& gradient,
187  const CollisionType& type, double tolerance, bool subtract_radii,
188  double maximum_value, bool stop_at_first_collision);
189 
190 protected:
191  Eigen::Isometry3d pose_;
192 };
193 
194 // determines set of collision spheres given a posed body; this is BAD!
195 // Allocation erorrs will happen; change this function so it does not return
196 // that vector by value
197 std::vector<CollisionSphere> determineCollisionSpheres(const bodies::Body* body, Eigen::Isometry3d& relativeTransform);
198 
199 // determines a set of gradients of the given collision spheres in the distance
200 // field
202  const std::vector<CollisionSphere>& sphere_list,
203  const EigenSTL::vector_Vector3d& sphere_centers, GradientInfo& gradient,
204  const CollisionType& type, double tolerance, bool subtract_radii, double maximum_value,
205  bool stop_at_first_collision);
206 
208  const std::vector<CollisionSphere>& sphere_list,
209  const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value,
210  double tolerance);
211 
213  const std::vector<CollisionSphere>& sphere_list,
214  const EigenSTL::vector_Vector3d& sphere_centers, double maximum_value,
215  double tolerance, unsigned int num_coll, std::vector<unsigned int>& colls);
216 
217 // forward declaration required for friending apparently
218 class BodyDecompositionVector;
219 
220 class BodyDecomposition
221 {
222  friend class BodyDecompositionVector;
223 
224 public:
226 
227  BodyDecomposition(const shapes::ShapeConstPtr& shape, double resolution, double padding = 0.01);
228 
229  BodyDecomposition(const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& poses,
230  double resolution, double padding);
231 
233 
234  Eigen::Isometry3d relative_cylinder_pose_;
235 
236  void replaceCollisionSpheres(const std::vector<CollisionSphere>& new_collision_spheres,
237  const Eigen::Isometry3d& new_relative_cylinder_pose)
238  {
239  // std::cerr << "Replacing " << collision_spheres_.size() << " with " <<
240  // new_collision_spheres.size() << std::endl;
241  collision_spheres_ = new_collision_spheres;
242  relative_cylinder_pose_ = new_relative_cylinder_pose;
243  }
244 
245  const std::vector<CollisionSphere>& getCollisionSpheres() const
246  {
247  return collision_spheres_;
248  }
249 
250  const std::vector<double>& getSphereRadii() const
251  {
253  }
254 
255  const EigenSTL::vector_Vector3d& getCollisionPoints() const
256  {
258  }
259 
260  const bodies::Body* getBody(unsigned int i) const
261  {
262  return bodies_.getBody(i);
263  }
264 
265  unsigned int getBodiesCount()
266  {
267  return bodies_.getCount();
268  }
269 
270  Eigen::Isometry3d getRelativeCylinderPose() const
271  {
273  }
274 
276  {
278  }
279 
280 protected:
281  void init(const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& poses,
282  double resolution, double padding);
283 
284 protected:
286 
288  std::vector<double> sphere_radii_;
289  std::vector<CollisionSphere> collision_spheres_;
290  EigenSTL::vector_Vector3d relative_collision_points_;
291 };
292 
294 {
295 public:
297 
298  PosedBodySphereDecomposition(const BodyDecompositionConstPtr& body_decomposition);
299 
300  const std::vector<CollisionSphere>& getCollisionSpheres() const
301  {
302  return body_decomposition_->getCollisionSpheres();
303  }
304 
305  const EigenSTL::vector_Vector3d& getSphereCenters() const
306  {
308  }
309 
310  const EigenSTL::vector_Vector3d& getCollisionPoints() const
311  {
313  }
314 
315  const std::vector<double>& getSphereRadii() const
316  {
317  return body_decomposition_->getSphereRadii();
318  }
320  {
322  }
323 
324  double getBoundingSphereRadius() const
325  {
326  return body_decomposition_->getRelativeBoundingSphere().radius;
327  }
328 
329  // assumed to be in reference frame, updates the pose of the body,
330  // the collision spheres, and the posed collision points
331  void updatePose(const Eigen::Isometry3d& linkTransform);
332 
333 protected:
334  BodyDecompositionConstPtr body_decomposition_;
336  EigenSTL::vector_Vector3d posed_collision_points_;
337  EigenSTL::vector_Vector3d sphere_centers_;
338 };
339 
341 {
342 public:
344 
345  PosedBodyPointDecomposition(const BodyDecompositionConstPtr& body_decomposition);
346 
347  PosedBodyPointDecomposition(const BodyDecompositionConstPtr& body_decomposition, const Eigen::Isometry3d& pose);
348 
349  PosedBodyPointDecomposition(const std::shared_ptr<const octomap::OcTree>& octree);
350 
351  const EigenSTL::vector_Vector3d& getCollisionPoints() const
352  {
354  }
355  // the collision spheres, and the posed collision points
356  void updatePose(const Eigen::Isometry3d& linkTransform);
357 
358 protected:
359  BodyDecompositionConstPtr body_decomposition_;
360  EigenSTL::vector_Vector3d posed_collision_points_;
361 };
362 
364 {
365 public:
367 
369  {
370  }
371 
372  const std::vector<CollisionSphere>& getCollisionSpheres() const
373  {
374  return collision_spheres_;
375  }
376 
377  const EigenSTL::vector_Vector3d& getSphereCenters() const
378  {
380  }
381 
382  const std::vector<double>& getSphereRadii() const
383  {
384  return sphere_radii_;
385  }
386 
387  void addToVector(PosedBodySphereDecompositionPtr& bd)
388  {
390  decomp_vector_.push_back(bd);
391  collision_spheres_.insert(collision_spheres_.end(), bd->getCollisionSpheres().begin(),
392  bd->getCollisionSpheres().end());
393  posed_collision_spheres_.insert(posed_collision_spheres_.end(), bd->getSphereCenters().begin(),
394  bd->getSphereCenters().end());
395  sphere_radii_.insert(sphere_radii_.end(), bd->getSphereRadii().begin(), bd->getSphereRadii().end());
396  }
397 
398  unsigned int getSize() const
399  {
400  return decomp_vector_.size();
401  }
402 
403  PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const
404  {
405  if (i >= decomp_vector_.size())
406  {
407  ROS_INFO_NAMED("collision_distance_field", "No body decomposition");
408  return empty_ptr_;
409  }
410  return decomp_vector_[i];
411  }
412 
413  void updatePose(unsigned int ind, const Eigen::Isometry3d& pose)
414  {
415  if (ind >= decomp_vector_.size())
416  {
417  ROS_WARN_NAMED("collision_distance_field", "Can't update pose");
418  return;
419  }
420  decomp_vector_[ind]->updatePose(pose);
421  for (unsigned int i = 0; i < decomp_vector_[ind]->getSphereCenters().size(); i++)
422  {
423  posed_collision_spheres_[sphere_index_map_[ind] + i] = decomp_vector_[ind]->getSphereCenters()[i];
424  }
425  }
426 
427 private:
428  PosedBodySphereDecompositionConstPtr empty_ptr_;
429  std::vector<PosedBodySphereDecompositionPtr> decomp_vector_;
430  std::vector<CollisionSphere> collision_spheres_;
431  EigenSTL::vector_Vector3d posed_collision_spheres_;
432  std::vector<double> sphere_radii_;
433  std::map<unsigned int, unsigned int> sphere_index_map_;
434 };
435 
437 {
438 public:
440 
442  {
443  }
444 
445  EigenSTL::vector_Vector3d getCollisionPoints() const
446  {
447  EigenSTL::vector_Vector3d ret_points;
448  for (const PosedBodyPointDecompositionPtr& decomp : decomp_vector_)
449  {
450  ret_points.insert(ret_points.end(), decomp->getCollisionPoints().begin(), decomp->getCollisionPoints().end());
451  }
452  return ret_points;
453  }
454 
455  void addToVector(PosedBodyPointDecompositionPtr& bd)
456  {
457  decomp_vector_.push_back(bd);
458  }
459 
460  unsigned int getSize() const
461  {
462  return decomp_vector_.size();
463  }
464 
465  PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const
466  {
467  if (i >= decomp_vector_.size())
468  {
469  ROS_INFO_NAMED("collision_distance_field", "No body decomposition");
470  return empty_ptr_;
471  }
472  return decomp_vector_[i];
473  }
474 
475  void updatePose(unsigned int ind, const Eigen::Isometry3d& pose)
476  {
477  if (ind < decomp_vector_.size())
478  {
479  decomp_vector_[ind]->updatePose(pose);
480  }
481  else
482  {
483  ROS_WARN_NAMED("collision_distance_field", "Can't update pose");
484  return;
485  }
486  }
487 
488 private:
489  PosedBodyPointDecompositionPtr empty_ptr_;
490  std::vector<PosedBodyPointDecompositionPtr> decomp_vector_;
491 };
492 
493 struct ProximityInfo
494 {
496 
497  std::string link_name;
498  std::string attached_object_name;
499  double proximity;
500  unsigned int sphere_index;
501  unsigned int att_index;
504 };
505 
506 bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1,
507  const PosedBodySphereDecompositionConstPtr& p2);
508 
509 void getCollisionSphereMarkers(const std_msgs::ColorRGBA& color, const std::string& frame_id, const std::string& ns,
510  const ros::Duration& dur,
511  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
512  visualization_msgs::MarkerArray& arr);
513 
514 void getProximityGradientMarkers(const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
515  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
516  const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
517  const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
518 
519 void getCollisionMarkers(const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
520  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
521  const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
522  const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
523 } // namespace collision_detection
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::PosedBodyPointDecomposition
Definition: collision_distance_field_types.h:372
collision_detection::PosedDistanceField::updatePose
void updatePose(const Eigen::Isometry3d &transform)
Definition: collision_distance_field_types.h:159
collision_detection::PosedBodySphereDecompositionVector::getSphereRadii
const std::vector< double > & getSphereRadii() const
Definition: collision_distance_field_types.h:414
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
distance_field::PropagationDistanceField::PropagationDistanceField
PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, double max_distance, bool propagate_negative_distances=false)
Constructor that initializes entire distance field to empty - all cells will be assigned maximum dist...
Definition: propagation_distance_field.cpp:78
Eigen
collision_detection::BodyDecomposition::bodies_
bodies::BodyVector bodies_
Definition: collision_distance_field_types.h:317
shapes
collision_detection::PosedBodyPointDecompositionVector::getCollisionPoints
EigenSTL::vector_Vector3d getCollisionPoints() const
Definition: collision_distance_field_types.h:477
collision_detection::PosedBodyPointDecompositionVector::getSize
unsigned int getSize() const
Definition: collision_distance_field_types.h:492
collision_detection::PosedBodySphereDecompositionVector::decomp_vector_
std::vector< PosedBodySphereDecompositionPtr > decomp_vector_
Definition: collision_distance_field_types.h:461
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
collision_detection::PosedBodyPointDecompositionVector
Definition: collision_distance_field_types.h:468
collision_detection::BodyDecomposition::getCollisionSpheres
const std::vector< CollisionSphere > & getCollisionSpheres() const
Definition: collision_distance_field_types.h:277
collision_detection::CollisionSphere::radius_
double radius_
Definition: collision_distance_field_types.h:107
collision_detection::PosedBodySphereDecompositionVector::posed_collision_spheres_
EigenSTL::vector_Vector3d posed_collision_spheres_
Definition: collision_distance_field_types.h:463
distance_field::PropagationDistanceField
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
Definition: propagation_distance_field.h:137
collision_detection::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
collision_detection::BodyDecomposition::~BodyDecomposition
~BodyDecomposition()
Definition: collision_distance_field_types.cpp:322
collision_detection::PosedBodyPointDecompositionVector::addToVector
void addToVector(PosedBodyPointDecompositionPtr &bd)
Definition: collision_distance_field_types.h:487
propagation_distance_field.h
collision_detection::BodyDecomposition::collision_spheres_
std::vector< CollisionSphere > collision_spheres_
Definition: collision_distance_field_types.h:321
distance_field::DistanceField::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 gradient is computed a...
Definition: distance_field.cpp:94
collision_detection::CollisionSphere::relative_vec_
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d relative_vec_
Definition: collision_distance_field_types.h:106
collision_detection::GradientInfo::sphere_radii
std::vector< double > sphere_radii
Definition: collision_distance_field_types.h:124
collision_detection::BodyDecomposition::relative_bounding_sphere_
bodies::BoundingSphere relative_bounding_sphere_
Definition: collision_distance_field_types.h:319
collision_detection::PosedBodySphereDecomposition::getCollisionSpheres
const std::vector< CollisionSphere > & getCollisionSpheres() const
Definition: collision_distance_field_types.h:332
collision_detection::BodyDecomposition::sphere_radii_
std::vector< double > sphere_radii_
Definition: collision_distance_field_types.h:320
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
collision_detection::BodyDecomposition::getRelativeCylinderPose
Eigen::Isometry3d getRelativeCylinderPose() const
Definition: collision_distance_field_types.h:302
collision_detection::CollisionType
CollisionType
Definition: collision_distance_field_types.h:89
collision_detection::BodyDecomposition::BodyDecompositionVector
friend class BodyDecompositionVector
Definition: collision_distance_field_types.h:254
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
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
collision_detection::PosedBodySphereDecompositionVector::collision_spheres_
std::vector< CollisionSphere > collision_spheres_
Definition: collision_distance_field_types.h:462
class_forward.h
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
collision_detection::BodyDecomposition::relative_collision_points_
EigenSTL::vector_Vector3d relative_collision_points_
Definition: collision_distance_field_types.h:322
collision_detection::ProximityInfo
Definition: collision_distance_field_types.h:525
bodies::Body
collision_detection::BodyDecomposition
Definition: collision_distance_field_types.h:252
collision_detection::PosedBodyPointDecomposition::getCollisionPoints
const EigenSTL::vector_Vector3d & getCollisionPoints() const
Definition: collision_distance_field_types.h:383
OcTree.h
collision_detection::PosedBodySphereDecomposition::getSphereRadii
const std::vector< double > & getSphereRadii() const
Definition: collision_distance_field_types.h:347
collision_detection::doBoundingSpheresIntersect
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
Definition: collision_distance_field_types.cpp:396
collision_detection::PosedBodySphereDecompositionVector
Definition: collision_distance_field_types.h:395
Identity
Identity
collision_detection::PosedBodyPointDecomposition::PosedBodyPointDecomposition
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecomposition(const BodyDecompositionConstPtr &body_decomposition)
Definition: collision_distance_field_types.cpp:327
bodies::BodyVector::getBody
const Body * getBody(unsigned int i) const
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
bodies::BoundingSphere
collision_detection::PosedBodySphereDecompositionVector::getCollisionSpheres
const std::vector< CollisionSphere > & getCollisionSpheres() const
Definition: collision_distance_field_types.h:404
console.h
collision_detection::PosedBodySphereDecompositionVector::sphere_index_map_
std::map< unsigned int, unsigned int > sphere_index_map_
Definition: collision_distance_field_types.h:465
collision_detection::GradientInfo::collision
bool collision
Definition: collision_distance_field_types.h:119
collision_detection::ProximityInfo::closest_point
Eigen::Vector3d closest_point
Definition: collision_distance_field_types.h:534
collision_detection::PosedBodySphereDecompositionVector::addToVector
void addToVector(PosedBodySphereDecompositionPtr &bd)
Definition: collision_distance_field_types.h:419
collision_detection::GradientInfo::joint_name
std::string joint_name
Definition: collision_distance_field_types.h:125
collision_detection::ProximityInfo::proximity
double proximity
Definition: collision_distance_field_types.h:531
collision_detection::CollisionSphere::CollisionSphere
CollisionSphere(const Eigen::Vector3d &rel, double radius)
Definition: collision_distance_field_types.h:99
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::PosedBodySphereDecompositionVector::getPosedBodySphereDecomposition
PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const
Definition: collision_distance_field_types.h:435
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
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
collision_detection::PosedDistanceField
Definition: collision_distance_field_types.h:146
collision_detection::BodyDecomposition::replaceCollisionSpheres
void replaceCollisionSpheres(const std::vector< CollisionSphere > &new_collision_spheres, const Eigen::Isometry3d &new_relative_cylinder_pose)
Definition: collision_distance_field_types.h:268
y
double y
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
collision_detection::PosedBodyPointDecompositionVector::getPosedBodyDecomposition
PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const
Definition: collision_distance_field_types.h:497
collision_detection::PosedBodySphereDecomposition::getBoundingSphereRadius
double getBoundingSphereRadius() const
Definition: collision_distance_field_types.h:356
collision_detection::PosedBodySphereDecomposition::updatePose
void updatePose(const Eigen::Isometry3d &linkTransform)
Definition: collision_distance_field_types.cpp:376
collision_detection::BodyDecomposition::getBodiesCount
unsigned int getBodiesCount()
Definition: collision_distance_field_types.h:297
collision_detection::GradientInfo::sphere_locations
EigenSTL::vector_Vector3d sphere_locations
Definition: collision_distance_field_types.h:120
shapes.h
collision_detection::NONE
@ NONE
Definition: collision_distance_field_types.h:123
collision_detection::PosedBodySphereDecompositionVector::updatePose
void updatePose(unsigned int ind, const Eigen::Isometry3d &pose)
Definition: collision_distance_field_types.h:445
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::GradientInfo::GradientInfo
GradientInfo()
Definition: collision_distance_field_types.h:112
collision_detection::ENVIRONMENT
@ ENVIRONMENT
Definition: collision_distance_field_types.h:126
collision_detection::PosedBodySphereDecompositionVector::PosedBodySphereDecompositionVector
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodySphereDecompositionVector()
Definition: collision_distance_field_types.h:400
bodies::BodyVector::getCount
std::size_t getCount() const
collision_detection::BodyDecomposition::getBody
const bodies::Body * getBody(unsigned int i) const
Definition: collision_distance_field_types.h:292
collision_detection::GradientInfo::clear
void clear()
Definition: collision_distance_field_types.h:127
distance_field.h
distance_field
Namespace for holding classes that generate distance fields.
Definition: distance_field.h:64
collision_detection::PosedBodyPointDecompositionVector::decomp_vector_
std::vector< PosedBodyPointDecompositionPtr > decomp_vector_
Definition: collision_distance_field_types.h:522
collision_detection::ProximityInfo::sphere_index
unsigned int sphere_index
Definition: collision_distance_field_types.h:532
collision_detection::PosedBodySphereDecomposition::body_decomposition_
BodyDecompositionConstPtr body_decomposition_
Definition: collision_distance_field_types.h:366
collision_detection::ProximityInfo::attached_object_name
std::string attached_object_name
Definition: collision_distance_field_types.h:530
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::relative_cylinder_pose_
Eigen::Isometry3d relative_cylinder_pose_
Definition: collision_distance_field_types.h:266
collision_detection::PosedBodySphereDecomposition::getBoundingSphereCenter
const Eigen::Vector3d & getBoundingSphereCenter() const
Definition: collision_distance_field_types.h:351
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::PosedBodyPointDecompositionVector::updatePose
void updatePose(unsigned int ind, const Eigen::Isometry3d &pose)
Definition: collision_distance_field_types.h:507
collision_detection::PosedBodySphereDecompositionVector::empty_ptr_
PosedBodySphereDecompositionConstPtr empty_ptr_
Definition: collision_distance_field_types.h:460
collision_detection::PosedBodySphereDecomposition::posed_collision_points_
EigenSTL::vector_Vector3d posed_collision_points_
Definition: collision_distance_field_types.h:368
collision_detection::BodyDecomposition::getRelativeBoundingSphere
const bodies::BoundingSphere & getRelativeBoundingSphere() const
Definition: collision_distance_field_types.h:307
collision_detection::PosedBodySphereDecomposition::sphere_centers_
EigenSTL::vector_Vector3d sphere_centers_
Definition: collision_distance_field_types.h:369
collision_detection::PosedBodyPointDecompositionVector::PosedBodyPointDecompositionVector
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedBodyPointDecompositionVector()
Definition: collision_distance_field_types.h:473
bodies.h
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
collision_detection::PosedDistanceField::PosedDistanceField
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedDistanceField(const Eigen::Vector3d &size, const Eigen::Vector3d &origin, double resolution, double max_distance, bool propagate_negative_distances=false)
Definition: collision_distance_field_types.h:151
collision_detection::ProximityInfo::link_name
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string link_name
Definition: collision_distance_field_types.h:529
collision_detection::ProximityInfo::closest_gradient
Eigen::Vector3d closest_gradient
Definition: collision_distance_field_types.h:535
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::BodyDecomposition::getCollisionPoints
const EigenSTL::vector_Vector3d & getCollisionPoints() const
Definition: collision_distance_field_types.h:287
x
double x
collision_detection::BodyDecomposition::getSphereRadii
const std::vector< double > & getSphereRadii() const
Definition: collision_distance_field_types.h:282
collision_detection::PosedBodySphereDecomposition
Definition: collision_distance_field_types.h:325
collision_detection::PosedBodyPointDecompositionVector::empty_ptr_
PosedBodyPointDecompositionPtr empty_ptr_
Definition: collision_distance_field_types.h:521
collision_detection::PosedBodySphereDecomposition::getSphereCenters
const EigenSTL::vector_Vector3d & getSphereCenters() const
Definition: collision_distance_field_types.h:337
collision_detection::PosedDistanceField::pose_
Eigen::Isometry3d pose_
Definition: collision_distance_field_types.h:223
bodies::BodyVector
collision_detection::ProximityInfo::att_index
unsigned int att_index
Definition: collision_distance_field_types.h:533
collision_detection::PosedBodySphereDecompositionVector::sphere_radii_
std::vector< double > sphere_radii_
Definition: collision_distance_field_types.h:464
ros::Duration
z
double z
collision_detection::PosedDistanceField::getPose
const Eigen::Isometry3d & getPose() const
Definition: collision_distance_field_types.h:164
collision_detection::PosedBodySphereDecompositionVector::getSize
unsigned int getSize() const
Definition: collision_distance_field_types.h:430
collision_detection::GradientInfo::closest_distance
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double closest_distance
Definition: collision_distance_field_types.h:118
collision_detection::PosedBodySphereDecompositionVector::getSphereCenters
const EigenSTL::vector_Vector3d & getSphereCenters() const
Definition: collision_distance_field_types.h:409
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
collision_detection::PosedBodySphereDecomposition::getCollisionPoints
const EigenSTL::vector_Vector3d & getCollisionPoints() const
Definition: collision_distance_field_types.h:342
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 3 2021 03:23:21