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 #pragma once
38 
39 #include <boost/array.hpp>
40 #include <boost/function.hpp>
41 #include <vector>
42 #include <string>
43 #include <map>
44 #include <set>
45 #include <Eigen/Core>
47 
48 namespace collision_detection
49 {
50 MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc
51 
53 namespace BodyTypes
54 {
56 enum Type
57 {
59  ROBOT_LINK,
60 
63 
66 };
67 } // namespace BodyTypes
68 
71 
73 struct Contact
74 {
76 
79 
82 
84  double depth;
85 
87  std::string body_name_1;
88 
91 
93  std::string body_name_2;
94 
97 
103 
106 };
107 
111 {
113  boost::array<double, 3> aabb_min;
114 
116  boost::array<double, 3> aabb_max;
117 
119  double cost;
120 
122  double getVolume() const
123  {
124  return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
125  }
126 
128  bool operator<(const CostSource& other) const
129  {
130  double c1 = cost * getVolume();
131  double c2 = other.cost * other.getVolume();
132  if (c1 > c2)
133  return true;
134  if (c1 < c2)
135  return false;
136  if (cost < other.cost)
137  return false;
138  if (cost > other.cost)
139  return true;
140  return aabb_min < other.aabb_min;
141  }
142 };
143 
146 {
147  CollisionResult() : collision(false), distance(std::numeric_limits<double>::max()), contact_count(0)
148  {
149  }
150  using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
151 
153 
155  void clear()
156  {
157  collision = false;
158  distance = std::numeric_limits<double>::max();
159  contact_count = 0;
160  contacts.clear();
161  cost_sources.clear();
162  }
163 
165  void print() const;
166 
168  bool collision;
169 
171  double distance;
172 
174  std::size_t contact_count;
175 
178 
180  std::set<CostSource> cost_sources;
181 };
182 
184 struct CollisionRequest
185 {
187  : distance(false)
188  , cost(false)
189  , contacts(false)
190  , max_contacts(1)
192  , max_cost_sources(1)
193  , verbose(false)
194  {
195  }
196  virtual ~CollisionRequest()
197  {
198  }
199 
201  std::string group_name;
202 
204  bool distance;
205 
207  bool cost;
208 
210  bool contacts;
211 
213  std::size_t max_contacts;
214 
217  std::size_t max_contacts_per_pair;
218 
220  std::size_t max_cost_sources;
221 
223  boost::function<bool(const CollisionResult&)> is_done;
224 
226  bool verbose;
227 };
228 
229 namespace DistanceRequestTypes
230 {
232 {
234  SINGLE,
235  LIMITED,
237 };
238 }
240 
243 {
246  , enable_signed_distance(false)
250  , acm(nullptr)
251  , distance_threshold(std::numeric_limits<double>::max())
252  , verbose(false)
253  , compute_gradient(false)
254  {
255  }
256 
258  void enableGroup(const moveit::core::RobotModelConstPtr& robot_model)
259  {
260  if (robot_model->hasJointModelGroup(group_name))
261  active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet();
262  else
264  }
265 
268 
271 
276 
278  std::size_t max_contacts_per_body;
279 
281  std::string group_name;
282 
284  const std::set<const moveit::core::LinkModel*>* active_components_only;
285 
288 
291  double distance_threshold;
292 
294  bool verbose;
295 
298  bool compute_gradient;
299 };
300 
302 struct DistanceResultsData // NOLINT(readability-identifier-naming) - suppress spurious clang-tidy warning
303 {
305  {
306  clear();
307  }
308 
310  double distance;
311 
314 
316  std::string link_names[2];
317 
320 
327 
329  void clear()
330  {
331  distance = std::numeric_limits<double>::max();
332  nearest_points[0].setZero();
333  nearest_points[1].setZero();
336  link_names[0] = "";
337  link_names[1] = "";
338  normal.setZero();
339  }
340 
342  bool operator<(const DistanceResultsData& other)
343  {
344  return (distance < other.distance);
345  }
346 
348  bool operator>(const DistanceResultsData& other)
349  {
350  return (distance > other.distance);
351  }
352 };
353 
355 using DistanceMap = std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> >;
356 
359 {
360  DistanceResult() : collision(false)
361  {
362  }
363 
365  bool collision;
366 
369 
372 
374  void clear()
375  {
376  collision = false;
378  distances.clear();
379  }
380 };
381 } // namespace collision_detection
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
collision_detection::CostSource::cost
double cost
The partial cost (the probability of existance for the object there is a collision with)
Definition: include/moveit/collision_detection/collision_common.h:151
collision_detection::DistanceResultsData::operator>
bool operator>(const DistanceResultsData &other)
Compare if the distance is greater than another.
Definition: include/moveit/collision_detection/collision_common.h:380
collision_detection::CollisionRequest::cost
bool cost
If true, a collision cost is computed.
Definition: include/moveit/collision_detection/collision_common.h:239
collision_detection::CostSource::aabb_min
boost::array< double, 3 > aabb_min
The minimum bound of the AABB defining the volume responsible for this partial cost.
Definition: include/moveit/collision_detection/collision_common.h:145
collision_detection::CollisionResult::ContactMap
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
Definition: include/moveit/collision_detection/collision_common.h:182
collision_detection::Contact::body_type_1
BodyType body_type_1
The type of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:122
collision_detection::Contact::depth
double depth
depth (penetration between bodies)
Definition: include/moveit/collision_detection/collision_common.h:116
collision_detection::DistanceRequest::DistanceRequest
DistanceRequest()
Definition: include/moveit/collision_detection/collision_common.h:276
collision_detection::CollisionRequest::~CollisionRequest
virtual ~CollisionRequest()
Definition: include/moveit/collision_detection/collision_common.h:228
collision_detection::BodyTypes::WORLD_OBJECT
@ WORLD_OBJECT
A body in the environment.
Definition: include/moveit/collision_detection/collision_common.h:97
collision_detection::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix)
collision_detection::CollisionResult::CollisionResult
CollisionResult()
Definition: include/moveit/collision_detection/collision_common.h:179
collision_detection::DistanceRequest::max_contacts_per_body
std::size_t max_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold)
Definition: include/moveit/collision_detection/collision_common.h:310
collision_detection::DistanceRequestTypes::LIMITED
@ LIMITED
Find a limited(max_contacts_per_body) set of contacts for a given pair.
Definition: include/moveit/collision_detection/collision_common.h:267
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
collision_detection::DistanceResultsData::link_names
std::string link_names[2]
The object link names.
Definition: include/moveit/collision_detection/collision_common.h:348
collision_detection::DistanceResultsData::clear
void clear()
Clear structure data.
Definition: include/moveit/collision_detection/collision_common.h:361
collision_detection::DistanceRequest::type
DistanceRequestType type
Definition: include/moveit/collision_detection/collision_common.h:307
collision_detection::CostSource::getVolume
double getVolume() const
Get the volume of the AABB around the cost source.
Definition: include/moveit/collision_detection/collision_common.h:154
collision_detection::DistanceRequestTypes::GLOBAL
@ GLOBAL
Find the global minimum.
Definition: include/moveit/collision_detection/collision_common.h:265
collision_detection::DistanceRequest::verbose
bool verbose
Log debug information.
Definition: include/moveit/collision_detection/collision_common.h:326
collision_detection::DistanceResult::clear
void clear()
Clear structure data.
Definition: include/moveit/collision_detection/collision_common.h:406
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
collision_detection::DistanceRequest::distance_threshold
double distance_threshold
Definition: include/moveit/collision_detection/collision_common.h:323
robot_model.h
collision_detection::CostSource
When collision costs are computed, this structure contains information about the partial cost incurre...
Definition: include/moveit/collision_detection/collision_common.h:142
collision_detection::CollisionRequest::is_done
boost::function< bool(const CollisionResult &)> is_done
Function call that decides whether collision detection should stop.
Definition: include/moveit/collision_detection/collision_common.h:255
collision_detection::Contact::body_name_2
std::string body_name_2
The id of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:125
collision_detection::DistanceResultsData::DistanceResultsData
DistanceResultsData()
Definition: include/moveit/collision_detection/collision_common.h:336
collision_detection::DistanceRequestTypes::ALL
@ ALL
Find all the contacts for a given pair.
Definition: include/moveit/collision_detection/collision_common.h:268
collision_detection::Contact::body_type_2
BodyType body_type_2
The type of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:128
collision_detection::CollisionRequest::distance
bool distance
If true, compute proximity distance.
Definition: include/moveit/collision_detection/collision_common.h:236
collision_detection::DistanceResultsData::operator<
bool operator<(const DistanceResultsData &other)
Compare if the distance is less than another.
Definition: include/moveit/collision_detection/collision_common.h:374
collision_detection::DistanceResultsData::body_types
BodyType body_types[2]
The object body type.
Definition: include/moveit/collision_detection/collision_common.h:351
collision_detection::DistanceResultsData::normal
Eigen::Vector3d normal
Definition: include/moveit/collision_detection/collision_common.h:358
collision_detection::Contact::body_name_1
std::string body_name_1
The id of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:119
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
collision_detection::Contact::nearest_points
Eigen::Vector3d nearest_points[2]
The two nearest points connecting the two bodies.
Definition: include/moveit/collision_detection/collision_common.h:137
collision_detection::CostSource::aabb_max
boost::array< double, 3 > aabb_max
The maximum bound of the AABB defining the volume responsible for this partial cost.
Definition: include/moveit/collision_detection/collision_common.h:148
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
collision_detection::BodyType
BodyTypes::Type BodyType
The types of bodies that are considered for collision.
Definition: include/moveit/collision_detection/collision_common.h:102
collision_detection::CollisionRequest::max_contacts
std::size_t max_contacts
Overall maximum number of contacts to compute.
Definition: include/moveit/collision_detection/collision_common.h:245
collision_detection::CollisionRequest::verbose
bool verbose
Flag indicating whether information about detected collisions should be reported.
Definition: include/moveit/collision_detection/collision_common.h:258
collision_detection::CollisionRequest::group_name
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)
Definition: include/moveit/collision_detection/collision_common.h:233
collision_detection::DistanceRequest::acm
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
Definition: include/moveit/collision_detection/collision_common.h:319
collision_detection::DistanceResult::collision
bool collision
Indicates if two objects were in collision.
Definition: include/moveit/collision_detection/collision_common.h:397
collision_detection::DistanceRequest::group_name
std::string group_name
The group name.
Definition: include/moveit/collision_detection/collision_common.h:313
collision_detection::DistanceResultsData
Generic representation of the distance information for a pair of objects.
Definition: include/moveit/collision_detection/collision_common.h:334
collision_detection::BodyTypes::ROBOT_ATTACHED
@ ROBOT_ATTACHED
A body attached to a robot link.
Definition: include/moveit/collision_detection/collision_common.h:94
collision_detection::CostSource::operator<
bool operator<(const CostSource &other) const
Order cost sources so that the most costly source is at the top.
Definition: include/moveit/collision_detection/collision_common.h:160
collision_detection::BodyTypes::ROBOT_LINK
@ ROBOT_LINK
A link on the robot.
Definition: include/moveit/collision_detection/collision_common.h:91
collision_detection::CollisionRequest::CollisionRequest
CollisionRequest()
Definition: include/moveit/collision_detection/collision_common.h:218
collision_detection::Contact::percent_interpolation
double percent_interpolation
The distance percentage between casted poses until collision.
Definition: include/moveit/collision_detection/collision_common.h:134
collision_detection::CollisionResult::contacts
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
Definition: include/moveit/collision_detection/collision_common.h:209
collision_detection::DistanceResultsData::nearest_points
Eigen::Vector3d nearest_points[2]
The nearest points.
Definition: include/moveit/collision_detection/collision_common.h:345
collision_detection::BodyTypes::Type
Type
The types of bodies that are considered for collision.
Definition: include/moveit/collision_detection/collision_common.h:88
collision_detection::Contact::pos
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
Definition: include/moveit/collision_detection/collision_common.h:110
collision_detection::DistanceResult::minimum_distance
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
Definition: include/moveit/collision_detection/collision_common.h:400
collision_detection::DistanceResult::distances
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
Definition: include/moveit/collision_detection/collision_common.h:403
collision_detection::DistanceRequestTypes::SINGLE
@ SINGLE
Find the global minimum for each pair.
Definition: include/moveit/collision_detection/collision_common.h:266
collision_detection::DistanceMap
std::map< const std::pair< std::string, std::string >, std::vector< DistanceResultsData > > DistanceMap
Mapping between the names of the collision objects and the DistanceResultData.
Definition: include/moveit/collision_detection/collision_common.h:387
std
collision_detection::DistanceRequestTypes::DistanceRequestType
DistanceRequestType
Definition: include/moveit/collision_detection/collision_common.h:263
collision_detection::DistanceResult
Result of a distance request.
Definition: include/moveit/collision_detection/collision_common.h:390
collision_detection::CollisionRequest::contacts
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
Definition: include/moveit/collision_detection/collision_common.h:242
collision_detection::Contact::normal
Eigen::Vector3d normal
normal unit vector at contact
Definition: include/moveit/collision_detection/collision_common.h:113
collision_detection::CollisionRequest::max_cost_sources
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned.
Definition: include/moveit/collision_detection/collision_common.h:252
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:200
collision_detection::CollisionResult::cost_sources
std::set< CostSource > cost_sources
These are the individual cost sources when costs are computed.
Definition: include/moveit/collision_detection/collision_common.h:212
collision_detection::DistanceRequest::enable_nearest_points
bool enable_nearest_points
Indicate if nearest point information should be calculated.
Definition: include/moveit/collision_detection/collision_common.h:299
collision_detection::DistanceResultsData::distance
double distance
The distance between two objects. If two objects are in collision, distance <= 0.
Definition: include/moveit/collision_detection/collision_common.h:342
collision_detection::CollisionRequest::max_contacts_per_pair
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...
Definition: include/moveit/collision_detection/collision_common.h:249
collision_detection::CollisionResult::print
void print() const
Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level...
Definition: src/collision_common.cpp:42
collision_detection::CollisionResult::clear
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
Definition: include/moveit/collision_detection/collision_common.h:187
collision_detection::DistanceRequest::compute_gradient
bool compute_gradient
Definition: include/moveit/collision_detection/collision_common.h:330
collision_detection::DistanceRequest
Representation of a distance-reporting request.
Definition: include/moveit/collision_detection/collision_common.h:274
collision_detection::CollisionResult::contact_count
std::size_t contact_count
Number of contacts returned.
Definition: include/moveit/collision_detection/collision_common.h:206
collision_detection::DistanceRequest::enable_signed_distance
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
Definition: include/moveit/collision_detection/collision_common.h:302
collision_detection::DistanceRequest::enableGroup
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on req_.
Definition: include/moveit/collision_detection/collision_common.h:290
collision_detection::CollisionResult::distance
double distance
Closest distance between two bodies.
Definition: include/moveit/collision_detection/collision_common.h:203
collision_detection::DistanceResult::DistanceResult
DistanceResult()
Definition: include/moveit/collision_detection/collision_common.h:392
collision_detection::DistanceRequest::active_components_only
const std::set< const moveit::core::LinkModel * > * active_components_only
The set of active components to check.
Definition: include/moveit/collision_detection/collision_common.h:316
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Oct 12 2021 02:24:44