collision_mesh_mesh_unit.hpp
Go to the documentation of this file.
1 #ifndef TESSERACT_COLLISION_COLLISION_MESH_MESH_UNIT_HPP
2 #define TESSERACT_COLLISION_COLLISION_MESH_MESH_UNIT_HPP
3 
4 #include <tesseract_collision/core/discrete_contact_manager.h>
5 #include <tesseract_collision/core/common.h>
6 #include <tesseract_geometry/geometries.h>
7 #include <tesseract_common/resource_locator.h>
8 
9 namespace tesseract_collision::test_suite
10 {
11 namespace detail
12 {
13 inline void addCollisionObjects(DiscreteContactManager& checker)
14 {
16  // Add sphere to checker
18  tesseract_geometry::Mesh::Ptr sphere;
19 
20  auto vertices = std::make_shared<tesseract_common::VectorVector3d>();
21  auto faces = std::make_shared<Eigen::VectorXi>();
22  tesseract_common::GeneralResourceLocator locator;
23  int num_faces =
24  loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/sphere_p25m.ply")->getFilePath(),
25  *vertices,
26  *faces,
27  true);
28  EXPECT_GT(num_faces, 0);
29 
30  sphere = std::make_shared<tesseract_geometry::Mesh>(vertices, faces);
31  EXPECT_TRUE(num_faces == sphere->getFaceCount());
32 
33  Eigen::Isometry3d sphere_pose;
34  sphere_pose.setIdentity();
35 
36  CollisionShapesConst obj1_shapes;
37  tesseract_common::VectorIsometry3d obj1_poses;
38  obj1_shapes.push_back(sphere);
39  obj1_poses.push_back(sphere_pose);
40 
41  checker.addCollisionObject("sphere_link", 0, obj1_shapes, obj1_poses, false);
42  checker.enableCollisionObject("sphere_link");
43 
45  // Add thin box to checker which is disabled
47  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
48  Eigen::Isometry3d thin_box_pose;
49  thin_box_pose.setIdentity();
50 
51  CollisionShapesConst obj2_shapes;
52  tesseract_common::VectorIsometry3d obj2_poses;
53  obj2_shapes.push_back(thin_box);
54  obj2_poses.push_back(thin_box_pose);
55 
56  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
57  checker.disableCollisionObject("thin_box_link");
58 
60  // Add second sphere to checker. If use_convex_mesh = true
61  // then this sphere will be added as a convex hull mesh.
63  CollisionShapePtr sphere1 = std::make_shared<tesseract_geometry::Mesh>(vertices, faces);
64  Eigen::Isometry3d sphere1_pose;
65  sphere1_pose.setIdentity();
66 
67  CollisionShapesConst obj3_shapes;
68  tesseract_common::VectorIsometry3d obj3_poses;
69  obj3_shapes.push_back(sphere1);
70  obj3_poses.push_back(sphere1_pose);
71 
72  checker.addCollisionObject("sphere1_link", 0, obj3_shapes, obj3_poses);
73 
75  // Add box and remove
77  CollisionShapePtr remove_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
78  Eigen::Isometry3d remove_box_pose;
79  remove_box_pose.setIdentity();
80 
81  CollisionShapesConst obj4_shapes;
82  tesseract_common::VectorIsometry3d obj4_poses;
83  obj4_shapes.push_back(remove_box);
84  obj4_poses.push_back(remove_box_pose);
85 
86  checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses);
87  EXPECT_TRUE(checker.getCollisionObjects().size() == 4);
88  EXPECT_TRUE(checker.hasCollisionObject("remove_box_link"));
89  checker.removeCollisionObject("remove_box_link");
90  EXPECT_FALSE(checker.hasCollisionObject("remove_box_link"));
91 
93  // Try functions on a link that does not exist
95  EXPECT_FALSE(checker.removeCollisionObject("link_does_not_exist"));
96  EXPECT_FALSE(checker.enableCollisionObject("link_does_not_exist"));
97  EXPECT_FALSE(checker.disableCollisionObject("link_does_not_exist"));
98 
100  // Try to add empty Collision Object
102  EXPECT_FALSE(
103  checker.addCollisionObject("empty_link", 0, CollisionShapesConst(), tesseract_common::VectorIsometry3d()));
104 
106  // Check sizes
108  EXPECT_TRUE(checker.getCollisionObjects().size() == 3);
109  for (const auto& co : checker.getCollisionObjects())
110  {
111  EXPECT_TRUE(checker.getCollisionObjectGeometries(co).size() == 1);
112  EXPECT_TRUE(checker.getCollisionObjectGeometriesTransforms(co).size() == 1);
113  for (const auto& cgt : checker.getCollisionObjectGeometriesTransforms(co))
114  {
115  EXPECT_TRUE(cgt.isApprox(Eigen::Isometry3d::Identity(), 1e-5));
116  }
117  }
118 }
119 } // namespace detail
120 
121 inline void runTest(DiscreteContactManager& checker)
122 {
123  // Add collision objects
124  detail::addCollisionObjects(checker);
125 
126  // Call it again to test adding same object
127  detail::addCollisionObjects(checker);
128 
130  // Test when object is in collision (Closest Feature Edge to Edge)
132  std::vector<std::string> active_links{ "sphere_link", "sphere1_link" };
133  checker.setActiveCollisionObjects(active_links);
134  std::vector<std::string> check_active_links = checker.getActiveCollisionObjects();
135  EXPECT_TRUE(tesseract_common::isIdentical<std::string>(active_links, check_active_links, false));
136 
137  EXPECT_TRUE(checker.getContactAllowedValidator() == nullptr);
138 
139  checker.setDefaultCollisionMargin(0);
140  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.0, 1e-5);
141 
142  // Test when object is inside another
143  tesseract_common::TransformMap location;
144  location["sphere_link"] = Eigen::Isometry3d::Identity();
145  location["sphere1_link"] = Eigen::Isometry3d::Identity();
146  location["sphere1_link"].translation()(0) = 0.2;
147  checker.setCollisionObjectsTransform(location);
148 
149  // Perform collision check
150  ContactResultMap result;
151  checker.contactTest(result, ContactRequest(ContactTestType::ALL));
152 
153  ContactResultVector result_vector;
154  result.flattenMoveResults(result_vector);
155 
156  EXPECT_TRUE(result_vector.size() >= 37);
157 
159  // Test object is out side the contact distance
161  location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0);
162  result.clear();
163  result_vector.clear();
164  checker.setCollisionObjectsTransform(location);
165 
166  checker.contactTest(result, ContactRequest(ContactTestType::ALL));
167  result.flattenCopyResults(result_vector);
168 
169  EXPECT_TRUE(result_vector.empty());
170 
172  // Test object inside the contact distance (Closest Feature Edge to Edge)
174  result.clear();
175  result_vector.clear();
176 
177  checker.setCollisionMarginData(CollisionMarginData(0.55));
178  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.55, 1e-5);
179  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
180  result.flattenMoveResults(result_vector);
181 
182  EXPECT_TRUE(!result_vector.empty());
183  EXPECT_NEAR(result_vector[0].distance, 0.52448, 0.001);
184 
185  std::vector<int> idx = { 0, 1, 1 };
186  if (result_vector[0].link_names[0] != "sphere_link")
187  idx = { 1, 0, -1 };
188 
189  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 0.23776, 0.001);
190  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 0.76224, 0.001);
191  EXPECT_NEAR(result_vector[0].nearest_points[0][1], result_vector[0].nearest_points[1][1], 0.001);
192  EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
193  EXPECT_GT((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)), 0.0);
194  EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(1, 0, 0)))), 0.00001);
195 
197  // Test object inside the contact distance (Closest Feature Vertex to Vertex)
199  location["sphere1_link"].translation() = Eigen::Vector3d(0, 1, 0);
200  result.clear();
201  result_vector.clear();
202 
203  // The closest feature of the mesh should be edge to edge
204  // Use different method for setting transforms
205  checker.setCollisionObjectsTransform("sphere1_link", location["sphere1_link"]);
206  checker.setCollisionMarginData(CollisionMarginData(0.55));
207  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.55, 1e-5);
208  checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST));
209  result.flattenCopyResults(result_vector);
210 
211  EXPECT_TRUE(!result_vector.empty());
212  EXPECT_NEAR(result_vector[0].distance, 0.5, 0.001);
213 
214  idx = { 0, 1, 1 };
215  if (result_vector[0].link_names[0] != "sphere_link")
216  idx = { 1, 0, -1 };
217 
218  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][1], 0.25, 0.001);
219  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][1], 0.75, 0.001);
220  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
221  EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
222  EXPECT_GT((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(0, 1, 0)), 0.0);
223  EXPECT_LT(std::abs(std::acos((idx[2] * result_vector[0].normal).dot(Eigen::Vector3d(0, 1, 0)))), 0.00001);
224 }
225 } // namespace tesseract_collision::test_suite
226 #endif // TESSERACT_COLLISION_COLLISION_MESH_MESH_UNIT_HPP
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
tesseract_geometry::Mesh::Ptr
std::shared_ptr< Mesh > Ptr
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::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
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_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::runTest
void runTest(ContinuousContactManager &checker)
Definition: collision_box_box_cast_unit.hpp:111
tesseract_common::GeneralResourceLocator
tesseract_collision::ContactTestType::ALL
@ ALL
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