collision_compound_mesh_sphere_unit.hpp
Go to the documentation of this file.
1 #ifndef TESSERACT_COLLISION_COLLISION_COMPOUND_MESH_SPHERE_UNIT_HPP
2 #define TESSERACT_COLLISION_COLLISION_COMPOUND_MESH_SPHERE_UNIT_HPP
3 
4 #include <tesseract_collision/bullet/convex_hull_utils.h>
5 #include <tesseract_collision/core/discrete_contact_manager.h>
6 #include <tesseract_collision/core/common.h>
7 #include <tesseract_geometry/geometries.h>
8 #include <tesseract_geometry/mesh_parser.h>
9 #include <tesseract_common/resource_locator.h>
10 
11 namespace tesseract_collision::test_suite
12 {
13 namespace detail
14 {
15 inline void addCollisionObjects(DiscreteContactManager& checker)
16 {
18  // Add box to checker
20  auto resource_locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
21  auto compound_mesh_resource = resource_locator->locateResource("package://tesseract_support/meshes/box_box.dae");
22  auto meshes =
23  tesseract_geometry::createMeshFromPath<tesseract_geometry::ConvexMesh>(compound_mesh_resource->getFilePath());
24  CollisionShapePtr box = std::make_shared<tesseract_geometry::CompoundMesh>(meshes);
25  Eigen::Isometry3d box_pose;
26  box_pose.setIdentity();
27 
28  CollisionShapesConst obj1_shapes;
29  tesseract_common::VectorIsometry3d obj1_poses;
30  obj1_shapes.push_back(box);
31  obj1_poses.push_back(box_pose);
32 
33  checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses, false);
34  checker.enableCollisionObject("box_link");
35 
37  // Add thin box to checker which is disabled
39  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
40  Eigen::Isometry3d thin_box_pose;
41  thin_box_pose.setIdentity();
42 
43  CollisionShapesConst obj2_shapes;
44  tesseract_common::VectorIsometry3d obj2_poses;
45  obj2_shapes.push_back(thin_box);
46  obj2_poses.push_back(thin_box_pose);
47 
48  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
49  checker.disableCollisionObject("thin_box_link");
50 
52  // Add sphere to checker. If use_convex_mesh = true then this
53  // sphere will be added as a convex hull mesh.
55  CollisionShapePtr sphere = std::make_shared<tesseract_geometry::Sphere>(0.25);
56 
57  Eigen::Isometry3d sphere_pose;
58  sphere_pose.setIdentity();
59 
60  CollisionShapesConst obj3_shapes;
61  tesseract_common::VectorIsometry3d obj3_poses;
62  obj3_shapes.push_back(sphere);
63  obj3_poses.push_back(sphere_pose);
64 
65  checker.addCollisionObject("sphere_link", 0, obj3_shapes, obj3_poses);
66 
68  // Add box and remove
70  CollisionShapePtr remove_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
71  Eigen::Isometry3d remove_box_pose;
72  remove_box_pose.setIdentity();
73 
74  CollisionShapesConst obj4_shapes;
75  tesseract_common::VectorIsometry3d obj4_poses;
76  obj4_shapes.push_back(remove_box);
77  obj4_poses.push_back(remove_box_pose);
78 
79  checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses);
80  EXPECT_TRUE(checker.getCollisionObjects().size() == 4);
81  EXPECT_TRUE(checker.hasCollisionObject("remove_box_link"));
82  checker.removeCollisionObject("remove_box_link");
83  EXPECT_FALSE(checker.hasCollisionObject("remove_box_link"));
84 
86  // Try functions on a link that does not exist
88  EXPECT_FALSE(checker.removeCollisionObject("link_does_not_exist"));
89  EXPECT_FALSE(checker.enableCollisionObject("link_does_not_exist"));
90  EXPECT_FALSE(checker.disableCollisionObject("link_does_not_exist"));
91 
93  // Try to add empty Collision Object
95  EXPECT_FALSE(
96  checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d()));
97 
99  // Check sizes
101  EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
102  for (const auto& co : checker.getCollisionObjects())
103  {
104  EXPECT_TRUE(checker.getCollisionObjectGeometries(co).size() == 1);
105  EXPECT_TRUE(checker.getCollisionObjectGeometriesTransforms(co).size() == 1);
106  for (const auto& cgt : checker.getCollisionObjectGeometriesTransforms(co))
107  {
108  EXPECT_TRUE(cgt.isApprox(Eigen::Isometry3d::Identity(), 1e-5));
109  }
110  }
111 }
112 
113 } // namespace detail
114 
115 inline void runTest(DiscreteContactManager& checker)
116 {
117  // Add collision objects
118  detail::addCollisionObjects(checker);
119 
120  // Call it again to test adding same object
121  detail::addCollisionObjects(checker);
122 
124  // Test when object is in collision
126  std::vector<std::string> active_links{ "box_link", "sphere_link" };
127  checker.setActiveCollisionObjects(active_links);
128  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
129  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
130 
131  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
132 
133  checker.setDefaultCollisionMargin(0.1);
134  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
135 
136  // Set the collision object transforms
137  tesseract_common::TransformMap location;
138  location["box_link"] = Eigen::Isometry3d::Identity();
139  location["sphere_link"] = Eigen::Isometry3d::Identity();
140  location["sphere_link"].translation()(0) = 0.2;
141  checker.setCollisionObjectsTransform(location);
142 
143  // Perform collision check
144  ContactResultMap result;
145  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
146 
147  ContactResultVector result_vector;
148  result.flattenMoveResults(result_vector);
149 
150  EXPECT_TRUE(!result_vector.empty());
151  EXPECT_NEAR(result_vector[0].distance, -0.19053635, 0.001);
152 
153  std::vector<int> idx = { 0, 1, 1 };
154  if (result_vector[0].link_names[0] != "box_link")
155  idx = { 1, 0, -1 };
156 
157  if (result_vector[0].single_contact_point)
158  {
159  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
160  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
161  std::abs(result_vector[0].nearest_points[0][0] - (-0.05)) > 0.001);
162  EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.0, 0.001);
163  EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001);
164  }
165  else
166  {
167  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.2, 0.001);
168  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
169  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0594636, 0.001);
170  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.2, 0.001);
171  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
172  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.25, 0.001);
173  }
174 
175  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 0.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] * -1.0, 0.001);
178 
179  // Compound mesh so check shape id
180  EXPECT_EQ(result_vector[0].shape_id[static_cast<size_t>(idx[0])], 0);
181 
183  // Test object is out side the contact distance
185  location["sphere_link"].translation() = Eigen::Vector3d(0, 0, -0.3);
186  result.clear();
187  result_vector.clear();
188 
189  // Use different method for setting transforms
190  checker.setCollisionObjectsTransform("sphere_link", location["sphere_link"]);
191  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
192  result.flattenCopyResults(result_vector);
193 
194  EXPECT_TRUE(result_vector.empty());
195 
197  // Test object inside the contact distance
199  result.clear();
200  result_vector.clear();
201 
202  checker.setCollisionMarginData(CollisionMarginData(0.251));
203  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.251, 1e-5);
204  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
205  result.flattenMoveResults(result_vector);
206 
207  EXPECT_TRUE(!result_vector.empty());
208  EXPECT_NEAR(result_vector[0].distance, 0.1094636, 0.001);
209 
210  idx = { 0, 1, 1 };
211  if (result_vector[0].link_names[0] != "box_link")
212  idx = { 1, 0, -1 };
213 
214  if (result_vector[0].single_contact_point)
215  {
216  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
217  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
218  std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001);
219  EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.0, 0.001);
220  EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001);
221  }
222  else
223  {
224  // Increased tolernace because of FCL from 0.001 to 0.002
225  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.0, 0.002);
226  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.002);
227  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0594636, 0.001);
228  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.0, 0.002);
229  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.002);
230  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], -0.05, 0.001);
231  }
232 
233  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 0.0, 0.001);
234  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
235  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * -1.0, 0.001);
236 
237  // Compound mesh so check shape id
238  EXPECT_EQ(result_vector[0].shape_id[static_cast<size_t>(idx[0])], 0);
239 
241  // Test collision against second shape
243  result.clear();
244  result_vector.clear();
245  location["sphere_link"].translation() = Eigen::Vector3d(0, 2.75, 0);
246 
247  checker.setCollisionObjectsTransform("sphere_link", location["sphere_link"]);
248  checker.setCollisionMarginData(CollisionMarginData(0.1));
249  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
250  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
251  result.flattenMoveResults(result_vector);
252 
253  EXPECT_TRUE(!result_vector.empty());
254  EXPECT_NEAR(result_vector[0].distance, 0.03130736, 0.001);
255 
256  idx = { 0, 1, 1 };
257  if (result_vector[0].link_names[0] != "box_link")
258  idx = { 1, 0, -1 };
259 
260  if (result_vector[0].single_contact_point)
261  {
262  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
263  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
264  std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001);
265  EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.0, 0.001);
266  EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001);
267  }
268  else
269  {
270  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.0, 0.001);
271  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 2.4702682, 0.001);
272  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0297318, 0.001);
273  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.0, 0.001);
274  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 2.5014002, 0.001);
275  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0264228, 0.001);
276  }
277 
278  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 0.0, 0.001);
279  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.9943989, 0.001);
280  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * -0.1056915, 0.001);
281 
282  // Compound mesh so check shape id
283  EXPECT_EQ(result_vector[0].shape_id[static_cast<size_t>(idx[0])], 0);
284 }
285 
286 } // namespace tesseract_collision::test_suite
287 
288 #endif // TESSERACT_COLLISION_COLLISION_COMPOUND_MESH_SPHERE_UNIT_HPP
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
mesh_parser.h
tesseract_collision::test_suite::detail::addCollisionObjects
void addCollisionObjects(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:11
resource_locator.h
tesseract_common::TransformMap
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
discrete_contact_manager.h
This is the discrete contact manager base class.
tesseract_collision::ContactResultVector
tesseract_common::AlignedVector< ContactResult > ContactResultVector
Definition: types.h:136
convex_hull_utils.h
This is a collection of common methods.
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
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_EQ
#define EXPECT_EQ(a, b)
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