From 1c15e6c2fccb905b107b2812fcf51e7f1ad3bc7f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 22 Dec 2021 15:20:12 +0100 Subject: [PATCH] Use separate vectors for disabled/(re)enabled collision pairs --- include/srdfdom/model.h | 19 +++++++++++------- include/srdfdom/srdf_writer.h | 22 +++++++++++---------- src/model.cpp | 14 +++++++------- src/srdf_writer.cpp | 36 ++++++++++++++++++----------------- test/test_parser.cpp | 13 ++++++++----- 5 files changed, 58 insertions(+), 46 deletions(-) diff --git a/include/srdfdom/model.h b/include/srdfdom/model.h index bc66ec1..6f99a7a 100644 --- a/include/srdfdom/model.h +++ b/include/srdfdom/model.h @@ -186,8 +186,6 @@ class Model /// The reason why the collision check was disabled/enabled std::string reason_; - - bool disabled_; }; // Some joints can be passive (not actuated). This structure specifies information about such joints @@ -209,10 +207,16 @@ class Model return no_default_collision_links_; } - /// Get the list of pairs of links for which we explicitly disable/enable collision - const std::vector& getCollisionPairs() const + /// Get the list of pairs of links for which we explicitly re-enable collision (after having disabled it via a default) + const std::vector& getEnabledCollisionPairs() const + { + return enabled_collision_pairs_; + } + + /// Get the list of pairs of links for which we explicitly disable collision + const std::vector& getDisabledCollisionPairs() const { - return collision_pairs_; + return disabled_collision_pairs_; } /// Get the list of groups defined for this model @@ -262,7 +266,7 @@ class Model void loadLinkSphereApproximations(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); void loadCollisionDefaults(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); void loadCollisionPairs(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml, const char* tag_name, - bool disabled); + std::vector& pairs); void loadPassiveJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); std::string name_; @@ -272,7 +276,8 @@ class Model std::vector end_effectors_; std::vector link_sphere_approximations_; std::vector no_default_collision_links_; - std::vector collision_pairs_; + std::vector enabled_collision_pairs_; + std::vector disabled_collision_pairs_; std::vector passive_joints_; }; typedef std::shared_ptr ModelSharedPtr; diff --git a/include/srdfdom/srdf_writer.h b/include/srdfdom/srdf_writer.h index 4fd9a36..b02711b 100644 --- a/include/srdfdom/srdf_writer.h +++ b/include/srdfdom/srdf_writer.h @@ -137,7 +137,7 @@ class SRDFWriter * * @param root - TinyXML root element to attach sub elements to */ - void createCollisionPairsXML(tinyxml2::XMLElement* root); + void createDisabledCollisionPairsXML(tinyxml2::XMLElement* root); /** * Generate XML for SRDF group states of each joint's position @@ -173,24 +173,26 @@ class SRDFWriter * * @param root - TinyXML root element to attach sub elements to */ - void createCollisionPairsXML(tinyxml2::XMLElement* root, const char* tag_name, bool disabled); + void createCollisionPairsXML(tinyxml2::XMLElement* root, const char* tag_name, + const std::vector& pairs); public: // ****************************************************************************************** // Group Datastructures // ****************************************************************************************** - std::vector groups_; - std::vector group_states_; - std::vector virtual_joints_; - std::vector end_effectors_; - std::vector link_sphere_approximations_; + std::vector groups_; + std::vector group_states_; + std::vector virtual_joints_; + std::vector end_effectors_; + std::vector link_sphere_approximations_; std::vector no_default_collision_links_; - std::vector collision_pairs_; - std::vector passive_joints_; + std::vector disabled_collision_pairs_; + std::vector enabled_collision_pairs_; + std::vector passive_joints_; // Store the SRDF Model for updating the kinematic_model - srdf::ModelSharedPtr srdf_model_; + ModelSharedPtr srdf_model_; // Robot name std::string robot_name_; diff --git a/src/model.cpp b/src/model.cpp index 734c607..8ac14ac 100644 --- a/src/model.cpp +++ b/src/model.cpp @@ -37,7 +37,6 @@ #include "srdfdom/model.h" #include #include -#include #include #include #include @@ -555,7 +554,7 @@ void srdf::Model::loadCollisionDefaults(const urdf::ModelInterface& urdf_model, } void srdf::Model::loadCollisionPairs(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml, - const char* tag_name, bool disabled) + const char* tag_name, std::vector& pairs) { for (XMLElement* c_xml = robot_xml->FirstChildElement(tag_name); c_xml; c_xml = c_xml->NextSiblingElement(tag_name)) { @@ -569,7 +568,7 @@ void srdf::Model::loadCollisionPairs(const urdf::ModelInterface& urdf_model, XML const char* reason = c_xml->Attribute("reason"); CollisionPair pair{ boost::trim_copy(std::string(link1)), boost::trim_copy(std::string(link2)), - reason ? reason : "", disabled }; + reason ? reason : "" }; if (!urdf_model.getLink(pair.link1_)) { CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable/enable collisons.", link1); @@ -580,7 +579,7 @@ void srdf::Model::loadCollisionPairs(const urdf::ModelInterface& urdf_model, XML CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable/enable collisons.", link2); continue; } - collision_pairs_.push_back(pair); + pairs.push_back(pair); } } @@ -640,8 +639,8 @@ bool srdf::Model::initXml(const urdf::ModelInterface& urdf_model, XMLElement* ro loadEndEffectors(urdf_model, robot_xml); loadLinkSphereApproximations(urdf_model, robot_xml); loadCollisionDefaults(urdf_model, robot_xml); - loadCollisionPairs(urdf_model, robot_xml, "enable_collisions", false); - loadCollisionPairs(urdf_model, robot_xml, "disable_collisions", true); + loadCollisionPairs(urdf_model, robot_xml, "enable_collisions", enabled_collision_pairs_); + loadCollisionPairs(urdf_model, robot_xml, "disable_collisions", disabled_collision_pairs_); loadPassiveJoints(urdf_model, robot_xml); return true; @@ -696,6 +695,7 @@ void srdf::Model::clear() end_effectors_.clear(); link_sphere_approximations_.clear(); no_default_collision_links_.clear(); - collision_pairs_.clear(); + disabled_collision_pairs_.clear(); + enabled_collision_pairs_.clear(); passive_joints_.clear(); } diff --git a/src/srdf_writer.cpp b/src/srdf_writer.cpp index d75ab99..d47e7fc 100644 --- a/src/srdf_writer.cpp +++ b/src/srdf_writer.cpp @@ -99,7 +99,8 @@ void SRDFWriter::initModel(const urdf::ModelInterface& robot_model, const srdf:: // Copy all read-only data from srdf model to this object no_default_collision_links_ = srdf_model_->getNoDefaultCollisionLinks(); - collision_pairs_ = srdf_model_->getCollisionPairs(); + disabled_collision_pairs_ = srdf_model_->getDisabledCollisionPairs(); + enabled_collision_pairs_ = srdf_model_->getEnabledCollisionPairs(); link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations(); groups_ = srdf_model_->getGroups(); virtual_joints_ = srdf_model_->getVirtualJoints(); @@ -195,8 +196,11 @@ void SRDFWriter::generateSRDF(XMLDocument& document) // Add Link Sphere approximations createLinkSphereApproximationsXML(robot_root); + // Create disable_default_collisions tags and tags to re-enable specific pairs + createCollisionDefaultsXML(robot_root); + // Add Disabled Collisions - createCollisionPairsXML(robot_root); + createDisabledCollisionPairsXML(robot_root); } // ****************************************************************************************** @@ -338,21 +342,19 @@ void SRDFWriter::createCollisionDefaultsXML(XMLElement* root) root->InsertEndChild(entry); } // Write enabled collision pairs - createCollisionPairsXML(root, "enable_collisions", false); + createCollisionPairsXML(root, "enable_collisions", enabled_collision_pairs_); } // ****************************************************************************************** // Generate XML for SRDF disabled/enabled collisions of robot link pairs // ****************************************************************************************** -void SRDFWriter::createCollisionPairsXML(XMLElement* root, const char* tag_name, bool disabled) +void SRDFWriter::createCollisionPairsXML(XMLElement* root, const char* tag_name, + const std::vector& pairs) { XMLDocument* doc = root->GetDocument(); - for (const srdf::Model::CollisionPair& pair : collision_pairs_) + for (const srdf::Model::CollisionPair& pair : pairs) { - if (pair.disabled_ != disabled) - continue; - // Create new element for each link pair XMLElement* entry = doc->NewElement(tag_name); entry->SetAttribute("link1", pair.link1_.c_str()); @@ -366,19 +368,19 @@ void SRDFWriter::createCollisionPairsXML(XMLElement* root, const char* tag_name, // ****************************************************************************************** // Generate XML for SRDF disabled collisions of robot link pairs // ****************************************************************************************** -void SRDFWriter::createCollisionPairsXML(XMLElement* root) +void SRDFWriter::createDisabledCollisionPairsXML(XMLElement* root) { XMLDocument* doc = root->GetDocument(); // Convenience comments - if (!collision_pairs_.empty()) // only show comments if there are corresponding elements - { - XMLComment* comment = doc->NewComment("DISABLE COLLISIONS: By default it is assumed that any link of the robot " - "could potentially come into collision with any other link in the robot. " - "This tag disables collision checking between a specified pair of links. "); - root->InsertEndChild(comment); - } - createCollisionPairsXML(root, "disable_collisions", true); + if (disabled_collision_pairs_.empty()) + return; + + XMLComment* comment = doc->NewComment("DISABLE COLLISIONS: By default it is assumed that any link of the robot " + "could potentially come into collision with any other link in the robot. " + "This tag disables collision checking between a specified pair of links. "); + root->InsertEndChild(comment); + createCollisionPairsXML(root, "disable_collisions", disabled_collision_pairs_); } // ****************************************************************************************** diff --git a/test/test_parser.cpp b/test/test_parser.cpp index 8c2b04b..045d1f5 100644 --- a/test/test_parser.cpp +++ b/test/test_parser.cpp @@ -93,21 +93,24 @@ TEST(TestCpp, testSimple) EXPECT_TRUE(s.getVirtualJoints().size() == 0); EXPECT_TRUE(s.getGroups().size() == 0); EXPECT_TRUE(s.getGroupStates().size() == 0); - EXPECT_TRUE(s.getCollisionPairs().empty()); + EXPECT_TRUE(s.getDisabledCollisionPairs().empty()); + EXPECT_TRUE(s.getEnabledCollisionPairs().empty()); EXPECT_TRUE(s.getEndEffectors().size() == 0); EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.2.srdf")); EXPECT_TRUE(s.getVirtualJoints().size() == 1); EXPECT_TRUE(s.getGroups().size() == 1); EXPECT_TRUE(s.getGroupStates().size() == 0); - EXPECT_TRUE(s.getCollisionPairs().empty()); + EXPECT_TRUE(s.getDisabledCollisionPairs().empty()); + EXPECT_TRUE(s.getEnabledCollisionPairs().empty()); EXPECT_TRUE(s.getEndEffectors().size() == 0); EXPECT_TRUE(s.initFile(*u, std::string(TEST_RESOURCE_LOCATION) + "/pr2_desc.1.srdf")); EXPECT_TRUE(s.getVirtualJoints().size() == 0); EXPECT_TRUE(s.getGroups().size() == 0); EXPECT_TRUE(s.getGroupStates().size() == 0); - EXPECT_TRUE(s.getCollisionPairs().empty()); + EXPECT_TRUE(s.getDisabledCollisionPairs().empty()); + EXPECT_TRUE(s.getEnabledCollisionPairs().empty()); EXPECT_TRUE(s.getEndEffectors().size() == 0); } @@ -121,8 +124,8 @@ TEST(TestCpp, testComplex) EXPECT_TRUE(s.getVirtualJoints().size() == 1); EXPECT_TRUE(s.getGroups().size() == 7); EXPECT_TRUE(s.getGroupStates().size() == 2); - EXPECT_TRUE(s.getCollisionPairs().size() == 2); - EXPECT_TRUE(s.getCollisionPairs()[0].reason_ == "adjacent"); + EXPECT_TRUE(s.getDisabledCollisionPairs().size() == 2); + EXPECT_TRUE(s.getDisabledCollisionPairs()[0].reason_ == "adjacent"); EXPECT_TRUE(s.getEndEffectors().size() == 2); EXPECT_TRUE(s.getVirtualJoints()[0].name_ == "world_joint");