Skip to content
Snippets Groups Projects
Commit 9fc42b65 authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Refactored MasslessBodySanitizer

parent 82ffdc30
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp
+ 91
− 119
View file @ 9fc42b65
#include "MasslessBodySanitizer.h"
#include "utils.h"
#include <boost/algorithm/string/join.hpp>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/math/Helpers.h>
using namespace VirtualRobot;
using namespace mjcf;
namespace tx = tinyxml2;
namespace VirtualRobot::mujoco
{
MasslessBodySanitizer::MasslessBodySanitizer(DocumentPtr& document, RobotPtr& robot) :
using namespace mjcf;
MasslessBodySanitizer::MasslessBodySanitizer(mjcf::Document& document, RobotPtr& robot) :
document(document), robot(robot)
{
}
{}
void MasslessBodySanitizer::sanitize()
{
// merge body leaf nodes with parent if they do not have a mass (inertial or geom)
XMLElement* root = document->robotRootBody();
Worldbody worldbody = document.worldbody();
for (XMLElement* body = root->FirstChildElement("body");
body;
body = body->NextSiblingElement("body"))
for (Body body = worldbody.firstChild<Body>();
body; body = body.nextSiblingElement<Body>())
{
sanitizeRecursion(body);
}
}
void MasslessBodySanitizer::sanitizeRecursion(XMLElement* body)
void MasslessBodySanitizer::sanitizeRecursion(mjcf::Body body)
{
assertElementIsBody(body);
std::string bodyName = body->Attribute("name");
RobotNodePtr bodyNode = robot->getRobotNode(bodyName);
RobotNodePtr bodyNode = robot->getRobotNode(body.name);
Eigen::Matrix4f accChildPose = Eigen::Matrix4f::Identity();
while (!hasMass(body))
while (!body.hasMass())
{
std::cout << t << bodyName << ": \t";
std::cout << t << body.name << ": \t";
if (!hasElementChild(body, "body"))
if (!body.hasChild<Body>())
{
// leaf => end of recursion
sanitizeLeafBody(body);
......@@ -51,8 +47,8 @@ void MasslessBodySanitizer::sanitizeRecursion(XMLElement* body)
// non-leaf body
// check whether there is only one child body
XMLElement* childBody = body->FirstChildElement("body");
if (!childBody->NextSiblingElement("body"))
Body childBody = body.firstChild<Body>();
if (!childBody.nextSiblingElement<Body>())
{
mergeBodies(body, childBody, accChildPose);
}
......@@ -60,148 +56,122 @@ void MasslessBodySanitizer::sanitizeRecursion(XMLElement* body)
{
std::cout << "Adding dummy inertial to massless body with >1 child bodies." << std::endl;
// add a small mass
document->addDummyInertial(body, true);
body.addDummyInertial();
break;
}
}
// recursive descend
for (XMLElement* child = body->FirstChildElement("body");
child;
child = child->NextSiblingElement("body"))
for (Body child = body.firstChild<Body>();
child; child = child.nextSiblingElement<Body>())
{
sanitizeRecursion(child);
}
}
void MasslessBodySanitizer::mergeBodies(XMLElement* body, XMLElement* childBody,
Eigen::Matrix4f& accChildPose)
template <class ChildT>
static void updatePos(ChildT child, const Eigen::Matrix3f& accChildPose)
{
std::string childBodyName = childBody->Attribute("name");
std::cout << "Merging with '" << childBodyName << "' ";
child.pos = math::Helpers::TransformPosition(accChildPose, child.pos);
}
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);
template <class ChildT>
static void updateOri(ChildT child, const Eigen::Matrix3f& accChildOri)
{
child.quat = Eigen::Quaternionf(accChildOri * child.quat.get());
}
template <>
void updateOri<Joint>(Joint child, const Eigen::Matrix3f& accChildOri)
{
child.axis = accChildOri * child.axis.get();
}
template <>
void updateOri<Light>(Light child, const Eigen::Matrix3f& accChildOri)
{
child.dir = accChildOri * child.dir.get();
}
template <class ChildT>
void doo(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 (tx::XMLNode* grandChild = childBody->FirstChild(); grandChild;
grandChild = grandChild->NextSibling())
for (Element<ChildT> grandChild = child.firstChild<ChildT>();
grandChild; grandChild = grandChild.template nextSiblingElement<ChildT>())
{
// clone grandchild
tx::XMLNode* grandChildClone = grandChild->DeepClone(nullptr);
ChildT elem = grandChild.deepClone();
XMLElement* 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. */
* 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");
}
updatePos(elem, childPose);
updateOri(elem, math::Helpers::Orientation(childPose));
}
// insert to body
body->InsertEndChild(grandChildClone);
body.insertEndChild(elem);
}
}
void MasslessBodySanitizer::mergeBodies(Body body, Body childBody, Eigen::Matrix4f& accChildPose)
{
const std::string childBodyName = childBody.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;
// merge childBody into body => move all its elements here
// while doing this, apply accChildPose to elements
doo<Joint>(body, childBody, accChildPose);
doo<Body>(body, childBody, accChildPose);
doo<Inertial>(body, childBody, accChildPose);
doo<Geom>(body, childBody, accChildPose);
doo<Site>(body, childBody, accChildPose);
doo<Camera>(body, childBody, accChildPose);
doo<Light>(body, childBody, accChildPose);
// update body name
MergedBodySet& bodySet = getMergedBodySetWith(body->Attribute("name"));
MergedBodySet& bodySet = getMergedBodySetWith(body.name);
bodySet.addBody(childBodyName);
body->SetAttribute("name", bodySet.getMergedBodyName().c_str());
body.name = bodySet.getMergedBodyName();
std::cout << "\t(new name: " << bodySet.getMergedBodyName() << ")" << std::endl;
// delete child
body->DeleteChild(childBody);
body.deleteChild(childBody);
}
void MasslessBodySanitizer::updateChildPos(XMLElement* 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>();
document->setElementPos(elem, pos);
}
void MasslessBodySanitizer::updateChildQuat(XMLElement* elem, const Eigen::Matrix3f& accChildOri)
{
const char* quatStr = elem->Attribute("quat");
Eigen::Quaternionf quat = quatStr ? strToQuat(quatStr) :
Eigen::Quaternionf::Identity();
quat = accChildOri * quat;
document->setElementQuat(elem, quat);
}
void MasslessBodySanitizer::updateChildAxis(XMLElement* elem, const Eigen::Matrix3f& accChildOri,
const char* attrName)
{
Eigen::Vector3f axis = strToVec(elem->Attribute(attrName));
axis = accChildOri * axis;
elem->SetAttribute(attrName, toAttr(axis).c_str());
if (strcmp(attrName, "axis") == 0)
{
document->setJointAxis(elem, axis);
}
else
{
elem->SetAttribute(attrName, toAttr(axis).c_str());
}
}
void MasslessBodySanitizer::sanitizeLeafBody(XMLElement* body)
void MasslessBodySanitizer::sanitizeLeafBody(Body body)
{
assert(!hasElementChild(body, "body"));
assert(!hasMass(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->NoChildren()) // is completely empty?
if (!body.hasChildren()) // is completely empty?
{
// leaf without geom: make it a site
std::cout << "Changing to site." << std::endl;
body->SetValue("site");
body.transform<Site>();
}
else
{
// add a small mass
std::cout << "Adding dummy inertial to massless leaf body with children." << std::endl;
document->addDummyInertial(body);
body.addDummyInertial();
}
}
......@@ -261,3 +231,5 @@ void MergedBodySet::updateMergedBodyName()
{
mergedBodyName = boost::algorithm::join(originalBodyNames, "~");
}
}
VirtualRobot/XML/mjcf/MasslessBodySanitizer.h
+ 8
− 14
View file @ 9fc42b65
#pragma once
#include "MjcfDocument.h"
#include <VirtualRobot.h>
#include <VirtualRobot/MJCF/Document.h>
namespace VirtualRobot
{
namespace mjcf
namespace mujoco
{
class MergedBodySet
......@@ -35,7 +36,7 @@ namespace mjcf
{
public:
MasslessBodySanitizer(DocumentPtr& document, RobotPtr& robot);
MasslessBodySanitizer(mjcf::Document& document, RobotPtr& robot);
void sanitize();
......@@ -48,22 +49,15 @@ namespace mjcf
private:
void sanitizeRecursion(mjcf::XMLElement* body);
void sanitizeLeafBody(mjcf::XMLElement* body);
void mergeBodies(XMLElement* body, XMLElement* childBody, Eigen::Matrix4f& accChildPose);
void updateChildPos(XMLElement* elem, const Eigen::Matrix4f& accChildPose);
void updateChildQuat(XMLElement* elem, const Eigen::Matrix3f& accChildOri);
void updateChildAxis(XMLElement* elem, const Eigen::Matrix3f& accChildOri,
const char* attrName = "axis");
void sanitizeRecursion(mjcf::Body body);
void sanitizeLeafBody(mjcf::Body body);
void mergeBodies(mjcf::Body body, mjcf::Body childBody, Eigen::Matrix4f& accChildPose);
const std::string t = "| ";
DocumentPtr& document;
mjcf::Document& document;
RobotPtr& robot;
std::vector<MergedBodySet> mergedBodySets;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment