collision_box_box_unit.hpp
Go to the documentation of this file.
1 #ifndef TESSERACT_COLLISION_COLLISION_BOX_BOX_UNIT_HPP
2 #define TESSERACT_COLLISION_COLLISION_BOX_BOX_UNIT_HPP
3 
4 #include <tesseract_collision/bullet/convex_hull_utils.h>
5 #include <tesseract_collision/core/continuous_contact_manager.h>
6 #include <tesseract_collision/core/discrete_contact_manager.h>
7 #include <tesseract_collision/core/common.h>
8 #include <tesseract_geometry/geometries.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, bool use_convex_mesh = false)
16 {
18  // Add box to checker
20  CollisionShapePtr box = std::make_shared<tesseract_geometry::Box>(1, 1, 1);
21  Eigen::Isometry3d box_pose;
22  box_pose.setIdentity();
23 
24  CollisionShapesConst obj1_shapes;
25  tesseract_common::VectorIsometry3d obj1_poses;
26  obj1_shapes.push_back(box);
27  obj1_poses.push_back(box_pose);
28 
29  checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses, false);
30  checker.enableCollisionObject("box_link");
31 
33  // Add thin box to checker which is disabled
35  CollisionShapePtr thin_box = std::make_shared<tesseract_geometry::Box>(0.1, 1, 1);
36  Eigen::Isometry3d thin_box_pose;
37  thin_box_pose.setIdentity();
38 
39  CollisionShapesConst obj2_shapes;
40  tesseract_common::VectorIsometry3d obj2_poses;
41  obj2_shapes.push_back(thin_box);
42  obj2_poses.push_back(thin_box_pose);
43 
44  checker.addCollisionObject("thin_box_link", 0, obj2_shapes, obj2_poses);
45  checker.disableCollisionObject("thin_box_link");
46 
48  // Add second box to checker. If use_convex_mesh = true then this
49  // box will be added as a convex hull mesh.
51  CollisionShapePtr second_box;
52 
53  if (use_convex_mesh)
54  {
55  auto mesh_vertices = std::make_shared<tesseract_common::VectorVector3d>();
56  auto mesh_faces = std::make_shared<Eigen::VectorXi>();
57  // TODO: Need to figure out why this test not pass of bullet when using the box_2m.ply mesh
58  tesseract_common::GeneralResourceLocator locator;
59  EXPECT_GT(loadSimplePlyFile(locator.locateResource("package://tesseract_support/meshes/box2_2m.ply")->getFilePath(),
60  *mesh_vertices,
61  *mesh_faces,
62  true),
63  0);
64 
65  auto mesh = std::make_shared<tesseract_geometry::Mesh>(mesh_vertices, mesh_faces);
66  second_box = makeConvexMesh(*mesh);
67  }
68  else
69  {
70  second_box = std::make_shared<tesseract_geometry::Box>(2, 2, 2);
71  }
72 
73  Eigen::Isometry3d second_box_pose;
74  second_box_pose.setIdentity();
75 
76  CollisionShapesConst obj3_shapes;
77  tesseract_common::VectorIsometry3d obj3_poses;
78  obj3_shapes.push_back(second_box);
79  obj3_poses.push_back(second_box_pose);
80 
81  checker.addCollisionObject("second_box_link", 0, obj3_shapes, obj3_poses);
82 
84  // Add box and remove
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(thin_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 runTestTyped(DiscreteContactManager& checker, ContactTestType test_type)
129 {
131  // Test when object is inside another
133  std::vector<std::string> active_links{ "box_link", "second_box_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.setCollisionMarginData(CollisionMarginData(0.1));
141  EXPECT_NEAR(checker.getCollisionMarginData().getCollisionMargin("box_link", "second_box_link"), 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["box_link"].translation()(0) = 0.2;
147  location["box_link"].translation()(1) = 0.1;
148  location["second_box_link"] = Eigen::Isometry3d::Identity();
149 
150  checker.setCollisionObjectsTransform(location);
151 
152  // Perform collision check
153  ContactResultMap result;
154  checker.contactTest(result, ContactRequest(test_type));
155 
156  ContactResultVector result_vector;
157  result.flattenMoveResults(result_vector);
158 
159  EXPECT_TRUE(!result_vector.empty());
160  EXPECT_NEAR(result_vector[0].distance, -1.30, 0.001);
161  EXPECT_NEAR(result_vector[0].nearest_points[0][1], result_vector[0].nearest_points[1][1], 0.001);
162  EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
163 
164  std::vector<int> idx = { 0, 1, 1 };
165  if (result_vector[0].link_names[0] != "box_link")
166  idx = { 1, 0, -1 };
167 
168  if (result_vector[0].single_contact_point)
169  {
170  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
171  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (-0.3)) > 0.001 &&
172  std::abs(result_vector[0].nearest_points[0][0] - (1.0)) > 0.001);
173  }
174  else
175  {
176  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], -0.3, 0.001);
177  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 1.0, 0.001);
178  }
179 
180  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * -1.0, 0.001);
181  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
182  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
183 
185  // Test object is outside the contact distance
187  {
188  location["box_link"].translation() = Eigen::Vector3d(1.60, 0, 0);
189  result.clear();
190  result_vector.clear();
191 
192  // Use different method for setting transforms
193  std::vector<std::string> names = { "box_link" };
194  tesseract_common::VectorIsometry3d transforms = { location["box_link"] };
195  checker.setCollisionObjectsTransform(names, transforms);
196  checker.contactTest(result, test_type);
197  result.flattenCopyResults(result_vector);
198 
199  EXPECT_TRUE(result_vector.empty());
200  }
202  // Test object is outside the contact distance only for this link pair
204  {
205  CollisionMarginData data = checker.getCollisionMarginData();
206  data.setCollisionMargin("not_box_link", "also_not_box_link", 1.7);
207  checker.setCollisionMarginData(data);
208 
209  EXPECT_EQ(checker.getCollisionMarginData().getMaxCollisionMargin(), 1.7);
210  EXPECT_NEAR(checker.getCollisionMarginData().getCollisionMargin("box_link", "second_box_link"), 0.1, 1e-5);
211  location["box_link"].translation() = Eigen::Vector3d(1.60, 0, 0);
212  result.clear();
213  result_vector.clear();
214 
215  // Use different method for setting transforms
216  std::vector<std::string> names = { "box_link" };
217  tesseract_common::VectorIsometry3d transforms = { location["box_link"] };
218  checker.setCollisionObjectsTransform(names, transforms);
219  checker.contactTest(result, test_type);
220  result.flattenMoveResults(result_vector);
221 
222  EXPECT_TRUE(result_vector.empty());
223  }
225  // Test object inside the contact distance only for this link pair
227  {
228  result.clear();
229  result_vector.clear();
230 
231  CollisionMarginData data(0.1);
232  data.setCollisionMargin("box_link", "second_box_link", 0.25);
233 
234  checker.setCollisionMarginData(data);
235  EXPECT_NEAR(checker.getCollisionMarginData().getCollisionMargin("box_link", "second_box_link"), 0.25, 1e-5);
236  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.25, 1e-5);
237  checker.contactTest(result, ContactRequest(test_type));
238  result.flattenCopyResults(result_vector);
239 
240  EXPECT_TRUE(!result_vector.empty());
241  EXPECT_NEAR(result_vector[0].distance, 0.1, 0.001);
242  EXPECT_NEAR(result_vector[0].nearest_points[0][1], result_vector[0].nearest_points[1][1], 0.001);
243  EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
244 
245  idx = { 0, 1, 1 };
246  if (result_vector[0].link_names[0] != "box_link")
247  idx = { 1, 0, -1 };
248 
249  if (result_vector[0].single_contact_point)
250  {
251  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
252  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (1.1)) > 0.001 &&
253  std::abs(result_vector[0].nearest_points[0][0] - (1.0)) > 0.001);
254  }
255  else
256  {
257  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 1.1, 0.001);
258  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 1.0, 0.001);
259  }
260 
261  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * -1.0, 0.001);
262  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
263  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
264  }
266  // Test object inside the contact distance
268  {
269  result.clear();
270  result_vector.clear();
271 
272  checker.setCollisionMarginData(CollisionMarginData(0.25));
273  EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.25, 1e-5);
274  checker.contactTest(result, ContactRequest(test_type));
275  result.flattenMoveResults(result_vector);
276 
277  EXPECT_TRUE(!result_vector.empty());
278  EXPECT_NEAR(result_vector[0].distance, 0.1, 0.001);
279  EXPECT_NEAR(result_vector[0].nearest_points[0][1], result_vector[0].nearest_points[1][1], 0.001);
280  EXPECT_NEAR(result_vector[0].nearest_points[0][2], result_vector[0].nearest_points[1][2], 0.001);
281 
282  idx = { 0, 1, 1 };
283  if (result_vector[0].link_names[0] != "box_link")
284  idx = { 1, 0, -1 };
285 
286  if (result_vector[0].single_contact_point)
287  {
288  EXPECT_NEAR(result_vector[0].nearest_points[0][0], result_vector[0].nearest_points[1][0], 0.001);
289  EXPECT_FALSE(std::abs(result_vector[0].nearest_points[0][0] - (1.1)) > 0.001 &&
290  std::abs(result_vector[0].nearest_points[0][0] - (1.0)) > 0.001);
291  }
292  else
293  {
294  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[0])][0], 1.1, 0.001);
295  EXPECT_NEAR(result_vector[0].nearest_points[static_cast<size_t>(idx[1])][0], 1.0, 0.001);
296  }
297 
298  EXPECT_NEAR(result_vector[0].normal[0], idx[2] * -1.0, 0.001);
299  EXPECT_NEAR(result_vector[0].normal[1], idx[2] * 0.0, 0.001);
300  EXPECT_NEAR(result_vector[0].normal[2], idx[2] * 0.0, 0.001);
301  }
302 }
303 } // namespace detail
304 
305 inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh = false)
306 {
307  // Add collision objects
308  detail::addCollisionObjects(checker, use_convex_mesh);
309 
310  // Call it again to test adding same object
311  detail::addCollisionObjects(checker, use_convex_mesh);
312 
313  detail::runTestTyped(checker, ContactTestType::FIRST);
314  detail::runTestTyped(checker, ContactTestType::CLOSEST);
315  detail::runTestTyped(checker, ContactTestType::ALL);
316 }
317 
318 } // namespace tesseract_collision::test_suite
319 #endif // COLLISION_BOX_BOX_UNIT_HPP
tesseract_common::VectorIsometry3d
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
tesseract_common::CollisionMarginData::setCollisionMargin
void setCollisionMargin(const std::string &obj1, const std::string &obj2, double collision_margin)
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::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
tesseract_collision::test_suite::detail::runTestTyped
void runTestTyped(DiscreteContactManager &checker, ContactTestType test_type)
Definition: collision_box_box_unit.hpp:128
discrete_contact_manager.h
This is the discrete contact manager base class.
tesseract_collision::DiscreteContactManager::removeCollisionObject
virtual bool removeCollisionObject(const std::string &name)=0
Remove an object from the checker.
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.
tesseract_common::CollisionMarginData
tesseract_common::CollisionMarginData::getCollisionMargin
double getCollisionMargin(const std::string &obj1, const std::string &obj2) const
EXPECT_TRUE
#define EXPECT_TRUE(args)
tesseract_collision::ContactResultMap::flattenMoveResults
void flattenMoveResults(ContactResultVector &v)
Definition: types.cpp:270
geometries.h
tesseract_collision::DiscreteContactManager::disableCollisionObject
virtual bool disableCollisionObject(const std::string &name)=0
Disable an object.
tesseract_collision::makeConvexMesh
tesseract_geometry::ConvexMesh::Ptr makeConvexMesh(const tesseract_geometry::Mesh &mesh)
Definition: convex_hull_utils.cpp:113
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::addCollisionObject
virtual bool addCollisionObject(const std::string &name, const int &mask_id, const CollisionShapesConst &shapes, const tesseract_common::VectorIsometry3d &shape_poses, bool enabled=true)=0
Add a object to the checker.
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::DiscreteContactManager::hasCollisionObject
virtual bool hasCollisionObject(const std::string &name) const =0
Find if a collision object already exists.
tesseract_collision::CollisionShapesConst
std::vector< CollisionShapeConstPtr > CollisionShapesConst
Definition: types.h:51
distance
template void distance(DistanceTraversalNodeBase< double > *node, BVHFrontList *front_list, int qsize)
tesseract_collision::DiscreteContactManager::enableCollisionObject
virtual bool enableCollisionObject(const std::string &name)=0
Enable an object.
tesseract_collision::DiscreteContactManager::getCollisionObjects
virtual const std::vector< std::string > & getCollisionObjects() const =0
Get all collision objects.
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::ContactTestType::FIRST
@ FIRST
tesseract_collision::ContactTestType
ContactTestType
Definition: types.h:66
tesseract_collision::DiscreteContactManager::getCollisionObjectGeometriesTransforms
virtual const tesseract_common::VectorIsometry3d & getCollisionObjectGeometriesTransforms(const std::string &name) const =0
Get a collision objects collision geometries transforms.
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::getCollisionObjectGeometries
virtual const CollisionShapesConst & getCollisionObjectGeometries(const std::string &name) const =0
Get a collision objects collision geometries.
tesseract_collision::DiscreteContactManager::getCollisionMarginData
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
tesseract_collision::ContactTestType::ALL
@ ALL
tesseract_collision::ContactResultMap::flattenCopyResults
void flattenCopyResults(ContactResultVector &v) const
Definition: types.cpp:282
tesseract_collision::ContactRequest
The ContactRequest struct.
Definition: types.h:300
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
continuous_contact_manager.h
This is the continuous contact manager base class.
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