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>
48 
49 namespace collision_detection
50 {
51 MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix);
52 
54 namespace BodyTypes
55 {
57 enum Type
58 {
61 
64 
67 };
68 }
69 
72 
74 struct Contact
75 {
77 
80 
83 
85  double depth;
86 
88  std::string body_name_1;
89 
91  BodyType body_type_1;
92 
94  std::string body_name_2;
95 
97  BodyType body_type_2;
98 };
99 
103 {
105  boost::array<double, 3> aabb_min;
106 
108  boost::array<double, 3> aabb_max;
109 
111  double cost;
112 
114  double getVolume() const
115  {
116  return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
117  }
118 
120  bool operator<(const CostSource& other) const
121  {
122  double c1 = cost * getVolume();
123  double c2 = other.cost * other.getVolume();
124  if (c1 > c2)
125  return true;
126  if (c1 < c2)
127  return false;
128  if (cost < other.cost)
129  return false;
130  if (cost > other.cost)
131  return true;
132  return aabb_min < other.aabb_min;
133  }
134 };
135 
138 {
139  CollisionResult() : collision(false), distance(std::numeric_limits<double>::max()), contact_count(0)
140  {
141  }
142  typedef std::map<std::pair<std::string, std::string>, std::vector<Contact> > ContactMap;
143 
145 
147  void clear()
148  {
149  collision = false;
150  distance = std::numeric_limits<double>::max();
151  contact_count = 0;
152  contacts.clear();
153  cost_sources.clear();
154  }
155 
157  bool collision;
158 
160  double distance;
161 
163  std::size_t contact_count;
164 
166  ContactMap contacts;
167 
169  std::set<CostSource> cost_sources;
170 };
171 
174 {
176  : distance(false)
177  , cost(false)
178  , contacts(false)
179  , max_contacts(1)
180  , max_contacts_per_pair(1)
181  , max_cost_sources(1)
182  , min_cost_density(0.2)
183  , verbose(false)
184  {
185  }
187  {
188  }
189 
191  std::string group_name;
192 
194  bool distance;
195 
197  bool cost;
198 
200  bool contacts;
201 
203  std::size_t max_contacts;
204 
208 
210  std::size_t max_cost_sources;
211 
214 
216  boost::function<bool(const CollisionResult&)> is_done;
217 
219  bool verbose;
220 };
221 
222 namespace DistanceRequestTypes
223 {
225 {
230 };
231 }
233 
236 {
238  : enable_nearest_points(false)
239  , enable_signed_distance(false)
240  , type(DistanceRequestType::GLOBAL)
241  , max_contacts_per_body(1)
242  , active_components_only(nullptr)
243  , acm(nullptr)
244  , distance_threshold(std::numeric_limits<double>::max())
245  , verbose(false)
246  , compute_gradient(false)
247  {
248  }
249 
251  void enableGroup(const robot_model::RobotModelConstPtr& robot_model)
252  {
253  if (robot_model->hasJointModelGroup(group_name))
254  active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet();
255  else
256  active_components_only = nullptr;
257  }
258 
261 
264 
268  DistanceRequestType type;
269 
272 
274  std::string group_name;
275 
277  const std::set<const robot_model::LinkModel*>* active_components_only;
278 
281 
285 
287  bool verbose;
288 
292 };
293 
296 {
298  {
299  clear();
300  }
301 
303  double distance;
304 
306  Eigen::Vector3d nearest_points[2];
307 
309  std::string link_names[2];
310 
312  BodyType body_types[2];
313 
320 
322  void clear()
323  {
324  distance = std::numeric_limits<double>::max();
325  nearest_points[0].setZero();
326  nearest_points[1].setZero();
327  body_types[0] = BodyType::WORLD_OBJECT;
328  body_types[1] = BodyType::WORLD_OBJECT;
329  link_names[0] = "";
330  link_names[1] = "";
331  normal.setZero();
332  }
333 
335  void operator=(const DistanceResultsData& other)
336  {
337  distance = other.distance;
338  nearest_points[0] = other.nearest_points[0];
339  nearest_points[1] = other.nearest_points[1];
340  link_names[0] = other.link_names[0];
341  link_names[1] = other.link_names[1];
342  body_types[0] = other.body_types[0];
343  body_types[1] = other.body_types[1];
344  normal = other.normal;
345  }
346 
348  bool operator<(const DistanceResultsData& other)
349  {
350  return (distance < other.distance);
351  }
352 
354  bool operator>(const DistanceResultsData& other)
355  {
356  return (distance > other.distance);
357  }
358 };
359 
361 typedef std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> > DistanceMap;
362 
365 {
366  DistanceResult() : collision(false)
367  {
368  }
369 
371  bool collision;
372 
375 
377  DistanceMap distances;
378 
380  void clear()
381  {
382  collision = false;
383  minimum_distance.clear();
384  distances.clear();
385  }
386 };
387 }
388 
389 #endif
DistanceRequestTypes::DistanceRequestType DistanceRequestType
Core components of MoveIt!
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...
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
std::map< const std::pair< std::string, std::string >, std::vector< DistanceResultsData > > DistanceMap
Mapping between the names of the collision objects and the DistanceResultData.
BodyType body_type_2
The type of the second body involved in the contact.
double getVolume() const
Get the volume of the AABB around the cost source.
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
bool operator<(const DistanceResultsData &other)
Compare if the distance is less than another.
bool operator>(const DistanceResultsData &other)
Compare if the distance is greater than another.
bool enable_nearest_points
Indicate if nearest point information should be calculated.
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
Generic interface to collision detection.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
bool collision
True if collision was found, false otherwise.
const std::set< const robot_model::LinkModel * > * active_components_only
The set of active components to check.
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
std::set< CostSource > cost_sources
These are the individual cost sources when costs are computed.
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. ...
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
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
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void enableGroup(const robot_model::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on req_.
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
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
Generic representation of the distance information for a pair of objects.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
bool operator<(const CostSource &other) const
Order cost sources so that the most costly source is at the top.
bool collision
Indicates if two objects were in collision.
void operator=(const DistanceResultsData &other)
Update structure data given DistanceResultsData object.
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_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold) ...
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.
Find a limited(max_contacts_per_body) set of contacts for a given pair.
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 Fri Oct 11 2019 03:56:03