test_bullet_continuous_collision_checking.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 
43 
47 
50 
51 #include <urdf_parser/urdf_parser.h>
53 
54 namespace cb = collision_detection_bullet;
55 
57 inline void setToHome(moveit::core::RobotState& panda_state)
58 {
59  panda_state.setToDefaultValues();
60  double joint2 = -0.785;
61  double joint4 = -2.356;
62  double joint6 = 1.571;
63  double joint7 = 0.785;
64  panda_state.setJointPositions("panda_joint2", &joint2);
65  panda_state.setJointPositions("panda_joint4", &joint4);
66  panda_state.setJointPositions("panda_joint6", &joint6);
67  panda_state.setJointPositions("panda_joint7", &joint7);
68  panda_state.update();
69 }
70 
72 {
73 protected:
74  void SetUp() override
75  {
77  robot_model_ok_ = static_cast<bool>(robot_model_);
78 
79  acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*robot_model_->getSRDF());
80 
81  cenv_ = std::make_shared<collision_detection::CollisionEnvBullet>(robot_model_);
82 
83  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
84 
86  }
87 
88  void TearDown() override
89  {
90  }
91 
92 protected:
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 {
107  // Add static box to checker
109  shapes::ShapePtr static_box(new shapes::Box(1, 1, 1));
110  Eigen::Isometry3d static_box_pose;
111  static_box_pose.setIdentity();
112 
113  std::vector<shapes::ShapeConstPtr> obj1_shapes;
115  std::vector<cb::CollisionObjectType> obj1_types;
116  obj1_shapes.push_back(static_box);
117  obj1_poses.push_back(static_box_pose);
118  obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
119 
120  cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper(
121  "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types));
122  checker.addCollisionObject(cow_1);
123 
125  // Add moving box to checker
127  shapes::ShapePtr moving_box(new shapes::Box(0.2, 0.2, 0.2));
128  Eigen::Isometry3d moving_box_pose;
129 
130  moving_box_pose.setIdentity();
131  moving_box_pose.translation() = Eigen::Vector3d(0.5, -0.5, 0);
132 
133  std::vector<shapes::ShapeConstPtr> obj2_shapes;
135  std::vector<cb::CollisionObjectType> obj2_types;
136  obj2_shapes.push_back(moving_box);
137  obj2_poses.push_back(moving_box_pose);
138  obj2_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
139 
140  cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper(
141  "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types));
142  checker.addCollisionObject(cow_2);
143 }
144 
146 {
148  // Add static box to checker
150  shapes::ShapePtr static_box(new shapes::Box(0.3, 0.3, 0.3));
151  Eigen::Isometry3d static_box_pose;
152  static_box_pose.setIdentity();
153 
154  std::vector<shapes::ShapeConstPtr> obj1_shapes;
156  std::vector<cb::CollisionObjectType> obj1_types;
157  obj1_shapes.push_back(static_box);
158  obj1_poses.push_back(static_box_pose);
159  obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
160 
161  cb::CollisionObjectWrapperPtr cow_1(new cb::CollisionObjectWrapper(
162  "static_box_link", collision_detection::BodyType::WORLD_OBJECT, obj1_shapes, obj1_poses, obj1_types));
163  checker.addCollisionObject(cow_1);
165  // Add moving mesh to checker
167 
168  std::vector<shapes::ShapeConstPtr> obj2_shapes;
170  std::vector<cb::CollisionObjectType> obj2_types;
171 
172  obj1_poses.push_back(static_box_pose);
173  obj1_types.push_back(cb::CollisionObjectType::USE_SHAPE_TYPE);
174 
175  Eigen::Isometry3d s_pose;
176  s_pose.setIdentity();
177 
178  std::string kinect = "package://moveit_resources_panda_description/meshes/collision/hand.stl";
179  auto s = std::shared_ptr<shapes::Shape>{ shapes::createMeshFromResource(kinect) };
180  obj2_shapes.push_back(s);
181  obj2_types.push_back(cb::CollisionObjectType::CONVEX_HULL);
182  obj2_poses.push_back(s_pose);
183 
184  cb::CollisionObjectWrapperPtr cow_2(new cb::CollisionObjectWrapper(
185  "moving_box_link", collision_detection::BodyType::WORLD_OBJECT, obj2_shapes, obj2_poses, obj2_types));
186  checker.addCollisionObject(cow_2);
187 }
188 
190  std::vector<collision_detection::Contact>& result_vector, Eigen::Isometry3d& start_pos,
191  Eigen::Isometry3d& end_pos)
192 {
194  // Test when object is inside another
196  checker.setActiveCollisionObjects({ "moving_box_link" });
197  checker.setContactDistanceThreshold(0.1);
198 
199  // Set the collision object transforms
200  checker.setCollisionObjectsTransform("static_box_link", Eigen::Isometry3d::Identity());
201  checker.setCastCollisionObjectsTransform("moving_box_link", start_pos, end_pos);
202 
203  // Perform collision check
205  request.contacts = true;
206  // cb::ContactResultMap result;
207  checker.contactTest(result, request, nullptr, false);
208 
209  for (const auto& contacts_all : result.contacts)
210  {
211  for (const auto& contact : contacts_all.second)
212  {
213  result_vector.push_back(contact);
214  }
215  }
216 }
217 
218 // TODO(j-petit): Add continuous to continuous collision checking
220 TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf)
221 {
224 
225  moveit::core::RobotState state1(robot_model_);
226  moveit::core::RobotState state2(robot_model_);
227 
228  setToHome(state1);
229  double joint2 = 0.15;
230  double joint4 = -3.0;
231  double joint5 = -0.8;
232  double joint7 = -0.785;
233  state1.setJointPositions("panda_joint2", &joint2);
234  state1.setJointPositions("panda_joint4", &joint4);
235  state1.setJointPositions("panda_joint5", &joint5);
236  state1.setJointPositions("panda_joint7", &joint7);
237  state1.update();
238 
239  cenv_->checkSelfCollision(req, res, state1, *acm_);
240  ASSERT_FALSE(res.collision);
241  res.clear();
242 
243  setToHome(state2);
244  double joint_5_other = 0.8;
245  state2.setJointPositions("panda_joint2", &joint2);
246  state2.setJointPositions("panda_joint4", &joint4);
247  state2.setJointPositions("panda_joint5", &joint_5_other);
248  state2.setJointPositions("panda_joint7", &joint7);
249  state2.update();
250 
251  cenv_->checkSelfCollision(req, res, state2, *acm_);
252  ASSERT_FALSE(res.collision);
253  res.clear();
254 
255  ROS_INFO_STREAM("Continous to continous collisions are not supported yet, therefore fail here.");
256  ASSERT_TRUE(res.collision);
257  res.clear();
258 }
259 
261 TEST_F(BulletCollisionDetectionTester, ContinuousCollisionWorld)
262 {
264  req.contacts = true;
265  req.max_contacts = 10;
267 
268  moveit::core::RobotState state1(robot_model_);
269  moveit::core::RobotState state2(robot_model_);
270 
271  setToHome(state1);
272  state1.update();
273 
274  setToHome(state2);
275  double joint_2{ 0.05 };
276  double joint_4{ -1.6 };
277  state2.setJointPositions("panda_joint2", &joint_2);
278  state2.setJointPositions("panda_joint4", &joint_4);
279  state2.update();
280 
281  cenv_->checkRobotCollision(req, res, state1, state2, *acm_);
282  ASSERT_FALSE(res.collision);
283  res.clear();
284 
285  // Adding the box which is not in collision with the individual states but with the casted one.
286  shapes::Shape* shape = new shapes::Box(0.1, 0.1, 0.1);
287  shapes::ShapeConstPtr shape_ptr(shape);
288 
289  Eigen::Isometry3d pos{ Eigen::Isometry3d::Identity() };
290  pos.translation().x() = 0.43;
291  pos.translation().y() = 0;
292  pos.translation().z() = 0.55;
293  cenv_->getWorld()->addToObject("box", shape_ptr, pos);
294 
295  cenv_->checkRobotCollision(req, res, state1, *acm_);
296  ASSERT_FALSE(res.collision);
297  res.clear();
298 
299  cenv_->checkRobotCollision(req, res, state2, *acm_);
300  ASSERT_FALSE(res.collision);
301  res.clear();
302 
303  cenv_->checkRobotCollision(req, res, state1, state2, *acm_);
304  ASSERT_TRUE(res.collision);
305  ASSERT_EQ(res.contact_count, 4u);
306  // test contact types
307  for (auto& contact_pair : res.contacts)
308  {
309  for (collision_detection::Contact& contact : contact_pair.second)
310  {
311  collision_detection::BodyType contact_type1 = contact.body_name_1 == "box" ?
314  collision_detection::BodyType contact_type2 = contact.body_name_2 == "box" ?
317  ASSERT_EQ(contact.body_type_1, contact_type1);
318  ASSERT_EQ(contact.body_type_2, contact_type2);
319  }
320  }
321  res.clear();
322 }
323 
324 TEST(ContinuousCollisionUnit, BulletCastBVHCollisionBoxBoxUnit)
325 {
327  std::vector<collision_detection::Contact> result_vector;
328 
329  Eigen::Isometry3d start_pos, end_pos;
330  start_pos.setIdentity();
331  start_pos.translation().x() = -2;
332  end_pos.setIdentity();
333  end_pos.translation().x() = 2;
334 
335  cb::BulletCastBVHManager checker;
336  addCollisionObjects(checker);
337  runTest(checker, result, result_vector, start_pos, end_pos);
338 
339  ASSERT_TRUE(result.collision);
340  EXPECT_NEAR(result_vector[0].depth, -0.6, 0.001);
341  EXPECT_NEAR(result_vector[0].percent_interpolation, 0.6, 0.001);
342 }
343 
344 TEST(ContinuousCollisionUnit, BulletCastMeshVsBox)
345 {
346  cb::BulletCastBVHManager checker;
347  addCollisionObjectsMesh(checker);
348 
349  Eigen::Isometry3d start_pos, end_pos;
350  start_pos.setIdentity();
351  start_pos.translation().x() = -1.9;
352  end_pos.setIdentity();
353  end_pos.translation().x() = 1.9;
354 
356  std::vector<collision_detection::Contact> result_vector;
357 
358  runTest(checker, result, result_vector, start_pos, end_pos);
359 
360  ASSERT_TRUE(result.collision);
361 }
362 
363 int main(int argc, char** argv)
364 {
365  testing::InitGoogleTest(&argc, argv);
366  return RUN_ALL_TESTS();
367 }
BulletCollisionDetectionTester::TearDown
void TearDown() override
Definition: test_bullet_continuous_collision_checking.cpp:88
ASSERT_FALSE
#define ASSERT_FALSE(condition)
collision_detection_bullet
Definition: basic_types.h:34
collision_detection::Contact::body_type_1
BodyType body_type_1
The type of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:122
moveit::core::RobotState::setJointPositions
void setJointPositions(const std::string &joint_name, const double *position)
Definition: robot_state.h:581
collision_detection_bullet::BulletCastBVHManager::setCastCollisionObjectsTransform
void setCastCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose1, const Eigen::Isometry3d &pose2)
Set a single cast (moving) collision object's tansforms.
Definition: bullet_cast_bvh_manager.cpp:91
collision_detection_bullet::BulletBVHManager::setCollisionObjectsTransform
void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose)
Set a single static collision object's tansform.
Definition: bullet_bvh_manager.cpp:137
bullet_cast_bvh_manager.h
BulletCollisionDetectionTester::robot_state_
moveit::core::RobotStatePtr robot_state_
Definition: test_bullet_continuous_collision_checking.cpp:101
addCollisionObjectsMesh
void addCollisionObjectsMesh(cb::BulletCastBVHManager &checker)
Definition: test_bullet_continuous_collision_checking.cpp:145
collision_detection::BodyTypes::WORLD_OBJECT
@ WORLD_OBJECT
A body in the environment.
Definition: include/moveit/collision_detection/collision_common.h:97
basic_types.h
BulletCollisionDetectionTester
Definition: test_bullet_continuous_collision_checking.cpp:71
s
XmlRpcServer s
runTest
void runTest(cb::BulletCastBVHManager &checker, collision_detection::CollisionResult &result, std::vector< collision_detection::Contact > &result_vector, Eigen::Isometry3d &start_pos, Eigen::Isometry3d &end_pos)
Definition: test_bullet_continuous_collision_checking.cpp:189
ros.h
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
BulletCollisionDetectionTester::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_bullet_continuous_collision_checking.cpp:95
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::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
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
collision_detection::Contact::body_name_2
std::string body_name_2
The id of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:125
collision_detection::Contact::body_type_2
BodyType body_type_2
The type of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:128
collision_detection_bullet::BulletBVHManager::setActiveCollisionObjects
void setActiveCollisionObjects(const std::vector< std::string > &names)
Set which collision objects are active.
Definition: bullet_bvh_manager.cpp:153
collision_detection::Contact::body_name_1
std::string body_name_1
The id of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:119
collision_common.h
addCollisionObjects
void addCollisionObjects(cb::BulletCastBVHManager &checker)
Definition: test_bullet_continuous_collision_checking.cpp:104
shapes::ShapePtr
std::shared_ptr< Shape > ShapePtr
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:720
shapes::Box
BulletCollisionDetectionTester::robot_model_ok_
bool robot_model_ok_
Definition: test_bullet_continuous_collision_checking.cpp:93
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
setToHome
void setToHome(moveit::core::RobotState &panda_state)
Brings the panda robot in user defined home position.
Definition: test_bullet_continuous_collision_checking.cpp:57
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
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
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:425
BulletCollisionDetectionTester::acm_
collision_detection::AllowedCollisionMatrixPtr acm_
Definition: test_bullet_continuous_collision_checking.cpp:99
TEST
TEST(ContinuousCollisionUnit, BulletCastBVHCollisionBoxBoxUnit)
Definition: test_bullet_continuous_collision_checking.cpp:324
collision_detection_bullet::CollisionObjectWrapper
Tesseract bullet collision object.
Definition: bullet_utils.h:173
ASSERT_TRUE
#define ASSERT_TRUE(condition)
shapes::createMeshFromResource
Mesh * createMeshFromResource(const std::string &resource)
collision_detection_bullet::BulletCastBVHManager::addCollisionObject
void addCollisionObject(const CollisionObjectWrapperPtr &cow) override
Add a tesseract collision object to the manager.
Definition: bullet_cast_bvh_manager.cpp:177
BulletCollisionDetectionTester::SetUp
void SetUp() override
Definition: test_bullet_continuous_collision_checking.cpp:74
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
bullet_discrete_bvh_manager.h
collision_detection::BodyTypes::ROBOT_LINK
@ ROBOT_LINK
A link on the robot.
Definition: include/moveit/collision_detection/collision_common.h:91
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)
collision_detection::BodyTypes::Type
Type
The types of bodies that are considered for collision.
Definition: include/moveit/collision_detection/collision_common.h:88
robot_model_test_utils.h
TEST_F
TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf)
Continuous self collision checks are not supported yet by the bullet integration.
Definition: test_bullet_continuous_collision_checking.cpp:220
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
collision_detection_bullet::BulletCastBVHManager
A bounding volume hierarchy (BVH) implementation of a tesseract contact manager.
Definition: bullet_cast_bvh_manager.h:74
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
gtest.h
collision_detection_bullet::BulletCastBVHManager::contactTest
void contactTest(collision_detection::CollisionResult &collisions, const collision_detection::CollisionRequest &req, const collision_detection::AllowedCollisionMatrix *acm, bool self) override
Perform a contact test for all objects.
Definition: bullet_cast_bvh_manager.cpp:161
collision_detection_bullet::AlignedVector
std::vector< T, Eigen::aligned_allocator< T > > AlignedVector
Definition: basic_types.h:53
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
collision_detection::CollisionResult::contact_count
std::size_t contact_count
Number of contacts returned.
Definition: include/moveit/collision_detection/collision_common.h:206
collision_env_bullet.h
BulletCollisionDetectionTester::cenv_
collision_detection::CollisionEnvPtr cenv_
Definition: test_bullet_continuous_collision_checking.cpp:97
robot_state.h
main
int main(int argc, char **argv)
Definition: test_bullet_continuous_collision_checking.cpp:363
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:116
collision_detection_bullet::BulletBVHManager::setContactDistanceThreshold
void setContactDistanceThreshold(double contact_distance)
Set the contact distance threshold for which collision should be considered through expanding the AAB...
Definition: bullet_bvh_manager.cpp:174


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Jan 18 2022 03:23:45