diff --git a/include/srdfdom/model.h b/include/srdfdom/model.h index 87e7b2f..6f99a7a 100644 --- a/include/srdfdom/model.h +++ b/include/srdfdom/model.h @@ -175,8 +175,8 @@ class Model std::vector spheres_; }; - /// The definition of a disabled collision between two links - struct DisabledCollision + /// The definition of a disabled/enabled collision between two links + struct CollisionPair { /// The name of the first link (as in URDF) of the disabled collision std::string link1_; @@ -184,7 +184,7 @@ class Model /// The name of the second link (as in URDF) of the disabled collision std::string link2_; - /// The reason why the collision check was disabled + /// The reason why the collision check was disabled/enabled std::string reason_; }; @@ -201,15 +201,23 @@ class Model return name_; } - /// Get the list of pairs of links that need not be checked for collisions (because they can never touch given the - /// geometry and kinematics of the robot) - const std::vector& getDisabledCollisionPairs() const + /// Get the list of links that should have collision checking disabled by default (and only selectively enabled) + const std::vector& getNoDefaultCollisionLinks() const { - return disabled_collisions_; + return no_default_collision_links_; } - /// \deprecated{ Use the version returning DisabledCollision } - [[deprecated]] std::vector > getDisabledCollisions() 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 disabled_collision_pairs_; + } /// Get the list of groups defined for this model const std::vector& getGroups() const @@ -256,7 +264,9 @@ class Model void loadGroupStates(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); void loadEndEffectors(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); void loadLinkSphereApproximations(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); - void loadDisabledCollisions(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, + std::vector& pairs); void loadPassiveJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml); std::string name_; @@ -265,7 +275,9 @@ class Model std::vector virtual_joints_; std::vector end_effectors_; std::vector link_sphere_approximations_; - std::vector disabled_collisions_; + std::vector no_default_collision_links_; + 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 c6465ea..b02711b 100644 --- a/include/srdfdom/srdf_writer.h +++ b/include/srdfdom/srdf_writer.h @@ -125,12 +125,19 @@ class SRDFWriter */ void createLinkSphereApproximationsXML(tinyxml2::XMLElement* root); + /** + * Generate XML for SRDF collision defaults + * + * @param root - TinyXML root element to attach sub elements to + */ + void createCollisionDefaultsXML(tinyxml2::XMLElement* root); + /** * Generate XML for SRDF disabled collisions of robot link pairs * * @param root - TinyXML root element to attach sub elements to */ - void createDisabledCollisionsXML(tinyxml2::XMLElement* root); + void createDisabledCollisionPairsXML(tinyxml2::XMLElement* root); /** * Generate XML for SRDF group states of each joint's position @@ -160,20 +167,32 @@ class SRDFWriter */ void createPassiveJointsXML(tinyxml2::XMLElement* root); +protected: + /** + * Generate XML for SRDF disabled/enabled collisions of robot link pairs + * + * @param root - TinyXML root element to attach sub elements to + */ + 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 disabled_collisions_; - std::vector passive_joints_; + 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 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 e53bd11..9b1b27f 100644 --- a/src/model.cpp +++ b/src/model.cpp @@ -532,35 +532,54 @@ void srdf::Model::loadLinkSphereApproximations(const urdf::ModelInterface& urdf_ } } -void srdf::Model::loadDisabledCollisions(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml) +void srdf::Model::loadCollisionDefaults(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml) { - for (XMLElement* c_xml = robot_xml->FirstChildElement("disable_collisions"); c_xml; - c_xml = c_xml->NextSiblingElement("disable_collisions")) + for (XMLElement* xml = robot_xml->FirstChildElement("disable_default_collisions"); xml; + xml = xml->NextSiblingElement("disable_default_collisions")) + { + const char* link_ = xml->Attribute("link"); + if (!link_) + { + CONSOLE_BRIDGE_logError("A disable_default_collisions tag needs to specify a link name"); + continue; + } + std::string link = boost::trim_copy(std::string(link_)); + if (!urdf_model.getLink(link)) + { + CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot specify collision default.", link_); + continue; + } + no_default_collision_links_.push_back(link); + } +} + +void srdf::Model::loadCollisionPairs(const urdf::ModelInterface& urdf_model, XMLElement* robot_xml, + 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)) { const char* link1 = c_xml->Attribute("link1"); const char* link2 = c_xml->Attribute("link2"); if (!link1 || !link2) { - CONSOLE_BRIDGE_logError("A pair of links needs to be specified to disable collisions"); + CONSOLE_BRIDGE_logError("A pair of links needs to be specified to disable/enable collisions"); continue; } - DisabledCollision dc; - dc.link1_ = boost::trim_copy(std::string(link1)); - dc.link2_ = boost::trim_copy(std::string(link2)); - if (!urdf_model.getLink(dc.link1_)) + const char* reason = c_xml->Attribute("reason"); + + CollisionPair pair{ boost::trim_copy(std::string(link1)), boost::trim_copy(std::string(link2)), + reason ? reason : "" }; + if (!urdf_model.getLink(pair.link1_)) { - CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link1); + CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable/enable collisons.", link1); continue; } - if (!urdf_model.getLink(dc.link2_)) + if (!urdf_model.getLink(pair.link2_)) { - CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable collisons.", link2); + CONSOLE_BRIDGE_logWarn("Link '%s' is not known to URDF. Cannot disable/enable collisons.", link2); continue; } - const char* reason = c_xml->Attribute("reason"); - if (reason) - dc.reason_ = std::string(reason); - disabled_collisions_.push_back(dc); + pairs.push_back(pair); } } @@ -619,7 +638,9 @@ bool srdf::Model::initXml(const urdf::ModelInterface& urdf_model, XMLElement* ro loadGroupStates(urdf_model, robot_xml); loadEndEffectors(urdf_model, robot_xml); loadLinkSphereApproximations(urdf_model, robot_xml); - loadDisabledCollisions(urdf_model, robot_xml); + loadCollisionDefaults(urdf_model, robot_xml); + 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; @@ -673,14 +694,8 @@ void srdf::Model::clear() virtual_joints_.clear(); end_effectors_.clear(); link_sphere_approximations_.clear(); - disabled_collisions_.clear(); + no_default_collision_links_.clear(); + enabled_collision_pairs_.clear(); + disabled_collision_pairs_.clear(); passive_joints_.clear(); } - -std::vector > srdf::Model::getDisabledCollisions() const -{ - std::vector > result; - for (std::size_t i = 0; i < disabled_collisions_.size(); ++i) - result.push_back(std::make_pair(disabled_collisions_[i].link1_, disabled_collisions_[i].link2_)); - return result; -} diff --git a/src/srdf_writer.cpp b/src/srdf_writer.cpp index c585a05..e907719 100644 --- a/src/srdf_writer.cpp +++ b/src/srdf_writer.cpp @@ -98,7 +98,9 @@ void SRDFWriter::initModel(const urdf::ModelInterface& robot_model, const srdf:: } // Copy all read-only data from srdf model to this object - disabled_collisions_ = srdf_model_->getDisabledCollisionPairs(); + no_default_collision_links_ = srdf_model_->getNoDefaultCollisionLinks(); + enabled_collision_pairs_ = srdf_model_->getEnabledCollisionPairs(); + disabled_collision_pairs_ = srdf_model_->getDisabledCollisionPairs(); link_sphere_approximations_ = srdf_model_->getLinkSphereApproximations(); groups_ = srdf_model_->getGroups(); virtual_joints_ = srdf_model_->getVirtualJoints(); @@ -194,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 - createDisabledCollisionsXML(robot_root); + createDisabledCollisionPairsXML(robot_root); } // ****************************************************************************************** @@ -315,34 +320,69 @@ void SRDFWriter::createLinkSphereApproximationsXML(XMLElement* root) } // ****************************************************************************************** -// Generate XML for SRDF disabled collisions of robot link pairs +// Generate XML for SRDF collision defaults of robot links // ****************************************************************************************** -void SRDFWriter::createDisabledCollisionsXML(XMLElement* root) +void SRDFWriter::createCollisionDefaultsXML(XMLElement* root) { XMLDocument* doc = root->GetDocument(); // Convenience comments - if (disabled_collisions_.size()) // only show comments if there are corresponding elements + if (!no_default_collision_links_.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 " + XMLComment* comment = doc->NewComment("DEFAULT 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. "); + "This tag allows to revert this behavior and disable collisions by default."); root->InsertEndChild(comment); } - for (std::vector::const_iterator pair_it = disabled_collisions_.begin(); - pair_it != disabled_collisions_.end(); ++pair_it) + for (const std::string& name : no_default_collision_links_) + { + XMLElement* entry = doc->NewElement("disable_default_collisions"); + entry->SetAttribute("link", name.c_str()); + root->InsertEndChild(entry); + } + // Write enabled collision pairs + 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, + const std::vector& pairs) +{ + XMLDocument* doc = root->GetDocument(); + + for (const srdf::Model::CollisionPair& pair : pairs) { // Create new element for each link pair - XMLElement* link_pair = doc->NewElement("disable_collisions"); - link_pair->SetAttribute("link1", pair_it->link1_.c_str()); - link_pair->SetAttribute("link2", pair_it->link2_.c_str()); - link_pair->SetAttribute("reason", pair_it->reason_.c_str()); + XMLElement* entry = doc->NewElement(tag_name); + entry->SetAttribute("link1", pair.link1_.c_str()); + entry->SetAttribute("link2", pair.link2_.c_str()); + entry->SetAttribute("reason", pair.reason_.c_str()); - root->InsertEndChild(link_pair); + root->InsertEndChild(entry); } } +// ****************************************************************************************** +// Generate XML for SRDF disabled collisions of robot link pairs +// ****************************************************************************************** +void SRDFWriter::createDisabledCollisionPairsXML(XMLElement* root) +{ + XMLDocument* doc = root->GetDocument(); + + // Convenience comments + 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_); +} + // ****************************************************************************************** // Generate XML for SRDF group states // ****************************************************************************************** diff --git a/test/test_parser.cpp b/test/test_parser.cpp index c55e0e2..045d1f5 100644 --- a/test/test_parser.cpp +++ b/test/test_parser.cpp @@ -94,6 +94,7 @@ TEST(TestCpp, testSimple) EXPECT_TRUE(s.getGroups().size() == 0); EXPECT_TRUE(s.getGroupStates().size() == 0); 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")); @@ -101,6 +102,7 @@ TEST(TestCpp, testSimple) EXPECT_TRUE(s.getGroups().size() == 1); EXPECT_TRUE(s.getGroupStates().size() == 0); 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")); @@ -108,6 +110,7 @@ TEST(TestCpp, testSimple) EXPECT_TRUE(s.getGroups().size() == 0); EXPECT_TRUE(s.getGroupStates().size() == 0); EXPECT_TRUE(s.getDisabledCollisionPairs().empty()); + EXPECT_TRUE(s.getEnabledCollisionPairs().empty()); EXPECT_TRUE(s.getEndEffectors().size() == 0); }