Skip to content

Commit

Permalink
Use separate vectors for disabled/(re)enabled collision pairs
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhaschke
rhaschke committed Dec 23, 2021
1 parent f6c7ce0 commit 1c15e6c
Show file tree
Hide file tree
Showing 5 changed files with 58 additions and 46 deletions.
19 changes: 12 additions & 7 deletions include/srdfdom/model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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<CollisionPair>& 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<CollisionPair>& getEnabledCollisionPairs() const
{
return enabled_collision_pairs_;
}

/// Get the list of pairs of links for which we explicitly disable collision
const std::vector<CollisionPair>& getDisabledCollisionPairs() const
{
return collision_pairs_;
return disabled_collision_pairs_;
}

/// Get the list of groups defined for this model
Expand Down Expand Up @@ -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<CollisionPair>& pairs);
void loadPassiveJoints(const urdf::ModelInterface& urdf_model, tinyxml2::XMLElement* robot_xml);

std::string name_;
Expand All @@ -272,7 +276,8 @@ class Model
std::vector<EndEffector> end_effectors_;
std::vector<LinkSpheres> link_sphere_approximations_;
std::vector<std::string> no_default_collision_links_;
std::vector<CollisionPair> collision_pairs_;
std::vector<CollisionPair> enabled_collision_pairs_;
std::vector<CollisionPair> disabled_collision_pairs_;
std::vector<PassiveJoint> passive_joints_;
};
typedef std::shared_ptr<Model> ModelSharedPtr;
Expand Down
22 changes: 12 additions & 10 deletions include/srdfdom/srdf_writer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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<Model::CollisionPair>& pairs);

public:
// ******************************************************************************************
// Group Datastructures
// ******************************************************************************************

std::vector<srdf::Model::Group> groups_;
std::vector<srdf::Model::GroupState> group_states_;
std::vector<srdf::Model::VirtualJoint> virtual_joints_;
std::vector<srdf::Model::EndEffector> end_effectors_;
std::vector<srdf::Model::LinkSpheres> link_sphere_approximations_;
std::vector<Model::Group> groups_;
std::vector<Model::GroupState> group_states_;
std::vector<Model::VirtualJoint> virtual_joints_;
std::vector<Model::EndEffector> end_effectors_;
std::vector<Model::LinkSpheres> link_sphere_approximations_;
std::vector<std::string> no_default_collision_links_;
std::vector<srdf::Model::CollisionPair> collision_pairs_;
std::vector<srdf::Model::PassiveJoint> passive_joints_;
std::vector<Model::CollisionPair> disabled_collision_pairs_;
std::vector<Model::CollisionPair> enabled_collision_pairs_;
std::vector<Model::PassiveJoint> passive_joints_;

// Store the SRDF Model for updating the kinematic_model
srdf::ModelSharedPtr srdf_model_;
ModelSharedPtr srdf_model_;

// Robot name
std::string robot_name_;
Expand Down
14 changes: 7 additions & 7 deletions src/model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@
#include "srdfdom/model.h"
#include <console_bridge/console.h>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/case_conv.hpp>
#include <algorithm>
#include <fstream>
#include <sstream>
Expand Down Expand Up @@ -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<CollisionPair>& pairs)
{
for (XMLElement* c_xml = robot_xml->FirstChildElement(tag_name); c_xml; c_xml = c_xml->NextSiblingElement(tag_name))
{
Expand All @@ -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);
Expand All @@ -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);
}
}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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();
}
36 changes: 19 additions & 17 deletions src/srdf_writer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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);
}

// ******************************************************************************************
Expand Down Expand Up @@ -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<Model::CollisionPair>& 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());
Expand All @@ -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_);
}

// ******************************************************************************************
Expand Down
13 changes: 8 additions & 5 deletions test/test_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand All @@ -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");
Expand Down

0 comments on commit 1c15e6c

Please sign in to comment.