test_fcl_collision_detection.cpp
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 
41 #include <moveit_resources/config.h>
42 
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 #include <boost/filesystem.hpp>
53 
56 
57 class FclCollisionDetectionTester : public testing::Test
58 {
59 protected:
60  void SetUp() override
61  {
62  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
63  std::string urdf_file = (res_path / "pr2_description/urdf/robot.xml").string();
64  std::string srdf_file = (res_path / "pr2_description/srdf/robot.xml").string();
65  kinect_dae_resource_ = "package://moveit_resources/pr2_description/urdf/meshes/sensors/kinect_v0/kinect.dae";
66 
67  srdf_model_.reset(new srdf::Model());
68  std::string xml_string;
69  std::fstream xml_file(urdf_file.c_str(), std::fstream::in);
70 
71  if (xml_file.is_open())
72  {
73  while (xml_file.good())
74  {
75  std::string line;
76  std::getline(xml_file, line);
77  xml_string += (line + "\n");
78  }
79  xml_file.close();
80  urdf_model_ = urdf::parseURDF(xml_string);
81  urdf_ok_ = static_cast<bool>(urdf_model_);
82  }
83  else
84  {
85  EXPECT_EQ("FAILED TO OPEN FILE", urdf_file);
86  urdf_ok_ = false;
87  }
88  srdf_ok_ = srdf_model_->initFile(*urdf_model_, srdf_file);
89 
91 
92  acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
93 
94  crobot_.reset(new DefaultCRobotType(kmodel_));
95  cworld_.reset(new DefaultCWorldType());
96  }
97 
98  void TearDown() override
99  {
100  }
101 
102 protected:
103  bool urdf_ok_;
104  bool srdf_ok_;
105 
106  urdf::ModelInterfaceSharedPtr urdf_model_;
108 
109  robot_model::RobotModelPtr kmodel_;
110 
111  collision_detection::CollisionRobotPtr crobot_;
112  collision_detection::CollisionWorldPtr cworld_;
113 
114  collision_detection::AllowedCollisionMatrixPtr acm_;
115 
116  std::string kinect_dae_resource_;
117 };
118 
120 {
121  ASSERT_TRUE(urdf_ok_);
122  ASSERT_TRUE(srdf_ok_);
123 }
124 
125 TEST_F(FclCollisionDetectionTester, DefaultNotInCollision)
126 {
128  kstate.setToDefaultValues();
129  kstate.update();
130 
133  crobot_->checkSelfCollision(req, res, kstate, *acm_);
134  ASSERT_FALSE(res.collision);
135 }
136 
138 {
143  // req.contacts = true;
144  // req.max_contacts = 100;
145 
147  kstate.setToDefaultValues();
148  kstate.update();
149 
150  Eigen::Affine3d offset = Eigen::Affine3d::Identity();
151  offset.translation().x() = .01;
152 
153  // kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
154  // kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
155  kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
156  kstate.updateStateWithLinkAt("base_bellow_link", offset);
157  kstate.update();
158 
159  acm_->setEntry("base_link", "base_bellow_link", false);
160  crobot_->checkSelfCollision(req, res1, kstate, *acm_);
161  ASSERT_TRUE(res1.collision);
162 
163  acm_->setEntry("base_link", "base_bellow_link", true);
164  crobot_->checkSelfCollision(req, res2, kstate, *acm_);
165  ASSERT_FALSE(res2.collision);
166 
167  // req.verbose = true;
168  // kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
169  // kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
170  kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
171  kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
172  kstate.update();
173 
174  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
175  crobot_->checkSelfCollision(req, res3, kstate, *acm_);
176  ASSERT_TRUE(res3.collision);
177 }
178 
180 {
182  req.contacts = true;
183  req.max_contacts = 1;
184 
186  kstate.setToDefaultValues();
187  kstate.update();
188 
189  Eigen::Affine3d offset = Eigen::Affine3d::Identity();
190  offset.translation().x() = .01;
191 
192  // kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
193  // kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
194  // kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
195  // kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
196 
197  kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
198  kstate.updateStateWithLinkAt("base_bellow_link", offset);
199  kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
200  kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
201  kstate.update();
202 
203  acm_->setEntry("base_link", "base_bellow_link", false);
204  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
205 
207  crobot_->checkSelfCollision(req, res, kstate, *acm_);
208  ASSERT_TRUE(res.collision);
209  EXPECT_EQ(res.contacts.size(), 1);
210  EXPECT_EQ(res.contacts.begin()->second.size(), 1);
211 
212  res.clear();
213  req.max_contacts = 2;
214  req.max_contacts_per_pair = 1;
215  // req.verbose = true;
216  crobot_->checkSelfCollision(req, res, kstate, *acm_);
217  ASSERT_TRUE(res.collision);
218  EXPECT_EQ(res.contacts.size(), 2);
219  EXPECT_EQ(res.contacts.begin()->second.size(), 1);
220 
221  res.contacts.clear();
222  res.contact_count = 0;
223 
224  req.max_contacts = 10;
225  req.max_contacts_per_pair = 2;
226  acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), false));
227  crobot_->checkSelfCollision(req, res, kstate, *acm_);
228  ASSERT_TRUE(res.collision);
229  EXPECT_LE(res.contacts.size(), 10);
230  EXPECT_LE(res.contact_count, 10);
231 }
232 
234 {
236  req.contacts = true;
237  req.max_contacts = 1;
238 
240  kstate.setToDefaultValues();
241  kstate.update();
242 
243  Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
244  Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
245 
246  pos1.translation().x() = 5.0;
247  pos2.translation().x() = 5.01;
248 
249  // kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
250  // kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
251  kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
252  kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
253  kstate.update();
254 
255  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
256 
258  crobot_->checkSelfCollision(req, res, kstate, *acm_);
259  ASSERT_TRUE(res.collision);
260  ASSERT_EQ(res.contacts.size(), 1);
261  ASSERT_EQ(res.contacts.begin()->second.size(), 1);
262 
263  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
264  it != res.contacts.end(); it++)
265  {
266  EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
267  }
268 
269  pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
270  pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
271  // kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
272  // kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
273  kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
274  kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
275  kstate.update();
276 
278  crobot_->checkSelfCollision(req, res2, kstate, *acm_);
279  ASSERT_TRUE(res2.collision);
280  ASSERT_EQ(res2.contacts.size(), 1);
281  ASSERT_EQ(res2.contacts.begin()->second.size(), 1);
282 
283  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
284  it != res2.contacts.end(); it++)
285  {
286  EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
287  }
288 
289  pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
290  pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0));
291  // kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
292  // kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
293  kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
294  kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
295  kstate.update();
296 
298  crobot_->checkSelfCollision(req, res2, kstate, *acm_);
299  ASSERT_FALSE(res3.collision);
300 }
301 
303 {
306 
307  acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
308 
310  kstate.setToDefaultValues();
311  kstate.update();
312 
313  Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
314  pos1.translation().x() = 5.0;
315 
316  // kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
317  kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
318  kstate.update();
319  crobot_->checkSelfCollision(req, res, kstate, *acm_);
320  ASSERT_FALSE(res.collision);
321 
322  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
323  cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
324 
326  cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
327  ASSERT_TRUE(res.collision);
328 
329  // deletes shape
330  cworld_->getWorld()->removeObject("box");
331 
332  shape = new shapes::Box(.1, .1, .1);
333  std::vector<shapes::ShapeConstPtr> shapes;
335  shapes.push_back(shapes::ShapeConstPtr(shape));
336  poses.push_back(Eigen::Affine3d::Identity());
337  std::vector<std::string> touch_links;
338  kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
339 
341  crobot_->checkSelfCollision(req, res, kstate, *acm_);
342  ASSERT_TRUE(res.collision);
343 
344  // deletes shape
345  kstate.clearAttachedBody("box");
346 
347  touch_links.push_back("r_gripper_palm_link");
348  touch_links.push_back("r_gripper_motor_accelerometer_link");
349  shapes[0].reset(new shapes::Box(.1, .1, .1));
350  kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
351  kstate.update();
352 
354  crobot_->checkSelfCollision(req, res, kstate, *acm_);
355  ASSERT_FALSE(res.collision);
356 
357  pos1.translation().x() = 5.01;
358  shapes::Shape* coll = new shapes::Box(.1, .1, .1);
359  cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
361  cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
362  ASSERT_TRUE(res.collision);
363 
364  acm_->setEntry("coll", "r_gripper_palm_link", true);
366  cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
367  ASSERT_TRUE(res.collision);
368 }
369 
371 {
373  kstate.setToDefaultValues();
374  kstate.update();
375 
378 
380  *(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
381 
383  new_crobot.checkSelfCollision(req, res, kstate);
384  double first_check = (ros::WallTime::now() - before).toSec();
385  before = ros::WallTime::now();
386  new_crobot.checkSelfCollision(req, res, kstate);
387  double second_check = (ros::WallTime::now() - before).toSec();
388 
389  EXPECT_LT(fabs(first_check - second_check), .05);
390 
391  std::vector<shapes::ShapeConstPtr> shapes;
392  shapes.resize(1);
393 
395 
397  poses.push_back(Eigen::Affine3d::Identity());
398 
399  std::vector<std::string> touch_links;
400  kstate.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link");
401 
402  before = ros::WallTime::now();
403  new_crobot.checkSelfCollision(req, res, kstate);
404  first_check = (ros::WallTime::now() - before).toSec();
405  before = ros::WallTime::now();
406  new_crobot.checkSelfCollision(req, res, kstate);
407  second_check = (ros::WallTime::now() - before).toSec();
408 
409  // the first check is going to take a while, as data must be constructed
410  EXPECT_LT(second_check, .1);
411 
413  *(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
414  before = ros::WallTime::now();
415  new_crobot.checkSelfCollision(req, res, kstate);
416  first_check = (ros::WallTime::now() - before).toSec();
417  before = ros::WallTime::now();
418  new_crobot.checkSelfCollision(req, res, kstate);
419  second_check = (ros::WallTime::now() - before).toSec();
420 
421  EXPECT_LT(fabs(first_check - second_check), .05);
422 }
423 
424 TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached)
425 {
428 
430  Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
431  Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
432  pos2.translation().x() = 10.0;
433 
434  cworld_->getWorld()->addToObject("kinect", shape, pos1);
435 
437  kstate.setToDefaultValues();
438  kstate.update();
439 
441  cworld_->checkRobotCollision(req, res, *crobot_, kstate);
442  double first_check = (ros::WallTime::now() - before).toSec();
443  before = ros::WallTime::now();
444  cworld_->checkRobotCollision(req, res, *crobot_, kstate);
445  double second_check = (ros::WallTime::now() - before).toSec();
446 
447  EXPECT_LT(second_check, .05);
448 
449  collision_detection::CollisionWorld::ObjectConstPtr object = cworld_->getWorld()->getObject("kinect");
450  cworld_->getWorld()->removeObject("kinect");
451 
454  kstate1.setToDefaultValues();
455  kstate2.setToDefaultValues();
456  kstate1.update();
457  kstate2.update();
458 
459  std::vector<std::string> touch_links;
460  kstate1.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
461 
462  EigenSTL::vector_Affine3d other_poses;
463  other_poses.push_back(pos2);
464 
465  // This creates a new set of constant properties for the attached body, which happens to be the same as the one above;
466  kstate2.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
467 
468  // going to take a while, but that's fine
470  crobot_->checkSelfCollision(req, res, kstate1);
471 
472  EXPECT_TRUE(res.collision);
473 
474  before = ros::WallTime::now();
475  crobot_->checkSelfCollision(req, res, kstate1, *acm_);
476  first_check = (ros::WallTime::now() - before).toSec();
477  before = ros::WallTime::now();
478  req.verbose = true;
480  crobot_->checkSelfCollision(req, res, kstate2, *acm_);
481  second_check = (ros::WallTime::now() - before).toSec();
482 
483  EXPECT_LT(first_check, .05);
484  EXPECT_LT(fabs(first_check - second_check), .1);
485 }
486 
487 TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed)
488 {
490  std::vector<shapes::ShapeConstPtr> shapes;
491  for (unsigned int i = 0; i < 10000; i++)
492  {
493  poses.push_back(Eigen::Affine3d::Identity());
494  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
495  }
497  cworld_->getWorld()->addToObject("map", shapes, poses);
498  double t = (ros::WallTime::now() - start).toSec();
499  EXPECT_GE(1.0, t);
500  // this is not really a failure; it is just that slow;
501  // looking into doing collision checking with a voxel grid.
502  ROS_INFO_NAMED("collision_detection.fcl", "Adding boxes took %g", t);
503 }
504 
506 {
508  kstate1.setToDefaultValues();
509  kstate1.update();
510 
511  Eigen::Affine3d kinect_pose;
512  kinect_pose.setIdentity();
513  shapes::ShapePtr kinect_shape;
515 
516  cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
517 
518  Eigen::Affine3d np;
519  for (unsigned int i = 0; i < 5; i++)
520  {
521  np = Eigen::Translation3d(i * .001, i * .001, i * .001) * Eigen::Quaterniond::Identity();
522  cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np);
525  cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
526  }
527 }
528 
529 TEST_F(FclCollisionDetectionTester, TestChangingShapeSize)
530 {
532  kstate1.setToDefaultValues();
533  kstate1.update();
534 
537 
538  ASSERT_FALSE(res1.collision);
539 
541  std::vector<shapes::ShapeConstPtr> shapes;
542  for (unsigned int i = 0; i < 5; i++)
543  {
544  cworld_->getWorld()->removeObject("shape");
545  shapes.clear();
546  poses.clear();
547  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
548  poses.push_back(Eigen::Affine3d::Identity());
549  cworld_->getWorld()->addToObject("shape", shapes, poses);
552  cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
553  ASSERT_TRUE(res.collision);
554  }
555 
556  Eigen::Affine3d kinect_pose = Eigen::Affine3d::Identity();
557  shapes::ShapePtr kinect_shape;
559  cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
562  cworld_->checkCollision(req2, res2, *crobot_, kstate1, *acm_);
563  ASSERT_TRUE(res2.collision);
564  for (unsigned int i = 0; i < 5; i++)
565  {
566  cworld_->getWorld()->removeObject("shape");
567  shapes.clear();
568  poses.clear();
569  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1 + i * .0001, 1 + i * .0001, 1 + i * .0001)));
570  poses.push_back(Eigen::Affine3d::Identity());
571  cworld_->getWorld()->addToObject("shape", shapes, poses);
574  cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
575  ASSERT_TRUE(res.collision);
576  }
577 }
578 
579 int main(int argc, char** argv)
580 {
581  testing::InitGoogleTest(&argc, argv);
582  return RUN_ALL_TESTS();
583 }
collision_detection::CollisionRobotFCL DefaultCRobotType
int main(int argc, char **argv)
#define ROS_INFO_NAMED(name,...)
ROSCPP_DECL void start()
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
TEST_F(FclCollisionDetectionTester, InitOK)
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
urdf::ModelInterfaceSharedPtr urdf_model_
#define M_PI
#define EXPECT_NEAR(a, b, prec)
std::shared_ptr< Shape > ShapePtr
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
collision_detection::CollisionRobotPtr crobot_
bool collision
True if collision was found, false otherwise.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
World::ObjectConstPtr ObjectConstPtr
collision_detection::AllowedCollisionMatrixPtr acm_
#define EXPECT_EQ(a, b)
std::size_t max_contacts
Overall maximum number of contacts to compute.
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
void update(bool force=false)
Update all transforms.
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
collision_detection::CollisionWorldPtr cworld_
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Affine3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
Definition: robot_state.h:1357
def xml_string(rootXml, addHeader=True)
collision_detection::CollisionWorldFCL DefaultCWorldType
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...
Mesh * createMeshFromResource(const std::string &resource)
static WallTime now()
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
#define EXPECT_TRUE(args)
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)...
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 21 2018 10:36:32