Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
collision_sphere_sphere_unit.hpp
Go to the documentation of this file.
1#ifndef TESSERACT_COLLISION_COLLISION_SPHERE_SPHERE_UNIT_HPP
2#define TESSERACT_COLLISION_COLLISION_SPHERE_SPHERE_UNIT_HPP
3
4#include <tesseract_collision/bullet/convex_hull_utils.h>
5#include <tesseract_collision/core/discrete_contact_manager.h>
6#include <tesseract_collision/core/common.h>
7#include <tesseract_geometry/geometries.h>
8
9namespace tesseract_collision::test_suite
10{
11namespace detail
12{
13inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex_mesh = false)
14{
16 // Add sphere to checker
18 CollisionShapePtr sphere;
19 if (use_convex_mesh)
20 {
21 auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
22 auto mesh_faces = std::make_shared<Eigen::VectorXi>();
23 EXPECT_GT(loadSimplePlyFile(
24 std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true),
25 0);
26
27 auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
28 sphere = makeConvexMesh(*mesh);
29 }
30 else
31 {
32 sphere = std::make_shared<tesseract_geometry::Sphere>(0.25);
33 }
34
35 Eigen::Isometry3d sphere_pose;
36 sphere_pose.setIdentity();
37
38 CollisionShapesConst obj1_shapes;
39 tesseract_common::VectorIsometry3d obj1_poses;
40 obj1_shapes.push_back(sphere);
41 obj1_poses.push_back(sphere_pose);
42
43 checker.addCollisionObject("sphere_link", 0, obj1_shapes, obj1_poses);
44
46 // Add thin box to checker which is disabled
48 CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
49 Eigen::Isometry3d thin_box_pose;
50 thin_box_pose.setIdentity();
51
52 CollisionShapesConst obj2_shapes;
53 tesseract_common::VectorIsometry3d obj2_poses;
54 obj2_shapes.push_back(thin_box);
55 obj2_poses.push_back(thin_box_pose);
56
57 checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses, false);
58
60 // Add second sphere to checker. If use_convex_mesh = true
61 // then this sphere will be added as a convex hull mesh.
63 CollisionShapePtr sphere1;
64
65 if (use_convex_mesh)
66 {
67 auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
68 auto mesh_faces = std::make_shared<Eigen::VectorXi>();
69 EXPECT_GT(loadSimplePlyFile(
70 std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true),
71 0);
72
73 auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
74 sphere1 = makeConvexMesh(*mesh);
75 }
76 else
77 {
78 sphere1 = std::make_shared<tesseract_geometry::Sphere>(0.25);
79 }
80
81 Eigen::Isometry3d sphere1_pose;
82 sphere1_pose.setIdentity();
83
84 CollisionShapesConst obj3_shapes;
85 tesseract_common::VectorIsometry3d obj3_poses;
86 obj3_shapes.push_back(sphere1);
87 obj3_poses.push_back(sphere1_pose);
88
89 checker.addCollisionObject("sphere1_link", 0, obj3_shapes, obj3_poses);
90
92 // Add box and remove
94 CollisionShapePtr remove_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
95 Eigen::Isometry3d remove_box_pose;
96 thin_box_pose.setIdentity();
97
98 CollisionShapesConst obj4_shapes;
99 tesseract_common::VectorIsometry3d obj4_poses;
100 obj4_shapes.push_back(remove_box);
101 obj4_poses.push_back(remove_box_pose);
102
103 checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses);
104 EXPECT_TRUE(checker.getCollisionObjects().size() == 4);
105 EXPECT_TRUE(checker.hasCollisionObject("remove_box_link"));
106 checker.removeCollisionObject("remove_box_link");
107 EXPECT_FALSE(checker.hasCollisionObject("remove_box_link"));
108 EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
109
111 // Try functions on a link that does not exist
113 EXPECT_FALSE(checker.removeCollisionObject("link_does_not_exist"));
114 EXPECT_FALSE(checker.enableCollisionObject("link_does_not_exist"));
115 EXPECT_FALSE(checker.disableCollisionObject("link_does_not_exist"));
116
118 // Try to add empty Collision Object
120 EXPECT_FALSE(
121 checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d()));
122 EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
123}
124
125inline void runTestPrimitive(DiscreteContactManager& checker)
126{
128 // Test when object is in collision
130 std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
131 checker.setActiveCollisionObjects(active_links);
132 std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
133 EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
134
135 EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr);
136
137 checker.setCollisionMarginData(CollisionMarginData(0.1));
138 EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
139
140 // Test when object is inside another
141 tesseract_common::TransformMap location;
142 location["sphere_link"] = Eigen::Isometry3d::Identity();
143 location["sphere1_link"] = Eigen::Isometry3d::Identity();
144 location["sphere1_link"].translation()(0) = 0.2;
145 checker.setCollisionObjectsTransform(location);
146
147 // Perform collision check
148 ContactResultMap result;
149 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
150
151 ContactResultVector result_vector;
152 result.flattenMoveResults(result_vector);
153
154 EXPECT_TRUE(!result_vector.empty());
155 EXPECT_NEAR(result_vector[0].distance, -0.30, 0.0001);
156
157 std::vector<int> idx = { 0, 1, 1 };
158 if (result_vector[0].link_names[0] != "sphere_link")
159 idx = { 1, 0, -1 };
160
161 if (result_vector[0].single_contact_point)
162 {
163 EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
164 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.25)) > 0.001 &&
165 std::abs(result_vector[0].nearest_points[0][0] - (-0.05)) > 0.001);
166 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
167 std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
168 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001 &&
169 std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001);
170 }
171 else
172 {
173 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
174 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
175 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
176 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
177 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
178 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
179 }
180
181 EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
182 EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
183 EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.0016); // TODO LEVI: This was increased due to FLC
184 // calculation because it is using GJK
185
187 // Test object is out side the contact distance
189 location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
190 result.clear();
191 result_vector.clear();
192 checker.setCollisionObjectsTransform("sphere1_link", location["sphere1_link"]);
193
194 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
195 result.flattenCopyResults(result_vector);
196
197 EXPECT_TRUE(result_vector.empty());
198
200 // Test object inside the contact distance
202 result.clear();
203 result_vector.clear();
204
205 checker.setCollisionMarginData(CollisionMarginData(0.52));
206 EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.52, 1e-5);
207 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
208 result.flattenMoveResults(result_vector);
209
210 EXPECT_TRUE(!result_vector.empty());
211 EXPECT_NEAR(result_vector[0].distance, 0.5, 0.0001);
212
213 idx = { 0, 1, 1 };
214 if (result_vector[0].link_names[0] != "sphere_link")
215 idx = { 1, 0, -1 };
216
217 if (result_vector[0].single_contact_point)
218 {
219 EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
220 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.25)) > 0.001 &&
221 std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001);
222 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
223 std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
224 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001 &&
225 std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001);
226 }
227 else
228 {
229 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
230 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
231 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
232 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.75, 0.001);
233 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
234 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
235 }
236
237 EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
238 EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
239 EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
240}
241
242inline void runTestConvex1(DiscreteContactManager& checker)
243{
245 // Test when object is in collision (Closest Feature Edge to Edge)
247 std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
248 checker.setActiveCollisionObjects(active_links);
249 std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
250 EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
251
252 EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr);
253
254 checker.setCollisionMarginData(CollisionMarginData(0.1));
255 EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
256
257 // Test when object is inside another
258 tesseract_common::TransformMap location;
259 location["sphere_link"] = Eigen::Isometry3d::Identity();
260 location["sphere1_link"] = Eigen::Isometry3d::Identity();
261 location["sphere1_link"].translation()(0) = 0.2;
262 checker.setCollisionObjectsTransform(location);
263
264 // Perform collision check
265 ContactResultMap result;
266 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
267
268 ContactResultVector result_vector;
269 result.flattenCopyResults(result_vector);
270
271 // Bullet: -0.270548 {0.232874,0,-0.025368} {-0.032874,0,0.025368}
272 // FCL: -0.270548 {0.232874,0,-0.025368} {-0.032874,0,0.025368}
273 EXPECT_TRUE(!result_vector.empty());
274 EXPECT_NEAR(result_vector[0].distance, -0.270548, 0.001);
275
276 std::vector<int> idx = { 0, 1, 1 };
277 if (result_vector[0].link_names[0] != "sphere_link")
278 idx = { 1, 0, -1 };
279
280 if (result_vector[0].single_contact_point)
281 {
282 EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
283 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.23776)) > 0.001 &&
284 std::abs(result_vector[0].nearest_points[0][0] - (-0.032874)) > 0.001);
285 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
286 std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
287 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (-0.025368)) > 0.001 &&
288 std::abs(result_vector[0].nearest_points[0][2] - (0.025368)) > 0.001);
289 }
290 else
291 {
292 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.232874, 0.001);
293 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
294 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], -0.025368, 0.001);
295 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.032874, 0.001);
296 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
297 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.025368, 0.001);
298 }
299
300 EXPECT_GT((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)), 0.0);
301 EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)))), 0.5);
302
304 // Test object is out side the contact distance
306 location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
307 result.clear();
308 result_vector.clear();
309 checker.setCollisionObjectsTransform(location);
310
311 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
312 result.flattenMoveResults(result_vector);
313
314 EXPECT_TRUE(result_vector.empty());
315}
316
317inline void runTestConvex2(DiscreteContactManager& checker)
318{
320 // Test object inside the contact distance (Closest Feature Edge to Edge)
322 ContactResultMap result;
323 ContactResultVector result_vector;
324
325 std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
326 checker.setActiveCollisionObjects(active_links);
327 std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
328 EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
329
330 EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr);
331
332 tesseract_common::TransformMap location;
333 location["sphere_link"] = Eigen::Isometry3d::Identity();
334 location["sphere1_link"] = Eigen::Isometry3d::Identity();
335 location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
336 checker.setCollisionObjectsTransform(location);
337
338 checker.setCollisionMarginData(CollisionMarginData(0.55));
339 EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.55, 1e-5);
340 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
341 result.flattenCopyResults(result_vector);
342
343 // Bullet: 0.524565 {0.237717,0,0} {0.7622825,0,0} Using blender this appears to be the correct result
344 // FCL: 0.546834 {0.237717,-0.0772317,0} {0.7622825,0.0772317}
345 EXPECT_TRUE(!result_vector.empty());
346 EXPECT_NEAR(result_vector[0].distance, 0.52448, 0.001);
347 EXPECT_NEAR(result_vector[0].nearest_points[0][1], result_vector[0].nearest_points[1][1], 0.001);
348 EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
349
350 std::vector<int> idx = { 0, 1, 1 };
351 if (result_vector[0].link_names[0] != "sphere_link")
352 idx = { 1, 0, -1 };
353
354 if (result_vector[0].single_contact_point)
355 {
356 EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
357 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.23776)) > 0.001 &&
358 std::abs(result_vector[0].nearest_points[0][0] - (0.76224)) > 0.001);
359 }
360 else
361 {
362 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.23776, 0.001);
363 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.76224, 0.001);
364 }
365
366 EXPECT_GT((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)), 0.0);
367 EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)))), 0.00001);
368}
369
370inline void runTestConvex3(DiscreteContactManager& checker)
371{
373 // Test when object is in collision (Closest Feature face to edge)
375 std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
376 checker.setActiveCollisionObjects(active_links);
377 std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
378 EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
379
380 EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr);
381
382 checker.setCollisionMarginData(CollisionMarginData(0.1));
383 EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
384
385 tesseract_common::TransformMap location;
386 location["sphere1_link"] = Eigen::Isometry3d::Identity();
387 location["sphere1_link"].translation()(1) = 0.2;
388 checker.setCollisionObjectsTransform(location);
389
390 // Perform collision check
391 ContactResultMap result;
392 ContactResultVector result_vector;
393 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
394 result.flattenMoveResults(result_vector);
395
396 // Bullet: -0.280223 {0.0425563,0.2308753,-0.0263040} {-0.0425563, -0.0308753, 0.0263040}
397 // FCL: -0.280223 {0.0425563,0.2308753,-0.0263040} {-0.0425563, -0.0308753, 0.0263040}
398 EXPECT_TRUE(!result_vector.empty());
399 EXPECT_NEAR(result_vector[0].distance, -0.280223, 0.001);
400
401 std::vector<int> idx = { 0, 1, 1 };
402 if (result_vector[0].link_names[0] != "sphere_link")
403 idx = { 1, 0, -1 };
404
405 if (result_vector[0].single_contact_point)
406 {
407 EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
408 EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.2308753)) > 0.001 &&
409 std::abs(result_vector[0].nearest_points[0][1] - (-0.0308753)) > 0.001);
410 EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0]), 0.0425563, 0.001);
411 EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2]), 0.0263040, 0.001);
412 }
413 else
414 {
415 EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0]), 0.0425563, 0.001);
416 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.2308753, 0.001);
417 EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2]), 0.0263040, 0.001);
418 EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0]), 0.0425563, 0.001);
419 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], -0.0308753, 0.001);
420 EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2]), 0.0263040, 0.001);
421 }
422
423 EXPECT_NEAR(std::abs(idx[2] * result_vector[0].normal[0]), 0.3037316, 0.001);
424 EXPECT_NEAR(idx[2] * result_vector[0].normal[1], 0.9340783, 0.001);
425 EXPECT_NEAR(std::abs(idx[2] * result_vector[0].normal[2]), 0.1877358, 0.001);
426 EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(0, 1, 0)))), 0.4);
427}
428
429inline void runTestConvex(DiscreteContactManager& checker)
430{
431 runTestConvex1(checker);
432 runTestConvex2(checker);
433 runTestConvex3(checker);
434}
435} // namespace detail
436
437inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh)
438{
439 // Add collision objects
440 detail::addCollisionObjects(checker, use_convex_mesh);
441
442 // Call it again to test adding same object
443 detail::addCollisionObjects(checker, use_convex_mesh);
444
445 if (use_convex_mesh)
446 detail::runTestConvex(checker);
447 else
448 detail::runTestPrimitive(checker);
449}
450} // namespace tesseract_collision::test_suite
451
452#endif // TESSERACT_COLLISION_COLLISION_SPHERE_SPHERE_UNIT_HPP
tesseract_collision::ContactResultMap
This structure hold contact results for link pairs.
Definition: types.h:154
tesseract_collision::ContactResultMap::clear
void clear()
This is a consurvative clear.
Definition: types.cpp:183
tesseract_collision::ContactResultMap::flattenMoveResults
void flattenMoveResults(ContactResultVector &v)
Definition: types.cpp:215
tesseract_collision::ContactResultMap::flattenCopyResults
void flattenCopyResults(ContactResultVector &v) const
Definition: types.cpp:227
tesseract_collision::DiscreteContactManager
Definition: discrete_contact_manager.h:41
tesseract_collision::DiscreteContactManager::setActiveCollisionObjects
virtual void setActiveCollisionObjects(const std::vector< std::string > &names)=0
Set which collision objects can move.
tesseract_collision::DiscreteContactManager::contactTest
virtual void contactTest(ContactResultMap &collisions, const ContactRequest &request)=0
Perform a contact test for all objects based.
tesseract_collision::DiscreteContactManager::setCollisionObjectsTransform
virtual void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose)=0
Set a single collision object's transforms.
tesseract_collision::DiscreteContactManager::getActiveCollisionObjects
virtual const std::vector< std::string > & getActiveCollisionObjects() const =0
Get which collision objects can move.
tesseract_collision::DiscreteContactManager::getCollisionMarginData
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
tesseract_collision::DiscreteContactManager::setCollisionMarginData
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data, CollisionMarginOverrideType override_type=CollisionMarginOverrideType::REPLACE)=0
Set the contact distance thresholds for which collision should be considered on a per pair basis.
tesseract_collision::DiscreteContactManager::getIsContactAllowedFn
virtual IsContactAllowedFn getIsContactAllowedFn() const =0
Get the active function for determining if two links are allowed to be in collision.
tesseract_common::CollisionMarginData::getMaxCollisionMargin
double getMaxCollisionMargin() const
Get the largest collision margin.
Definition: collision_margin_data.h:165
EXPECT_NEAR
EXPECT_NEAR(config.contact_manager_config.margin_data.getDefaultCollisionMargin(), 5, 1e-6)
result_vector
tesseract_collision::ContactResultVector result_vector
Definition: collision_core_unit.cpp:410
distance
results distance
Definition: collision_core_unit.cpp:139
link_names
results link_names[0]
Definition: collision_core_unit.cpp:146
normal
results normal
Definition: collision_core_unit.cpp:154
EXPECT_FALSE
EXPECT_FALSE(tesseract_collision::isContactAllowed("base_link", "link_2", acm, false))
nearest_points
results nearest_points[0]
Definition: collision_core_unit.cpp:140
single_contact_point
results single_contact_point
Definition: collision_core_unit.cpp:161
EXPECT_TRUE
EXPECT_TRUE(tesseract_common::isIdentical< tesseract_collision::ObjectPairKey >(pairs, check_pairs, false))
common.h
This is a collection of common methods.
convex_hull_utils.h
This is a collection of common methods.
discrete_contact_manager.h
This is the discrete contact manager base class.
geometries.h
Tesseract Geometries.
EXPECT_GT
EXPECT_GT(m.m.condition, 1e+20)
tesseract_collision::test_suite::detail::runTestConvex3
void runTestConvex3(DiscreteContactManager &checker)
Definition: collision_sphere_sphere_unit.hpp:370
tesseract_collision::test_suite::detail::runTestConvex2
void runTestConvex2(DiscreteContactManager &checker)
Definition: collision_sphere_sphere_unit.hpp:317
tesseract_collision::test_suite::detail::addCollisionObjects
void addCollisionObjects(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:11
tesseract_collision::test_suite::detail::runTestConvex
void runTestConvex(DiscreteContactManager &checker)
Definition: collision_box_sphere_unit.hpp:236
tesseract_collision::test_suite::detail::runTestConvex1
void runTestConvex1(DiscreteContactManager &checker)
Definition: collision_sphere_sphere_unit.hpp:242
tesseract_collision::test_suite::detail::runTestPrimitive
void runTestPrimitive(DiscreteContactManager &checker)
Definition: collision_box_sphere_unit.hpp:123
tesseract_collision::test_suite
Definition: large_dataset_benchmarks.hpp:17
tesseract_collision::test_suite::runTest
void runTest(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:111
tesseract_collision::CollisionMarginData
tesseract_common::CollisionMarginData CollisionMarginData
Definition: types.h:50
tesseract_collision::CollisionShapePtr
tesseract_geometry::Geometry::Ptr CollisionShapePtr
Definition: types.h:49
tesseract_collision::CollisionShapesConst
std::vector< tesseract_geometry::Geometry::ConstPtr > CollisionShapesConst
Definition: types.h:47
tesseract_collision::ContactResultVector
tesseract_common::AlignedVector< ContactResult > ContactResultVector
Definition: types.h:134
tesseract_collision::loadSimplePlyFile
int loadSimplePlyFile(const std::string &path, tesseract_common::VectorVector3d &vertices, Eigen::VectorXi &faces, bool triangles_only=false)
Loads a simple ply file given a path.
Definition: common.cpp:285
tesseract_collision::ContactTestType::CLOSEST
@ CLOSEST
tesseract_collision::makeConvexMesh
tesseract_geometry::ConvexMesh::Ptr makeConvexMesh(const tesseract_geometry::Mesh &mesh)
Definition: convex_hull_utils.cpp:114
tesseract_common::TransformMap
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
Definition: types.h:66
tesseract_common::isIdentical< std::string >
template bool isIdentical< std::string >(const std::vector< std::string > &, const std::vector< std::string > &, bool, const std::function< bool(const std::string &, const std::string &)> &, const std::function< bool(const std::string &, const std::string &)> &)
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
Definition: types.h:62
tesseract_collision::ContactRequest
The ContactRequest struct.
Definition: types.h:294
active_links
std::vector< std::string > active_links
Definition: tesseract_environment_collision.cpp:112
sphere
auto sphere
Definition: tesseract_geometry_unit.cpp:23
mesh
auto mesh
Definition: tesseract_geometry_unit.cpp:25
Generated on Thu Mar 30 2023 23:19:51 for Tesseract by doxygen 1.9.5