diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h index 8c280ee248e..550ecef5448 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h @@ -88,6 +88,9 @@ class AllowedCollisionMatrix * will be ignored. */ AllowedCollisionMatrix(const std::vector& names, bool allowed = false); + /** @brief Construct from an SRDF representation */ + AllowedCollisionMatrix(const srdf::Model& srdf); + /** @brief Construct the structure from a message representation */ AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg); diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 9b0bf7097ff..ac2c2ebf458 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -78,14 +78,7 @@ class CollisionDetectorPandaTest : public testing::Test robot_model_ = moveit::core::loadTestingRobotModel("panda"); robot_model_ok_ = static_cast(robot_model_); - acm_ = std::make_shared(); - - // load collision defaults - for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks()) - acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER); - // allow collisions for pairs that have been disabled - for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_); + acm_ = std::make_shared(*robot_model_->getSRDF()); cenv_ = value_->allocateEnv(robot_model_); diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index e5a0d390714..e0164e6708b 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -51,6 +51,19 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector& n setEntry(names[i], names[j], allowed); } +AllowedCollisionMatrix::AllowedCollisionMatrix(const srdf::Model& srdf) +{ + // load collision defaults + for (const std::string& name : srdf.getNoDefaultCollisionLinks()) + setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); + // re-enable specific collision pairs + for (auto const& collision : srdf.getEnabledCollisionPairs()) + setEntry(collision.link1_, collision.link2_, false); + // *finally* disable selected collision pairs + for (auto const& collision : srdf.getDisabledCollisionPairs()) + setEntry(collision.link1_, collision.link2_, true); +} + AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg) { if (msg.entry_names.size() != msg.entry_values.size() || diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 06c6300f148..ffda13ee807 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -76,14 +76,7 @@ class BulletCollisionDetectionTester : public testing::Test robot_model_ = moveit::core::loadTestingRobotModel("panda"); robot_model_ok_ = static_cast(robot_model_); - acm_ = std::make_shared(); - - // load collision defaults - for (const std::string& name : robot_model_->getSRDF()->getNoDefaultCollisionLinks()) - acm_->setDefaultEntry(name, collision_detection::AllowedCollision::NEVER); - // allow collisions for pairs that have been disabled - for (auto const& collision : robot_model_->getSRDF()->getCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, collision.disabled_); + acm_ = std::make_shared(*robot_model_->getSRDF()); cenv_ = std::make_shared(robot_model_); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 5686241a2b5..2dd7a101866 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -151,16 +151,7 @@ void PlanningScene::initialize() robot_state_->setToDefaultValues(); robot_state_->update(); - acm_ = std::make_shared(); - - // load collision defaults - for (const std::string& name : getRobotModel()->getSRDF()->getNoDefaultCollisionLinks()) - acm_->setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); - // load collision pairs - for (auto const& collision : getRobotModel()->getSRDF()->getEnabledCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, false); - for (auto const& collision : getRobotModel()->getSRDF()->getDisabledCollisionPairs()) - acm_->setEntry(collision.link1_, collision.link2_, true); + acm_ = std::make_shared(*getRobotModel()->getSRDF()); setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } diff --git a/moveit_setup_assistant/src/tools/moveit_config_data.cpp b/moveit_setup_assistant/src/tools/moveit_config_data.cpp index 084ecabbdea..59d2532aa6f 100644 --- a/moveit_setup_assistant/src/tools/moveit_config_data.cpp +++ b/moveit_setup_assistant/src/tools/moveit_config_data.cpp @@ -140,14 +140,17 @@ planning_scene::PlanningScenePtr MoveItConfigData::getPlanningScene() // ****************************************************************************************** void MoveItConfigData::loadAllowedCollisionMatrix() { - // Clear the allowed collision matrix allowed_collision_matrix_.clear(); - // Update the allowed collision matrix, in case there has been a change - for (const auto& collision : srdf_->collision_pairs_) - { - allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, collision.disabled_); - } + // load collision defaults + for (const std::string& name : srdf_->no_default_collision_links_) + allowed_collision_matrix_.setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); + // re-enable specific collision pairs + for (auto const& collision : srdf_->enabled_collision_pairs_) + allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, false); + // *finally* disable selected collision pairs + for (auto const& collision : srdf_->disabled_collision_pairs_) + allowed_collision_matrix_.setEntry(collision.link1_, collision.link2_, true); } // ******************************************************************************************