diff --git a/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp b/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp
index 03be2d119bcc476d97ffdafd6fe1ae148dcf40bd..4f5c7e35ad1876414344f595e114ef1533bc08b2 100644
--- a/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp
+++ b/VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp
@@ -19,7 +19,7 @@ void MasslessBodySanitizer::sanitize()
{
// merge body leaf nodes with parent if they do not have a mass (inertial or geom)
- XMLElement* root = document->worldbody()->FirstChildElement("body");
+ XMLElement* root = document->robotRootBody();
for (XMLElement* body = root->FirstChildElement("body");
body;
@@ -58,7 +58,9 @@ void MasslessBodySanitizer::sanitizeRecursion(XMLElement* body)
}
else
{
- std::cout << "[WARN] Massless body with >1 child bodies." << std::endl;
+ std::cout << "Adding dummy inertial to massless body with >1 child bodies." << std::endl;
+ // add a small mass
+ document->addDummyInertial(body, true);
break;
}
}
@@ -198,7 +200,7 @@ void MasslessBodySanitizer::sanitizeLeafBody(XMLElement* body)
else
{
// add a small mass
- std::cout << "Adding dummy inertial." << std::endl;
+ std::cout << "Adding dummy inertial to massless leaf body with children." << std::endl;
document->addDummyInertial(body);
}
}
diff --git a/VirtualRobot/XML/mjcf/MjcfDocument.cpp b/VirtualRobot/XML/mjcf/MjcfDocument.cpp
index 8c62d514659616eded949921f982fbd9ba9cf247..0224841e54cda8a1c3c464a1d0d36c0dc1027a54 100644
--- a/VirtualRobot/XML/mjcf/MjcfDocument.cpp
+++ b/VirtualRobot/XML/mjcf/MjcfDocument.cpp
@@ -40,6 +40,26 @@ XMLElement* Document::addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen:
return texSkybox;
}
+XMLElement* Document::addMocapBody(const std::string& name, float geomSize)
+{
+ /*
+ <body name="hand-ctrl" mocap="true" >
+ <geom type="box" size="0.01 0.01 0.01" contype="0" conaffinity="0" rgba="0.9 0.5 0.5 0.5" />
+ </body>
+ */
+ XMLElement* mocap = addNewElement(worldbody(), "body");
+ mocap->SetAttribute("name", name.c_str());
+ mocap->SetAttribute("mocap", "true");
+
+ // add geom for visualization
+
+ XMLElement* geom = addNewElement(mocap, "geom");
+ geom->SetAttribute("type", "box");
+ setAttr(geom, "size", Eigen::Vector3f{geomSize, geomSize, geomSize});
+
+ return mocap;
+}
+
XMLElement* Document::addDefaultsClass(const std::string& className)
@@ -130,11 +150,11 @@ XMLElement* Document::addInertialElement(XMLElement* body, RobotNodePtr node)
return inertial;
}
-XMLElement* Document::addDummyInertial(XMLElement* body)
+XMLElement*Document::addDummyInertial(XMLElement* body, bool first)
{
assertElementIsBody(body);
- XMLElement* inertial = addNewElement(body, "inertial");
+ XMLElement* inertial = addNewElement(body, "inertial", "", first);
inertial->SetAttribute("pos", toAttr(Eigen::Vector3f(0, 0, 0)).c_str());
inertial->SetAttribute("mass", dummyMass);
@@ -180,6 +200,12 @@ XMLElement* Document::addJointElement(XMLElement* body, RobotNodePtr node)
return joint;
}
+XMLElement* Document::addFreeJointElement(XMLElement* body)
+{
+ assertElementIsBody(body);
+ return addNewElement(body, "freejoint");
+}
+
XMLElement* Document::addMeshElement(const std::string& name, const std::string& file, const std::string& className)
{
XMLElement* mesh = addNewElement(asset(), "mesh", className);
@@ -214,6 +240,17 @@ XMLElement*Document::addActuatorVelocityElement(const std::string& jointName, fl
return addActuatorShortcut("velocity", jointName, jointName, "kv", kv);
}
+XMLElement* Document::addEqualityWeld(const std::string& name, const std::string& body1, const std::string& body2, const std::string& className)
+{
+ XMLElement* weld = addNewElement(equality(), "weld", className);
+
+ weld->SetAttribute("name", name.c_str());
+ weld->SetAttribute("body1", body1.c_str());
+ weld->SetAttribute("body2", body2.c_str());
+
+ return weld;
+}
+
XMLElement* Document::addActuatorShortcut(
const std::string& type, const std::string& name, const std::string& jointName,
diff --git a/VirtualRobot/XML/mjcf/MjcfDocument.h b/VirtualRobot/XML/mjcf/MjcfDocument.h
index 4b9239d6c81a48c0a43153d6369f0fbe82585dd8..6f89d5c7c98c7737c2b6b64d27d0dc015612e86f 100644
--- a/VirtualRobot/XML/mjcf/MjcfDocument.h
+++ b/VirtualRobot/XML/mjcf/MjcfDocument.h
@@ -82,6 +82,8 @@ namespace mjcf
/// Add a skybox texture asset (only one allowed).
XMLElement* addSkyboxTexture(const Eigen::Vector3f& rgb1, const Eigen::Vector3f& rgb2);
+ /// Add a mocap body with the given name to the worldbody.
+ XMLElement* addMocapBody(const std::string& name, float geomSize);
/// Add a body element to a parent from a RobotNode.
/// Adds inertial and joint element, if necessary.
@@ -90,9 +92,11 @@ namespace mjcf
/// Add an inertial element to a body from a RobotNode.
XMLElement* addInertialElement(XMLElement* body, RobotNodePtr node);
/// Add a dummy inertial element with small mass and identity inertia matrix.
- XMLElement* addDummyInertial(XMLElement* body);
+ XMLElement* addDummyInertial(XMLElement* body, bool first=false);
/// Add a joint element to a body from a RobotNode.
XMLElement* addJointElement(XMLElement* body, RobotNodePtr node);
+ /// Add a free joint element to a body.
+ XMLElement* addFreeJointElement(XMLElement* body);
/// Add a geom to a body, referencing a mesh.
XMLElement* addGeomElement(XMLElement* body, const std::string& meshName);
@@ -111,7 +115,8 @@ namespace mjcf
const std::string& jointName, bool ctrlLimited, const Eigen::Vector2f& ctrlRange, float kp = 0);
XMLElement* addActuatorVelocityElement(const std::string& jointName, float kv = 0);
-
+ XMLElement* addEqualityWeld(const std::string& name, const std::string& body1, const std::string& body2,
+ const std::string& className="");
/// Set the pos and quat attribute of an element.
void setElementPose(XMLElement* elem, const Eigen::Matrix4f& pose);
diff --git a/VirtualRobot/XML/mjcf/MujocoIO.cpp b/VirtualRobot/XML/mjcf/MujocoIO.cpp
index 38c7027554e69339cdef30101ec502fe9364fd3d..43d3fbde20b5032920dc55ca74623f1bd8528d01 100644
--- a/VirtualRobot/XML/mjcf/MujocoIO.cpp
+++ b/VirtualRobot/XML/mjcf/MujocoIO.cpp
@@ -36,6 +36,12 @@ void MujocoIO::saveMJCF(const std::string& filename, const std::string& basePath
addSkybox();
+ if (withMocapBody)
+ {
+ std::cout << "Adding mocap body ..." << std::endl;
+ addMocapBody();
+ }
+
std::cout << "Creating bodies structure ..." << std::endl;
makeNodeBodies();
@@ -135,6 +141,38 @@ void MujocoIO::addSkybox()
}
+void MujocoIO::addMocapBody()
+{
+ std::string className = "mocap";
+ float geomSize = 0.01f;
+
+ std::string bodyName;
+ {
+ std::stringstream ss;
+ ss << robot->getName() << "_Mocap";
+ bodyName = ss.str();
+ }
+
+ // add defaults class
+ XMLElement* defClass = document->addDefaultsClass(className);
+
+ document->addDefaultAttr(defClass, "geom", "rgba",
+ toAttr(Eigen::Vector4f(.9f, .5f, .5f, .5f)).c_str());
+
+ document->addDefaultAttr(defClass, "equality", "solimp",
+ toAttr(Eigen::Vector3f{.95f, .99f, .001f}).c_str());
+ document->addDefaultAttr(defClass, "equality", "solref",
+ toAttr(Eigen::Vector2f{ .02f, 1.f}).c_str());
+
+ // add body
+ XMLElement* mocap = document->addMocapBody(bodyName, geomSize);
+ mocap->SetAttribute("childclass", className.c_str());
+
+ // add equality weld constraint
+ document->addEqualityWeld(bodyName, robot->getName(), bodyName, className);
+}
+
+
void MujocoIO::makeNodeBodies()
{
nodeBodies.clear();
@@ -145,6 +183,12 @@ void MujocoIO::makeNodeBodies()
// add root
XMLElement* robotRootBody = document->addRobotRootBodyElement(robot->getName());
+ if (withMocapBody)
+ {
+ document->addDummyInertial(robotRootBody);
+ document->addFreeJointElement(robotRootBody);
+ }
+
XMLElement* root = document->addBodyElement(robotRootBody, rootNode);
nodeBodies[rootNode->getName()] = root;
assert(root);
@@ -449,12 +493,18 @@ std::vector<const mjcf::XMLElement*> MujocoIO::getAllElements(const std::string&
return visitor.foundElements;
}
+void MujocoIO::setLengthScaling(float value)
+{
+ lengthScaling = value;
+}
+
void MujocoIO::setActuatorType(ActuatorType value)
{
actuatorType = value;
}
-void MujocoIO::setLengthScaling(float value)
+void MujocoIO::setWithMocapBody(bool enabled)
{
- lengthScaling = value;
+ withMocapBody = enabled;
}
+
diff --git a/VirtualRobot/XML/mjcf/MujocoIO.h b/VirtualRobot/XML/mjcf/MujocoIO.h
index db3d6d2ab44d3f946506084400b3f496aacd2db3..7ab57bb0f49ab5ea2264b4d8e2b3f7efba786fb5 100644
--- a/VirtualRobot/XML/mjcf/MujocoIO.h
+++ b/VirtualRobot/XML/mjcf/MujocoIO.h
@@ -38,6 +38,16 @@ namespace mjcf
/// Set the actuator type.
void setActuatorType(ActuatorType value);
+ /**
+ * @brief Enable or disable adding of a mocap body controlling the robot pose.
+ *
+ * @param enabled If true:
+ * - Add a free joint to the robot root body mocap body
+ * - Add a mocap body in the world body (called <RobotName>_Mocap)
+ * - Add a weld constraint welding them together.
+ */
+ void setWithMocapBody(bool enabled);
+
private:
@@ -54,6 +64,9 @@ namespace mjcf
/// Add a skybox texture asset.
void addSkybox();
+ /// Add a mocap body with defaults group.
+ void addMocapBody();
+
/// Construct the body structure corresponding to the robot nodes.
void makeNodeBodies();
/// Add a body element for a robot node. If its parent does not exist,
@@ -82,6 +95,9 @@ namespace mjcf
/// The actuator type.
ActuatorType actuatorType = ActuatorType::MOTOR;
+ /// Add a mocap
+ bool withMocapBody = false;
+
// Paths
@@ -90,6 +106,7 @@ namespace mjcf
boost::filesystem::path outputMeshRelDirectory;
boost::filesystem::path outputMeshDirectory() { return outputDirectory / outputMeshRelDirectory; }
+
// Input
/// The input robot.
@@ -100,7 +117,7 @@ namespace mjcf
/// The built MJCF document.
DocumentPtr document = nullptr;
-
+
// Processing
diff --git a/VirtualRobot/examples/Simox2Mjcf/main.cpp b/VirtualRobot/examples/Simox2Mjcf/main.cpp
index a86cd3ca2c60abd9439558709ff9acc163cd8e2a..e3fa1d1e819829a4890ff95a56e7ab1c489dddb5 100644
--- a/VirtualRobot/examples/Simox2Mjcf/main.cpp
+++ b/VirtualRobot/examples/Simox2Mjcf/main.cpp
@@ -19,6 +19,7 @@ void printUsage(const char* argv0)
<< "[--outdir <output directory>] "
<< "[--meshRelDir <relative mesh directory>] "
<< "[--actuator {motor|position|velocity}] "
+ << "[--mocap {y|n}]"
<< std::endl;
}
@@ -33,6 +34,8 @@ int main(int argc, char* argv[])
RuntimeEnvironment::considerKey("outdir");
RuntimeEnvironment::considerKey("meshRelDir");
RuntimeEnvironment::considerKey("actuator");
+ RuntimeEnvironment::considerKey("mocap");
+
RuntimeEnvironment::processCommandLine(argc, argv);
//RuntimeEnvironment::print();
@@ -77,12 +80,14 @@ int main(int argc, char* argv[])
return -1;
}
+ bool mocap = RuntimeEnvironment::checkParameter("mocap", "n") == "y";
std::cout << "Input file: " << inputFilename << std::endl;
std::cout << "Output dir: " << outputDir << std::endl;
std::cout << "Output mesh dir: " << outputDir / meshRelDir << std::endl;
//std::cout << "Actuator type: " << actuatorType << std::endl;
+ std::cout << "With mocap body: " << (mocap ? "yes" : "no ");
std::cout << "Loading robot ..." << std::endl;
@@ -107,6 +112,7 @@ int main(int argc, char* argv[])
// direct API
mjcf::MujocoIO mujocoIO(robot);
mujocoIO.setActuatorType(actuatorType);
+ mujocoIO.setWithMocapBody(mocap);
mujocoIO.saveMJCF(inputFilename.filename().string(), outputDir.string(), meshRelDir);
}
}