diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 6e4b359c39e8c6222c18b0b022e42b0866c71331..936be8c1f96901536641ad781f26c35557e6f965 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -374,9 +374,12 @@ SET(SOURCES
XML/ObjectIO.cpp
XML/RobotIO.cpp
XML/SceneIO.cpp
+
XML/mujoco/BodySanitizer.cpp
XML/mujoco/DummyMassBodySanitizer.cpp
XML/mujoco/MergingBodySanitizer.cpp
+ XML/mujoco/Mesh.cpp
+ XML/mujoco/MeshConverter.cpp
XML/mujoco/MujocoIO.cpp
XML/mujoco/RobotMjcf.cpp
)
@@ -599,9 +602,12 @@ SET(INCLUDES
XML/ObjectIO.h
XML/RobotIO.h
XML/SceneIO.h
+
XML/mujoco/BodySanitizer.h
XML/mujoco/DummyMassBodySanitizer.h
XML/mujoco/MergingBodySanitizer.h
+ XML/mujoco/Mesh.h
+ XML/mujoco/MeshConverter.h
XML/mujoco/MujocoIO.h
XML/mujoco/RobotMjcf.h
diff --git a/VirtualRobot/XML/mujoco/Mesh.cpp b/VirtualRobot/XML/mujoco/Mesh.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9cad112d7f0b2925f89d38fa50027cb2e0eb922c
--- /dev/null
+++ b/VirtualRobot/XML/mujoco/Mesh.cpp
@@ -0,0 +1,290 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package MujocoX::ArmarXObjects::Mesh
+ * @author Rainer Kartmann ( rainer dot kartmann at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "Mesh.h"
+
+#include <fstream>
+#include <random>
+#include <stdexcept>
+
+#include <VirtualRobot/VirtualRobotChecks.h>
+
+
+namespace VirtualRobot::mujoco
+{
+
+
+Mesh Mesh::fromFile(const std::string& filename)
+{
+ Mesh mesh;
+ mesh.read(filename);
+ return mesh;
+}
+
+Mesh::Mesh()
+{
+}
+
+static void readInt32(std::istream& is, int32_t& value)
+{
+ is.read(reinterpret_cast<char*>(&value), sizeof(int32_t));
+}
+
+static void writeInt32(std::ostream& os, const int32_t& value)
+{
+ os.write(reinterpret_cast<const char*>(&value), sizeof(int32_t));
+}
+
+
+template <typename ValueT>
+static void readVector(std::istream& is, std::vector<ValueT>& vector)
+{
+ is.read(reinterpret_cast<char*>(vector.data()),
+ static_cast<std::streamsize>(vector.size() * sizeof(ValueT)));
+}
+
+template <typename ValueT>
+static void writeVector(std::ostream& os, const std::vector<ValueT>& vector)
+{
+ os.write(reinterpret_cast<const char*>(vector.data()),
+ static_cast<std::streamsize>(vector.size() * sizeof(ValueT)));
+}
+
+void Mesh::read(const std::string& filename)
+{
+ std::ifstream ifs(filename, std::ios::binary);
+ if (!ifs.is_open())
+ {
+ throw std::runtime_error("Could not open file: " + filename);
+ }
+ read(ifs);
+ setFilename(filename);
+}
+
+void Mesh::read(std::istream& is)
+{
+ readInt32(is, _nvertex);
+ readInt32(is, _nnormal);
+ readInt32(is, _ntexcoord);
+ readInt32(is, _nface);
+
+ resizeVectors();
+
+ readVector<float>(is, _vertex_positions);
+ readVector<float>(is, _vertex_normals);
+ readVector<float>(is, _vertex_texcoords);
+ readVector<int32_t>(is, _face_vertex_indices);
+}
+
+
+void Mesh::write(const std::string& filename) const
+{
+ std::ofstream ofs(filename, std::ios::binary);
+ if (!ofs.is_open())
+ {
+ throw std::runtime_error("Could not open file: " + filename);
+ }
+ write(ofs);
+}
+
+void Mesh::write(std::ostream& os) const
+{
+ VR_CHECK_EQUAL(_nvertex * 3, static_cast<int>(_vertex_positions.size()));
+ VR_CHECK_EQUAL(_nnormal * 3, static_cast<int>(_vertex_normals.size()));
+ VR_CHECK_EQUAL(_ntexcoord * 2, static_cast<int>(_vertex_texcoords.size()));
+ VR_CHECK_EQUAL(_nface * 3, static_cast<int>(_face_vertex_indices.size()));
+
+ writeInt32(os, _nvertex);
+ writeInt32(os, _nnormal);
+ writeInt32(os, _ntexcoord);
+ writeInt32(os, _nface);
+
+ writeVector<float>(os, _vertex_positions);
+ writeVector<float>(os, _vertex_normals);
+ writeVector<float>(os, _vertex_texcoords);
+ writeVector<int32_t>(os, _face_vertex_indices);
+}
+
+template <typename EigenVectorT, typename ValueT>
+EigenVectorT getEntry(const std::vector<ValueT>& vector, int32_t i)
+{
+ VR_CHECK_NONNEGATIVE(i);
+ return EigenVectorT(&vector.data()[EigenVectorT::SizeAtCompileTime * i]);
+}
+
+Eigen::Vector3f Mesh::vertexPosition(int32_t i) const
+{
+ return getEntry<Eigen::Vector3f>(_vertex_positions, i);
+}
+
+Eigen::Vector3f Mesh::vertexNormal(int32_t i) const
+{
+ return getEntry<Eigen::Vector3f>(_vertex_normals, i);
+}
+
+Eigen::Vector2f Mesh::vertexTexcoord(int32_t i) const
+{
+ return getEntry<Eigen::Vector2f>(_vertex_texcoords, i);
+}
+
+Eigen::Vector3i Mesh::faceVertexIndex(int32_t i) const
+{
+ return getEntry<Eigen::Vector3i>(_face_vertex_indices, i);
+}
+
+
+template <typename Derived>
+void addEntries(std::vector<typename Eigen::MatrixBase<Derived>::Scalar>& vector,
+ const Eigen::MatrixBase<Derived>& values)
+{
+ for (long i = 0; i < values.size(); ++i)
+ {
+ vector.push_back(values(i));
+ }
+}
+
+void Mesh::addVertexPosition(const Eigen::Vector3f& position)
+{
+ addEntries(_vertex_positions, position);
+ ++_nvertex;
+}
+
+void Mesh::addVertexNormal(const Eigen::Vector3f& normal)
+{
+ addEntries(_vertex_normals, normal.normalized());
+ ++_nnormal;
+}
+
+void Mesh::addVertexTexcoord(const Eigen::Vector2f& texcoord)
+{
+ addEntries(_vertex_texcoords, texcoord);
+ ++_ntexcoord;
+}
+
+void Mesh::addFaceVertexIndex(const Eigen::Vector3i& face)
+{
+ addEntries(_face_vertex_indices, face);
+ ++_nface;
+}
+
+
+Eigen::Matrix<float, 3, 2> Mesh::getVertexPositionsBoundingBox() const
+{
+ Eigen::Array3f min;
+ Eigen::Array3f max;
+ min.setConstant(std::numeric_limits<float>::max());
+ max.setConstant(std::numeric_limits<float>::min());
+
+ VR_CHECK_NONNEGATIVE(_nvertex);
+
+ for (int i = 0; i < _nvertex; ++i)
+ {
+ Eigen::Vector3f pos = vertexPosition(i);
+ min = min.min(pos.array());
+ max = max.max(pos.array());
+ }
+
+ Eigen::Matrix<float, 3, 2> bb;
+ bb.col(0) = min.matrix();
+ bb.col(1) = max.matrix();
+ return bb;
+}
+
+void Mesh::setTextureFile(const std::string& path)
+{
+ _texture_file = path;
+}
+
+const std::string& Mesh::getTextureFile() const
+{
+ return _texture_file;
+}
+
+void Mesh::setFilename(const std::string& path)
+{
+ _filename = path;
+}
+
+const std::string& Mesh::getFilename() const
+{
+ return _filename;
+}
+
+
+std::vector<Mesh> Mesh::splitRandomly(std::size_t num) const
+{
+ std::vector<Mesh> meshes(num);
+
+ std::default_random_engine gen(std::random_device{}());
+ std::uniform_int_distribution<std::size_t> distrib(0, num - 1);
+
+ for (int faceIndex = 0; faceIndex < nface(); ++faceIndex)
+ {
+ // select mesh receiving the face
+ std::size_t meshIndex = distrib(gen);
+ Mesh& mesh = meshes[meshIndex];
+
+ const Eigen::Vector3i faceVertexIndexIn = faceVertexIndex(faceIndex);
+
+ // copy the face
+ Eigen::Vector3i faceVertexIndexOut = faceVertexIndex(faceIndex);
+ for (int i = 0; i < faceVertexIndexIn.size(); ++i)
+ {
+ int vertexIndex = faceVertexIndexIn(i);
+
+ // store index of new vertex
+ faceVertexIndexOut(i) = mesh.nvertex();
+
+ // add vertex
+ mesh.addVertexPosition(vertexPosition(vertexIndex));
+
+ if (hasNormals())
+ {
+ mesh.addVertexNormal(vertexPosition(vertexIndex));
+ }
+ if (hasTexcoords())
+ {
+ mesh.addVertexTexcoord(vertexTexcoord(vertexIndex));
+ }
+ }
+ // add face vertex index
+ mesh.addFaceVertexIndex(faceVertexIndexOut);
+ }
+
+ return meshes;
+}
+
+void Mesh::resizeVectors()
+{
+ VR_CHECK_NONNEGATIVE(_nvertex);
+ VR_CHECK_NONNEGATIVE(_nnormal);
+ VR_CHECK_NONNEGATIVE(_ntexcoord);
+ VR_CHECK_NONNEGATIVE(_nface);
+
+ _vertex_positions.resize(static_cast<std::size_t>(3 * _nvertex));
+ _vertex_normals.resize(static_cast<std::size_t>(3 * _nnormal));
+ _vertex_texcoords.resize(static_cast<std::size_t>(2 * _ntexcoord));
+ _face_vertex_indices.resize(static_cast<std::size_t>(3 * _nface));
+}
+
+
+}
diff --git a/VirtualRobot/XML/mujoco/Mesh.h b/VirtualRobot/XML/mujoco/Mesh.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb6d16805f30ce76655894c99ff9bff01e8a45f5
--- /dev/null
+++ b/VirtualRobot/XML/mujoco/Mesh.h
@@ -0,0 +1,175 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package MujocoX::ArmarXObjects::Mesh
+ * @author Rainer Kartmann ( rainer dot kartmann at student dot kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+
+
+namespace VirtualRobot::mujoco
+{
+ class Mesh
+ {
+ public:
+
+ static Mesh fromFile(const std::string& filename);
+
+ public:
+
+ /// Constructor.
+ Mesh();
+
+ /// Read from a file.
+ void read(const std::string& filename);
+ /// Read from an in stream.
+ void read(std::istream& is);
+
+ /// Write to a file.
+ void write(const std::string& filename) const;
+ /// Write to an out stream.
+ void write(std::ostream& os) const;
+
+ int32_t nvertex() const
+ {
+ return _nvertex;
+ }
+ int32_t nnormal() const
+ {
+ return _nnormal;
+ }
+ int32_t ntexcoord() const
+ {
+ return _ntexcoord;
+ }
+ int32_t nface() const
+ {
+ return _nface;
+ }
+
+ std::vector<float>& vertexPositions()
+ {
+ return _vertex_positions;
+ }
+ const std::vector<float>& vertexPositions() const
+ {
+ return _vertex_positions;
+ }
+
+ std::vector<float>& vertexNormals()
+ {
+ return _vertex_normals;
+ }
+ const std::vector<float>& vertexNormals() const
+ {
+ return _vertex_normals;
+ }
+
+ std::vector<float>& vertexTexcoords()
+ {
+ return _vertex_texcoords;
+ }
+ const std::vector<float>& vertexTexcoords() const
+ {
+ return _vertex_texcoords;
+ }
+
+ std::vector<int32_t>& faceVertexIndices()
+ {
+ return _face_vertex_indices;
+ }
+ const std::vector<int32_t>& faceVertexIndices() const
+ {
+ return _face_vertex_indices;
+ }
+
+ Eigen::Vector3f vertexPosition(int32_t i) const;
+ Eigen::Vector3f vertexNormal(int32_t i) const;
+ Eigen::Vector2f vertexTexcoord(int32_t i) const;
+ Eigen::Vector3i faceVertexIndex(int32_t i) const;
+
+ bool hasNormals() const
+ {
+ return _nnormal > 0;
+ }
+ bool hasTexcoords() const
+ {
+ return _ntexcoord > 0;
+ }
+ bool hasFaces() const
+ {
+ return _nface > 0;
+ }
+
+
+ void addVertexPosition(const Eigen::Vector3f& position);
+ void addVertexNormal(const Eigen::Vector3f& normal);
+ void addVertexTexcoord(const Eigen::Vector2f& texcoord);
+ void addFaceVertexIndex(const Eigen::Vector3i& face);
+
+
+ /**
+ * @brief Get the bounding box of vertex positions.
+ * The bounding box is returned as a 3x2 matrix:
+ * min max
+ * ( x_min x_max ) < x limits
+ * ( y_min y_max ) < y limits
+ * ( z_min z_max ) < z limits
+ */
+ Eigen::Matrix<float, 3, 2> getVertexPositionsBoundingBox() const;
+
+ void setTextureFile(const std::string& path);
+ const std::string& getTextureFile() const;
+
+ /**
+ * @brief Split this mesh into num meshes by randomly assigning its
+ * faces to the new meshes.
+ */
+ std::vector<Mesh> splitRandomly(std::size_t num) const;
+
+ void setFilename(const std::string& path);
+ const std::string& getFilename() const;
+
+
+ private:
+
+ /// Resize the vectors to the according number of elements.
+ /// _n* values must be nonnegative.
+ void resizeVectors();
+
+ int32_t _nvertex = 0;
+ int32_t _nnormal = 0;
+ int32_t _ntexcoord = 0;
+ int32_t _nface = 0;
+
+ std::vector<float> _vertex_positions; // 3*nvertex
+ std::vector<float> _vertex_normals; // 3*nnormal
+ std::vector<float> _vertex_texcoords; // 2*ntexcoord
+ std::vector<int32_t> _face_vertex_indices; // 3*nface
+
+ std::string _texture_file;
+ std::string _filename;
+ };
+}
diff --git a/VirtualRobot/XML/mujoco/MeshConverter.cpp b/VirtualRobot/XML/mujoco/MeshConverter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2cd45dcd5e5119b68f40f90ac8ba6d8ce9b10a5a
--- /dev/null
+++ b/VirtualRobot/XML/mujoco/MeshConverter.cpp
@@ -0,0 +1,134 @@
+#include "MeshConverter.h"
+
+#include <VirtualRobot/VirtualRobotChecks.h>
+
+
+namespace VirtualRobot::mujoco
+{
+
+
+VirtualRobot::TriMeshModelPtr MeshConverter::toVirtualRobotPtr(const Mesh& mesh, float scaling)
+{
+ return VirtualRobot::TriMeshModelPtr(
+ new VirtualRobot::TriMeshModel(toVirtualRobot(mesh, scaling)));
+}
+
+VirtualRobot::TriMeshModel MeshConverter::toVirtualRobot(const Mesh& mj, float scaling)
+{
+ VirtualRobot::TriMeshModel vr;
+
+ // copy vertices
+ for (int i = 0; i < mj.nvertex(); ++i)
+ {
+ vr.addVertex(mj.vertexPosition(i) * scaling);
+ }
+
+ // copy normals
+ for (int i = 0; i < mj.nnormal(); ++i)
+ {
+ vr.addNormal(mj.vertexNormal(i));
+ }
+
+ // copy faces
+ for (int i = 0; i < mj.nface(); ++i)
+ {
+ Eigen::Vector3i faceVertexIndex = mj.faceVertexIndex(i);
+
+ // check before casting
+ VR_CHECK_NONNEGATIVE(faceVertexIndex.minCoeff());
+ VR_CHECK_LESS(faceVertexIndex.maxCoeff(), static_cast<int>(vr.vertices.size()));
+
+ Eigen::Matrix<unsigned int, 3, 1> vertexIDs = faceVertexIndex.cast<unsigned int>();
+
+ VirtualRobot::MathTools::TriangleFace triFace;
+
+ // set position indices
+ triFace.set(vertexIDs(0), vertexIDs(1), vertexIDs(2));
+
+ // set normals (if present)
+ if (vertexIDs.maxCoeff() < vr.normals.size())
+ {
+ triFace.setNormal(vertexIDs(0), vertexIDs(1), vertexIDs(2));
+
+ Eigen::Vector3f normal = Eigen::Vector3f::Zero();
+ for (int i = 0; i < vertexIDs.size(); ++i)
+ {
+ normal += vr.normals[vertexIDs(i)];
+ }
+ triFace.normal = normal / vertexIDs.size();
+ }
+ else
+ {
+ // create a normal from the vertices
+ triFace.normal = vr.CreateNormal(vr.vertices[triFace.id1], vr.vertices[triFace.id2],
+ vr.vertices[triFace.id3]);
+ }
+
+ vr.addFace(triFace);
+ }
+
+ return vr;
+}
+
+
+template <typename ValueT>
+static ValueT max3(ValueT a, ValueT b, ValueT c)
+{
+ return std::max(a, std::max(b, c));
+}
+
+Mesh MeshConverter::fromVirtualRobot(const VirtualRobot::TriMeshModel& vr, float scaling)
+{
+ Mesh mj;
+
+ const auto& vertices = vr.vertices;
+ const auto& normals = vr.normals;
+
+ for (const auto& face : vr.faces)
+ {
+ VR_CHECK_EQUAL(mj.nvertex(), mj.nnormal());
+
+ int index = static_cast<int>(mj.nvertex());
+
+ VR_CHECK_LESS_EQUAL(max3(face.id1, face.id2, face.id3), vertices.size());
+ mj.addVertexPosition(vertices.at(face.id1) * scaling);
+ mj.addVertexPosition(vertices.at(face.id2) * scaling);
+ mj.addVertexPosition(vertices.at(face.id3) * scaling);
+
+ if (max3(face.idNormal1, face.idNormal2, face.idNormal3) < normals.size())
+ {
+ mj.addVertexNormal(normals.at(face.idNormal1));
+ mj.addVertexNormal(normals.at(face.idNormal2));
+ mj.addVertexNormal(normals.at(face.idNormal3));
+ }
+ else
+ {
+ // take anything in face.normal, normalize it and use it
+ Eigen::Vector3f normal = face.normal;
+ if (normal.isZero(1e-6f))
+ {
+ VR_WARNING << "No normal information in TriMeshModel";
+ normal = Eigen::Vector3f::UnitX();
+ }
+ normal.normalize();
+
+ // add it 3 times (once for each vertex position)
+ int i = 3;
+ while (i-- > 0) // use convergence operator
+ {
+ mj.addVertexNormal(normal);
+ }
+ }
+
+ mj.addFaceVertexIndex({ index, index + 1, index + 2 });
+ }
+
+ return mj;
+}
+
+Mesh MeshConverter::toMujoco(const VirtualRobot::TriMeshModel& triMeshModel, float scaling)
+{
+ return fromVirtualRobot(triMeshModel, scaling);
+}
+
+}
diff --git a/VirtualRobot/XML/mujoco/MeshConverter.h b/VirtualRobot/XML/mujoco/MeshConverter.h
new file mode 100644
index 0000000000000000000000000000000000000000..d63fac5c2c555e48737a33f947cc30b6d45d2fb0
--- /dev/null
+++ b/VirtualRobot/XML/mujoco/MeshConverter.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+
+#include "Mesh.h"
+
+
+namespace VirtualRobot::mujoco
+{
+
+ class MeshConverter
+ {
+ public:
+
+ // From mujoco::Mesh
+
+ /// Construct a VirtualRobot::TriMeshModel from mujoco::Mesh.
+ static VirtualRobot::TriMeshModel toVirtualRobot(const mujoco::Mesh& mesh,
+ float scaling = 1.0);
+ static VirtualRobot::TriMeshModelPtr toVirtualRobotPtr(const mujoco::Mesh& mesh,
+ float scaling = 1.0);
+
+
+ // To mujoco::Mesh
+
+ /// Construct mujoco::Mesh from a VirtualRobot::TriMeshModel.
+ static mujoco::Mesh fromVirtualRobot(const VirtualRobot::TriMeshModel& triMeshModel,
+ float scaling = 1.0);
+
+ /// Construct mujoco::Mesh from a VirtualRobot::TriMeshModel.
+ static mujoco::Mesh toMujoco(const VirtualRobot::TriMeshModel& triMeshModel,
+ float scaling = 1.0);
+ private:
+
+ /// Private constructor.
+ MeshConverter() = default;
+
+ };
+
+}