collision_core_unit.cpp
Go to the documentation of this file.
1 #include <tesseract_common/macros.h>
2 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3 #include <gtest/gtest.h>
4 #include <vector>
5 #include <string>
6 TESSERACT_COMMON_IGNORE_WARNINGS_POP
7 
8 #include <tesseract_collision/core/common.h>
9 #include <tesseract_collision/core/types.h>
10 #include <tesseract_collision/core/serialization.h>
11 #include <tesseract_common/contact_allowed_validator.h>
12 #include <tesseract_common/unit_test_utils.h>
13 #include <tesseract_common/utils.h>
14 
15 class TestContactAllowedValidator : public tesseract_common::ContactAllowedValidator
16 {
17 public:
18  bool operator()(const std::string& s1, const std::string& s2) const override
19  {
20  return (tesseract_common::makeOrderedLinkPair("base_link", "link_1") ==
21  tesseract_common::makeOrderedLinkPair(s1, s2));
22  }
23 };
24 
25 TEST(TesseractCoreUnit, ContactManagerConfigTest) // NOLINT
26 {
27  { // Default Construction
28  tesseract_collision::ContactManagerConfig config;
29  EXPECT_FALSE(config.default_margin.has_value());
30  EXPECT_EQ(config.pair_margin_override_type, tesseract_collision::CollisionMarginPairOverrideType::NONE);
31  EXPECT_TRUE(config.pair_margin_data.empty());
32  EXPECT_EQ(config.acm_override_type, tesseract_collision::ACMOverrideType::NONE);
33  EXPECT_TRUE(config.acm.getAllAllowedCollisions().empty());
34  EXPECT_TRUE(config.modify_object_enabled.empty());
35 
36  tesseract_common::testSerialization<tesseract_collision::ContactManagerConfig>(config, "ContactManagerConfig");
37 
38  config.incrementMargins(0.025);
39  EXPECT_FALSE(config.default_margin.has_value());
40  EXPECT_EQ(config.pair_margin_override_type, tesseract_collision::CollisionMarginPairOverrideType::NONE);
41  EXPECT_TRUE(config.pair_margin_data.empty());
42  EXPECT_EQ(config.acm_override_type, tesseract_collision::ACMOverrideType::NONE);
43  EXPECT_TRUE(config.acm.getAllAllowedCollisions().empty());
44  EXPECT_TRUE(config.modify_object_enabled.empty());
45 
46  config.scaleMargins(2.0);
47  EXPECT_FALSE(config.default_margin.has_value());
48  EXPECT_EQ(config.pair_margin_override_type, tesseract_collision::CollisionMarginPairOverrideType::NONE);
49  EXPECT_TRUE(config.pair_margin_data.empty());
50  EXPECT_EQ(config.acm_override_type, tesseract_collision::ACMOverrideType::NONE);
51  EXPECT_TRUE(config.acm.getAllAllowedCollisions().empty());
52  EXPECT_TRUE(config.modify_object_enabled.empty());
53  }
54 
55  { // Construction
56  tesseract_collision::ContactManagerConfig config(0.025);
57  EXPECT_TRUE(config.default_margin.has_value());
58  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(config.default_margin.value(), 0.025)); // NOLINT
59  EXPECT_EQ(config.pair_margin_override_type, tesseract_collision::CollisionMarginPairOverrideType::NONE);
60  EXPECT_TRUE(config.pair_margin_data.empty());
61  EXPECT_EQ(config.acm_override_type, tesseract_collision::ACMOverrideType::NONE);
62  EXPECT_TRUE(config.acm.getAllAllowedCollisions().empty());
63  EXPECT_TRUE(config.modify_object_enabled.empty());
64 
65  tesseract_common::testSerialization<tesseract_collision::ContactManagerConfig>(config, "ContactManagerConfig");
66 
67  config.incrementMargins(0.025);
68  EXPECT_TRUE(config.default_margin.has_value());
69  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(config.default_margin.value(), 0.05)); // NOLINT
70  EXPECT_EQ(config.pair_margin_override_type, tesseract_collision::CollisionMarginPairOverrideType::NONE);
71  EXPECT_TRUE(config.pair_margin_data.empty());
72  EXPECT_EQ(config.acm_override_type, tesseract_collision::ACMOverrideType::NONE);
73  EXPECT_TRUE(config.acm.getAllAllowedCollisions().empty());
74  EXPECT_TRUE(config.modify_object_enabled.empty());
75 
76  config.scaleMargins(2.0);
77  EXPECT_TRUE(config.default_margin.has_value());
78  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(config.default_margin.value(), 2.0 * 0.05)); // NOLINT
79  EXPECT_EQ(config.pair_margin_override_type, tesseract_collision::CollisionMarginPairOverrideType::NONE);
80  EXPECT_TRUE(config.pair_margin_data.empty());
81  EXPECT_EQ(config.acm_override_type, tesseract_collision::ACMOverrideType::NONE);
82  EXPECT_TRUE(config.acm.getAllAllowedCollisions().empty());
83  EXPECT_TRUE(config.modify_object_enabled.empty());
84  }
85 
86  { // Construction
87  tesseract_collision::ContactManagerConfig config(0.025);
88  config.pair_margin_data.setCollisionMargin("link1", "link2", 0.05);
89  config.pair_margin_override_type = tesseract_collision::CollisionMarginPairOverrideType::MODIFY;
90 
91  tesseract_common::testSerialization<tesseract_collision::ContactManagerConfig>(config, "ContactManagerConfig");
92 
93  config.incrementMargins(0.025);
94  EXPECT_TRUE(config.default_margin.has_value());
95  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(config.default_margin.value(), 0.05)); // NOLINT
96  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(
97  config.pair_margin_data.getCollisionMargin("link1", "link2").value(), 0.05 + 0.025)); // NOLINT
98 
99  config.scaleMargins(2.0);
100  EXPECT_TRUE(config.default_margin.has_value());
101  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(config.default_margin.value(), 2.0 * 0.05)); // NOLINT
102  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(
103  config.pair_margin_data.getCollisionMargin("link1", "link2").value(), 2.0 * (0.05 + 0.025))); // NOLINT
104  }
105 
106  {
107  tesseract_collision::ContactManagerConfig config;
108  EXPECT_NO_THROW(config.validate()); // NOLINT
109  }
110 
111  {
112  tesseract_collision::ContactManagerConfig config(0.1);
113  EXPECT_NO_THROW(config.validate()); // NOLINT
114  }
115 
116  {
117  tesseract_collision::ContactManagerConfig config;
118  config.default_margin = 0.1;
119  EXPECT_NO_THROW(config.validate()); // NOLINT
120  }
121 
122  {
123  tesseract_collision::ContactManagerConfig config;
124  config.pair_margin_data.setCollisionMargin("a", "b", 0.1);
125  EXPECT_ANY_THROW(config.validate()); // NOLINT
126  }
127 
128  {
129  tesseract_collision::ContactManagerConfig config;
130  config.pair_margin_override_type = tesseract_collision::CollisionMarginPairOverrideType::MODIFY;
131  config.pair_margin_data.setCollisionMargin("a", "b", 0.1);
132  EXPECT_NO_THROW(config.validate()); // NOLINT
133  }
134 
135  {
136  tesseract_collision::ContactManagerConfig config;
137  config.acm.addAllowedCollision("a", "b", "never");
138  EXPECT_ANY_THROW(config.validate()); // NOLINT
139  }
140 
141  {
142  tesseract_collision::ContactManagerConfig config;
143  config.acm_override_type = tesseract_collision::ACMOverrideType::OR;
144  config.acm.addAllowedCollision("a", "b", "never");
145  EXPECT_NO_THROW(config.validate()); // NOLINT
146  }
147 }
148 
149 TEST(TesseractCoreUnit, getCollisionObjectPairsUnit) // NOLINT
150 {
151  std::vector<std::string> active_links{ "link_1", "link_2", "link_3" };
152  std::vector<std::string> static_links{ "base_link", "part_link" };
153 
154  std::vector<tesseract_collision::ObjectPairKey> check_pairs;
155  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_2"));
156  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_3"));
157  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_2", "link_3"));
158  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_1"));
159  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_2"));
160  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_3"));
161  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_1"));
162  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_2"));
163  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_3"));
164 
165  std::vector<tesseract_collision::ObjectPairKey> pairs =
166  tesseract_collision::getCollisionObjectPairs(active_links, static_links);
167 
168  EXPECT_TRUE(tesseract_common::isIdentical<tesseract_collision::ObjectPairKey>(pairs, check_pairs, false));
169 
170  // Now check provided a is contact allowed function
171  auto validator = std::make_shared<TestContactAllowedValidator>();
172 
173  check_pairs.clear();
174  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_2"));
175  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_3"));
176  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_2", "link_3"));
177  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_2"));
178  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_3"));
179  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_1"));
180  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_2"));
181  check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_3"));
182 
183  pairs = tesseract_collision::getCollisionObjectPairs(active_links, static_links, validator);
184 
185  EXPECT_TRUE(tesseract_common::isIdentical<tesseract_collision::ObjectPairKey>(pairs, check_pairs, false));
186 }
187 
188 TEST(TesseractCoreUnit, isContactAllowedUnit) // NOLINT
189 {
190  auto validator = std::make_shared<TestContactAllowedValidator>();
191 
192  EXPECT_TRUE(tesseract_collision::isContactAllowed("base_link", "base_link", validator, false));
193  EXPECT_FALSE(tesseract_collision::isContactAllowed("base_link", "link_2", validator, false));
194  EXPECT_TRUE(tesseract_collision::isContactAllowed("base_link", "link_1", validator, true));
195 }
196 
197 TEST(TesseractCoreUnit, scaleVerticesUnit) // NOLINT
198 {
199  tesseract_common::VectorVector3d base_vertices{};
200  base_vertices.emplace_back(0, 0, 0);
201  base_vertices.emplace_back(0, 0, 1);
202  base_vertices.emplace_back(0, 1, 1);
203  base_vertices.emplace_back(0, 1, 0);
204  base_vertices.emplace_back(1, 0, 0);
205  base_vertices.emplace_back(1, 0, 1);
206  base_vertices.emplace_back(1, 1, 1);
207  base_vertices.emplace_back(1, 1, 0);
208 
209  // Test identity scale
210  // NOLINTNEXTLINE(cppcoreguidelines-init-variables)
211  tesseract_common::VectorVector3d test_vertices{ base_vertices };
212  tesseract_collision::scaleVertices(test_vertices, Eigen::Vector3d(1, 1, 1));
213  for (std::size_t i = 0; i < 8; ++i)
214  {
215  EXPECT_TRUE(test_vertices[i].isApprox(base_vertices[i]));
216  }
217 
218  // Test scale by 10
219  test_vertices = base_vertices;
220  tesseract_collision::scaleVertices(test_vertices, Eigen::Vector3d(10, 10, 10));
221  EXPECT_TRUE(test_vertices[0].isApprox(Eigen::Vector3d(-4.5, -4.5, -4.5)));
222  EXPECT_TRUE(test_vertices[1].isApprox(Eigen::Vector3d(-4.5, -4.5, 5.5)));
223  EXPECT_TRUE(test_vertices[2].isApprox(Eigen::Vector3d(-4.5, 5.5, 5.5)));
224  EXPECT_TRUE(test_vertices[3].isApprox(Eigen::Vector3d(-4.5, 5.5, -4.5)));
225  EXPECT_TRUE(test_vertices[4].isApprox(Eigen::Vector3d(5.5, -4.5, -4.5)));
226  EXPECT_TRUE(test_vertices[5].isApprox(Eigen::Vector3d(5.5, -4.5, 5.5)));
227  EXPECT_TRUE(test_vertices[6].isApprox(Eigen::Vector3d(5.5, 5.5, 5.5)));
228  EXPECT_TRUE(test_vertices[7].isApprox(Eigen::Vector3d(5.5, 5.5, -4.5)));
229 
230  // Test scale by 10 with center (0, 0, 0)
231  test_vertices = base_vertices;
232  tesseract_collision::scaleVertices(test_vertices, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(10, 10, 10));
233  EXPECT_TRUE(test_vertices[0].isApprox(Eigen::Vector3d(0, 0, 0)));
234  EXPECT_TRUE(test_vertices[1].isApprox(Eigen::Vector3d(0, 0, 10)));
235  EXPECT_TRUE(test_vertices[2].isApprox(Eigen::Vector3d(0, 10, 10)));
236  EXPECT_TRUE(test_vertices[3].isApprox(Eigen::Vector3d(0, 10, 0)));
237  EXPECT_TRUE(test_vertices[4].isApprox(Eigen::Vector3d(10, 0, 0)));
238  EXPECT_TRUE(test_vertices[5].isApprox(Eigen::Vector3d(10, 0, 10)));
239  EXPECT_TRUE(test_vertices[6].isApprox(Eigen::Vector3d(10, 10, 10)));
240  EXPECT_TRUE(test_vertices[7].isApprox(Eigen::Vector3d(10, 10, 0)));
241 }
242 
243 TEST(TesseractCoreUnit, ContactResultsUnit) // NOLINT
244 {
245  tesseract_collision::ContactResult results;
246 
247  EXPECT_EQ(results.distance, std::numeric_limits<double>::max());
248  EXPECT_TRUE(results.nearest_points[0].isApprox(Eigen::Vector3d::Zero()));
249  EXPECT_TRUE(results.nearest_points[1].isApprox(Eigen::Vector3d::Zero()));
250  EXPECT_TRUE(results.nearest_points_local[0].isApprox(Eigen::Vector3d::Zero()));
251  EXPECT_TRUE(results.nearest_points_local[1].isApprox(Eigen::Vector3d::Zero()));
252  EXPECT_TRUE(results.transform[0].isApprox(Eigen::Isometry3d::Identity()));
253  EXPECT_TRUE(results.transform[1].isApprox(Eigen::Isometry3d::Identity()));
254  EXPECT_TRUE(results.link_names[0].empty());
255  EXPECT_TRUE(results.link_names[1].empty());
256  EXPECT_EQ(results.shape_id[0], -1);
257  EXPECT_EQ(results.shape_id[1], -1);
258  EXPECT_EQ(results.subshape_id[0], -1);
259  EXPECT_EQ(results.subshape_id[1], -1);
260  EXPECT_EQ(results.type_id[0], 0);
261  EXPECT_EQ(results.type_id[1], 0);
262  EXPECT_TRUE(results.normal.isApprox(Eigen::Vector3d::Zero()));
263  EXPECT_EQ(results.cc_time[0], -1);
264  EXPECT_EQ(results.cc_time[1], -1);
265  EXPECT_EQ(results.cc_type[0], tesseract_collision::ContinuousCollisionType::CCType_None);
266  EXPECT_EQ(results.cc_type[1], tesseract_collision::ContinuousCollisionType::CCType_None);
267  EXPECT_TRUE(results.cc_transform[0].isApprox(Eigen::Isometry3d::Identity()));
268  EXPECT_TRUE(results.cc_transform[1].isApprox(Eigen::Isometry3d::Identity()));
269  EXPECT_EQ(results.single_contact_point, false);
270 
271  tesseract_common::testSerialization<tesseract_collision::ContactResult>(results, "ContactResult");
272 
273  results.distance = 10;
274  results.nearest_points[0] = Eigen::Vector3d(1, 2, 3);
275  results.nearest_points[1] = Eigen::Vector3d(1, 2, 3);
276  results.nearest_points_local[0] = Eigen::Vector3d(1, 2, 3);
277  results.nearest_points_local[1] = Eigen::Vector3d(1, 2, 3);
278  results.transform[0] = Eigen::Isometry3d::Identity() * Eigen::Translation3d(1, 2, 3);
279  results.transform[1] = Eigen::Isometry3d::Identity() * Eigen::Translation3d(1, 2, 3);
280  results.link_names[0] = "notempty";
281  results.link_names[1] = "notempty";
282  results.shape_id[0] = 5;
283  results.shape_id[1] = 5;
284  results.subshape_id[0] = 10;
285  results.subshape_id[1] = 10;
286  results.type_id[0] = 3;
287  results.type_id[1] = 3;
288  results.normal = Eigen::Vector3d(1, 2, 3);
289  results.cc_time[0] = 7;
290  results.cc_time[1] = 7;
291  results.cc_type[0] = tesseract_collision::ContinuousCollisionType::CCType_Between;
292  results.cc_type[1] = tesseract_collision::ContinuousCollisionType::CCType_Time0;
293  results.cc_transform[0] = Eigen::Isometry3d::Identity() * Eigen::Translation3d(1, 2, 3);
294  results.cc_transform[1] = Eigen::Isometry3d::Identity() * Eigen::Translation3d(1, 2, 3);
295  results.single_contact_point = true;
296 
297  tesseract_common::testSerialization<tesseract_collision::ContactResult>(results, "ContactResult");
298 
299  tesseract_collision::ContactResult copy_results(results);
300  EXPECT_TRUE(copy_results == results);
301 
302  results.clear();
303  EXPECT_TRUE(copy_results != results);
304  EXPECT_EQ(results.distance, std::numeric_limits<double>::max());
305  EXPECT_TRUE(results.nearest_points[0].isApprox(Eigen::Vector3d::Zero()));
306  EXPECT_TRUE(results.nearest_points[1].isApprox(Eigen::Vector3d::Zero()));
307  EXPECT_TRUE(results.nearest_points_local[0].isApprox(Eigen::Vector3d::Zero()));
308  EXPECT_TRUE(results.nearest_points_local[1].isApprox(Eigen::Vector3d::Zero()));
309  EXPECT_TRUE(results.transform[0].isApprox(Eigen::Isometry3d::Identity()));
310  EXPECT_TRUE(results.transform[1].isApprox(Eigen::Isometry3d::Identity()));
311  EXPECT_TRUE(results.link_names[0].empty());
312  EXPECT_TRUE(results.link_names[1].empty());
313  EXPECT_EQ(results.shape_id[0], -1);
314  EXPECT_EQ(results.shape_id[1], -1);
315  EXPECT_EQ(results.subshape_id[0], -1);
316  EXPECT_EQ(results.subshape_id[1], -1);
317  EXPECT_EQ(results.type_id[0], 0);
318  EXPECT_EQ(results.type_id[1], 0);
319  EXPECT_TRUE(results.normal.isApprox(Eigen::Vector3d::Zero()));
320  EXPECT_EQ(results.cc_time[0], -1);
321  EXPECT_EQ(results.cc_time[1], -1);
322  EXPECT_EQ(results.cc_type[0], tesseract_collision::ContinuousCollisionType::CCType_None);
323  EXPECT_EQ(results.cc_type[1], tesseract_collision::ContinuousCollisionType::CCType_None);
324  EXPECT_TRUE(results.cc_transform[0].isApprox(Eigen::Isometry3d::Identity()));
325  EXPECT_TRUE(results.cc_transform[1].isApprox(Eigen::Isometry3d::Identity()));
326  EXPECT_EQ(results.single_contact_point, false);
327 
328  tesseract_common::testSerialization<tesseract_collision::ContactResult>(results, "ContactResult");
329 }
330 
331 TEST(TesseractCoreUnit, ContactResultMapUnit) // NOLINT
332 {
333  { // Test construction state
334  tesseract_collision::ContactResultMap result_map;
335  EXPECT_EQ(result_map.count(), 0);
336  EXPECT_EQ(result_map.size(), 0);
337  EXPECT_EQ(result_map.getContainer().size(), 0);
338  EXPECT_FALSE(result_map.getSummary().empty());
339 
340  tesseract_common::testSerialization<tesseract_collision::ContactResultMap>(result_map, "ContactResultMap");
341  }
342 
343  auto key1 = tesseract_common::makeOrderedLinkPair("link1", "link2");
344  auto key2 = tesseract_common::makeOrderedLinkPair("link2", "link3");
345 
346  { // Test addContactResult single method
347  tesseract_collision::ContactResultMap result_map;
348  result_map.addContactResult(key1, tesseract_collision::ContactResult{});
349  EXPECT_FALSE(result_map.getSummary().empty());
350  EXPECT_EQ(result_map.count(), 1);
351  EXPECT_EQ(result_map.size(), 1);
352  EXPECT_FALSE(result_map.empty());
353  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
354  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
355  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
356  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
357  EXPECT_EQ(result_map.getContainer().size(), 1);
358  auto it = result_map.find(key1);
359  EXPECT_TRUE(it != result_map.end());
360  EXPECT_EQ(it->second.size(), 1);
361 
362  tesseract_common::testSerialization<tesseract_collision::ContactResultMap>(result_map, "ContactResultMap");
363 
364  result_map.addContactResult(key1, tesseract_collision::ContactResult{});
365  EXPECT_FALSE(result_map.getSummary().empty());
366  EXPECT_EQ(result_map.count(), 2);
367  EXPECT_EQ(result_map.size(), 1);
368  EXPECT_FALSE(result_map.empty());
369  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
370  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
371  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
372  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
373  EXPECT_EQ(result_map.getContainer().size(), 1);
374  it = result_map.find(key1);
375  EXPECT_TRUE(it != result_map.end());
376  EXPECT_EQ(it->second.size(), 2);
377 
378  tesseract_common::testSerialization<tesseract_collision::ContactResultMap>(result_map, "ContactResultMap");
379 
380  result_map.addContactResult(key2, tesseract_collision::ContactResult{});
381  EXPECT_FALSE(result_map.getSummary().empty());
382  EXPECT_EQ(result_map.count(), 3);
383  EXPECT_EQ(result_map.size(), 2);
384  EXPECT_FALSE(result_map.empty());
385  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
386  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
387  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
388  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
389  EXPECT_EQ(result_map.getContainer().size(), 2);
390  it = result_map.find(key1);
391  EXPECT_TRUE(it != result_map.end());
392  EXPECT_EQ(it->second.size(), 2);
393 
394  it = result_map.find(key2);
395  EXPECT_TRUE(it != result_map.end());
396  EXPECT_EQ(it->second.size(), 1);
397 
398  tesseract_common::testSerialization<tesseract_collision::ContactResultMap>(result_map, "ContactResultMap");
399 
400  tesseract_collision::ContactResultMap copy_result_map(result_map);
401  EXPECT_TRUE(copy_result_map == result_map);
402 
403  // test clear
404  result_map.clear();
405  EXPECT_TRUE(copy_result_map != result_map);
406  EXPECT_FALSE(result_map.getSummary().empty());
407  EXPECT_EQ(result_map.count(), 0);
408  EXPECT_EQ(result_map.size(), 0);
409  EXPECT_TRUE(result_map.empty());
410  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
411  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
412  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
413  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
414  EXPECT_EQ(result_map.getContainer().size(), 2);
415  it = result_map.find(key1);
416  EXPECT_TRUE(it != result_map.end());
417  EXPECT_EQ(it->second.size(), 0);
418  EXPECT_TRUE(it->second.capacity() > 0);
419 
420  it = result_map.find(key2);
421  EXPECT_TRUE(it != result_map.end());
422  EXPECT_EQ(it->second.size(), 0);
423  EXPECT_TRUE(it->second.capacity() > 0);
424 
425  // test release
426  result_map.release();
427  EXPECT_EQ(result_map.count(), 0);
428  EXPECT_EQ(result_map.size(), 0);
429  EXPECT_TRUE(result_map.empty());
430  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
431  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
432  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
433  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
434  EXPECT_TRUE(result_map.getContainer().empty());
435 
436  tesseract_common::testSerialization<tesseract_collision::ContactResultMap>(result_map, "ContactResultMap");
437  }
438 
439  { // Test addContactResult vector method
440  tesseract_collision::ContactResultMap result_map;
441  result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
442  EXPECT_EQ(result_map.count(), 2);
443  EXPECT_EQ(result_map.size(), 1);
444  EXPECT_FALSE(result_map.empty());
445  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
446  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
447  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
448  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
449  EXPECT_EQ(result_map.getContainer().size(), 1);
450  auto it = result_map.find(key1);
451  EXPECT_TRUE(it != result_map.end());
452  EXPECT_EQ(it->second.size(), 2);
453 
454  result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
455  EXPECT_EQ(result_map.count(), 4);
456  EXPECT_EQ(result_map.size(), 1);
457  EXPECT_FALSE(result_map.empty());
458  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
459  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
460  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
461  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
462  EXPECT_EQ(result_map.getContainer().size(), 1);
463  it = result_map.find(key1);
464  EXPECT_TRUE(it != result_map.end());
465  EXPECT_EQ(it->second.size(), 4);
466 
467  result_map.addContactResult(key2, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
468  EXPECT_EQ(result_map.count(), 6);
469  EXPECT_EQ(result_map.size(), 2);
470  EXPECT_FALSE(result_map.empty());
471  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
472  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
473  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
474  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
475  EXPECT_EQ(result_map.getContainer().size(), 2);
476  it = result_map.find(key1);
477  EXPECT_TRUE(it != result_map.end());
478  EXPECT_EQ(it->second.size(), 4);
479 
480  it = result_map.find(key2);
481  EXPECT_TRUE(it != result_map.end());
482  EXPECT_EQ(it->second.size(), 2);
483 
484  // test release
485  result_map.release();
486  EXPECT_EQ(result_map.count(), 0);
487  EXPECT_EQ(result_map.size(), 0);
488  EXPECT_TRUE(result_map.empty());
489  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
490  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
491  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
492  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
493  EXPECT_TRUE(result_map.getContainer().empty());
494 
495  tesseract_common::testSerialization<tesseract_collision::ContactResultMap>(result_map, "ContactResultMap");
496  }
497 
498  { // Test setContactResult single method
499  tesseract_collision::ContactResultMap result_map;
500  result_map.setContactResult(key1, tesseract_collision::ContactResult{});
501  EXPECT_EQ(result_map.count(), 1);
502  EXPECT_EQ(result_map.size(), 1);
503  EXPECT_FALSE(result_map.empty());
504  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
505  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
506  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
507  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
508  EXPECT_EQ(result_map.getContainer().size(), 1);
509  auto it = result_map.find(key1);
510  EXPECT_TRUE(it != result_map.end());
511  EXPECT_EQ(it->second.size(), 1);
512 
513  result_map.addContactResult(key1, tesseract_collision::ContactResult{});
514  EXPECT_EQ(result_map.count(), 2);
515  EXPECT_EQ(result_map.size(), 1);
516  EXPECT_FALSE(result_map.empty());
517  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
518  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
519  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
520  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
521  EXPECT_EQ(result_map.getContainer().size(), 1);
522  it = result_map.find(key1);
523  EXPECT_TRUE(it != result_map.end());
524  EXPECT_EQ(it->second.size(), 2);
525 
526  result_map.setContactResult(key1, tesseract_collision::ContactResult{});
527  EXPECT_EQ(result_map.count(), 1);
528  EXPECT_EQ(result_map.size(), 1);
529  EXPECT_FALSE(result_map.empty());
530  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
531  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
532  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
533  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
534  EXPECT_EQ(result_map.getContainer().size(), 1);
535  it = result_map.find(key1);
536  EXPECT_TRUE(it != result_map.end());
537  EXPECT_EQ(it->second.size(), 1);
538 
539  result_map.setContactResult(key2, tesseract_collision::ContactResult{});
540  EXPECT_EQ(result_map.count(), 2);
541  EXPECT_EQ(result_map.size(), 2);
542  EXPECT_FALSE(result_map.empty());
543  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
544  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
545  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
546  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
547  EXPECT_EQ(result_map.getContainer().size(), 2);
548  it = result_map.find(key1);
549  EXPECT_TRUE(it != result_map.end());
550  EXPECT_EQ(it->second.size(), 1);
551 
552  it = result_map.find(key2);
553  EXPECT_TRUE(it != result_map.end());
554  EXPECT_EQ(it->second.size(), 1);
555 
556  // test clear
557  result_map.clear();
558  EXPECT_EQ(result_map.count(), 0);
559  EXPECT_EQ(result_map.size(), 0);
560  EXPECT_EQ(result_map.getContainer().size(), 2);
561  it = result_map.find(key1);
562  EXPECT_TRUE(it != result_map.end());
563  EXPECT_EQ(it->second.size(), 0);
564  EXPECT_TRUE(it->second.capacity() > 0);
565 
566  it = result_map.find(key2);
567  EXPECT_TRUE(it != result_map.end());
568  EXPECT_EQ(it->second.size(), 0);
569  EXPECT_TRUE(it->second.capacity() > 0);
570 
571  // test shrink to fit
572  result_map.shrinkToFit();
573  EXPECT_EQ(result_map.count(), 0);
574  EXPECT_EQ(result_map.size(), 0);
575  EXPECT_TRUE(result_map.empty());
576  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
577  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
578  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
579  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
580  EXPECT_TRUE(result_map.getContainer().empty());
581  }
582 
583  { // Test setContactResult vector method
584  tesseract_collision::ContactResultMap result_map;
585  result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
586  EXPECT_EQ(result_map.count(), 2);
587  EXPECT_EQ(result_map.size(), 1);
588  EXPECT_FALSE(result_map.empty());
589  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
590  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
591  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
592  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
593  EXPECT_EQ(result_map.getContainer().size(), 1);
594  auto it = result_map.find(key1);
595  EXPECT_TRUE(it != result_map.end());
596  EXPECT_EQ(it->second.size(), 2);
597 
598  result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
599  EXPECT_EQ(result_map.count(), 4);
600  EXPECT_EQ(result_map.size(), 1);
601  EXPECT_FALSE(result_map.empty());
602  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
603  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
604  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
605  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
606  EXPECT_EQ(result_map.getContainer().size(), 1);
607  it = result_map.find(key1);
608  EXPECT_TRUE(it != result_map.end());
609  EXPECT_EQ(it->second.size(), 4);
610 
611  result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
612  EXPECT_EQ(result_map.count(), 2);
613  EXPECT_EQ(result_map.size(), 1);
614  EXPECT_FALSE(result_map.empty());
615  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
616  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
617  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
618  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
619  EXPECT_EQ(result_map.getContainer().size(), 1);
620  it = result_map.find(key1);
621  EXPECT_TRUE(it != result_map.end());
622  EXPECT_EQ(it->second.size(), 2);
623 
624  result_map.setContactResult(key2, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
625  EXPECT_EQ(result_map.count(), 4);
626  EXPECT_EQ(result_map.size(), 2);
627  EXPECT_FALSE(result_map.empty());
628  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
629  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
630  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
631  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
632  EXPECT_EQ(result_map.getContainer().size(), 2);
633  it = result_map.find(key1);
634  EXPECT_TRUE(it != result_map.end());
635  EXPECT_EQ(it->second.size(), 2);
636 
637  it = result_map.find(key2);
638  EXPECT_TRUE(it != result_map.end());
639  EXPECT_EQ(it->second.size(), 2);
640 
641  // test release
642  result_map.release();
643  EXPECT_EQ(result_map.count(), 0);
644  EXPECT_EQ(result_map.size(), 0);
645  EXPECT_TRUE(result_map.empty());
646  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
647  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
648  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
649  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
650  EXPECT_TRUE(result_map.getContainer().empty());
651  }
652 
653  { // flatten move
654  tesseract_collision::ContactResultMap result_map;
655  result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
656  result_map.addContactResult(key2, tesseract_collision::ContactResult{});
657  EXPECT_EQ(result_map.count(), 3);
658  EXPECT_EQ(result_map.size(), 2);
659  EXPECT_FALSE(result_map.empty());
660  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
661  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
662  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
663  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
664  EXPECT_EQ(result_map.getContainer().size(), 2);
665  auto it = result_map.find(key1);
666  EXPECT_TRUE(it != result_map.end());
667  EXPECT_EQ(it->second.size(), 2);
668  it = result_map.find(key2);
669  EXPECT_TRUE(it != result_map.end());
670  EXPECT_EQ(it->second.size(), 1);
671 
672  tesseract_collision::ContactResultVector result_vector;
673  result_map.flattenMoveResults(result_vector);
674  EXPECT_EQ(result_vector.size(), 3);
675 
676  EXPECT_EQ(result_map.count(), 0);
677  EXPECT_EQ(result_map.size(), 0);
678  EXPECT_TRUE(result_map.empty());
679  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
680  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
681  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
682  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
683  EXPECT_EQ(result_map.getContainer().size(), 2);
684  it = result_map.find(key1);
685  EXPECT_TRUE(it != result_map.end());
686  EXPECT_EQ(it->second.size(), 0);
687  EXPECT_TRUE(it->second.capacity() > 0);
688 
689  it = result_map.find(key2);
690  EXPECT_TRUE(it != result_map.end());
691  EXPECT_EQ(it->second.size(), 0);
692  EXPECT_TRUE(it->second.capacity() > 0);
693  }
694 
695  { // flatten copy
696  tesseract_collision::ContactResultMap result_map;
697  result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
698  result_map.setContactResult(key2, tesseract_collision::ContactResult{});
699  EXPECT_EQ(result_map.count(), 3);
700  EXPECT_EQ(result_map.size(), 2);
701  EXPECT_FALSE(result_map.empty());
702  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
703  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
704  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
705  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
706  EXPECT_EQ(result_map.getContainer().size(), 2);
707  auto it = result_map.find(key1);
708  EXPECT_TRUE(it != result_map.end());
709  EXPECT_EQ(it->second.size(), 2);
710  it = result_map.find(key2);
711  EXPECT_TRUE(it != result_map.end());
712  EXPECT_EQ(it->second.size(), 1);
713 
714  tesseract_collision::ContactResultVector result_vector;
715  result_map.flattenCopyResults(result_vector);
716  EXPECT_EQ(result_vector.size(), 3);
717 
718  EXPECT_EQ(result_map.count(), 3);
719  EXPECT_EQ(result_map.size(), 2);
720  EXPECT_FALSE(result_map.empty());
721  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
722  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
723  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
724  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
725  EXPECT_EQ(result_map.getContainer().size(), 2);
726  it = result_map.find(key1);
727  EXPECT_TRUE(it != result_map.end());
728  EXPECT_EQ(it->second.size(), 2);
729 
730  it = result_map.find(key2);
731  EXPECT_TRUE(it != result_map.end());
732  EXPECT_EQ(it->second.size(), 1);
733  }
734 
735  { // flatten reference wrapper
736  tesseract_collision::ContactResultMap result_map;
737  result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
738  result_map.addContactResult(key2, tesseract_collision::ContactResult{});
739  EXPECT_EQ(result_map.count(), 3);
740  EXPECT_EQ(result_map.size(), 2);
741  EXPECT_FALSE(result_map.empty());
742  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
743  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
744  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
745  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
746  EXPECT_EQ(result_map.getContainer().size(), 2);
747  auto it = result_map.find(key1);
748  EXPECT_TRUE(it != result_map.end());
749  EXPECT_EQ(it->second.size(), 2);
750  it = result_map.find(key2);
751  EXPECT_TRUE(it != result_map.end());
752  EXPECT_EQ(it->second.size(), 1);
753 
754  std::vector<std::reference_wrapper<tesseract_collision::ContactResult>> result_vector;
755  result_map.flattenWrapperResults(result_vector);
756  EXPECT_EQ(result_vector.size(), 3);
757 
758  EXPECT_EQ(result_map.count(), 3);
759  EXPECT_EQ(result_map.size(), 2);
760  EXPECT_FALSE(result_map.empty());
761  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
762  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
763  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
764  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
765  EXPECT_EQ(result_map.getContainer().size(), 2);
766  it = result_map.find(key1);
767  EXPECT_TRUE(it != result_map.end());
768  EXPECT_EQ(it->second.size(), 2);
769 
770  it = result_map.find(key2);
771  EXPECT_TRUE(it != result_map.end());
772  EXPECT_EQ(it->second.size(), 1);
773  }
774 
775  { // flatten reference wrapper const
776  tesseract_collision::ContactResultMap result_map;
777  result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
778  result_map.setContactResult(key2, tesseract_collision::ContactResult{});
779  EXPECT_EQ(result_map.count(), 3);
780  EXPECT_EQ(result_map.size(), 2);
781  EXPECT_FALSE(result_map.empty());
782  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
783  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
784  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
785  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
786  EXPECT_EQ(result_map.getContainer().size(), 2);
787  auto it = result_map.find(key1);
788  EXPECT_TRUE(it != result_map.end());
789  EXPECT_EQ(it->second.size(), 2);
790  it = result_map.find(key2);
791  EXPECT_TRUE(it != result_map.end());
792  EXPECT_EQ(it->second.size(), 1);
793 
794  std::vector<std::reference_wrapper<const tesseract_collision::ContactResult>> result_vector;
795  result_map.flattenWrapperResults(result_vector);
796  EXPECT_EQ(result_vector.size(), 3);
797 
798  EXPECT_EQ(result_map.count(), 3);
799  EXPECT_EQ(result_map.size(), 2);
800  EXPECT_FALSE(result_map.empty());
801  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
802  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
803  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
804  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
805  EXPECT_EQ(result_map.getContainer().size(), 2);
806  it = result_map.find(key1);
807  EXPECT_TRUE(it != result_map.end());
808  EXPECT_EQ(it->second.size(), 2);
809 
810  it = result_map.find(key2);
811  EXPECT_TRUE(it != result_map.end());
812  EXPECT_EQ(it->second.size(), 1);
813  }
814 
815  { // filter
816  tesseract_collision::ContactResultMap result_map;
817  result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} });
818  result_map.addContactResult(key2, tesseract_collision::ContactResult{});
819  EXPECT_EQ(result_map.count(), 3);
820  EXPECT_EQ(result_map.size(), 2);
821  EXPECT_FALSE(result_map.empty());
822  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
823  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
824  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
825  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
826  EXPECT_EQ(result_map.getContainer().size(), 2);
827  auto it = result_map.find(key1);
828  EXPECT_TRUE(it != result_map.end());
829  EXPECT_EQ(it->second.size(), 2);
830  it = result_map.find(key2);
831  EXPECT_TRUE(it != result_map.end());
832  EXPECT_EQ(it->second.size(), 1);
833 
834  auto filter = [key1](tesseract_collision::ContactResultMap::PairType& pair) {
835  if (key1 == pair.first)
836  pair.second.clear();
837  };
838  result_map.filter(filter);
839 
840  EXPECT_EQ(result_map.count(), 1);
841  EXPECT_EQ(result_map.size(), 1);
842  EXPECT_FALSE(result_map.empty());
843  EXPECT_TRUE(result_map.begin() == result_map.getContainer().begin());
844  EXPECT_TRUE(result_map.end() == result_map.getContainer().end());
845  EXPECT_TRUE(result_map.cbegin() == result_map.getContainer().cbegin());
846  EXPECT_TRUE(result_map.cend() == result_map.getContainer().cend());
847  EXPECT_EQ(result_map.getContainer().size(), 2);
848  it = result_map.find(key1);
849  EXPECT_TRUE(it != result_map.end());
850  EXPECT_EQ(it->second.size(), 0);
851  EXPECT_TRUE(it->second.capacity() > 0);
852 
853  it = result_map.find(key2);
854  EXPECT_TRUE(it != result_map.end());
855  EXPECT_EQ(it->second.size(), 1);
856  }
857 }
858 
859 TEST(TesseractCoreUnit, ContactRequestUnit) // NOLINT
860 {
861  {
862  tesseract_collision::ContactRequest request;
863  EXPECT_EQ(request.type, tesseract_collision::ContactTestType::ALL);
864  EXPECT_TRUE(request.calculate_penetration);
865  EXPECT_TRUE(request.calculate_distance);
866  EXPECT_EQ(request.contact_limit, 0);
867  EXPECT_TRUE(request.is_valid == nullptr);
868 
869  tesseract_common::testSerialization<tesseract_collision::ContactRequest>(request, "ContactRequest");
870  }
871 
872  {
873  tesseract_collision::ContactRequest request(tesseract_collision::ContactTestType::FIRST);
874  EXPECT_EQ(request.type, tesseract_collision::ContactTestType::FIRST);
875  EXPECT_TRUE(request.calculate_penetration);
876  EXPECT_TRUE(request.calculate_distance);
877  EXPECT_EQ(request.contact_limit, 0);
878  EXPECT_TRUE(request.is_valid == nullptr);
879 
880  tesseract_common::testSerialization<tesseract_collision::ContactRequest>(request, "ContactRequest");
881  }
882 }
883 
884 TEST(TesseractCoreUnit, CollisionCheckConfigUnit) // NOLINT
885 {
886  { // Default Constructor
887  tesseract_collision::CollisionCheckConfig config;
888  EXPECT_EQ(config.contact_request, tesseract_collision::ContactRequest());
889  EXPECT_EQ(config.type, tesseract_collision::CollisionEvaluatorType::DISCRETE);
890  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(config.longest_valid_segment_length, 0.005));
891  EXPECT_EQ(config.check_program_mode, tesseract_collision::CollisionCheckProgramType::ALL);
892 
893  tesseract_common::testSerialization<tesseract_collision::CollisionCheckConfig>(config, "CollisionCheckConfig");
894  }
895  {
896  tesseract_collision::ContactRequest request;
897  tesseract_collision::CollisionCheckConfig config(request,
898  tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE,
899  0.5,
900  tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START);
901 
902  EXPECT_EQ(config.contact_request, request);
903  EXPECT_EQ(config.type, tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE);
904  EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(config.longest_valid_segment_length, 0.5));
905  EXPECT_EQ(config.check_program_mode, tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START);
906 
907  tesseract_common::testSerialization<tesseract_collision::CollisionCheckConfig>(config, "CollisionCheckConfig");
908  }
909 }
910 
911 TEST(TesseractCoreUnit, ContactTrajectorySubstepResultsUnit) // NOLINT
912 {
913  // Test constructor with start and end states
914  {
915  int substep_number = 5;
916  Eigen::VectorXd start_state(3);
917  Eigen::VectorXd end_state(3);
918  start_state << 1.0, 2.0, 3.0;
919  end_state << 4.0, 5.0, 6.0;
920 
921  tesseract_collision::ContactTrajectorySubstepResults results(substep_number, start_state, end_state);
922 
923  EXPECT_EQ(results.substep, substep_number);
924  EXPECT_TRUE(results.state0.isApprox(start_state));
925  EXPECT_TRUE(results.state1.isApprox(end_state));
926  EXPECT_EQ(results.numContacts(), 0);
927  }
928 
929  // Test constructor with single state
930  {
931  int substep_number = 3;
932  Eigen::VectorXd state(2);
933  state << 1.5, 2.5;
934 
935  tesseract_collision::ContactTrajectorySubstepResults results(substep_number, state);
936 
937  EXPECT_EQ(results.substep, substep_number);
938  EXPECT_TRUE(results.state0.isApprox(state));
939  EXPECT_TRUE(results.state1.isApprox(state));
940  EXPECT_EQ(results.numContacts(), 0);
941  }
942 
943  // Test with contacts
944  {
945  int substep_number = 2;
946  Eigen::VectorXd state(1);
947  state << 1.0;
948 
949  tesseract_collision::ContactTrajectorySubstepResults results(substep_number, state);
950 
951  // Create contact results
952  auto key1 = tesseract_common::makeOrderedLinkPair("link1", "link2");
953  auto key2 = tesseract_common::makeOrderedLinkPair("link3", "link4");
954 
955  tesseract_collision::ContactResult cr1;
956  cr1.distance = -0.1;
957  cr1.link_names[0] = "link1";
958  cr1.link_names[1] = "link2";
959 
960  tesseract_collision::ContactResult cr2;
961  cr2.distance = -0.2;
962  cr2.link_names[0] = "link3";
963  cr2.link_names[1] = "link4";
964 
965  tesseract_collision::ContactResultVector crv1 = { cr1 };
966  tesseract_collision::ContactResultVector crv2 = { cr2 };
967 
968  results.contacts.addContactResult(key1, crv1);
969  results.contacts.addContactResult(key2, crv2);
970 
971  EXPECT_EQ(results.numContacts(), 2);
972 
973  // Test worstCollision
974  tesseract_collision::ContactResultVector worst = results.worstCollision();
975  EXPECT_EQ(worst.size(), 1);
976  EXPECT_EQ(worst[0].distance, -0.2);
977  EXPECT_EQ(worst[0].link_names[0], "link3");
978  EXPECT_EQ(worst[0].link_names[1], "link4");
979  }
980 
981  // Test with no contacts
982  {
983  tesseract_collision::ContactTrajectorySubstepResults results;
984  tesseract_collision::ContactResultVector worst = results.worstCollision();
985  EXPECT_EQ(worst.size(), 0);
986  }
987 }
988 TEST(TesseractCoreUnit, ContactTrajectoryStepResultsUnit) // NOLINT
989 {
990  // Test constructor with start and end states and num_substeps
991  {
992  int step_number = 3;
993  Eigen::VectorXd start_state(3);
994  Eigen::VectorXd end_state(3);
995  start_state << 1.0, 2.0, 3.0;
996  end_state << 4.0, 5.0, 6.0;
997  int num_substeps = 5;
998 
999  tesseract_collision::ContactTrajectoryStepResults results(step_number, start_state, end_state, num_substeps);
1000 
1001  EXPECT_EQ(results.step, step_number);
1002  EXPECT_TRUE(results.state0.isApprox(start_state));
1003  EXPECT_TRUE(results.state1.isApprox(end_state));
1004  EXPECT_EQ(results.total_substeps, num_substeps);
1005  EXPECT_EQ(results.substeps.size(), num_substeps);
1006  EXPECT_EQ(results.numSubsteps(), num_substeps);
1007  EXPECT_EQ(results.numContacts(), 0);
1008  }
1009 
1010  // Test constructor with single state
1011  {
1012  int step_number = 2;
1013  Eigen::VectorXd state(2);
1014  state << 1.5, 2.5;
1015 
1016  tesseract_collision::ContactTrajectoryStepResults results(step_number, state);
1017  // The constructor with a single state defaults to 2 substeps
1018  int expected_substeps = 2;
1019 
1020  EXPECT_EQ(results.step, step_number);
1021  EXPECT_TRUE(results.state0.isApprox(state));
1022  EXPECT_TRUE(results.state1.isApprox(state));
1023  EXPECT_EQ(results.total_substeps, expected_substeps);
1024  EXPECT_EQ(results.substeps.size(), expected_substeps);
1025  EXPECT_EQ(results.numSubsteps(), expected_substeps);
1026  EXPECT_EQ(results.numContacts(), 0);
1027  }
1028 
1029  // Test resize method
1030  {
1031  int step_number = 1;
1032  Eigen::VectorXd state(1);
1033  state << 1.0;
1034 
1035  tesseract_collision::ContactTrajectoryStepResults results(step_number, state);
1036  EXPECT_EQ(results.substeps.size(), 2); // Default is 2
1037 
1038  int new_size = 4;
1039  results.resize(new_size);
1040 
1041  EXPECT_EQ(results.total_substeps, new_size);
1042  EXPECT_EQ(results.substeps.size(), new_size);
1043  EXPECT_EQ(results.numSubsteps(), new_size);
1044  }
1045 
1046  // Test with contacts in substeps
1047  {
1048  int step_number = 1;
1049  Eigen::VectorXd start_state(2);
1050  Eigen::VectorXd end_state(2);
1051  start_state << 0.0, 0.0;
1052  end_state << 1.0, 1.0;
1053  int num_substeps = 3;
1054 
1055  tesseract_collision::ContactTrajectoryStepResults results(step_number, start_state, end_state, num_substeps);
1056 
1057  // Initialize substep states and indices
1058  for (size_t i = 0; i < results.substeps.size(); ++i)
1059  {
1060  auto& substep = results.substeps[i];
1061  if (substep.state0.size() == 0)
1062  substep.state0 = start_state;
1063  if (substep.state1.size() == 0)
1064  substep.state1 = end_state;
1065  if (substep.substep < 0)
1066  substep.substep = static_cast<int>(i);
1067  }
1068 
1069  // Create contact results for first substep
1070  auto key1 = tesseract_common::makeOrderedLinkPair("link1", "link2");
1071  auto key2 = tesseract_common::makeOrderedLinkPair("link3", "link4");
1072 
1073  tesseract_collision::ContactResult cr1;
1074  cr1.distance = -0.1;
1075  cr1.link_names[0] = "link1";
1076  cr1.link_names[1] = "link2";
1077 
1078  tesseract_collision::ContactResult cr2;
1079  cr2.distance = -0.2;
1080  cr2.link_names[0] = "link3";
1081  cr2.link_names[1] = "link4";
1082 
1083  tesseract_collision::ContactResultVector crv1 = { cr1 };
1084  tesseract_collision::ContactResultVector crv2 = { cr2 };
1085 
1086  results.substeps[0].contacts.addContactResult(key1, crv1);
1087 
1088  // Create contact results for second substep
1089  tesseract_collision::ContactResult cr3;
1090  cr3.distance = -0.3;
1091  cr3.link_names[0] = "link1";
1092  cr3.link_names[1] = "link2";
1093 
1094  tesseract_collision::ContactResultVector crv3 = { cr3 };
1095 
1096  results.substeps[1].contacts.addContactResult(key1, crv3);
1097  results.substeps[1].contacts.addContactResult(key2, crv2);
1098 
1099  // Test numContacts
1100  EXPECT_EQ(results.numContacts(), 3);
1101 
1102  // Test worstSubstep
1103  tesseract_collision::ContactTrajectorySubstepResults worst_substep = results.worstSubstep();
1104  EXPECT_EQ(worst_substep.substep, 1);
1105 
1106  // Test worstCollision
1107  tesseract_collision::ContactResultVector worst_collision = results.worstCollision();
1108  EXPECT_EQ(worst_collision.size(), 1);
1109  if (!worst_collision.empty())
1110  {
1111  EXPECT_EQ(worst_collision[0].distance, -0.3);
1112  }
1113 
1114  // Test mostCollisionsSubstep
1115  tesseract_collision::ContactTrajectorySubstepResults most_collisions = results.mostCollisionsSubstep();
1116  EXPECT_EQ(most_collisions.substep, 1);
1117  EXPECT_EQ(most_collisions.numContacts(), 2);
1118  }
1119 
1120  // Test with no contacts
1121  {
1122  int step_number = 0;
1123  Eigen::VectorXd state(1);
1124  state << 0.0;
1125 
1126  tesseract_collision::ContactTrajectoryStepResults results(step_number, state);
1127 
1128  // Explicitly initialize substep states and indices
1129  for (size_t i = 0; i < results.substeps.size(); ++i)
1130  {
1131  auto& substep = results.substeps[i];
1132  if (substep.state0.size() == 0)
1133  substep.state0 = state;
1134  if (substep.state1.size() == 0)
1135  substep.state1 = state;
1136  if (substep.substep < 0)
1137  substep.substep = static_cast<int>(i);
1138  }
1139 
1140  EXPECT_EQ(results.numContacts(), 0);
1141 
1142  tesseract_collision::ContactTrajectorySubstepResults worst_substep = results.worstSubstep();
1143  EXPECT_EQ(worst_substep.substep, -1); // Expect default index when no contacts
1144 
1145  tesseract_collision::ContactResultVector worst_collision = results.worstCollision();
1146  EXPECT_EQ(worst_collision.size(), 0);
1147 
1148  tesseract_collision::ContactTrajectorySubstepResults most_collisions = results.mostCollisionsSubstep();
1149  EXPECT_EQ(most_collisions.substep, -1); // Expect default index when no contacts
1150  EXPECT_EQ(most_collisions.numContacts(), 0);
1151  }
1152 }
1153 
1154 TEST(TesseractCoreUnit, ContactTrajectoryResultsUnit) // NOLINT
1155 {
1156  // Test constructor with joint names
1157  {
1158  std::vector<std::string> joint_names = { "joint1", "joint2", "joint3" };
1159  tesseract_collision::ContactTrajectoryResults results(joint_names);
1160 
1161  EXPECT_EQ(results.joint_names, joint_names);
1162  EXPECT_EQ(results.total_steps, 0);
1163  EXPECT_EQ(results.steps.size(), 0);
1164  EXPECT_EQ(results.numSteps(), 0);
1165  EXPECT_EQ(results.numContacts(), 0);
1166  }
1167 
1168  // Test constructor with joint names and num_steps
1169  {
1170  std::vector<std::string> joint_names = { "joint1", "joint2" };
1171  int num_steps = 3;
1172 
1173  tesseract_collision::ContactTrajectoryResults results(joint_names, num_steps);
1174 
1175  EXPECT_EQ(results.joint_names, joint_names);
1176  EXPECT_EQ(results.total_steps, num_steps);
1177  EXPECT_EQ(results.steps.size(), num_steps);
1178  EXPECT_EQ(results.numSteps(), num_steps);
1179  EXPECT_EQ(results.numContacts(), 0);
1180  }
1181 
1182  // Test resize method
1183  {
1184  std::vector<std::string> joint_names = { "joint1" };
1185 
1186  tesseract_collision::ContactTrajectoryResults results(joint_names);
1187  EXPECT_EQ(results.steps.size(), 0);
1188 
1189  results.resize(5);
1190  EXPECT_EQ(results.total_steps, 5);
1191  EXPECT_EQ(results.steps.size(), 5);
1192  EXPECT_EQ(results.numSteps(), 5);
1193  }
1194 
1195  // Test with contacts in steps
1196  {
1197  std::vector<std::string> joint_names = { "joint1", "joint2" };
1198  int num_steps = 2;
1199 
1200  tesseract_collision::ContactTrajectoryResults results(joint_names, num_steps);
1201 
1202  // Setup Step 1
1203  Eigen::VectorXd step1_start(2);
1204  Eigen::VectorXd step1_end(2);
1205  step1_start << 0.0, 0.0;
1206  step1_end << 1.0, 1.0;
1207 
1208  results.steps[0].step = 0;
1209  results.steps[0].state0 = step1_start;
1210  results.steps[0].state1 = step1_end;
1211  results.steps[0].resize(2); // 2 substeps for first step
1212 
1213  // Create contact results for step 1, substep 0
1214  auto key1 = tesseract_common::makeOrderedLinkPair("link1", "link2");
1215 
1216  tesseract_collision::ContactResult cr1;
1217  cr1.distance = -0.1;
1218  cr1.link_names[0] = "link1";
1219  cr1.link_names[1] = "link2";
1220 
1221  tesseract_collision::ContactResultVector crv1 = { cr1 };
1222 
1223  results.steps[0].substeps[0].substep = 0;
1224  results.steps[0].substeps[0].contacts.addContactResult(key1, crv1);
1225 
1226  // Setup Step 2
1227  Eigen::VectorXd step2_start(2);
1228  Eigen::VectorXd step2_end(2);
1229  step2_start << 1.0, 1.0;
1230  step2_end << 2.0, 2.0;
1231 
1232  results.steps[1].step = 1;
1233  results.steps[1].state0 = step2_start;
1234  results.steps[1].state1 = step2_end;
1235  results.steps[1].resize(3); // 3 substeps for second step
1236 
1237  // Create contact results for step 2, substep 1
1238  auto key2 = tesseract_common::makeOrderedLinkPair("link3", "link4");
1239 
1240  tesseract_collision::ContactResult cr2;
1241  cr2.distance = -0.3;
1242  cr2.link_names[0] = "link3";
1243  cr2.link_names[1] = "link4";
1244 
1245  tesseract_collision::ContactResultVector crv2 = { cr2 };
1246 
1247  results.steps[1].substeps[1].substep = 1;
1248  results.steps[1].substeps[1].contacts.addContactResult(key2, crv2);
1249 
1250  // Create another contact for step 2, substep 2
1251  tesseract_collision::ContactResult cr3;
1252  cr3.distance = -0.2;
1253  cr3.link_names[0] = "link3";
1254  cr3.link_names[1] = "link4";
1255 
1256  tesseract_collision::ContactResultVector crv3 = { cr3 };
1257 
1258  results.steps[1].substeps[2].substep = 2;
1259  results.steps[1].substeps[2].contacts.addContactResult(key2, crv3);
1260 
1261  // Test numContacts
1262  EXPECT_EQ(results.numContacts(), 3);
1263 
1264  // Test worstStep
1265  tesseract_collision::ContactTrajectoryStepResults worst_step = results.worstStep();
1266  EXPECT_EQ(worst_step.step, 1);
1267 
1268  // Test worstCollision
1269  tesseract_collision::ContactResultVector worst_collision = results.worstCollision();
1270  EXPECT_EQ(worst_collision.size(), 1);
1271  EXPECT_EQ(worst_collision[0].distance, -0.3);
1272  EXPECT_EQ(worst_collision[0].link_names[0], "link3");
1273  EXPECT_EQ(worst_collision[0].link_names[1], "link4");
1274 
1275  // Test mostCollisionsStep
1276  tesseract_collision::ContactTrajectoryStepResults most_collisions = results.mostCollisionsStep();
1277  EXPECT_EQ(most_collisions.step, 1);
1278  EXPECT_EQ(most_collisions.numContacts(), 2);
1279 
1280  // Test table output methods - only checking that they don't crash
1281  EXPECT_NO_THROW(results.trajectoryCollisionResultsTable());
1282 
1283  std::stringstream ss = results.trajectoryCollisionResultsTable();
1284  EXPECT_FALSE(ss.str().empty());
1285 
1286  // Test frequency output method
1287  EXPECT_NO_THROW(results.collisionFrequencyPerLink());
1288 
1289  std::stringstream freq_ss = results.collisionFrequencyPerLink();
1290  EXPECT_FALSE(freq_ss.str().empty());
1291  }
1292 
1293  // Test with no contacts
1294  {
1295  std::vector<std::string> joint_names = { "joint1" };
1296  tesseract_collision::ContactTrajectoryResults results(joint_names);
1297 
1298  EXPECT_EQ(results.numContacts(), 0);
1299 
1300  tesseract_collision::ContactTrajectoryStepResults worst_step = results.worstStep();
1301  EXPECT_EQ(worst_step.step, -1);
1302 
1303  tesseract_collision::ContactResultVector worst_collision = results.worstCollision();
1304  EXPECT_EQ(worst_collision.size(), 0);
1305 
1306  tesseract_collision::ContactTrajectoryStepResults most_collisions = results.mostCollisionsStep();
1307  EXPECT_EQ(most_collisions.step, -1);
1308  EXPECT_EQ(most_collisions.numContacts(), 0);
1309 
1310  // Empty results should still produce output without crashing
1311  EXPECT_NO_THROW(results.trajectoryCollisionResultsTable());
1312  EXPECT_NO_THROW(results.collisionFrequencyPerLink());
1313 
1314  std::stringstream ss = results.trajectoryCollisionResultsTable();
1315  EXPECT_FALSE(ss.str().empty());
1316  EXPECT_TRUE(ss.str().find("No contacts detected") != std::string::npos);
1317 
1318  std::stringstream freq_ss = results.collisionFrequencyPerLink();
1319  EXPECT_FALSE(freq_ss.str().empty());
1320  EXPECT_TRUE(freq_ss.str().find("No contacts detected") != std::string::npos);
1321  }
1322 }
1323 
1324 int main(int argc, char** argv)
1325 {
1326  testing::InitGoogleTest(&argc, argv);
1327 
1328  return RUN_ALL_TESTS();
1329 }
tesseract_collision::ContactResultMap::empty
bool empty() const
Check if results are present.
Definition: types.cpp:229
tesseract_collision::ContactResultMap::end
ConstIteratorType end() const
returns an iterator to the end
Definition: types.cpp:260
tesseract_collision::ContactManagerConfig::validate
void validate() const
Check for errors and throw exception if they exist.
Definition: types.cpp:395
tesseract_collision::isContactAllowed
bool isContactAllowed(const std::string &name1, const std::string &name2, const std::shared_ptr< const tesseract_common::ContactAllowedValidator > &validator, bool verbose=false)
Determine if contact is allowed between two objects.
Definition: common.cpp:85
tesseract_common::ContactAllowedValidator
tesseract_common::CollisionMarginPairData::getCollisionMargin
std::optional< double > getCollisionMargin(const std::string &obj1, const std::string &obj2) const
tesseract_collision::ContactTrajectorySubstepResults::numContacts
int numContacts() const
Definition: types.cpp:454
tesseract_collision::ContactManagerConfig
Contains parameters used to configure a contact manager before a series of contact checks.
Definition: types.h:420
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::ContactResult::cc_time
std::array< double, 2 > cc_time
This is between 0 and 1 indicating the point of contact.
Definition: types.h:111
tesseract_collision::ContactTrajectoryStepResults::total_substeps
int total_substeps
Definition: types.h:555
tesseract_collision::ContactTrajectoryResults::resize
void resize(int num_steps)
Definition: types.cpp:555
contact_allowed_validator.h
tesseract_common::AllowedCollisionMatrix::getAllAllowedCollisions
const AllowedCollisionEntries & getAllAllowedCollisions() const
types.h
Tesseracts Collision Forward Declarations.
tesseract_collision::ContactResultMap::setContactResult
ContactResult & setContactResult(const KeyType &key, ContactResult result)
Set contact results for the provided key.
Definition: types.cpp:122
TestContactAllowedValidator::operator()
bool operator()(const std::string &s1, const std::string &s2) const override
Definition: collision_core_unit.cpp:18
tesseract_collision::ContactResult::shape_id
std::array< int, 2 > shape_id
The two shapes that are in contact. Each link can be made up of multiple shapes.
Definition: types.h:94
tesseract_collision::ACMOverrideType::OR
@ OR
New ContactAllowedValidator combines the contact manager fn and the ACM generated fn with and OR.
utils.h
tesseract_collision::ContactTrajectoryStepResults::mostCollisionsSubstep
ContactTrajectorySubstepResults mostCollisionsSubstep() const
Definition: types.cpp:528
tesseract_collision::ContactTrajectoryResults::mostCollisionsStep
ContactTrajectoryStepResults mostCollisionsStep() const
Definition: types.cpp:593
tesseract_collision::ContactResultMap::size
std::size_t size() const
Get the size of the map.
Definition: types.cpp:214
tesseract_common::almostEqualRelativeAndAbs
bool almostEqualRelativeAndAbs(const Eigen::Ref< const Eigen::VectorXd > &v1, const Eigen::Ref< const Eigen::VectorXd > &v2, const Eigen::Ref< const Eigen::VectorXd > &max_diff, const Eigen::Ref< const Eigen::VectorXd > &max_rel_diff)
tesseract_collision::ContactResultMap::addContactResult
ContactResult & addContactResult(const KeyType &key, ContactResult result)
Add contact results for the provided key.
Definition: types.cpp:103
tesseract_collision::ContactResultMap::getSummary
std::string getSummary() const
Get a brief summary of the most frequently colliding link pair.
Definition: types.cpp:325
tesseract_common::CollisionMarginPairData::empty
bool empty() const
tesseract_collision::ContactRequest::type
ContactTestType type
This controls the exit condition for the contact test type.
Definition: types.h:303
tesseract_collision::ContactTrajectoryResults::numContacts
int numContacts() const
Definition: types.cpp:563
tesseract_collision::ContactTrajectoryResults::collisionFrequencyPerLink
std::stringstream collisionFrequencyPerLink() const
Definition: types.cpp:842
tesseract_collision::ContactResult::normal
Eigen::Vector3d normal
The normal vector to move the two objects out of contact in world coordinates.
Definition: types.h:109
tesseract_collision::ContactManagerConfig::incrementMargins
void incrementMargins(double increment)
Increment all margins by input amount. Useful for inflating or reducing margins.
Definition: types.cpp:377
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
tesseract_collision::ContactResultVector
tesseract_common::AlignedVector< ContactResult > ContactResultVector
Definition: types.h:136
tesseract_collision::ContactTrajectoryStepResults::substeps
std::vector< ContactTrajectorySubstepResults > substeps
Definition: types.h:551
tesseract_collision::ContactResultMap::clear
void clear()
This is a consurvative clear.
Definition: types.cpp:231
tesseract_collision::ContactTrajectoryStepResults::numContacts
int numContacts() const
Definition: types.cpp:495
tesseract_collision::ContactTrajectoryResults::total_steps
int total_steps
Definition: types.h:593
tesseract_collision::ContactRequest::contact_limit
long contact_limit
This is used if the ContactTestType is set to LIMITED, where the test will exit when number of contac...
Definition: types.h:313
tesseract_collision::ContactTrajectoryResults::worstCollision
tesseract_collision::ContactResultVector worstCollision() const
Definition: types.cpp:587
tesseract_collision::ContactTrajectoryResults
The ContactTrajectoryResults struct is the top level struct for tracking contacts in a trajectory....
Definition: types.h:563
tesseract_collision::ContactResultMap::filter
void filter(const FilterFn &filter)
Filter out results using the provided function.
Definition: types.cpp:306
tesseract_collision::CollisionCheckConfig::type
CollisionEvaluatorType type
Specifies the type of collision check to be performed. Default: DISCRETE.
Definition: types.h:480
tesseract_collision::ContactResultMap::getContainer
const ContainerType & getContainer() const
Get the underlying container.
Definition: types.cpp:256
tesseract_collision::ContactManagerConfig::modify_object_enabled
std::unordered_map< std::string, bool > modify_object_enabled
Each key is an object name. Objects will be enabled/disabled based on the value. Objects that aren't ...
Definition: types.h:440
EXPECT_TRUE
#define EXPECT_TRUE(args)
tesseract_collision::ContactResultMap::flattenMoveResults
void flattenMoveResults(ContactResultVector &v)
Definition: types.cpp:270
tesseract_common::CollisionMarginPairData::setCollisionMargin
void setCollisionMargin(const std::string &obj1, const std::string &obj2, double margin)
tesseract_collision::CollisionCheckConfig::check_program_mode
CollisionCheckProgramType check_program_mode
Secifies the mode used when collision checking program/trajectory. Default: ALL.
Definition: types.h:486
tesseract_collision::ContinuousCollisionType::CCType_Between
@ CCType_Between
tesseract_collision::ContactResultMap::flattenWrapperResults
void flattenWrapperResults(std::vector< std::reference_wrapper< ContactResult >> &v)
Definition: types.cpp:290
tesseract_collision::scaleVertices
void scaleVertices(tesseract_common::VectorVector3d &vertices, const Eigen::Vector3d &center, const Eigen::Vector3d &scale)
Apply scaling to the geometry coordinates.
Definition: common.cpp:155
tesseract_collision::ContactManagerConfig::scaleMargins
void scaleMargins(double scale)
Scale all margins by input value.
Definition: types.cpp:386
tesseract_collision::getCollisionObjectPairs
std::vector< ObjectPairKey > getCollisionObjectPairs(const std::vector< std::string > &active_links, const std::vector< std::string > &static_links, const std::shared_ptr< const tesseract_common::ContactAllowedValidator > &validator=nullptr)
Get a vector of possible collision object pairs.
Definition: common.cpp:45
tesseract_collision::ContactResult::subshape_id
std::array< int, 2 > subshape_id
Some shapes like octomap and mesh have subshape (boxes and triangles)
Definition: types.h:96
tesseract_collision::ContactResultMap
This structure hold contact results for link pairs.
Definition: types.h:155
tesseract_collision::ContactTrajectoryStepResults::resize
void resize(int num_substeps)
Definition: types.cpp:487
tesseract_collision::ContactResultMap::release
void release()
Fully clear all internal data.
Definition: types.cpp:250
tesseract_collision::ContactTrajectoryStepResults::worstCollision
tesseract_collision::ContactResultVector worstCollision() const
Definition: types.cpp:520
tesseract_collision::CollisionCheckConfig::contact_request
ContactRequest contact_request
ContactRequest that will be used for this check. Default test type: ALL.
Definition: types.h:477
tesseract_collision::ContactManagerConfig::default_margin
std::optional< double > default_margin
override the default contact margin
Definition: types.h:426
tesseract_collision::ContactTrajectorySubstepResults
The ContactTrajectorySubstepResults struct is the lowest level struct for tracking contacts in a traj...
Definition: types.h:497
tesseract_collision::CollisionCheckConfig
This is a high level structure containing common information that collision checking utilities need....
Definition: types.h:469
tesseract_collision::ContinuousCollisionType::CCType_None
@ CCType_None
tesseract_collision::ContactTrajectoryStepResults::state0
Eigen::VectorXd state0
Definition: types.h:553
tesseract_collision::ContactTrajectoryStepResults::numSubsteps
int numSubsteps() const
Definition: types.cpp:493
tesseract_collision::ContactTrajectoryResults::worstStep
ContactTrajectoryStepResults worstStep() const
Definition: types.cpp:571
tesseract_collision::ContactTrajectorySubstepResults::contacts
tesseract_collision::ContactResultMap contacts
Definition: types.h:513
tesseract_collision::ContactTrajectoryStepResults::step
int step
Definition: types.h:552
tesseract_collision::ContactResultMap::PairType
typename std::pair< const KeyType, MappedType > PairType
Definition: types.h:164
tesseract_common::VectorVector3d
std::vector< Eigen::Vector3d > VectorVector3d
tesseract_collision::ContactRequest::is_valid
std::shared_ptr< const ContactResultValidator > is_valid
This provides a user defined function approve/reject contact results.
Definition: types.h:316
TestContactAllowedValidator
Definition: collision_core_unit.cpp:15
tesseract_collision::ContactResult::type_id
std::array< int, 2 > type_id
A user defined type id that is added to the contact shapes.
Definition: types.h:90
TESSERACT_COMMON_IGNORE_WARNINGS_POP
Definition: create_convex_hull.cpp:37
distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
tesseract_collision::ContactResult::clear
void clear()
reset to default values
Definition: types.cpp:36
tesseract_collision::ContinuousCollisionType::CCType_Time0
@ CCType_Time0
tesseract_collision::CollisionCheckProgramType::ALL
@ ALL
Check all states.
main
int main(int argc, char **argv)
Definition: collision_core_unit.cpp:1324
tesseract_collision::ContactTrajectorySubstepResults::state1
Eigen::VectorXd state1
Definition: types.h:516
tesseract_collision::ContactTrajectorySubstepResults::substep
int substep
Definition: types.h:514
common.h
This is a collection of common methods.
tesseract_collision::ContactTrajectoryResults::trajectoryCollisionResultsTable
std::stringstream trajectoryCollisionResultsTable() const
Definition: types.cpp:610
tesseract_collision::ContactManagerConfig::pair_margin_override_type
CollisionMarginPairOverrideType pair_margin_override_type
Identify how the collision margin pair data should be applied to the contact manager.
Definition: types.h:429
tesseract_collision::ContactTrajectoryResults::numSteps
int numSteps() const
Definition: types.cpp:561
tesseract_collision::ContactResultMap::cend
ConstIteratorType cend() const
returns an iterator to the end
Definition: types.cpp:264
tesseract_collision::ContactTestType::FIRST
@ FIRST
tesseract_collision::ContactResultMap::find
ConstIteratorType find(const KeyType &key) const
Definition: types.cpp:268
tesseract_collision::ContactResult::distance
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double distance
The distance between two links.
Definition: types.h:88
tesseract_collision::ContactResult::cc_transform
std::array< Eigen::Isometry3d, 2 > cc_transform
The transform of link in world coordinates at its desired final location. Note: This is not the locat...
Definition: types.h:120
tesseract_collision::ContactResultMap::shrinkToFit
void shrinkToFit()
Remove map entries with no contact results.
Definition: types.cpp:243
tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE
@ LVS_DISCRETE
Discrete contact manager interpolating using longest valid segment.
tesseract_collision::ContactResultMap::count
long count() const
Get the total number of contact results storted.
Definition: types.cpp:212
tesseract_collision::ContactResultMap::cbegin
ConstIteratorType cbegin() const
returns an iterator to the beginning
Definition: types.cpp:262
tesseract_common::makeOrderedLinkPair
LinkNamesPair makeOrderedLinkPair(const std::string &link_name1, const std::string &link_name2)
tesseract_collision::ContactResult::transform
std::array< Eigen::Isometry3d, 2 > transform
The transform of link in world coordinates.
Definition: types.h:102
serialization.h
Tesseracts Collision Serialization.
tesseract_collision::CollisionEvaluatorType::DISCRETE
@ DISCRETE
Discrete contact manager using only steps specified.
tesseract_collision::ContactTrajectoryStepResults
The ContactTrajectoryStepResults struct is the second level struct for tracking contacts in a traject...
Definition: types.h:524
tesseract_common::AllowedCollisionMatrix::addAllowedCollision
virtual void addAllowedCollision(const std::string &link_name1, const std::string &link_name2, const std::string &reason)
tesseract_collision::ContactTrajectoryResults::joint_names
std::vector< std::string > joint_names
Definition: types.h:592
tesseract_collision::ContactTrajectoryStepResults::worstSubstep
ContactTrajectorySubstepResults worstSubstep() const
Definition: types.cpp:503
unit_test_utils.h
tesseract_collision::ContactResult::single_contact_point
bool single_contact_point
Some collision checkers only provide a single contact point for a given pair. This is used to indicat...
Definition: types.h:125
tesseract_collision::ContactManagerConfig::pair_margin_data
CollisionMarginPairData pair_margin_data
Stores collision margin pair data.
Definition: types.h:431
tesseract_collision::ContactResult::nearest_points
std::array< Eigen::Vector3d, 2 > nearest_points
The nearest point on both links in world coordinates.
Definition: types.h:98
tesseract_collision::ContactResultMap::begin
ConstIteratorType begin() const
returns an iterator to the beginning
Definition: types.cpp:258
tesseract_collision::ContactManagerConfig::acm
tesseract_common::AllowedCollisionMatrix acm
Additional AllowedCollisionMatrix to consider for this collision check.
Definition: types.h:434
tesseract_collision::ContactResult::nearest_points_local
std::array< Eigen::Vector3d, 2 > nearest_points_local
The nearest point on both links in local(link) coordinates.
Definition: types.h:100
tesseract_collision::ACMOverrideType::NONE
@ NONE
Do not apply AllowedCollisionMatrix.
macros.h
tesseract_collision::ContactTestType::ALL
@ ALL
TEST
TEST(TesseractCoreUnit, ContactManagerConfigTest)
Definition: collision_core_unit.cpp:25
tesseract_collision::ContactResultMap::flattenCopyResults
void flattenCopyResults(ContactResultVector &v) const
Definition: types.cpp:282
tesseract_collision::ContactTrajectoryResults::steps
std::vector< ContactTrajectoryStepResults > steps
Definition: types.h:591
tesseract_collision::ContactResult::link_names
std::array< std::string, 2 > link_names
The two links that are in contact.
Definition: types.h:92
tesseract_collision::ContactResult
Definition: types.h:81
tesseract_collision::ContactRequest
The ContactRequest struct.
Definition: types.h:300
tesseract_collision::ContactTrajectorySubstepResults::state0
Eigen::VectorXd state0
Definition: types.h:515
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
tesseract_collision::ContactTrajectoryStepResults::state1
Eigen::VectorXd state1
Definition: types.h:554
tesseract_collision::ContactResult::cc_type
std::array< ContinuousCollisionType, 2 > cc_type
The type of continuous contact.
Definition: types.h:113
tesseract_collision::ContactManagerConfig::acm_override_type
ACMOverrideType acm_override_type
Specifies how to combine the ContactAllowedValidator from acm with the one preset in the contact mana...
Definition: types.h:436
tesseract_collision::ContactRequest::calculate_penetration
bool calculate_penetration
This enables the calculation of penetration contact data if two objects are in collision.
Definition: types.h:306
tesseract_collision::CollisionCheckConfig::longest_valid_segment_length
double longest_valid_segment_length
Longest valid segment to use if type supports lvs. Default: 0.005.
Definition: types.h:483
tesseract_collision::ContactTrajectorySubstepResults::worstCollision
tesseract_collision::ContactResultVector worstCollision() const
Definition: types.cpp:456
tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START
@ ALL_EXCEPT_START
Check all states except the start state.
tesseract_common::CollisionMarginPairOverrideType::NONE
@ NONE

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