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>
49 
50 namespace collision_detection
51 {
52 MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix);
53 
55 namespace BodyTypes
56 {
58 enum Type
59 {
62 
65 
68 };
69 }
70 
73 
75 struct Contact
76 {
78 
80  Eigen::Vector3d pos;
81 
83  Eigen::Vector3d normal;
84 
86  double depth;
87 
89  std::string body_name_1;
90 
92  BodyType body_type_1;
93 
95  std::string body_name_2;
96 
98  BodyType body_type_2;
99 };
100 
104 {
106  boost::array<double, 3> aabb_min;
107 
109  boost::array<double, 3> aabb_max;
110 
112  double cost;
113 
115  double getVolume() const
116  {
117  return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
118  }
119 
121  bool operator<(const CostSource& other) const
122  {
123  double c1 = cost * getVolume();
124  double c2 = other.cost * other.getVolume();
125  if (c1 > c2)
126  return true;
127  if (c1 < c2)
128  return false;
129  if (cost < other.cost)
130  return false;
131  if (cost > other.cost)
132  return true;
133  return aabb_min < other.aabb_min;
134  }
135 };
136 
139 {
140  CollisionResult() : collision(false), distance(std::numeric_limits<double>::max()), contact_count(0)
141  {
142  }
143  typedef std::map<std::pair<std::string, std::string>, std::vector<Contact> > ContactMap;
144 
146 
148  void clear()
149  {
150  collision = false;
151  distance = std::numeric_limits<double>::max();
152  contact_count = 0;
153  contacts.clear();
154  cost_sources.clear();
155  }
156 
158  bool collision;
159 
161  double distance;
162 
164  std::size_t contact_count;
165 
168  ContactMap contacts;
169 
171  std::set<CostSource> cost_sources;
172 };
173 
176 {
178  : distance(false)
179  , cost(false)
180  , contacts(false)
181  , max_contacts(1)
182  , max_contacts_per_pair(1)
183  , max_cost_sources(1)
184  , min_cost_density(0.2)
185  , verbose(false)
186  {
187  }
189  {
190  }
191 
193  std::string group_name;
194 
196  bool distance;
197 
199  bool cost;
200 
202  bool contacts;
203 
205  std::size_t max_contacts;
206 
210 
212  std::size_t max_cost_sources;
213 
216 
218  boost::function<bool(const CollisionResult&)> is_done;
219 
221  bool verbose;
222 };
223 
224 namespace DistanceRequestTypes
225 {
227 {
232 };
233 }
235 
237 {
239  : enable_nearest_points(false)
240  , enable_signed_distance(false)
241  , type(DistanceRequestType::GLOBAL)
242  , max_contacts_per_body(1)
243  , active_components_only(nullptr)
244  , acm(nullptr)
245  , distance_threshold(std::numeric_limits<double>::max())
246  , verbose(false)
247  , compute_gradient(false)
248  {
249  }
250 
252  void enableGroup(const robot_model::RobotModelConstPtr& kmodel)
253  {
254  if (kmodel->hasJointModelGroup(group_name))
255  active_components_only = &kmodel->getJointModelGroup(group_name)->getUpdatedLinkModelsSet();
256  else
257  active_components_only = nullptr;
258  }
259 
262 
265 
269  DistanceRequestType type;
270 
273 
275  std::string group_name;
276 
278  const std::set<const robot_model::LinkModel*>* active_components_only;
279 
282 
286 
288  bool verbose;
289 
293 };
294 
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 
319  Eigen::Vector3d normal;
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 
360 typedef std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> > DistanceMap;
361 
363 {
364  DistanceResult() : collision(false)
365  {
366  }
367 
369  bool collision;
370 
373 
375  DistanceMap distances;
376 
378  void clear()
379  {
380  collision = false;
381  minimum_distance.clear();
382  distances.clear();
383  }
384 };
385 }
386 
387 #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 Sat Apr 21 2018 02:54:51