contact_manager_config_unit.hpp
Go to the documentation of this file.
1 #ifndef TESSERACT_COLLISION_CONTACT_MANAGER_CONFIG_UNIT_H
2 #define TESSERACT_COLLISION_CONTACT_MANAGER_CONFIG_UNIT_H
3 
4 #include <tesseract_collision/core/discrete_contact_manager.h>
5 #include <tesseract_collision/core/continuous_contact_manager.h>
6 #include <tesseract_collision/core/common.h>
7 #include <tesseract_geometry/geometries.h>
8 
9 namespace tesseract_collision::test_suite
10 {
11 namespace detail
12 {
13 inline void addCollisionObjects(DiscreteContactManager& checker)
14 {
16  // Add sphere to checker
18  CollisionShapePtr sphere = std::make_shared<tesseract_geometry::Sphere>(0.25);
19 
20  Eigen::Isometry3d sphere_pose;
21  sphere_pose.setIdentity();
22 
23  CollisionShapesConst obj1_shapes;
24  tesseract_common::VectorIsometry3d obj1_poses;
25  obj1_shapes.push_back(sphere);
26  obj1_poses.push_back(sphere_pose);
27 
28  checker.addCollisionObject("sphere_link", 0, obj1_shapes, obj1_poses);
29 
31  // Add thin box to checker which is disabled
33  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
34  Eigen::Isometry3d thin_box_pose;
35  thin_box_pose.setIdentity();
36 
37  CollisionShapesConst obj2_shapes;
38  tesseract_common::VectorIsometry3d obj2_poses;
39  obj2_shapes.push_back(thin_box);
40  obj2_poses.push_back(thin_box_pose);
41 
42  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
43 
45  // Add second sphere to checker. If use_convex_mesh = true
46  // then this sphere will be added as a convex hull mesh.
48  CollisionShapePtr sphere1 = std::make_shared<tesseract_geometry::Sphere>(0.25);
49 
50  Eigen::Isometry3d sphere1_pose;
51  sphere1_pose.setIdentity();
52 
53  CollisionShapesConst obj3_shapes;
54  tesseract_common::VectorIsometry3d obj3_poses;
55  obj3_shapes.push_back(sphere1);
56  obj3_poses.push_back(sphere1_pose);
57 
58  checker.addCollisionObject("sphere1_link", 0, obj3_shapes, obj3_poses);
59 }
60 
61 inline void addCollisionObjects(ContinuousContactManager& checker)
62 {
64  // Add sphere to checker
66  CollisionShapePtr sphere = std::make_shared<tesseract_geometry::Sphere>(0.25);
67 
68  Eigen::Isometry3d sphere_pose;
69  sphere_pose.setIdentity();
70  sphere_pose.translation()[2] = 0.25;
71 
72  CollisionShapesConst obj1_shapes;
73  tesseract_common::VectorIsometry3d obj1_poses;
74  obj1_shapes.push_back(sphere);
75  obj1_poses.push_back(sphere_pose);
76 
77  checker.addCollisionObject("sphere_link", 0, obj1_shapes, obj1_poses, false);
78  EXPECT_FALSE(checker.isCollisionObjectEnabled("sphere_link"));
79  checker.enableCollisionObject("sphere_link");
80  EXPECT_TRUE(checker.isCollisionObjectEnabled("sphere_link"));
81 
83  // Add thin box to checker which is disabled
85  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
86  Eigen::Isometry3d thin_box_pose;
87  thin_box_pose.setIdentity();
88 
89  CollisionShapesConst obj2_shapes;
90  tesseract_common::VectorIsometry3d obj2_poses;
91  obj2_shapes.push_back(thin_box);
92  obj2_poses.push_back(thin_box_pose);
93 
94  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
95 
97  // Add second sphere to checker. If use_convex_mesh = true
98  // then this sphere will be added as a convex hull mesh.
100  CollisionShapePtr sphere1 = std::make_shared<tesseract_geometry::Sphere>(0.25);
101 
102  Eigen::Isometry3d sphere1_pose;
103  sphere1_pose.setIdentity();
104  sphere1_pose.translation()[2] = 0.25;
105 
106  CollisionShapesConst obj3_shapes;
107  tesseract_common::VectorIsometry3d obj3_poses;
108  obj3_shapes.push_back(sphere1);
109  obj3_poses.push_back(sphere1_pose);
110 
111  checker.addCollisionObject("sphere1_link", 0, obj3_shapes, obj3_poses);
112  EXPECT_TRUE(checker.isCollisionObjectEnabled("sphere1_link"));
113 }
114 
115 } // namespace detail
116 
117 inline void runTest(DiscreteContactManager& checker)
118 {
119  // Add collision objects
120  detail::addCollisionObjects(checker);
121 
123  // Test when object is in collision
125  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
126  checker.setActiveCollisionObjects(active_links);
127  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
128  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
129 
130  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
131 
132  checker.setDefaultCollisionMargin(0.5);
133  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5);
134  EXPECT_TRUE(checker.isCollisionObjectEnabled("thin_box_link"));
135 
136  ContactManagerConfig config;
137  config.modify_object_enabled["thin_box_link"] = false;
138 
139  checker.applyContactManagerConfig(config);
140  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5);
141  EXPECT_FALSE(checker.isCollisionObjectEnabled("thin_box_link"));
142 
143  config.default_margin = 0.1;
144  checker.applyContactManagerConfig(config);
145  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
146 
147  // Test when object is inside another
148  tesseract_common::TransformMap location;
149  location["sphere_link"] = Eigen::Isometry3d::Identity();
150  location["sphere1_link"] = Eigen::Isometry3d::Identity();
151  location["sphere1_link"].translation()(0) = 0.2;
152  checker.setCollisionObjectsTransform(location);
153 
154  // Perform collision check
155  ContactResultMap result;
156  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
157 
158  ContactResultVector result_vector;
159  result.flattenMoveResults(result_vector);
160 
161  EXPECT_TRUE(!result_vector.empty());
162  EXPECT_NEAR(result_vector[0].distance, -0.30, 0.0001);
163 
164  std::vector<int> idx = { 0, 1, 1 };
165  if (result_vector[0].link_names[0] != "sphere_link")
166  idx = { 1, 0, -1 };
167 
168  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
169  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
170  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
171  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
172  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
173  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
174 
175  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
176  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
177  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.0016); // TODO LEVI: This was increased due to FLC
178  // calculation because it is using GJK
179 
181  // Test object is out side the contact distance
183  location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
184  result.clear();
185  result_vector.clear();
186  checker.setCollisionObjectsTransform("sphere1_link", location["sphere1_link"]);
187 
188  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
189  result.flattenCopyResults(result_vector);
190 
191  EXPECT_TRUE(result_vector.empty());
192 
194  // Test object inside the contact distance
196  result.clear();
197  result_vector.clear();
198 
199  config = ContactManagerConfig(0.52);
200  checker.applyContactManagerConfig(config);
201  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.52, 1e-5);
202  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
203  result.flattenMoveResults(result_vector);
204 
205  EXPECT_TRUE(!result_vector.empty());
206  EXPECT_NEAR(result_vector[0].distance, 0.5, 0.0001);
207 
208  idx = { 0, 1, 1 };
209  if (result_vector[0].link_names[0] != "sphere_link")
210  idx = { 1, 0, -1 };
211 
212  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
213  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
214  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
215  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.75, 0.001);
216  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
217  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
218 
219  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
220  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
221  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
222 }
223 
224 inline void runTest(ContinuousContactManager& checker)
225 {
226  // Add collision objects
227  detail::addCollisionObjects(checker);
228 
230  // Test when object is in collision at cc_time 0.5
232  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
233  checker.setActiveCollisionObjects(active_links);
234  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
235  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
236 
237  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
238 
239  checker.setDefaultCollisionMargin(0.5);
240  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5);
241  EXPECT_TRUE(checker.isCollisionObjectEnabled("thin_box_link"));
242 
243  ContactManagerConfig config;
244  config.modify_object_enabled["thin_box_link"] = false;
245 
246  checker.applyContactManagerConfig(config);
247  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5);
248  EXPECT_FALSE(checker.isCollisionObjectEnabled("thin_box_link"));
249 
250  config.default_margin = 0.1;
251  checker.applyContactManagerConfig(config);
252  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
253 
254  // Set the start location
255  tesseract_common::TransformMap location_start;
256  location_start["sphere_link"] = Eigen::Isometry3d::Identity();
257  location_start["sphere_link"].translation()(0) = -0.2;
258  location_start["sphere_link"].translation()(1) = -1.0;
259 
260  location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
261  location_start["sphere1_link"].translation()(0) = 0.2;
262  location_start["sphere1_link"].translation()(2) = -1.0;
263 
264  // Set the end location
265  tesseract_common::TransformMap location_end;
266  location_end["sphere_link"] = Eigen::Isometry3d::Identity();
267  location_end["sphere_link"].translation()(0) = -0.2;
268  location_end["sphere_link"].translation()(1) = 1.0;
269 
270  location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
271  location_end["sphere1_link"].translation()(0) = 0.2;
272  location_end["sphere1_link"].translation()(2) = 1.0;
273 
274  checker.setCollisionObjectsTransform(location_start, location_end);
275 
276  // Perform collision check
277  ContactResultMap result;
278  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
279 
280  ContactResultVector result_vector;
281  result.flattenCopyResults(result_vector);
282 
283  EXPECT_TRUE(!result_vector.empty());
284  EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001);
285 
286  std::vector<int> idx = { 0, 1, 1 };
287  if (result_vector[0].link_names[0] != "sphere_link")
288  idx = { 1, 0, -1 };
289 
290  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.5, 0.001);
291  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
292 
293  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
294  ContinuousCollisionType::CCType_Between);
295  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
296  ContinuousCollisionType::CCType_Between);
297 
298  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.05, 0.001);
299  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
300  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
301 
302  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
303  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
304  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
305 
306  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
307  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
308  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
309  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.25, 0.001);
310  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
311  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
312 
313  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
314  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
315  EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
316  EXPECT_TRUE(
317  result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
318 
319  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
320  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
321  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
322 
324  // Test when object is in collision at cc_time 0.333 and 0.5
326 
327  // Set the start location
328  location_start["sphere_link"] = Eigen::Isometry3d::Identity();
329  location_start["sphere_link"].translation()(0) = -0.2;
330  location_start["sphere_link"].translation()(1) = -0.5;
331 
332  location_start["sphere1_link"] = Eigen::Isometry3d::Identity();
333  location_start["sphere1_link"].translation()(0) = 0.2;
334  location_start["sphere1_link"].translation()(2) = -1.0;
335 
336  // Set the end location
337  location_end["sphere_link"] = Eigen::Isometry3d::Identity();
338  location_end["sphere_link"].translation()(0) = -0.2;
339  location_end["sphere_link"].translation()(1) = 1.0;
340 
341  location_end["sphere1_link"] = Eigen::Isometry3d::Identity();
342  location_end["sphere1_link"].translation()(0) = 0.2;
343  location_end["sphere1_link"].translation()(2) = 1.0;
344 
345  checker.setCollisionObjectsTransform(location_start, location_end);
346 
347  // Perform collision check
348  result.clear();
349  result_vector.clear();
350  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
351 
352  result.flattenMoveResults(result_vector);
353 
354  EXPECT_TRUE(!result_vector.empty());
355  EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001);
356 
357  idx = { 0, 1, 1 };
358  if (result_vector[0].link_names[0] != "sphere_link")
359  idx = { 1, 0, -1 };
360 
361  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[0])], 0.3333, 0.001);
362  EXPECT_NEAR(result_vector[0].cc_time[static_cast<size_t>(idx[1])], 0.5, 0.001);
363 
364  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[0]))] ==
365  ContinuousCollisionType::CCType_Between);
366  EXPECT_TRUE(result_vector[0].cc_type[static_cast<size_t>(static_cast<size_t>(idx[1]))] ==
367  ContinuousCollisionType::CCType_Between);
368 
369  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.05, 0.001);
370  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
371  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
372 
373  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
374  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
375  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
376 
377  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][0], 0.25, 0.001);
378  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
379  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[0])][2], 0.25, 0.001);
380  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][0], -0.25, 0.001);
381  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
382  EXPECT_NEAR(result_vector[0].nearest_points_local[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
383 
384  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[0])].isApprox(location_start["sphere_link"], 0.0001));
385  EXPECT_TRUE(result_vector[0].transform[static_cast<size_t>(idx[1])].isApprox(location_start["sphere1_link"], 0.0001));
386  EXPECT_TRUE(result_vector[0].cc_transform[static_cast<size_t>(idx[0])].isApprox(location_end["sphere_link"], 0.0001));
387  EXPECT_TRUE(
388  result_vector[0].cc_transform[static_cast<size_t>(idx[1])].isApprox(location_end["sphere1_link"], 0.0001));
389 
390  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
391  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
392  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
393 }
394 } // namespace tesseract_collision::test_suite
395 #endif // TESSERACT_COLLISION_CONTACT_MANAGER_CONFIG_UNIT_H
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
tesseract_collision::test_suite::detail::addCollisionObjects
void addCollisionObjects(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:11
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
EXPECT_TRUE
#define EXPECT_TRUE(args)
geometries.h
tesseract_collision::ContinuousCollisionType::CCType_Between
@ CCType_Between
tesseract_collision::test_suite
Definition: large_dataset_benchmarks.hpp:17
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
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
common.h
This is a collection of common methods.
tesseract_collision::test_suite::runTest
void runTest(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:111
EXPECT_FALSE
#define EXPECT_FALSE(args)
continuous_contact_manager.h
This is the continuous contact manager base class.
tesseract_collision::ContactTestType::CLOSEST
@ CLOSEST

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