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 {
75  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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 
144 struct CollisionResult;
145 
147 struct CollisionRequest
148 {
150  : distance(false)
152  , cost(false)
153  , contacts(false)
156  , max_cost_sources(1)
157  , verbose(false)
158  {
159  }
161  {
162  }
163 
165  std::string group_name;
166 
168  bool distance;
169 
171  bool detailed_distance;
172 
174  bool cost;
175 
177  bool contacts;
178 
180  std::size_t max_contacts;
181 
184  std::size_t max_contacts_per_pair;
185 
187  std::size_t max_cost_sources;
188 
190  boost::function<bool(const CollisionResult&)> is_done;
191 
193  bool verbose;
194 };
195 
196 namespace DistanceRequestTypes
197 {
199 {
201  SINGLE,
202  LIMITED,
204 };
205 }
207 
210 {
213  , enable_signed_distance(false)
217  , acm(nullptr)
218  , distance_threshold(std::numeric_limits<double>::max())
219  , verbose(false)
220  , compute_gradient(false)
221  {
222  }
223 
225  void enableGroup(const moveit::core::RobotModelConstPtr& robot_model)
226  {
227  if (robot_model->hasJointModelGroup(group_name))
228  active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet();
229  else
231  }
232 
235 
238 
243 
245  std::size_t max_contacts_per_body;
246 
248  std::string group_name;
249 
251  const std::set<const moveit::core::LinkModel*>* active_components_only;
252 
255 
258  double distance_threshold;
259 
261  bool verbose;
262 
265  bool compute_gradient;
266 };
267 
269 struct DistanceResultsData // NOLINT(readability-identifier-naming) - suppress spurious clang-tidy warning
270 {
272  {
273  clear();
274  }
275 
277  double distance;
278 
281 
283  std::string link_names[2];
284 
287 
294 
296  void clear()
297  {
298  distance = std::numeric_limits<double>::max();
299  nearest_points[0].setZero();
300  nearest_points[1].setZero();
303  link_names[0] = "";
304  link_names[1] = "";
305  normal.setZero();
306  }
307 
309  bool operator<(const DistanceResultsData& other)
310  {
311  return (distance < other.distance);
312  }
313 
315  bool operator>(const DistanceResultsData& other)
316  {
317  return (distance > other.distance);
318  }
319 };
320 
322 using DistanceMap = std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> >;
323 
326 {
327  DistanceResult() : collision(false)
328  {
329  }
330 
332  bool collision;
333 
336 
339 
341  void clear()
342  {
343  collision = false;
345  distances.clear();
346  }
347 };
348 
350 struct CollisionResult
351 {
352  CollisionResult() : collision(false), distance(std::numeric_limits<double>::max()), contact_count(0)
353  {
354  }
355  using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
356 
357  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
358 
360  void clear()
361  {
362  collision = false;
363  distance = std::numeric_limits<double>::max();
365  contact_count = 0;
366  contacts.clear();
367  cost_sources.clear();
368  }
369 
371  void print() const;
372 
374  bool collision;
375 
377  double distance;
378 
381 
383  std::size_t contact_count;
384 
387 
389  std::set<CostSource> cost_sources;
390 };
391 } // 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:347
collision_detection::CollisionRequest::cost
bool cost
If true, a collision cost is computed.
Definition: include/moveit/collision_detection/collision_common.h:206
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:387
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:243
collision_detection::CollisionRequest::~CollisionRequest
virtual ~CollisionRequest()
Definition: include/moveit/collision_detection/collision_common.h:192
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:384
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:277
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:234
collision_detection::CollisionResult::distance_result
DistanceResult distance_result
Distance data for each link.
Definition: include/moveit/collision_detection/collision_common.h:412
collision_detection::DistanceResultsData::link_names
std::string link_names[2]
The object link names.
Definition: include/moveit/collision_detection/collision_common.h:315
collision_detection::DistanceResultsData::clear
void clear()
Clear structure data.
Definition: include/moveit/collision_detection/collision_common.h:328
collision_detection::DistanceRequest::type
DistanceRequestType type
Definition: include/moveit/collision_detection/collision_common.h:274
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:232
collision_detection::DistanceRequest::verbose
bool verbose
Log debug information.
Definition: include/moveit/collision_detection/collision_common.h:293
collision_detection::DistanceResult::clear
void clear()
Clear structure data.
Definition: include/moveit/collision_detection/collision_common.h:373
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:179
collision_detection::DistanceRequest::distance_threshold
double distance_threshold
Definition: include/moveit/collision_detection/collision_common.h:290
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:222
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:303
collision_detection::DistanceRequestTypes::ALL
@ ALL
Find all the contacts for a given pair.
Definition: include/moveit/collision_detection/collision_common.h:235
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:200
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:341
collision_detection::DistanceResultsData::body_types
BodyType body_types[2]
The object body type.
Definition: include/moveit/collision_detection/collision_common.h:318
collision_detection::DistanceResultsData::normal
Eigen::Vector3d normal
Definition: include/moveit/collision_detection/collision_common.h:325
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.
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:382
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:212
collision_detection::CollisionRequest::verbose
bool verbose
Flag indicating whether information about detected collisions should be reported.
Definition: include/moveit/collision_detection/collision_common.h:225
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:197
collision_detection::DistanceRequest::acm
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
Definition: include/moveit/collision_detection/collision_common.h:286
collision_detection::DistanceResult::collision
bool collision
Indicates if two objects were in collision.
Definition: include/moveit/collision_detection/collision_common.h:364
collision_detection::DistanceRequest::group_name
std::string group_name
The group name.
Definition: include/moveit/collision_detection/collision_common.h:280
collision_detection::DistanceResultsData
Generic representation of the distance information for a pair of objects.
Definition: include/moveit/collision_detection/collision_common.h:301
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::CollisionRequest::detailed_distance
bool detailed_distance
If true, return detailed distance information. Distance must be set to true as well.
Definition: include/moveit/collision_detection/collision_common.h:203
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:181
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:418
collision_detection::DistanceResultsData::nearest_points
Eigen::Vector3d nearest_points[2]
The nearest points.
Definition: include/moveit/collision_detection/collision_common.h:312
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:367
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:370
collision_detection::DistanceRequestTypes::SINGLE
@ SINGLE
Find the global minimum for each pair.
Definition: include/moveit/collision_detection/collision_common.h:233
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:354
std
collision_detection::DistanceRequestTypes::DistanceRequestType
DistanceRequestType
Definition: include/moveit/collision_detection/collision_common.h:230
collision_detection::DistanceResult
Result of a distance request.
Definition: include/moveit/collision_detection/collision_common.h:357
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:209
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:219
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:406
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:421
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:266
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:309
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:216
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:392
collision_detection::DistanceRequest::compute_gradient
bool compute_gradient
Definition: include/moveit/collision_detection/collision_common.h:297
collision_detection::DistanceRequest
Representation of a distance-reporting request.
Definition: include/moveit/collision_detection/collision_common.h:241
collision_detection::CollisionResult::contact_count
std::size_t contact_count
Number of contacts returned.
Definition: include/moveit/collision_detection/collision_common.h:415
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:269
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:257
collision_detection::CollisionResult::distance
double distance
Closest distance between two bodies.
Definition: include/moveit/collision_detection/collision_common.h:409
collision_detection::DistanceResult::DistanceResult
DistanceResult()
Definition: include/moveit/collision_detection/collision_common.h:359
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:283
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 Sat Jul 6 2024 02:26:31