include/moveit/collision_detection/collision_common.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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 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: Ioan Sucan */
36 
37 #ifndef MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_
38 #define MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_
39 
40 #include <boost/array.hpp>
41 #include <boost/function.hpp>
42 #include <vector>
43 #include <string>
44 #include <map>
45 #include <set>
46 #include <Eigen/Core>
47 #include <console_bridge/console.h>
48 
49 namespace collision_detection
50 {
52 namespace BodyTypes
53 {
55 enum Type
56 {
59 
62 
65 };
66 }
67 
70 
72 struct Contact
73 {
75 
77  Eigen::Vector3d pos;
78 
80  Eigen::Vector3d normal;
81 
83  double depth;
84 
86  std::string body_name_1;
87 
89  BodyType body_type_1;
90 
92  std::string body_name_2;
93 
95  BodyType body_type_2;
96 };
97 
101 {
103  boost::array<double, 3> aabb_min;
104 
106  boost::array<double, 3> aabb_max;
107 
109  double cost;
110 
112  double getVolume() const
113  {
114  return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
115  }
116 
118  bool operator<(const CostSource& other) const
119  {
120  double c1 = cost * getVolume();
121  double c2 = other.cost * other.getVolume();
122  if (c1 > c2)
123  return true;
124  if (c1 < c2)
125  return false;
126  if (cost < other.cost)
127  return false;
128  if (cost > other.cost)
129  return true;
130  return aabb_min < other.aabb_min;
131  }
132 };
133 
136 {
137  CollisionResult() : collision(false), distance(std::numeric_limits<double>::max()), contact_count(0)
138  {
139  }
140  typedef std::map<std::pair<std::string, std::string>, std::vector<Contact> > ContactMap;
141 
143 
145  void clear()
146  {
147  collision = false;
148  distance = std::numeric_limits<double>::max();
149  contact_count = 0;
150  contacts.clear();
151  cost_sources.clear();
152  }
153 
155  bool collision;
156 
158  double distance;
159 
161  std::size_t contact_count;
162 
165  ContactMap contacts;
166 
168  std::set<CostSource> cost_sources;
169 };
170 
173 {
175  : distance(false)
176  , cost(false)
177  , contacts(false)
178  , max_contacts(1)
179  , max_contacts_per_pair(1)
180  , max_cost_sources(1)
181  , min_cost_density(0.2)
182  , verbose(false)
183  {
184  }
186  {
187  }
188 
190  std::string group_name;
191 
193  bool distance;
194 
196  bool cost;
197 
199  bool contacts;
200 
202  std::size_t max_contacts;
203 
207 
209  std::size_t max_cost_sources;
210 
213 
215  boost::function<bool(const CollisionResult&)> is_done;
216 
218  bool verbose;
219 };
220 }
221 
222 #endif
double getVolume() const
Get the volume of the AABB around the cost source.
double cost
The partial cost (the probability of existance for the object there is a collision with) ...
Type
The types of bodies that are considered for collision.
boost::array< double, 3 > aabb_min
The minimum bound of the AABB defining the volume responsible for this partial cost.
double min_cost_density
When costs are computed, this is the minimum cost density for a CostSource to be included in the resu...
bool operator<(const CostSource &other) const
Order cost sources so that the most costly source is at the top.
BodyType body_type_2
The type of the second body involved in the contact.
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
Generic interface to collision detection.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
bool collision
True if collision was found, false otherwise.
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
std::set< CostSource > cost_sources
When costs are computed, the individual cost sources are.
boost::function< bool(const CollisionResult &)> is_done
Function call that decides whether collision detection should stop.
bool verbose
std::size_t max_contacts
Overall maximum number of contacts to compute.
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
boost::array< double, 3 > aabb_max
The maximum bound of the AABB defining the volume responsible for this partial cost.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
std::string body_name_2
The id of the second body involved in the contact.
Eigen::Vector3d normal
normal unit vector at contact
std::string body_name_1
The id of the first body involved in the contact.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned...
BodyType body_type_1
The type of the first body involved in the contact.
When collision costs are computed, this structure contains information about the partial cost incurre...
BodyTypes::Type BodyType
The types of bodies that are considered for collision.
double max(double a, double b)
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44