moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_collision_common_panda.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, Jens Petit
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 copyright holder 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: Jens Petit */
36 
43 
44 #include <urdf_parser/urdf_parser.h>
45 #include <geometric_shapes/shape_operations.h>
46 
47 #include <gtest/gtest.h>
48 #include <sstream>
49 #include <algorithm>
50 #include <ctype.h>
51 #include <fstream>
52 
54 inline void setToHome(moveit::core::RobotState& panda_state)
55 {
56  panda_state.setToDefaultValues();
57  double joint2 = -0.785;
58  double joint4 = -2.356;
59  double joint6 = 1.571;
60  double joint7 = 0.785;
61  panda_state.setJointPositions("panda_joint2", &joint2);
62  panda_state.setJointPositions("panda_joint4", &joint4);
63  panda_state.setJointPositions("panda_joint6", &joint6);
64  panda_state.setJointPositions("panda_joint7", &joint7);
65  panda_state.update();
66 }
67 
68 template <class CollisionAllocatorType>
69 class CollisionDetectorPandaTest : public testing::Test
70 {
71 public:
72  std::shared_ptr<CollisionAllocatorType> value_;
73 
74 protected:
75  void SetUp() override
76  {
77  value_ = std::make_shared<CollisionAllocatorType>();
79  robot_model_ok_ = static_cast<bool>(robot_model_);
80 
81  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*robot_model_->getSRDF());
82 
83  cenv_ = value_->allocateEnv(robot_model_);
84 
85  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
87  }
88 
89  void TearDown() override
90  {
91  }
92 
94 
95  moveit::core::RobotModelPtr robot_model_;
96 
97  collision_detection::CollisionEnvPtr cenv_;
98 
99  collision_detection::AllowedCollisionMatrixPtr acm_;
100 
101  moveit::core::RobotStatePtr robot_state_;
102 };
103 
105 
108 {
109  ASSERT_TRUE(this->robot_model_ok_);
110 }
111 
114 {
117  this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
118  ASSERT_FALSE(res.collision);
119 }
120 
123 {
124  // Sets the joints into a colliding configuration
125  double joint2 = 0.15;
126  double joint4 = -3.0;
127  this->robot_state_->setJointPositions("panda_joint2", &joint2);
128  this->robot_state_->setJointPositions("panda_joint4", &joint4);
129  this->robot_state_->update();
130 
133  this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
134  ASSERT_TRUE(res.collision);
135 }
136 
139 {
142 
143  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
144  shapes::ShapeConstPtr shape_ptr(shape);
145 
146  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
147  pos1.translation().z() = 0.3;
148  this->cenv_->getWorld()->addToObject("box", pos1, shape_ptr, Eigen::Isometry3d::Identity());
149 
150  this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
151  ASSERT_FALSE(res.collision);
152  res.clear();
153 
154  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
155  ASSERT_TRUE(res.collision);
156  res.clear();
157 
158  this->cenv_->getWorld()->moveObject("box", pos1);
159  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
160  ASSERT_TRUE(res.collision);
161  res.clear();
162 
163  this->cenv_->getWorld()->moveObject("box", pos1);
164  ASSERT_FALSE(res.collision);
165 }
166 
169 {
172  req.max_contacts = 10;
173  req.contacts = true;
174  req.verbose = true;
175 
176  shapes::Shape* shape = new shapes::Box(.4, .4, .4);
177  shapes::ShapeConstPtr shape_ptr(shape);
178 
179  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
180  pos1.translation().z() = 0.3;
181  this->cenv_->getWorld()->addToObject("box", pos1, shape_ptr, Eigen::Isometry3d::Identity());
182 
183  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
184  ASSERT_TRUE(res.collision);
185  ASSERT_GE(res.contact_count, 3u);
186  res.clear();
187 }
188 
191 {
193  req.contacts = true;
194  req.max_contacts = 10;
196 
197  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
198  ASSERT_FALSE(res.collision);
199  res.clear();
200 
201  // Adding the box right in front of the robot hand
202  shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1);
203  shapes::ShapeConstPtr shape_ptr(shape);
204 
205  Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
206  pos.translation().x() = 0.43;
207  pos.translation().y() = 0;
208  pos.translation().z() = 0.55;
209  this->cenv_->getWorld()->addToObject("box", pos, shape_ptr, Eigen::Isometry3d::Identity());
210 
211  this->cenv_->setLinkPadding("panda_hand", 0.08);
212  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
213  ASSERT_TRUE(res.collision);
214  res.clear();
215 
216  this->cenv_->setLinkPadding("panda_hand", 0.0);
217  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
218  ASSERT_FALSE(res.collision);
219 }
220 
223 {
225  req.distance = true;
227  this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
228  ASSERT_FALSE(res.collision);
229  EXPECT_NEAR(res.distance, 0.022, 0.001);
230 }
231 
233 {
235  req.distance = true;
237 
238  // Adding the box right in front of the robot hand
239  shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1);
240  shapes::ShapeConstPtr shape_ptr(shape);
241 
242  Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
243  pos.translation().x() = 0.43;
244  pos.translation().y() = 0;
245  pos.translation().z() = 0.55;
246  this->cenv_->getWorld()->addToObject("box", pos, shape_ptr, Eigen::Isometry3d::Identity());
247 
248  this->cenv_->setLinkPadding("panda_hand", 0.0);
249  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
250  ASSERT_FALSE(res.collision);
251  EXPECT_NEAR(res.distance, 0.029, 0.01);
252 }
253 
254 template <class CollisionAllocatorType>
255 class DistanceCheckPandaTest : public CollisionDetectorPandaTest<CollisionAllocatorType>
256 {
257 };
258 
260 
262 {
263  std::set<const moveit::core::LinkModel*> active_components{ this->robot_model_->getLinkModel("panda_hand") };
266  req.active_components_only = &active_components;
267  req.enable_signed_distance = true;
268 
269  random_numbers::RandomNumberGenerator rng(0x47110815);
270  double min_distance = std::numeric_limits<double>::max();
271  for (int i = 0; i < 10; ++i)
272  {
274 
275  shapes::ShapeConstPtr shape = std::make_shared<const shapes::Cylinder>(rng.uniform01(), rng.uniform01());
276  Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() };
277  pose.translation() =
278  Eigen::Vector3d(rng.uniformReal(0.1, 2.0), rng.uniformReal(0.1, 2.0), rng.uniformReal(1.2, 1.7));
279  double quat[4];
280  rng.quaternion(quat);
281  pose.linear() = Eigen::Quaterniond(quat[0], quat[1], quat[2], quat[3]).toRotationMatrix();
282 
283  this->cenv_->getWorld()->addToObject("collection", Eigen::Isometry3d::Identity(), shape, pose);
284  this->cenv_->getWorld()->removeObject("object");
285  this->cenv_->getWorld()->addToObject("object", pose, shape, Eigen::Isometry3d::Identity());
286 
287  this->cenv_->distanceRobot(req, res, *this->robot_state_);
288  auto& distances1 = res.distances[std::pair<std::string, std::string>("collection", "panda_hand")];
289  auto& distances2 = res.distances[std::pair<std::string, std::string>("object", "panda_hand")];
290  ASSERT_EQ(distances1.size(), 1u) << "no distance reported for collection/panda_hand";
291  ASSERT_EQ(distances2.size(), 1u) << "no distance reported for object/panda_hand";
292 
293  double collection_distance = distances1[0].distance;
294  min_distance = std::min(min_distance, distances2[0].distance);
295  ASSERT_NEAR(collection_distance, min_distance, 1e-5)
296  << "distance to collection is greater than distance to minimum of individual objects after " << i << " rounds";
297  }
298 }
299 
300 // FCL < 0.6 completely fails the DistancePoints test, so we have two test
301 // suites, one with and one without the test.
302 template <class CollisionAllocatorType>
303 class DistanceFullPandaTest : public DistanceCheckPandaTest<CollisionAllocatorType>
304 {
305 };
306 
308 
311 {
312  // Adding the box right in front of the robot hand
313  shapes::Box* shape = new shapes::Box(0.1, 0.1, 0.1);
314  shapes::ShapeConstPtr shape_ptr(shape);
315 
316  Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
317  pos.translation().x() = 0.43;
318  pos.translation().y() = 0;
319  pos.translation().z() = 0.55;
320  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos);
321 
323  req.acm = &*this->acm_;
325  req.enable_nearest_points = true;
326  req.max_contacts_per_body = 1;
327 
329  this->cenv_->distanceRobot(req, res, *this->robot_state_);
330 
331  // Checks a particular point is inside the box
332  auto check_in_box = [&](const Eigen::Vector3d& p) {
333  Eigen::Vector3d in_box = pos.inverse() * p;
334 
335  constexpr double eps = 1e-5;
336  EXPECT_LE(std::abs(in_box.x()), shape->size[0] + eps);
337  EXPECT_LE(std::abs(in_box.y()), shape->size[1] + eps);
338  EXPECT_LE(std::abs(in_box.z()), shape->size[2] + eps);
339  };
340 
341  // Check that all points reported on "box" are actually on the box and not
342  // on the robot
343  for (auto& distance : res.distances)
344  {
345  for (auto& pair : distance.second)
346  {
347  if (pair.link_names[0] == "box")
348  check_in_box(pair.nearest_points[0]);
349  else if (pair.link_names[1] == "box")
350  check_in_box(pair.nearest_points[1]);
351  else
352  ADD_FAILURE() << "Unrecognized link names";
353  }
354  }
355 }
356 
357 REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision,
358  RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld);
359 
361 
collision_detection::CollisionEnvPtr cenv_
moveit::core::RobotModelPtr robot_model_
std::shared_ptr< CollisionAllocatorType > value_
moveit::core::RobotStatePtr robot_state_
collision_detection::AllowedCollisionMatrixPtr acm_
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void update(bool force=false)
Update all transforms.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:515
@ SINGLE
Find the global minimum for each pair.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
p
Definition: pick.py:62
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
Representation of a collision checking request.
bool verbose
Flag indicating whether information about detected collisions should be reported.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
bool distance
If true, compute proximity distance.
Representation of a collision checking result.
double distance
Closest distance between two bodies.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
bool collision
True if collision was found, false otherwise.
std::size_t contact_count
Number of contacts returned.
Representation of a distance-reporting request.
const std::set< const moveit::core::LinkModel * > * active_components_only
The set of active components to check.
std::size_t max_contacts_per_body
Maximum number of contacts to store for bodies (multiple bodies may be within distance threshold)
bool enable_signed_distance
Indicate if a signed distance should be calculated in a collision.
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
bool enable_nearest_points
Indicate if nearest point information should be calculated.
Result of a distance request.
DistanceMap distances
A map of distance data for each link in the req.active_components_only.
TYPED_TEST_CASE_P(CollisionDetectorPandaTest)
void setToHome(moveit::core::RobotState &panda_state)
Brings the panda robot in user defined home position.
TYPED_TEST_P(CollisionDetectorPandaTest, InitOK)
Correct setup testing.
REGISTER_TYPED_TEST_SUITE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld)