Unified Collision Environment Bullet (#1572)

* Templated tests adapted for unified collision env

* Unified Bullet collision environment:
  * broadphase filtering adapted for early culling
  * ACM from SRDF in test
  * cleanup for bullet single collision env
  * removed link2castcow for CCD and only use constructor of COW directly
  * parent class for collision managers
  * removed function pointer for ACM check
  * removed extra self-collision manager and use only single manager
  * speed benchmark for unified environment

* PR review:
  * more descriptive variable names
  * added user to TODO

* PR review:
  * shortening namespace
  * documentation improvements
  * virtual destructor of BVH manager
  * remove extra speed benchmark
  * bugfix for not initialized managers

* Licenses revised of old tesseract files

* PR review:
  * replaced include guards through pragma
  * used default instead of empty {} for ctor/dtor

* Comments in Bullet readme about thread safety and speed

moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h

@@ -38,8 +38,7 @@
 #include <moveit/robot_state/robot_state.h>
 #include <moveit/utils/robot_model_test_utils.h>
 #include <moveit/collision_detection/collision_common.h>
-#include <moveit/collision_detection/collision_world.h>
-#include <moveit/collision_detection/collision_robot.h>
+#include <moveit/collision_detection/collision_env.h>
 #include <moveit/collision_detection/collision_detector_allocator.h>
 
 #include <urdf_parser/urdf_parser.h>
@@ -89,8 +88,7 @@ class CollisionDetectorPandaTest : public testing::Test
     for (const srdf::Model::DisabledCollision& it : dc)
       acm_->setEntry(it.link1_, it.link2_, true);
 
-    crobot_ = value_->allocateRobot(robot_model_);
-    cworld_ = value_->allocateWorld(collision_detection::WorldPtr(new collision_detection::World()));
+    cenv_ = value_->allocateEnv(robot_model_);
 
     robot_state_.reset(new robot_state::RobotState(robot_model_));
     setToHome(*robot_state_);
@@ -104,8 +102,7 @@ class CollisionDetectorPandaTest : public testing::Test
 
   robot_model::RobotModelPtr robot_model_;
 
-  collision_detection::CollisionWorldPtr cworld_;
-  collision_detection::CollisionRobotPtr crobot_;
+  collision_detection::CollisionEnvPtr cenv_;
 
   collision_detection::AllowedCollisionMatrixPtr acm_;
 
@@ -125,7 +122,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DefaultNotInCollision)
 {
   collision_detection::CollisionRequest req;
   collision_detection::CollisionResult res;
-  this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
+  this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_FALSE(res.collision);
 }
 
@@ -141,30 +138,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, LinksInCollision)
 
   collision_detection::CollisionRequest req;
   collision_detection::CollisionResult res;
-  this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
-  ASSERT_TRUE(res.collision);
-}
-
-// TODO: Add collision check capability within world itself and then enable test
-/** \brief Two boxes in collision in the world environment. */
-TYPED_TEST_P(CollisionDetectorPandaTest, DISABLED_WorldToWorldCollision)
-{
-  collision_detection::CollisionRequest req;
-  collision_detection::CollisionResult res;
-
-  shapes::Shape* shape = new shapes::Box(.5, .5, .5);
-  shapes::ShapeConstPtr shape_ptr(shape);
-
-  Eigen::Isometry3d pos_1 = Eigen::Isometry3d::Identity();
-  pos_1.translation().x() = 1;
-  this->cworld_->getWorld()->addToObject("box", shape_ptr, pos_1);
-
-  Eigen::Isometry3d pos_2 = Eigen::Isometry3d::Identity();
-  pos_2.translation().x() = 1.2;
-  this->cworld_->getWorld()->addToObject("box_2", shape_ptr, pos_2);
-
-  this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_);
-
+  this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_TRUE(res.collision);
 }
 
@@ -179,22 +153,22 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1)
 
   Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
   pos1.translation().z() = 0.3;
-  this->cworld_->getWorld()->addToObject("box", shape_ptr, pos1);
+  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos1);
 
-  this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
+  this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_FALSE(res.collision);
   res.clear();
 
-  this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_TRUE(res.collision);
   res.clear();
 
-  this->cworld_->getWorld()->moveObject("box", pos1);
-  this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_);
+  this->cenv_->getWorld()->moveObject("box", pos1);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_TRUE(res.collision);
   res.clear();
 
-  this->cworld_->getWorld()->moveObject("box", pos1);
+  this->cenv_->getWorld()->moveObject("box", pos1);
   ASSERT_FALSE(res.collision);
 }
 
@@ -212,8 +186,8 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_2)
 
   Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
   pos1.translation().z() = 0.3;
-  this->cworld_->getWorld()->addToObject("box", shape_ptr, pos1);
-  this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_);
+  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos1);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_TRUE(res.collision);
   ASSERT_GE(res.contact_count, 3u);
   res.clear();
@@ -227,7 +201,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest)
   req.max_contacts = 10;
   collision_detection::CollisionResult res;
 
-  this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_FALSE(res.collision);
   res.clear();
 
@@ -239,15 +213,15 @@ TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest)
   pos.translation().x() = 0.43;
   pos.translation().y() = 0;
   pos.translation().z() = 0.55;
-  this->cworld_->getWorld()->addToObject("box", shape_ptr, pos);
+  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos);
 
-  this->crobot_->setLinkPadding("panda_hand", 0.08);
-  this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_);
+  this->cenv_->setLinkPadding("panda_hand", 0.08);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_TRUE(res.collision);
   res.clear();
 
-  this->crobot_->setLinkPadding("panda_hand", 0.0);
-  this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_);
+  this->cenv_->setLinkPadding("panda_hand", 0.0);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_FALSE(res.collision);
 }
 
@@ -257,7 +231,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DistanceSelf)
   collision_detection::CollisionRequest req;
   req.distance = true;
   collision_detection::CollisionResult res;
-  this->crobot_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
+  this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_FALSE(res.collision);
   EXPECT_NEAR(res.distance, 0.13, 0.01);
 }
@@ -276,14 +250,13 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld)
   pos.translation().x() = 0.43;
   pos.translation().y() = 0;
   pos.translation().z() = 0.55;
-  this->cworld_->getWorld()->addToObject("box", shape_ptr, pos);
+  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos);
 
-  this->crobot_->setLinkPadding("panda_hand", 0.0);
-  this->cworld_->checkRobotCollision(req, res, *this->crobot_, *this->robot_state_, *this->acm_);
+  this->cenv_->setLinkPadding("panda_hand", 0.0);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_FALSE(res.collision);
   EXPECT_NEAR(res.distance, 0.029, 0.01);
 }
 
 REGISTER_TYPED_TEST_CASE_P(CollisionDetectorPandaTest, InitOK, DefaultNotInCollision, LinksInCollision,
-                           DISABLED_WorldToWorldCollision, RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest,
-                           DistanceSelf, DistanceWorld);
+                           RobotWorldCollision_1, RobotWorldCollision_2, PaddingTest, DistanceSelf, DistanceWorld);

moveit_core/collision_detection_bullet/CMakeLists.txt

@@ -1,11 +1,11 @@
 set(MOVEIT_LIB_NAME moveit_collision_detection_bullet)
 
 add_library(${MOVEIT_LIB_NAME}
-  src/collision_robot_bullet.cpp
-  src/collision_world_bullet.cpp
   src/bullet_integration/bullet_utils.cpp
   src/bullet_integration/bullet_discrete_bvh_manager.cpp
   src/bullet_integration/bullet_cast_bvh_manager.cpp
+  src/collision_env_bullet.cpp
+  src/bullet_integration/bullet_bvh_manager.cpp
 )
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 

moveit_core/collision_detection_bullet/README.md

@@ -1,6 +1,8 @@
 ### Using Bullet as a collision checker [experimental]
-To use Bullet as a collision checker you can:
-a) create a new planning scene and manually switch to Bullet using the planning_scene.setActiveCollisionDetector()
-b) manually switch to Bullet using the `CollisionPluginLoader`
-c) manually instantiate a `CollisionRobotBullet` and `CollisionWorldBullet`
-d) use rosparam to set the collision checker (preferred but not tested yet) when starting a new `move_group`
+To use Bullet as a collision checker you can manually switch to Bullet using the planning_scene.setActiveCollisionDetector()
+
+### Speed Benchmarks
+For speed comparisons to FCL please see (relative link to README of benchmark script will be added).
+
+### Current Limitations
+The collision detection using Bullet is not thread safe as the internal collision managers are `mutable` members of the collision environment.

moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h

@@ -18,8 +18,7 @@
 
 /* Author: Levi Armstrong */
 
-#ifndef MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_
-#define MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_
+#pragma once
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
@@ -44,12 +43,6 @@ template <typename Key, typename Value>
 using AlignedUnorderedMap = std::unordered_map<Key, Value, std::hash<Key>, std::equal_to<Key>,
                                                Eigen::aligned_allocator<std::pair<const Key, Value>>>;
 
-/** @brief Checks if contact is allowed or not. Returns true if yes, otherwise false.
- *
- * The order of strings should not matter, the function should handled by the function. */
-typedef std::function<bool(const std::string&, const std::string&, const collision_detection::AllowedCollisionMatrix*)>
-    IsContactAllowedFn;
-
 enum class CollisionObjectType
 {
   USE_SHAPE_TYPE = 0, /**< @brief Infer the type from the type specified in the shapes::Shape class */
@@ -66,17 +59,9 @@ struct ContactTestData
 {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  ContactTestData(const std::vector<std::string>& active, const double& contact_distance, const IsContactAllowedFn& fn,
-                  collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req,
-                  const collision_detection::AllowedCollisionMatrix* acm)
-    : active(active)
-    , contact_distance(contact_distance)
-    , fn(fn)
-    , acm(acm)
-    , res(res)
-    , req(req)
-    , done(false)
-    , pair_done(false)
+  ContactTestData(const std::vector<std::string>& active, const double& contact_distance,
+                  collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req)
+    : active(active), contact_distance(contact_distance), res(res), req(req), done(false), pair_done(false)
   {
   }
 
@@ -85,12 +70,6 @@ struct ContactTestData
   /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */
   const double& contact_distance;
 
-  /** \brief User defined function which checks if contact is allowed between two objects */
-  const IsContactAllowedFn& fn;
-
-  /** \brief Indicates collision objects which are allowed to be in contact */
-  const collision_detection::AllowedCollisionMatrix* acm;
-
   collision_detection::CollisionResult& res;
   const collision_detection::CollisionRequest& req;
 
@@ -102,5 +81,3 @@ struct ContactTestData
 };
 
 }  // namespace collision_detection_bullet
-
-#endif  // MOVEIT_COLLISION_DETECTION_BULLET_BULLET_INTEGRATION_BASIC_TYPES_H_