contact_checker_common.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (Apache License)
3  *
4  * Copyright (c) 2017, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Licensed under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17  *********************************************************************/
18 
19 /* Author: Levi Armstrong */
20 
21 #pragma once
22 
23 #include <LinearMath/btConvexHullComputer.h>
24 #include <cstdio>
25 #include <Eigen/Geometry>
26 #include <fstream>
27 
31 
33 {
40 inline std::pair<std::string, std::string> getObjectPairKey(const std::string& obj1, const std::string& obj2)
41 {
42  return obj1 < obj2 ? std::make_pair(obj1, obj2) : std::make_pair(obj2, obj1);
43 }
44 
50 inline bool isLinkActive(const std::vector<std::string>& active, const std::string& name)
51 {
52  return active.empty() || (std::find(active.begin(), active.end(), name) != active.end());
53 }
54 
58 inline collision_detection::Contact* processResult(ContactTestData& cdata, collision_detection::Contact& contact,
59  const std::pair<std::string, std::string>& key, bool found)
60 {
61  // add deepest penetration / smallest distance to result
62  if (cdata.req.distance)
63  {
64  if (contact.depth < cdata.res.distance)
65  {
66  cdata.res.distance = contact.depth;
67  }
68  }
69 
70  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet",
71  "Contact btw " << key.first << " and " << key.second << " dist: " << contact.depth);
72  // case if pair hasn't a contact yet
73  if (!found)
74  {
75  if (contact.depth <= 0)
76  {
77  cdata.res.collision = true;
78  }
79 
80  std::vector<collision_detection::Contact> data;
81 
82  // if we dont want contacts we are done here
83  if (!cdata.req.contacts)
84  {
85  if (!cdata.req.distance)
86  {
87  cdata.done = true;
88  }
89  return nullptr;
90  }
91  else
92  {
93  data.reserve(cdata.req.max_contacts_per_pair);
94  data.emplace_back(contact);
95  cdata.res.contact_count++;
96  }
97 
98  if (cdata.res.contact_count >= cdata.req.max_contacts)
99  {
100  if (!cdata.req.distance)
101  {
102  cdata.done = true;
103  }
104  }
105 
106  if (cdata.req.max_contacts_per_pair == 1u)
107  {
108  cdata.pair_done = true;
109  }
110 
111  return &(cdata.res.contacts.insert(std::make_pair(key, data)).first->second.back());
112  }
113  else
114  {
115  std::vector<collision_detection::Contact>& dr = cdata.res.contacts[key];
116  dr.emplace_back(contact);
117  cdata.res.contact_count++;
118 
119  if (dr.size() >= cdata.req.max_contacts_per_pair)
120  {
121  cdata.pair_done = true;
122  }
123 
124  if (cdata.res.contact_count >= cdata.req.max_contacts)
125  {
126  if (!cdata.req.distance)
127  {
128  cdata.done = true;
129  }
130  }
131 
132  return &(dr.back());
133  }
134 
135  return nullptr;
136 }
137 
150 inline int createConvexHull(AlignedVector<Eigen::Vector3d>& vertices, std::vector<int>& faces,
151  const AlignedVector<Eigen::Vector3d>& input, double shrink = -1, double shrinkClamp = -1)
152 {
153  vertices.clear();
154  faces.clear();
155 
156  btConvexHullComputer conv;
157  btAlignedObjectArray<btVector3> points;
158  points.reserve(static_cast<int>(input.size()));
159  for (const Eigen::Vector3d& v : input)
160  {
161  points.push_back(btVector3(static_cast<btScalar>(v[0]), static_cast<btScalar>(v[1]), static_cast<btScalar>(v[2])));
162  }
163 
164  btScalar val = conv.compute(&points[0].getX(), sizeof(btVector3), points.size(), static_cast<btScalar>(shrink),
165  static_cast<btScalar>(shrinkClamp));
166  if (val < 0)
167  {
168  ROS_ERROR("Failed to create convex hull");
169  return -1;
170  }
171 
172  int num_verts = conv.vertices.size();
173  vertices.reserve(static_cast<size_t>(num_verts));
174  for (int i = 0; i < num_verts; i++)
175  {
176  btVector3& v = conv.vertices[i];
177  vertices.push_back(Eigen::Vector3d(v.getX(), v.getY(), v.getZ()));
178  }
179 
180  int num_faces = conv.faces.size();
181  faces.reserve(static_cast<size_t>(3 * num_faces));
182  for (int i = 0; i < num_faces; i++)
183  {
184  std::vector<int> face;
185  face.reserve(3);
186 
187  const btConvexHullComputer::Edge* source_edge = &(conv.edges[conv.faces[i]]);
188  int a = source_edge->getSourceVertex();
189  face.push_back(a);
190 
191  int b = source_edge->getTargetVertex();
192  face.push_back(b);
193 
194  const btConvexHullComputer::Edge* edge = source_edge->getNextEdgeOfFace();
195  int c = edge->getTargetVertex();
196  face.push_back(c);
197 
198  edge = edge->getNextEdgeOfFace();
199  c = edge->getTargetVertex();
200  while (c != a)
201  {
202  face.push_back(c);
203 
204  edge = edge->getNextEdgeOfFace();
205  c = edge->getTargetVertex();
206  }
207  faces.push_back(static_cast<int>(face.size()));
208  faces.insert(faces.end(), face.begin(), face.end());
209  }
210 
211  return num_faces;
212 }
213 
214 } // namespace collision_detection_bullet
collision_detection_bullet
Definition: basic_types.h:34
collision_detection::Contact::depth
double depth
depth (penetration between bodies)
Definition: include/moveit/collision_detection/collision_common.h:116
obj2
CollisionObject< S > * obj2
basic_types.h
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
collision_detection_bullet::getObjectPairKey
std::pair< std::string, std::string > getObjectPairKey(const std::string &obj1, const std::string &obj2)
Get a key for two object to search the collision matrix.
Definition: contact_checker_common.h:56
obj1
CollisionObject< S > * obj1
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
collision_common.h
collision_matrix.h
ROS_ERROR
#define ROS_ERROR(...)
collision_detection_bullet::isLinkActive
bool isLinkActive(const std::vector< std::string > &active, const std::string &name)
This will check if a link is active provided a list. If the list is empty the link is considered acti...
Definition: contact_checker_common.h:66
collision_detection_bullet::createConvexHull
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.
Definition: contact_checker_common.h:166
collision_detection_bullet::processResult
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.
Definition: contact_checker_common.h:74
std
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89


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