From 8bd37a10e6f8960be55b8400fb6d3bfd7dfd5602 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Wed, 26 Jun 2019 17:11:30 +0200
Subject: [PATCH] Moved body sanitizers
---
VirtualRobot/CMakeLists.txt | 12 +-
VirtualRobot/XML/mujoco/BodySanitizer.cpp | 15 --
VirtualRobot/XML/mujoco/BodySanitizer.h | 44 ----
.../XML/mujoco/DummyMassBodySanitizer.cpp | 51 ----
.../XML/mujoco/DummyMassBodySanitizer.h | 25 --
.../XML/mujoco/MergingBodySanitizer.cpp | 239 ------------------
.../XML/mujoco/MergingBodySanitizer.h | 88 -------
VirtualRobot/XML/mujoco/MujocoIO.cpp | 4 +-
VirtualRobot/XML/mujoco/MujocoIO.h | 2 +-
9 files changed, 9 insertions(+), 471 deletions(-)
delete mode 100644 VirtualRobot/XML/mujoco/BodySanitizer.cpp
delete mode 100644 VirtualRobot/XML/mujoco/BodySanitizer.h
delete mode 100644 VirtualRobot/XML/mujoco/DummyMassBodySanitizer.cpp
delete mode 100644 VirtualRobot/XML/mujoco/DummyMassBodySanitizer.h
delete mode 100644 VirtualRobot/XML/mujoco/MergingBodySanitizer.cpp
delete mode 100644 VirtualRobot/XML/mujoco/MergingBodySanitizer.h
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index ed462ed94..b769b0486 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -376,13 +376,13 @@ SET(SOURCES
XML/SceneIO.cpp
XML/mujoco/exceptions.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
+ XML/mujoco/body_sanitizer/BodySanitizer.cpp
+ XML/mujoco/body_sanitizer/DummyMassBodySanitizer.cpp
+ XML/mujoco/body_sanitizer/MergingBodySanitizer.cpp
)
SET(INCLUDES
@@ -605,13 +605,13 @@ SET(INCLUDES
XML/SceneIO.h
XML/mujoco/exceptions.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
+ XML/mujoco/body_sanitizer/BodySanitizer.h
+ XML/mujoco/body_sanitizer/DummyMassBodySanitizer.h
+ XML/mujoco/body_sanitizer/MergingBodySanitizer.h
)
diff --git a/VirtualRobot/XML/mujoco/BodySanitizer.cpp b/VirtualRobot/XML/mujoco/BodySanitizer.cpp
deleted file mode 100644
index fce60b3f9..000000000
--- a/VirtualRobot/XML/mujoco/BodySanitizer.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-#include "BodySanitizer.h"
-
-
-namespace VirtualRobot::mujoco
-{
-
-const std::string BodySanitizer::t = "| ";
-
-
-BodySanitizer::BodySanitizer() = default;
-
-BodySanitizer::~BodySanitizer() = default;
-
-
-}
diff --git a/VirtualRobot/XML/mujoco/BodySanitizer.h b/VirtualRobot/XML/mujoco/BodySanitizer.h
deleted file mode 100644
index 2c9a985bc..000000000
--- a/VirtualRobot/XML/mujoco/BodySanitizer.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/MJCF/Document.h>
-
-
-namespace VirtualRobot::mujoco
-{
-
- /**
- * @brief Sanitizer for MJCF bodies without mass.
- *
- * MuJoCo does not allow non-static bodies to have 0 mass. However, this
- * is the case when the robot model contains elements representing joints,
- * which are not associated with some geometry.
- *
- * A body sanitizer edits massless bodies to fix this issue.
- */
- class BodySanitizer
- {
- public:
-
- /// Constructor.
- BodySanitizer();
-
- /// Virtual destructor.
- virtual ~BodySanitizer();
-
-
- /**
- * @brief Sanitize the given root body and its direct and indirect children.
- * @param document The MJCF document.
- * @param root The root body to sanitize.
- */
- virtual void sanitize(mjcf::Document& document, mjcf::Body root) = 0;
-
-
- protected:
-
- /// "Tab" string for logging.
- static const std::string t;
-
- };
-
-}
diff --git a/VirtualRobot/XML/mujoco/DummyMassBodySanitizer.cpp b/VirtualRobot/XML/mujoco/DummyMassBodySanitizer.cpp
deleted file mode 100644
index 8c65b879b..000000000
--- a/VirtualRobot/XML/mujoco/DummyMassBodySanitizer.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-#include "DummyMassBodySanitizer.h"
-
-
-namespace VirtualRobot::mujoco
-{
-
-struct DummyMassVisitor : public mjcf::Visitor
-{
- DummyMassVisitor(mjcf::Document& document, const std::string& t = "| ");
-
- virtual bool visitEnter(const mjcf::AnyElement& element) override;
-
- const std::string& t;
-};
-
-DummyMassVisitor::DummyMassVisitor(mjcf::Document& document, const std::string& t) :
- mjcf::Visitor(document), t(t)
-{}
-
-bool DummyMassVisitor::visitEnter(const mjcf::AnyElement& element)
-{
- static const bool front = true;
-
- if (element.is<mjcf::Body>())
- {
- mjcf::Body body = element.as<mjcf::Body>();
- if (!body.hasMass())
- {
- std::cout << t << "Body '" << body.name << ": \tAdd dummy inertial." << std::endl;
- body.addDummyInertial(front);
- }
- }
- return true;
-}
-
-
-
-DummyMassBodySanitizer::DummyMassBodySanitizer()
-{}
-
-void DummyMassBodySanitizer::sanitize(mjcf::Document& document, mjcf::Body root)
-{
- DummyMassVisitor visitor(document, t);
- root.accept(visitor);
-}
-
-
-
-
-
-}
diff --git a/VirtualRobot/XML/mujoco/DummyMassBodySanitizer.h b/VirtualRobot/XML/mujoco/DummyMassBodySanitizer.h
deleted file mode 100644
index 10f363221..000000000
--- a/VirtualRobot/XML/mujoco/DummyMassBodySanitizer.h
+++ /dev/null
@@ -1,25 +0,0 @@
-#pragma once
-
-#include "BodySanitizer.h"
-
-
-namespace VirtualRobot::mujoco
-{
-
- /**
- * @brief A body sanitizer adding a small dummy mass to massless bodies.
- */
- class DummyMassBodySanitizer : public BodySanitizer
- {
- public:
-
- /// Constructor.
- DummyMassBodySanitizer();
-
-
- /// @see BodySanitizer::sanitize()
- virtual void sanitize(mjcf::Document& document, mjcf::Body root) override;
-
- };
-
-}
diff --git a/VirtualRobot/XML/mujoco/MergingBodySanitizer.cpp b/VirtualRobot/XML/mujoco/MergingBodySanitizer.cpp
deleted file mode 100644
index 08998fd81..000000000
--- a/VirtualRobot/XML/mujoco/MergingBodySanitizer.cpp
+++ /dev/null
@@ -1,239 +0,0 @@
-#include "MergingBodySanitizer.h"
-
-#include <boost/algorithm/string/join.hpp>
-
-#include <VirtualRobot/math/Helpers.h>
-
-
-namespace VirtualRobot::mujoco
-{
-
-using namespace mjcf;
-
-
-// MergedBodyList
-
-MergedBodyList::MergedBodyList() = default;
-
-MergedBodyList::MergedBodyList(const std::string& bodyName)
-{
- addBody(bodyName);
-}
-
-void MergedBodyList::addBody(const std::string& bodyName)
-{
- originalBodyNames.push_back(bodyName);
- updateMergedBodyName();
-}
-
-bool MergedBodyList::containsBody(const std::string& bodyName) const
-{
- return std::find(originalBodyNames.begin(), originalBodyNames.end(),
- bodyName) != originalBodyNames.end();
-}
-
-const std::string& MergedBodyList::getMergedBodyName() const
-{
- return mergedBodyName;
-}
-
-void MergedBodyList::updateMergedBodyName()
-{
- mergedBodyName = boost::algorithm::join(originalBodyNames, "~");
-}
-
-
-// MergingBodySanitizer
-
-MergingBodySanitizer::MergingBodySanitizer(RobotPtr robot) : robot(robot)
-{}
-
-void MergingBodySanitizer::setLengthScale(float toMeter)
-{
- this->lengthScale = toMeter;
-}
-
-void MergingBodySanitizer::sanitize(Document&, Body root)
-{
- // Merge body leaf nodes with parent if they do not have a mass (inertial or geom).
-
- for (Body body = root.firstChild<Body>(); body; body = body.nextSiblingElement<Body>())
- {
- sanitizeRecursive(body);
- }
-}
-
-
-void MergingBodySanitizer::sanitizeRecursive(mjcf::Body body)
-{
- RobotNodePtr bodyNode = robot->getRobotNode(body.name);
- Eigen::Matrix4f accChildPose = Eigen::Matrix4f::Identity();
-
- while (!body.hasMass())
- {
- std::cout << t << body.name << ": \t";
-
- if (!body.hasChild<Body>())
- {
- // Leaf => end of recursion.
- sanitizeLeafBody(body);
- return;
- }
-
- // Non-leaf body.
- // Check whether there is only one child body.
- Body childBody = body.firstChild<Body>();
- if (!childBody.nextSiblingElement<Body>())
- {
- mergeBodies(body, childBody, accChildPose);
- }
- else
- {
- std::cout << "Adding dummy inertial to massless body with >1 child bodies." << std::endl;
- // add a small mass
- body.addDummyInertial();
- break;
- }
- }
-
- // recursive descend
- for (Body child = body.firstChild<Body>();
- child; child = child.nextSiblingElement<Body>())
- {
- sanitizeRecursive(child);
- }
-}
-
-static void updatePos(AnyElement element, const Eigen::Matrix4f& accChildPose)
-{
- const Eigen::Vector3f pos = element.getAttribute<Eigen::Vector3f>("pos", Eigen::Vector3f::Zero());
- element.setAttribute("pos", math::Helpers::TransformPosition(accChildPose, pos));
-}
-
-static void updateOri(AnyElement element, const Eigen::Matrix3f& accChildOri)
-{
- if (element.is<Joint>())
- {
- Joint joint = element.as<Joint>();
- joint.axis = accChildOri * joint.axis.get();
- }
- else if (element.is<Light>())
- {
- Light light = element.as<Light>();
- light.dir = accChildOri * light.dir.get();
- }
- else
- {
- const Eigen::Quaternionf quat =
- element.getAttribute<Eigen::Quaternionf>("quat", Eigen::Quaternionf::Identity());
- element.setAttribute("quat", Eigen::Quaternionf(accChildOri * quat));
- }
-}
-
-void copyChildren(Body body, Body child, const Eigen::Matrix4f& childPose)
-{
- // Merge childBody into body => move all its elements here.
- // While doing this, apply accChildPose to elements.
- for (AnyElement grandChild = child.firstChild<AnyElement>();
- grandChild; grandChild = grandChild.template nextSiblingElement<AnyElement>())
- {
- // Clone grandchild.
- AnyElement elem = grandChild.deepClone();
-
- if (elem)
- {
- /* Adapt pose/axis elements in child. Their poses/axes will be
- * relative to body's frame, so the transformation from body
- * to child will be lost. Therefore, apply accChildPose to
- * their poses/axes. */
-
- updatePos(elem, childPose);
- updateOri(elem, math::Helpers::Orientation(childPose));
- }
-
- // Insert to body
- body.insertEndChild(elem);
- }
-}
-
-
-
-void MergingBodySanitizer::mergeBodies(Body body, Body childBody, Eigen::Matrix4f& accChildPose)
-{
- std::cout << "Merging with '" << childBody.name << "' " << std::endl;
-
- RobotNodePtr childNode = robot->getRobotNode(childBody.name);
- Eigen::Matrix4f childPose = childNode->getTransformationFrom(childNode->getParent());
-
- // Scale position.
- math::Helpers::Position(childPose) = lengthScale * math::Helpers::Position(childPose);
-
- // Update accumulated child pose.
- // Merged child's frame w.r.t. body's frame.
- accChildPose = childPose * accChildPose;
-
- // Merge childBody into body => move all its elements here.
- // While doing this, apply accChildPose to elements.
- copyChildren(body, childBody, accChildPose);
-
- // Update body name.
- MergedBodyList& bodySet = getMergedBodySetWith(body.name);
- bodySet.addBody(childBody.name);
- body.name = bodySet.getMergedBodyName();
-
- std::cout << t << "\t(new name: '" << bodySet.getMergedBodyName() << "')" << std::endl;
-
- // Delete child.
- body.deleteChild(childBody);
-}
-
-
-void MergingBodySanitizer::sanitizeLeafBody(Body body)
-{
- VR_ASSERT_MESSAGE(!body.hasChild<Body>(), "Leaf body must not have a child body.");
- VR_ASSERT_MESSAGE(!body.hasMass(), "Leaf body must not have mass.");
-
- if (!body.hasChildren()) // is completely empty?
- {
- // Leaf without geom: make it a site.
- std::cout << "Changing to site." << std::endl;
- body.transform<Site>();
- }
- else
- {
- // Add a small mass.
- std::cout << "Adding dummy inertial to massless leaf body with children." << std::endl;
- body.addDummyInertial();
- }
-}
-
-const std::vector<MergedBodyList>& MergingBodySanitizer::getMergedBodyLists() const
-{
- return mergedBodyLists;
-}
-
-const std::string& MergingBodySanitizer::getMergedBodyName(const std::string& originalBodyName)
-{
- return getMergedBodySetWith(originalBodyName).getMergedBodyName();
-}
-
-MergedBodyList& MergingBodySanitizer::getMergedBodySetWith(const std::string& bodyName)
-{
- for (auto& set : mergedBodyLists)
- {
- if (set.containsBody(bodyName))
- {
- return set;
- }
- }
-
- // Not found => add.
- mergedBodyLists.push_back(MergedBodyList(bodyName));
-
- return mergedBodyLists.back();
-}
-
-
-
-
-}
diff --git a/VirtualRobot/XML/mujoco/MergingBodySanitizer.h b/VirtualRobot/XML/mujoco/MergingBodySanitizer.h
deleted file mode 100644
index b62df97c6..000000000
--- a/VirtualRobot/XML/mujoco/MergingBodySanitizer.h
+++ /dev/null
@@ -1,88 +0,0 @@
-#pragma once
-
-#include <VirtualRobot/Robot.h>
-
-#include "BodySanitizer.h"
-
-
-namespace VirtualRobot::mujoco
-{
- class MergedBodyList
- {
- public:
-
- MergedBodyList();
- MergedBodyList(const std::string& bodyName);
-
- void addBody(const std::string& bodyName);
- bool containsBody(const std::string& bodyName) const;
-
- const std::string& getMergedBodyName() const;
-
-
- private:
-
- void updateMergedBodyName();
-
- std::string mergedBodyName;
- std::vector<std::string> originalBodyNames;
-
- };
-
-
- /**
- * @brief A body sanitizer merging massless bodies to new bodies.
- *
- * Massless bodies are merged along a kinematic chain until a body with
- * mass or a body with multiple children is encountered. The encountered
- * body is the last one included in the merged body.
- *
- * If the last body has multiple children but no mass, a small dummy mass
- * is added.
- */
- class MergingBodySanitizer : public BodySanitizer
- {
- public:
-
- /// Constructor.
- MergingBodySanitizer(RobotPtr robot);
-
-
- /// Set the scaling for lengths (e.g. positions) (to m).
- void setLengthScale(float toMeter);
-
-
- /// @see BodySanitizer::sanitize()
- /// @param document Ignored by this class.
- virtual void sanitize(mjcf::Document& document, mjcf::Body root) override;
-
-
- // Results.
-
- const std::vector<MergedBodyList>& getMergedBodyLists() const;
-
- const std::string& getMergedBodyName(const std::string& originalBodyName);
-
- MergedBodyList& getMergedBodySetWith(const std::string& bodyName);
-
-
- private:
-
- void sanitizeRecursive(mjcf::Body body);
- void sanitizeLeafBody(mjcf::Body body);
-
- void mergeBodies(mjcf::Body body, mjcf::Body childBody, Eigen::Matrix4f& accChildPose);
-
- /// The robot.
- RobotPtr robot;
-
- /// Scaling factor of lengths (to m).
- float lengthScale = 1.0f;
-
-
- // Results.
- std::vector<MergedBodyList> mergedBodyLists;
-
- };
-
-}
diff --git a/VirtualRobot/XML/mujoco/MujocoIO.cpp b/VirtualRobot/XML/mujoco/MujocoIO.cpp
index 0654a5cef..ad33526dc 100644
--- a/VirtualRobot/XML/mujoco/MujocoIO.cpp
+++ b/VirtualRobot/XML/mujoco/MujocoIO.cpp
@@ -9,8 +9,8 @@
#include <VirtualRobot/Visualization/TriMeshModel.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include "DummyMassBodySanitizer.h"
-#include "MergingBodySanitizer.h"
+#include "body_sanitizer/DummyMassBodySanitizer.h"
+#include "body_sanitizer/MergingBodySanitizer.h"
namespace fs = std::filesystem;
diff --git a/VirtualRobot/XML/mujoco/MujocoIO.h b/VirtualRobot/XML/mujoco/MujocoIO.h
index a7c073896..d85e99369 100644
--- a/VirtualRobot/XML/mujoco/MujocoIO.h
+++ b/VirtualRobot/XML/mujoco/MujocoIO.h
@@ -5,8 +5,8 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/MJCF/Document.h>
-#include "BodySanitizer.h"
#include "RobotMjcf.h"
+#include "body_sanitizer/BodySanitizer.h"
namespace VirtualRobot::mujoco
--
GitLab