moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
collision_common.hpp
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 <array>
40#include <functional>
41#include <vector>
42#include <string>
43#include <map>
44#include <set>
45#include <Eigen/Core>
47
48namespace collision_detection
49{
50MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix); // Defines AllowedCollisionMatrixPtr, ConstPtr, WeakPtr... etc
51
53namespace BodyTypes
54{
67} // namespace BodyTypes
68
71
73struct Contact
74{
76
78 Eigen::Vector3d pos;
79
81 Eigen::Vector3d normal;
82
84 double depth = 0.0;
85
87 std::string body_name_1;
88
91
93 std::string body_name_2;
94
97
103
105 Eigen::Vector3d nearest_points[2];
106};
107
111{
113 std::array<double, 3> aabb_min;
114
116 std::array<double, 3> aabb_max;
117
119 double cost = 0.0;
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
144struct CollisionResult;
145
148{
151 std::string group_name = "";
152
155
158
160 bool distance = false;
161
163 bool detailed_distance = false;
164
166 bool cost = false;
167
169 bool contacts = false;
170
172 std::size_t max_contacts = 1;
173
176 std::size_t max_contacts_per_pair = 1;
177
179 std::size_t max_cost_sources = 1;
180
182 std::function<bool(const CollisionResult&)> is_done = nullptr;
183
185 bool verbose = false;
186};
187
188namespace DistanceRequestTypes
189{
197}
199
202{
203 /*** \brief Compute \e active_components_only_ based on \e req_. This
204 includes links that are in the kinematic model but outside this group, if those links are descendants of
205 joints in this group that have their values updated. */
206 void enableGroup(const moveit::core::RobotModelConstPtr& robot_model)
207 {
208 if (robot_model->hasJointModelGroup(group_name))
209 {
210 active_components_only = &robot_model->getJointModelGroup(group_name)->getUpdatedLinkModelsSet();
211 }
212 else
213 {
214 active_components_only = nullptr;
215 }
216 }
217
220
223
227 DistanceRequestType type = DistanceRequestType::GLOBAL;
228
230 std::size_t max_contacts_per_body = 1;
231
233 std::string group_name = "";
234
236 const std::set<const moveit::core::LinkModel*>* active_components_only = nullptr;
237
239 const AllowedCollisionMatrix* acm = nullptr;
240
243 double distance_threshold = std::numeric_limits<double>::max();
244
246 bool verbose = false;
247
250 bool compute_gradient = false;
251};
252
254// TODO(#267): Enable check - for some reason clang-tidy wants to rename this struct to "i0"
255struct DistanceResultsData // NOLINT(readability-identifier-naming) - suppress spurious clang-tidy warning
256{
258 {
259 clear();
260 }
261
263 double distance;
264
266
268 Eigen::Vector3d nearest_points[2];
269
271 std::string link_names[2];
272
275
281 Eigen::Vector3d normal;
282
284 void clear()
285 {
286 distance = std::numeric_limits<double>::max();
287 nearest_points[0].setZero();
288 nearest_points[1].setZero();
289 body_types[0] = BodyType::WORLD_OBJECT;
290 body_types[1] = BodyType::WORLD_OBJECT;
291 link_names[0] = "";
292 link_names[1] = "";
293 normal.setZero();
294 }
295
298 {
299 return (distance < other.distance);
300 }
301
304 {
305 return (distance > other.distance);
306 }
307};
308
310using DistanceMap = std::map<const std::pair<std::string, std::string>, std::vector<DistanceResultsData> >;
311
314{
316 bool collision = false;
317
320
323
325 void clear()
326 {
327 collision = false;
329 distances.clear();
330 }
331};
332
335{
337
339 void clear()
340 {
341 collision = false;
342 distance = std::numeric_limits<double>::max();
344 contact_count = 0;
345 contacts.clear();
346 cost_sources.clear();
347 }
348
350 void print() const;
351
353 bool collision = false;
354
356 double distance = std::numeric_limits<double>::max();
357
360
362 std::size_t contact_count = 0;
363
365 using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
367
369 std::set<CostSource> cost_sources;
370};
371} // namespace collision_detection
ROS/KDL based interface for the inverse kinematics of the PR2 arm.
#define MOVEIT_CLASS_FORWARD(C)
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Type
The types of bodies that are considered for collision.
@ ROBOT_ATTACHED
A body attached to a robot link.
@ WORLD_OBJECT
A body in the environment.
@ ALL
Find all the contacts for a given pair.
@ LIMITED
Find a limited(max_contacts_per_body) set of contacts for a given pair.
@ SINGLE
Find the global minimum for each pair.
std::map< const std::pair< std::string, std::string >, std::vector< DistanceResultsData > > DistanceMap
Mapping between the names of the collision objects and the DistanceResultData.
Representation of a collision checking request.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)....
bool pad_self_collisions
If true, do self collision check with padded robot links.
bool verbose
Flag indicating whether information about detected collisions should be reported.
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
bool pad_environment_collisions
If true, use padded collision environment.
bool cost
If true, a collision cost is computed.
std::size_t max_contacts
Overall maximum number of contacts to compute.
std::function< bool(const CollisionResult &)> is_done
Function call that decides whether collision detection should stop.
bool detailed_distance
If true, return detailed distance information. Distance must be set to true as well.
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...
bool distance
If true, compute proximity distance.
Representation of a collision checking result.
void print() const
Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level...
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
A map returning the pairs of body ids in contact, plus their contact details.
DistanceResult distance_result
Distance data for each link.
std::set< CostSource > cost_sources
These are the individual cost sources when costs are computed.
double distance
Closest distance between two bodies.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
bool collision
True if collision was found, false otherwise.
std::size_t contact_count
Number of contacts returned.
Definition of a contact point.
std::string body_name_2
The id of the second body involved in the contact.
double percent_interpolation
The distance percentage between casted poses until collision.
Eigen::Vector3d nearest_points[2]
The two nearest points connecting the two bodies.
std::string body_name_1
The id of the first body involved in the contact.
BodyType body_type_1
The type of the first body involved in the contact.
BodyType body_type_2
The type of the second body involved in the contact.
Eigen::Vector3d normal
normal unit vector at contact
double depth
depth (penetration between bodies)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
When collision costs are computed, this structure contains information about the partial cost incurre...
double getVolume() const
Get the volume of the AABB around the cost source.
std::array< double, 3 > aabb_max
The maximum bound of the AABB defining the volume responsible for this partial cost.
double cost
The partial cost (the probability of existence for the object there is a collision with)
std::array< double, 3 > aabb_min
The minimum bound of the AABB defining the volume responsible for this partial cost.
bool operator<(const CostSource &other) const
Order cost sources so that the most costly source is at the top.
Representation of a distance-reporting request.
const std::set< const moveit::core::LinkModel * > * active_components_only
The set of active components to check.
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
std::string group_name
The group name.
std::size_t max_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold)
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
bool enable_nearest_points
Indicate if nearest point information should be calculated.
Result of a distance request.
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
bool collision
Indicates if two objects were in collision.
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
void clear()
Clear structure data.
Generic representation of the distance information for a pair of objects.
BodyType body_types[2]
The object body type.
bool operator<(const DistanceResultsData &other)
Compare if the distance is less than another.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d nearest_points[2]
The nearest points.
bool operator>(const DistanceResultsData &other)
Compare if the distance is greater than another.
std::string link_names[2]
The object link names.
double distance
The distance between two objects. If two objects are in collision, distance <= 0.