From d91eb4992c8ab28a69db579d358592acfad2c432 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Tue, 2 Apr 2019 12:19:34 +0200
Subject: [PATCH] Added MJCF library files
---
VirtualRobot/CMakeLists.txt | 43 +++
VirtualRobot/MJCF/Document.cpp | 96 ++++++
VirtualRobot/MJCF/Document.h | 210 ++++++++++++
VirtualRobot/MJCF/elements/Attribute.cpp | 1 +
VirtualRobot/MJCF/elements/Attribute.h | 296 +++++++++++++++++
VirtualRobot/MJCF/elements/Element.cpp | 2 +
VirtualRobot/MJCF/elements/Element.h | 373 ++++++++++++++++++++++
VirtualRobot/MJCF/elements/actuator.cpp | 84 +++++
VirtualRobot/MJCF/elements/actuator.h | 143 +++++++++
VirtualRobot/MJCF/elements/asset.cpp | 55 ++++
VirtualRobot/MJCF/elements/asset.h | 93 ++++++
VirtualRobot/MJCF/elements/body.cpp | 155 +++++++++
VirtualRobot/MJCF/elements/body.h | 163 ++++++++++
VirtualRobot/MJCF/elements/compiler.cpp | 9 +
VirtualRobot/MJCF/elements/compiler.h | 14 +
VirtualRobot/MJCF/elements/contact.cpp | 49 +++
VirtualRobot/MJCF/elements/contact.h | 62 ++++
VirtualRobot/MJCF/elements/custom.cpp | 6 +
VirtualRobot/MJCF/elements/custom.h | 11 +
VirtualRobot/MJCF/elements/default.cpp | 31 ++
VirtualRobot/MJCF/elements/default.h | 57 ++++
VirtualRobot/MJCF/elements/elements.cpp | 11 +
VirtualRobot/MJCF/elements/elements.h | 88 +++++
VirtualRobot/MJCF/elements/equality.cpp | 48 +++
VirtualRobot/MJCF/elements/equality.h | 96 ++++++
VirtualRobot/MJCF/elements/exceptions.cpp | 28 ++
VirtualRobot/MJCF/elements/exceptions.h | 42 +++
VirtualRobot/MJCF/elements/has_member.hpp | 34 ++
VirtualRobot/MJCF/elements/keyframe.cpp | 9 +
VirtualRobot/MJCF/elements/keyframe.h | 15 +
VirtualRobot/MJCF/elements/mjcf_utils.cpp | 82 +++++
VirtualRobot/MJCF/elements/mjcf_utils.h | 213 ++++++++++++
VirtualRobot/MJCF/elements/option.cpp | 16 +
VirtualRobot/MJCF/elements/option.h | 71 ++++
VirtualRobot/MJCF/elements/sensor.cpp | 9 +
VirtualRobot/MJCF/elements/sensor.h | 14 +
VirtualRobot/MJCF/elements/size.cpp | 8 +
VirtualRobot/MJCF/elements/size.h | 16 +
VirtualRobot/MJCF/elements/statistic.cpp | 9 +
VirtualRobot/MJCF/elements/statistic.h | 15 +
VirtualRobot/MJCF/elements/tendon.cpp | 10 +
VirtualRobot/MJCF/elements/tendon.h | 16 +
VirtualRobot/MJCF/elements/visual.cpp | 9 +
VirtualRobot/MJCF/elements/visual.h | 14 +
44 files changed, 2826 insertions(+)
create mode 100644 VirtualRobot/MJCF/Document.cpp
create mode 100644 VirtualRobot/MJCF/Document.h
create mode 100644 VirtualRobot/MJCF/elements/Attribute.cpp
create mode 100644 VirtualRobot/MJCF/elements/Attribute.h
create mode 100644 VirtualRobot/MJCF/elements/Element.cpp
create mode 100644 VirtualRobot/MJCF/elements/Element.h
create mode 100644 VirtualRobot/MJCF/elements/actuator.cpp
create mode 100644 VirtualRobot/MJCF/elements/actuator.h
create mode 100644 VirtualRobot/MJCF/elements/asset.cpp
create mode 100644 VirtualRobot/MJCF/elements/asset.h
create mode 100644 VirtualRobot/MJCF/elements/body.cpp
create mode 100644 VirtualRobot/MJCF/elements/body.h
create mode 100644 VirtualRobot/MJCF/elements/compiler.cpp
create mode 100644 VirtualRobot/MJCF/elements/compiler.h
create mode 100644 VirtualRobot/MJCF/elements/contact.cpp
create mode 100644 VirtualRobot/MJCF/elements/contact.h
create mode 100644 VirtualRobot/MJCF/elements/custom.cpp
create mode 100644 VirtualRobot/MJCF/elements/custom.h
create mode 100644 VirtualRobot/MJCF/elements/default.cpp
create mode 100644 VirtualRobot/MJCF/elements/default.h
create mode 100644 VirtualRobot/MJCF/elements/elements.cpp
create mode 100644 VirtualRobot/MJCF/elements/elements.h
create mode 100644 VirtualRobot/MJCF/elements/equality.cpp
create mode 100644 VirtualRobot/MJCF/elements/equality.h
create mode 100644 VirtualRobot/MJCF/elements/exceptions.cpp
create mode 100644 VirtualRobot/MJCF/elements/exceptions.h
create mode 100644 VirtualRobot/MJCF/elements/has_member.hpp
create mode 100644 VirtualRobot/MJCF/elements/keyframe.cpp
create mode 100644 VirtualRobot/MJCF/elements/keyframe.h
create mode 100644 VirtualRobot/MJCF/elements/mjcf_utils.cpp
create mode 100644 VirtualRobot/MJCF/elements/mjcf_utils.h
create mode 100644 VirtualRobot/MJCF/elements/option.cpp
create mode 100644 VirtualRobot/MJCF/elements/option.h
create mode 100644 VirtualRobot/MJCF/elements/sensor.cpp
create mode 100644 VirtualRobot/MJCF/elements/sensor.h
create mode 100644 VirtualRobot/MJCF/elements/size.cpp
create mode 100644 VirtualRobot/MJCF/elements/size.h
create mode 100644 VirtualRobot/MJCF/elements/statistic.cpp
create mode 100644 VirtualRobot/MJCF/elements/statistic.h
create mode 100644 VirtualRobot/MJCF/elements/tendon.cpp
create mode 100644 VirtualRobot/MJCF/elements/tendon.h
create mode 100644 VirtualRobot/MJCF/elements/visual.cpp
create mode 100644 VirtualRobot/MJCF/elements/visual.h
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 52d4638a6..22ef15278 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -307,6 +307,27 @@ SET(SOURCES
math/statistics/BoxPlot.cpp
math/statistics/Histogram.cpp
+ MJCF/Document.cpp
+ MJCF/elements/Attribute.cpp
+ MJCF/elements/Element.cpp
+ MJCF/elements/actuator.cpp
+ MJCF/elements/asset.cpp
+ MJCF/elements/body.cpp
+ MJCF/elements/contact.cpp
+ MJCF/elements/custom.cpp
+ MJCF/elements/default.cpp
+ MJCF/elements/elements.cpp
+ MJCF/elements/equality.cpp
+ MJCF/elements/exceptions.cpp
+ MJCF/elements/keyframe.cpp
+ MJCF/elements/mjcf_utils.cpp
+ MJCF/elements/option.cpp
+ MJCF/elements/sensor.cpp
+ MJCF/elements/size.cpp
+ MJCF/elements/statistic.cpp
+ MJCF/elements/tendon.cpp
+ MJCF/elements/visual.cpp
+
Nodes/CameraSensor.cpp
Nodes/CameraSensorFactory.cpp
Nodes/ContactSensor.cpp
@@ -490,6 +511,28 @@ SET(INCLUDES
math/statistics/BoxPlot.h
math/statistics/Histogram.h
+ MJCF/Document.h
+ MJCF/elements/Attribute.h
+ MJCF/elements/Element.h
+ MJCF/elements/actuator.h
+ MJCF/elements/asset.h
+ MJCF/elements/body.h
+ MJCF/elements/contact.h
+ MJCF/elements/custom.h
+ MJCF/elements/default.h
+ MJCF/elements/elements.h
+ MJCF/elements/equality.h
+ MJCF/elements/exceptions.h
+ MJCF/elements/keyframe.h
+ MJCF/elements/mjcf_utils.h
+ MJCF/elements/option.h
+ MJCF/elements/sensor.h
+ MJCF/elements/size.h
+ MJCF/elements/statistic.h
+ MJCF/elements/tendon.h
+ MJCF/elements/visual.h
+ MJCF/elements/has_member.hpp
+
Nodes/CameraSensor.h
Nodes/CameraSensorFactory.h
Nodes/ConditionedLock.h
diff --git a/VirtualRobot/MJCF/Document.cpp b/VirtualRobot/MJCF/Document.cpp
new file mode 100644
index 000000000..e2616200f
--- /dev/null
+++ b/VirtualRobot/MJCF/Document.cpp
@@ -0,0 +1,96 @@
+#include "Document.h"
+
+#include "elements/exceptions.h"
+
+
+using namespace mjcf;
+
+
+Document::Document()
+{
+ // create root element
+ tinyxml2::XMLElement* xml = doc.NewElement(MujocoRoot::tag.c_str());
+ doc.InsertEndChild(xml);
+ root.reset(new MujocoRoot(this, xml));
+}
+
+void Document::loadFile(const std::string& fileName)
+{
+ tinyxml2::XMLError error = doc.LoadFile(fileName.c_str());
+ if (error != tinyxml2::XML_SUCCESS)
+ {
+ throw MjcfIOError(doc.ErrorStr());
+ }
+}
+
+void Document::saveFile(const std::string& fileName)
+{
+ tinyxml2::XMLError error = doc.SaveFile(fileName.c_str());
+ if (error != tinyxml2::XML_SUCCESS)
+ {
+ throw MjcfIOError(doc.ErrorStr());
+ }
+}
+
+void Document::deepCopyFrom(const Document& source)
+{
+ source.doc.DeepCopy(&this->doc);
+}
+
+
+
+std::string Document::getModelName() const
+{
+ if (root->isAttributeSet("model"))
+ {
+ return root->getAttribute("model");
+ }
+ else
+ {
+ return "";
+ }
+}
+void Document::setModelName(const std::string& name)
+{
+ root->setAttribute("model", name);
+}
+
+Include Document::addInclude(const std::string& filename)
+{
+ Include include = root->addChild<Include>();
+ include.file = filename;
+ return include;
+}
+
+void Document::setNewElementClass(const std::string& className, bool excludeBody)
+{
+ this->newElementClass = className;
+ this->newElementClassExcludeBody = excludeBody;
+
+ if (!className.empty() && !default_().hasChild<DefaultClass>("class", className))
+ {
+ default_().addClass(className);
+ }
+}
+
+
+float Document::getFloatCompPrecision() const
+{
+ return floatCompPrecision;
+}
+
+void Document::setFloatCompPrecision(float value)
+{
+ floatCompPrecision = value;
+}
+
+float Document::getDummyMass() const
+{
+ return dummyMass;
+}
+
+void Document::setDummyMass(float value)
+{
+ dummyMass = value;
+}
+
diff --git a/VirtualRobot/MJCF/Document.h b/VirtualRobot/MJCF/Document.h
new file mode 100644
index 000000000..b27de93e9
--- /dev/null
+++ b/VirtualRobot/MJCF/Document.h
@@ -0,0 +1,210 @@
+#pragma once
+
+#include <memory>
+#include <set>
+
+#include <Eigen/Eigen>
+
+#include <VirtualRobot/Util/xml/tinyxml2.h>
+
+#include "elements/elements.h"
+
+
+namespace mjcf
+{
+
+ /**
+ * @brief A MJCF (Mujoco XML) document.
+ */
+ class Document
+ {
+
+ public:
+
+ /// Constructor.
+ Document();
+
+ /// Set the precision for float comparison.
+ float getFloatCompPrecision() const;
+ /// Set the precision for float comparison (used e.g. when comparing
+ /// to zero or identity).
+ void setFloatCompPrecision(float value);
+ /// Get the mass of dummy inertial elements.
+ float getDummyMass() const;
+ /// Set the mass of dummy inertial elements.
+ void setDummyMass(float value);
+
+ /// Load from an MJCF file.
+ void loadFile(const std::string& fileName);
+ /// Save to an MJCF file.
+ void saveFile(const std::string& fileName);
+
+ /// Make a deep copy of source to this.
+ void deepCopyFrom(const Document& source);
+
+
+ std::string getModelName() const;
+ /// Set the name of the Mujoco model.
+ void setModelName(const std::string& name);
+
+
+ // Section elements (children of top-level 'mujoco' element).
+ CompilerSection compiler() { return section<CompilerSection>(); }
+ OptionSection option() { return section<OptionSection>(); }
+ SizeSection size() { return section<SizeSection>(); }
+ VisualSection visual() { return section<VisualSection>(); }
+ StatisticSection statistic() { return section<StatisticSection>(); }
+ DefaultSection default_() { return section<DefaultSection>(); }
+ AssetSection asset() { return section<AssetSection>(); }
+ Worldbody worldbody() { return section<Worldbody>(); }
+ ContactSection contact() { return section<ContactSection>(); }
+ EqualitySection equality() { return section<EqualitySection>(); }
+ TendonSection tendon() { return section<TendonSection>(); }
+ ActuatorSection actuator() { return section<ActuatorSection>(); }
+ SensorSection sensor() { return section<SensorSection>(); }
+ KeyframeSection keyframe() { return section<KeyframeSection>(); }
+
+ Include addInclude(const std::string& filename);
+
+ /**
+ * @brief Set the class attribute of all new applicable elements
+ * (after calling this method). Pass an empty string to disable class attributes.
+ * @param excludeBody If true (default), the class will not be set on
+ * new body elements and its children (inertial, joint, ...).
+ * They should be the childclass attribute of the robot root body.
+ */
+ void setNewElementClass(const std::string& className, bool excludeBody = true);
+
+
+ template <class Derived>
+ friend class Element;
+
+
+ private:
+
+ // METHODS
+
+ /// Add a new element to a parent.
+ /// @param className If not empty, set the class attribute of the new element.
+ /// @param first If true, will be inserted as first, otherweise at end (default)
+ template <class ElementD, class ParentD>
+ ElementD createElement(Element<ParentD> parent, const std::string& className = "", bool first = false);
+
+ /// Return the first child element of parent with the given element name.
+ /// If it does not exist, create it.
+ template <class ElementD, class ParentD>
+ ElementD getOrCreateElement(Element<ParentD>& parent);
+
+ /// Gets the section element (child of root element). If it does not exist, it is created.
+ template <class SectionD>
+ SectionD section();
+
+
+ private:
+
+ // ATTRIBUTES
+
+ /// The document.
+ tinyxml2::XMLDocument doc;
+
+ /// The "mujoco" root element.
+ std::unique_ptr<MujocoRoot> root;
+
+
+ /// Precision when comparing floats (e.g. with zero).
+ float floatCompPrecision = 1e-6f;
+ /// Mass used for dummy inertial elements.
+ float dummyMass = 0.0001f;
+
+ /// The class added to new elements, if applicable.
+ std::string newElementClass = "";
+ /// Indicate whether body elements and their children shall be
+ /// exluded from setting the class attribute.
+ bool newElementClassExcludeBody = true;
+
+
+ };
+
+ using DocumentPtr = std::unique_ptr<Document>;
+
+
+
+#include "elements/has_member.hpp"
+
+ define_has_member(class_);
+
+ template <class ElementD, class ParentD>
+ ElementD Document::createElement(Element<ParentD> parent, const std::string& className, bool first)
+ {
+ ElementD element(this, doc.NewElement(ElementD::Derived::tag.c_str()));
+
+ auto applyNewElementClass = [this]()
+ {
+ bool isSet = !newElementClass.empty();
+ bool hasClass = has_member(ElementD, class_);
+ bool excludeBecauseBody = std::is_same<ParentD, Body>() && newElementClassExcludeBody;
+ // body itself does not have a class attribute
+
+ bool inDefaultClass = std::is_same<ParentD, DefaultClass>();
+
+ return isSet // must not be empty
+ && hasClass // must have class attribute
+ && !excludeBecauseBody // not excluded because of body exclusion
+ && !inDefaultClass; // not part of default class
+ };
+
+
+ if (!className.empty())
+ {
+ element.setAttribute("class", className);
+ }
+ else if (applyNewElementClass())
+ {
+ element.setAttribute("class", newElementClass);
+ }
+
+ if (first)
+ {
+ parent.insertFirstChild(element);
+ }
+ else
+ {
+ parent.insertEndChild(element);
+ }
+ return element;
+ }
+
+// cleanup macros by has_member.hpp
+#undef define_has_member
+#undef has_member
+
+
+ template <class ElementD, class ParentD>
+ ElementD Document::getOrCreateElement(Element<ParentD>& parent)
+ {
+ ElementD element = parent.template firstChild<ElementD>();
+
+ if (!element)
+ {
+ element = createElement<ElementD>(parent);
+ }
+
+ return element;
+ }
+
+
+ template<class SectionT>
+ SectionT Document::section()
+ {
+ return getOrCreateElement<SectionT>(*root);
+ }
+
+ // Implementation of Element::createNewElement, which depends on the
+ // definition of Document.
+ template <class D>
+ template <class ParentD, class ElementD>
+ ElementD Element<D>::createElement(Element<ParentD> parent, const std::string& className)
+ {
+ return _document->createElement<ElementD, ParentD>(parent, className);
+ }
+}
diff --git a/VirtualRobot/MJCF/elements/Attribute.cpp b/VirtualRobot/MJCF/elements/Attribute.cpp
new file mode 100644
index 000000000..0491e59f9
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/Attribute.cpp
@@ -0,0 +1 @@
+#include "Attribute.h"
diff --git a/VirtualRobot/MJCF/elements/Attribute.h b/VirtualRobot/MJCF/elements/Attribute.h
new file mode 100644
index 000000000..252684a48
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/Attribute.h
@@ -0,0 +1,296 @@
+#pragma once
+
+#include "Element.h"
+
+
+namespace mjcf
+{
+
+ /**
+ * @class Base class for an attribute of an Element<Derived>.
+ * Stores a pointer to the owning element.
+ */
+ template <class Derived>
+ class AttributeBase
+ {
+ public:
+
+ AttributeBase() : _owner(nullptr)
+ {}
+
+ /// Constructor.
+ /// @param owner the owning element (Element instance of which this is a member).
+ AttributeBase(Element<Derived>& owner) : _owner(&owner)
+ {}
+
+
+ /* _owner must always point to the element this was constructed with.
+ * We therefore delete copy and move constructors (otherwise, the owner
+ * will be unavailable. In addition, _owner is constant, so it is not
+ * changed by copy or move operators. */
+
+ /// Deleted copy constructor.
+ AttributeBase(const AttributeBase<Derived>& other) = delete;
+ /// Deleted move constructor.
+ AttributeBase(AttributeBase<Derived>&& other) = delete;
+
+
+ protected:
+ /// Get the owning element.
+ Element<Derived>& owner() { return *_owner; }
+ const Element<Derived>& owner() const { return *_owner; }
+
+ private:
+ /// The owning element (Element instance of which this is a member).
+ /// It must not change after construction, and thus is const-qualified.
+ Element<Derived>* const _owner;
+
+ };
+
+
+
+ /**
+ * @class An attribute of an Element<Derived> of type AttrT.
+ */
+ template <class Derived, typename AttrT>
+ class Attribute : public AttributeBase<Derived>
+ {
+ public:
+
+ /// Constructor.
+ Attribute(Element<Derived>& element, const std::string& name)
+ : AttributeBase<Derived>(element), name(name)
+ {}
+
+ /// Get the attribute value (or the default value if the attribute does not exist).
+ virtual AttrT get() const = 0;
+ /// Set the attribute value.
+ void set(const AttrT& value)
+ {
+ VR_ASSERT(this->owner());
+ this->owner().template setAttribute<AttrT>(name, value);
+ }
+
+ /// Conversion operator to AttrT for reading.
+ operator AttrT()
+ {
+ return this->get();
+ }
+
+ /// Copy assignment operator for writing an attribute value.
+ Attribute& operator=(const AttrT& value)
+ {
+ this->set(value);
+ return *this;
+ }
+
+ /// Move assignment operator for writing an attribute value.
+ Attribute& operator=(AttrT&& value)
+ {
+ this->set(value);
+ return *this;
+ }
+
+ /// Indicate whether the attribute is required (implies no default).
+ virtual bool isRequired() const = 0;
+ /// Indicate whether the attribute is optional or has a default.
+ bool isOptional() const { return !isRequired(); }
+ /// Indicate whether the attribute has a default (implies optional).
+ virtual bool hasDefault() const = 0;
+
+
+ protected:
+
+ std::string name; ///< The attribute name.
+
+ };
+
+
+
+ /**
+ * @class An attribute of an Element<Derived> of type AttrT
+ * that is required or optional and has no default value.
+ */
+ template <class Derived, typename AttrT>
+ class AttributeNoDef : public Attribute<Derived, AttrT>
+ {
+ using Base = Attribute<Derived, AttrT>;
+ public:
+
+ /// Construct an required or optional attribute without a default.
+ AttributeNoDef(Element<Derived>& element, const std::string& name, bool required)
+ : Base(element, name), required(required)
+ {}
+
+ AttrT get() const override
+ {
+ VR_ASSERT(this->owner());
+ return this->owner().template getAttribute<AttrT>(this->name);
+ }
+
+ bool isRequired() const override { return required; }
+ bool hasDefault() const override { return false; }
+
+ using Base::operator=;
+
+
+ private:
+
+ bool required; ///< Whether the attribute is optional.
+ };
+
+
+
+ /**
+ * @class An attribute of an Element<Derived> of type AttrT
+ * that is optional and has a default value.
+ */
+ template <class Derived, typename AttrT>
+ class AttributeDef : public Attribute<Derived, AttrT>
+ {
+ using Base = Attribute<Derived, AttrT>;
+
+ public:
+
+ /// Construct an optional attribute with a default.
+ AttributeDef(Element<Derived>& element, const std::string& name, const AttrT& defaultValue)
+ : Base(element, name), defaultValue(defaultValue)
+ {}
+
+ AttrT get() const override
+ {
+ VR_ASSERT(this->owner());
+ return this->owner().template getAttribute<AttrT>(this->name, this->defaultValue);
+ }
+
+ bool isRequired() const override { return false; }
+ bool hasDefault() const override { return true; }
+
+ using Base::operator=;
+
+
+ private:
+
+ AttrT defaultValue; ///< The default value.
+ };
+
+
+// Macro for a requried or optional Attribute in an Element<Derived> of type AttrT
+// with potentially different memberName and attrName
+#define mjcf_AttributeFullNoDef(Derived, AttrT, memberName, attrName, required) \
+ AttributeNoDef<Derived, AttrT> memberName {*this, attrName, required}
+
+// Macro for an optional Attribute in an Element<Derived> of type AttrT
+// with potentially different memberName and attrName.
+#define mjcf_AttributeFullDef(Derived, AttrT, memberName, attrName, defaultValue) \
+ AttributeDef<Derived, AttrT> memberName {*this, attrName, defaultValue}
+
+
+// A requried Attribute in an Element<Derived> of type AttrT.
+#define mjcf_AttributeReq(Derived, AttrT, name) \
+ mjcf_AttributeFullNoDef(Derived, AttrT, name, #name, true)
+
+#define mjcf_AttributeOpt(Derived, AttrT, name) \
+ mjcf_AttributeFullNoDef(Derived, AttrT, name, #name, false)
+
+// An optional Attribute in an Element<Derived> of type AttrT.
+#define mjcf_AttributeDef(Derived, AttrT, name, defaultValue) \
+ mjcf_AttributeFullDef(Derived, AttrT, name, #name, defaultValue)
+
+
+// Attributes with a name that is a keyword (e.g. "class").
+#define mjcf_AttributeKwReq(Derived, AttrT, name) \
+ mjcf_AttributeFullNoDef(Derived, AttrT, name ## _, #name, true)
+
+#define mjcf_AttributeKwOpt(Derived, AttrT, name) \
+ mjcf_AttributeFullNoDef(Derived, AttrT, name ## _, #name, false)
+
+#define mjcf_AttributeKwDef(Derived, AttrT, name, defaultValue) \
+ mjcf_AttributeFullDef(Derived, AttrT, name ## _, #name, defaultValue)
+
+
+// bool shorthands
+#define mjcf_BoolAttributeReq(Derived, name) \
+ mjcf_AttributeReq(Derived, bool, name)
+#define mjcf_BoolAttributeOpt(Derived, name) \
+ mjcf_AttributeOpt(Derived, bool, name)
+#define mjcf_BoolAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, bool, name, defaultValue)
+
+// int shorthands
+#define mjcf_IntAttributeReq(Derived, name) \
+ mjcf_AttributeReq(Derived, int, name)
+#define mjcf_IntAttributeOpt(Derived, name) \
+ mjcf_AttributeOpt(Derived, int, name)
+#define mjcf_IntAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, int, name, defaultValue)
+
+
+// float shorthands
+#define mjcf_FloatAttributeReq(Derived, name) \
+ mjcf_AttributeReq(Derived, float, name)
+#define mjcf_FloatAttributeOpt(Derived, name) \
+ mjcf_AttributeOpt(Derived, float, name)
+#define mjcf_FloatAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, float, name, defaultValue)
+
+// string shorthands
+#define mjcf_StringAttributeReq(Derived, memberName) \
+ mjcf_AttributeReq(Derived, std::string, memberName)
+#define mjcf_StringAttributeOpt(Derived, memberName) \
+ mjcf_AttributeOpt(Derived, std::string, memberName)
+#define mjcf_StringAttributeDef(Derived, memberName, defaultValue) \
+ mjcf_AttributeDef(Derived, std::string, memberName, defaultValue)
+
+#define mjcf_StringAttributeDefEmpty(Derived, memberName, defaultValue) \
+ mjcf_StringAttributeDef(Derived, std::string, memberName, "")
+
+
+// vector shorthands
+
+#define mjcf_Vector2fAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, Eigen::Vector2f, name, defaultValue)
+
+#define mjcf_Vector3fAttributeReq(Derived, name) \
+ mjcf_AttributeReq(Derived, Eigen::Vector3f, name)
+#define mjcf_Vector3fAttributeOpt(Derived, name) \
+ mjcf_AttributeOpt(Derived, Eigen::Vector3f, name)
+#define mjcf_Vector3fAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, Eigen::Vector3f, name, defaultValue)
+
+
+// name
+#define mjcf_NameAttribute(Derived) \
+ mjcf_StringAttributeOpt(Derived, name)
+
+// class
+#define mjcf_ClassAttribute(Derived) \
+ mjcf_AttributeKwReq(Derived, std::string, class)
+
+// group
+#define mjcf_GroupAttribute(Derived) \
+ mjcf_IntAttributeDef(Derived, group, 0)
+
+
+// RGB
+#define mjcf_RgbAttributeDef(Derived, name, defaultValue) \
+ mjcf_Vector3fAttributeDef(Derived, name, defaultValue)
+
+#define mjcf_RgbaAttributeDef(Derived, name, defaultValue) \
+ mjcf_AttributeDef(Derived, Eigen::Vector4f, name, defaultValue)
+
+
+// pos
+#define mjcf_PosAttribute(Derived) \
+ mjcf_Vector3fAttributeDef(Derived, pos, Eigen::Vector3f::Zero())
+
+// quat
+#define mjcf_QuatAttribute(Derived) \
+ mjcf_AttributeDef(Derived, Eigen::Quaternionf, quat, Eigen::Quaternionf::Identity())
+
+// pose = pos + quat
+#define mjcf_PoseAttributes(Derived) \
+ mjcf_PosAttribute(Derived);\
+ mjcf_QuatAttribute(Derived)
+
+}
diff --git a/VirtualRobot/MJCF/elements/Element.cpp b/VirtualRobot/MJCF/elements/Element.cpp
new file mode 100644
index 000000000..114513eaf
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/Element.cpp
@@ -0,0 +1,2 @@
+#include "Element.h"
+
diff --git a/VirtualRobot/MJCF/elements/Element.h b/VirtualRobot/MJCF/elements/Element.h
new file mode 100644
index 000000000..355791e30
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/Element.h
@@ -0,0 +1,373 @@
+#pragma once
+
+#include <VirtualRobot.h>
+#include <VirtualRobot/Util/xml/tinyxml2.h>
+
+#include "exceptions.h"
+#include "mjcf_utils.h"
+
+
+namespace mjcf
+{
+
+ // Forward declaration.
+ class Document;
+
+
+ template <class _Derived>
+ class Element
+ {
+ public:
+
+ using Derived = _Derived;
+
+ template <class OtherDerived>
+ friend class Element;
+
+
+ public:
+
+ // CONSTRUCTORS
+
+ /// Constructor.
+ Element(Document* _document, tinyxml2::XMLElement* elem);
+
+
+ /// Indicate whether this is an element of the given type.
+ template <class OtherT>
+ bool isElement() const { return std::is_same<Derived, typename OtherT::Derived>(); }
+
+
+ // ATTRIBUTES
+
+ /// Indicate whether this element has an attribute with the given name.
+ bool isAttributeSet(const std::string& attrName) const;
+
+ /// Get attribute value.
+ template <typename AttrT = std::string>
+ AttrT getAttribute(const std::string& name) const;
+ /// Get the attribute value, or the default value if it does not exist.
+ template <typename AttrT = std::string>
+ AttrT getAttribute(const std::string& name, const AttrT& defaultValue) const;
+ /// Set the attribute.
+ template <typename AttrT>
+ void setAttribute(const std::string& name, const AttrT& value);
+
+ std::vector<std::string> getSetAttributeNames() const;
+
+
+ // CHILDREN
+
+ /// Indicate whether there is a child of the given type.
+ template <class OtherDerived>
+ bool hasChild() const;
+ /// Indicate whether there is a child for which the given predicate is true.
+ template <class OtherDerived>
+ bool hasChild(std::function<bool(OtherDerived)> predicate) const;
+ /// Indicate whether there is a child with the given attribute value.
+ template <class OtherDerived>
+ bool hasChild(const std::string& attrName, const std::string& attrValue) const;
+
+ /// Get the first child of the given type.
+ template <class OtherDerived>
+ OtherDerived firstChild() const;
+ /// Get the first child of the given type for which the given predicate is true.
+ template <class OtherDerived>
+ OtherDerived firstChild(std::function<bool(OtherDerived)> predicate) const;
+ /// Get the first child of the given type with the given attribute value.
+ template <class OtherDerived>
+ OtherDerived firstChild(const std::string& attrName, const std::string& attrValue) const;
+
+
+ // SIBLINGS
+
+ /// Get the next sibling element of the given type.
+ template <class OtherDerived>
+ OtherDerived nextSiblingElement() const;
+ /// Get the next sibling element of the given type for which the predicate is true.
+ template <class OtherDerived>
+ OtherDerived nextSiblingElement(std::function<bool(OtherDerived)> predicate) const;
+ /// Get the next sibling element of the given type with the given attribute value.
+ template <class OtherDerived>
+ OtherDerived nextSiblingElement(const std::string& attrName, const std::string& attrValue) const;
+
+
+ // INSERTION & DELETION
+
+ /// Add a new child of this with of given type.
+ template <class OtherDerived>
+ OtherDerived addChild(const std::string& className = "");
+
+ /// Get the first child of type OtherDerived. If there is none, add one.
+ template <class OtherDerived>
+ OtherDerived getOrCreateChild();
+
+
+ /// Insert an element at the front.
+ template <class OtherDerived>
+ void insertFirstChild(const Element<OtherDerived>& element);
+ /// Insert an element at the end.
+ template <class OtherDerived>
+ void insertEndChild(const Element<OtherDerived>& element);
+
+ /// Delete given child from this.
+ template <class OtherDerived>
+ void deleteChild(const Element<OtherDerived>& element);
+
+
+ // OPERATORS
+
+ /// Indicate whether this contains a valid element.
+ operator bool() const { return elem != nullptr; }
+
+ template <typename Derived>
+ friend std::ostream& operator<<(std::ostream& os, const Element<Derived>& element);
+
+
+ protected:
+
+ Document& document() { return *_document; }
+ const Document& document() const { return *_document; }
+
+
+ private:
+
+ /// Use document to create a new element of type ElementD with given parent.
+ template <class ParentD, class ElementD>
+ ElementD createElement(Element<ParentD> parent, const std::string& className = "");
+
+ void assertElemValueEqualsTag();
+
+ template <class OtherDerived>
+ std::function<bool(OtherDerived)> predicateWithAttrib(
+ const std::string& attrName, const std::string& attrValue) const;
+
+ Document* _document;
+ tinyxml2::XMLElement* elem;
+
+ };
+
+ template <class D>
+ Element<D>::Element(Document* document, tinyxml2::XMLElement* elem) :
+ _document(document), elem(elem)
+ {
+ assertElemValueEqualsTag();
+ }
+
+ template <class D>
+ bool Element<D>::isAttributeSet(const std::string& attrName) const
+ {
+ return elem->Attribute(attrName.c_str()) != nullptr;
+ }
+
+ template <class D>
+ template <typename AttrT>
+ AttrT Element<D>::getAttribute(const std::string& name) const
+ {
+ const char* attr = elem->Attribute(name.c_str());
+ if (!attr)
+ {
+ throw AttribNotSetError(name);
+ }
+ AttrT value;
+ fromAttr(attr, value);
+ return value;
+ }
+
+ template <class D>
+ template <typename AttrT>
+ AttrT Element<D>::getAttribute(const std::string& name, const AttrT& defaultValue) const
+ {
+ return isAttributeSet(name) ? getAttribute<AttrT>(name) : defaultValue;
+ }
+
+ template <class D>
+ template<typename AttrT>
+ void Element<D>::setAttribute(const std::string& name, const AttrT& value)
+ {
+ std::string attrStr = toAttr(value);
+ elem->SetAttribute(name.c_str(), attrStr.c_str());
+ }
+
+ template <class D>
+ std::vector<std::string> Element<D>::getSetAttributeNames() const
+ {
+ std::vector<std::string> names;
+ for (auto* attr = elem->FirstAttribute(); attr; attr = attr->Next())
+ {
+ names.emplace_back(attr->Name());
+ }
+ return names;
+ }
+
+ template <class D>
+ template <class OtherD>
+ bool Element<D>::hasChild() const
+ {
+ return firstChild<OtherD>();
+ }
+
+ template <class D>
+ template <class OtherD>
+ bool Element<D>::hasChild(std::function<bool(OtherD)> predicate) const
+ {
+ return firstChild<OtherD>(predicate);
+ }
+
+ template <class D>
+ template <class OtherD>
+ bool Element<D>::hasChild(const std::string& attrName, const std::string& attrValue) const
+ {
+ return firstChild<OtherD>(attrName, attrValue);
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::firstChild() const
+ {
+ return OtherD(_document, /*may be null*/ elem->FirstChildElement(OtherD::tag.c_str()));
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::firstChild(std::function<bool(OtherD)> predicate) const
+ {
+ for (OtherD child = firstChild<OtherD>(); child;
+ child = child.template nextSiblingElement<OtherD>())
+ {
+ if (predicate(child))
+ {
+ return child;
+ }
+ }
+ return OtherD(_document, nullptr);
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::firstChild(const std::string& attrName, const std::string& attrValue) const
+ {
+ return firstChild<OtherD>(predicateWithAttrib<OtherD>(attrName, attrValue));
+ }
+
+
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::nextSiblingElement() const
+ {
+ return OtherD(_document, elem->NextSiblingElement(OtherD::tag.c_str()));
+ }
+
+ template <class D>
+ template<class OtherD>
+ OtherD Element<D>::nextSiblingElement(std::function<bool (OtherD)> predicate) const
+ {
+ for (OtherD sibling = nextSiblingElement<OtherD>(); sibling;
+ sibling = sibling.template nextSiblingElement<OtherD>())
+ {
+ if (predicate(sibling))
+ {
+ return sibling;
+ }
+ }
+ return OtherD(_document, nullptr);
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::addChild(const std::string& className)
+ {
+ return createElement<D, OtherD>(*this, className);
+ }
+
+ template <class D>
+ template <class OtherD>
+ OtherD Element<D>::getOrCreateChild()
+ {
+ OtherD child = firstChild<OtherD>();
+ if (!child)
+ {
+ child = addChild<OtherD>();
+ }
+ return child;
+ }
+
+
+ template <class D>
+ template <class OtherD>
+ void Element<D>::insertFirstChild(const Element<OtherD>& element)
+ {
+ elem->InsertFirstChild(element.elem);
+ }
+
+ template <class D>
+ template <class OtherD>
+ void Element<D>::insertEndChild(const Element<OtherD>& element)
+ {
+ elem->InsertEndChild(element.elem);
+ }
+
+ template <class D>
+ template <class OtherD>
+ void Element<D>::deleteChild(const Element<OtherD>& element)
+ {
+ elem->DeleteChild(element.elem);
+ }
+
+ template <class D>
+ template<class OtherD>
+ OtherD Element<D>::nextSiblingElement(const std::string& attrName, const std::string& attrValue) const
+ {
+ return nextSiblingElement<OtherD>(predicateWithAttrib<OtherD>(attrName, attrValue));
+ }
+
+#include <stdexcept>
+
+ template <class D>
+ void Element<D>::assertElemValueEqualsTag()
+ {
+
+ if (elem)
+ {
+ VR_ASSERT_MESSAGE(elem->Value() == Derived::tag,
+ "Element tag '" + std::string(elem->Value()) + "' must be '" + Derived::tag + "'");
+ }
+ }
+
+ template <class D>
+ template <class OtherDerived>
+ std::function<bool(OtherDerived)> Element<D>::predicateWithAttrib(
+ const std::string& attrName, const std::string& attrValue) const
+ {
+ return [&attrName, &attrValue](OtherDerived e)
+ {
+ return e.isAttributeSet(attrName) && e.getAttribute(attrName) == attrValue;
+ };
+ }
+
+ template <class D>
+ std::ostream& operator<<(std::ostream& os, const Element<D>& element)
+ {
+ os << "MJCF Element '" << D::tag << "' (-> " << element.elem << ")";
+ return os;
+ }
+
+
+#define mjcf_ElementDerivedConstructorsBase(Base, Derived) \
+ Derived() : Base<Derived>(nullptr, nullptr) {} \
+ Derived(Document* document, tinyxml2::XMLElement* elem) : Base<Derived>(document, elem) {} \
+ Derived(const Derived& other) : Base<Derived>(other) {} \
+ Derived(Derived&& other) : Base<Derived>(other) {} \
+ Derived& operator=(const Derived& other) \
+ { Base<Derived>::operator=(other); return *this; } \
+ Derived& operator=(Derived&& other) \
+ { Base<Derived>::operator=(other); return *this; }
+
+
+
+#define mjcf_ElementDerivedConstructors(Derived) \
+ mjcf_ElementDerivedConstructorsBase(Element, Derived)
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/actuator.cpp b/VirtualRobot/MJCF/elements/actuator.cpp
new file mode 100644
index 000000000..440cff2d6
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/actuator.cpp
@@ -0,0 +1,84 @@
+#include "actuator.h"
+
+#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string ActuatorGeneral::tag = "general";
+const std::string ActuatorMotor::tag = "motor";
+const std::string ActuatorPosition::tag = "position";
+const std::string ActuatorVelocity::tag = "velocity";
+const std::string ActuatorCylinder::tag = "cylinder";
+const std::string ActuatorMuscle::tag = "muscle";
+
+const std::string ActuatorSection::tag = "actuator";
+
+
+Eigen::VectorXf ActuatorGeneral::dynprmDefault()
+{
+ Eigen::VectorXf v(10);
+ v.setZero();
+ v(0) = 1;
+ return v;
+}
+
+Eigen::VectorXf ActuatorGeneral::gainprmDefault()
+{
+ Eigen::VectorXf v(10);
+ v.setZero();
+ v(0) = 1;
+ return v;
+}
+
+Eigen::VectorXf ActuatorGeneral::biasprmDefault()
+{
+ Eigen::VectorXf v(10);
+ v.setZero();
+ return v;
+}
+
+Eigen::Vector2f ActuatorMuscle::timeconstDefault()
+{
+ return Eigen::Vector2f(0.01, 0.04);
+}
+
+Eigen::Vector2f ActuatorMuscle::rangeDefault()
+{
+ return Eigen::Vector2f(0.75, 1.05);
+}
+
+
+template <class ElementD>
+ElementD addActuator(ActuatorSection& as, const std::string& joint)
+{
+ ElementD motor = as.addChild<ElementD>();
+ motor.joint = joint;
+ return motor;
+}
+
+ActuatorMotor ActuatorSection::addMotor(const std::string& joint)
+{
+ return addActuator<ActuatorMotor>(*this, joint);
+}
+
+ActuatorPosition ActuatorSection::addPosition(const std::string& joint, float kp)
+{
+ ActuatorPosition position = addActuator<ActuatorPosition>(*this, joint);
+ if (kp >= 0)
+ {
+ position.kp = kp;
+ }
+ return position;
+}
+
+ActuatorVelocity ActuatorSection::addVelocity(const std::string& joint, float kv)
+{
+ ActuatorVelocity velocity = addActuator<ActuatorVelocity>(*this, joint);
+ if (kv >= 0)
+ {
+ velocity.kv = kv;
+ }
+ return velocity;
+}
diff --git a/VirtualRobot/MJCF/elements/actuator.h b/VirtualRobot/MJCF/elements/actuator.h
new file mode 100644
index 000000000..048290b48
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/actuator.h
@@ -0,0 +1,143 @@
+#pragma once
+
+#include "Attribute.h"
+
+
+namespace mjcf
+{
+
+static Eigen::Vector6f gearDefault()
+{
+ Eigen::Vector6f v;
+ v << 1, 0, 0, 0, 0, 0;
+ return v;
+}
+
+
+#define mjcf_ActuatorAttributes(Derived) \
+ mjcf_NameAttribute(Derived); \
+ mjcf_ClassAttribute(Derived); \
+ \
+ mjcf_GroupAttribute(Derived); \
+ \
+ mjcf_BoolAttributeDef(Derived, ctrllimited, false); \
+ mjcf_BoolAttributeDef(Derived, forcelimited, false); \
+ \
+ mjcf_Vector2fAttributeDef(Derived, ctrlrange, Eigen::Vector2f::Zero()); \
+ mjcf_Vector2fAttributeDef(Derived, forcerange, Eigen::Vector2f::Zero()); \
+ mjcf_Vector2fAttributeDef(Derived, lengthrange, Eigen::Vector2f::Zero()); \
+ \
+ mjcf_AttributeDef(Derived, Eigen::Vector6f, gear, gearDefault()); \
+ mjcf_FloatAttributeDef(Derived, cranklength, 0); \
+ \
+ mjcf_StringAttributeOpt(Derived, joint); \
+ mjcf_StringAttributeOpt(Derived, jointinparent); \
+ mjcf_StringAttributeOpt(Derived, site); \
+ mjcf_StringAttributeOpt(Derived, tendon); \
+ mjcf_StringAttributeOpt(Derived, cranksite); \
+ mjcf_StringAttributeOpt(Derived, slidersite)
+
+
+struct ActuatorGeneral : public Element<ActuatorGeneral>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorGeneral)
+
+ mjcf_ActuatorAttributes(ActuatorGeneral);
+
+ // dyntype : [none, integrator, filter, muscle, user],
+ mjcf_StringAttributeDef(ActuatorGeneral, dyntype, "none");
+ // gaintype : [fixed, muscle, user], "fixed"
+ mjcf_StringAttributeDef(ActuatorGeneral, gaintype, "fixed");
+ // biastype : [none, affine, muscle, user], "none"
+ mjcf_StringAttributeDef(ActuatorGeneral, biastype, "none");
+
+ // dynprm : real(10), "1 0 ... 0"
+ mjcf_AttributeDef(ActuatorGeneral, Eigen::VectorXf, dynprm, dynprmDefault());
+ // gainprm : real(10), "1 0 ... 0"
+ mjcf_AttributeDef(ActuatorGeneral, Eigen::VectorXf, gainprm, gainprmDefault());
+ // biasprm : real(10), "1 0 ... 0"
+ mjcf_AttributeDef(ActuatorGeneral, Eigen::VectorXf, biasprm, biasprmDefault());
+
+
+private:
+ static Eigen::VectorXf dynprmDefault();
+ static Eigen::VectorXf gainprmDefault();
+ static Eigen::VectorXf biasprmDefault();
+
+};
+
+struct ActuatorMotor : public Element<ActuatorMotor>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorMotor)
+
+ mjcf_ActuatorAttributes(ActuatorMotor);
+};
+
+struct ActuatorPosition : public Element<ActuatorPosition>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorPosition)
+
+ mjcf_ActuatorAttributes(ActuatorPosition);
+ mjcf_FloatAttributeDef(ActuatorPosition, kp, 1);
+};
+
+struct ActuatorVelocity : public Element<ActuatorVelocity>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorVelocity)
+
+ mjcf_ActuatorAttributes(ActuatorVelocity);
+ mjcf_FloatAttributeDef(ActuatorVelocity, kv, 1);
+};
+
+struct ActuatorCylinder : public Element<ActuatorCylinder>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorCylinder)
+
+ mjcf_ActuatorAttributes(ActuatorCylinder);
+ mjcf_FloatAttributeDef(ActuatorCylinder, timeconst, 1);
+ mjcf_FloatAttributeDef(ActuatorCylinder, area, 1);
+ mjcf_FloatAttributeOpt(ActuatorCylinder, diameter);
+ mjcf_Vector3fAttributeDef(ActuatorCylinder, bias, Eigen::Vector3f::Zero());
+};
+
+struct ActuatorMuscle : public Element<ActuatorMuscle>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorMuscle)
+
+ mjcf_ActuatorAttributes(ActuatorMuscle);
+
+ mjcf_Vector2fAttributeDef(ActuatorMuscle, timeconst, timeconstDefault());
+ mjcf_Vector2fAttributeDef(ActuatorMuscle, range, rangeDefault());
+
+ mjcf_FloatAttributeDef(ActuatorMuscle, force, -1);
+ mjcf_FloatAttributeDef(ActuatorMuscle, scale, 200);
+ mjcf_FloatAttributeDef(ActuatorMuscle, lmin, 0.5);
+ mjcf_FloatAttributeDef(ActuatorMuscle, lmax, 1.6f);
+ mjcf_FloatAttributeDef(ActuatorMuscle, vmax, 1.5);
+ mjcf_FloatAttributeDef(ActuatorMuscle, fpmax, 1.3f);
+ mjcf_FloatAttributeDef(ActuatorMuscle, vvmax, 1.2f);
+
+private:
+ static Eigen::Vector2f timeconstDefault();
+ static Eigen::Vector2f rangeDefault();
+};
+
+
+struct ActuatorSection : public Element<ActuatorSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ActuatorSection)
+
+ ActuatorMotor addMotor(const std::string& jointName);
+ ActuatorPosition addPosition(const std::string& joint, float kp = -1);
+ ActuatorVelocity addVelocity(const std::string& joint, float kv = -1);
+
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/asset.cpp b/VirtualRobot/MJCF/elements/asset.cpp
new file mode 100644
index 000000000..7774133ad
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/asset.cpp
@@ -0,0 +1,55 @@
+#include "asset.h"
+
+#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string Texture::tag = "texture";
+const std::string Material::tag = "material";
+const std::string Mesh::tag = "mesh";
+const std::string AssetSection::tag = "asset";
+
+
+Texture AssetSection::addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2)
+{
+ Texture texSkybox = addChild<Texture>();
+
+ texSkybox.type = "skybox";
+ texSkybox.builtin = "gradient";
+ texSkybox.width = 128;
+ texSkybox.height = 128;
+ texSkybox.rgb1 = rgb1;
+ texSkybox.rgb2 = rgb2;
+
+ return texSkybox;
+}
+
+Texture AssetSection::addTextureFile(const std::string& name, const std::string& file,
+ bool vflip, bool hflip)
+{
+ Texture texture = addChild<Texture>();
+ texture.name = name;
+ texture.file = file;
+ texture.type = "2d";
+ texture.vflip = vflip;
+ texture.hflip = hflip;
+ return texture;
+}
+
+Mesh AssetSection::addMesh(const std::string& name, const std::string& file)
+{
+ Mesh mesh = addChild<Mesh>();
+ mesh.name = name;
+ mesh.file = file;
+ return mesh;
+}
+
+Material AssetSection::addMaterial(const std::string& name, const std::string& textureName)
+{
+ Material material = addChild<Material>();
+ material.name = name;
+ material.texture = textureName;
+ return material;
+}
diff --git a/VirtualRobot/MJCF/elements/asset.h b/VirtualRobot/MJCF/elements/asset.h
new file mode 100644
index 000000000..bd9d25bc2
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/asset.h
@@ -0,0 +1,93 @@
+#pragma once
+
+#include "Attribute.h"
+
+
+namespace mjcf
+{
+
+struct Texture : public Element<Texture>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Texture)
+
+ mjcf_NameAttribute(Texture);
+ mjcf_StringAttributeDef(Texture, type, "cube");
+ mjcf_StringAttributeOpt(Texture, file);
+
+ mjcf_AttributeDef(Texture, Eigen::Vector2i, gridsize, Eigen::Vector2i::Ones());
+ mjcf_StringAttributeDef(Texture, gridlayout, "............");
+
+ mjcf_StringAttributeDef(Texture, builtin, "none");
+ mjcf_StringAttributeDef(Texture, mark, "none");
+ mjcf_RgbAttributeDef(Texture, markrgb, Eigen::Vector3f::Zero());
+ mjcf_FloatAttributeDef(Texture, random, 0.01f);
+
+ mjcf_IntAttributeDef(Texture, width, 0);
+ mjcf_IntAttributeDef(Texture, height, 0);
+
+ mjcf_RgbAttributeDef(Texture, rgb1, 0.8f * Eigen::Vector3f::Ones());
+ mjcf_RgbAttributeDef(Texture, rgb2, 0.5f * Eigen::Vector3f::Ones());
+
+ mjcf_BoolAttributeDef(Texture, hflip, false);
+ mjcf_BoolAttributeDef(Texture, vflip, true);
+};
+
+
+struct Mesh : public Element<Mesh>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Mesh)
+
+ mjcf_NameAttribute(Mesh);
+ mjcf_ClassAttribute(Mesh);
+
+ mjcf_StringAttributeOpt(Mesh, file);
+
+ mjcf_Vector3fAttributeDef(Mesh, scale, Eigen::Vector3f::Ones());
+ mjcf_BoolAttributeDef(Mesh, smoothnormal, false);
+
+ mjcf_Vector3fAttributeDef(Mesh, refpos, Eigen::Vector3f::Ones());
+ mjcf_AttributeDef(Mesh, Eigen::Quaternionf, refquat, Eigen::Quaternionf::Identity());
+};
+
+
+struct Material : public Element<Material>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Material)
+
+ mjcf_NameAttribute(Material);
+ mjcf_ClassAttribute(Material);
+
+ mjcf_StringAttributeOpt(Material, texture);
+
+ mjcf_AttributeDef(Material, Eigen::Vector2f, texrepeat, Eigen::Vector2f::Ones());
+ mjcf_BoolAttributeDef(Material, texuniform, false);
+
+ mjcf_FloatAttributeDef(Material, emission, 0);
+ mjcf_FloatAttributeDef(Material, specular, 0);
+ mjcf_FloatAttributeDef(Material, shininess, 0);
+ mjcf_FloatAttributeDef(Material, reflectance, 0);
+ mjcf_RgbaAttributeDef(Material, rgba, Eigen::Vector4f::Ones());
+};
+
+
+
+struct AssetSection : public Element<AssetSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(AssetSection)
+
+ Texture addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2);
+
+ Texture addTextureFile(const std::string& name, const std::string& file,
+ bool vflip = false, bool hflip = false);
+
+ /// Add a mesh asset with a name and a file.
+ Mesh addMesh(const std::string& name, const std::string& file);
+
+ Material addMaterial(const std::string& name, const std::string& textureName);
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/body.cpp b/VirtualRobot/MJCF/elements/body.cpp
new file mode 100644
index 000000000..e455072b9
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/body.cpp
@@ -0,0 +1,155 @@
+#include "body.h"
+
+#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string Body::tag = "body";
+const std::string Geom::tag = "geom";
+const std::string Inertial::tag = "inertial";
+const std::string Joint::tag = "joint";
+const std::string FreeJoint::tag = "freejoint";
+const std::string Worldbody::tag = "worldbody";
+
+
+void Inertial::inertiaFromMatrix(const Eigen::Matrix3f& matrix)
+{
+ if (matrix.isDiagonal(1e-9f))
+ {
+ this->diaginertia = matrix.diagonal();
+ }
+ else
+ {
+ /* Full inertia matrix M. Since M is 3-by-3 and symmetric,
+ * it is specified using only 6 numbers in the following order:
+ * M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3).
+ * -- http://www.mujoco.org/book/XMLreference.html#inertial
+ */
+
+ Eigen::Vector6f inertia;
+ inertia << matrix(0, 0), matrix(1, 1), matrix(2, 2),
+ matrix(0, 1), matrix(0, 2), matrix(1, 2);
+ this->fullinertia = inertia;
+ }
+}
+
+bool Body::hasMass()
+{
+ return hasChild<Geom>() || hasChild<Inertial>();
+}
+
+Body Body::addBody(const std::string& name)
+{
+ Body body = addChild<Body>();
+
+ if (!name.empty())
+ {
+ body.name = name;
+ }
+
+ return body;
+}
+
+Inertial Body::addInertial()
+{
+ return addChild<Inertial>();
+}
+
+Inertial Body::addInertial(const Eigen::Vector3f& pos, float mass, const Eigen::Matrix3f& matrix)
+{
+ Inertial inertial = addInertial();
+
+ inertial.pos = pos;
+ inertial.mass = mass;
+
+ if (matrix.isDiagonal(document().getFloatCompPrecision()))
+ {
+ inertial.diaginertia = matrix.diagonal();
+ }
+ else
+ {
+ /* from mujoco xml reference:
+ * "Full inertia matrix M. Since M is 3-by-3 and symmetric, it is
+ * specified using only 6 numbers in the following order:
+ * M(1,1), M(2,2), M(3,3), M(1,2), M(1,3), M(2,3)."*/
+ Eigen::Vector6f inertia;
+ inertia << matrix(0, 0), matrix(1, 1), matrix(2, 2),
+ matrix(0, 1), matrix(0, 2), matrix(1, 2);
+ inertial.fullinertia = inertia;
+ }
+
+ return inertial;
+}
+
+Inertial Body::addDummyInertial()
+{
+ Inertial inertial = addInertial();
+
+ inertial.pos = Eigen::Vector3f(0, 0, 0);
+ inertial.mass = document().getDummyMass();
+ inertial.diaginertia = Eigen::Vector3f::Ones();
+
+ return inertial;
+}
+
+Joint Body::addJoint()
+{
+ return addChild<Joint>();
+}
+
+FreeJoint Body::addFreeJoint()
+{
+ return addChild<FreeJoint>();
+
+}
+
+Geom Body::addGeom(const std::string& type)
+{
+ Geom geom = addChild<Geom>();
+
+ geom.type = type;
+
+ return geom;
+}
+
+Geom Body::addGeomMesh(const std::string& meshName, const std::string& materialName)
+{
+ Geom geom = addGeom("mesh");
+
+ geom.mesh = meshName;
+ if (!materialName.empty())
+ {
+ geom.material = materialName;
+ }
+
+ return geom;
+}
+
+
+Body Worldbody::addMocapBody(const std::string& name, float geomSize)
+{
+ Body mocap = addChild<Body>();
+ mocap.name = name;
+ mocap.mocap = true;
+ // add geom for visualization
+
+ Geom geom = mocap.addChild<Geom>();
+ geom.type = "box";
+ geom.size = geomSize * Eigen::Vector3f::Ones();
+
+ return mocap;
+}
+
+Body Worldbody::addBody(const std::string& name)
+{
+ Body body = addChild<Body>();
+
+ if (!name.empty())
+ {
+ body.name = name;
+ }
+
+ return body;
+}
diff --git a/VirtualRobot/MJCF/elements/body.h b/VirtualRobot/MJCF/elements/body.h
new file mode 100644
index 000000000..04fe63286
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/body.h
@@ -0,0 +1,163 @@
+#pragma once
+
+#include "Attribute.h"
+
+
+namespace mjcf
+{
+
+
+struct Inertial : public Element<Inertial>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Inertial)
+
+ mjcf_PoseAttributes(Inertial);
+
+ mjcf_AttributeReq(Inertial, float, mass);
+
+ mjcf_AttributeOpt(Inertial, Eigen::Vector3f, diaginertia);
+ mjcf_AttributeOpt(Inertial, Eigen::Vector6f, fullinertia);
+
+ /// Sets diaginertia or fullinertia (as adequate) from the given matrix.
+ void inertiaFromMatrix(const Eigen::Matrix3f& matrix);
+};
+
+
+struct Joint : public Element<Joint>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Joint)
+
+ void setAxis(const Eigen::Vector3f& axis);
+
+ mjcf_NameAttribute(Joint);
+ mjcf_ClassAttribute(Joint);
+
+ // free, ball, slide, hinge
+ mjcf_StringAttributeDef(Joint, type, "hinge");
+ mjcf_IntAttributeDef(Joint, group, 0);
+
+ mjcf_PosAttribute(Joint);
+ mjcf_Vector3fAttributeDef(Joint, axis, Eigen::Vector3f(0, 0, 1));
+
+ mjcf_AttributeDef(Joint, Eigen::Vector2f, springdamper, Eigen::Vector2f::Zero());
+ mjcf_FloatAttributeDef(Joint, stiffness, 0);
+
+ mjcf_BoolAttributeDef(Joint, limited, false);
+ mjcf_AttributeDef(Joint, Eigen::Vector2f, range, Eigen::Vector2f::Zero());
+
+ mjcf_FloatAttributeDef(Joint, margin, 0);
+
+ mjcf_FloatAttributeDef(Joint, ref, 0);
+ mjcf_FloatAttributeDef(Joint, springref, 0);
+
+ mjcf_FloatAttributeDef(Joint, armature, 0);
+ mjcf_FloatAttributeDef(Joint, damping, 0);
+ mjcf_FloatAttributeDef(Joint, frictionloss, 0);
+};
+
+
+struct FreeJoint : public Element<FreeJoint>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(FreeJoint)
+
+ mjcf_NameAttribute(FreeJoint);
+ mjcf_IntAttributeDef(FreeJoint, group, 0);
+};
+
+
+struct Geom : public Element<Geom>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Geom)
+
+ mjcf_NameAttribute(Geom);
+ mjcf_ClassAttribute(Geom);
+
+ // plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh
+ mjcf_StringAttributeDef(Geom, type, "sphere");
+
+ mjcf_IntAttributeDef(Geom, contype, 1);
+ mjcf_IntAttributeDef(Geom, conaffinity, 1);
+ mjcf_IntAttributeDef(Geom, condim, 3);
+
+ mjcf_IntAttributeDef(Geom, group, 0);
+
+ mjcf_Vector3fAttributeDef(Geom, size, Eigen::Vector3f::Zero());
+ mjcf_StringAttributeOpt(Geom, material);
+ mjcf_RgbaAttributeDef(Geom, rgba, Eigen::Vector4f(.5, .5, .5, 1));
+
+ mjcf_Vector3fAttributeDef(Geom, friction, Eigen::Vector3f(1, 0.005f, 0.0001f));
+
+ mjcf_FloatAttributeOpt(Geom, mass);
+ mjcf_FloatAttributeDef(Geom, density, 1000);
+
+ mjcf_FloatAttributeDef(Geom, solmix, 1);
+
+ mjcf_FloatAttributeDef(Geom, marin, 0);
+ mjcf_FloatAttributeDef(Geom, gap, 0);
+
+ mjcf_AttributeOpt(Geom, Eigen::Vector6f, fromto);
+
+ mjcf_PoseAttributes(Geom);
+
+ mjcf_StringAttributeOpt(Geom, hfield);
+ mjcf_StringAttributeOpt(Geom, mesh);
+
+ mjcf_FloatAttributeDef(Geom, fitscale, 1);
+};
+
+
+struct Body : public Element<Body>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Body)
+
+ bool hasMass();
+
+ /// Add a child body element.
+ Body addBody(const std::string& name = "");
+
+ /// Add an inertial element.
+ Inertial addInertial();
+ /// Add an inertial element with given settings.
+ Inertial addInertial(const Eigen::Vector3f& pos, float mass, const Eigen::Matrix3f& matrix);
+ /// Add a dummy inertial element with small mass and identity inertia matrix.
+ Inertial addDummyInertial();
+
+
+ /// Add a joint element to a body.
+ Joint addJoint();
+ /// Add a free joint element to a body.
+ FreeJoint addFreeJoint();
+
+ /// Add a geom to a body, referencing a mesh.
+ Geom addGeom(const std::string& type);
+ /// Add a mesh geom with optional material (for texture).
+ Geom addGeomMesh(const std::string& meshName, const std::string& materialName = "");
+
+
+ mjcf_NameAttribute(Body);
+ mjcf_StringAttributeOpt(Body, childclass);
+
+ mjcf_BoolAttributeDef(Body, mocap, false);
+
+ mjcf_PoseAttributes(Body);
+};
+
+struct Worldbody : public Element<Worldbody>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Worldbody)
+
+ /// Add a body element.
+ Body addBody(const std::string& name = "");
+
+ /// Add a mocap body with the given name to the worldbody.
+ Body addMocapBody(const std::string& name, float geomSize);
+
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/compiler.cpp b/VirtualRobot/MJCF/elements/compiler.cpp
new file mode 100644
index 000000000..cfa52011b
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/compiler.cpp
@@ -0,0 +1,9 @@
+#include "compiler.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string CompilerSection::tag = "compiler";
diff --git a/VirtualRobot/MJCF/elements/compiler.h b/VirtualRobot/MJCF/elements/compiler.h
new file mode 100644
index 000000000..054929963
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/compiler.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "Attribute.h"
+
+namespace mjcf
+{
+
+struct CompilerSection : public Element<CompilerSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(CompilerSection)
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/contact.cpp b/VirtualRobot/MJCF/elements/contact.cpp
new file mode 100644
index 000000000..e60977b27
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/contact.cpp
@@ -0,0 +1,49 @@
+#include "contact.h"
+
+#include "../Document.h"
+
+using namespace mjcf;
+
+
+const std::string ContactPair::tag = "pair";
+const std::string ContactExclude::tag = "exclude";
+const std::string ContactSection::tag = "contact";
+
+
+Eigen::Vector5f ContactPair::condimDefault()
+{
+ Eigen::Vector5f v;
+ v << 1, 1, 0.005f, 0.0001f, 0.0001f;
+ return v;
+}
+
+ContactPair ContactSection::addPair(const Geom& geom1, const Geom& geom2)
+{
+ return addPair(geom1.name.get(), geom2.name.get());
+}
+
+ContactPair ContactSection::addPair(const std::string& geom1Name, const std::string& geom2Name)
+{
+ ContactPair pair = addChild<ContactPair>();
+
+ pair.geom1 = geom1Name;
+ pair.geom2 = geom2Name;
+
+ return pair;
+}
+
+
+ContactExclude ContactSection::addExclude(const Body& body1, const Body& body2)
+{
+ return addExclude(body1.name.get(), body2.name.get());
+}
+
+ContactExclude ContactSection::addExclude(const std::string& body1Name, const std::string& body2Name)
+{
+ ContactExclude exclude = addChild<ContactExclude>();
+
+ exclude.body1 = body1Name;
+ exclude.body2 = body2Name;
+
+ return exclude;
+}
diff --git a/VirtualRobot/MJCF/elements/contact.h b/VirtualRobot/MJCF/elements/contact.h
new file mode 100644
index 000000000..f5bbe7bfb
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/contact.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include "Attribute.h"
+#include "body.h"
+
+
+namespace mjcf
+{
+
+
+struct ContactPair : public Element<ContactPair>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ContactPair)
+
+ mjcf_NameAttribute(ContactPair);
+ mjcf_ClassAttribute(ContactPair);
+
+ mjcf_StringAttributeReq(ContactPair, geom1);
+ mjcf_StringAttributeReq(ContactPair, geom2);
+
+ mjcf_IntAttributeDef(ContactPair, condim, 3);
+
+ mjcf_AttributeDef(ContactPair, Eigen::Vector5f, friction, condimDefault());
+
+ mjcf_FloatAttributeDef(ContactPair, margin, 0);
+ mjcf_FloatAttributeDef(ContactPair, gap, 0);
+
+private:
+ static Eigen::Vector5f condimDefault();
+};
+
+
+struct ContactExclude : public Element<ContactExclude>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ContactExclude)
+
+ mjcf_NameAttribute(ContactExclude);
+ mjcf_StringAttributeReq(ContactExclude, body1);
+ mjcf_StringAttributeReq(ContactExclude, body2);
+};
+
+
+struct ContactSection : public Element<ContactSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(ContactSection)
+
+ /// Add a conact/pair element between the given geoms.
+ ContactPair addPair(const Geom& geom1, const Geom& geom2);
+ /// Add a conact/pair element between the given geoms.
+ ContactPair addPair(const std::string& geom1Name, const std::string& geom2Name);
+
+ /// Add a conact/exclude element between the given bodies.
+ ContactExclude addExclude(const Body& body1, const Body& body2);
+ /// Add a conact/exclude element between the given bodies.
+ ContactExclude addExclude(const std::string& body1Name, const std::string& body2Name);
+};
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/custom.cpp b/VirtualRobot/MJCF/elements/custom.cpp
new file mode 100644
index 000000000..4f2655738
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/custom.cpp
@@ -0,0 +1,6 @@
+#include "compiler.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
diff --git a/VirtualRobot/MJCF/elements/custom.h b/VirtualRobot/MJCF/elements/custom.h
new file mode 100644
index 000000000..56d302f89
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/custom.h
@@ -0,0 +1,11 @@
+#pragma once
+
+#include "Attribute.h"
+
+namespace mjcf
+{
+
+
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/default.cpp b/VirtualRobot/MJCF/elements/default.cpp
new file mode 100644
index 000000000..cfc82ab53
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/default.cpp
@@ -0,0 +1,31 @@
+#include "default.h"
+
+#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string DefaultClass::tag = "default";
+const std::string DefaultSection::tag = "default";
+
+
+bool DefaultSection::hasClass(const std::string& className)
+{
+ return !hasChild<mjcf::DefaultClass>("class", className);
+}
+
+DefaultClass DefaultSection::getClass(const std::string& className)
+{
+ DefaultClass def = firstChild<DefaultClass>("class", className);
+ if (!def)
+ {
+ def = addClass(className);
+ }
+ return def ;
+}
+
+DefaultClass DefaultSection::addClass(const std::string& className)
+{
+ return addChild<DefaultClass>(className);
+}
diff --git a/VirtualRobot/MJCF/elements/default.h b/VirtualRobot/MJCF/elements/default.h
new file mode 100644
index 000000000..826c17017
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/default.h
@@ -0,0 +1,57 @@
+#pragma once
+
+#include "Attribute.h"
+
+
+namespace mjcf
+{
+
+
+struct DefaultClass : public Element<DefaultClass>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(DefaultClass)
+
+ mjcf_ClassAttribute(DefaultClass);
+
+ template <class ElementD>
+ ElementD getElement();
+
+ template <class ElementD, typename AttrT>
+ ElementD setElementAttr(const std::string& attrName, const AttrT& attrValue);
+};
+
+
+struct DefaultSection : public Element<DefaultSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(DefaultSection)
+
+ /// Indicate whether there is a default class with the given name.
+ bool hasClass(const std::string& className);
+
+ /// Get the default class for given class name. If it does not exist, add it.
+ DefaultClass getClass(const std::string& className);
+
+ /// Add a new default class with given class name.
+ DefaultClass addClass(const std::string& className);
+};
+
+
+
+template<class ElementD>
+ElementD DefaultClass::getElement()
+{
+ return getOrCreateChild<ElementD>();
+}
+
+template<class ElementD, typename AttrT>
+ElementD DefaultClass::setElementAttr(const std::string& attrName, const AttrT& attrValue)
+{
+ ElementD element = getElement<ElementD>();
+ element.setAttribute(attrName, attrValue);
+ return element;
+}
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/elements.cpp b/VirtualRobot/MJCF/elements/elements.cpp
new file mode 100644
index 000000000..b87609cd3
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/elements.cpp
@@ -0,0 +1,11 @@
+#include "elements.h"
+
+using namespace mjcf;
+
+
+const std::string MujocoRoot::tag = "mujoco";
+const std::string Include::tag = "include";
+
+
+
+
diff --git a/VirtualRobot/MJCF/elements/elements.h b/VirtualRobot/MJCF/elements/elements.h
new file mode 100644
index 000000000..6a57c0c19
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/elements.h
@@ -0,0 +1,88 @@
+#pragma once
+
+#include "actuator.h"
+#include "asset.h"
+#include "body.h"
+#include "compiler.h"
+#include "contact.h"
+#include "default.h"
+#include "equality.h"
+#include "keyframe.h"
+#include "option.h"
+#include "sensor.h"
+#include "size.h"
+#include "statistic.h"
+#include "tendon.h"
+#include "visual.h"
+
+
+namespace mjcf
+{
+
+
+/// @see http://www.mujoco.org/book/XMLreference.html#mujoco
+struct MujocoRoot : public Element<MujocoRoot>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(MujocoRoot)
+
+ mjcf_StringAttributeDef(MujocoRoot, model, "MuJoCo Model");
+};
+
+
+/// @see http://www.mujoco.org/book/XMLreference.html#include
+struct Include : public Element<Include>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(Include)
+
+ mjcf_StringAttributeReq(Include, file);
+};
+
+
+}
+
+
+#undef mjcf_AttributeFullNoDef
+#undef mjcf_AttributeFullDef
+
+#undef mjcf_AttributeReq
+#undef mjcf_AttributeOpt
+#undef mjcf_AttributeDef
+
+#undef mjcf_AttributeKwReq
+#undef mjcf_AttributeKwOpt
+#undef mjcf_AttributeKwDef
+
+#undef mjcf_BoolAttributeReq
+#undef mjcf_BoolAttributeOpt
+#undef mjcf_BoolAttributeDef
+
+#undef mjcf_IntAttributeReq
+#undef mjcf_IntAttributeOpt
+#undef mjcf_IntAttributeDef
+
+#undef mjcf_FloatAttributeReq
+#undef mjcf_FloatAttributeOpt
+#undef mjcf_FloatAttributeDef
+
+#undef mjcf_StringAttributeReq
+#undef mjcf_StringAttributeOpt
+#undef mjcf_StringAttributeDef
+#undef mjcf_StringAttributeDefEmpty
+
+#undef mjcf_Vector2fAttributeDef
+#undef mjcf_Vector3fAttributeReq
+#undef mjcf_Vector3fAttributeOpt
+#undef mjcf_Vector3fAttributeDef
+
+#undef mjcf_NameAttribute
+#undef mjcf_ClassAttribute
+#undef mjcf_GroupAttribute
+
+#undef mjcf_RgbAttributeDef
+#undef mjcf_RgbaAttributeDef
+
+#undef mjcf_PosAttribute
+#undef mjcf_QuatAttribute
+#undef mjcf_PoseAttributes
diff --git a/VirtualRobot/MJCF/elements/equality.cpp b/VirtualRobot/MJCF/elements/equality.cpp
new file mode 100644
index 000000000..aedc14c37
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/equality.cpp
@@ -0,0 +1,48 @@
+#include "equality.h"
+
+#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string EqualityConnect::tag = "connect";
+const std::string EqualityWeld::tag = "weld";
+const std::string EqualityJoint::tag = "joint";
+const std::string EqualityTendon::tag = "tendon";
+const std::string EqualityDistance::tag = "distance";
+const std::string EqualitySection::tag = "equality";
+
+
+Eigen::Vector7f EqualityWeld::relposeDefault()
+{
+ Eigen::Vector7f v;
+ v << 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f;
+ return v;
+}
+
+
+Eigen::Vector5f EqualityJoint::polycoefDefault()
+{
+ Eigen::Vector5f v;
+ v << 0, 1, 0, 0, 0;
+ return v;
+}
+
+Eigen::Vector5f EqualityTendon::polycoefDefault()
+{
+ Eigen::Vector5f v;
+ v << 0, 1, 0, 0, 0;
+ return v;
+}
+
+EqualityWeld EqualitySection::addWeld(const std::string& name, const std::string& body1, const std::string& body2)
+{
+ EqualityWeld weld = addChild<EqualityWeld>();
+
+ weld.name = name;
+ weld.body1 = body1;
+ weld.body2 = body2;
+
+ return weld;
+}
diff --git a/VirtualRobot/MJCF/elements/equality.h b/VirtualRobot/MJCF/elements/equality.h
new file mode 100644
index 000000000..1550a50cf
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/equality.h
@@ -0,0 +1,96 @@
+#pragma once
+
+#include "Attribute.h"
+
+
+namespace mjcf
+{
+
+#define mjcf_EqualityAttribs(Derived, entityName) \
+ mjcf_NameAttribute(Derived); \
+ mjcf_ClassAttribute(Derived); \
+ mjcf_BoolAttributeDef(Derived, active, true); \
+ mjcf_StringAttributeReq(Derived, entityName ## 1); \
+ mjcf_StringAttributeOpt(Derived, entityName ## 2)
+
+
+
+struct EqualityConnect : public Element<EqualityConnect>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityConnect)
+
+ mjcf_EqualityAttribs(EqualityConnect, body);
+
+ mjcf_Vector3fAttributeReq(EqualityConnect, anchor);
+};
+
+
+struct EqualityWeld : public Element<EqualityWeld>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityWeld)
+
+ mjcf_EqualityAttribs(EqualityWeld, body);
+
+ mjcf_AttributeDef(EqualityWeld, Eigen::Vector7f, relpose, relposeDefault());
+
+private:
+ static Eigen::Vector7f relposeDefault();
+
+};
+
+
+struct EqualityJoint : public Element<EqualityJoint>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityJoint)
+
+ mjcf_EqualityAttribs(EqualityJoint, joint);
+
+ mjcf_AttributeDef(EqualityJoint, Eigen::Vector5f, polycoef, polycoefDefault());
+
+private:
+ static Eigen::Vector5f polycoefDefault();
+
+};
+
+
+struct EqualityTendon : public Element<EqualityTendon>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityTendon)
+
+ mjcf_EqualityAttribs(EqualityTendon, tendon);
+
+ mjcf_AttributeDef(EqualityTendon, Eigen::Vector5f, polycoef, polycoefDefault());
+
+private:
+ static Eigen::Vector5f polycoefDefault();
+
+};
+
+struct EqualityDistance : public Element<EqualityDistance>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualityDistance)
+
+ mjcf_EqualityAttribs(EqualityDistance, geom);
+
+ mjcf_FloatAttributeDef(EqualityDistance, distance, 0);
+};
+
+
+struct EqualitySection : public Element<EqualitySection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(EqualitySection)
+
+ EqualityWeld addWeld(const std::string& name, const std::string& body1,
+ const std::string& body2);
+
+};
+
+
+#undef mjcf_EqualityAttribs
+}
diff --git a/VirtualRobot/MJCF/elements/exceptions.cpp b/VirtualRobot/MJCF/elements/exceptions.cpp
new file mode 100644
index 000000000..dc7ccbfdb
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/exceptions.cpp
@@ -0,0 +1,28 @@
+#include "exceptions.h"
+
+#include <typeinfo>
+
+
+namespace mjcf
+{
+
+MjcfError::MjcfError(const std::string& message) : std::runtime_error(message)
+{}
+
+MjcfIOError::MjcfIOError(const std::string& message) : MjcfError (message)
+{}
+
+AttribNotSetError::AttribNotSetError(const std::string& name) :
+ MjcfError("Attrib '" + name + "' (without default) not set.")
+{}
+
+ParseAttributeError::ParseAttributeError(
+ const std::string& source,
+ const std::type_info& targetType,
+ const std::string& reason) :
+ MjcfError("Could not parse attribute string '" + source + " to " + targetType.name() + ".\n"
+ "Reason: " + reason)
+{}
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/exceptions.h b/VirtualRobot/MJCF/elements/exceptions.h
new file mode 100644
index 000000000..93b207f3d
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/exceptions.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <stdexcept>
+
+namespace mjcf
+{
+
+ class MjcfError : public std::runtime_error
+ {
+ public:
+ MjcfError(const std::string& message);
+ };
+
+
+ /// Indicates an error in load/saveFile.
+ class MjcfIOError : public MjcfError
+ {
+ public:
+ MjcfIOError(const std::string& message);
+ };
+
+
+ /// Indicates that an attribute without default was not set when it was
+ /// attempted to read.
+ class AttribNotSetError : public MjcfError
+ {
+ public:
+ AttribNotSetError(const std::string& name);
+ };
+
+ /// Indicates that an attribute without default was not set when it was
+ /// attempted to read.
+ class ParseAttributeError : public MjcfError
+ {
+ public:
+ ParseAttributeError(const std::string& source, const std::type_info& targetType,
+ const std::string& reason);
+ };
+
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/has_member.hpp b/VirtualRobot/MJCF/elements/has_member.hpp
new file mode 100644
index 000000000..1f4f2239a
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/has_member.hpp
@@ -0,0 +1,34 @@
+// from: https://gist.github.com/maddouri/0da889b331d910f35e05ba3b7b9d869b
+
+// A compile-time method for checking the existence of a class member
+// @see https://general-purpose.io/2017/03/10/checking-the-existence-of-a-cpp-class-member-at-compile-time/
+
+// This code uses "decltype" which, according to http://en.cppreference.com/w/cpp/compiler_support
+// should be supported by Clang 2.9+, GCC 4.3+ and MSVC 2010+ (if you have an older compiler, please upgrade :)
+// As of "constexpr", if not supported by your compiler, you could try "const"
+// or use the value as an inner enum value e.g. enum { value = ... }
+
+// check "test_has_member.cpp" for a usage example
+
+/// Defines a "has_member_member_name" class template
+///
+/// This template can be used to check if its "T" argument
+/// has a data or function member called "member_name"
+#define define_has_member(member_name) \
+ template <typename T> \
+ class has_member_##member_name \
+ { \
+ typedef char yes_type; \
+ typedef long no_type; \
+ template <typename U> static yes_type test(decltype(&U::member_name)); \
+ template <typename U> static no_type test(...); \
+ public: \
+ static constexpr bool value = sizeof(test<T>(0)) == sizeof(yes_type); \
+ }
+
+/// Shorthand for testing if "class_" has a member called "member_name"
+///
+/// @note "define_has_member(member_name)" must be used
+/// before calling "has_member(class_, member_name)"
+#define has_member(class_, member_name) has_member_##member_name<class_>::value
+
diff --git a/VirtualRobot/MJCF/elements/keyframe.cpp b/VirtualRobot/MJCF/elements/keyframe.cpp
new file mode 100644
index 000000000..5ba1e0678
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/keyframe.cpp
@@ -0,0 +1,9 @@
+#include "keyframe.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string KeyframeSection::tag = "keyframe";
diff --git a/VirtualRobot/MJCF/elements/keyframe.h b/VirtualRobot/MJCF/elements/keyframe.h
new file mode 100644
index 000000000..c4ff60b13
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/keyframe.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "Attribute.h"
+
+namespace mjcf
+{
+
+struct KeyframeSection : public Element<KeyframeSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(KeyframeSection)
+};
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/mjcf_utils.cpp b/VirtualRobot/MJCF/elements/mjcf_utils.cpp
new file mode 100644
index 000000000..5ce463791
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/mjcf_utils.cpp
@@ -0,0 +1,82 @@
+#include "mjcf_utils.h"
+
+#include <algorithm>
+#include <map>
+
+
+namespace mjcf
+{
+
+ ActuatorType toActuatorType(std::string str)
+ {
+ static const std::map<std::string, ActuatorType> map
+ {
+ {"motor", ActuatorType::MOTOR},
+ {"position", ActuatorType::POSITION},
+ {"velocity", ActuatorType::VELOCITY}
+ };
+ std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+ return map.at(str);
+ }
+
+ std::size_t commonPrefixLength(const std::string& a, const std::string& b)
+ {
+ const std::string* smaller = &a;
+ const std::string* bigger = &b;
+ if (b.size() < a.size())
+ {
+ std::swap(smaller, bigger);
+ }
+
+ auto mismatch = std::mismatch(smaller->begin(), smaller->end(),
+ bigger->begin()).first;
+ return std::size_t(std::distance(smaller->begin(), mismatch));
+ }
+
+ template <>
+ std::string toAttr<bool>(const bool& b)
+ {
+ static const std::string strings[] = { "false", "true" };
+ return strings[int(b)];
+ }
+
+
+ Eigen::Vector2f strToVec2(const char* string)
+ {
+ Eigen::Vector2f v;
+ sscanf(string, "%f %f", &v(0), &v(1));
+ return v;
+ }
+
+ Eigen::Vector3f strToVec3(const char* string)
+ {
+ Eigen::Vector3f v;
+ sscanf(string, "%f %f %f", &v(0), &v(1), &v(2));
+ return v;
+ }
+
+ Eigen::Quaternionf strToQuat(const char* string)
+ {
+ Eigen::Quaternionf q;
+ sscanf(string, "%f %f %f %f", &q.w(), &q.x(), &q.y(), &q.z());
+ return q;
+ }
+
+
+
+ bool equals(const char* lhs, const char* rhs)
+ {
+ return strcmp(lhs, rhs) == 0;
+ }
+
+}
+
+bool std::operator==(const char* lhs, const std::string& rhs)
+{
+ return mjcf::equals(lhs, rhs.c_str());
+}
+
+bool std::operator==(const std::string& lhs, const char* rhs)
+{
+ return mjcf::equals(lhs.c_str(), rhs);
+}
diff --git a/VirtualRobot/MJCF/elements/mjcf_utils.h b/VirtualRobot/MJCF/elements/mjcf_utils.h
new file mode 100644
index 000000000..ceb155891
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/mjcf_utils.h
@@ -0,0 +1,213 @@
+#pragma once
+
+#include <string>
+#include <type_traits>
+
+#include <Eigen/Eigen>
+#include <boost/lexical_cast.hpp>
+
+#include "exceptions.h"
+
+
+namespace std
+{
+ bool operator==(const char* lhs, const string& rhs);
+ bool operator==(const string& lhs, const char* rhs);
+}
+
+namespace Eigen
+{
+ using Vector5f = Eigen::Matrix<float, 5, 1>;
+ using Vector6f = Eigen::Matrix<float, 6, 1>;
+ using Vector7f = Eigen::Matrix<float, 7, 1>;
+
+ template<typename Derived>
+ struct is_matrix_expression :
+ std::is_base_of<Eigen::MatrixBase<typename std::decay<Derived>::type>, typename std::decay<Derived>::type>
+ {};
+
+ template<typename Derived>
+ struct is_quaternion_expression :
+ std::is_base_of<Eigen::QuaternionBase<typename std::decay<Derived>::type>, typename std::decay<Derived>::type>
+ {};
+
+ template<typename Derived>
+ struct is_eigen_expression :
+ is_matrix_expression<Derived>, is_quaternion_expression<Derived>
+ {};
+}
+
+
+namespace mjcf
+{
+
+ enum class ActuatorType
+ {
+ MOTOR, POSITION, VELOCITY
+ };
+
+ ActuatorType toActuatorType(std::string str);
+
+ // Get lenght of common prefix of two strings (was used for mergin body names).
+ std::size_t commonPrefixLength(const std::string& a, const std::string& b);
+
+ bool equals(const char* lhs, const char* rhs);
+
+
+ // VALUE -> ATTRIBUTE
+
+ // Convert to MJCF XML attribute format.
+
+ template <typename AttrT,
+ typename std::enable_if<!Eigen::is_eigen_expression<AttrT>::value, AttrT>::type* = nullptr>
+ std::string toAttr(const AttrT& b);
+ template <>
+ std::string toAttr<bool>(const bool& b);
+ template <typename Derived>
+ std::string toAttr(const Eigen::MatrixBase<Derived>& mat);
+ template <typename Derived>
+ std::string toAttr(const Eigen::QuaternionBase<Derived>& quat);
+
+
+ // ATTRIBUTE -> VALUE
+
+ // Convert from MJCF XML attribute.
+ Eigen::Vector2f strToVec2(const char* string);
+ Eigen::Vector3f strToVec3(const char* string);
+ Eigen::Quaternionf strToQuat(const char* string);
+
+
+
+ /// Eigen Matrix and vectors
+ template <typename Derived>
+ void fromAttr(const std::string& valueStr, Eigen::MatrixBase<Derived>& value);
+ /// Eigen Quaternions
+ template <typename Derived>
+ void fromAttr(const std::string& valueStr, Eigen::QuaternionBase<Derived>& value);
+
+ /// Single values via boost::lexical cast. Only enabled for non-Eigen types.
+ template <typename AttrT,
+ typename std::enable_if<!Eigen::is_eigen_expression<AttrT>::value, AttrT>::type* = nullptr>
+ void fromAttr(const std::string& valueStr, AttrT& value);
+
+
+ template <typename Scalar>
+ std::vector<Scalar> parseCoeffs(const std::string& string, char delim = ' ');
+
+
+ // DEFINITIONS of templated methods
+
+ template <typename AttrT,
+ typename std::enable_if<!Eigen::is_eigen_expression<AttrT>::value, AttrT>::type*>
+ std::string toAttr(const AttrT& b)
+ {
+ static_assert (!Eigen::is_eigen_expression<AttrT>::value, "Resolved an Eigen type.");
+ static_assert (!std::is_same<bool, AttrT>::value, "Resolved bool.");
+ return boost::lexical_cast<std::string>(b);
+ }
+
+ template<typename Derived>
+ std::string toAttr(const Eigen::MatrixBase<Derived>& mat)
+ {
+ static const Eigen::IOFormat iof
+ {
+ 7, 0, "", " ", "", "", "", ""
+ };
+
+ std::stringstream ss;
+ ss << mat.format(iof);
+ return ss.str();
+ }
+
+ template<typename Derived>
+ std::string toAttr(const Eigen::QuaternionBase<Derived>& quat)
+ {
+ std::stringstream ss;
+ ss << quat.w() << " " << quat.x() << " " << quat.y() << " " << quat.z();
+ return ss.str();
+ }
+
+
+ template <typename AttrT,
+ typename std::enable_if<!Eigen::is_eigen_expression<AttrT>::value, AttrT>::type*>
+ void fromAttr(const std::string& valueStr, AttrT& value)
+ {
+ static_assert (!Eigen::is_eigen_expression<AttrT>::value, "Resolved an Eigen type.");
+ value = boost::lexical_cast<AttrT>(valueStr);
+ }
+
+ template <typename Derived>
+ void fromAttr(const std::string& valueStr, Eigen::MatrixBase<Derived>& value)
+ {
+ using Matrix = Eigen::MatrixBase<Derived>;
+ using Scalar = typename Matrix::Scalar;
+
+ std::vector<Scalar> coeffs;
+ try
+ {
+ coeffs = parseCoeffs<Scalar>(valueStr);
+ }
+ catch (const std::bad_cast& e)
+ {
+ throw mjcf::ParseAttributeError(valueStr, typeid(Matrix), e.what());
+ }
+
+ if (value.size() >= 0 && static_cast<long>(coeffs.size()) != value.size())
+ {
+ throw mjcf::ParseAttributeError(valueStr, typeid(Matrix), "Number of coefficients does not match.");
+ }
+
+ long i = 0;
+ for (auto& coeff : coeffs)
+ {
+ value(i) = coeff;
+ }
+ }
+
+ template <typename Derived>
+ void fromAttr(const std::string& valueStr, Eigen::QuaternionBase<Derived>& value)
+ {
+ using Quaternion = Eigen::QuaternionBase<Derived>;
+ using Scalar = typename Quaternion::Scalar;
+ std::vector<Scalar> coeffs;
+
+ try
+ {
+ coeffs = parseCoeffs<Scalar>(valueStr);
+ }
+ catch (const std::bad_cast& e)
+ {
+ throw mjcf::ParseAttributeError(valueStr, typeid(Quaternion), e.what());
+ }
+
+
+ if (coeffs.size() != 4)
+ {
+ throw mjcf::ParseAttributeError(valueStr, typeid(Quaternion),
+ "Number of coefficients does not match.");
+ }
+
+ value.w() = coeffs[0];
+ value.x() = coeffs[1];
+ value.y() = coeffs[2];
+ value.z() = coeffs[3];
+ }
+
+
+ template <typename Scalar>
+ std::vector<Scalar> parseCoeffs(const std::string& string, char delim)
+ {
+ std::vector<Scalar> coeffs;
+
+ std::stringstream ss(string);
+ for (std::string valueStr; std::getline(ss, valueStr, delim);)
+ {
+ // may throw boost::bad_lexical_cast (derives from std::bad_cast)
+ Scalar value = boost::lexical_cast<Scalar>(valueStr);
+ coeffs.push_back(value);
+ }
+
+ return coeffs;
+ }
+}
+
diff --git a/VirtualRobot/MJCF/elements/option.cpp b/VirtualRobot/MJCF/elements/option.cpp
new file mode 100644
index 000000000..a28581475
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/option.cpp
@@ -0,0 +1,16 @@
+#include "option.h"
+
+#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string OptionFlag::tag = "flag";
+const std::string OptionSection::tag = "option";
+
+
+OptionFlag OptionSection::flag()
+{
+ return getOrCreateChild<OptionFlag>();
+}
diff --git a/VirtualRobot/MJCF/elements/option.h b/VirtualRobot/MJCF/elements/option.h
new file mode 100644
index 000000000..b673dfcdd
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/option.h
@@ -0,0 +1,71 @@
+#pragma once
+
+#include "Attribute.h"
+#include "mjcf_utils.h"
+
+namespace mjcf
+{
+
+
+struct OptionFlag : public Element<OptionFlag>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(OptionFlag)
+};
+
+struct OptionSection : public Element<OptionSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(OptionSection)
+
+
+ /// Get (or create) the option/flag child.
+ OptionFlag flag();
+
+
+ mjcf_FloatAttributeDef(OptionSection, timestep, 0.002f);
+ mjcf_FloatAttributeDef(OptionSection, impratio, 1);
+
+ mjcf_Vector3fAttributeDef(OptionSection, gravity, Eigen::Vector3f(0, 0, -9.81f));
+
+ mjcf_Vector3fAttributeDef(OptionSection, wind, Eigen::Vector3f::Zero());
+ mjcf_Vector3fAttributeDef(OptionSection, magnetic, Eigen::Vector3f(0, -0.5, 0));
+
+ mjcf_FloatAttributeDef(OptionSection, density, 0);
+ mjcf_FloatAttributeDef(OptionSection, viscosity, 0);
+
+ mjcf_FloatAttributeDef(OptionSection, o_margin, 0);
+ mjcf_Vector2fAttributeDef(OptionSection, o_solref, Eigen::Vector2f(0.02, 1));
+ mjcf_AttributeDef(OptionSection, Eigen::Vector5f, o_solimp, o_solimp_default());
+
+ // [Euler, RK4]
+ mjcf_StringAttributeDef(OptionSection, integrator, "Euler");
+ // [all, predefined, dynamic]
+ mjcf_StringAttributeDef(OptionSection, collision, "all");
+ // [pyramidal, elliptic]
+ mjcf_StringAttributeDef(OptionSection, cone, "pyramidal");
+ // [dense, sparse, auto]
+ mjcf_StringAttributeDef(OptionSection, jacobian, "auto");
+ // [PGS, CG, Newton]
+ mjcf_StringAttributeDef(OptionSection, solver, "Newton");
+
+ mjcf_IntAttributeDef(OptionSection, iterations, 100);
+ mjcf_FloatAttributeDef(OptionSection, tolerance, 1e-8f);
+ mjcf_IntAttributeDef(OptionSection, noslip_iterations, 0);
+ mjcf_FloatAttributeDef(OptionSection, noslip_tolerance, 1e-6f);
+ mjcf_IntAttributeDef(OptionSection, mpr_iterations, 50);
+ mjcf_FloatAttributeDef(OptionSection, mpr_tolerance, 1e-6f);
+
+private:
+
+ static Eigen::Vector5f o_solimp_default()
+ {
+ Eigen::Vector5f v;
+ v << 0.9f, 0.95f, 0.001f, 0.5, 2;
+ return v;
+ }
+};
+
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/sensor.cpp b/VirtualRobot/MJCF/elements/sensor.cpp
new file mode 100644
index 000000000..4d9a02cef
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/sensor.cpp
@@ -0,0 +1,9 @@
+#include "sensor.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string SensorSection::tag = "sensor";
diff --git a/VirtualRobot/MJCF/elements/sensor.h b/VirtualRobot/MJCF/elements/sensor.h
new file mode 100644
index 000000000..f9122689c
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/sensor.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "Attribute.h"
+
+namespace mjcf
+{
+
+struct SensorSection : public Element<SensorSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(SensorSection)
+};
+
+}
diff --git a/VirtualRobot/MJCF/elements/size.cpp b/VirtualRobot/MJCF/elements/size.cpp
new file mode 100644
index 000000000..79e6a61c5
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/size.cpp
@@ -0,0 +1,8 @@
+#include "size.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+const std::string SizeSection::tag = "size";
diff --git a/VirtualRobot/MJCF/elements/size.h b/VirtualRobot/MJCF/elements/size.h
new file mode 100644
index 000000000..1dffaa63e
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/size.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "Attribute.h"
+
+namespace mjcf
+{
+
+struct SizeSection : public Element<SizeSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(SizeSection)
+};
+
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/statistic.cpp b/VirtualRobot/MJCF/elements/statistic.cpp
new file mode 100644
index 000000000..5dfd99ff8
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/statistic.cpp
@@ -0,0 +1,9 @@
+#include "statistic.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string StatisticSection::tag = "statistic";
diff --git a/VirtualRobot/MJCF/elements/statistic.h b/VirtualRobot/MJCF/elements/statistic.h
new file mode 100644
index 000000000..543b6d113
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/statistic.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "Attribute.h"
+
+namespace mjcf
+{
+
+struct StatisticSection : public Element<StatisticSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(StatisticSection)
+};
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/tendon.cpp b/VirtualRobot/MJCF/elements/tendon.cpp
new file mode 100644
index 000000000..a9148a1bb
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/tendon.cpp
@@ -0,0 +1,10 @@
+#include "tendon.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string TendonSection::tag = "tendon";
+
diff --git a/VirtualRobot/MJCF/elements/tendon.h b/VirtualRobot/MJCF/elements/tendon.h
new file mode 100644
index 000000000..671cfa636
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/tendon.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "Attribute.h"
+
+namespace mjcf
+{
+
+
+struct TendonSection : public Element<TendonSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(TendonSection)
+};
+
+
+}
diff --git a/VirtualRobot/MJCF/elements/visual.cpp b/VirtualRobot/MJCF/elements/visual.cpp
new file mode 100644
index 000000000..72dd1bb81
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/visual.cpp
@@ -0,0 +1,9 @@
+#include "visual.h"
+
+//#include "../Document.h"
+
+
+using namespace mjcf;
+
+
+const std::string VisualSection::tag = "visual";
diff --git a/VirtualRobot/MJCF/elements/visual.h b/VirtualRobot/MJCF/elements/visual.h
new file mode 100644
index 000000000..d52ffe2a6
--- /dev/null
+++ b/VirtualRobot/MJCF/elements/visual.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "Attribute.h"
+
+namespace mjcf
+{
+
+struct VisualSection : public Element<VisualSection>
+{
+ static const std::string tag;
+ mjcf_ElementDerivedConstructors(VisualSection)
+};
+
+}
--
GitLab