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 
45 #include <urdf_parser/urdf_parser.h>
46 
47 #include <fstream>
48 #include <gtest/gtest.h>
49 #include <sstream>
50 #include <algorithm>
51 #include <ctype.h>
52 #include <boost/filesystem.hpp>
53 
54 #include <ros/package.h>
55 
57 
58 class DistanceFieldCollisionDetectionTester : public testing::Test
59 {
60 protected:
61  void SetUp() override
62  {
64 
65  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), true);
66 
67  std::map<std::string, std::vector<collision_detection::CollisionSphere>> link_body_decompositions;
68  cenv_ = std::make_shared<DefaultCEnvType>(robot_model_, link_body_decompositions);
69  }
70 
71  void TearDown() override
72  {
73  }
74 
75 protected:
76  moveit::core::RobotModelPtr robot_model_;
77 
78  moveit::core::TransformsPtr ftf_;
79  moveit::core::TransformsConstPtr ftf_const_;
80 
81  collision_detection::CollisionEnvPtr cenv_;
82 
83  collision_detection::AllowedCollisionMatrixPtr acm_;
84 };
85 
87 {
89  robot_state.setToDefaultValues();
90  robot_state.update();
91 
94  req.group_name = "whole_body";
95  cenv_->checkSelfCollision(req, res, robot_state, *acm_);
96  ASSERT_FALSE(res.collision);
97 }
98 
100 {
102  robot_state.setToDefaultValues();
103  robot_state.update();
104 
108 
109  req.group_name = "right_arm";
110  cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
111  std::map<std::string, double> torso_val;
112  torso_val["torso_lift_joint"] = .15;
113  robot_state.setVariablePositions(torso_val);
114  robot_state.update();
115  cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
116  cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
117 }
118 
120 {
125  // req.contacts = true;
126  // req.max_contacts = 100;
127  req.group_name = "whole_body";
128 
130  robot_state.setToDefaultValues();
131 
132  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
133  offset.translation().x() = .01;
134 
135  robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
136  robot_state.updateStateWithLinkAt("base_bellow_link", offset);
137 
138  acm_->setEntry("base_link", "base_bellow_link", false);
139  cenv_->checkSelfCollision(req, res1, robot_state, *acm_);
140  ASSERT_TRUE(res1.collision);
141 
142  acm_->setEntry("base_link", "base_bellow_link", true);
143  cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
144  ASSERT_FALSE(res2.collision);
145 
146  robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
147  robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
148 
149  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
150  cenv_->checkSelfCollision(req, res3, robot_state, *acm_);
151  ASSERT_TRUE(res3.collision);
152 }
153 
155 {
157  req.contacts = true;
158  req.max_contacts = 1;
159  req.group_name = "whole_body";
160 
162  robot_state.setToDefaultValues();
163 
164  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
165  offset.translation().x() = .01;
166 
167  robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
168  robot_state.updateStateWithLinkAt("base_bellow_link", offset);
169 
170  robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
171  robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
172 
173  acm_->setEntry("base_link", "base_bellow_link", false);
174  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
175 
177  cenv_->checkSelfCollision(req, res, robot_state, *acm_);
178  ASSERT_TRUE(res.collision);
179  EXPECT_EQ(res.contacts.size(), 1u);
180  EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
181 
182  res.clear();
183  req.max_contacts = 2;
184  req.max_contacts_per_pair = 1;
185  // req.verbose = true;
186  cenv_->checkSelfCollision(req, res, robot_state, *acm_);
187  ASSERT_TRUE(res.collision);
188  EXPECT_EQ(res.contact_count, 2u);
189  EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
190 
191  res.contacts.clear();
192  res.contact_count = 0;
193 
194  req.max_contacts = 10;
195  req.max_contacts_per_pair = 2;
196  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), false);
197  cenv_->checkSelfCollision(req, res, robot_state, *acm_);
198  ASSERT_TRUE(res.collision);
199  EXPECT_LE(res.contacts.size(), 10u);
200  EXPECT_LE(res.contact_count, 10u);
201 }
202 
204 {
206  req.contacts = true;
207  req.max_contacts = 1;
208  req.group_name = "whole_body";
209 
211  robot_state.setToDefaultValues();
212 
213  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
214  Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
215 
216  pos1.translation().x() = 5.0;
217  pos2.translation().x() = 5.01;
218 
219  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
220  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
221 
222  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
223 
225  cenv_->checkSelfCollision(req, res, robot_state, *acm_);
226  ASSERT_TRUE(res.collision);
227  ASSERT_EQ(res.contacts.size(), 1u);
228  ASSERT_EQ(res.contacts.begin()->second.size(), 1u);
229 
230  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
231  it != res.contacts.end(); it++)
232  {
233  EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
234  }
235 
236  pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
237  pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
238 
239  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
240  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
241 
243  cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
244  ASSERT_TRUE(res2.collision);
245  ASSERT_EQ(res2.contacts.size(), 1u);
246  ASSERT_EQ(res2.contacts.begin()->second.size(), 1u);
247 
248  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
249  it != res2.contacts.end(); it++)
250  {
251  EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
252  }
253 
254  pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
255  pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(M_PI / 4.0, 0.0, M_PI / 4.0, 0.0));
256 
257  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
258  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
259 
261  cenv_->checkSelfCollision(req, res2, robot_state, *acm_);
262  ASSERT_FALSE(res3.collision);
263 }
264 
266 {
269 
270  req.group_name = "right_arm";
271 
272  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), true);
273 
275  robot_state.setToDefaultValues();
276  robot_state.update();
277 
278  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
279  pos1.translation().x() = 1.0;
280 
281  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
282  cenv_->checkSelfCollision(req, res, robot_state, *acm_);
283  ASSERT_FALSE(res.collision);
284 
285  shapes::Shape* shape = new shapes::Box(.25, .25, .25);
286  cenv_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
287 
289  cenv_->checkRobotCollision(req, res, robot_state, *acm_);
290  ASSERT_TRUE(res.collision);
291 
292  // deletes shape
293  cenv_->getWorld()->removeObject("box");
294 
295  const auto identity = Eigen::Isometry3d::Identity();
296  std::vector<shapes::ShapeConstPtr> shapes;
298  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.25, .25, .25)));
299  poses.push_back(identity);
300  std::set<std::string> touch_links;
301  trajectory_msgs::JointTrajectory empty_state;
302 
303  robot_state.attachBody(std::make_unique<moveit::core::AttachedBody>(
304  robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state));
305 
307  cenv_->checkSelfCollision(req, res, robot_state, *acm_);
308  ASSERT_TRUE(res.collision);
309 
310  // deletes shape
311  robot_state.clearAttachedBody("box");
312 
313  touch_links.insert("r_gripper_palm_link");
314  shapes[0] = std::make_shared<shapes::Box>(.1, .1, .1);
315 
316  robot_state.attachBody(std::make_unique<moveit::core::AttachedBody>(
317  robot_state.getLinkModel("r_gripper_palm_link"), "box", identity, shapes, poses, touch_links, empty_state));
318 
320  cenv_->checkSelfCollision(req, res, robot_state, *acm_);
321  // ASSERT_FALSE(res.collision);
322 
323  pos1.translation().x() = 1.01;
324  shapes::Shape* coll = new shapes::Box(.1, .1, .1);
325  cenv_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
327  cenv_->checkRobotCollision(req, res, robot_state, *acm_);
328  ASSERT_TRUE(res.collision);
329 
330  acm_->setEntry("coll", "r_gripper_palm_link", true);
332  cenv_->checkRobotCollision(req, res, robot_state, *acm_);
333  ASSERT_TRUE(res.collision);
334 }
335 
336 TEST(DistanceFieldCollisionDetectionPadding, Sphere)
337 {
338  geometry_msgs::Pose origin;
339  origin.orientation.w = 1.0;
340  moveit::core::RobotModelPtr robot_model{
341  moveit::core::RobotModelBuilder{ "test", "base" }.addCollisionSphere("base", 0.04, origin).build()
342  };
343  ASSERT_TRUE(static_cast<bool>(robot_model));
344 
345  auto world = std::make_shared<collision_detection::World>();
346  world->addToObject("box", Eigen::Isometry3d::Identity(), std::make_shared<shapes::Box>(.02, .02, .02),
347  Eigen::Isometry3d::Identity());
348 
350  robot_model,
351  world,
352  std::map<std::string, std::vector<collision_detection::CollisionSphere>>(),
353  0.1, // size_x
354  0.1, // size_y
355  0.2, // size_z
356  Eigen::Vector3d(0, 0, 0), // origin
361  0.04 // non-zero padding
362  };
363 
364  // required to invoke collision checks
366  collision_detection::AllowedCollisionMatrix acm{ robot_model->getLinkModelNames() };
369 
370  cenv.checkRobotCollision(req, res, robot_state, acm);
371  EXPECT_TRUE(res.collision) << "Trivial collision between two primitives should be detected";
372  res.clear();
373 
374  world->setObjectPose("box", Eigen::Isometry3d::Identity() * Eigen::Translation3d(0, 0, 0.08));
375  cenv.checkRobotCollision(req, res, robot_state, acm);
376  EXPECT_TRUE(res.collision) << "Collision should be detected when box is in padding area of base";
377  res.clear();
378 
379  world->setObjectPose("box", Eigen::Isometry3d::Identity() * Eigen::Translation3d(0, 0, 0.1));
380  cenv.checkRobotCollision(req, res, robot_state, acm);
381  EXPECT_FALSE(res.collision) << "No collision should be reported when box is outside of padding area";
382  res.clear();
383 }
384 
385 int main(int argc, char** argv)
386 {
387  testing::InitGoogleTest(&argc, argv);
388  return RUN_ALL_TESTS();
389 }
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
collision_detection::DEFAULT_COLLISION_TOLERANCE
static const double DEFAULT_COLLISION_TOLERANCE
Definition: collision_env_distance_field.h:85
collision_distance_field_types.h
DistanceFieldCollisionDetectionTester::SetUp
void SetUp() override
Definition: test_collision_distance_field.cpp:61
DistanceFieldCollisionDetectionTester::acm_
collision_detection::AllowedCollisionMatrixPtr acm_
Definition: test_collision_distance_field.cpp:83
DistanceFieldCollisionDetectionTester
Definition: test_collision_distance_field.cpp:58
shape_operations.h
DistanceFieldCollisionDetectionTester::cenv_
collision_detection::CollisionEnvPtr cenv_
Definition: test_collision_distance_field.cpp:81
collision_detection::DEFAULT_MAX_PROPOGATION_DISTANCE
static const double DEFAULT_MAX_PROPOGATION_DISTANCE
Definition: collision_env_distance_field.h:86
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
collision_env_distance_field.h
shapes::Shape
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:179
robot_model.h
EXPECT_TRUE
#define EXPECT_TRUE(args)
transforms.h
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix.
Definition: collision_matrix.h:112
shapes::Box
TEST
TEST(DistanceFieldCollisionDetectionPadding, Sphere)
Definition: test_collision_distance_field.cpp:336
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:382
EXPECT_NEAR
#define EXPECT_NEAR(a, b, prec)
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:212
DistanceFieldCollisionDetectionTester::ftf_const_
moveit::core::TransformsConstPtr ftf_const_
Definition: test_collision_distance_field.cpp:79
collision_detection::CollisionEnvDistanceField
Definition: collision_env_distance_field.h:90
DistanceFieldCollisionDetectionTester::ftf_
moveit::core::TransformsPtr ftf_
Definition: test_collision_distance_field.cpp:78
collision_detection::CollisionRequest::group_name
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)
Definition: include/moveit/collision_detection/collision_common.h:197
TEST_F
TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision)
Definition: test_collision_distance_field.cpp:86
moveit::core::RobotModelBuilder::build
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
Definition: robot_model_test_utils.cpp:484
package.h
DefaultCEnvType
collision_detection::CollisionEnvDistanceField DefaultCEnvType
Definition: test_collision_distance_field.cpp:56
moveit::core::RobotModelBuilder::addCollisionSphere
RobotModelBuilder & addCollisionSphere(const std::string &link_name, double dims, geometry_msgs::Pose origin)
Adds a collision sphere to a specific link.
Definition: robot_model_test_utils.cpp:333
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:418
DistanceFieldCollisionDetectionTester::TearDown
void TearDown() override
Definition: test_collision_distance_field.cpp:71
robot_model_test_utils.h
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition: robot_model_test_utils.h:158
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:209
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:406
main
int main(int argc, char **argv)
Definition: test_collision_distance_field.cpp:385
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:216
collision_detection::CollisionResult::clear
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
Definition: include/moveit/collision_detection/collision_common.h:392
DistanceFieldCollisionDetectionTester::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_collision_distance_field.cpp:76
collision_detection::CollisionResult::contact_count
std::size_t contact_count
Number of contacts returned.
Definition: include/moveit/collision_detection/collision_common.h:415
robot_state.h
EXPECT_EQ
#define EXPECT_EQ(a, b)
EXPECT_FALSE
#define EXPECT_FALSE(args)
collision_detection::DEFAULT_USE_SIGNED_DISTANCE_FIELD
static const bool DEFAULT_USE_SIGNED_DISTANCE_FIELD
Definition: collision_env_distance_field.h:83
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:117
collision_detection::DEFAULT_RESOLUTION
static const double DEFAULT_RESOLUTION
Definition: collision_env_distance_field.h:84


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jul 21 2024 02:24:29