test_fcl_env.cpp
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 
37 #include <gtest/gtest.h>
38 #include <ros/ros.h>
39 
40 #include <moveit/collision_detection/collision_common.h>
41 
42 #include <moveit/robot_model/robot_model.h>
43 #include <moveit/robot_state/robot_state.h>
44 #include <moveit/utils/robot_model_test_utils.h>
45 
46 #include <moveit/collision_detection_fcl/collision_common.h>
47 #include <moveit/collision_detection_fcl/collision_env_fcl.h>
48 
49 #include <urdf_parser/urdf_parser.h>
50 #include <geometric_shapes/shape_operations.h>
51 
53 inline void setToHome(moveit::core::RobotState& panda_state)
54 {
55  panda_state.setToDefaultValues();
56  double joint2 = -0.785;
57  double joint4 = -2.356;
58  double joint6 = 1.571;
59  double joint7 = 0.785;
60  panda_state.setJointPositions("panda_joint2", &joint2);
61  panda_state.setJointPositions("panda_joint4", &joint4);
62  panda_state.setJointPositions("panda_joint6", &joint6);
63  panda_state.setJointPositions("panda_joint7", &joint7);
64  panda_state.update();
65 }
66 
67 class CollisionDetectionEnvTest : public testing::Test
68 {
69 protected:
70  void SetUp() override
71  {
72  robot_model_ = moveit::core::loadTestingRobotModel("panda");
73  robot_model_ok_ = static_cast<bool>(robot_model_);
74 
75  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(robot_model_->getLinkModelNames(), false);
76 
77  acm_->setEntry("panda_link0", "panda_link1", true);
78  acm_->setEntry("panda_link1", "panda_link2", true);
79  acm_->setEntry("panda_link2", "panda_link3", true);
80  acm_->setEntry("panda_link3", "panda_link4", true);
81  acm_->setEntry("panda_link4", "panda_link5", true);
82  acm_->setEntry("panda_link5", "panda_link6", true);
83  acm_->setEntry("panda_link6", "panda_link7", true);
84  acm_->setEntry("panda_link7", "panda_hand", true);
85  acm_->setEntry("panda_hand", "panda_rightfinger", true);
86  acm_->setEntry("panda_hand", "panda_leftfinger", true);
87  acm_->setEntry("panda_rightfinger", "panda_leftfinger", true);
88  acm_->setEntry("panda_link5", "panda_link7", true);
89  acm_->setEntry("panda_link6", "panda_hand", true);
90 
91  c_env_ = std::make_shared<collision_detection::CollisionEnvFCL>(robot_model_);
92 
93  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
94 
95  setToHome(*robot_state_);
96  }
97 
98  void TearDown() override
99  {
100  }
101 
102 protected:
103  bool robot_model_ok_;
104 
105  moveit::core::RobotModelPtr robot_model_;
106 
107  collision_detection::CollisionEnvPtr c_env_;
108 
109  collision_detection::AllowedCollisionMatrixPtr acm_;
110 
111  moveit::core::RobotStatePtr robot_state_;
112 };
113 
115 TEST_F(CollisionDetectionEnvTest, InitOK)
116 {
117  ASSERT_TRUE(robot_model_ok_);
118 }
119 
121 TEST_F(CollisionDetectionEnvTest, DefaultNotInCollision)
122 {
123  collision_detection::CollisionRequest req;
124  collision_detection::CollisionResult res;
125  c_env_->checkSelfCollision(req, res, *robot_state_, *acm_);
126  ASSERT_FALSE(res.collision);
127 }
128 
130 TEST_F(CollisionDetectionEnvTest, LinksInCollision)
131 {
132  // Sets the joint values to zero which is a colliding configuration
133  robot_state_->setToDefaultValues();
134  robot_state_->update();
135 
136  collision_detection::CollisionRequest req;
137  collision_detection::CollisionResult res;
138  c_env_->checkSelfCollision(req, res, *robot_state_, *acm_);
139  ASSERT_TRUE(res.collision);
140 }
141 
143 TEST_F(CollisionDetectionEnvTest, RobotWorldCollision_1)
144 {
145  collision_detection::CollisionRequest req;
146  collision_detection::CollisionResult res;
147 
148  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
149  shapes::ShapeConstPtr shape_ptr(shape);
150 
151  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
152  pos1.translation().z() = 0.3;
153  c_env_->getWorld()->addToObject("box", shape_ptr, pos1);
154 
155  c_env_->checkSelfCollision(req, res, *robot_state_, *acm_);
156  ASSERT_FALSE(res.collision);
157  res.clear();
158 
159  c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
160  ASSERT_TRUE(res.collision);
161  res.clear();
162 
163  c_env_->getWorld()->moveObject("box", pos1);
164  c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
165  ASSERT_TRUE(res.collision);
166  res.clear();
167 
168  c_env_->getWorld()->moveObject("box", pos1);
169  ASSERT_FALSE(res.collision);
170 }
171 
173 TEST_F(CollisionDetectionEnvTest, RobotWorldCollision_2)
174 {
175  collision_detection::CollisionRequest req;
176  collision_detection::CollisionResult res;
177  req.max_contacts = 10;
178  req.contacts = true;
179  req.verbose = true;
180 
181  shapes::Shape* shape = new shapes::Box(.4, .4, .4);
182  shapes::ShapeConstPtr shape_ptr(shape);
183 
184  Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
185  pos1.translation().z() = 0.3;
186  c_env_->getWorld()->addToObject("box", shape_ptr, pos1);
187  c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
188  ASSERT_TRUE(res.collision);
189  ASSERT_GE(res.contact_count, 3u);
190  res.clear();
191 }
192 
194 TEST_F(CollisionDetectionEnvTest, PaddingTest)
195 {
196  collision_detection::CollisionRequest req;
197  req.contacts = true;
198  req.max_contacts = 10;
199  collision_detection::CollisionResult res;
200 
201  c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
202  ASSERT_FALSE(res.collision);
203  res.clear();
204 
205  // Adding the box right in front of the robot hand
206  shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1);
207  shapes::ShapeConstPtr shape_ptr(shape);
208 
209  Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
210  pos.translation().x() = 0.43;
211  pos.translation().y() = 0;
212  pos.translation().z() = 0.55;
213  c_env_->getWorld()->addToObject("box", shape_ptr, pos);
214 
215  c_env_->setLinkPadding("panda_hand", 0.08);
216  c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
217  ASSERT_TRUE(res.collision);
218  res.clear();
219 
220  c_env_->setLinkPadding("panda_hand", 0.0);
221  c_env_->checkRobotCollision(req, res, *robot_state_, *acm_);
222  ASSERT_FALSE(res.collision);
223 }
224 
228 TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionSelf)
229 {
230  collision_detection::CollisionRequest req;
231  collision_detection::CollisionResult res;
232 
233  moveit::core::RobotState state1(robot_model_);
234  moveit::core::RobotState state2(robot_model_);
235 
236  setToHome(state1);
237  double joint2 = 0.15;
238  double joint4 = -3.0;
239  double joint5 = -0.8;
240  double joint7 = -0.785;
241  state1.setJointPositions("panda_joint2", &joint2);
242  state1.setJointPositions("panda_joint4", &joint4);
243  state1.setJointPositions("panda_joint5", &joint5);
244  state1.setJointPositions("panda_joint7", &joint7);
245  state1.update();
246 
247  c_env_->checkSelfCollision(req, res, state1, *acm_);
248  ASSERT_FALSE(res.collision);
249  res.clear();
250 
251  setToHome(state2);
252  double joint_5_other = 0.8;
253  state2.setJointPositions("panda_joint2", &joint2);
254  state2.setJointPositions("panda_joint4", &joint4);
255  state2.setJointPositions("panda_joint5", &joint_5_other);
256  state2.setJointPositions("panda_joint7", &joint7);
257  state2.update();
258 
259  c_env_->checkSelfCollision(req, res, state2, *acm_);
260  ASSERT_FALSE(res.collision);
261  res.clear();
262 
263  // c_env_->checkSelfCollision(req, res, state1, state2, *acm_);
264  ROS_INFO_STREAM("Continous to continous collisions are not supported yet, therefore fail here.");
265  ASSERT_TRUE(res.collision);
266  res.clear();
267 }
268 
272 TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionWorld)
273 {
274  collision_detection::CollisionRequest req;
275  req.contacts = true;
276  req.max_contacts = 10;
277  collision_detection::CollisionResult res;
278 
279  moveit::core::RobotState state1(robot_model_);
280  moveit::core::RobotState state2(robot_model_);
281 
282  setToHome(state1);
283  state1.update();
284 
285  setToHome(state2);
286  double joint_2{ 0.05 };
287  double joint_4{ -1.6 };
288  state2.setJointPositions("panda_joint2", &joint_2);
289  state2.setJointPositions("panda_joint4", &joint_4);
290  state2.update();
291 
292  c_env_->checkRobotCollision(req, res, state1, state2, *acm_);
293  ASSERT_FALSE(res.collision);
294  res.clear();
295 
296  // Adding the box which is not in collision with the individual states but sits right between them.
297  shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1);
298  shapes::ShapeConstPtr shape_ptr(shape);
299 
300  Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
301  pos.translation().x() = 0.43;
302  pos.translation().y() = 0;
303  pos.translation().z() = 0.55;
304  c_env_->getWorld()->addToObject("box", shape_ptr, pos);
305 
306  c_env_->checkRobotCollision(req, res, state1, *acm_);
307  ASSERT_FALSE(res.collision);
308  res.clear();
309 
310  c_env_->checkRobotCollision(req, res, state2, *acm_);
311  ASSERT_FALSE(res.collision);
312  res.clear();
313 
314  c_env_->checkRobotCollision(req, res, state1, state2, *acm_);
315  ASSERT_TRUE(res.collision);
316  ASSERT_EQ(res.contact_count, 4u);
317  res.clear();
318 }
319 
320 int main(int argc, char** argv)
321 {
322  testing::InitGoogleTest(&argc, argv);
323  return RUN_ALL_TESTS();
324 }
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:581
collision_common.h
CollisionDetectionEnvTest
Definition: test_fcl_env.cpp:67
ros.h
shape_operations.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
shapes::Shape
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:179
robot_model.h
collision_common.h
CollisionDetectionEnvTest::acm_
collision_detection::AllowedCollisionMatrixPtr acm_
Definition: test_fcl_env.cpp:109
setToHome
void setToHome(moveit::core::RobotState &panda_state)
Brings the panda robot in user defined home position.
Definition: test_fcl_env.cpp:53
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:729
shapes::Box
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:382
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
collision_detection::CollisionRequest::verbose
bool verbose
Flag indicating whether information about detected collisions should be reported.
Definition: include/moveit/collision_detection/collision_common.h:225
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:434
TEST_F
TEST_F(CollisionDetectionEnvTest, InitOK)
Correct setup testing.
Definition: test_fcl_env.cpp:115
CollisionDetectionEnvTest::SetUp
void SetUp() override
Definition: test_fcl_env.cpp:70
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
CollisionDetectionEnvTest::robot_model_ok_
bool robot_model_ok_
Definition: test_fcl_env.cpp:103
CollisionDetectionEnvTest::c_env_
collision_detection::CollisionEnvPtr c_env_
Definition: test_fcl_env.cpp:107
robot_model_test_utils.h
collision_env_fcl.h
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
CollisionDetectionEnvTest::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: test_fcl_env.cpp:111
main
int main(int argc, char **argv)
Definition: test_fcl_env.cpp:320
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
CollisionDetectionEnvTest::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_fcl_env.cpp:105
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
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
CollisionDetectionEnvTest::TearDown
void TearDown() override
Definition: test_fcl_env.cpp:98
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 Sat May 3 2025 02:25:32