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 
79  Eigen::Vector3d pos;
80 
82  Eigen::Vector3d normal;
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 
167  ContactMap contacts;
168 
170  std::set<CostSource> cost_sources;
171 };
172 
175 {
177  : distance(false)
178  , cost(false)
179  , contacts(false)
180  , max_contacts(1)
181  , max_contacts_per_pair(1)
182  , max_cost_sources(1)
183  , min_cost_density(0.2)
184  , verbose(false)
185  {
186  }
188  {
189  }
190 
192  std::string group_name;
193 
195  bool distance;
196 
198  bool cost;
199 
201  bool contacts;
202 
204  std::size_t max_contacts;
205 
209 
211  std::size_t max_cost_sources;
212 
215 
217  boost::function<bool(const CollisionResult&)> is_done;
218 
220  bool verbose;
221 };
222 
223 namespace DistanceRequestTypes
224 {
226 {
231 };
232 }
234 
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& kmodel)
252  {
253  if (kmodel->hasJointModelGroup(group_name))
254  active_components_only = &kmodel->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 
295 {
297  {
298  clear();
299  }
300 
302  double distance;
303 
305  Eigen::Vector3d nearest_points[2];
306 
308  std::string link_names[2];
309 
311  BodyType body_types[2];
312 
318  Eigen::Vector3d normal;
319 
321  void clear()
322  {
323  distance = std::numeric_limits<double>::max();
324  nearest_points[0].setZero();
325  nearest_points[1].setZero();
326  body_types[0] = BodyType::WORLD_OBJECT;
327  body_types[1] = BodyType::WORLD_OBJECT;
328  link_names[0] = "";
329  link_names[1] = "";
330  normal.setZero();
331  }
332 
334  void operator=(const DistanceResultsData& other)
335  {
336  distance = other.distance;
337  nearest_points[0] = other.nearest_points[0];
338  nearest_points[1] = other.nearest_points[1];
339  link_names[0] = other.link_names[0];
340  link_names[1] = other.link_names[1];
341  body_types[0] = other.body_types[0];
342  body_types[1] = other.body_types[1];
343  normal = other.normal;
344  }
345 
347  bool operator<(const DistanceResultsData& other)
348  {
349  return (distance < other.distance);
350  }
351 
353  bool operator>(const DistanceResultsData& other)
354  {
355  return (distance > other.distance);
356  }
357 };
358 
359 typedef std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> > DistanceMap;
360 
362 {
363  DistanceResult() : collision(false)
364  {
365  }
366 
368  bool collision;
369 
372 
374  DistanceMap distances;
375 
377  void clear()
378  {
379  collision = false;
380  minimum_distance.clear();
381  distances.clear();
382  }
383 };
384 }
385 
386 #endif
double getVolume() const
Get the volume of the AABB around the cost source.
DistanceRequestTypes::DistanceRequestType DistanceRequestType
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.
std::map< const std::pair< std::string, std::string >, std::vector< DistanceResultsData > > DistanceMap
BodyType body_type_2
The type of the second body involved in the contact.
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 ids of the bodies in contact, plus information about the contacts themse...
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.
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
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
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. ...
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...
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.
Find a limited(max_contacts_per_body) set of contacts for a given pair.
Eigen::Vector3d normal
normal unit vector at contact
double distance
The distance between two objects. If two objects are in collision, distance <= 0. ...
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.
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 Wed Dec 12 2018 03:48:59