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 
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::Isometry3d::Identity())
126  {
127  }
128 
129  void updatePose(const Eigen::Isometry3d& transform)
130  {
131  pose_ = transform;
132  }
133 
134  const Eigen::Isometry3d& 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::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::Isometry3d 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::Isometry3d& 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_Isometry3d& poses,
232  double resolution, double padding);
233 
235 
236  Eigen::Isometry3d relative_cylinder_pose_;
237 
238  void replaceCollisionSpheres(const std::vector<CollisionSphere>& new_collision_spheres,
239  const Eigen::Isometry3d& 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::Isometry3d 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_Isometry3d& poses,
284  double resolution, 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  }
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::Isometry3d& 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::Isometry3d& pose);
350 
351  PosedBodyPointDecomposition(const 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::Isometry3d& 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::Isometry3d& 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::Isometry3d& 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;
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
PosedBodyPointDecompositionConstPtr getPosedBodyDecomposition(unsigned int i) const
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)
#define ROS_INFO_NAMED(name,...)
PosedBodySphereDecompositionConstPtr getPosedBodySphereDecomposition(unsigned int i) const
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
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)
std::vector< PosedBodyPointDecompositionPtr > decomp_vector_
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
Identity
Generic interface to collision detection.
const EigenSTL::vector_Vector3d & getSphereCenters() const
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
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...
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...
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)
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...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string link_name
const std::vector< double > & getSphereRadii() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d relative_vec_
void replaceCollisionSpheres(const std::vector< CollisionSphere > &new_collision_spheres, const Eigen::Isometry3d &new_relative_cylinder_pose)
const std::vector< CollisionSphere > & getCollisionSpheres() const
const std::vector< CollisionSphere > & getCollisionSpheres() const
Namespace for holding classes that generate distance fields.
const geometry_msgs::Pose * pose_
void updatePose(const Eigen::Isometry3d &transform)
void updatePose(unsigned int ind, const Eigen::Isometry3d &pose)
const bodies::Body * getBody(unsigned int i) const
std::vector< PosedBodySphereDecompositionPtr > decomp_vector_
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
double x
void updatePose(unsigned int ind, const Eigen::Isometry3d &pose)
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
const bodies::BoundingSphere & getRelativeBoundingSphere() 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
const EigenSTL::vector_Vector3d & getCollisionPoints() const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Oct 11 2019 03:56:03