From 6310e1b24c0a59860635d3bb13f38b537444b431 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@student.kit.edu>
Date: Tue, 13 Nov 2018 11:30:43 +0100
Subject: [PATCH] Refactored pose/axis update (now forward instead of
backwards)
---
.../examples/MjcfConverter/MjcfConverter.cpp | 4 +-
.../examples/MjcfConverter/MjcfConverter.h | 7 +-
VirtualRobot/examples/MjcfConverter/main.cpp | 1 -
.../MjcfConverter/mjcf/MjcfDocument.cpp | 16 --
.../MjcfConverter/mjcf/MjcfDocument.h | 20 +-
.../mjcf/MjcfMasslessBodySanitizer.cpp | 182 +++++++++++-------
.../mjcf/MjcfMasslessBodySanitizer.h | 11 +-
.../examples/MjcfConverter/mjcf/utils.cpp | 20 ++
.../examples/MjcfConverter/mjcf/utils.h | 30 ++-
9 files changed, 175 insertions(+), 116 deletions(-)
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
index 3d18e8cfc..c9d7237a0 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.cpp
@@ -3,8 +3,8 @@
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include "utils.h"
-#include "xml_visitors.h"
+#include "mjcf/utils.h"
+#include "mjcf/xml_visitors.h"
diff --git a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
index 0bd82a8ed..17a5dc73d 100644
--- a/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
+++ b/VirtualRobot/examples/MjcfConverter/MjcfConverter.h
@@ -5,10 +5,9 @@
#include <VirtualRobot/Robot.h>
-#include "exceptions.h"
-#include "SimoxXMLDocument.h"
-#include "MjcfDocument.h"
-#include "MjcfMasslessBodySanitizer.h"
+#include "mjcf/exceptions.h"
+#include "mjcf/MjcfDocument.h"
+#include "mjcf/MjcfMasslessBodySanitizer.h"
namespace VirtualRobot
diff --git a/VirtualRobot/examples/MjcfConverter/main.cpp b/VirtualRobot/examples/MjcfConverter/main.cpp
index 85140df6c..c50c22877 100644
--- a/VirtualRobot/examples/MjcfConverter/main.cpp
+++ b/VirtualRobot/examples/MjcfConverter/main.cpp
@@ -3,7 +3,6 @@
#include <VirtualRobot/RuntimeEnvironment.h>
#include "MjcfConverter.h"
-#include "utils.h"
using namespace VirtualRobot;
diff --git a/VirtualRobot/examples/MjcfConverter/mjcf/MjcfDocument.cpp b/VirtualRobot/examples/MjcfConverter/mjcf/MjcfDocument.cpp
index 4bab25adf..e803fda40 100644
--- a/VirtualRobot/examples/MjcfConverter/mjcf/MjcfDocument.cpp
+++ b/VirtualRobot/examples/MjcfConverter/mjcf/MjcfDocument.cpp
@@ -256,19 +256,3 @@ Element* Document::topLevelElement(const std::string& name)
}
-std::string Document::toAttr(bool b)
-{
- static const std::string strings[] = { "false", "true" };
- return strings[int(b)];
-}
-
-std::string Document::toAttr(const Eigen::Quaternionf& quat)
-{
- std::stringstream ss;
- ss << quat.w() << " " << quat.x() << " " << quat.y() << " " << quat.z();
- return ss.str();
-}
-
-
-
-
diff --git a/VirtualRobot/examples/MjcfConverter/mjcf/MjcfDocument.h b/VirtualRobot/examples/MjcfConverter/mjcf/MjcfDocument.h
index 123ff373b..4c3ef944b 100644
--- a/VirtualRobot/examples/MjcfConverter/mjcf/MjcfDocument.h
+++ b/VirtualRobot/examples/MjcfConverter/mjcf/MjcfDocument.h
@@ -85,13 +85,7 @@ namespace mjcf
/// Gets the top-level element (child of root element) with a name.
/// If it does not exist, it is created.
Element* topLevelElement(const std::string& name);
-
-
- /// Convert to MJCF XML attribute format.
- std::string toAttr(bool b);
- template <int dim>
- std::string toAttr(const Eigen::Matrix<float, dim, 1>& v);
- std::string toAttr(const Eigen::Quaternionf& v);
+
/// Scaling for lengths, such as positions and translations.
float lengthScaling = 0.001f;
@@ -100,9 +94,6 @@ namespace mjcf
/// Mass used for dummy inertial elements.
float dummyMass = 0.0001f;
- /// IOFormat for vectors.
- Eigen::IOFormat iofVector {5, 0, "", " ", "", "", "", ""};
-
/// The "mujoco" root element.
Element* root;
@@ -113,14 +104,7 @@ namespace mjcf
using DocumentPtr = std::unique_ptr<Document>;
-
- template <int dim>
- std::string Document::toAttr(const Eigen::Matrix<float, dim, 1>& v)
- {
- std::stringstream ss;
- ss << v.format(iofVector);
- return ss.str();
- }
+
diff --git a/VirtualRobot/examples/MjcfConverter/mjcf/MjcfMasslessBodySanitizer.cpp b/VirtualRobot/examples/MjcfConverter/mjcf/MjcfMasslessBodySanitizer.cpp
index a7d6c14b1..05b926dd6 100644
--- a/VirtualRobot/examples/MjcfConverter/mjcf/MjcfMasslessBodySanitizer.cpp
+++ b/VirtualRobot/examples/MjcfConverter/mjcf/MjcfMasslessBodySanitizer.cpp
@@ -36,94 +36,31 @@ void MjcfMasslessBodySanitizer::sanitizeRecursion(Element* body)
{
assertElementIsBody(body);
- std::cout << "- Node '" << body->Attribute("name") << "': " << std::endl;
- const std::string t = " | ";
-
std::string bodyName = body->Attribute("name");
RobotNodePtr bodyNode = robot->getRobotNode(bodyName);
- Eigen::Matrix4f bodyPose = bodyNode->getTransformationFrom(bodyNode->getParent());
+ Eigen::Matrix4f accChildPose = Eigen::Matrix4f::Identity();
while (!hasMass(body))
{
- std::cout << t << "Massless" << std::endl;
+ std::cout << t << bodyName << ": \t";
if (!hasElementChild(body, "body"))
{
// leaf => end of recursion
- std::cout << t << "Leaf" << std::endl;
sanitizeLeafBody(body);
- return;
+ return;
}
// non-leaf body
- std::cout << t << "Non-leaf" << std::endl;
-
// check whether there is only one child body
Element* childBody = body->FirstChildElement("body");
if (!childBody->NextSiblingElement("body"))
{
- std::string childBodyName = childBody->Attribute("name");
-
- std::cout << t << "Single child body => merging '" << childBodyName
- << "' into '" << bodyName << "'" << std::endl;
-
- // check child for pose attributes
- if (childBody->Attribute("pos") || childBody->Attribute("quat"))
- {
- // update body's transform and joint axes
-
- RobotNodePtr childNode = robot->getRobotNode(childBodyName);
- Eigen::Matrix4f childPose = childNode->getTransformationFrom(childNode->getParent());
-
- // body's pose
- bodyPose = bodyPose * childPose;
- document->setBodyPose(body, bodyPose);
-
- /* Adapt axes of joints in body:
- * The axes will be relative to the new pose. Therefore, the additional rotation
- * of the child must be subtracted from the joint axis.
- */
-
- Eigen::Matrix3f revChildOri = childPose.block<3,3>(0, 0).transpose();
-
- for (Element* joint = body->FirstChildElement("joint"); joint;
- joint = joint->NextSiblingElement("joint"))
- {
- Eigen::Vector3f axis = strToVec(joint->Attribute("axis"));
- // apply child orientation
- axis = revChildOri * axis;
- document->setJointAxis(joint, axis);
- }
- }
-
-
- // merge childBody into body => move all its elements here
- for (tx::XMLNode* grandChild = childBody->FirstChild(); grandChild;
- grandChild = grandChild->NextSibling())
- {
- std::cout << t << " | Moving '" << grandChild->Value() << "'" << std::endl;
-
- // clone grandchild
- tx::XMLNode* grandChildClone = grandChild->DeepClone(nullptr);
-
- // insert to body
- body->InsertEndChild(grandChildClone);
- }
-
- // update body name
- MergedBodySet& bodySet = getMergedBodySetWith(bodyName);
- bodySet.addBody(childBodyName);
- body->SetAttribute("name", bodySet.getMergedBodyName().c_str());
-
- // delete child
- body->DeleteChild(childBody);
-
- std::cout << t << "=> New body name: " << bodySet.getMergedBodyName() << std::endl;
+ mergeBodies(body, childBody, accChildPose);
}
else
{
- std::cout << "[WARN]" << t << "Massless body with >1 child body: "
- << body->Attribute("name") << std::endl;
+ std::cout << "[WARN] Massless body with >1 child bodies." << std::endl;
break;
}
}
@@ -140,6 +77,106 @@ void MjcfMasslessBodySanitizer::sanitizeRecursion(Element* body)
}
+void MjcfMasslessBodySanitizer::mergeBodies(Element* body, Element* childBody,
+ Eigen::Matrix4f& accChildPose)
+{
+ std::string childBodyName = childBody->Attribute("name");
+
+ std::cout << "Merging with '" << childBodyName << "' ";
+
+ RobotNodePtr childNode = robot->getRobotNode(childBodyName);
+ Eigen::Matrix4f childPose = childNode->getTransformationFrom(childNode->getParent());
+
+ // update accumulated child pose
+ // merged child's frame w.r.t. body's frame
+ accChildPose = childPose * accChildPose;
+ Eigen::Matrix3f accChildOri = accChildPose.block<3,3>(0, 0);
+
+ // merge childBody into body => move all its elements here
+ // while doing this, apply accChildPose to elements
+ for (tx::XMLNode* grandChild = childBody->FirstChild(); grandChild;
+ grandChild = grandChild->NextSibling())
+ {
+ // clone grandchild
+ tx::XMLNode* grandChildClone = grandChild->DeepClone(nullptr);
+
+ Element* elem = grandChildClone->ToElement();
+ 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. */
+
+ if (isElement(elem, "joint"))
+ {
+ // update pos and axis
+ updateChildPos(elem, accChildPose);
+ updateChildAxis(elem, accChildOri);
+ }
+ else if (isElement(elem, "body")
+ || isElement(elem, "inertial")
+ || isElement(elem, "geom")
+ || isElement(elem, "site")
+ || isElement(elem, "camera"))
+ {
+ updateChildPos(elem, accChildPose);
+ updateChildQuat(elem, accChildOri);
+ }
+ else if (isElement(elem, "light"))
+ {
+ updateChildPos(elem, accChildPose);
+ updateChildAxis(elem, accChildOri, "dir");
+ }
+ }
+
+ // insert to body
+ body->InsertEndChild(grandChildClone);
+ }
+
+ // update body name
+ MergedBodySet& bodySet = getMergedBodySetWith(body->Attribute("name"));
+ bodySet.addBody(childBodyName);
+ body->SetAttribute("name", bodySet.getMergedBodyName().c_str());
+
+ std::cout << "(new name: " << bodySet.getMergedBodyName() << ")" << std::endl;
+
+ // delete child
+ body->DeleteChild(childBody);
+}
+
+void MjcfMasslessBodySanitizer::updateChildPos(Element* elem, const Eigen::Matrix4f& accChildPose)
+{
+ const char* posStr = elem->Attribute("pos");
+ Eigen::Vector3f pos = posStr ? strToVec(posStr) :
+ Eigen::Vector3f::Zero();
+
+ Eigen::Vector4f posHom;
+ posHom << pos, 1;
+ posHom = accChildPose * posHom;
+ pos = posHom.head<3>();
+
+ elem->SetAttribute("pos", toAttr(pos).c_str());
+}
+
+void MjcfMasslessBodySanitizer::updateChildQuat(Element* elem, const Eigen::Matrix3f& accChildOri)
+{
+ const char* quatStr = elem->Attribute("quat");
+ Eigen::Quaternionf quat = quatStr ? strToQuat(quatStr) :
+ Eigen::Quaternionf::Identity();
+
+ quat = accChildOri * quat;
+ elem->SetAttribute("quat", toAttr(quat).c_str());
+}
+
+void MjcfMasslessBodySanitizer::updateChildAxis(Element* elem, const Eigen::Matrix3f& accChildOri,
+ const char* attrName)
+{
+ Eigen::Vector3f axis = strToVec(elem->Attribute("axis"));
+ axis = accChildOri * axis;
+ elem->SetAttribute(attrName, toAttr(axis).c_str());
+}
+
void MjcfMasslessBodySanitizer::sanitizeLeafBody(Element* body)
{
assert(!hasElementChild(body, "body"));
@@ -148,13 +185,13 @@ void MjcfMasslessBodySanitizer::sanitizeLeafBody(Element* body)
if (body->NoChildren()) // is completely empty?
{
// leaf without geom: make it a site
- std::cout << " | Empty => Changing body '" << body->Attribute("name") << "' to site." << std::endl;
+ std::cout << "Changing to site." << std::endl;
body->SetValue("site");
}
else
{
// add a small mass
- std::cout << " | Not empty => Adding dummy inertial." << std::endl;
+ std::cout << "Adding dummy inertial." << std::endl;
document->addDummyInertial(body);
}
}
@@ -197,13 +234,14 @@ MergedBodySet::MergedBodySet(const std::string& bodyName)
void MergedBodySet::addBody(const std::string& bodyName)
{
- originalBodyNames.insert(bodyName);
+ originalBodyNames.push_back(bodyName);
updateMergedBodyName();
}
bool MergedBodySet::containsBody(const std::string& bodyName) const
{
- return originalBodyNames.find(bodyName) != originalBodyNames.end();
+ return std::find(originalBodyNames.begin(), originalBodyNames.end(),
+ bodyName) != originalBodyNames.end();
}
const std::string& MergedBodySet::getMergedBodyName() const
diff --git a/VirtualRobot/examples/MjcfConverter/mjcf/MjcfMasslessBodySanitizer.h b/VirtualRobot/examples/MjcfConverter/mjcf/MjcfMasslessBodySanitizer.h
index 65b1f0988..a2f1170c0 100644
--- a/VirtualRobot/examples/MjcfConverter/mjcf/MjcfMasslessBodySanitizer.h
+++ b/VirtualRobot/examples/MjcfConverter/mjcf/MjcfMasslessBodySanitizer.h
@@ -26,7 +26,7 @@ namespace mjcf
void updateMergedBodyName();
std::string mergedBodyName;
- std::set<std::string> originalBodyNames;
+ std::vector<std::string> originalBodyNames;
};
@@ -51,7 +51,16 @@ namespace mjcf
void sanitizeRecursion(mjcf::Element* body);
void sanitizeLeafBody(mjcf::Element* body);
+ void mergeBodies(Element* body, Element* childBody, Eigen::Matrix4f& accChildPose);
+ void updateChildPos(Element* elem, const Eigen::Matrix4f& accChildPose);
+ void updateChildQuat(Element* elem, const Eigen::Matrix3f& accChildOri);
+ void updateChildAxis(Element* elem, const Eigen::Matrix3f& accChildOri,
+ const char* attrName = "axis");
+
+
+
+ const std::string t = "| ";
DocumentPtr& document;
diff --git a/VirtualRobot/examples/MjcfConverter/mjcf/utils.cpp b/VirtualRobot/examples/MjcfConverter/mjcf/utils.cpp
index f740c5b03..d79c42add 100644
--- a/VirtualRobot/examples/MjcfConverter/mjcf/utils.cpp
+++ b/VirtualRobot/examples/MjcfConverter/mjcf/utils.cpp
@@ -30,6 +30,13 @@ Eigen::Vector3f strToVec(const char* string)
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;
+}
+
std::size_t commonPrefixLength(const std::string& a, const std::string& b)
{
const std::string* smaller = &a;
@@ -64,5 +71,18 @@ bool isElement(const mjcf::Element& elem, const std::string& tag)
return std::strcmp(elem.Value(), tag.c_str()) == 0;
}
+std::string toAttr(bool b)
+{
+ static const std::string strings[] = { "false", "true" };
+ return strings[int(b)];
+}
+
+std::string toAttr(const Eigen::Quaternionf& quat)
+{
+ std::stringstream ss;
+ ss << quat.w() << " " << quat.x() << " " << quat.y() << " " << quat.z();
+ return ss.str();
+}
+
}
diff --git a/VirtualRobot/examples/MjcfConverter/mjcf/utils.h b/VirtualRobot/examples/MjcfConverter/mjcf/utils.h
index ad1701de5..2d59c00cc 100644
--- a/VirtualRobot/examples/MjcfConverter/mjcf/utils.h
+++ b/VirtualRobot/examples/MjcfConverter/mjcf/utils.h
@@ -17,18 +17,44 @@ bool hasElementChild(const mjcf::Element* elem, const std::string& elemName);
bool hasMass(const mjcf::Element* body);
+// Convert to MJCF XML attribute format.
+std::string toAttr(bool b);
+template <int dim>
+std::string toAttr(const Eigen::Matrix<float, dim, 1>& v);
+std::string toAttr(const Eigen::Quaternionf& v);
+
Eigen::Vector3f strToVec(const char* string);
+Eigen::Quaternionf strToQuat(const char* string);
std::size_t commonPrefixLength(const std::string& a, const std::string& b);
+template <class StringT>
+void assertElementIs(const mjcf::Element* elem, const StringT tag);
+
+void assertElementIsBody(const mjcf::Element* elem);
+
+
+
+
+// DEFINITIONS of templated methods
+
+
+template <int dim>
+std::string toAttr(const Eigen::Matrix<float, dim, 1>& v)
+{
+ static const Eigen::IOFormat iofVector {7, 0, "", " ", "", "", "", ""};
+
+ std::stringstream ss;
+ ss << v.format(iofVector);
+ return ss.str();
+}
+
template <class StringT>
void assertElementIs(const mjcf::Element* elem, const StringT tag)
{
assert(isElement(elem, tag));
}
-void assertElementIsBody(const mjcf::Element* elem);
-
}
--
GitLab