collision_sphere_sphere_cast_unit.hpp
Go to the documentation of this file.
1 #ifndef TESSERACT_COLLISION_COLLISION_SPHERE_SPHERE_CAST_UNIT_HPP
2 #define TESSERACT_COLLISION_COLLISION_SPHERE_SPHERE_CAST_UNIT_HPP
3 
4 #include <tesseract_collision/bullet/convex_hull_utils.h>
5 #include <tesseract_collision/core/continuous_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(ContinuousContactManager& 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  sphere_pose.translation()[2] = 0.25;
44 
45  CollisionShapesConst obj1_shapes;
46  tesseract_common::VectorIsometry3d obj1_poses;
47  obj1_shapes.push_back(sphere);
48  obj1_poses.push_back(sphere_pose);
49 
50  checker.addCollisionObject("sphere_link", 0, obj1_shapes, obj1_poses, false);
51  EXPECT_FALSE(checker.isCollisionObjectEnabled("sphere_link"));
52  checker.enableCollisionObject("sphere_link");
53  EXPECT_TRUE(checker.isCollisionObjectEnabled("sphere_link"));
54 
56  // Add thin box to checker which is disabled
58  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
59  Eigen::Isometry3d thin_box_pose;
60  thin_box_pose.setIdentity();
61 
62  CollisionShapesConst obj2_shapes;
63  tesseract_common::VectorIsometry3d obj2_poses;
64  obj2_shapes.push_back(thin_box);
65  obj2_poses.push_back(thin_box_pose);
66 
67  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
68  EXPECT_TRUE(checker.isCollisionObjectEnabled("thin_box_link"));
69  checker.disableCollisionObject("thin_box_link");
70  EXPECT_FALSE(checker.isCollisionObjectEnabled("thin_box_link"));
71 
73  // Add second sphere to checker. If use_convex_mesh = true
74  // then this sphere will be added as a convex hull mesh.
76  CollisionShapePtr sphere1;
77 
78  if (use_convex_mesh)
79  {
80  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
81  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
82  EXPECT_GT(
83  loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(),
84  *mesh_vertices,
85  *mesh_faces,
86  true),
87  0);
88 
89  auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
90  sphere1 = makeConvexMesh(*mesh);
91  }
92  else
93  {
94  sphere1 = std::make_shared<tesseract_geometry::Sphere>(0.25);
95  }
96 
97  Eigen::Isometry3d sphere1_pose;
98  sphere1_pose.setIdentity();
99  sphere1_pose.translation()[2] = 0.25;
100 
101  CollisionShapesConst obj3_shapes;
102  tesseract_common::VectorIsometry3d obj3_poses;
103  obj3_shapes.push_back(sphere1);
104  obj3_poses.push_back(sphere1_pose);
105 
106  checker.addCollisionObject("sphere1_link", 0, obj3_shapes, obj3_poses);
107  EXPECT_TRUE(checker.isCollisionObjectEnabled("sphere1_link"));
108 
110  // Add box and remove
112  CollisionShapePtr remove_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
113  Eigen::Isometry3d remove_box_pose;
114  remove_box_pose.setIdentity();
115 
116  CollisionShapesConst obj4_shapes;
117  tesseract_common::VectorIsometry3d obj4_poses;
118  obj4_shapes.push_back(remove_box);
119  obj4_poses.push_back(remove_box_pose);
120 
121  checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses);
122  EXPECT_TRUE(checker.getCollisionObjects().size() == 4);
123  EXPECT_TRUE(checker.hasCollisionObject("remove_box_link"));
124  checker.removeCollisionObject("remove_box_link");
125  EXPECT_FALSE(checker.hasCollisionObject("remove_box_link"));
126  EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
127 
129  // Try functions on a link that does not exist
131  EXPECT_FALSE(checker.removeCollisionObject("link_does_not_exist"));
132  EXPECT_FALSE(checker.enableCollisionObject("link_does_not_exist"));
133  EXPECT_FALSE(checker.disableCollisionObject("link_does_not_exist"));
134  EXPECT_FALSE(checker.isCollisionObjectEnabled("link_does_not_exist"));
135 
137  // Try to add empty Collision Object
139  EXPECT_FALSE(
140  checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d()));
141  EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
142 }
143 
144 inline void runTestPrimitive(ContinuousContactManager& checker)
145 {
147  // Test when object is in collision at cc_time 0.5
149  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
150  checker.setActiveCollisionObjects(active_links);
151  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
152  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
153 
154  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
155 
156  checker.setCollisionMarginData(CollisionMarginData(0.1));
157  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
158 
159  // Set the start location
160  tesseract_common::TransformMap location_start;
161  location_start["sphere_link"] = Eigen::Isometry3d::Identity();
162  location_start["sphere_link"].translation()(0) = -0.2;
163  location_start["sphere_link"].translation()(1) = -1.0;
164 
165  location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
166  location_start["sphere1_link"].translation()(0) = 0.2;
167  location_start["sphere1_link"].translation()(2) = -1.0;
168 
169  // Set the end location
170  tesseract_common::TransformMap location_end;
171  location_end["sphere_link"] = Eigen::Isometry3d::Identity();
172  location_end["sphere_link"].translation()(0) = -0.2;
173  location_end["sphere_link"].translation()(1) = 1.0;
174 
175  location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
176  location_end["sphere1_link"].translation()(0) = 0.2;
177  location_end["sphere1_link"].translation()(2) = 1.0;
178 
179  checker.setCollisionObjectsTransform(location_start, location_end);
180 
181  // Perform collision check
182  ContactResultMap result;
183  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
184 
185  ContactResultVector result_vector;
186  result.flattenMoveResults(result_vector);
187 
188  EXPECT_TRUE(!result_vector.empty());
189  EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001);
190 
191  std::vector<int> idx = { 0, 1, 1 };
192  if (result_vector[0].link_names[0] != "sphere_link")
193  idx = { 1, 0, -1 };
194 
195  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.5, 0.001);
196  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
197 
198  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
199  ContinuousCollisionType::CCType_Between);
200  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
201  ContinuousCollisionType::CCType_Between);
202 
203  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.05, 0.001);
204  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
205  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
206 
207  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
208  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
209  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
210 
211  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
212  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
213  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
214  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.25, 0.001);
215  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
216  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
217 
218  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
219  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
220  EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
221  EXPECT_TRUE(
222  result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
223 
224  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
225  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
226  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
227 
229  // Test when object is in collision at cc_time 0.333 and 0.5
231 
232  // Set the start location
233  location_start["sphere_link"] = Eigen::Isometry3d::Identity();
234  location_start["sphere_link"].translation()(0) = -0.2;
235  location_start["sphere_link"].translation()(1) = -0.5;
236 
237  location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
238  location_start["sphere1_link"].translation()(0) = 0.2;
239  location_start["sphere1_link"].translation()(2) = -1.0;
240 
241  // Set the end location
242  location_end["sphere_link"] = Eigen::Isometry3d::Identity();
243  location_end["sphere_link"].translation()(0) = -0.2;
244  location_end["sphere_link"].translation()(1) = 1.0;
245 
246  location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
247  location_end["sphere1_link"].translation()(0) = 0.2;
248  location_end["sphere1_link"].translation()(2) = 1.0;
249 
250  checker.setCollisionObjectsTransform(location_start, location_end);
251 
252  // Perform collision check
253  result.clear();
254  result_vector.clear();
255  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
256 
257  result.flattenCopyResults(result_vector);
258 
259  EXPECT_TRUE(!result_vector.empty());
260  EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001);
261 
262  idx = { 0, 1, 1 };
263  if (result_vector[0].link_names[0] != "sphere_link")
264  idx = { 1, 0, -1 };
265 
266  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.3333, 0.001);
267  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
268 
269  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
270  ContinuousCollisionType::CCType_Between);
271  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[1]))] ==
272  ContinuousCollisionType::CCType_Between);
273 
274  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.05, 0.001);
275  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
276  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
277 
278  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
279  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
280  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
281 
282  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
283  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
284  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
285  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.25, 0.001);
286  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
287  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
288 
289  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
290  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
291  EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
292  EXPECT_TRUE(
293  result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
294 
295  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
296  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
297  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
298 }
299 
300 inline void runTestConvex(ContinuousContactManager& checker)
301 {
303  // Test when object is in collision at cc_time 0.5
305  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
306  checker.setActiveCollisionObjects(active_links);
307  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
308  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
309 
310  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
311 
312  checker.setCollisionMarginData(CollisionMarginData(0.1));
313  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
314 
315  // Set the start location
316  tesseract_common::TransformMap location_start;
317  location_start["sphere_link"] = Eigen::Isometry3d::Identity();
318  location_start["sphere_link"].translation()(0) = -0.2;
319  location_start["sphere_link"].translation()(1) = -1.0;
320 
321  location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
322  location_start["sphere1_link"].translation()(0) = 0.2;
323  location_start["sphere1_link"].translation()(2) = -1.0;
324 
325  // Set the end location
326  tesseract_common::TransformMap location_end;
327  location_end["sphere_link"] = Eigen::Isometry3d::Identity();
328  location_end["sphere_link"].translation()(0) = -0.2;
329  location_end["sphere_link"].translation()(1) = 1.0;
330 
331  location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
332  location_end["sphere1_link"].translation()(0) = 0.2;
333  location_end["sphere1_link"].translation()(2) = 1.0;
334 
335  checker.setCollisionObjectsTransform(location_start, location_end);
336 
337  // Perform collision check
338  ContactResultMap result;
339  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
340 
341  ContactResultVector result_vector;
342  result.flattenMoveResults(result_vector);
343 
344  EXPECT_TRUE(!result_vector.empty());
345  EXPECT_NEAR(result_vector[0].distance, -0.0754, 0.001);
346 
347  std::vector<int> idx = { 0, 1, 1 };
348  if (result_vector[0].link_names[0] != "sphere_link")
349  idx = { 1, 0, -1 };
350 
351  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.5, 0.001);
352  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
353 
354  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
355  ContinuousCollisionType::CCType_Between);
356  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
357  ContinuousCollisionType::CCType_Between);
358 
359  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.0377, 0.001);
360  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
361  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
362 
363  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.0377, 0.001);
364  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
365  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
366 
367  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.2377, 0.001);
368  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
369  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
370  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.2377, 0.001);
371  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
372  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
373 
374  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
375  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
376  EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
377  EXPECT_TRUE(
378  result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
379 
380  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
381  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
382  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
383 
385  // Test when object is in collision at cc_time 0.333 and 0.5
387 
388  // Set the start location
389  location_start["sphere_link"] = Eigen::Isometry3d::Identity();
390  location_start["sphere_link"].translation()(0) = -0.2;
391  location_start["sphere_link"].translation()(1) = -0.5;
392 
393  location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
394  location_start["sphere1_link"].translation()(0) = 0.2;
395  location_start["sphere1_link"].translation()(2) = -1.0;
396 
397  // Set the end location
398  location_end["sphere_link"] = Eigen::Isometry3d::Identity();
399  location_end["sphere_link"].translation()(0) = -0.2;
400  location_end["sphere_link"].translation()(1) = 1.0;
401 
402  location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
403  location_end["sphere1_link"].translation()(0) = 0.2;
404  location_end["sphere1_link"].translation()(2) = 1.0;
405 
406  checker.setCollisionObjectsTransform(location_start, location_end);
407 
408  // Perform collision check
409  result.clear();
410  result_vector.clear();
411  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
412 
413  result.flattenCopyResults(result_vector);
414 
415  EXPECT_TRUE(!result_vector.empty());
416  EXPECT_NEAR(result_vector[0].distance, -0.0754, 0.001);
417 
418  idx = { 0, 1, 1 };
419  if (result_vector[0].link_names[0] != "sphere_link")
420  idx = { 1, 0, -1 };
421 
422  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.3848, 0.001);
423  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
424 
425  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
426  ContinuousCollisionType::CCType_Between);
427  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[1]))] ==
428  ContinuousCollisionType::CCType_Between);
429 
430  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.0377, 0.001);
431  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0772, 0.001);
432  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
433 
434  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.0377, 0.001);
435  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0772, 0.001);
436  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
437 
438  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.2377, 0.001);
439  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
440  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
441  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.2377, 0.001);
442  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
443  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
444 
445  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
446  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
447  EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
448  EXPECT_TRUE(
449  result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
450 
451  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
452  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
453  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
454 }
455 } // namespace detail
456 
457 inline void runTest(ContinuousContactManager& checker, bool use_convex_mesh)
458 {
459  // Add collision objects
460  detail::addCollisionObjects(checker, use_convex_mesh);
461 
462  // Call it again to test adding same object
463  detail::addCollisionObjects(checker, use_convex_mesh);
464 
465  if (use_convex_mesh)
466  detail::runTestConvex(checker);
467  else
468  detail::runTestPrimitive(checker);
469 }
470 
471 } // namespace tesseract_collision::test_suite
472 #endif // TESSERACT_COLLISION_COLLISION_SPHERE_SPHERE_CAST_UNIT_HPP
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
tesseract_common::CollisionMarginData::getMaxCollisionMargin
double getMaxCollisionMargin() const
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
tesseract_collision::ContinuousContactManager::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::ContinuousContactManager::getActiveCollisionObjects
virtual const std::vector< std::string > & getActiveCollisionObjects() const =0
Get which collision objects can move.
tesseract_collision::ContinuousContactManager::removeCollisionObject
virtual bool removeCollisionObject(const std::string &name)=0
Remove an object from the checker.
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
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
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::ContinuousCollisionType::CCType_Between
@ CCType_Between
tesseract_collision::makeConvexMesh
tesseract_geometry::ConvexMesh::Ptr makeConvexMesh(const tesseract_geometry::Mesh &mesh)
Definition: convex_hull_utils.cpp:113
tesseract_collision::ContinuousContactManager
Definition: continuous_contact_manager.h:41
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
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
tesseract_collision::ContinuousContactManager::getCollisionObjects
virtual const std::vector< std::string > & getCollisionObjects() const =0
Get all collision objects.
tesseract_collision::ContinuousContactManager::addCollisionObject
virtual bool addCollisionObject(const std::string &name, const int &mask_id, const CollisionShapesConst &shapes, const tesseract_common::VectorIsometry3d &shape_poses, bool enabled=true)=0
Add a collision object to the checker.
tesseract_collision::ContinuousContactManager::disableCollisionObject
virtual bool disableCollisionObject(const std::string &name)=0
Disable an object.
tesseract_collision::ContinuousContactManager::setCollisionObjectsTransform
virtual void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose)=0
Set a single static collision object's tansforms.
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
tesseract_collision::ContinuousContactManager::hasCollisionObject
virtual bool hasCollisionObject(const std::string &name) const =0
Find if a collision object already exists.
distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
tesseract_collision::ContinuousContactManager::enableCollisionObject
virtual bool enableCollisionObject(const std::string &name)=0
Enable an object.
tesseract_collision::ContinuousContactManager::isCollisionObjectEnabled
virtual bool isCollisionObjectEnabled(const std::string &name) const =0
Check if collision object is enabled.
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_common::GeneralResourceLocator
tesseract_collision::ContinuousContactManager::setCollisionMarginData
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data)=0
Set the contact distance threshold.
tesseract_collision::ContinuousContactManager::contactTest
virtual void contactTest(ContactResultMap &collisions, const ContactRequest &request)=0
Perform a contact test for all objects based.
tesseract_collision::ContactResultMap::flattenCopyResults
void flattenCopyResults(ContactResultVector &v) const
Definition: types.cpp:282
tesseract_collision::ContactRequest
The ContactRequest struct.
Definition: types.h:300
tesseract_collision::ContinuousContactManager::setActiveCollisionObjects
virtual void setActiveCollisionObjects(const std::vector< std::string > &names)=0
Set which collision objects can move.
EXPECT_FALSE
#define EXPECT_FALSE(args)
continuous_contact_manager.h
This is the continuous contact manager base class.
tesseract_collision::ContinuousContactManager::getCollisionMarginData
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
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