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