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 
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_);
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 int main(int argc, char** argv)
337 {
338  testing::InitGoogleTest(&argc, argv);
339  return RUN_ALL_TESTS();
340 }
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
ASSERT_FALSE
#define ASSERT_FALSE(condition)
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
EXPECT_LE
#define EXPECT_LE(val1, val2)
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
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
DistanceFieldCollisionDetectionTester
Definition: test_collision_distance_field.cpp:58
shape_operations.h
DistanceFieldCollisionDetectionTester::cenv_
collision_detection::CollisionEnvPtr cenv_
Definition: test_collision_distance_field.cpp:81
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
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
robot_model.h
transforms.h
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
shapes::Box
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
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
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
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
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:233
TEST_F
TEST_F(DistanceFieldCollisionDetectionTester, DefaultNotInCollision)
Definition: test_collision_distance_field.cpp:86
ASSERT_TRUE
#define ASSERT_TRUE(condition)
package.h
DefaultCEnvType
collision_detection::CollisionEnvDistanceField DefaultCEnvType
Definition: test_collision_distance_field.cpp:56
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
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
DistanceFieldCollisionDetectionTester::TearDown
void TearDown() override
Definition: test_collision_distance_field.cpp:71
robot_model_test_utils.h
M_PI
#define M_PI
testing::Test
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
main
int main(int argc, char **argv)
Definition: test_collision_distance_field.cpp:336
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
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:206
robot_state.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:117


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Nov 24 2022 03:32:15