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 #include <tesseract_common/resource_locator.h>
9 
10 namespace tesseract_collision::test_suite
11 {
12 namespace detail
13 {
14 inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex_mesh = false)
15 {
16  tesseract_common::GeneralResourceLocator locator;
17 
19  // Add sphere to checker
21  CollisionShapePtr sphere;
22  if (use_convex_mesh)
23  {
24  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
25  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
26  EXPECT_GT(
27  loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(),
28  *mesh_vertices,
29  *mesh_faces,
30  true),
31  0);
32 
33  auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
34  sphere = makeConvexMesh(*mesh);
35  }
36  else
37  {
38  sphere = std::make_shared<tesseract_geometry::Sphere>(0.25);
39  }
40 
41  Eigen::Isometry3d sphere_pose;
42  sphere_pose.setIdentity();
43 
44  CollisionShapesConst obj1_shapes;
45  tesseract_common::VectorIsometry3d obj1_poses;
46  obj1_shapes.push_back(sphere);
47  obj1_poses.push_back(sphere_pose);
48 
49  checker.addCollisionObject("sphere_link", 0, obj1_shapes, obj1_poses);
50 
52  // Add thin box to checker which is disabled
54  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
55  Eigen::Isometry3d thin_box_pose;
56  thin_box_pose.setIdentity();
57 
58  CollisionShapesConst obj2_shapes;
59  tesseract_common::VectorIsometry3d obj2_poses;
60  obj2_shapes.push_back(thin_box);
61  obj2_poses.push_back(thin_box_pose);
62 
63  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses, false);
64 
66  // Add second sphere to checker. If use_convex_mesh = true
67  // then this sphere will be added as a convex hull mesh.
69  CollisionShapePtr sphere1;
70 
71  if (use_convex_mesh)
72  {
73  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
74  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
75  EXPECT_GT(
76  loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(),
77  *mesh_vertices,
78  *mesh_faces,
79  true),
80  0);
81 
82  auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
83  sphere1 = makeConvexMesh(*mesh);
84  }
85  else
86  {
87  sphere1 = std::make_shared<tesseract_geometry::Sphere>(0.25);
88  }
89 
90  Eigen::Isometry3d sphere1_pose;
91  sphere1_pose.setIdentity();
92 
93  CollisionShapesConst obj3_shapes;
94  tesseract_common::VectorIsometry3d obj3_poses;
95  obj3_shapes.push_back(sphere1);
96  obj3_poses.push_back(sphere1_pose);
97 
98  checker.addCollisionObject("sphere1_link", 0, obj3_shapes, obj3_poses);
99 
101  // Add box and remove
103  CollisionShapePtr remove_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
104  Eigen::Isometry3d remove_box_pose;
105  remove_box_pose.setIdentity();
106 
107  CollisionShapesConst obj4_shapes;
108  tesseract_common::VectorIsometry3d obj4_poses;
109  obj4_shapes.push_back(remove_box);
110  obj4_poses.push_back(remove_box_pose);
111 
112  checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses);
113  EXPECT_TRUE(checker.getCollisionObjects().size() == 4);
114  EXPECT_TRUE(checker.hasCollisionObject("remove_box_link"));
115  checker.removeCollisionObject("remove_box_link");
116  EXPECT_FALSE(checker.hasCollisionObject("remove_box_link"));
117  EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
118 
120  // Try functions on a link that does not exist
122  EXPECT_FALSE(checker.removeCollisionObject("link_does_not_exist"));
123  EXPECT_FALSE(checker.enableCollisionObject("link_does_not_exist"));
124  EXPECT_FALSE(checker.disableCollisionObject("link_does_not_exist"));
125 
127  // Try to add empty Collision Object
129  EXPECT_FALSE(
130  checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d()));
131  EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
132 }
133 
134 inline void runTestPrimitive(DiscreteContactManager& checker)
135 {
137  // Test when object is in collision
139  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
140  checker.setActiveCollisionObjects(active_links);
141  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
142  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
143 
144  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
145 
146  checker.setCollisionMarginData(CollisionMarginData(0.1));
147  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
148 
149  // Test when object is inside another
150  tesseract_common::TransformMap location;
151  location["sphere_link"] = Eigen::Isometry3d::Identity();
152  location["sphere1_link"] = Eigen::Isometry3d::Identity();
153  location["sphere1_link"].translation()(0) = 0.2;
154  checker.setCollisionObjectsTransform(location);
155 
156  // Perform collision check
157  ContactResultMap result;
158  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
159 
160  ContactResultVector result_vector;
161  result.flattenMoveResults(result_vector);
162 
163  EXPECT_TRUE(!result_vector.empty());
164  EXPECT_NEAR(result_vector[0].distance, -0.30, 0.0001);
165 
166  std::vector<int> idx = { 0, 1, 1 };
167  if (result_vector[0].link_names[0] != "sphere_link")
168  idx = { 1, 0, -1 };
169 
170  if (result_vector[0].single_contact_point)
171  {
172  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
173  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.25)) > 0.001 &&
174  std::abs(result_vector[0].nearest_points[0][0] - (-0.05)) > 0.001);
175  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
176  std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
177  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001 &&
178  std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001);
179  }
180  else
181  {
182  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
183  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
184  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
185  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
186  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
187  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
188  }
189 
190  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
191  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
192  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.0016); // TODO LEVI: This was increased due to FLC
193  // calculation because it is using GJK
194 
196  // Test object is out side the contact distance
198  location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
199  result.clear();
200  result_vector.clear();
201  checker.setCollisionObjectsTransform("sphere1_link", location["sphere1_link"]);
202 
203  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
204  result.flattenCopyResults(result_vector);
205 
206  EXPECT_TRUE(result_vector.empty());
207 
209  // Test object inside the contact distance
211  result.clear();
212  result_vector.clear();
213 
214  checker.setCollisionMarginData(CollisionMarginData(0.52));
215  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.52, 1e-5);
216  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
217  result.flattenMoveResults(result_vector);
218 
219  EXPECT_TRUE(!result_vector.empty());
220  EXPECT_NEAR(result_vector[0].distance, 0.5, 0.0001);
221 
222  idx = { 0, 1, 1 };
223  if (result_vector[0].link_names[0] != "sphere_link")
224  idx = { 1, 0, -1 };
225 
226  if (result_vector[0].single_contact_point)
227  {
228  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
229  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.25)) > 0.001 &&
230  std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001);
231  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
232  std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
233  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001 &&
234  std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001);
235  }
236  else
237  {
238  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
239  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
240  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
241  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.75, 0.001);
242  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
243  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
244  }
245 
246  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
247  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
248  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
249 }
250 
251 inline void runTestPrimitiveDistanceDisabled(DiscreteContactManager& checker)
252 {
254  // Test when object is in collision
256  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
257  checker.setActiveCollisionObjects(active_links);
258  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
259  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
260 
261  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
262 
263  checker.setCollisionMarginData(CollisionMarginData(0.1));
264  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
265 
266  // Test when object is inside another
267  tesseract_common::TransformMap location;
268  location["sphere_link"] = Eigen::Isometry3d::Identity();
269  location["sphere1_link"] = Eigen::Isometry3d::Identity();
270  location["sphere1_link"].translation()(0) = 0.2;
271  checker.setCollisionObjectsTransform(location);
272 
273  // Perform collision check
274  ContactResultMap result;
275  ContactRequest contact_request(ContactTestType::CLOSEST);
276  contact_request.calculate_distance = false;
277  checker.contactTest(result, contact_request);
278 
279  ContactResultVector result_vector;
280  result.flattenMoveResults(result_vector);
281 
282  EXPECT_TRUE(!result_vector.empty());
283  EXPECT_NEAR(result_vector[0].distance, -0.30, 0.0001);
284 
285  std::vector<int> idx = { 0, 1, 1 };
286  if (result_vector[0].link_names[0] != "sphere_link")
287  idx = { 1, 0, -1 };
288 
289  if (result_vector[0].single_contact_point)
290  {
291  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
292  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.25)) > 0.001 &&
293  std::abs(result_vector[0].nearest_points[0][0] - (-0.05)) > 0.001);
294  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
295  std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
296  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001 &&
297  std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001);
298  }
299  else
300  {
301  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
302  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
303  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
304  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
305  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
306  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
307  }
308 
309  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
310  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
311  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.0016); // TODO LEVI: This was increased due to FLC
312  // calculation because it is using GJK
313 
315  // Test object is out side the contact distance
317  location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
318  result.clear();
319  result_vector.clear();
320  checker.setCollisionObjectsTransform("sphere1_link", location["sphere1_link"]);
321 
322  checker.contactTest(result, contact_request);
323  result.flattenCopyResults(result_vector);
324 
325  EXPECT_TRUE(result_vector.empty());
326 
328  // Test object inside the contact distance
330  result.clear();
331  result_vector.clear();
332 
333  checker.setCollisionMarginData(CollisionMarginData(0.52));
334  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.52, 1e-5);
335  checker.contactTest(result, contact_request);
336  result.flattenMoveResults(result_vector);
337 
338  EXPECT_TRUE(!result_vector.empty());
339  EXPECT_NEAR(result_vector[0].distance, 0.5, 0.0001);
340 
341  idx = { 0, 1, 1 };
342  if (result_vector[0].link_names[0] != "sphere_link")
343  idx = { 1, 0, -1 };
344 
345  if (result_vector[0].single_contact_point)
346  {
347  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
348  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.25)) > 0.001 &&
349  std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001);
350  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
351  std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
352  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001 &&
353  std::abs(result_vector[0].nearest_points[0][2] - (0.0)) > 0.001);
354  }
355  else
356  {
357  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
358  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
359  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
360  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.75, 0.001);
361  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
362  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
363  }
364 
365  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
366  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
367  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
368 }
369 
370 inline void runTestConvex1(DiscreteContactManager& checker)
371 {
373  // Test when object is in collision (Closest Feature Edge 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.getContactAllowedValidator() == nullptr);
381 
382  checker.setCollisionMarginData(CollisionMarginData(0.1));
383  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
384 
385  // Test when object is inside another
386  tesseract_common::TransformMap location;
387  location["sphere_link"] = Eigen::Isometry3d::Identity();
388  location["sphere1_link"] = Eigen::Isometry3d::Identity();
389  location["sphere1_link"].translation()(0) = 0.2;
390  checker.setCollisionObjectsTransform(location);
391 
392  // Perform collision check
393  ContactResultMap result;
394  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
395 
396  ContactResultVector result_vector;
397  result.flattenCopyResults(result_vector);
398 
399  // Bullet: -0.270548 {0.232874,0,-0.025368} {-0.032874,0,0.025368}
400  // FCL: -0.270548 {0.232874,0,-0.025368} {-0.032874,0,0.025368}
401  EXPECT_TRUE(!result_vector.empty());
402  EXPECT_NEAR(result_vector[0].distance, -0.270548, 0.001);
403 
404  std::vector<int> idx = { 0, 1, 1 };
405  if (result_vector[0].link_names[0] != "sphere_link")
406  idx = { 1, 0, -1 };
407 
408  if (result_vector[0].single_contact_point)
409  {
410  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
411  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.23776)) > 0.001 &&
412  std::abs(result_vector[0].nearest_points[0][0] - (-0.032874)) > 0.001);
413  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
414  std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
415  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (-0.025368)) > 0.001 &&
416  std::abs(result_vector[0].nearest_points[0][2] - (0.025368)) > 0.001);
417  }
418  else
419  {
420  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.232874, 0.001);
421  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
422  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], -0.025368, 0.001);
423  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.032874, 0.001);
424  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
425  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.025368, 0.001);
426  }
427 
428  EXPECT_GT((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)), 0.0);
429  EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)))), 0.5);
430 
432  // Test object is out side the contact distance
434  location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
435  result.clear();
436  result_vector.clear();
437  checker.setCollisionObjectsTransform(location);
438 
439  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
440  result.flattenMoveResults(result_vector);
441 
442  EXPECT_TRUE(result_vector.empty());
443 }
444 
445 inline void runTestConvex2(DiscreteContactManager& checker)
446 {
448  // Test object inside the contact distance (Closest Feature Edge to Edge)
450  ContactResultMap result;
451  ContactResultVector result_vector;
452 
453  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
454  checker.setActiveCollisionObjects(active_links);
455  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
456  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
457 
458  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
459 
460  tesseract_common::TransformMap location;
461  location["sphere_link"] = Eigen::Isometry3d::Identity();
462  location["sphere1_link"] = Eigen::Isometry3d::Identity();
463  location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
464  checker.setCollisionObjectsTransform(location);
465 
466  checker.setCollisionMarginData(CollisionMarginData(0.55));
467  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.55, 1e-5);
468  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
469  result.flattenCopyResults(result_vector);
470 
471  // Bullet: 0.524565 {0.237717,0,0} {0.7622825,0,0} Using blender this appears to be the correct result
472  // FCL: 0.546834 {0.237717,-0.0772317,0} {0.7622825,0.0772317}
473  EXPECT_TRUE(!result_vector.empty());
474  EXPECT_NEAR(result_vector[0].distance, 0.52448, 0.001);
475  EXPECT_NEAR(result_vector[0].nearest_points[0][1], result_vector[0].nearest_points[1][1], 0.001);
476  EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
477 
478  std::vector<int> idx = { 0, 1, 1 };
479  if (result_vector[0].link_names[0] != "sphere_link")
480  idx = { 1, 0, -1 };
481 
482  if (result_vector[0].single_contact_point)
483  {
484  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
485  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.23776)) > 0.001 &&
486  std::abs(result_vector[0].nearest_points[0][0] - (0.76224)) > 0.001);
487  }
488  else
489  {
490  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.23776, 0.001);
491  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.76224, 0.001);
492  }
493 
494  EXPECT_GT((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)), 0.0);
495  EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)))), 0.00001);
496 }
497 
498 inline void runTestConvex3(DiscreteContactManager& checker)
499 {
501  // Test when object is in collision (Closest Feature face to edge)
503  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
504  checker.setActiveCollisionObjects(active_links);
505  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
506  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
507 
508  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
509 
510  checker.setCollisionMarginData(CollisionMarginData(0.1));
511  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
512 
513  tesseract_common::TransformMap location;
514  location["sphere1_link"] = Eigen::Isometry3d::Identity();
515  location["sphere1_link"].translation()(1) = 0.2;
516  checker.setCollisionObjectsTransform(location);
517 
518  // Perform collision check
519  ContactResultMap result;
520  ContactResultVector result_vector;
521  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
522  result.flattenMoveResults(result_vector);
523 
524  // Bullet: -0.280223 {0.0425563,0.2308753,-0.0263040} {-0.0425563, -0.0308753, 0.0263040}
525  // FCL: -0.280223 {0.0425563,0.2308753,-0.0263040} {-0.0425563, -0.0308753, 0.0263040}
526  EXPECT_TRUE(!result_vector.empty());
527  EXPECT_NEAR(result_vector[0].distance, -0.280223, 0.001);
528 
529  std::vector<int> idx = { 0, 1, 1 };
530  if (result_vector[0].link_names[0] != "sphere_link")
531  idx = { 1, 0, -1 };
532 
533  if (result_vector[0].single_contact_point)
534  {
535  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
536  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.2308753)) > 0.001 &&
537  std::abs(result_vector[0].nearest_points[0][1] - (-0.0308753)) > 0.001);
538  EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0]), 0.0425563, 0.001);
539  EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2]), 0.0263040, 0.001);
540  }
541  else
542  {
543  EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0]), 0.0425563, 0.001);
544  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.2308753, 0.001);
545  EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2]), 0.0263040, 0.001);
546  EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0]), 0.0425563, 0.001);
547  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], -0.0308753, 0.001);
548  EXPECT_NEAR(std::abs(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2]), 0.0263040, 0.001);
549  }
550 
551  EXPECT_NEAR(std::abs(idx[2] * result_vector[0].normal[0]), 0.3037316, 0.001);
552  EXPECT_NEAR(idx[2] * result_vector[0].normal[1], 0.9340783, 0.001);
553  EXPECT_NEAR(std::abs(idx[2] * result_vector[0].normal[2]), 0.1877358, 0.001);
554  EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(0, 1, 0)))), 0.4);
555 }
556 
557 inline void runTestConvex(DiscreteContactManager& checker)
558 {
559  runTestConvex1(checker);
560  runTestConvex2(checker);
561  runTestConvex3(checker);
562 }
563 } // namespace detail
564 
565 inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh)
566 {
567  // Add collision objects
568  detail::addCollisionObjects(checker, use_convex_mesh);
569 
570  // Call it again to test adding same object
571  detail::addCollisionObjects(checker, use_convex_mesh);
572 
573  if (use_convex_mesh)
574  detail::runTestConvex(checker);
575  else
576  {
577  detail::runTestPrimitive(checker);
578  detail::runTestPrimitiveDistanceDisabled(checker);
579  }
580 }
581 } // namespace tesseract_collision::test_suite
582 
583 #endif // TESSERACT_COLLISION_COLLISION_SPHERE_SPHERE_UNIT_HPP
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
tesseract_common::CollisionMarginData::getMaxCollisionMargin
double getMaxCollisionMargin() const
tesseract_collision::ContactRequest::calculate_distance
bool calculate_distance
This enables the calculation of distance data if two objects are within the contact threshold.
Definition: types.h:309
tesseract_collision::DiscreteContactManager::getActiveCollisionObjects
virtual const std::vector< std::string > & getActiveCollisionObjects() const =0
Get which collision objects can move.
tesseract_collision::DiscreteContactManager::setCollisionMarginData
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data)=0
Set the contact distance threshold.
tesseract_collision::test_suite::detail::addCollisionObjects
void addCollisionObjects(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:11
resource_locator.h
tesseract_common::TransformMap
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
discrete_contact_manager.h
This is the discrete contact manager base class.
tesseract_collision::ContactResultVector
tesseract_common::AlignedVector< ContactResult > ContactResultVector
Definition: types.h:136
tesseract_collision::ContactResultMap::clear
void clear()
This is a consurvative clear.
Definition: types.cpp:231
tesseract_collision::test_suite::detail::runTestConvex1
void runTestConvex1(DiscreteContactManager &checker)
Definition: collision_sphere_sphere_unit.hpp:370
tesseract_collision::DiscreteContactManager::getContactAllowedValidator
virtual std::shared_ptr< const tesseract_common::ContactAllowedValidator > getContactAllowedValidator() const =0
Get the active function for determining if two links are allowed to be in collision.
tesseract_collision::DiscreteContactManager
Definition: discrete_contact_manager.h:41
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:289
convex_hull_utils.h
This is a collection of common methods.
EXPECT_TRUE
#define EXPECT_TRUE(args)
tesseract_collision::ContactResultMap::flattenMoveResults
void flattenMoveResults(ContactResultVector &v)
Definition: types.cpp:270
geometries.h
tesseract_collision::makeConvexMesh
tesseract_geometry::ConvexMesh::Ptr makeConvexMesh(const tesseract_geometry::Mesh &mesh)
Definition: convex_hull_utils.cpp:113
tesseract_collision::test_suite::detail::runTestPrimitiveDistanceDisabled
void runTestPrimitiveDistanceDisabled(DiscreteContactManager &checker)
Definition: collision_sphere_sphere_unit.hpp:251
tesseract_collision::test_suite::detail::runTestPrimitive
void runTestPrimitive(DiscreteContactManager &checker)
Definition: collision_box_sphere_unit.hpp:128
tesseract_collision::test_suite
Definition: large_dataset_benchmarks.hpp:17
tesseract_collision::ContactResultMap
This structure hold contact results for link pairs.
Definition: types.h:155
tesseract_collision::test_suite::detail::runTestConvex2
void runTestConvex2(DiscreteContactManager &checker)
Definition: collision_sphere_sphere_unit.hpp:445
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
tesseract_collision::test_suite::detail::runTestConvex3
void runTestConvex3(DiscreteContactManager &checker)
Definition: collision_sphere_sphere_unit.hpp:498
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::CollisionShapePtr
std::shared_ptr< tesseract_geometry::Geometry > CollisionShapePtr
Definition: types.h:50
tesseract_collision::CollisionShapesConst
std::vector< CollisionShapeConstPtr > CollisionShapesConst
Definition: types.h:51
distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
tesseract_common::GeneralResourceLocator::locateResource
std::shared_ptr< Resource > locateResource(const std::string &url) const override
common.h
This is a collection of common methods.
tesseract_collision::test_suite::detail::runTestConvex
void runTestConvex(DiscreteContactManager &checker)
Definition: collision_box_sphere_unit.hpp:241
tesseract_collision::test_suite::runTest
void runTest(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:111
tesseract_collision::DiscreteContactManager::setActiveCollisionObjects
virtual void setActiveCollisionObjects(const std::vector< std::string > &names)=0
Set which collision objects can move.
tesseract_common::GeneralResourceLocator
tesseract_collision::DiscreteContactManager::getCollisionMarginData
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
tesseract_collision::ContactResultMap::flattenCopyResults
void flattenCopyResults(ContactResultVector &v) const
Definition: types.cpp:282
tesseract_collision::ContactRequest
The ContactRequest struct.
Definition: types.h:300
EXPECT_FALSE
#define EXPECT_FALSE(args)
tesseract_collision::ContactTestType::CLOSEST
@ CLOSEST
tesseract_collision::CollisionMarginData
tesseract_common::CollisionMarginData CollisionMarginData
Definition: types.h:52

tesseract_collision
Author(s): Levi Armstrong
autogenerated on Sun May 18 2025 03:01:52