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