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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33