diff --git a/VirtualRobot/XML/mjcf/MujocoIO.cpp b/VirtualRobot/XML/mjcf/MujocoIO.cpp
index 0c509df94b72379c0334ca11ffeb0c65282e6846..b69699aa4362d499c3d3e43f8d9378bca7b0e895 100644
--- a/VirtualRobot/XML/mjcf/MujocoIO.cpp
+++ b/VirtualRobot/XML/mjcf/MujocoIO.cpp
@@ -19,34 +19,29 @@ MujocoIO::MujocoIO() :
{
}
-void MujocoIO::saveMJCF(RobotPtr robot, const std::string& filename, const std::string& basePath, const std::string& meshDir)
+void MujocoIO::saveMJCF(RobotPtr robot,
+ const std::string& filename, const std::string& basePath,
+ const std::string& meshRelDir)
{
THROW_VR_EXCEPTION_IF(!robot, "Given RobotPtr robot is null.");
THROW_VR_EXCEPTION_IF(filename.empty(), "Given filename is empty.");
this->robot = robot;
- this->outputDirectory = basePath;
- this->outputFileName = filename;
- this->outputMeshRelDirectory = meshDir;
+
+ setPaths(filename, basePath, meshRelDir);
document.reset(new mjcf::Document());
document->setModelName(robot->getName());
- document->compiler()->SetAttribute("angle", "radian");
- document->compiler()->SetAttribute("balanceinertia", "true");
-
- XMLElement* defaultsClass = document->addDefaultsClass(robot->getName());
- std::stringstream ss;
- ss << "Add default values for " << robot->getName() << " here.";
- defaultsClass->InsertFirstChild(document->NewComment(ss.str().c_str()));
+ makeCompiler();
+ makeDefaultsGroup();
document->setNewElementClass(robot->getName(), true);
-
- makeEnvironment();
+ addSkybox();
std::cout << "Creating bodies structure ..." << std::endl;
- addNodeBodies();
+ makeNodeBodies();
std::cout << "Adding meshes and geoms ..." << std::endl;
addNodeBodyMeshes();
@@ -84,15 +79,55 @@ void MujocoIO::saveMJCF(RobotPtr robot, const std::string& filename, const std::
document->SaveFile((outputDirectory / outputFileName).c_str());
}
+void MujocoIO::setPaths(const std::string& filename, const std::string& basePath, const std::string& meshRelDir)
+{
+ outputDirectory = basePath;
+ outputFileName = filename;
+ outputMeshRelDirectory = meshRelDir;
+
+ ensureDirectoriesExist();
+}
+
+void MujocoIO::ensureDirectoriesExist()
+{
+ auto ensureDirExists = [](const fs::path& dir, const std::string& errMsgName)
+ {
+ if (!fs::is_directory(dir))
+ {
+ std::cout << "Creating directory: " << dir << std::endl;
+ bool success = fs::create_directories(dir);
+ THROW_VR_EXCEPTION_IF(!success, "Could not create " << errMsgName << ": " << dir);
+ }
+ };
+
+ ensureDirExists(outputDirectory, "output directory");
+ ensureDirExists(outputMeshDirectory(), "output mesh directory");
+}
+
+void MujocoIO::makeCompiler()
+{
+ document->compiler()->SetAttribute("angle", "radian");
+ document->compiler()->SetAttribute("balanceinertia", "true");
+}
-void MujocoIO::makeEnvironment()
+void MujocoIO::makeDefaultsGroup()
+{
+ XMLElement* defaultsClass = document->addDefaultsClass(robot->getName());
+
+ std::stringstream comment;
+ comment << "Add default values for " << robot->getName() << " here.";
+ defaultsClass->InsertFirstChild(document->NewComment(comment.str().c_str()));
+}
+
+
+void MujocoIO::addSkybox()
{
document->addSkyboxTexture(Eigen::Vector3f(.8f, .9f, .95f),
Eigen::Vector3f(.4f, .6f, .8f));
}
-void MujocoIO::addNodeBodies()
+void MujocoIO::makeNodeBodies()
{
nodeBodies.clear();
@@ -129,12 +164,10 @@ void MujocoIO::addNodeBodyMeshes()
std::cout << "Node " << node->getName() << ":\t";
fs::path srcMeshPath = visualization->getFilename();
- assert(srcMeshPath.is_absolute());
fs::path dstMeshFileName = srcMeshPath.filename();
dstMeshFileName.replace_extension("stl");
- fs::path dstMeshRelPath = outputMeshRelDirectory / dstMeshFileName;
- fs::path dstMeshPath = outputDirectory / dstMeshRelPath;
+ fs::path dstMeshPath = outputMeshDirectory() / dstMeshFileName;
if (!fs::exists(dstMeshPath))
{
@@ -161,7 +194,9 @@ void MujocoIO::addNodeBodyMeshes()
<< " -o " << dstMeshPath.string();
// run command
+ std::cout << "----------------------------------------------------------" << std::endl;
int r = system(convertCommand.str().c_str());
+ std::cout << "----------------------------------------------------------" << std::endl;
if (r != 0)
{
std::cout << "Command returned with error: " << r << "\n"
@@ -177,7 +212,8 @@ void MujocoIO::addNodeBodyMeshes()
}
else
{
- std::cout << "skipping (" << dstMeshRelPath.string() << " already exists)";
+ std::cout << "skipping (" << outputMeshRelDirectory / dstMeshFileName
+ << " already exists)";
}
std::cout << std::endl;
@@ -185,7 +221,7 @@ void MujocoIO::addNodeBodyMeshes()
// add asset
std::string meshName = node->getName();
- document->addMeshElement(meshName, (outputDirectory/dstMeshRelPath).string());
+ document->addMeshElement(meshName, fs::absolute(dstMeshPath).string());
// add geom to body
XMLElement* body = nodeBodies[node->getName()];
@@ -356,4 +392,9 @@ std::vector<const mjcf::XMLElement*> MujocoIO::getAllElements(const std::string&
return visitor.foundElements;
}
+void MujocoIO::setLengthScaling(float value)
+{
+ lengthScaling = value;
+}
+
diff --git a/VirtualRobot/XML/mjcf/MujocoIO.h b/VirtualRobot/XML/mjcf/MujocoIO.h
index bbe32208ad9b497650fc227b7ddd4aced4b79288..4a580b19eb3d5b9f1b2710f41955b1e4fe010414 100644
--- a/VirtualRobot/XML/mjcf/MujocoIO.h
+++ b/VirtualRobot/XML/mjcf/MujocoIO.h
@@ -18,31 +18,54 @@ namespace mjcf
{
public:
+ /// Constructor.
MujocoIO();
-
+ /**
+ * @brief Create a Mujoco XML (MJCF) document for the given robot.
+ * @param robot the robot
+ * @param filename the output filename (without directory)
+ * @param basePath the output directory
+ * @param meshRelDir the directory relative to basePath where meshes shall be placed
+ */
void saveMJCF(RobotPtr robot, const std::string& filename,
- const std::string& basePath, const std::string& meshDir);
-
+ const std::string& basePath, const std::string& meshRelDir);
- private:
+ /// Set the scaling for lenghts.
+ void setLengthScaling(float value);
- void setPaths(const std::string& inputFilename,
- const std::string& outputDirectory);
-
- void makeEnvironment();
+ private:
- void addNodeBodies();
+ /// Set the output path members.
+ void setPaths(const std::string& filename, const std::string& basePath,
+ const std::string& meshRelDir);
+ /// Create output directories if the do not exist.
+ void ensureDirectoriesExist();
+
+ /// Make the compiler section.
+ void makeCompiler();
+ /// Add a defaults group for the robot.
+ void makeDefaultsGroup();
+ /// Add a skybox texture asset.
+ void addSkybox();
+
+ /// Construct the body structure corresponding to the robot nodes.
+ void makeNodeBodies();
+ /// Add a body element for a robot node. If its parent does not exist,
+ /// create the parent body first.
mjcf::XMLElement* addNodeBody(RobotNodePtr node);
+ /// Convert meshes and add mesh assets for robot nodes with visualization.
void addNodeBodyMeshes();
-
-
+
+ /// Add contact exclude elements for IgnoreCollision elements.
void addContactExcludes();
+ /// Add actuators for all joints.
void addActuators();
+ /// Scale all lengths by lengthScaling.
void scaleLengths();
std::vector<const mjcf::XMLElement*> getAllElements(const std::string& elemName);
@@ -59,7 +82,7 @@ namespace mjcf
boost::filesystem::path outputDirectory;
boost::filesystem::path outputFileName;
boost::filesystem::path outputMeshRelDirectory;
-
+ boost::filesystem::path outputMeshDirectory() { return outputDirectory / outputMeshRelDirectory; }
// Input
diff --git a/VirtualRobot/examples/Simox2Mjcf/main.cpp b/VirtualRobot/examples/Simox2Mjcf/main.cpp
index ab479ed2659045cf87ef064e868352f8290320d9..ace47c5eb1e99fc86257c710e5ea6c5f6cfdde00 100644
--- a/VirtualRobot/examples/Simox2Mjcf/main.cpp
+++ b/VirtualRobot/examples/Simox2Mjcf/main.cpp
@@ -78,5 +78,5 @@ int main(int argc, char* argv[])
throw; // rethrow
}
- //RobotIO::saveMJCF(robot, inputFilename.filename().string(), outputDir.string(), meshRelDir);
+ RobotIO::saveMJCF(robot, inputFilename.filename().string(), outputDir.string(), meshRelDir);
}