diff --git a/VirtualRobot/examples/MjcfConverter/CMakeLists.txt b/VirtualRobot/examples/MjcfConverter/CMakeLists.txt index 73623bad088f6b3ebdab5ff0b595bf3ce159fb74..95eab5a63d557bda4537fa74b2c18627f73655fb 100644 --- a/VirtualRobot/examples/MjcfConverter/CMakeLists.txt +++ b/VirtualRobot/examples/MjcfConverter/CMakeLists.txt @@ -12,17 +12,21 @@ if (Boost_FOUND) main.cpp exceptions.cpp utils.cpp + xml_visitors.cpp MjcfConverter.cpp MjcfDocument.cpp + SimoxXMLDocument.cpp ) set(HEADERS exceptions.h utils.h + xml_visitors.h MjcfConverter.h MjcfDocument.h + SimoxXMLDocument.h ) set(LIBS diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp index 7ee32e44a6c18c366987f1af40f47b8002e1b52d..788aab09faa9f83c9da63b0d226003d851e4c5a7 100644 --- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp +++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp @@ -74,12 +74,7 @@ void MjcfConverter::loadInputFile() throw; // rethrow } - inputXML.reset(new tx::XMLDocument()); - - if (inputXML->LoadFile(inputFilePath.c_str())) - { - throw MjcfXmlLoadFileFailed(inputXML->ErrorName(), inputXML->ErrorStr()); - } + inputXML.LoadFile(inputFilePath.string()); } void MjcfConverter::writeOutputFile() @@ -103,9 +98,6 @@ void MjcfConverter::convertToMjcf() makeEnvironment(); - std::cout << "Collecting collision model and visualization files..." << std::endl; - gatherCollisionAndVisualizationFiles(); - std::cout << "Creating bodies structure ..." << std::endl; addNodeBodies(); @@ -129,36 +121,7 @@ void MjcfConverter::convertToMjcf() return; } -void MjcfConverter::gatherCollisionAndVisualizationFiles() -{ - nodeCollisionModelFiles.clear(); - nodeVisualizationFiles.clear(); - - tx::XMLNode* xmlRobot = inputXML->FirstChildElement("Robot"); - assert(xmlRobot); - - for (tx::XMLElement* xmlRobotNode = xmlRobot->FirstChildElement("RobotNode"); - xmlRobotNode; - xmlRobotNode = xmlRobotNode->NextSiblingElement("RobotNode")) - { - std::string nodeName = xmlRobotNode->Attribute("name"); - - tx::XMLElement* xmlVisu = xmlRobotNode->FirstChildElement("Visualization"); - tx::XMLElement* xmlColModel = xmlRobotNode->FirstChildElement("CollisionModel"); - - if (xmlVisu) - { - std::string filename = xmlVisu->FirstChildElement("File")->GetText(); - nodeVisualizationFiles[nodeName] = filename; - } - - if (xmlColModel) - { - std::string filename = xmlColModel->FirstChildElement("File")->GetText(); - nodeCollisionModelFiles[nodeName] = filename; - } - } -} + void MjcfConverter::addNodeBodies() { @@ -185,15 +148,14 @@ void MjcfConverter::addNodeBodyMeshes() for (RobotNodePtr node : robot->getRobotNodes()) { - auto item = nodeVisualizationFiles.find(node->getName()); - if (item == nodeVisualizationFiles.end()) + if (!inputXML.hasVisualizationFile(node)) { continue; } std::cout << "Node " << node->getName() << ":\t"; - fs::path srcMeshPath = item->second; + fs::path srcMeshPath = inputXML.collisionModelFile(node); if (srcMeshPath.is_relative()) { @@ -309,7 +271,7 @@ void MjcfConverter::sanitizeMasslessBodyRecursion(mjcf::Element* body) std::cout << "- Node '" << body->Attribute("name") << "': " << std::endl; // leaf => end of recursion - if (!hasElement(body, "body")) + if (!hasElementChild(body, "body")) { std::cout << " | Leaf"; if (!hasMass(body)) @@ -412,7 +374,7 @@ void MjcfConverter::sanitizeMasslessBodyRecursion(mjcf::Element* body) void MjcfConverter::sanitizeMasslessLeafBody(mjcf::Element* body) { - assert(!hasElement(body, "body")); + assert(!hasElementChild(body, "body")); assert(!hasMass(body)); if (body->NoChildren()) // is completely empty? diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h index 68614c132a6a9cfa8eddaf3df40ced6646302f63..805ba6df392849996e6043d7ae5fec58f7c44ca1 100644 --- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h +++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h @@ -6,6 +6,7 @@ #include "exceptions.h" +#include "SimoxXMLDocument.h" #include "MjcfDocument.h" @@ -65,7 +66,7 @@ namespace VirtualRobot // Input - std::unique_ptr<tinyxml2::XMLDocument> inputXML; + SimoxXMLDocument inputXML; RobotPtr robot; @@ -77,9 +78,7 @@ namespace VirtualRobot // Processing std::map<std::string, mjcf::Element*> nodeBodies; - - std::map<std::string, std::string> nodeCollisionModelFiles; - std::map<std::string, std::string> nodeVisualizationFiles; + }; diff --git a/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp b/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp index 9a2c93584720f7e57defa6a60bf605fd02d8b07c..1e3b637c8f7429755ec618e7f47c88b8d81a0ba5 100644 --- a/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp +++ b/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp @@ -4,6 +4,7 @@ #include <VirtualRobot/Nodes/RobotNodeRevolute.h> #include "utils.h" +#include "xml_visitors.h" using namespace VirtualRobot; @@ -142,11 +143,7 @@ Element* Document::addJointElement(Element* body, RobotNodePtr node) Element* joint = addNewElement(body, "joint"); - { - std::stringstream jointName; - jointName << node->getName() << "_joint"; - joint->SetAttribute("name", jointName.str().c_str()); - } + joint->SetAttribute("name", node->getName().c_str()); // get the axis Eigen::Vector3f axis; @@ -262,30 +259,4 @@ 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 7d9636b1fab54eabbea78ac1c4e71654e5483701..24a24c7c5908af381be20fb43fc4c7448fd11d3c 100644 --- a/VirtualRobot/examples/MjcfConverter/MjcfDocument.h +++ b/VirtualRobot/examples/MjcfConverter/MjcfDocument.h @@ -23,9 +23,7 @@ namespace mjcf Document(); - void setModelName(const std::string& name); - - + // Top level elements Element* compiler() { return topLevelElement("compiler"); } Element* option() { return topLevelElement("option"); } Element* size() { return topLevelElement("size"); } @@ -42,46 +40,67 @@ namespace mjcf Element* keyframe() { return topLevelElement("keyframe"); } + /// Set the name of the Mujoco model. + void setModelName(const std::string& name); + + /// Add a skybox texture asset (only one allowed). Element* addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2); - Element* addNewElement(Element* parent, const std::string& elemName, - bool first = false); + /// Add a new element with a name (tag) to a parent. + /// @param first if true, will be inserted as first, otherweise at end (default) + Element* addNewElement(Element* parent, const std::string& elemName, bool first = false); + + /// Add a body element to a parent from a RobotNode. + /// Adds inertial and joint element, if necessary. Element* addBodyElement(Element* parent, RobotNodePtr node); + /// Add an inertial element to a body from a RobotNode. + Element* addInertialElement(Element* body, RobotNodePtr node); + /// Add a dummy inertial element with small mass and identity inertia matrix. Element* addDummyInertial(Element* body); - Element* addGeomElement(Element* body, const std::string& meshName); - + /// Add a joint element to a body from a RobotNode. + Element* addJointElement(Element* body, RobotNodePtr node); + /// Add a geom to a body, referencing a mesh. + Element* addGeomElement(Element* body, const std::string& meshName); + /// Add a mesh asset with a name and a file. Element* addMeshElement(const std::string& name, const std::string& file); + + /// Set the pos and quat attribute of a body. void setBodyPose(Element* body, const Eigen::Matrix4f& pose); + /// 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); + private: - /// Gets the top-level element (child of root element) with the given - /// name. If it does not exist, it is created. + /// Gets the top-level element (child of root element) with a name. + /// If it does not exist, it is created. Element* topLevelElement(const std::string& name); - Element* addInertialElement(Element* body, RobotNodePtr node); - Element* addJointElement(Element* body, RobotNodePtr node); - + /// Convert to MJCF XML attribute format. std::string toAttr(bool b); template <int dim> std::string toAttr(const Eigen::Matrix<float, dim, 1>& v); std::string toAttr(const Eigen::Quaternionf& v); - + /// Scaling for lengths, such as positions and translations. float lengthScaling = 0.001f; + /// Precision when comparing floats (e.g. with zero). float floatCompPrecision = 1e-6f; + /// Mass used for dummy inertial elements. float dummyMass = 0.0001f; + /// IOFormat for vectors. Eigen::IOFormat iofVector {5, 0, "", " ", "", "", "", ""}; /// The "mujoco" root element. @@ -104,20 +123,7 @@ namespace mjcf } - 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. - - }; - + } } diff --git a/VirtualRobot/examples/MjcfConverter/SimoxXMLDocument.cpp b/VirtualRobot/examples/MjcfConverter/SimoxXMLDocument.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c8fbb201f8f241200ea5748b8c761148f6b06c75 --- /dev/null +++ b/VirtualRobot/examples/MjcfConverter/SimoxXMLDocument.cpp @@ -0,0 +1,102 @@ +#include "SimoxXMLDocument.h" + +#include "exceptions.h" +#include "utils.h" + + +using namespace VirtualRobot; +namespace tx = tinyxml2; + + +SimoxXMLDocument::SimoxXMLDocument() +{ +} + +void SimoxXMLDocument::LoadFile(const char* filename) +{ + int errID = Base::LoadFile(filename); + if (errID) + { + throw MjcfXmlLoadFileFailed(ErrorName(), ErrorStr()); + } + + collisionModelFiles.clear(); + visualizationFiles.clear(); + + tx::XMLNode* xmlRobot = FirstChildElement("Robot"); + assert(xmlRobot); + + RobotNodeVisitor visitor(*this); + xmlRobot->Accept(&visitor); +} + + +void SimoxXMLDocument::LoadFile(const std::string& filename) +{ + LoadFile(filename.c_str()); +} + +bool SimoxXMLDocument::hasCollisionModelFile(RobotNodePtr robotNode) const +{ + return hasEntry(robotNode, collisionModelFiles); +} + +bool SimoxXMLDocument::hasVisualizationFile(RobotNodePtr robotNode) const +{ + return hasEntry(robotNode, visualizationFiles); +} + +boost::filesystem::path SimoxXMLDocument::collisionModelFile(RobotNodePtr robotNode) const +{ + return getEntry(robotNode, collisionModelFiles); +} + +boost::filesystem::path SimoxXMLDocument::visualizationFile(RobotNodePtr robotNode) const +{ + return getEntry(robotNode, visualizationFiles); +} + +bool SimoxXMLDocument::hasEntry(RobotNodePtr robotNode, const NamePathMap& map) const +{ + return map.find(robotNode->getName()) != map.end(); +} + +boost::filesystem::path SimoxXMLDocument::getEntry( + RobotNodePtr robotNode, const SimoxXMLDocument::NamePathMap& map) const +{ + auto item = map.find(robotNode->getName()); + if (item == map.end()) + { + return boost::filesystem::path(""); + } + return item->second; +} + + +RobotNodeVisitor::RobotNodeVisitor(SimoxXMLDocument& document) : + document(document) +{ +} + +bool RobotNodeVisitor::VisitEnter(const tinyxml2::XMLElement& robotNode, const tinyxml2::XMLAttribute*) +{ + if (isElement(&robotNode, "RobotNode")) + { + std::string nodeName = robotNode.Attribute("name"); + + const tx::XMLElement* xmlVisu = robotNode.FirstChildElement("Visualization"); + const tx::XMLElement* xmlColModel = robotNode.FirstChildElement("CollisionModel"); + + if (xmlVisu) + { + std::string filename = xmlVisu->FirstChildElement("File")->GetText(); + document.visualizationFiles[nodeName] = filename; + } + + if (xmlColModel) + { + std::string filename = xmlColModel->FirstChildElement("File")->GetText(); + document.collisionModelFiles[nodeName] = filename; + } + } +} diff --git a/VirtualRobot/examples/MjcfConverter/SimoxXMLDocument.h b/VirtualRobot/examples/MjcfConverter/SimoxXMLDocument.h new file mode 100644 index 0000000000000000000000000000000000000000..d496b69f3210cf499c0fb556db23efac641fc092 --- /dev/null +++ b/VirtualRobot/examples/MjcfConverter/SimoxXMLDocument.h @@ -0,0 +1,64 @@ +#pragma once + +#include <string> +#include <map> + +#include <boost/filesystem/path.hpp> + +#include <VirtualRobot/Tools/tinyxml2.h> +#include <VirtualRobot/Nodes/RobotNode.h> + + +namespace VirtualRobot +{ + +class SimoxXMLDocument : private tinyxml2::XMLDocument +{ + using Base = tinyxml2::XMLDocument; + friend class RobotNodeVisitor; + + +public: + + SimoxXMLDocument(); + + void LoadFile(const char* filename); + void LoadFile(const std::string& filename); + + + bool hasCollisionModelFile(RobotNodePtr robotNode) const; + bool hasVisualizationFile(RobotNodePtr robotNode) const; + + boost::filesystem::path collisionModelFile(RobotNodePtr robotNode) const; + boost::filesystem::path visualizationFile(RobotNodePtr robotNode) const; + + +private: + + using NamePathMap = std::map<std::string, boost::filesystem::path>; + + bool hasEntry(RobotNodePtr robotNode, const NamePathMap& map) const; + boost::filesystem::path getEntry(RobotNodePtr robotNode, const NamePathMap& map) const; + + NamePathMap collisionModelFiles; + NamePathMap visualizationFiles; + + + +}; + + +class RobotNodeVisitor : public tinyxml2::XMLVisitor +{ +public: + RobotNodeVisitor(SimoxXMLDocument& document); + + virtual bool VisitEnter(const tinyxml2::XMLElement&, const tinyxml2::XMLAttribute*) override; + +private: + SimoxXMLDocument& document; + +}; + + +} diff --git a/VirtualRobot/examples/MjcfConverter/main.cpp b/VirtualRobot/examples/MjcfConverter/main.cpp index 6062682a8d5f55b3c2d2970ad2e63814c7558cc6..2ed219f078bdb8b9099359e1fd4ca70d287d2ead 100644 --- a/VirtualRobot/examples/MjcfConverter/main.cpp +++ b/VirtualRobot/examples/MjcfConverter/main.cpp @@ -1,10 +1,6 @@ #include <boost/filesystem.hpp> -#include <VirtualRobot/Robot.h> #include <VirtualRobot/RuntimeEnvironment.h> -#include <VirtualRobot/VirtualRobotException.h> - -#include <string> #include "MjcfConverter.h" @@ -12,33 +8,44 @@ using namespace VirtualRobot; +#define VR_INFO std::cout << "[INFO] " +#define VR_WARNING std::cerr << "[WARN] " +#define VR_ERROR std::cerr << "[ERROR] " + + int main(int argc, char* argv[]) { VirtualRobot::RuntimeEnvironment::considerKey("robot"); VirtualRobot::RuntimeEnvironment::processCommandLine(argc, argv); // VirtualRobot::RuntimeEnvironment::print(); - std::string inputFilename("robots/examples/loadRobot/RobotExample.xml"); - VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(inputFilename); - + std::string inputFilename; if (VirtualRobot::RuntimeEnvironment::hasValue("robot")) { std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot"); - std::cout << "robFile: " << robFile << std::endl; if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile)) { inputFilename = robFile; } + else + { + VR_INFO << "Something is wrong with " << robFile; + } + } + else + { + VR_INFO << "Usage: " << argv[0] << " --robot <simox robot file>"; + return 0; } boost::filesystem::path outputDir(inputFilename); outputDir.remove_filename(); outputDir /= "mjcf"; - std::cout << "Input file: " << inputFilename << std::endl; - std::cout << "Output dir: " << outputDir << std::endl; + VR_INFO << "Input file: " << inputFilename << std::endl; + VR_INFO << "Output dir: " << outputDir << std::endl; MjcfConverter converter; converter.convert(inputFilename, outputDir.string()); diff --git a/VirtualRobot/examples/MjcfConverter/utils.cpp b/VirtualRobot/examples/MjcfConverter/utils.cpp index 168b362492ade597775e905ca5b46195202e3c2d..7c3df9dadf37a7b2a7906e1ccccc75204f1c9a38 100644 --- a/VirtualRobot/examples/MjcfConverter/utils.cpp +++ b/VirtualRobot/examples/MjcfConverter/utils.cpp @@ -6,31 +6,21 @@ namespace VirtualRobot { -void assertElementIs(mjcf::Element* elem, const char* tag) -{ - assert(std::strcmp(elem->Value(), tag) == 0); -} - -void assertElementIs(mjcf::Element* elem, const std::string& tag) -{ - assert(std::strcmp(elem->Value(), tag.c_str()) == 0); -} - -void assertElementIsBody(mjcf::Element* elem) +void assertElementIsBody(const mjcf::Element* elem) { assertElementIs(elem, "body"); } -bool hasElement(mjcf::Element* elem, const std::string& elemName) +bool hasElementChild(const mjcf::Element* elem, const std::string& elemName) { return elem->FirstChildElement(elemName.c_str()) != nullptr; } -bool hasMass(mjcf::Element* body) +bool hasMass(const mjcf::Element* body) { assertElementIsBody(body); - return hasElement(body, "geom") || hasElement(body, "inertial"); + return hasElementChild(body, "geom") || hasElementChild(body, "inertial"); } Eigen::Vector3f strToVec(const char* string) @@ -54,5 +44,15 @@ std::size_t commonPrefixLength(const std::string& a, const std::string& b) return std::size_t(std::distance(smaller->begin(), mismatch)); } +bool isElement(const mjcf::Element* elem, const char* tag) +{ + return std::strcmp(elem->Value(), tag) == 0; +} + +bool isElement(const mjcf::Element* elem, const std::string& tag) +{ + return std::strcmp(elem->Value(), tag.c_str()) == 0; +} + } diff --git a/VirtualRobot/examples/MjcfConverter/utils.h b/VirtualRobot/examples/MjcfConverter/utils.h index 0853d34b0d2d0b60397891c8077f95af5afe7abc..e86d6aba7151502f90763c832eefb5ed88918720 100644 --- a/VirtualRobot/examples/MjcfConverter/utils.h +++ b/VirtualRobot/examples/MjcfConverter/utils.h @@ -6,20 +6,27 @@ namespace VirtualRobot { -void assertElementIs(mjcf::Element* elem, const char* tag); -void assertElementIs(mjcf::Element* elem, const std::string& tag); -void assertElementIsBody(mjcf::Element* elem); +bool isElement(const mjcf::Element* elem, const char* tag); +bool isElement(const mjcf::Element* elem, const std::string& tag); +bool hasElementChild(const mjcf::Element* elem, const std::string& elemName); -bool hasElement(mjcf::Element* elem, const std::string& elemName); - -bool hasMass(mjcf::Element* body); +bool hasMass(const mjcf::Element* body); Eigen::Vector3f strToVec(const char* string); std::size_t commonPrefixLength(const std::string& a, const std::string& b); + +template <class StringT> +void assertElementIs(const mjcf::Element* elem, const StringT tag) +{ + assert(isElement(elem, tag)); +} + +void assertElementIsBody(const mjcf::Element* elem); + } diff --git a/VirtualRobot/examples/MjcfConverter/xml_visitors.cpp b/VirtualRobot/examples/MjcfConverter/xml_visitors.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4e25d63b72f4a215e91159030954f5584a344e0e --- /dev/null +++ b/VirtualRobot/examples/MjcfConverter/xml_visitors.cpp @@ -0,0 +1,32 @@ +#include "xml_visitors.h" + +using namespace VirtualRobot::mjcf; + + +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/xml_visitors.h b/VirtualRobot/examples/MjcfConverter/xml_visitors.h new file mode 100644 index 0000000000000000000000000000000000000000..32e597ebf816870a588e4bae28e8c1b7516b70bd --- /dev/null +++ b/VirtualRobot/examples/MjcfConverter/xml_visitors.h @@ -0,0 +1,30 @@ +#pragma once + +#include "MjcfDocument.h" + + +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. + +}; + +} +}