tesseract_environment_utils.cpp
Go to the documentation of this file.
1 #include <tesseract_common/macros.h>
2 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
3 #include <gtest/gtest.h>
4 #include <algorithm>
5 #include <vector>
6 #include <tesseract_urdf/urdf_parser.h>
7 #include <tesseract_common/resource_locator.h>
8 #include <tesseract_common/utils.h>
9 TESSERACT_COMMON_IGNORE_WARNINGS_POP
10 
11 #include <tesseract_environment/environment.h>
12 #include <tesseract_environment/utils.h>
13 
14 #include <tesseract_collision/core/discrete_contact_manager.h>
15 #include <tesseract_collision/core/continuous_contact_manager.h>
16 
17 #include <tesseract_scene_graph/graph.h>
18 
19 #include <tesseract_srdf/srdf_model.h>
20 
21 using namespace tesseract_scene_graph;
22 using namespace tesseract_srdf;
23 using namespace tesseract_collision;
24 using namespace tesseract_environment;
25 
26 SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator& locator)
27 {
28  std::string path = "package://tesseract_support/urdf/boxbot.urdf";
29  return tesseract_urdf::parseURDFFile(locator.locateResource(path)->getFilePath(), locator);
30 }
31 
32 SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph, const tesseract_common::ResourceLocator& locator)
33 {
34  std::string path = "package://tesseract_support/urdf/boxbot.srdf";
35 
36  auto srdf = std::make_shared<SRDFModel>();
37  srdf->initFile(scene_graph, locator.locateResource(path)->getFilePath(), locator);
38 
39  return srdf;
40 }
41 
42 template <typename ManagerType>
43 void checkIsAllowedFnOverride(std::unique_ptr<ManagerType> manager)
44 {
45  ContactManagerConfig config;
46 
47  // ASSIGN
48  {
49  config.acm.addAllowedCollision("allowed_link_1a", "allowed_link_2a", "Unit test");
50  config.acm_override_type = ACMOverrideType::ASSIGN;
51  manager->applyContactManagerConfig(config);
52  auto fn = manager->getContactAllowedValidator();
53  EXPECT_TRUE((*fn)("allowed_link_1a", "allowed_link_2a"));
54  }
55 
56  // NONE
57  {
58  // Manager currently allows: a
59  config.acm.addAllowedCollision("allowed_link_1b", "allowed_link_2b", "Unit test");
60  config.acm_override_type = ACMOverrideType::NONE;
61  EXPECT_ANY_THROW(manager->applyContactManagerConfig(config)); // NOLINT
62  }
63 
64  // OR
65  {
66  // Manager currently allows: a
67  config.acm.addAllowedCollision("allowed_link_1c", "allowed_link_2c", "Unit test");
68  config.acm_override_type = ACMOverrideType::OR;
69  manager->applyContactManagerConfig(config);
70  auto fn = manager->getContactAllowedValidator();
71  EXPECT_TRUE((*fn)("allowed_link_1a", "allowed_link_2a"));
72  EXPECT_TRUE((*fn)("allowed_link_1c", "allowed_link_2c"));
73  }
74 
75  // AND
76  {
77  // Manager currently allows: a, c
78  config.acm.removeAllowedCollision("allowed_link_1a", "allowed_link_2a");
79  config.acm_override_type = ACMOverrideType::AND;
80  manager->applyContactManagerConfig(config);
81  auto fn = manager->getContactAllowedValidator();
82  EXPECT_FALSE((*fn)("allowed_link_1a", "allowed_link_2a"));
83  EXPECT_TRUE((*fn)("allowed_link_1c", "allowed_link_2c"));
84  }
85 }
86 
87 TEST(TesseractEnvironmentUtils, applyContactManagerConfigIsAllowed) // NOLINT
88 {
89  tesseract_common::GeneralResourceLocator locator;
90  auto scene_graph = getSceneGraph(locator);
91  EXPECT_TRUE(scene_graph != nullptr);
92 
93  auto srdf = getSRDFModel(*scene_graph, locator);
94  EXPECT_TRUE(srdf != nullptr);
95 
96  auto env = std::make_shared<Environment>();
97  bool success = env->init(*scene_graph, srdf);
98  EXPECT_TRUE(success);
99 
100  checkIsAllowedFnOverride<DiscreteContactManager>(env->getDiscreteContactManager());
101  checkIsAllowedFnOverride<ContinuousContactManager>(env->getContinuousContactManager());
102 }
103 
104 TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLINT
105 {
106  tesseract_common::GeneralResourceLocator locator;
107 
108  auto scene_graph = getSceneGraph(locator);
109  EXPECT_TRUE(scene_graph != nullptr);
110 
111  auto srdf = getSRDFModel(*scene_graph, locator);
112  EXPECT_TRUE(srdf != nullptr);
113 
114  auto env = std::make_shared<Environment>();
115  bool success = env->init(*scene_graph, srdf);
116  EXPECT_TRUE(success);
117 
118  // Setup CollisionCheckConfig
119  CollisionCheckConfig default_collision_check_config;
120  default_collision_check_config.longest_valid_segment_length = 0.1;
121  default_collision_check_config.contact_request.type = tesseract_collision::ContactTestType::FIRST;
122  default_collision_check_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
123 
124  ContactManagerConfig default_contact_manager_config;
125  default_contact_manager_config.default_margin = 0.0;
126 
127  tesseract_collision::ContactResultMap contacts;
128  // Check Discrete
129  {
130  auto contact_manager_config = default_contact_manager_config;
131  DiscreteContactManager::Ptr manager = env->getDiscreteContactManager();
132  std::vector<std::string> active_links = { "boxbot_link", "test_box_link" };
133  manager->setActiveCollisionObjects(active_links);
134 
135  // Put the boxes 0.1m in collision
136  tesseract_common::TransformMap tmap;
137  tmap["boxbot_link"] = Eigen::Isometry3d::Identity();
138  tmap["test_box_link"] = Eigen::Isometry3d::Identity();
139  tmap["test_box_link"].translate(Eigen::Vector3d(0.9, 0, 0));
140 
141  // In collision by default
142  {
143  manager->applyContactManagerConfig(contact_manager_config);
144  contacts.clear();
145  checkTrajectoryState(contacts, *manager, tmap, default_collision_check_config.contact_request);
146  EXPECT_FALSE(contacts.empty());
147  }
148 
149  // Not in collision if link disabled
150  {
151  contact_manager_config.modify_object_enabled["boxbot_link"] = false;
152  manager->applyContactManagerConfig(contact_manager_config);
153  contacts.clear();
154  checkTrajectoryState(contacts, *manager, tmap, default_collision_check_config.contact_request);
155  EXPECT_TRUE(contacts.empty());
156  }
157 
158  // Re-enable it. Now in collision again
159  {
160  contact_manager_config.modify_object_enabled["boxbot_link"] = true;
161  manager->applyContactManagerConfig(contact_manager_config);
162  contacts.clear();
163  checkTrajectoryState(contacts, *manager, tmap, default_collision_check_config.contact_request);
164  EXPECT_FALSE(contacts.empty());
165  }
166 
167  // Disable a link that doesn't exist. Still in collision
168  {
169  contact_manager_config.modify_object_enabled["nonexistant_link"] = false;
170  manager->applyContactManagerConfig(contact_manager_config);
171  contacts.clear();
172  checkTrajectoryState(contacts, *manager, tmap, default_collision_check_config.contact_request);
173  EXPECT_FALSE(contacts.empty());
174  }
175  }
176 
177  // Check Continuous
178  {
179  auto contact_manager_config = default_contact_manager_config;
180  ContinuousContactManager::Ptr manager = env->getContinuousContactManager();
181  std::vector<std::string> active_links = { "boxbot_link", "test_box_link" };
182  manager->setActiveCollisionObjects(active_links);
183 
184  // Put the swept volume of the boxes 0.1m in collision
185  tesseract_common::TransformMap tmap1;
186  tmap1["boxbot_link"] = Eigen::Isometry3d::Identity();
187  tmap1["test_box_link"] = Eigen::Isometry3d::Identity();
188  tmap1["test_box_link"].translate(Eigen::Vector3d(0.9, 2, 0));
189 
190  tesseract_common::TransformMap tmap2;
191  tmap2["boxbot_link"] = Eigen::Isometry3d::Identity();
192  tmap2["test_box_link"] = Eigen::Isometry3d::Identity();
193  tmap2["test_box_link"].translate(Eigen::Vector3d(0.9, -2, 0));
194 
195  {
196  contacts.clear();
197  checkTrajectorySegment(contacts, *manager, tmap1, tmap2, default_collision_check_config.contact_request);
198  // In collision by default
199  EXPECT_FALSE(contacts.empty());
200  }
201 
202  // Not in collision if link disabled
203  {
204  contact_manager_config.modify_object_enabled["boxbot_link"] = false;
205  manager->applyContactManagerConfig(contact_manager_config);
206  contacts.clear();
207  checkTrajectorySegment(contacts, *manager, tmap1, tmap2, default_collision_check_config.contact_request);
208  EXPECT_TRUE(contacts.empty());
209  }
210 
211  // Re-enable it. Now in collision again
212  {
213  contact_manager_config.modify_object_enabled["boxbot_link"] = true;
214  manager->applyContactManagerConfig(contact_manager_config);
215  contacts.clear();
216  checkTrajectorySegment(contacts, *manager, tmap1, tmap2, default_collision_check_config.contact_request);
217  EXPECT_FALSE(contacts.empty());
218  }
219 
220  // Disable a link that doesn't exist. Still in collision
221  {
222  contact_manager_config.modify_object_enabled["nonexistant_link"] = false;
223  manager->applyContactManagerConfig(contact_manager_config);
224  contacts.clear();
225  checkTrajectorySegment(contacts, *manager, tmap1, tmap2, default_collision_check_config.contact_request);
226  EXPECT_FALSE(contacts.empty());
227  }
228  }
229 }
230 
231 TEST(TesseractEnvironmentUtils, checkTrajectoryState) // NOLINT
232 {
233  tesseract_common::GeneralResourceLocator locator;
234 
235  auto scene_graph = getSceneGraph(locator);
236  EXPECT_TRUE(scene_graph != nullptr);
237 
238  auto srdf = getSRDFModel(*scene_graph, locator);
239  EXPECT_TRUE(srdf != nullptr);
240 
241  auto env = std::make_shared<Environment>();
242  bool success = env->init(*scene_graph, srdf);
243  EXPECT_TRUE(success);
244 
245  // Setup CollisionCheckConfig
246  CollisionCheckConfig default_collision_check_config;
247  default_collision_check_config.longest_valid_segment_length = 0.1;
248  default_collision_check_config.contact_request.type = tesseract_collision::ContactTestType::FIRST;
249  default_collision_check_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE;
250 
251  ContactManagerConfig default_contact_manager_config;
252  default_contact_manager_config.default_margin = 0.0;
253 
254  tesseract_collision::ContactResultMap contacts;
255  // Check Discrete
256  {
257  auto contact_manager_config = default_contact_manager_config;
258  DiscreteContactManager::Ptr manager = env->getDiscreteContactManager();
259  std::vector<std::string> active_links = { "boxbot_link", "test_box_link" };
260  manager->setActiveCollisionObjects(active_links);
261 
262  // Put the boxes 0.05m away from each other
263  tesseract_common::TransformMap tmap;
264  tmap["boxbot_link"] = Eigen::Isometry3d::Identity();
265  tmap["test_box_link"] = Eigen::Isometry3d::Identity();
266  tmap["test_box_link"].translate(Eigen::Vector3d(1.05, 0, 0));
267 
268  // Not in collision
269  {
270  manager->applyContactManagerConfig(contact_manager_config);
271  contacts.clear();
272  checkTrajectoryState(contacts, *manager, tmap, default_collision_check_config.contact_request);
273  EXPECT_TRUE(contacts.empty());
274  }
275  // In collision if manager->applyContactManagerConfig works correctly
276  {
277  contact_manager_config.default_margin = 0.1;
278  manager->applyContactManagerConfig(contact_manager_config);
279  contacts.clear();
280  checkTrajectoryState(contacts, *manager, tmap, default_collision_check_config.contact_request);
281  EXPECT_FALSE(contacts.empty());
282  }
283  // Not collision if checkTrajectoryState applies the config
284  {
285  contact_manager_config.default_margin = 0.0;
286  manager->applyContactManagerConfig(contact_manager_config);
287  contacts.clear();
288  checkTrajectoryState(contacts, *manager, tmap, default_collision_check_config.contact_request);
289  EXPECT_TRUE(contacts.empty());
290  }
291  // In collision if checkTrajectoryState applies the config
292  {
293  contact_manager_config.default_margin = 0.1;
294  manager->applyContactManagerConfig(contact_manager_config);
295  contacts.clear();
296  checkTrajectoryState(contacts, *manager, tmap, default_collision_check_config.contact_request);
297  EXPECT_FALSE(contacts.empty());
298  }
299  }
300 
301  // Check Continuous
302  {
303  auto contact_manager_config = default_contact_manager_config;
304  ContinuousContactManager::Ptr manager = env->getContinuousContactManager();
305  std::vector<std::string> active_links = { "boxbot_link", "test_box_link" };
306  manager->setActiveCollisionObjects(active_links);
307 
308  // Put the swept volume of the boxes 0.05m away from each other
309  tesseract_common::TransformMap tmap1;
310  tmap1["boxbot_link"] = Eigen::Isometry3d::Identity();
311  tmap1["test_box_link"] = Eigen::Isometry3d::Identity();
312  tmap1["test_box_link"].translate(Eigen::Vector3d(1.05, 2, 0));
313 
314  tesseract_common::TransformMap tmap2;
315  tmap2["boxbot_link"] = Eigen::Isometry3d::Identity();
316  tmap2["test_box_link"] = Eigen::Isometry3d::Identity();
317  tmap2["test_box_link"].translate(Eigen::Vector3d(1.05, -2, 0));
318 
319  // Not in collision
320  {
321  manager->applyContactManagerConfig(contact_manager_config);
322  contacts.clear();
323  checkTrajectorySegment(contacts, *manager, tmap1, tmap2, default_collision_check_config.contact_request);
324  EXPECT_TRUE(contacts.empty());
325  }
326  // In collision if manager->applyContactManagerConfig works correctly
327  {
328  contact_manager_config.default_margin = 0.1;
329  manager->applyContactManagerConfig(contact_manager_config);
330  contacts.clear();
331  checkTrajectorySegment(contacts, *manager, tmap1, tmap2, default_collision_check_config.contact_request);
332  EXPECT_FALSE(contacts.empty());
333  }
334  // Not collision if checkTrajectoryState applies the config
335  {
336  contact_manager_config.default_margin = 0.0;
337  manager->applyContactManagerConfig(contact_manager_config);
338  contacts.clear();
339  checkTrajectorySegment(contacts, *manager, tmap1, tmap2, default_collision_check_config.contact_request);
340  EXPECT_TRUE(contacts.empty());
341  }
342  // In collision if checkTrajectoryState applies the config
343  {
344  contact_manager_config.default_margin = 0.1;
345  manager->applyContactManagerConfig(contact_manager_config);
346  contacts.clear();
347  checkTrajectorySegment(contacts, *manager, tmap1, tmap2, default_collision_check_config.contact_request);
348  EXPECT_FALSE(contacts.empty());
349  }
350  }
351 }
352 
353 int main(int argc, char** argv)
354 {
355  testing::InitGoogleTest(&argc, argv);
356 
357  return RUN_ALL_TESTS();
358 }
tesseract_collision::ContactResultMap::empty
bool empty() const
graph.h
tesseract_srdf::SRDFModel::Ptr
std::shared_ptr< SRDFModel > Ptr
tesseract_srdf
tesseract_environment
Definition: command.h:45
tesseract_collision::ContactManagerConfig
tesseract_collision::ContinuousContactManager::Ptr
std::shared_ptr< ContinuousContactManager > Ptr
utils.h
tesseract_environment::checkTrajectoryState
void checkTrajectoryState(tesseract_collision::ContactResultMap &contact_results, tesseract_collision::ContinuousContactManager &manager, const tesseract_common::TransformMap &state, const tesseract_collision::ContactRequest &contact_request)
Should perform a discrete collision check a state first configuring manager with config.
Definition: utils.cpp:100
getSRDFModel
SRDFModel::Ptr getSRDFModel(const SceneGraph &scene_graph, const tesseract_common::ResourceLocator &locator)
Definition: tesseract_environment_utils.cpp:32
resource_locator.h
tesseract_common::TransformMap
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
tesseract_common::AllowedCollisionMatrix::removeAllowedCollision
virtual void removeAllowedCollision(const std::string &link_name)
tesseract_collision::ContactRequest::type
ContactTestType type
tesseract_common::ResourceLocator::locateResource
virtual std::shared_ptr< Resource > locateResource(const std::string &url) const=0
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
tesseract_collision::ContactResultMap::clear
void clear()
tesseract_scene_graph::SceneGraph
tesseract_collision::CollisionCheckConfig::type
CollisionEvaluatorType type
EXPECT_TRUE
#define EXPECT_TRUE(args)
urdf_parser.h
discrete_contact_manager.h
utils.h
Tesseract Environment Utility Functions.
tesseract_urdf::parseURDFFile
std::unique_ptr< tesseract_scene_graph::SceneGraph > parseURDFFile(const std::string &path, const tesseract_common::ResourceLocator &locator)
tesseract_collision::ContactResultMap
tesseract_collision::CollisionCheckConfig::contact_request
ContactRequest contact_request
tesseract_collision::ContactManagerConfig::default_margin
std::optional< double > default_margin
tesseract_collision::CollisionCheckConfig
main
int main(int argc, char **argv)
Definition: tesseract_environment_utils.cpp:353
tesseract_common::ResourceLocator
TESSERACT_COMMON_IGNORE_WARNINGS_POP
checkIsAllowedFnOverride
void checkIsAllowedFnOverride(std::unique_ptr< ManagerType > manager)
Definition: tesseract_environment_utils.cpp:43
continuous_contact_manager.h
srdf_model.h
tesseract_collision::ContactTestType::FIRST
@ FIRST
environment.h
tesseract_collision::CollisionEvaluatorType::DISCRETE
@ DISCRETE
TEST
TEST(TesseractEnvironmentUtils, applyContactManagerConfigIsAllowed)
Definition: tesseract_environment_utils.cpp:87
tesseract_common::GeneralResourceLocator
tesseract_common::AllowedCollisionMatrix::addAllowedCollision
virtual void addAllowedCollision(const std::string &link_name1, const std::string &link_name2, const std::string &reason)
getSceneGraph
SceneGraph::Ptr getSceneGraph(const tesseract_common::ResourceLocator &locator)
Definition: tesseract_environment_utils.cpp:26
tesseract_collision
tesseract_collision::ContactManagerConfig::acm
tesseract_common::AllowedCollisionMatrix acm
macros.h
tesseract_scene_graph::SceneGraph::Ptr
std::shared_ptr< SceneGraph > Ptr
tesseract_scene_graph
tesseract_collision::DiscreteContactManager::Ptr
std::shared_ptr< DiscreteContactManager > Ptr
EXPECT_FALSE
#define EXPECT_FALSE(args)
tesseract_collision::ContactManagerConfig::acm_override_type
ACMOverrideType acm_override_type
tesseract_environment::checkTrajectorySegment
void checkTrajectorySegment(tesseract_collision::ContactResultMap &contact_results, tesseract_collision::ContinuousContactManager &manager, const tesseract_common::TransformMap &state0, const tesseract_common::TransformMap &state1, const tesseract_collision::ContactRequest &contact_request)
Should perform a continuous collision check between two states only passing along the contact_request...
Definition: utils.cpp:77
tesseract_collision::CollisionCheckConfig::longest_valid_segment_length
double longest_valid_segment_length

tesseract_environment
Author(s): Levi Armstrong
autogenerated on Sun May 18 2025 03:02:21