collision_box_cone_unit.hpp
Go to the documentation of this file.
1 #ifndef TESSERACT_COLLISION_COLLISION_BOX_CONE_UNIT_HPP
2 #define TESSERACT_COLLISION_COLLISION_BOX_CONE_UNIT_HPP
3 
4 #include <tesseract_collision/core/discrete_contact_manager.h>
5 #include <tesseract_geometry/geometries.h>
6 
7 namespace tesseract_collision::test_suite
8 {
9 namespace detail
10 {
11 inline void addCollisionObjects(DiscreteContactManager& checker)
12 {
14  // Add box to checker
16  CollisionShapePtr box = std::make_shared<tesseract_geometry::Box>(1, 1, 1);
17  Eigen::Isometry3d box_pose;
18  box_pose.setIdentity();
19 
20  CollisionShapesConst obj1_shapes;
21  tesseract_common::VectorIsometry3d obj1_poses;
22  obj1_shapes.push_back(box);
23  obj1_poses.push_back(box_pose);
24 
25  checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses, false);
26  checker.enableCollisionObject("box_link");
27 
29  // Add thin box to checker which is disabled
31  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
32  Eigen::Isometry3d thin_box_pose;
33  thin_box_pose.setIdentity();
34 
35  CollisionShapesConst obj2_shapes;
36  tesseract_common::VectorIsometry3d obj2_poses;
37  obj2_shapes.push_back(thin_box);
38  obj2_poses.push_back(thin_box_pose);
39 
40  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
41  checker.disableCollisionObject("thin_box_link");
42 
44  // Add cone to checker. If use_convex_mesh = true then this
45  // cone will be added as a convex hull mesh.
47  CollisionShapePtr cone = std::make_shared<tesseract_geometry::Cone>(0.25, 0.25);
48  Eigen::Isometry3d cone_pose;
49  cone_pose.setIdentity();
50 
51  CollisionShapesConst obj3_shapes;
52  tesseract_common::VectorIsometry3d obj3_poses;
53  obj3_shapes.push_back(cone);
54  obj3_poses.push_back(cone_pose);
55 
56  checker.addCollisionObject("cone_link", 0, obj3_shapes, obj3_poses);
57 
59  // Add box and remove
61  CollisionShapePtr remove_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
62  Eigen::Isometry3d remove_box_pose;
63  remove_box_pose.setIdentity();
64 
65  CollisionShapesConst obj4_shapes;
66  tesseract_common::VectorIsometry3d obj4_poses;
67  obj4_shapes.push_back(remove_box);
68  obj4_poses.push_back(remove_box_pose);
69 
70  checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses);
71  EXPECT_TRUE(checker.getCollisionObjects().size() == 4);
72  EXPECT_TRUE(checker.hasCollisionObject("remove_box_link"));
73  checker.removeCollisionObject("remove_box_link");
74  EXPECT_FALSE(checker.hasCollisionObject("remove_box_link"));
75 
77  // Try functions on a link that does not exist
79  EXPECT_FALSE(checker.removeCollisionObject("link_does_not_exist"));
80  EXPECT_FALSE(checker.enableCollisionObject("link_does_not_exist"));
81  EXPECT_FALSE(checker.disableCollisionObject("link_does_not_exist"));
82 
84  // Try to add empty Collision Object
86  EXPECT_FALSE(
87  checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d()));
88 
90  // Check sizes
92  EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
93  for (const auto& co : checker.getCollisionObjects())
94  {
95  EXPECT_TRUE(checker.getCollisionObjectGeometries(co).size() == 1);
96  EXPECT_TRUE(checker.getCollisionObjectGeometriesTransforms(co).size() == 1);
97  for (const auto& cgt : checker.getCollisionObjectGeometriesTransforms(co))
98  {
99  EXPECT_TRUE(cgt.isApprox(Eigen::Isometry3d::Identity(), 1e-5));
100  }
101  }
102 }
103 } // namespace detail
104 
105 inline void runTest(DiscreteContactManager& checker)
106 {
107  // Add collision objects
108  detail::addCollisionObjects(checker);
109 
110  // Call it again to test adding same object
111  detail::addCollisionObjects(checker);
112 
114  // Test when object is in collision
116  std::vector<std::string> active_links{ "box_link", "cone_link" };
117  checker.setActiveCollisionObjects(active_links);
118  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
119  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
120 
121  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
122 
123  checker.setCollisionMarginData(CollisionMarginData(0.5));
124  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.5, 1e-5);
125 
126  checker.setCollisionMarginPair("box_link", "cone_link", 0.1);
127 
128  // Set the collision object transforms
129  tesseract_common::TransformMap location;
130  location["box_link"] = Eigen::Isometry3d::Identity();
131  location["cone_link"] = Eigen::Isometry3d::Identity();
132  location["cone_link"].translation()(0) = 0.2;
133  checker.setCollisionObjectsTransform(location);
134 
135  // Perform collision check
136  ContactResultMap result;
137  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
138 
139  ContactResultVector result_vector;
140  result.flattenMoveResults(result_vector);
141 
142  EXPECT_TRUE(!result_vector.empty());
143  EXPECT_NEAR(result_vector[0].distance, -0.55, 0.0001);
144 
145  std::vector<int> idx = { 0, 1, 1 };
146  if (result_vector[0].link_names[0] != "box_link")
147  idx = { 1, 0, -1 };
148 
149  if (result_vector[0].single_contact_point)
150  {
151  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
152  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
153  std::abs(result_vector[0].nearest_points[0][0] - (-0.05)) > 0.001);
154  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
155  std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
156  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (-0.125)) > 0.001 &&
157  std::abs(result_vector[0].nearest_points[0][2] - (-0.125)) > 0.001);
158  }
159  else
160  {
161  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.5, 0.001);
162  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
163  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], -0.125, 0.001);
164  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
165  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
166  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], -0.125, 0.001);
167  }
168 
169  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
170  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
171  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
172 
174  // Test object is out side the contact distance
176  location["cone_link"].translation() = Eigen::Vector3d(1, 0, 0);
177  result.clear();
178  result_vector.clear();
179 
180  // Use different method for setting transforms
181  checker.setCollisionObjectsTransform("cone_link", location["cone_link"]);
182  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
183  result.flattenCopyResults(result_vector);
184 
185  EXPECT_TRUE(result_vector.empty());
186 
188  // Test object inside the contact distance
190  result.clear();
191  result_vector.clear();
192 
193  checker.setCollisionMarginData(CollisionMarginData(0.251));
194  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.251, 1e-5);
195  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
196  result.flattenMoveResults(result_vector);
197 
198  EXPECT_TRUE(!result_vector.empty());
199  EXPECT_NEAR(result_vector[0].distance, 0.25, 0.001);
200 
201  idx = { 0, 1, 1 };
202  if (result_vector[0].link_names[0] != "box_link")
203  idx = { 1, 0, -1 };
204 
205  if (result_vector[0].single_contact_point)
206  {
207  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
208  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
209  std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001);
210  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001 &&
211  std::abs(result_vector[0].nearest_points[0][1] - (0.0)) > 0.001);
212  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][2] - (-0.125)) > 0.001 &&
213  std::abs(result_vector[0].nearest_points[0][2] - (-0.125)) > 0.001);
214  }
215  else
216  {
217  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.5, 0.001);
218  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
219  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], -0.125, 0.001);
220  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.75, 0.001);
221  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
222  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], -0.125, 0.001);
223  }
224 
225  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
226  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
227  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
228 }
229 } // namespace tesseract_collision::test_suite
230 #endif // TESSERACT_COLLISION_COLLISION_BOX_CONE_UNIT_HPP
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::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
tesseract_collision::test_suite::runTest
void runTest(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:111
EXPECT_FALSE
#define EXPECT_FALSE(args)
tesseract_collision::ContactTestType::CLOSEST
@ CLOSEST
tesseract_collision::CollisionMarginData
tesseract_common::CollisionMarginData CollisionMarginData
Definition: types.h:52

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