From 7d42d85e124539c4643c424a1b7b24a852cd4c64 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Wed, 7 Nov 2018 10:24:07 +0100
Subject: [PATCH] Added example MjcfConverter
---
VirtualRobot/examples/CMakeLists.txt | 1 +
.../examples/MjcfConverter/CMakeLists.txt | 40 +++++++
.../examples/MjcfConverter/MjcfConverter.cpp | 101 ++++++++++++++++++
.../examples/MjcfConverter/MjcfConverter.h | 33 ++++++
.../examples/MjcfConverter/MjcfDocument.cpp | 9 ++
.../examples/MjcfConverter/MjcfDocument.h | 22 ++++
VirtualRobot/examples/MjcfConverter/main.cpp | 47 ++++++++
7 files changed, 253 insertions(+)
create mode 100644 VirtualRobot/examples/MjcfConverter/CMakeLists.txt
create mode 100644 VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
create mode 100644 VirtualRobot/examples/MjcfConverter/MjcfConverter.h
create mode 100644 VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
create mode 100644 VirtualRobot/examples/MjcfConverter/MjcfDocument.h
create mode 100644 VirtualRobot/examples/MjcfConverter/main.cpp
diff --git a/VirtualRobot/examples/CMakeLists.txt b/VirtualRobot/examples/CMakeLists.txt
index 3d80def1f..29e9c197f 100644
--- a/VirtualRobot/examples/CMakeLists.txt
+++ b/VirtualRobot/examples/CMakeLists.txt
@@ -21,6 +21,7 @@ ADD_SUBDIRECTORY(GenericIK)
ADD_SUBDIRECTORY(reachability)
ADD_SUBDIRECTORY(stability)
ADD_SUBDIRECTORY(GraspEditor)
+ADD_SUBDIRECTORY(MjcfConverter)
ADD_SUBDIRECTORY(ReachabilityMap)
ADD_SUBDIRECTORY(loadURDFRobot)
diff --git a/VirtualRobot/examples/MjcfConverter/CMakeLists.txt b/VirtualRobot/examples/MjcfConverter/CMakeLists.txt
new file mode 100644
index 000000000..3ce6cedd0
--- /dev/null
+++ b/VirtualRobot/examples/MjcfConverter/CMakeLists.txt
@@ -0,0 +1,40 @@
+project(MjcfConverter)
+
+find_package(TinyXML2)
+if (${TINYXML2_FOUND})
+ include_directories(${TINYXML2_INCLUDE_DIR})
+endif()
+
+
+set(SOURCES
+ main.cpp
+ MjcfConverter.cpp
+ MjcfDocument.cpp
+)
+
+set(HEADERS
+ MjcfConverter.h
+ MjcfDocument.h
+)
+
+
+add_executable(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+set_target_properties(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR})
+set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER "Examples")
+
+target_link_libraries(${PROJECT_NAME} VirtualRobot ${TINYXML2_LIBRARIES})
+
+
+#######################################################################################
+############################ Setup for installation ###################################
+#######################################################################################
+
+install(TARGETS ${PROJECT_NAME}
+ # IMPORTANT: Add the library to the "export-set"
+ EXPORT SimoxTargets
+ RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT bin
+ COMPONENT dev)
+
+message( STATUS " ** Simox application ${PROJECT_NAME} will be placed into " ${Simox_BIN_DIR})
+message( STATUS " ** Simox application ${PROJECT_NAME} will be installed into " ${INSTALL_BIN_DIR})
+
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
new file mode 100644
index 000000000..27bdd3af3
--- /dev/null
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
@@ -0,0 +1,101 @@
+#include "MjcfConverter.h"
+
+#include <tinyxml2.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+
+using namespace VirtualRobot;
+namespace tx = tinyxml2;
+
+
+MjcfConverter::MjcfConverter()
+{
+}
+
+void MjcfConverter::convert(const std::string& inputSimoxXmlFile, const std::string& outputMjcfFile)
+{
+ RobotPtr robot = loadInputFile(inputSimoxXmlFile);
+ MjcfDocumentPtr mjcfDoc = convertToMjcf(robot);
+ writeOutputFile(*mjcfDoc, outputMjcfFile);
+}
+
+RobotPtr MjcfConverter::loadInputFile(const std::string& inputFilename)
+{
+ try
+ {
+ RobotPtr robot = RobotIO::loadRobot(inputFilename, RobotIO::eStructure);
+ assert(robot);
+ return robot;
+ }
+ catch (const VirtualRobotException&)
+ {
+ throw; // rethrow
+ }
+}
+
+MjcfDocumentPtr MjcfConverter::convertToMjcf(RobotPtr robot)
+{
+ MjcfDocumentPtr doc(new MjcfDocument());
+
+ tx::XMLElement* elMujoco = doc->NewElement("mujoco");
+ doc->InsertEndChild(elMujoco);
+
+
+
+
+ /*
+ cout << "******** Robot ********" << endl;
+ cout << "* Name: " << robot->getName() << endl;
+ cout << "* Type: " << robot->getType() << endl;
+
+ if (robot->getRootNode())
+ {
+ cout << "* Root Node: " << robot->getRootNode()->getName() << endl;
+ }
+ else
+ {
+ cout << "* Root Node: not set" << endl;
+ }
+
+ cout << endl;
+
+ if (robot->getRootNode())
+ {
+ robot->getRootNode()->print(true, true);
+ }
+
+ cout << endl;
+
+ std::vector<RobotNodeSetPtr> robotNodeSets = robot->getRobotNodeSets();
+
+ if (robotNodeSets.size() > 0)
+ {
+ cout << "* RobotNodeSets:" << endl;
+
+ std::vector<RobotNodeSetPtr>::iterator iter = robotNodeSets.begin();
+
+ while (iter != robotNodeSets.end())
+ {
+ cout << "----------------------------------" << endl;
+ (*iter)->print();
+ iter++;
+ }
+
+ cout << endl;
+ }
+
+ cout << "******** Robot ********" << endl;
+ */
+
+ return doc;
+}
+
+void MjcfConverter::writeOutputFile(MjcfDocument& mjcfDoc,
+ const std::string& outputFilename)
+{
+ std::cout << "Writing to " << outputFilename << std::endl;
+ mjcfDoc.Print();
+ mjcfDoc.SaveFile(outputFilename.c_str());
+}
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
new file mode 100644
index 000000000..010297ccd
--- /dev/null
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+
+#include "MjcfDocument.h"
+
+
+namespace VirtualRobot
+{
+
+ class MjcfConverter
+ {
+ public:
+
+ MjcfConverter();
+
+
+ void convert(const std::string& inputSimoxXmlFile,
+ const std::string& outputMjcfFile);
+
+
+ private:
+
+ RobotPtr loadInputFile(const std::string& inputFilename);
+
+ MjcfDocumentPtr convertToMjcf(RobotPtr robot);
+
+ void writeOutputFile(MjcfDocument& mjcfDoc,
+ const std::string& outputFilename);
+
+ };
+
+}
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp b/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
new file mode 100644
index 000000000..e5d5d9b5b
--- /dev/null
+++ b/VirtualRobot/examples/MjcfConverter/MjcfDocument.cpp
@@ -0,0 +1,9 @@
+#include "MjcfDocument.h"
+
+using namespace VirtualRobot;
+
+
+MjcfDocument::MjcfDocument()
+{
+
+}
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfDocument.h b/VirtualRobot/examples/MjcfConverter/MjcfDocument.h
new file mode 100644
index 000000000..b4ccf422e
--- /dev/null
+++ b/VirtualRobot/examples/MjcfConverter/MjcfDocument.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <memory>
+#include <tinyxml2.h>
+
+
+namespace VirtualRobot
+{
+
+ class MjcfDocument : public tinyxml2::XMLDocument
+ {
+
+ public:
+
+ MjcfDocument();
+
+
+ };
+
+ using MjcfDocumentPtr = std::unique_ptr<MjcfDocument>;
+
+}
diff --git a/VirtualRobot/examples/MjcfConverter/main.cpp b/VirtualRobot/examples/MjcfConverter/main.cpp
new file mode 100644
index 000000000..f405471f9
--- /dev/null
+++ b/VirtualRobot/examples/MjcfConverter/main.cpp
@@ -0,0 +1,47 @@
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+#include <VirtualRobot/VirtualRobotException.h>
+
+#include <string>
+
+#include "MjcfConverter.h"
+
+
+using namespace VirtualRobot;
+
+
+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);
+
+
+ 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;
+ }
+ }
+
+
+
+ std::stringstream outputFilename;
+ auto indexDot = inputFilename.find_last_of(".");
+ outputFilename << inputFilename.substr(0, indexDot)
+ << "_mjcf" << inputFilename.substr(indexDot);
+
+ std::cout << "Input file: " << inputFilename << std::endl;
+ std::cout << "Output file: " << outputFilename.str() << std::endl;
+
+ MjcfConverter converter;
+ converter.convert(inputFilename, outputFilename.str());
+
+}
--
GitLab