From f0bc366ce58514fe6d96f7dd5523b83be9a738df Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Tue, 13 Nov 2018 09:37:38 +0100
Subject: [PATCH] Outfactored massless body sanitizing
---
.../examples/MjcfConverter/CMakeLists.txt | 2 +
.../examples/MjcfConverter/MjcfConverter.cpp | 232 ++++--------------
.../examples/MjcfConverter/MjcfConverter.h | 13 +-
.../MjcfMasslessBodySanitizer.cpp | 203 +++++++++++++++
.../MjcfConverter/MjcfMasslessBodySanitizer.h | 58 +++++
5 files changed, 314 insertions(+), 194 deletions(-)
create mode 100644 VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.cpp
create mode 100644 VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.h
diff --git a/VirtualRobot/examples/MjcfConverter/CMakeLists.txt b/VirtualRobot/examples/MjcfConverter/CMakeLists.txt
index 95eab5a63..45f303f21 100644
--- a/VirtualRobot/examples/MjcfConverter/CMakeLists.txt
+++ b/VirtualRobot/examples/MjcfConverter/CMakeLists.txt
@@ -16,6 +16,7 @@ if (Boost_FOUND)
MjcfConverter.cpp
MjcfDocument.cpp
+ MjcfMasslessBodySanitizer.cpp
SimoxXMLDocument.cpp
)
@@ -26,6 +27,7 @@ if (Boost_FOUND)
MjcfConverter.h
MjcfDocument.h
+ MjcfMasslessBodySanitizer.h
SimoxXMLDocument.h
)
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
index 3b8ec2748..0dec1f9d4 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
@@ -7,13 +7,15 @@
#include "xml_visitors.h"
+
using namespace VirtualRobot;
namespace tx = tinyxml2;
namespace fs = boost::filesystem;
using Element = mjcf::Element;
-MjcfConverter::MjcfConverter()
+MjcfConverter::MjcfConverter() :
+ masslessBodySanitizer(document, robot)
{
}
@@ -23,7 +25,45 @@ void MjcfConverter::convert(const std::string& inputSimoxXmlFile,
setPaths(inputSimoxXmlFile, outputDirectory);
loadInputFile();
- convertToMjcf();
+
+ document.reset(new mjcf::Document());
+
+ document->setModelName(robot->getName());
+ document->compiler()->SetAttribute("angle", "radian");
+ document->compiler()->SetAttribute("balanceinertia", "true");
+
+ makeEnvironment();
+
+ std::cout << "Creating bodies structure ..." << std::endl;
+ addNodeBodies();
+
+ std::cout << "Adding meshes and geoms ..." << std::endl;
+ addNodeBodyMeshes();
+
+ std::cout << "===========================" << std::endl
+ << "Current model: " << std::endl
+ << "--------------" << std::endl;
+ document->Print();
+ std::cout << "===========================" << std::endl;
+
+ std::cout << "Merging massless bodies ..." << std::endl;
+ masslessBodySanitizer.sanitize();
+
+ std::cout << "Adding contact excludes ..." << std::endl;
+ addContactExcludes();
+
+ std::cout << "Adding actuators ..." << std::endl;
+ addActuators();
+
+ std::cout << "Done.";
+
+ std::cout << std::endl;
+ std::cout << "===========================" << std::endl
+ << "Output file: " << std::endl
+ << "------------" << std::endl;
+ document->Print();
+ std::cout << "===========================" << std::endl;
+
writeOutputFile();
}
@@ -80,53 +120,11 @@ void MjcfConverter::loadInputFile()
void MjcfConverter::writeOutputFile()
{
assert(!outputFileName.empty());
-
- std::cout << std::endl;
- document->Print();
-
std::cout << "Writing to " << outputFileName << std::endl;
document->SaveFile(outputFileName.c_str());
}
-void MjcfConverter::convertToMjcf()
-{
- document.reset(new mjcf::Document());
-
- document->setModelName(robot->getName());
- document->compiler()->SetAttribute("angle", "radian");
- document->compiler()->SetAttribute("balanceinertia", "true");
-
- makeEnvironment();
-
- std::cout << "Creating bodies structure ..." << std::endl;
- addNodeBodies();
-
- std::cout << "Adding meshes and geoms ..." << std::endl;
- addNodeBodyMeshes();
-
- std::cout << "===========================" << std::endl
- << "Current model: " << std::endl
- << "--------------" << std::endl;
- document->Print();
- std::cout << "===========================" << std::endl;
-
- std::cout << "Merging empty bodies ..." << std::endl;
- sanitizeMasslessBodies();
-
- std::cout << "Adding contact excludes ..." << std::endl;
- document->addContactExcludes(nodeBodies[robot->getRootNode()->getName()]);
-
- std::cout << "Adding actuators ..." << std::endl;
- addActuators();
-
- std::cout << "Done.";
-
- return;
-}
-
-
-
void MjcfConverter::addNodeBodies()
{
nodeBodies.clear();
@@ -255,155 +253,17 @@ Element* MjcfConverter::addNodeBody(RobotNodePtr node)
return element;
}
-void MjcfConverter::sanitizeMasslessBodies()
-{
- // merge body leaf nodes with parent if they do not have a geom
-
- // assume we have leaf
-
- Element* root = document->worldbody()->FirstChildElement("body");
-
- for (Element* body = root->FirstChildElement("body");
- body;
- body = body->NextSiblingElement("body"))
- {
- sanitizeMasslessBodyRecursion(body);
- }
-}
-
-void MjcfConverter::sanitizeMasslessBodyRecursion(mjcf::Element* body)
+void MjcfConverter::addContactExcludes()
{
- assertElementIsBody(body);
-
- std::cout << "- Node '" << body->Attribute("name") << "': " << std::endl;
- const std::string t = " | ";
-
- std::string bodyName = body->Attribute("name");
- RobotNodePtr bodyNode = robot->getRobotNode(bodyName);
- Eigen::Matrix4f bodyPose = bodyNode->getTransformationFrom(bodyNode->getParent());
+ RobotNodePtr rootNode = robot->getRootNode();
- while (!hasMass(body))
+ for (RobotNodePtr node : robot->getRobotNodes())
{
- std::cout << t << "Massless" << std::endl;
-
- if (!hasElementChild(body, "body"))
- {
- // leaf => end of recursion
- std::cout << t << "Leaf" << std::endl;
- sanitizeMasslessLeafBody(body);
- return;
- }
-
- // non-leaf body
- std::cout << t << "Non-leaf" << std::endl;
-
- // check whether there is only one child body
- Element* childBody = body->FirstChildElement("body");
- if (!childBody->NextSiblingElement("body"))
+ for (std::string& ignore : node->getPhysics().ignoreCollisions)
{
- std::string childBodyName = childBody->Attribute("name");
-
- std::cout << t << "Single child body => merging '" << childBodyName
- << "' into '" << bodyName << "'" << std::endl;
-
- // check child for pose attributes
- if (childBody->Attribute("pos") || childBody->Attribute("quat"))
- {
- // update body's transform and joint axes
-
- RobotNodePtr childNode = robot->getRobotNode(childBodyName);
- Eigen::Matrix4f childPose = childNode->getTransformationFrom(childNode->getParent());
-
- // body's pose
- bodyPose = bodyPose * childPose;
- document->setBodyPose(body, bodyPose);
-
- /* Adapt axes of joints in body:
- * The axes will be relative to the new pose. Therefore, the additional rotation
- * of the child must be subtracted from the joint axis.
- */
-
- Eigen::Matrix3f revChildOri = childPose.block<3,3>(0, 0).transpose();
-
- for (Element* joint = body->FirstChildElement("joint"); joint;
- joint = joint->NextSiblingElement("joint"))
- {
- Eigen::Vector3f axis = strToVec(joint->Attribute("axis"));
- // apply child orientation
- axis = revChildOri * axis;
- document->setJointAxis(joint, axis);
- }
- }
-
-
- // merge childBody into body => move all its elements here
- for (tx::XMLNode* grandChild = childBody->FirstChild(); grandChild;
- grandChild = grandChild->NextSibling())
- {
- std::cout << t << " | Moving '" << grandChild->Value() << "'" << std::endl;
-
- // clone grandchild
- tx::XMLNode* grandChildClone = grandChild->DeepClone(nullptr);
-
- // insert to body
- body->InsertEndChild(grandChildClone);
- }
-
- // update body name
-
- std::stringstream newName;
- /*
- std::size_t prefixSize = commonPrefixLength(bodyName, childBodyName);
- newName << bodyName.substr(0, prefixSize)
- << bodyName.substr(prefixSize) << "~" << childBodyName.substr(prefixSize);
- */
- newName << bodyName << "~" << childBodyName;
- body->SetAttribute("name", newName.str().c_str());
-
- // delete child
- body->DeleteChild(childBody);
-
- std::cout << t << "=> New body name: " << newName.str() << std::endl;
- }
- else
- {
- VR_WARNING << t << "Massless body with >1 child body: "
- << body->Attribute("name") << std::endl;
- break;
}
}
-
-
- // recursive descend
-
- for (Element* child = body->FirstChildElement("body");
- child;
- child = child->NextSiblingElement("body"))
- {
- sanitizeMasslessBodyRecursion(child);
- }
-
-}
-
-void MjcfConverter::sanitizeMasslessLeafBody(mjcf::Element* body)
-{
-
- assert(!hasElementChild(body, "body"));
- assert(!hasMass(body));
-
- if (body->NoChildren()) // is completely empty?
- {
- // leaf without geom: make it a site
- std::cout << " | Empty => Changing body '" << body->Attribute("name") << "' to site." << std::endl;
- body->SetValue("site");
- }
- else
- {
- // add a small mass
- std::cout << " | Not empty => Adding dummy inertial." << std::endl;
- document->addDummyInertial(body);
- }
}
void MjcfConverter::addActuators()
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
index 3e87fd5d6..0bd82a8ed 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
@@ -8,6 +8,7 @@
#include "exceptions.h"
#include "SimoxXMLDocument.h"
#include "MjcfDocument.h"
+#include "MjcfMasslessBodySanitizer.h"
namespace VirtualRobot
@@ -30,8 +31,6 @@ namespace VirtualRobot
void loadInputFile();
void writeOutputFile();
- void convertToMjcf();
-
void setPaths(const std::string& inputFilename,
const std::string& outputDirectory);
@@ -43,11 +42,6 @@ namespace VirtualRobot
void addNodeBodyMeshes();
- void sanitizeMasslessBodies();
- void sanitizeMasslessBodyRecursion(mjcf::Element* body);
-
- void sanitizeMasslessLeafBody(mjcf::Element* body);
-
void addContactExcludes();
void addActuators();
@@ -81,9 +75,12 @@ namespace VirtualRobot
// Processing
+ mjcf::MjcfMasslessBodySanitizer masslessBodySanitizer;
+
std::map<std::string, mjcf::Element*> nodeBodies;
-
+ std::map<std::string, std::string> mergedBodyNames;
+
};
}
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.cpp b/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.cpp
new file mode 100644
index 000000000..61e019727
--- /dev/null
+++ b/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.cpp
@@ -0,0 +1,203 @@
+#include "MjcfMasslessBodySanitizer.h"
+
+#include <boost/algorithm/string/join.hpp>
+
+
+#include "utils.h"
+
+
+using namespace VirtualRobot;
+using namespace mjcf;
+namespace tx = tinyxml2;
+
+
+MjcfMasslessBodySanitizer::MjcfMasslessBodySanitizer(DocumentPtr& document, RobotPtr& robot) :
+ document(document), robot(robot)
+{
+
+}
+
+void MjcfMasslessBodySanitizer::sanitize()
+{
+ // merge body leaf nodes with parent if they do not have a mass (inertial or geom)
+
+ Element* root = document->worldbody()->FirstChildElement("body");
+
+ for (Element* body = root->FirstChildElement("body");
+ body;
+ body = body->NextSiblingElement("body"))
+ {
+ sanitizeRecursion(body);
+ }
+}
+
+void MjcfMasslessBodySanitizer::sanitizeRecursion(Element* body)
+{
+ assertElementIsBody(body);
+
+ std::cout << "- Node '" << body->Attribute("name") << "': " << std::endl;
+ const std::string t = " | ";
+
+ std::string bodyName = body->Attribute("name");
+ RobotNodePtr bodyNode = robot->getRobotNode(bodyName);
+ Eigen::Matrix4f bodyPose = bodyNode->getTransformationFrom(bodyNode->getParent());
+
+ while (!hasMass(body))
+ {
+ std::cout << t << "Massless" << std::endl;
+
+ if (!hasElementChild(body, "body"))
+ {
+ // leaf => end of recursion
+ std::cout << t << "Leaf" << std::endl;
+ sanitizeLeafBody(body);
+ return;
+ }
+
+ // non-leaf body
+ std::cout << t << "Non-leaf" << std::endl;
+
+ // check whether there is only one child body
+ Element* childBody = body->FirstChildElement("body");
+ if (!childBody->NextSiblingElement("body"))
+ {
+ std::string childBodyName = childBody->Attribute("name");
+
+ std::cout << t << "Single child body => merging '" << childBodyName
+ << "' into '" << bodyName << "'" << std::endl;
+
+ // check child for pose attributes
+ if (childBody->Attribute("pos") || childBody->Attribute("quat"))
+ {
+ // update body's transform and joint axes
+
+ RobotNodePtr childNode = robot->getRobotNode(childBodyName);
+ Eigen::Matrix4f childPose = childNode->getTransformationFrom(childNode->getParent());
+
+ // body's pose
+ bodyPose = bodyPose * childPose;
+ document->setBodyPose(body, bodyPose);
+
+ /* Adapt axes of joints in body:
+ * The axes will be relative to the new pose. Therefore, the additional rotation
+ * of the child must be subtracted from the joint axis.
+ */
+
+ Eigen::Matrix3f revChildOri = childPose.block<3,3>(0, 0).transpose();
+
+ for (Element* joint = body->FirstChildElement("joint"); joint;
+ joint = joint->NextSiblingElement("joint"))
+ {
+ Eigen::Vector3f axis = strToVec(joint->Attribute("axis"));
+ // apply child orientation
+ axis = revChildOri * axis;
+ document->setJointAxis(joint, axis);
+ }
+ }
+
+
+ // merge childBody into body => move all its elements here
+ for (tx::XMLNode* grandChild = childBody->FirstChild(); grandChild;
+ grandChild = grandChild->NextSibling())
+ {
+ std::cout << t << " | Moving '" << grandChild->Value() << "'" << std::endl;
+
+ // clone grandchild
+ tx::XMLNode* grandChildClone = grandChild->DeepClone(nullptr);
+
+ // insert to body
+ body->InsertEndChild(grandChildClone);
+ }
+
+ // update body name
+ MergedBodySet bodySet = getMergedBodySetWith(bodyName);
+ bodySet.addBody(childBodyName);
+ body->SetAttribute("name", bodySet.getMergedBodyName().c_str());
+
+ // delete child
+ body->DeleteChild(childBody);
+
+ std::cout << t << "=> New body name: " << bodySet.getMergedBodyName() << std::endl;
+ }
+ else
+ {
+ VR_WARNING << t << "Massless body with >1 child body: "
+ << body->Attribute("name") << std::endl;
+ break;
+ }
+ }
+
+
+ // recursive descend
+
+ for (Element* child = body->FirstChildElement("body");
+ child;
+ child = child->NextSiblingElement("body"))
+ {
+ sanitizeRecursion(child);
+ }
+
+}
+
+void MjcfMasslessBodySanitizer::sanitizeLeafBody(Element* body)
+{
+ assert(!hasElementChild(body, "body"));
+ assert(!hasMass(body));
+
+ if (body->NoChildren()) // is completely empty?
+ {
+ // leaf without geom: make it a site
+ std::cout << " | Empty => Changing body '" << body->Attribute("name") << "' to site." << std::endl;
+ body->SetValue("site");
+ }
+ else
+ {
+ // add a small mass
+ std::cout << " | Not empty => Adding dummy inertial." << std::endl;
+ document->addDummyInertial(body);
+ }
+}
+
+MergedBodySet& MjcfMasslessBodySanitizer::getMergedBodySetWith(const std::string& bodyName)
+{
+ for (auto& set : mergedBodySets)
+ {
+ if (set.containsBody(bodyName))
+ {
+ return set;
+ }
+ }
+
+ // not found => add
+ MergedBodySet newSet;
+ newSet.addBody(bodyName);
+ mergedBodySets.push_back(newSet);
+
+ return mergedBodySets.back();
+}
+
+const std::vector<MergedBodySet>& MjcfMasslessBodySanitizer::getMergedBodySets() const
+{
+ return mergedBodySets;
+}
+
+void MergedBodySet::addBody(const std::string& bodyName)
+{
+ originalBodyNames.insert(bodyName);
+ updateMergedBodyName();
+}
+
+bool MergedBodySet::containsBody(const std::string& bodyName) const
+{
+ return originalBodyNames.find(bodyName) != originalBodyNames.end();
+}
+
+const std::string& MergedBodySet::getMergedBodyName() const
+{
+ return mergedBodyName;
+}
+
+void MergedBodySet::updateMergedBodyName()
+{
+ this->mergedBodyName = boost::algorithm::join(originalBodyNames, "~");
+}
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.h b/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.h
new file mode 100644
index 000000000..14a2087a7
--- /dev/null
+++ b/VirtualRobot/examples/MjcfConverter/MjcfMasslessBodySanitizer.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include "MjcfDocument.h"
+
+
+namespace VirtualRobot
+{
+namespace mjcf
+{
+
+ class MergedBodySet
+ {
+ public:
+
+ void addBody(const std::string& bodyName);
+ bool containsBody(const std::string& bodyName) const;
+
+ const std::string& getMergedBodyName() const;
+
+
+ private:
+
+ void updateMergedBodyName();
+
+ std::string mergedBodyName;
+ std::set<std::string> originalBodyNames;
+
+ };
+
+
+ class MjcfMasslessBodySanitizer
+ {
+ public:
+
+ MjcfMasslessBodySanitizer(DocumentPtr& document, RobotPtr& robot);
+
+ void sanitize();
+
+ const std::vector<MergedBodySet>& getMergedBodySets() const;
+
+
+ private:
+
+ void sanitizeRecursion(mjcf::Element* body);
+ void sanitizeLeafBody(mjcf::Element* body);
+
+ MergedBodySet& getMergedBodySetWith(const std::string& bodyName);
+
+
+ DocumentPtr& document;
+ RobotPtr& robot;
+
+ std::vector<MergedBodySet> mergedBodySets;
+
+ };
+
+}
+}
--
GitLab