test_collision_common_pr2.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: E. Gil Jones */
36 
43 #include <urdf_parser/urdf_parser.h>
45 
46 #include <gtest/gtest.h>
47 #include <sstream>
48 #include <algorithm>
49 #include <ctype.h>
50 #include <fstream>
51 
52 template <class CollisionAllocatorType>
54 {
55 public:
56  std::shared_ptr<CollisionAllocatorType> value_;
57 
58 protected:
60  {
61  }
62 
63  void SetUp() override
64  {
65  value_ = std::make_shared<CollisionAllocatorType>();
67  robot_model_ok_ = static_cast<bool>(robot_model_);
68  kinect_dae_resource_ = "package://moveit_resources_pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae";
69 
70  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), true);
71 
72  cenv_ = value_->allocateEnv(robot_model_);
73  }
74 
75  void TearDown() override
76  {
77  }
78 
79  bool robot_model_ok_;
80 
81  moveit::core::RobotModelPtr robot_model_;
82 
83  collision_detection::CollisionEnvPtr cenv_;
84 
85  collision_detection::AllowedCollisionMatrixPtr acm_;
86 
87  std::string kinect_dae_resource_;
88 };
89 
90 #ifdef NDEBUG
91 #define EXPECT_TIME_LT(EXPR, VAL) EXPECT_LT(EXPR, VAL)
92 #else // Don't perform timing checks in Debug mode (but evaluate expression)
93 #define EXPECT_TIME_LT(EXPR, VAL) (void)(EXPR)
94 #endif
95 
97 
99 {
100  ASSERT_TRUE(this->robot_model_ok_);
101 }
102 
103 TYPED_TEST_P(CollisionDetectorTest, DefaultNotInCollision)
104 {
105  moveit::core::RobotState robot_state(this->robot_model_);
106  robot_state.setToDefaultValues();
107  robot_state.update();
108 
111  this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
112  ASSERT_FALSE(res.collision);
113 }
114 
116 {
121  // req.contacts = true;
122  // req.max_contacts = 100;
123 
124  moveit::core::RobotState robot_state(this->robot_model_);
125  robot_state.setToDefaultValues();
126  robot_state.update();
127 
128  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
129  offset.translation().x() = .01;
130 
131  // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity());
132  // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
133  robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
134  robot_state.updateStateWithLinkAt("base_bellow_link", offset);
135  robot_state.update();
136 
137  this->acm_->setEntry("base_link", "base_bellow_link", false);
138  this->cenv_->checkSelfCollision(req, res1, robot_state, *this->acm_);
139  ASSERT_TRUE(res1.collision);
140 
141  this->acm_->setEntry("base_link", "base_bellow_link", true);
142  this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_);
143  ASSERT_FALSE(res2.collision);
144 
145  // req.verbose = true;
146  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity());
147  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
148  robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
149  robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
150  robot_state.update();
151 
152  this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
153  this->cenv_->checkSelfCollision(req, res3, robot_state, *this->acm_);
154  ASSERT_TRUE(res3.collision);
155 }
156 
158 {
160  req.contacts = true;
161  req.max_contacts = 1;
162 
163  moveit::core::RobotState robot_state(this->robot_model_);
164  robot_state.setToDefaultValues();
165  robot_state.update();
166 
167  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
168  offset.translation().x() = .01;
169 
170  // robot_state.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity());
171  // robot_state.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
172  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Isometry3d::Identity());
173  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
174 
175  robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
176  robot_state.updateStateWithLinkAt("base_bellow_link", offset);
177  robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
178  robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
179  robot_state.update();
180 
181  this->acm_->setEntry("base_link", "base_bellow_link", false);
182  this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
183 
185  this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
186  ASSERT_TRUE(res.collision);
187  EXPECT_EQ(res.contacts.size(), 1u);
188  EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
189 
190  res.clear();
191  req.max_contacts = 2;
192  req.max_contacts_per_pair = 1;
193  // req.verbose = true;
194  this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
195  ASSERT_TRUE(res.collision);
196  EXPECT_EQ(res.contacts.size(), 2u);
197  EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
198 
199  res.contacts.clear();
200  res.contact_count = 0;
201 
202  req.max_contacts = 10;
203  req.max_contacts_per_pair = 2;
204  this->acm_ =
205  std::make_shared<collision_detection::AllowedCollisionMatrix>(this->robot_model_->getLinkModelNames(), false);
206  this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
207  ASSERT_TRUE(res.collision);
208  EXPECT_LE(res.contacts.size(), 10u);
209  EXPECT_LE(res.contact_count, 10u);
210 }
211 
213 {
215  req.contacts = true;
216  req.max_contacts = 1;
217 
218  moveit::core::RobotState robot_state(this->robot_model_);
219  robot_state.setToDefaultValues();
220  robot_state.update();
221 
222  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
223  Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
224 
225  pos1.translation().x() = 5.0;
226  pos2.translation().x() = 5.01;
227 
228  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
229  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
230  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
231  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
232  robot_state.update();
233 
234  this->acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
235 
237  this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
238  ASSERT_TRUE(res.collision);
239  ASSERT_EQ(res.contacts.size(), 1u);
240  ASSERT_EQ(res.contacts.begin()->second.size(), 1u);
241 
242  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
243  it != res.contacts.end(); it++)
244  {
245  EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
246  }
247 
248  pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
249  pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) *
250  Eigen::Quaterniond(sqrt(1 - pow(0.258, 2)), 0.0, 0.258, 0.0));
251  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
252  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
253  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
254  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
255  robot_state.update();
256 
258  this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_);
259  ASSERT_TRUE(res2.collision);
260  ASSERT_EQ(res2.contacts.size(), 1u);
261  ASSERT_EQ(res2.contacts.begin()->second.size(), 1u);
262 
263  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
264  it != res2.contacts.end(); it++)
265  {
266  EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
267  }
268 
269  pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
270  pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) *
271  Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0).normalized());
272  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
273  // robot_state.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
274  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
275  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
276  robot_state.update();
277 
279  this->cenv_->checkSelfCollision(req, res2, robot_state, *this->acm_);
280  ASSERT_FALSE(res3.collision);
281 }
282 
284 {
287 
288  this->acm_ =
289  std::make_shared<collision_detection::AllowedCollisionMatrix>(this->robot_model_->getLinkModelNames(), true);
290 
291  moveit::core::RobotState robot_state(this->robot_model_);
292  robot_state.setToDefaultValues();
293  robot_state.update();
294 
295  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
296  pos1.translation().x() = 5.0;
297 
298  // robot_state.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
299  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
300  robot_state.update();
301  this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
302  ASSERT_FALSE(res.collision);
303 
304  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
305  this->cenv_->getWorld()->addToObject("box", pos1, shapes::ShapeConstPtr(shape), Eigen::Isometry3d::Identity());
306 
308  this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_);
309  ASSERT_TRUE(res.collision);
310 
311  // deletes shape
312  this->cenv_->getWorld()->removeObject("box");
313 
314  shape = new shapes::Box(.1, .1, .1);
315  std::vector<shapes::ShapeConstPtr> shapes;
317  shapes.push_back(shapes::ShapeConstPtr(shape));
318  poses.push_back(Eigen::Isometry3d::Identity());
319  std::vector<std::string> touch_links;
320  robot_state.attachBody("box", poses[0], shapes, poses, touch_links, "r_gripper_palm_link");
321 
323  this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
324  ASSERT_TRUE(res.collision);
325 
326  // deletes shape
327  robot_state.clearAttachedBody("box");
328 
329  touch_links.push_back("r_gripper_palm_link");
330  touch_links.push_back("r_gripper_motor_accelerometer_link");
331  shapes[0] = std::make_shared<shapes::Box>(.1, .1, .1);
332  robot_state.attachBody("box", poses[0], shapes, poses, touch_links, "r_gripper_palm_link");
333  robot_state.update();
334 
336  this->cenv_->checkSelfCollision(req, res, robot_state, *this->acm_);
337  ASSERT_FALSE(res.collision);
338 
339  pos1.translation().x() = 5.01;
340  shapes::Shape* coll = new shapes::Box(.1, .1, .1);
341  this->cenv_->getWorld()->addToObject("coll", pos1, shapes::ShapeConstPtr(coll), Eigen::Isometry3d::Identity());
343  this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_);
344  ASSERT_TRUE(res.collision);
345 
346  this->acm_->setEntry("coll", "r_gripper_palm_link", true);
348  this->cenv_->checkRobotCollision(req, res, robot_state, *this->acm_);
349  ASSERT_TRUE(res.collision);
350 }
351 
353 {
354  moveit::core::RobotState robot_state(this->robot_model_);
355  robot_state.setToDefaultValues();
356  robot_state.update();
357 
360 
361  collision_detection::CollisionEnvPtr new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld());
362 
364  new_cenv->checkSelfCollision(req, res, robot_state);
365  double first_check = (ros::WallTime::now() - before).toSec();
366  before = ros::WallTime::now();
367  new_cenv->checkSelfCollision(req, res, robot_state);
368  double second_check = (ros::WallTime::now() - before).toSec();
369 
370  EXPECT_TIME_LT(fabs(first_check - second_check), .05);
371 
372  std::vector<shapes::ShapeConstPtr> shapes;
373  shapes.resize(1);
374 
375  shapes[0].reset(shapes::createMeshFromResource(this->kinect_dae_resource_));
376 
378  poses.push_back(Eigen::Isometry3d::Identity());
379 
380  std::vector<std::string> touch_links;
381  robot_state.attachBody("kinect", poses[0], shapes, poses, touch_links, "r_gripper_palm_link");
382 
383  before = ros::WallTime::now();
384  new_cenv->checkSelfCollision(req, res, robot_state);
385  first_check = (ros::WallTime::now() - before).toSec();
386  before = ros::WallTime::now();
387  new_cenv->checkSelfCollision(req, res, robot_state);
388  second_check = (ros::WallTime::now() - before).toSec();
389 
390  // the first check is going to take a while, as data must be constructed
391  EXPECT_TIME_LT(second_check, .1);
392 
393  collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld());
394  before = ros::WallTime::now();
395  new_cenv->checkSelfCollision(req, res, robot_state);
396  first_check = (ros::WallTime::now() - before).toSec();
397  before = ros::WallTime::now();
398  new_cenv->checkSelfCollision(req, res, robot_state);
399  second_check = (ros::WallTime::now() - before).toSec();
400 
401  EXPECT_TIME_LT(fabs(first_check - second_check), .05);
402 }
403 
404 TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached)
405 {
408 
409  shapes::ShapeConstPtr shape(shapes::createMeshFromResource(this->kinect_dae_resource_));
410  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
411  Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
412  pos2.translation().x() = 10.0;
413 
414  this->cenv_->getWorld()->addToObject("kinect", pos1, shape, Eigen::Isometry3d::Identity());
415 
416  moveit::core::RobotState robot_state(this->robot_model_);
417  robot_state.setToDefaultValues();
418  robot_state.update();
419 
421  this->cenv_->checkRobotCollision(req, res, robot_state);
422  double first_check = (ros::WallTime::now() - before).toSec();
423  before = ros::WallTime::now();
424  this->cenv_->checkRobotCollision(req, res, robot_state);
425  double second_check = (ros::WallTime::now() - before).toSec();
426 
427  EXPECT_TIME_LT(second_check, .05);
428 
429  collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect");
430  this->cenv_->getWorld()->removeObject("kinect");
431 
432  moveit::core::RobotState robot_state1(this->robot_model_);
433  moveit::core::RobotState robot_state2(this->robot_model_);
434  robot_state1.setToDefaultValues();
435  robot_state2.setToDefaultValues();
436  robot_state1.update();
437  robot_state2.update();
438 
439  std::vector<std::string> touch_links;
440  Eigen::Isometry3d identity_transform{ Eigen::Isometry3d::Identity() };
441  robot_state1.attachBody("kinect", identity_transform, object->shapes_, object->shape_poses_, touch_links,
442  "r_gripper_palm_link");
443 
444  EigenSTL::vector_Isometry3d other_poses;
445  other_poses.push_back(pos2);
446 
447  // This creates a new set of constant properties for the attached body, which happens to be the same as the one above;
448  robot_state2.attachBody("kinect", identity_transform, object->shapes_, object->shape_poses_, touch_links,
449  "r_gripper_palm_link");
450 
451  // going to take a while, but that's fine
453  this->cenv_->checkSelfCollision(req, res, robot_state1);
454 
455  EXPECT_TRUE(res.collision);
456 
457  before = ros::WallTime::now();
458  this->cenv_->checkSelfCollision(req, res, robot_state1, *this->acm_);
459  first_check = (ros::WallTime::now() - before).toSec();
460  before = ros::WallTime::now();
461  req.verbose = true;
463  this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_);
464  second_check = (ros::WallTime::now() - before).toSec();
465 
466  EXPECT_TIME_LT(first_check, .05);
467  EXPECT_TIME_LT(fabs(first_check - second_check), .1);
468 }
469 
470 TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed)
471 {
473  std::vector<shapes::ShapeConstPtr> shapes;
474  for (unsigned int i = 0; i < 10000; i++)
475  {
476  poses.push_back(Eigen::Isometry3d::Identity());
477  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
478  }
480  this->cenv_->getWorld()->addToObject("map", shapes, poses);
481  double t = (ros::WallTime::now() - start).toSec();
482  // TODO (j-petit): investigate why bullet collision checking is considerably slower here
483  EXPECT_TIME_LT(t, 5.0);
484  // this is not really a failure; it is just that slow;
485  // looking into doing collision checking with a voxel grid.
486  ROS_INFO_NAMED("collision_detection.bullet", "Adding boxes took %g", t);
487 }
488 
490 {
491  moveit::core::RobotState robot_state1(this->robot_model_);
492  robot_state1.setToDefaultValues();
493  robot_state1.update();
494 
495  Eigen::Isometry3d kinect_pose;
496  kinect_pose.setIdentity();
497  shapes::ShapePtr kinect_shape;
498  kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_));
499 
500  this->cenv_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
501 
502  Eigen::Isometry3d np;
503  for (unsigned int i = 0; i < 5; i++)
504  {
505  np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity();
506  this->cenv_->getWorld()->moveShapeInObject("kinect", kinect_shape, np);
509  this->cenv_->checkCollision(req, res, robot_state1, *this->acm_);
510  }
511 }
512 
513 TYPED_TEST_P(CollisionDetectorTest, TestChangingShapeSize)
514 {
515  moveit::core::RobotState robot_state1(this->robot_model_);
516  robot_state1.setToDefaultValues();
517  robot_state1.update();
518 
521 
522  ASSERT_FALSE(res1.collision);
523 
525  std::vector<shapes::ShapeConstPtr> shapes;
526  for (unsigned int i = 0; i < 5; i++)
527  {
528  this->cenv_->getWorld()->removeObject("shape");
529  shapes.clear();
530  poses.clear();
531  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
532  poses.push_back(Eigen::Isometry3d::Identity());
533  this->cenv_->getWorld()->addToObject("shape", shapes, poses);
536  this->cenv_->checkCollision(req, res, robot_state1, *this->acm_);
537  ASSERT_TRUE(res.collision);
538  }
539 
540  Eigen::Isometry3d kinect_pose = Eigen::Isometry3d::Identity();
541  shapes::ShapePtr kinect_shape;
542  kinect_shape.reset(shapes::createMeshFromResource(this->kinect_dae_resource_));
543  this->cenv_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
546  this->cenv_->checkCollision(req2, res2, robot_state1, *this->acm_);
547  ASSERT_TRUE(res2.collision);
548  for (unsigned int i = 0; i < 5; i++)
549  {
550  this->cenv_->getWorld()->removeObject("shape");
551  shapes.clear();
552  poses.clear();
553  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
554  poses.push_back(Eigen::Isometry3d::Identity());
555  this->cenv_->getWorld()->addToObject("shape", shapes, poses);
558  this->cenv_->checkCollision(req, res, robot_state1, *this->acm_);
559  ASSERT_TRUE(res.collision);
560  }
561 }
562 
563 REGISTER_TYPED_TEST_CASE_P(CollisionDetectorTest, InitOK, DefaultNotInCollision, LinksInCollision, ContactReporting,
564  ContactPositions, AttachedBodyTester, DiffSceneTester, ConvertObjectToAttached,
565  TestCollisionMapAdditionSpeed, MoveMesh, TestChangingShapeSize);
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
ASSERT_FALSE
#define ASSERT_FALSE(condition)
TYPED_TEST_CASE_P
TYPED_TEST_CASE_P(CollisionDetectorTest)
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
EXPECT_LE
#define EXPECT_LE(val1, val2)
CollisionDetectorTest::CollisionDetectorTest
CollisionDetectorTest()
Definition: test_collision_common_pr2.h:91
CollisionDetectorTest::value_
std::shared_ptr< CollisionAllocatorType > value_
Definition: test_collision_common_pr2.h:88
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
CollisionDetectorTest::kinect_dae_resource_
std::string kinect_dae_resource_
Definition: test_collision_common_pr2.h:119
shape_operations.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
shapes::Shape
testing::Test
TYPED_TEST_P
TYPED_TEST_P(CollisionDetectorTest, InitOK)
Definition: test_collision_common_pr2.h:98
ASSERT_EQ
#define ASSERT_EQ(val1, val2)
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
EXPECT_TRUE
#define EXPECT_TRUE(condition)
robot_model.h
CollisionDetectorTest::TearDown
void TearDown() override
Definition: test_collision_common_pr2.h:107
collision_common.h
shapes::ShapePtr
std::shared_ptr< Shape > ShapePtr
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:720
CollisionDetectorTest
Definition: test_collision_common_pr2.h:53
shapes::Box
ROS_INFO_NAMED
#define ROS_INFO_NAMED(name,...)
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
collision_env.h
ros::WallTime::now
static WallTime now()
collision_detection::CollisionRequest::max_contacts
std::size_t max_contacts
Overall maximum number of contacts to compute.
Definition: include/moveit/collision_detection/collision_common.h:245
moveit::core::RobotState::attachBody
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
Definition: robot_state.cpp:1057
CollisionDetectorTest::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_collision_common_pr2.h:113
collision_detection::CollisionRequest::verbose
bool verbose
Flag indicating whether information about detected collisions should be reported.
Definition: include/moveit/collision_detection/collision_common.h:258
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:425
CollisionDetectorTest::acm_
collision_detection::AllowedCollisionMatrixPtr acm_
Definition: test_collision_common_pr2.h:117
collision_detection::CollisionEnv::ObjectConstPtr
World::ObjectConstPtr ObjectConstPtr
Definition: collision_env.h:284
ASSERT_TRUE
#define ASSERT_TRUE(condition)
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
EXPECT_TIME_LT
#define EXPECT_TIME_LT(EXPR, VAL)
Definition: test_collision_common_pr2.h:93
ros::WallTime
REGISTER_TYPED_TEST_CASE_P
REGISTER_TYPED_TEST_CASE_P(CollisionDetectorTest, InitOK, DefaultNotInCollision, LinksInCollision, ContactReporting, ContactPositions, AttachedBodyTester, DiffSceneTester, ConvertObjectToAttached, TestCollisionMapAdditionSpeed, MoveMesh, TestChangingShapeSize)
collision_detection::CollisionResult::contacts
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
Definition: include/moveit/collision_detection/collision_common.h:209
CollisionDetectorTest::robot_model_ok_
bool robot_model_ok_
Definition: test_collision_common_pr2.h:111
start
ROSCPP_DECL void start()
robot_model_test_utils.h
CollisionDetectorTest::SetUp
void SetUp() override
Definition: test_collision_common_pr2.h:95
M_PI
#define M_PI
collision_detection::CollisionRequest::contacts
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
Definition: include/moveit/collision_detection/collision_common.h:242
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:200
gtest.h
collision_detection::CollisionRequest::max_contacts_per_pair
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
Definition: include/moveit/collision_detection/collision_common.h:249
collision_detection::CollisionResult::clear
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
Definition: include/moveit/collision_detection/collision_common.h:187
collision_detection::CollisionResult::contact_count
std::size_t contact_count
Number of contacts returned.
Definition: include/moveit/collision_detection/collision_common.h:206
CollisionDetectorTest::cenv_
collision_detection::CollisionEnvPtr cenv_
Definition: test_collision_common_pr2.h:115
robot_state.h
t
geometry_msgs::TransformStamped t
collision_detector_allocator.h
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:116


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat May 21 2022 02:24:56