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 
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 
55 #include <ros/package.h>
56 
59 
60 class DistanceFieldCollisionDetectionTester : public testing::Test
61 {
62 protected:
63  void SetUp() override
64  {
66 
67  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
68 
69  std::map<std::string, std::vector<collision_detection::CollisionSphere>> link_body_decompositions;
70  crobot_.reset(new DefaultCRobotType(robot_model_, link_body_decompositions));
71  cworld_.reset(new DefaultCWorldType());
72  }
73 
74  void TearDown() override
75  {
76  }
77 
78 protected:
79  moveit::core::RobotModelPtr robot_model_;
80 
81  robot_state::TransformsPtr ftf_;
82  robot_state::TransformsConstPtr ftf_const_;
83 
84  collision_detection::CollisionRobotPtr crobot_;
85  collision_detection::CollisionWorldPtr cworld_;
86 
87  collision_detection::AllowedCollisionMatrixPtr acm_;
88 };
89 
91 {
93  robot_state.setToDefaultValues();
94  robot_state.update();
95 
98  req.group_name = "whole_body";
99  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
100  ASSERT_FALSE(res.collision);
101 }
102 
104 {
106  robot_state.setToDefaultValues();
107  robot_state.update();
108 
112 
113  req.group_name = "right_arm";
114  crobot_->checkSelfCollision(req, res1, robot_state, *acm_);
115  std::map<std::string, double> torso_val;
116  torso_val["torso_lift_joint"] = .15;
117  robot_state.setVariablePositions(torso_val);
118  robot_state.update();
119  crobot_->checkSelfCollision(req, res1, robot_state, *acm_);
120  crobot_->checkSelfCollision(req, res1, robot_state, *acm_);
121 }
122 
124 {
129  // req.contacts = true;
130  // req.max_contacts = 100;
131  req.group_name = "whole_body";
132 
134  robot_state.setToDefaultValues();
135 
136  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
137  offset.translation().x() = .01;
138 
139  robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
140  robot_state.updateStateWithLinkAt("base_bellow_link", offset);
141 
142  acm_->setEntry("base_link", "base_bellow_link", false);
143  crobot_->checkSelfCollision(req, res1, robot_state, *acm_);
144  ASSERT_TRUE(res1.collision);
145 
146  acm_->setEntry("base_link", "base_bellow_link", true);
147  crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
148  ASSERT_FALSE(res2.collision);
149 
150  robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
151  robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
152 
153  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
154  crobot_->checkSelfCollision(req, res3, robot_state, *acm_);
155  ASSERT_TRUE(res3.collision);
156 }
157 
159 {
161  req.contacts = true;
162  req.max_contacts = 1;
163  req.group_name = "whole_body";
164 
166  robot_state.setToDefaultValues();
167 
168  Eigen::Isometry3d offset = Eigen::Isometry3d::Identity();
169  offset.translation().x() = .01;
170 
171  robot_state.updateStateWithLinkAt("base_link", Eigen::Isometry3d::Identity());
172  robot_state.updateStateWithLinkAt("base_bellow_link", offset);
173 
174  robot_state.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Isometry3d::Identity());
175  robot_state.updateStateWithLinkAt("l_gripper_palm_link", offset);
176 
177  acm_->setEntry("base_link", "base_bellow_link", false);
178  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
179 
181  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
182  ASSERT_TRUE(res.collision);
183  EXPECT_EQ(res.contacts.size(), 1u);
184  EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
185 
186  res.clear();
187  req.max_contacts = 2;
188  req.max_contacts_per_pair = 1;
189  // req.verbose = true;
190  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
191  ASSERT_TRUE(res.collision);
192  EXPECT_EQ(res.contact_count, 2u);
193  EXPECT_EQ(res.contacts.begin()->second.size(), 1u);
194 
195  res.contacts.clear();
196  res.contact_count = 0;
197 
198  req.max_contacts = 10;
199  req.max_contacts_per_pair = 2;
200  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), false));
201  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
202  ASSERT_TRUE(res.collision);
203  EXPECT_LE(res.contacts.size(), 10u);
204  EXPECT_LE(res.contact_count, 10u);
205 }
206 
208 {
210  req.contacts = true;
211  req.max_contacts = 1;
212  req.group_name = "whole_body";
213 
215  robot_state.setToDefaultValues();
216 
217  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
218  Eigen::Isometry3d pos2 = Eigen::Isometry3d::Identity();
219 
220  pos1.translation().x() = 5.0;
221  pos2.translation().x() = 5.01;
222 
223  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
224  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
225 
226  acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
227 
229  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
230  ASSERT_TRUE(res.collision);
231  ASSERT_EQ(res.contacts.size(), 1u);
232  ASSERT_EQ(res.contacts.begin()->second.size(), 1u);
233 
234  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
235  it != res.contacts.end(); it++)
236  {
237  EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
238  }
239 
240  pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
241  pos2 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
242 
243  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
244  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
245 
247  crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
248  ASSERT_TRUE(res2.collision);
249  ASSERT_EQ(res2.contacts.size(), 1u);
250  ASSERT_EQ(res2.contacts.begin()->second.size(), 1u);
251 
252  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
253  it != res2.contacts.end(); it++)
254  {
255  EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
256  }
257 
258  pos1 = Eigen::Isometry3d(Eigen::Translation3d(3.0, 0.0, 0.0) * Eigen::Quaterniond::Identity());
259  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));
260 
261  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
262  robot_state.updateStateWithLinkAt("l_gripper_palm_link", pos2);
263 
265  crobot_->checkSelfCollision(req, res2, robot_state, *acm_);
266  ASSERT_FALSE(res3.collision);
267 }
268 
270 {
273 
274  req.group_name = "right_arm";
275 
276  acm_.reset(new collision_detection::AllowedCollisionMatrix(robot_model_->getLinkModelNames(), true));
277 
279  robot_state.setToDefaultValues();
280  robot_state.update();
281 
282  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
283  pos1.translation().x() = 1.0;
284 
285  robot_state.updateStateWithLinkAt("r_gripper_palm_link", pos1);
286  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
287  ASSERT_FALSE(res.collision);
288 
289  shapes::Shape* shape = new shapes::Box(.25, .25, .25);
290  cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
291 
293  cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
294  ASSERT_TRUE(res.collision);
295 
296  // deletes shape
297  cworld_->getWorld()->removeObject("box");
298 
299  std::vector<shapes::ShapeConstPtr> shapes;
301  shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.25, .25, .25)));
302  poses.push_back(Eigen::Isometry3d::Identity());
303  std::set<std::string> touch_links;
304  trajectory_msgs::JointTrajectory empty_state;
306  robot_state.getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state);
307 
308  robot_state.attachBody(attached_body);
309 
311  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
312  ASSERT_TRUE(res.collision);
313 
314  // deletes shape
315  robot_state.clearAttachedBody("box");
316 
317  touch_links.insert("r_gripper_palm_link");
318  shapes[0].reset(new shapes::Box(.1, .1, .1));
319 
321  robot_state.getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state);
322  robot_state.attachBody(attached_body_1);
323 
325  crobot_->checkSelfCollision(req, res, robot_state, *acm_);
326  // ASSERT_FALSE(res.collision);
327 
328  pos1.translation().x() = 1.01;
329  shapes::Shape* coll = new shapes::Box(.1, .1, .1);
330  cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
332  cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
333  ASSERT_TRUE(res.collision);
334 
335  acm_->setEntry("coll", "r_gripper_palm_link", true);
337  cworld_->checkRobotCollision(req, res, *crobot_, robot_state, *acm_);
338  ASSERT_TRUE(res.collision);
339 }
340 
341 int main(int argc, char** argv)
342 {
343  testing::InitGoogleTest(&argc, argv);
344  return RUN_ALL_TESTS();
345 }
Core components of MoveIt!
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
int main(int argc, char **argv)
collision_detection::CollisionRobotDistanceField DefaultCRobotType
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 body ids in contact, plus their contact details.
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.
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...
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
void updateStateWithLinkAt(const std::string &link_name, const Eigen::Isometry3d &transform, bool backward=false)
Update the state after setting a particular link to the input global transform pose.
Definition: robot_state.h:1466
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:161
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...
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:128
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
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 Thu Mar 17 2022 02:51:04