Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
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
9namespace tesseract_collision::test_suite
10{
11namespace detail
12{
13inline void addCollisionObjects(ContinuousContactManager& 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 sphere_pose.translation()[2] = 0.25;
38
39 CollisionShapesConst obj1_shapes;
40 tesseract_common::VectorIsometry3d obj1_poses;
41 obj1_shapes.push_back(sphere);
42 obj1_poses.push_back(sphere_pose);
43
44 checker.addCollisionObject("sphere_link", 0, obj1_shapes, obj1_poses, false);
45 EXPECT_FALSE(checker.isCollisionObjectEnabled("sphere_link"));
46 checker.enableCollisionObject("sphere_link");
47 EXPECT_TRUE(checker.isCollisionObjectEnabled("sphere_link"));
48
50 // Add thin box to checker which is disabled
52 CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
53 Eigen::Isometry3d thin_box_pose;
54 thin_box_pose.setIdentity();
55
56 CollisionShapesConst obj2_shapes;
57 tesseract_common::VectorIsometry3d obj2_poses;
58 obj2_shapes.push_back(thin_box);
59 obj2_poses.push_back(thin_box_pose);
60
61 checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
62 EXPECT_TRUE(checker.isCollisionObjectEnabled("thin_box_link"));
63 checker.disableCollisionObject("thin_box_link");
64 EXPECT_FALSE(checker.isCollisionObjectEnabled("thin_box_link"));
65
67 // Add second sphere to checker. If use_convex_mesh = true
68 // then this sphere will be added as a convex hull mesh.
70 CollisionShapePtr sphere1;
71
72 if (use_convex_mesh)
73 {
74 auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
75 auto mesh_faces = std::make_shared<Eigen::VectorXi>();
76 EXPECT_GT(loadSimplePlyFile(
77 std::string(TESSERACT_SUPPORT_DIR) + "/meshes/sphere_p25m.ply", *mesh_vertices, *mesh_faces, true),
78 0);
79
80 auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
81 sphere1 = makeConvexMesh(*mesh);
82 }
83 else
84 {
85 sphere1 = std::make_shared<tesseract_geometry::Sphere>(0.25);
86 }
87
88 Eigen::Isometry3d sphere1_pose;
89 sphere1_pose.setIdentity();
90 sphere1_pose.translation()[2] = 0.25;
91
92 CollisionShapesConst obj3_shapes;
93 tesseract_common::VectorIsometry3d obj3_poses;
94 obj3_shapes.push_back(sphere1);
95 obj3_poses.push_back(sphere1_pose);
96
97 checker.addCollisionObject("sphere1_link", 0, obj3_shapes, obj3_poses);
98 EXPECT_TRUE(checker.isCollisionObjectEnabled("sphere1_link"));
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 thin_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 EXPECT_FALSE(checker.isCollisionObjectEnabled("link_does_not_exist"));
126
128 // Try to add empty Collision Object
130 EXPECT_FALSE(
131 checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d()));
132 EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
133}
134
135inline void runTestPrimitive(ContinuousContactManager& checker)
136{
138 // Test when object is in collision at cc_time 0.5
140 std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
141 checker.setActiveCollisionObjects(active_links);
142 std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
143 EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
144
145 EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr);
146
147 checker.setCollisionMarginData(CollisionMarginData(0.1));
148 EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
149
150 // Set the start location
151 tesseract_common::TransformMap location_start;
152 location_start["sphere_link"] = Eigen::Isometry3d::Identity();
153 location_start["sphere_link"].translation()(0) = -0.2;
154 location_start["sphere_link"].translation()(1) = -1.0;
155
156 location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
157 location_start["sphere1_link"].translation()(0) = 0.2;
158 location_start["sphere1_link"].translation()(2) = -1.0;
159
160 // Set the end location
161 tesseract_common::TransformMap location_end;
162 location_end["sphere_link"] = Eigen::Isometry3d::Identity();
163 location_end["sphere_link"].translation()(0) = -0.2;
164 location_end["sphere_link"].translation()(1) = 1.0;
165
166 location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
167 location_end["sphere1_link"].translation()(0) = 0.2;
168 location_end["sphere1_link"].translation()(2) = 1.0;
169
170 checker.setCollisionObjectsTransform(location_start, location_end);
171
172 // Perform collision check
173 ContactResultMap result;
174 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
175
176 ContactResultVector result_vector;
177 result.flattenMoveResults(result_vector);
178
179 EXPECT_TRUE(!result_vector.empty());
180 EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001);
181
182 std::vector<int> idx = { 0, 1, 1 };
183 if (result_vector[0].link_names[0] != "sphere_link")
184 idx = { 1, 0, -1 };
185
186 EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.5, 0.001);
187 EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
188
189 EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
190 ContinuousCollisionType::CCType_Between);
191 EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
192 ContinuousCollisionType::CCType_Between);
193
194 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.05, 0.001);
195 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
196 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
197
198 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
199 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
200 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
201
202 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
203 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
204 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
205 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.25, 0.001);
206 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
207 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
208
209 EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
210 EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
211 EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
212 EXPECT_TRUE(
213 result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
214
215 EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
216 EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
217 EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
218
220 // Test when object is in collision at cc_time 0.333 and 0.5
222
223 // Set the start location
224 location_start["sphere_link"] = Eigen::Isometry3d::Identity();
225 location_start["sphere_link"].translation()(0) = -0.2;
226 location_start["sphere_link"].translation()(1) = -0.5;
227
228 location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
229 location_start["sphere1_link"].translation()(0) = 0.2;
230 location_start["sphere1_link"].translation()(2) = -1.0;
231
232 // Set the end location
233 location_end["sphere_link"] = Eigen::Isometry3d::Identity();
234 location_end["sphere_link"].translation()(0) = -0.2;
235 location_end["sphere_link"].translation()(1) = 1.0;
236
237 location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
238 location_end["sphere1_link"].translation()(0) = 0.2;
239 location_end["sphere1_link"].translation()(2) = 1.0;
240
241 checker.setCollisionObjectsTransform(location_start, location_end);
242
243 // Perform collision check
244 result.clear();
245 result_vector.clear();
246 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
247
248 result.flattenCopyResults(result_vector);
249
250 EXPECT_TRUE(!result_vector.empty());
251 EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001);
252
253 idx = { 0, 1, 1 };
254 if (result_vector[0].link_names[0] != "sphere_link")
255 idx = { 1, 0, -1 };
256
257 EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.3333, 0.001);
258 EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
259
260 EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
261 ContinuousCollisionType::CCType_Between);
262 EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[1]))] ==
263 ContinuousCollisionType::CCType_Between);
264
265 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.05, 0.001);
266 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
267 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
268
269 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
270 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
271 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
272
273 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
274 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
275 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
276 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.25, 0.001);
277 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
278 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
279
280 EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
281 EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
282 EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
283 EXPECT_TRUE(
284 result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
285
286 EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
287 EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
288 EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
289}
290
291inline void runTestConvex(ContinuousContactManager& checker)
292{
294 // Test when object is in collision at cc_time 0.5
296 std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
297 checker.setActiveCollisionObjects(active_links);
298 std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
299 EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
300
301 EXPECT_TRUE(checker.getIsContactAllowedFn() == nullptr);
302
303 checker.setCollisionMarginData(CollisionMarginData(0.1));
304 EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
305
306 // Set the start location
307 tesseract_common::TransformMap location_start;
308 location_start["sphere_link"] = Eigen::Isometry3d::Identity();
309 location_start["sphere_link"].translation()(0) = -0.2;
310 location_start["sphere_link"].translation()(1) = -1.0;
311
312 location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
313 location_start["sphere1_link"].translation()(0) = 0.2;
314 location_start["sphere1_link"].translation()(2) = -1.0;
315
316 // Set the end location
317 tesseract_common::TransformMap location_end;
318 location_end["sphere_link"] = Eigen::Isometry3d::Identity();
319 location_end["sphere_link"].translation()(0) = -0.2;
320 location_end["sphere_link"].translation()(1) = 1.0;
321
322 location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
323 location_end["sphere1_link"].translation()(0) = 0.2;
324 location_end["sphere1_link"].translation()(2) = 1.0;
325
326 checker.setCollisionObjectsTransform(location_start, location_end);
327
328 // Perform collision check
329 ContactResultMap result;
330 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
331
332 ContactResultVector result_vector;
333 result.flattenMoveResults(result_vector);
334
335 EXPECT_TRUE(!result_vector.empty());
336 EXPECT_NEAR(result_vector[0].distance, -0.0754, 0.001);
337
338 std::vector<int> idx = { 0, 1, 1 };
339 if (result_vector[0].link_names[0] != "sphere_link")
340 idx = { 1, 0, -1 };
341
342 EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.5, 0.001);
343 EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
344
345 EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
346 ContinuousCollisionType::CCType_Between);
347 EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
348 ContinuousCollisionType::CCType_Between);
349
350 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.0377, 0.001);
351 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
352 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
353
354 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.0377, 0.001);
355 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
356 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
357
358 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.2377, 0.001);
359 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
360 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
361 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.2377, 0.001);
362 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
363 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
364
365 EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
366 EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
367 EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
368 EXPECT_TRUE(
369 result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
370
371 EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
372 EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
373 EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
374
376 // Test when object is in collision at cc_time 0.333 and 0.5
378
379 // Set the start location
380 location_start["sphere_link"] = Eigen::Isometry3d::Identity();
381 location_start["sphere_link"].translation()(0) = -0.2;
382 location_start["sphere_link"].translation()(1) = -0.5;
383
384 location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
385 location_start["sphere1_link"].translation()(0) = 0.2;
386 location_start["sphere1_link"].translation()(2) = -1.0;
387
388 // Set the end location
389 location_end["sphere_link"] = Eigen::Isometry3d::Identity();
390 location_end["sphere_link"].translation()(0) = -0.2;
391 location_end["sphere_link"].translation()(1) = 1.0;
392
393 location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
394 location_end["sphere1_link"].translation()(0) = 0.2;
395 location_end["sphere1_link"].translation()(2) = 1.0;
396
397 checker.setCollisionObjectsTransform(location_start, location_end);
398
399 // Perform collision check
400 result.clear();
401 result_vector.clear();
402 checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
403
404 result.flattenCopyResults(result_vector);
405
406 EXPECT_TRUE(!result_vector.empty());
407 EXPECT_NEAR(result_vector[0].distance, -0.0754, 0.001);
408
409 idx = { 0, 1, 1 };
410 if (result_vector[0].link_names[0] != "sphere_link")
411 idx = { 1, 0, -1 };
412
413 EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.3848, 0.001);
414 EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
415
416 EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
417 ContinuousCollisionType::CCType_Between);
418 EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[1]))] ==
419 ContinuousCollisionType::CCType_Between);
420
421 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.0377, 0.001);
422 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0772, 0.001);
423 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
424
425 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.0377, 0.001);
426 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0772, 0.001);
427 EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
428
429 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.2377, 0.001);
430 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
431 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
432 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.2377, 0.001);
433 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
434 EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
435
436 EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
437 EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
438 EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
439 EXPECT_TRUE(
440 result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
441
442 EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
443 EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
444 EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
445}
446} // namespace detail
447
448inline void runTest(ContinuousContactManager& checker, bool use_convex_mesh)
449{
450 // Add collision objects
451 detail::addCollisionObjects(checker, use_convex_mesh);
452
453 // Call it again to test adding same object
454 detail::addCollisionObjects(checker, use_convex_mesh);
455
456 if (use_convex_mesh)
457 detail::runTestConvex(checker);
458 else
459 detail::runTestPrimitive(checker);
460}
461
462} // namespace tesseract_collision::test_suite
463#endif // TESSERACT_COLLISION_COLLISION_SPHERE_SPHERE_CAST_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::ContinuousContactManager
Definition: continuous_contact_manager.h:41
tesseract_collision::ContinuousContactManager::isCollisionObjectEnabled
virtual bool isCollisionObjectEnabled(const std::string &name) const =0
Check if collision object is enabled.
tesseract_collision::ContinuousContactManager::setActiveCollisionObjects
virtual void setActiveCollisionObjects(const std::vector< std::string > &names)=0
Set which collision objects can move.
tesseract_collision::ContinuousContactManager::getCollisionObjects
virtual const std::vector< std::string > & getCollisionObjects() const =0
Get all collision objects.
tesseract_collision::ContinuousContactManager::hasCollisionObject
virtual bool hasCollisionObject(const std::string &name) const =0
Find if a collision object already exists.
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::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::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::ContinuousContactManager::removeCollisionObject
virtual bool removeCollisionObject(const std::string &name)=0
Remove an object from the checker.
tesseract_collision::ContinuousContactManager::getActiveCollisionObjects
virtual const std::vector< std::string > & getActiveCollisionObjects() const =0
Get which collision objects can move.
tesseract_collision::ContinuousContactManager::enableCollisionObject
virtual bool enableCollisionObject(const std::string &name)=0
Enable an object.
tesseract_collision::ContinuousContactManager::getCollisionMarginData
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
tesseract_collision::ContinuousContactManager::getIsContactAllowedFn
virtual IsContactAllowedFn getIsContactAllowedFn() const =0
Get the active function for determining if two links are allowed to be in collision.
tesseract_collision::ContinuousContactManager::contactTest
virtual void contactTest(ContactResultMap &collisions, const ContactRequest &request)=0
Perform a contact test for all objects based.
tesseract_common::CollisionMarginData::getMaxCollisionMargin
double getMaxCollisionMargin() const
Get the largest collision margin.
Definition: collision_margin_data.h:165
transform
results transform[0]
Definition: collision_core_unit.cpp:144
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
cc_type
results cc_type[0]
Definition: collision_core_unit.cpp:157
cc_time
results cc_time[0]
Definition: collision_core_unit.cpp:155
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
cc_transform
results cc_transform[0]
Definition: collision_core_unit.cpp:159
nearest_points_local
results nearest_points_local[0]
Definition: collision_core_unit.cpp:142
EXPECT_TRUE
EXPECT_TRUE(tesseract_common::isIdentical< tesseract_collision::ObjectPairKey >(pairs, check_pairs, false))
common.h
This is a collection of common methods.
continuous_contact_manager.h
This is the continuous contact manager base class.
convex_hull_utils.h
This is a collection of common methods.
geometries.h
Tesseract Geometries.
EXPECT_GT
EXPECT_GT(m.m.condition, 1e+20)
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::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::ContinuousCollisionType::CCType_Between
@ CCType_Between
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