From b677917351e8f03acce648f0f735d954f71412d0 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Thu, 8 Nov 2018 13:33:35 +0100
Subject: [PATCH] Contact excludes
---
.../examples/MjcfConverter/MjcfConverter.cpp | 7 +--
.../examples/MjcfConverter/MjcfConverter.h | 2 +-
.../examples/MjcfConverter/MjcfDocument.cpp | 45 +++++++++++++++++++
.../examples/MjcfConverter/MjcfDocument.h | 25 +++++++++--
4 files changed, 70 insertions(+), 9 deletions(-)
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
index c398514f0..7ee32e44a 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
@@ -121,6 +121,8 @@ void MjcfConverter::convertToMjcf()
std::cout << "Merging empty bodies ..." << std::endl;
sanitizeMasslessBodies();
+ std::cout << "Adding contact excludes ..." << std::endl;
+ document->addContactExcludes(nodeBodies[robot->getRootNode()->getName()]);
std::cout << "Done.";
@@ -431,8 +433,3 @@ void MjcfConverter::sanitizeMasslessLeafBody(mjcf::Element* body)
-
-
-
-
-
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
index ab4393e44..68614c132 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
@@ -48,7 +48,7 @@ namespace VirtualRobot
void sanitizeMasslessLeafBody(mjcf::Element* body);
-
+ void addContactExcludes();
// Paths
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp b/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
index 76db3a5b8..9a2c93584 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
+++ b/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
@@ -219,6 +219,23 @@ void Document::setJointAxis(Element* joint, const Eigen::Vector3f& axis)
}
+void Document::addContactExcludes(Element* rootBody)
+{
+ ContactExcludeVisitor visitor(*this);
+ rootBody->Accept(&visitor);
+}
+
+Element* Document::addContactExclude(const Element& body1, const Element& body2)
+{
+ Element* exclude = addNewElement(contact(), "exclude");
+
+ exclude->SetAttribute("body1", body1.Attribute("name"));
+ exclude->SetAttribute("body2", body2.Attribute("name"));
+
+ return exclude;
+}
+
+
Element* Document::topLevelElement(const std::string& name)
{
Element* elem = root->FirstChildElement(name.c_str());
@@ -244,3 +261,31 @@ std::string Document::toAttr(const Eigen::Quaternionf& quat)
}
+
+ContactExcludeVisitor::ContactExcludeVisitor(Document& document) :
+ document(document)
+{
+}
+
+bool ContactExcludeVisitor::VisitEnter(
+ const tinyxml2::XMLElement& body, const tinyxml2::XMLAttribute*)
+{
+ if (std::string(body.Value()) != "body")
+ {
+ return true;
+ }
+
+ if (!firstSkipped)
+ {
+ firstSkipped = true;
+ return true;
+ }
+
+ const Element* parent = body.Parent()->ToElement();
+ assert(parent);
+
+ document.addContactExclude(*parent, body);
+
+ return true;
+}
+
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfDocument.h b/VirtualRobot/examples/MjcfConverter/MjcfDocument.h
index c2691e98a..7d9636b1f 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfDocument.h
+++ b/VirtualRobot/examples/MjcfConverter/MjcfDocument.h
@@ -59,6 +59,9 @@ namespace mjcf
void setJointAxis(Element* joint, const Eigen::Vector3f& axis);
+ void addContactExcludes(Element* rootBody);
+ Element* addContactExclude(const Element& body1, const Element& body2);
+
private:
/// Gets the top-level element (child of root element) with the given
@@ -81,12 +84,12 @@ namespace mjcf
Eigen::IOFormat iofVector {5, 0, "", " ", "", "", "", ""};
-
-
-
/// The "mujoco" root element.
Element* root;
+
+ friend class ContactExcludeVisitor;
+
};
using DocumentPtr = std::unique_ptr<Document>;
@@ -100,5 +103,21 @@ namespace mjcf
return ss.str();
}
+
+ class ContactExcludeVisitor : public tinyxml2::XMLVisitor
+ {
+ public:
+ ContactExcludeVisitor(Document& document);
+
+ // XMLVisitor interface
+ virtual bool VisitEnter(const tinyxml2::XMLElement&, const tinyxml2::XMLAttribute*) override;
+
+ private:
+ Document& document;
+ bool firstSkipped = false; ///< Used to skip the root element.
+
+ };
+
+
}
}
--
GitLab