collision_box_sphere_unit.hpp
Go to the documentation of this file.
1 #ifndef TESSERACT_COLLISION_COLLISION_BOX_SPHERE_UNIT_HPP
2 #define TESSERACT_COLLISION_COLLISION_BOX_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_common/resource_locator.h>
9 
10 namespace tesseract_collision::test_suite
11 {
12 namespace detail
13 {
14 inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex_mesh = false)
15 {
17  // Add box to checker
19  CollisionShapePtr box = std::make_shared<tesseract_geometry::Box>(1, 1, 1);
20  Eigen::Isometry3d box_pose;
21  box_pose.setIdentity();
22 
23  CollisionShapesConst obj1_shapes;
24  tesseract_common::VectorIsometry3d obj1_poses;
25  obj1_shapes.push_back(box);
26  obj1_poses.push_back(box_pose);
27 
28  checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses, false);
29  checker.enableCollisionObject("box_link");
30 
32  // Add thin box to checker which is disabled
34  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
35  Eigen::Isometry3d thin_box_pose;
36  thin_box_pose.setIdentity();
37 
38  CollisionShapesConst obj2_shapes;
39  tesseract_common::VectorIsometry3d obj2_poses;
40  obj2_shapes.push_back(thin_box);
41  obj2_poses.push_back(thin_box_pose);
42 
43  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
44  checker.disableCollisionObject("thin_box_link");
45 
47  // Add sphere to checker. If use_convex_mesh = true then this
48  // sphere will be added as a convex hull mesh.
50  CollisionShapePtr sphere;
51 
52  if (use_convex_mesh)
53  {
54  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
55  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
56  tesseract_common::GeneralResourceLocator locator;
57  EXPECT_GT(
58  loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(),
59  *mesh_vertices,
60  *mesh_faces,
61  true),
62  0);
63 
64  auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
65  sphere = makeConvexMesh(*mesh);
66  }
67  else
68  {
69  sphere = std::make_shared<tesseract_geometry::Sphere>(0.25);
70  }
71 
72  Eigen::Isometry3d sphere_pose;
73  sphere_pose.setIdentity();
74 
75  CollisionShapesConst obj3_shapes;
76  tesseract_common::VectorIsometry3d obj3_poses;
77  obj3_shapes.push_back(sphere);
78  obj3_poses.push_back(sphere_pose);
79 
80  checker.addCollisionObject("sphere_link", 0, obj3_shapes, obj3_poses);
81 
83  // Add box and remove
85  CollisionShapePtr remove_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
86  Eigen::Isometry3d remove_box_pose;
87  remove_box_pose.setIdentity();
88 
89  CollisionShapesConst obj4_shapes;
90  tesseract_common::VectorIsometry3d obj4_poses;
91  obj4_shapes.push_back(remove_box);
92  obj4_poses.push_back(remove_box_pose);
93 
94  checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses);
95  EXPECT_TRUE(checker.getCollisionObjects().size() == 4);
96  EXPECT_TRUE(checker.hasCollisionObject("remove_box_link"));
97  checker.removeCollisionObject("remove_box_link");
98  EXPECT_FALSE(checker.hasCollisionObject("remove_box_link"));
99 
101  // Try functions on a link that does not exist
103  EXPECT_FALSE(checker.removeCollisionObject("link_does_not_exist"));
104  EXPECT_FALSE(checker.enableCollisionObject("link_does_not_exist"));
105  EXPECT_FALSE(checker.disableCollisionObject("link_does_not_exist"));
106 
108  // Try to add empty Collision Object
110  EXPECT_FALSE(
111  checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d()));
112 
114  // Check sizes
116  EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
117  for (const auto& co : checker.getCollisionObjects())
118  {
119  EXPECT_TRUE(checker.getCollisionObjectGeometries(co).size() == 1);
120  EXPECT_TRUE(checker.getCollisionObjectGeometriesTransforms(co).size() == 1);
121  for (const auto& cgt : checker.getCollisionObjectGeometriesTransforms(co))
122  {
123  EXPECT_TRUE(cgt.isApprox(Eigen::Isometry3d::Identity(), 1e-5));
124  }
125  }
126 }
127 
128 inline void runTestPrimitive(DiscreteContactManager& checker)
129 {
131  // Test when object is in collision
133  std::vector<std::string> active_links{ "box_link", "sphere_link" };
134  checker.setActiveCollisionObjects(active_links);
135  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
136  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
137 
138  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
139 
140  checker.setDefaultCollisionMargin(0.1);
141  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
142 
143  // Set the collision object transforms
144  tesseract_common::TransformMap location;
145  location["box_link"] = Eigen::Isometry3d::Identity();
146  location["sphere_link"] = Eigen::Isometry3d::Identity();
147  location["sphere_link"].translation()(0) = 0.2;
148  checker.setCollisionObjectsTransform(location);
149 
150  // Perform collision check
151  ContactResultMap result;
152  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
153 
154  ContactResultVector result_vector;
155  result.flattenMoveResults(result_vector);
156 
157  EXPECT_TRUE(!result_vector.empty());
158  EXPECT_NEAR(result_vector[0].distance, -0.55, 0.001);
159 
160  std::vector<int> idx = { 0, 1, 1 };
161  if (result_vector[0].link_names[0] != "box_link")
162  idx = { 1, 0, -1 };
163 
164  if (result_vector[0].single_contact_point)
165  {
166  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
167  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
168  std::abs(result_vector[0].nearest_points[0][0] - (-0.05)) > 0.001);
169  EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.0, 0.001);
170  EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001);
171  }
172  else
173  {
174  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.5, 0.001);
175  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
176  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
177  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.05, 0.001);
178  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
179  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
180  }
181 
182  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
183  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
184  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
185 
187  // Test object is out side the contact distance
189  location["sphere_link"].translation() = Eigen::Vector3d(1, 0, 0);
190  result.clear();
191  result_vector.clear();
192 
193  // Use different method for setting transforms
194  checker.setCollisionObjectsTransform("sphere_link", location["sphere_link"]);
195  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
196  result.flattenCopyResults(result_vector);
197 
198  EXPECT_TRUE(result_vector.empty());
199 
201  // Test object inside the contact distance
203  result.clear();
204  result_vector.clear();
205 
206  checker.setCollisionMarginData(CollisionMarginData(0.251));
207  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.251, 1e-5);
208  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
209  result.flattenMoveResults(result_vector);
210 
211  EXPECT_TRUE(!result_vector.empty());
212  EXPECT_NEAR(result_vector[0].distance, 0.25, 0.001);
213 
214  idx = { 0, 1, 1 };
215  if (result_vector[0].link_names[0] != "box_link")
216  idx = { 1, 0, -1 };
217 
218  if (result_vector[0].single_contact_point)
219  {
220  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
221  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
222  std::abs(result_vector[0].nearest_points[0][0] - (0.75)) > 0.001);
223  EXPECT_NEAR(result_vector[0].nearest_points[0][1], 0.0, 0.001);
224  EXPECT_NEAR(result_vector[0].nearest_points[0][2], 0.0, 0.001);
225  }
226  else
227  {
228  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.5, 0.001);
229  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.0, 0.001);
230  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][2], 0.0, 0.001);
231  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.75, 0.001);
232  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.0, 0.001);
233  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][2], 0.0, 0.001);
234  }
235 
236  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * 1.0, 0.001);
237  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
238  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
239 }
240 
241 inline void runTestConvex(DiscreteContactManager& checker)
242 {
244  // Test when object is in collision
246  std::vector<std::string> active_links{ "box_link", "sphere_link" };
247  checker.setActiveCollisionObjects(active_links);
248  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
249  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
250 
251  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
252 
253  checker.setDefaultCollisionMargin(0.1);
254  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5);
255 
256  // Set the collision object transforms
257  tesseract_common::TransformMap location;
258  location["box_link"] = Eigen::Isometry3d::Identity();
259  location["sphere_link"] = Eigen::Isometry3d::Identity();
260  location["sphere_link"].translation()(0) = 0.2;
261  checker.setCollisionObjectsTransform(location);
262 
263  // Perform collision check
264  ContactResultMap result;
265  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
266 
267  ContactResultVector result_vector;
268  result.flattenCopyResults(result_vector);
269 
270  EXPECT_TRUE(!result_vector.empty());
271  EXPECT_NEAR(result_vector[0].distance, -0.53776, 0.001);
272  EXPECT_NEAR(result_vector[0].nearest_points[0][1], result_vector[0].nearest_points[1][1], 0.001);
273  EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
274 
275  std::vector<int> idx = { 0, 1, 1 };
276  if (result_vector[0].link_names[0] != "box_link")
277  idx = { 1, 0, -1 };
278 
279  if (result_vector[0].single_contact_point)
280  {
281  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
282  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
283  std::abs(result_vector[0].nearest_points[0][0] - (-0.03776)) > 0.001);
284  }
285  else
286  {
287  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.5, 0.001);
288  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], -0.03776, 0.001);
289  }
290 
291  EXPECT_GT((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)), 0.0);
292  EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)))), 0.00001);
293 
295  // Test object is out side the contact distance
297  location["sphere_link"].translation() = Eigen::Vector3d(1, 0, 0);
298  result.clear();
299  result_vector.clear();
300  checker.setCollisionObjectsTransform(location);
301 
302  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
303  result.flattenMoveResults(result_vector);
304 
305  EXPECT_TRUE(result_vector.empty());
306 
308  // Test object inside the contact distance
310  result.clear();
311  result_vector.clear();
312 
313  checker.setCollisionMarginData(CollisionMarginData(0.27));
314  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.27, 1e-5);
315  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
316  result.flattenCopyResults(result_vector);
317 
318  EXPECT_TRUE(!result_vector.empty());
319  EXPECT_NEAR(result_vector[0].distance, 0.26224, 0.001);
320  EXPECT_NEAR(result_vector[0].nearest_points[0][1], result_vector[0].nearest_points[1][1], 0.001);
321  EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
322 
323  idx = { 0, 1, 1 };
324  if (result_vector[0].link_names[0] != "box_link")
325  idx = { 1, 0, -1 };
326 
327  if (result_vector[0].single_contact_point)
328  {
329  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
330  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (0.5)) > 0.001 &&
331  std::abs(result_vector[0].nearest_points[0][0] - (0.76224)) > 0.001);
332  }
333  else
334  {
335  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.5, 0.001);
336  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.76224, 0.001);
337  }
338 
339  EXPECT_GT((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)), 0.0);
340  EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)))), 0.00001);
341 }
342 } // namespace detail
343 
344 inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh = false)
345 {
346  // Add collision objects
347  detail::addCollisionObjects(checker, use_convex_mesh);
348 
349  // Call it again to test adding same object
350  detail::addCollisionObjects(checker, use_convex_mesh);
351 
352  if (use_convex_mesh)
353  detail::runTestConvex(checker);
354  else
355  detail::runTestPrimitive(checker);
356 }
357 
358 } // namespace tesseract_collision::test_suite
359 #endif // TESSERACT_COLLISION_COLLISION_BOX_SPHERE_UNIT_HPP
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
tesseract_common::CollisionMarginData::getMaxCollisionMargin
double getMaxCollisionMargin() const
tesseract_collision::DiscreteContactManager::getActiveCollisionObjects
virtual const std::vector< std::string > & getActiveCollisionObjects() const =0
Get which collision objects can move.
tesseract_collision::DiscreteContactManager::setCollisionMarginData
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data)=0
Set the contact distance threshold.
tesseract_collision::DiscreteContactManager::setDefaultCollisionMargin
virtual void setDefaultCollisionMargin(double default_collision_margin)=0
Set the default collision margin.
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
tesseract_collision::ContactResultMap::clear
void clear()
This is a consurvative clear.
Definition: types.cpp:231
tesseract_collision::DiscreteContactManager::getContactAllowedValidator
virtual std::shared_ptr< const tesseract_common::ContactAllowedValidator > getContactAllowedValidator() const =0
Get the active function for determining if two links are allowed to be in collision.
tesseract_collision::DiscreteContactManager
Definition: discrete_contact_manager.h:41
tesseract_collision::loadSimplePlyFile
int loadSimplePlyFile(const std::string &path, tesseract_common::VectorVector3d &vertices, Eigen::VectorXi &faces, bool triangles_only=false)
Loads a simple ply file given a path.
Definition: common.cpp:289
convex_hull_utils.h
This is a collection of common methods.
EXPECT_TRUE
#define EXPECT_TRUE(args)
tesseract_collision::ContactResultMap::flattenMoveResults
void flattenMoveResults(ContactResultVector &v)
Definition: types.cpp:270
geometries.h
tesseract_collision::makeConvexMesh
tesseract_geometry::ConvexMesh::Ptr makeConvexMesh(const tesseract_geometry::Mesh &mesh)
Definition: convex_hull_utils.cpp:113
tesseract_collision::test_suite::detail::runTestPrimitive
void runTestPrimitive(DiscreteContactManager &checker)
Definition: collision_box_sphere_unit.hpp:128
tesseract_collision::test_suite
Definition: large_dataset_benchmarks.hpp:17
tesseract_collision::ContactResultMap
This structure hold contact results for link pairs.
Definition: types.h:155
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
tesseract_collision::DiscreteContactManager::contactTest
virtual void contactTest(ContactResultMap &collisions, const ContactRequest &request)=0
Perform a contact test for all objects based.
tesseract_collision::DiscreteContactManager::setCollisionObjectsTransform
virtual void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose)=0
Set a single collision object's transforms.
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
distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
tesseract_common::GeneralResourceLocator::locateResource
std::shared_ptr< Resource > locateResource(const std::string &url) const override
common.h
This is a collection of common methods.
tesseract_collision::test_suite::detail::runTestConvex
void runTestConvex(DiscreteContactManager &checker)
Definition: collision_box_sphere_unit.hpp:241
tesseract_collision::test_suite::runTest
void runTest(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:111
tesseract_collision::DiscreteContactManager::setActiveCollisionObjects
virtual void setActiveCollisionObjects(const std::vector< std::string > &names)=0
Set which collision objects can move.
tesseract_common::GeneralResourceLocator
tesseract_collision::DiscreteContactManager::getCollisionMarginData
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
tesseract_collision::ContactResultMap::flattenCopyResults
void flattenCopyResults(ContactResultVector &v) const
Definition: types.cpp:282
tesseract_collision::ContactRequest
The ContactRequest struct.
Definition: types.h:300
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