diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
index 0dec1f9d4dcd734e108b2b88d21c1566a9eb6ada..3d18e8cfc2584a77a22bdb50a49554f3eb11085f 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
@@ -257,13 +257,28 @@ void MjcfConverter::addContactExcludes()
{
RobotNodePtr rootNode = robot->getRootNode();
+ std::vector<std::pair<std::string, std::string>> excludePairs;
+
for (RobotNodePtr node : robot->getRobotNodes())
{
for (std::string& ignore : node->getPhysics().ignoreCollisions)
{
-
+ // I found an <IgnoreCollision> element referring to a non-existing node.
+ // => check node existence here
+ if (robot->hasRobotNode(ignore))
+ {
+ excludePairs.push_back({node->getName(), ignore});
+ }
}
}
+
+ // resolve body names and add exludes
+ for (auto& excludePair : excludePairs)
+ {
+ std::string body1 = masslessBodySanitizer.getMergedBodyName(excludePair.first);
+ std::string body2 = masslessBodySanitizer.getMergedBodyName(excludePair.second);
+ document->addContactExclude(body1, body2);
+ }
}
void MjcfConverter::addActuators()
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp b/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
index 31ec809d2225087f3a261cb08a15262c40c1e960..4bab25adf657476a5a91d5aa8db30d37580907a4 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
+++ b/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
@@ -229,19 +229,17 @@ void Document::setJointAxis(Element* joint, const Eigen::Vector3f& axis)
joint->SetAttribute("axis", toAttr(axis).c_str());
}
-
-void Document::addContactExcludes(Element* rootBody)
+Element*Document::addContactExclude(const Element& body1, const Element& body2)
{
- ContactExcludeVisitor visitor(*this);
- rootBody->Accept(&visitor);
+ return addContactExclude(body1.Attribute("name"), body2.Attribute("name"));
}
-Element* Document::addContactExclude(const Element& body1, const Element& body2)
+Element* Document::addContactExclude(const std::string& body1Name, const std::string& body2Name)
{
Element* exclude = addNewElement(contact(), "exclude");
- exclude->SetAttribute("body1", body1.Attribute("name"));
- exclude->SetAttribute("body2", body2.Attribute("name"));
+ exclude->SetAttribute("body1", body1Name.c_str());
+ exclude->SetAttribute("body2", body2Name.c_str());
return exclude;
}
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfDocument.h b/VirtualRobot/examples/MjcfConverter/MjcfDocument.h
index 26a4616fbb42dece494d39b7b40f1a0c41b3e9de..123ff373b4791ca26ec1d41724225da16b633969 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfDocument.h
+++ b/VirtualRobot/examples/MjcfConverter/MjcfDocument.h
@@ -74,11 +74,10 @@ namespace mjcf
/// Set the axis attribute of a joint.
void setJointAxis(Element* joint, const Eigen::Vector3f& axis);
- /// Add contact/exclude elements between parent and child bodies,
- /// starting from the given body.
- void addContactExcludes(Element* rootBody);
/// Add a conact/exclude element between the given bodies.
Element* addContactExclude(const Element& body1, const Element& body2);
+ /// Add a conact/exclude element between the given bodies.
+ Element* addContactExclude(const std::string& body1Name, const std::string& body2Name);
private:
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.cpp b/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.cpp
index 61e01972786628095450b8d38bd28e1b6d290086..a7d6c14b161800ad65bddd009e98491ff1d1fd5b 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.cpp
+++ b/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.cpp
@@ -31,6 +31,7 @@ void MjcfMasslessBodySanitizer::sanitize()
}
}
+
void MjcfMasslessBodySanitizer::sanitizeRecursion(Element* body)
{
assertElementIsBody(body);
@@ -110,7 +111,7 @@ void MjcfMasslessBodySanitizer::sanitizeRecursion(Element* body)
}
// update body name
- MergedBodySet bodySet = getMergedBodySetWith(bodyName);
+ MergedBodySet& bodySet = getMergedBodySetWith(bodyName);
bodySet.addBody(childBodyName);
body->SetAttribute("name", bodySet.getMergedBodyName().c_str());
@@ -121,7 +122,7 @@ void MjcfMasslessBodySanitizer::sanitizeRecursion(Element* body)
}
else
{
- VR_WARNING << t << "Massless body with >1 child body: "
+ std::cout << "[WARN]" << t << "Massless body with >1 child body: "
<< body->Attribute("name") << std::endl;
break;
}
@@ -158,7 +159,17 @@ void MjcfMasslessBodySanitizer::sanitizeLeafBody(Element* body)
}
}
-MergedBodySet& MjcfMasslessBodySanitizer::getMergedBodySetWith(const std::string& bodyName)
+const std::vector<MergedBodySet>& MjcfMasslessBodySanitizer::getMergedBodySets() const
+{
+ return mergedBodySets;
+}
+
+const std::string& MjcfMasslessBodySanitizer::getMergedBodyName(const std::string& originalBodyName)
+{
+ return getMergedBodySetWith(originalBodyName).getMergedBodyName();
+}
+
+MergedBodySet&MjcfMasslessBodySanitizer::getMergedBodySetWith(const std::string& bodyName)
{
for (auto& set : mergedBodySets)
{
@@ -169,16 +180,19 @@ MergedBodySet& MjcfMasslessBodySanitizer::getMergedBodySetWith(const std::string
}
// not found => add
- MergedBodySet newSet;
- newSet.addBody(bodyName);
- mergedBodySets.push_back(newSet);
+ mergedBodySets.push_back(MergedBodySet(bodyName));
return mergedBodySets.back();
}
-const std::vector<MergedBodySet>& MjcfMasslessBodySanitizer::getMergedBodySets() const
+
+MergedBodySet::MergedBodySet()
{
- return mergedBodySets;
+}
+
+MergedBodySet::MergedBodySet(const std::string& bodyName)
+{
+ addBody(bodyName);
}
void MergedBodySet::addBody(const std::string& bodyName)
@@ -199,5 +213,5 @@ const std::string& MergedBodySet::getMergedBodyName() const
void MergedBodySet::updateMergedBodyName()
{
- this->mergedBodyName = boost::algorithm::join(originalBodyNames, "~");
+ mergedBodyName = boost::algorithm::join(originalBodyNames, "~");
}
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.h b/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.h
index 14a2087a7e23219f562b5812534adaad6f85ee2d..65b1f0988933a7b689600872ab063f236fc9d852 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.h
+++ b/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.h
@@ -12,6 +12,9 @@ namespace mjcf
{
public:
+ MergedBodySet();
+ MergedBodySet(const std::string& bodyName);
+
void addBody(const std::string& bodyName);
bool containsBody(const std::string& bodyName) const;
@@ -26,7 +29,7 @@ namespace mjcf
std::set<std::string> originalBodyNames;
};
-
+
class MjcfMasslessBodySanitizer
{
@@ -38,13 +41,17 @@ namespace mjcf
const std::vector<MergedBodySet>& getMergedBodySets() const;
+ const std::string& getMergedBodyName(const std::string& originalBodyName);
+
+ MergedBodySet& getMergedBodySetWith(const std::string& bodyName);
+
private:
void sanitizeRecursion(mjcf::Element* body);
void sanitizeLeafBody(mjcf::Element* body);
- MergedBodySet& getMergedBodySetWith(const std::string& bodyName);
+
DocumentPtr& document;
diff --git a/VirtualRobot/examples/MjcfConverter/xml_visitors.cpp b/VirtualRobot/examples/MjcfConverter/xml_visitors.cpp
index 7c610986303c1b14d9fc080ad09c052e6d23e4c2..e99f3e6cf5f21456986c84f6644084ff16efb012 100644
--- a/VirtualRobot/examples/MjcfConverter/xml_visitors.cpp
+++ b/VirtualRobot/examples/MjcfConverter/xml_visitors.cpp
@@ -4,33 +4,6 @@
using namespace VirtualRobot::mjcf;
-ContactExcludeVisitor::ContactExcludeVisitor(Document& document) :
- document(document)
-{
-}
-
-bool ContactExcludeVisitor::VisitEnter(
- const tinyxml2::XMLElement& body, const tinyxml2::XMLAttribute*)
-{
- if (!isElement(body, "body"))
- {
- return true;
- }
-
- if (!firstSkipped)
- {
- firstSkipped = true;
- return true;
- }
-
- const Element* parent = body.Parent()->ToElement();
- assert(parent);
-
- document.addContactExclude(*parent, body);
-
- return true;
-}
-
ListElementsVisitor::ListElementsVisitor(const std::string& elemName) :
elementName(elemName)
diff --git a/VirtualRobot/examples/MjcfConverter/xml_visitors.h b/VirtualRobot/examples/MjcfConverter/xml_visitors.h
index 1d29ac923c06f362caaca8d4fea4514faf96292e..31c01ab81477cc21d86b9f4a929642341508fe22 100644
--- a/VirtualRobot/examples/MjcfConverter/xml_visitors.h
+++ b/VirtualRobot/examples/MjcfConverter/xml_visitors.h
@@ -8,24 +8,6 @@ namespace VirtualRobot
namespace mjcf
{
-class ContactExcludeVisitor : public tinyxml2::XMLVisitor
-{
-public:
-
- ContactExcludeVisitor(Document& document);
- virtual ~ContactExcludeVisitor() override {}
-
- // XMLVisitor interface
- virtual bool VisitEnter(const tinyxml2::XMLElement&, const tinyxml2::XMLAttribute*) override;
-
-
-private:
-
- Document& document; ///< The document.
- bool firstSkipped = false; ///< Used to skip the root element.
-
-};
-
class ListElementsVisitor : public tinyxml2::XMLVisitor
{