moveit2
The MoveIt Motion Planning Framework for ROS 2.
contact_checker_common.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD-2-Clause)
3  *
4  * Copyright (c) 2021, Southwest Research Institute
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  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
28  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  *********************************************************************/
31 
32 /* Author: Jorge Nicho
33  * Author: Levi Armstrong
34  */
35 
37 
38 #include <rclcpp/logger.hpp>
39 #include <rclcpp/logging.hpp>
40 
41 static const rclcpp::Logger BULLET_LOGGER = rclcpp::get_logger("collision_detection.bullet");
42 
44 {
46  const std::pair<std::string, std::string>& key, bool found)
47 {
48  // add deepest penetration / smallest distance to result
49  if (cdata.req.distance)
50  {
51  if (contact.depth < cdata.res.distance)
52  {
53  cdata.res.distance = contact.depth;
54  }
55  }
56 
57  RCLCPP_DEBUG_STREAM(BULLET_LOGGER, "Contact btw " << key.first << " and " << key.second << " dist: " << contact.depth);
58  // case if pair hasn't a contact yet
59  if (!found)
60  {
61  if (contact.depth <= 0)
62  {
63  cdata.res.collision = true;
64  }
65 
66  std::vector<collision_detection::Contact> data;
67 
68  // if we don't want contacts we are done here
69  if (!cdata.req.contacts)
70  {
71  if (!cdata.req.distance)
72  {
73  cdata.done = true;
74  }
75  return nullptr;
76  }
77  else
78  {
79  data.reserve(cdata.req.max_contacts_per_pair);
80  data.emplace_back(contact);
81  cdata.res.contact_count++;
82  }
83 
84  if (cdata.res.contact_count >= cdata.req.max_contacts)
85  {
86  if (!cdata.req.distance)
87  {
88  cdata.done = true;
89  }
90  }
91 
92  if (cdata.req.max_contacts_per_pair == 1u)
93  {
94  cdata.pair_done = true;
95  }
96 
97  return &(cdata.res.contacts.insert(std::make_pair(key, data)).first->second.back());
98  }
99  else
100  {
101  std::vector<collision_detection::Contact>& dr = cdata.res.contacts[key];
102  dr.emplace_back(contact);
103  cdata.res.contact_count++;
104 
105  if (dr.size() >= cdata.req.max_contacts_per_pair)
106  {
107  cdata.pair_done = true;
108  }
109 
110  if (cdata.res.contact_count >= cdata.req.max_contacts)
111  {
112  if (!cdata.req.distance)
113  {
114  cdata.done = true;
115  }
116  }
117 
118  return &(dr.back());
119  }
120 
121  return nullptr;
122 }
123 
124 int createConvexHull(AlignedVector<Eigen::Vector3d>& vertices, std::vector<int>& faces,
125  const AlignedVector<Eigen::Vector3d>& input, double shrink, double shrinkClamp)
126 {
127  vertices.clear();
128  faces.clear();
129 
130  btConvexHullComputer conv;
131  btAlignedObjectArray<btVector3> points;
132  points.reserve(static_cast<int>(input.size()));
133  for (const Eigen::Vector3d& v : input)
134  {
135  points.push_back(btVector3(static_cast<btScalar>(v[0]), static_cast<btScalar>(v[1]), static_cast<btScalar>(v[2])));
136  }
137 
138  btScalar val = conv.compute(&points[0].getX(), sizeof(btVector3), points.size(), static_cast<btScalar>(shrink),
139  static_cast<btScalar>(shrinkClamp));
140  if (val < 0)
141  {
142  RCLCPP_ERROR(BULLET_LOGGER, "Failed to create convex hull");
143  return -1;
144  }
145 
146  int num_verts = conv.vertices.size();
147  vertices.reserve(static_cast<size_t>(num_verts));
148  for (int i = 0; i < num_verts; ++i)
149  {
150  btVector3& v = conv.vertices[i];
151  vertices.push_back(Eigen::Vector3d(v.getX(), v.getY(), v.getZ()));
152  }
153 
154  int num_faces = conv.faces.size();
155  faces.reserve(static_cast<size_t>(3 * num_faces));
156  for (int i = 0; i < num_faces; ++i)
157  {
158  std::vector<int> face;
159  face.reserve(3);
160 
161  const btConvexHullComputer::Edge* source_edge = &(conv.edges[conv.faces[i]]);
162  int a = source_edge->getSourceVertex();
163  face.push_back(a);
164 
165  int b = source_edge->getTargetVertex();
166  face.push_back(b);
167 
168  const btConvexHullComputer::Edge* edge = source_edge->getNextEdgeOfFace();
169  int c = edge->getTargetVertex();
170  face.push_back(c);
171 
172  edge = edge->getNextEdgeOfFace();
173  c = edge->getTargetVertex();
174  while (c != a)
175  {
176  face.push_back(c);
177 
178  edge = edge->getNextEdgeOfFace();
179  c = edge->getTargetVertex();
180  }
181  faces.push_back(static_cast<int>(face.size()));
182  faces.insert(faces.end(), face.begin(), face.end());
183  }
184 
185  return num_faces;
186 }
187 
188 } // namespace collision_detection_bullet
collision_detection::Contact * processResult(ContactTestData &cdata, collision_detection::Contact &contact, const std::pair< std::string, std::string > &key, bool found)
Stores a single contact result in the requested way.
std::vector< T, Eigen::aligned_allocator< T > > AlignedVector
Definition: basic_types.h:37
int createConvexHull(AlignedVector< Eigen::Vector3d > &vertices, std::vector< int > &faces, const AlignedVector< Eigen::Vector3d > &input, double shrink=-1, double shrinkClamp=-1)
Create a convex hull from vertices using Bullet Convex Hull Computer.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
a
Definition: plan.py:54
const rclcpp::Logger BULLET_LOGGER
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
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.
double distance
Closest distance between two bodies.
bool collision
True if collision was found, false otherwise.
std::size_t contact_count
Number of contacts returned.
Definition of a contact point.
double depth
depth (penetration between bodies)
Bundles the data for a collision query.
Definition: basic_types.h:59
const collision_detection::CollisionRequest & req
Definition: basic_types.h:74
bool done
Indicates if search is finished.
Definition: basic_types.h:77
collision_detection::CollisionResult & res
Definition: basic_types.h:73
bool pair_done
Indicates if search between a single pair is finished.
Definition: basic_types.h:80