test_collision_distance_field.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, 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 the 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 
43 #include <moveit_resources/config.h>
44 
46 #include <urdf_parser/urdf_parser.h>
47 
48 #include <fstream>
49 #include <gtest/gtest.h>
50 #include <sstream>
51 #include <algorithm>
52 #include <ctype.h>
53 #include <boost/filesystem.hpp>
54 
57 
58 class DistanceFieldCollisionDetectionTester : public testing::Test
59 {
60 protected:
61  virtual void SetUp()
62  {
63  srdf_model_.reset(new srdf::Model());
64  std::string xml_string;
65  std::fstream xml_file(MOVEIT_TEST_RESOURCES_DIR "/pr2_description/urdf/robot.xml", std::fstream::in);
66  if (xml_file.is_open())
67  {
68  while (xml_file.good())
69  {
70  std::string line;
71  std::getline(xml_file, line);
72  xml_string += (line + "\n");
73  }
74  xml_file.close();
75  urdf_model_ = urdf::parseURDF(xml_string);
76  urdf_ok_ = urdf_model_ ? true : false;
77  }
78  else
79  urdf_ok_ = false;
80  srdf_ok_ = srdf_model_->initFile(*urdf_model_, MOVEIT_TEST_RESOURCES_DIR "/pr2_description/srdf/robot.xml");
81 
83 
84  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
85 
86  std::map<std::string, std::vector<collision_detection::CollisionSphere>> link_body_decompositions;
87  crobot_.reset(new DefaultCRobotType(robot_model_, link_body_decompositions));
88  cworld_.reset(new DefaultCWorldType());
89  }
90 
91  virtual void TearDown()
92  {
93  }
94 
95 protected:
96  bool urdf_ok_;
97  bool srdf_ok_;
98 
99  urdf::ModelInterfaceSharedPtr urdf_model_;
101 
102  robot_model::RobotModelPtr robot_model_;
103 
104  robot_state::TransformsPtr ftf_;
105  robot_state::TransformsConstPtr ftf_const_;
106 
107  collision_detection::CollisionRobotPtr crobot_;
108  collision_detection::CollisionWorldPtr cworld_;
109 
110  collision_detection::AllowedCollisionMatrixPtr acm_;
111 };
112 
114 {
116  kstate.setToDefaultValues();
117  kstate.update();
118 
119  ASSERT_TRUE(urdf_ok_ && srdf_ok_);
120 
123  req.group_name = "whole_body";
124  crobot_->checkSelfCollision(req, res, kstate, *acm_);
125  ASSERT_FALSE(res.collision);
126 }
127 
129 {
131  kstate.setToDefaultValues();
132  kstate.update();
133 
137 
138  req.group_name = "right_arm";
139  crobot_->checkSelfCollision(req, res1, kstate, *acm_);
140  std::map<std::string, double> torso_val;
141  torso_val["torso_lift_joint"] = .15;
142  kstate.setVariablePositions(torso_val);
143  kstate.update();
144  crobot_->checkSelfCollision(req, res1, kstate, *acm_);
145  crobot_->checkSelfCollision(req, res1, kstate, *acm_);
146 }
147 
149 {
154  // req.contacts = true;
155  // req.max_contacts = 100;
156  req.group_name = "whole_body";
157 
159  kstate.setToDefaultValues();
160 
161  Eigen::Affine3d offset = Eigen::Affine3d::Identity();
162  offset.translation().x() = .01;
163 
164  kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
165  kstate.updateStateWithLinkAt("base_bellow_link", offset);
166 
167  acm_->setEntry("base_link", "base_bellow_link", false);
168  crobot_->checkSelfCollision(req, res1, kstate, *acm_);
169  ASSERT_TRUE(res1.collision);
170 
171  acm_->setEntry("base_link", "base_bellow_link", true);
172  crobot_->checkSelfCollision(req, res2, kstate, *acm_);
173  ASSERT_FALSE(res2.collision);
174 
175  kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
176  kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
177 
178  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
179  crobot_->checkSelfCollision(req, res3, kstate, *acm_);
180  ASSERT_TRUE(res3.collision);
181 }
182 
184 {
186  req.contacts = true;
187  req.max_contacts = 1;
188  req.group_name = "whole_body";
189 
191  kstate.setToDefaultValues();
192 
193  Eigen::Affine3d offset = Eigen::Affine3d::Identity();
194  offset.translation().x() = .01;
195 
196  kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
197  kstate.updateStateWithLinkAt("base_bellow_link", offset);
198 
199  kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
200  kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
201 
202  acm_->setEntry("base_link", "base_bellow_link", false);
203  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
204 
206  crobot_->checkSelfCollision(req, res, kstate, *acm_);
207  ASSERT_TRUE(res.collision);
208  EXPECT_EQ(res.contacts.size(), 1);
209  EXPECT_EQ(res.contacts.begin()->second.size(), 1);
210 
211  res.clear();
212  req.max_contacts = 2;
213  req.max_contacts_per_pair = 1;
214  // req.verbose = true;
215  crobot_->checkSelfCollision(req, res, kstate, *acm_);
216  ASSERT_TRUE(res.collision);
217  EXPECT_EQ(res.contact_count, 2);
218  EXPECT_EQ(res.contacts.begin()->second.size(), 1);
219 
220  res.contacts.clear();
221  res.contact_count = 0;
222 
223  req.max_contacts = 10;
224  req.max_contacts_per_pair = 2;
225  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false));
226  crobot_->checkSelfCollision(req, res, kstate, *acm_);
227  ASSERT_TRUE(res.collision);
228  EXPECT_LE(res.contacts.size(), 10);
229  EXPECT_LE(res.contact_count, 10);
230 }
231 
233 {
235  req.contacts = true;
236  req.max_contacts = 1;
237  req.group_name = "whole_body";
238 
240  kstate.setToDefaultValues();
241 
242  Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
243  Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
244 
245  pos1.translation().x() = 5.0;
246  pos2.translation().x() = 5.01;
247 
248  kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
249  kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
250 
251  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
252 
254  crobot_->checkSelfCollision(req, res, kstate, *acm_);
255  ASSERT_TRUE(res.collision);
256  ASSERT_EQ(res.contacts.size(), 1);
257  ASSERT_EQ(res.contacts.begin()->second.size(), 1);
258 
259  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
260  it != res.contacts.end(); it++)
261  {
262  EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
263  }
264 
265  pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
266  pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
267 
268  kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
269  kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
270 
272  crobot_->checkSelfCollision(req, res2, kstate, *acm_);
273  ASSERT_TRUE(res2.collision);
274  ASSERT_EQ(res2.contacts.size(), 1);
275  ASSERT_EQ(res2.contacts.begin()->second.size(), 1);
276 
277  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
278  it != res2.contacts.end(); it++)
279  {
280  EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
281  }
282 
283  pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
284  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));
285 
286  kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
287  kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
288 
290  crobot_->checkSelfCollision(req, res2, kstate, *acm_);
291  ASSERT_FALSE(res3.collision);
292 }
293 
295 {
298 
299  req.group_name = "right_arm";
300 
301  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
302 
304  kstate.setToDefaultValues();
305  kstate.update();
306 
307  Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
308  pos1.translation().x() = 1.0;
309 
310  kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
311  crobot_->checkSelfCollision(req, res, kstate, *acm_);
312  ASSERT_FALSE(res.collision);
313 
314  shapes::Shape* shape = new shapes::Box(.25, .25, .25);
315  cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
316 
318  cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
319  ASSERT_TRUE(res.collision);
320 
321  // deletes shape
322  cworld_->getWorld()->removeObject("box");
323 
324  std::vector<shapes::ShapeConstPtr> shapes;
326  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.25, .25, .25)));
327  poses.push_back(Eigen::Affine3d::Identity());
328  std::set<std::string> touch_links;
329  trajectory_msgs::JointTrajectory empty_state;
331  kstate.getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state);
332 
333  kstate.attachBody(attached_body);
334 
336  crobot_->checkSelfCollision(req, res, kstate, *acm_);
337  ASSERT_TRUE(res.collision);
338 
339  // deletes shape
340  kstate.clearAttachedBody("box");
341 
342  touch_links.insert("r_gripper_palm_link");
343  shapes[0].reset(new shapes::Box(.1, .1, .1));
344 
346  kstate.getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state);
347  kstate.attachBody(attached_body_1);
348 
350  crobot_->checkSelfCollision(req, res, kstate, *acm_);
351  // ASSERT_FALSE(res.collision);
352 
353  pos1.translation().x() = 1.01;
354  shapes::Shape* coll = new shapes::Box(.1, .1, .1);
355  cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
357  cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
358  ASSERT_TRUE(res.collision);
359 
360  acm_->setEntry("coll", "r_gripper_palm_link", true);
362  cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
363  ASSERT_TRUE(res.collision);
364 }
365 
366 int main(int argc, char** argv)
367 {
368  testing::InitGoogleTest(&argc, argv);
369  return RUN_ALL_TESTS();
370 }
int main(int argc, char **argv)
collision_detection::CollisionRobotDistanceField DefaultCRobotType
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
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...
TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision)
#define M_PI
#define EXPECT_NEAR(a, b, prec)
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
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...
#define EXPECT_EQ(a, b)
std::size_t max_contacts
Overall maximum number of contacts to compute.
void update(bool force=false)
Update all transforms.
collision_detection::CollisionWorldPtr cworld_
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
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:1360
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
def xml_string(rootXml, addHeader=True)
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...
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:156
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
collision_detection::CollisionWorldDistanceField DefaultCWorldType
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
collision_detection::AllowedCollisionMatrixPtr acm_
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 Sat Feb 9 2019 04:09:03