Fixed length scaling

VirtualRobot/XML/mjcf/MasslessBodySanitizer.cpp

@@ -154,7 +154,7 @@ void MasslessBodySanitizer::updateChildPos(Element* elem, const Eigen::Matrix4f&
     posHom = accChildPose * posHom;
     pos = posHom.head<3>();
     
-    elem->SetAttribute("pos", toAttr(pos).c_str());
+    document->setElementPos(elem, pos);
 }
 
 void MasslessBodySanitizer::updateChildQuat(Element* elem, const Eigen::Matrix3f& accChildOri)
@@ -164,15 +164,24 @@ void MasslessBodySanitizer::updateChildQuat(Element* elem, const Eigen::Matrix3f
                                         Eigen::Quaternionf::Identity();
     
     quat = accChildOri * quat;
-    elem->SetAttribute("quat", toAttr(quat).c_str());
+    document->setElementQuat(elem, quat);
 }
 
 void MasslessBodySanitizer::updateChildAxis(Element* elem, const Eigen::Matrix3f& accChildOri, 
                                                 const char* attrName)
 {
-    Eigen::Vector3f axis = strToVec(elem->Attribute("axis"));
+    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(Element* body)

VirtualRobot/XML/mjcf/MjcfDocument.cpp

@@ -97,7 +97,7 @@ Element* Document::addBodyElement(Element* parent, RobotNodePtr node)
     if (node->hasParent())
     {
         Eigen::Matrix4f tf = node->getTransformationFrom(node->getParent());
-        setBodyPose(body, tf);
+        setElementPose(body, tf);
     }
     
     if (node->isRotationalJoint() || node->isTranslationalJoint())
@@ -138,7 +138,7 @@ Element* Document::addInertialElement(Element* body, RobotNodePtr node)
     
     Element* inertial = addNewElement(body, "inertial");
     
-    Eigen::Vector3f pos = node->getCoMLocal() * lengthScaling;
+    Eigen::Vector3f pos = node->getCoMLocal();
     inertial->SetAttribute("pos", toAttr(pos).c_str());
     inertial->SetAttribute("mass", double(node->getMass()));
     
@@ -206,10 +206,6 @@ Element* Document::addJointElement(Element* body, RobotNodePtr node)
     if (!node->isLimitless())
     {
         Eigen::Vector2f range(node->getJointLimitLow(), node->getJointLimitHigh());
-        if (node->isTranslationalJoint())
-        {
-            range *= lengthScaling;
-        }
         joint->SetAttribute("range", toAttr(range).c_str());
     }
     
@@ -236,29 +232,38 @@ Element*Document::addMotorElement(const std::string& jointName, const std::strin
     return motor;
 }
 
-void Document::setBodyPose(Element* body, const Eigen::Matrix4f& pose)
+void Document::setElementPose(Element* elem, const Eigen::Matrix4f& pose)
 {
-    Eigen::Vector3f pos = pose.block<3,1>(0, 3) * lengthScaling;
+    Eigen::Vector3f pos = pose.block<3,1>(0, 3);
     Eigen::Matrix3f oriMat = pose.block<3,3>(0, 0);
     
     Eigen::Quaternionf quat(oriMat);
     
+    setElementPos(elem, pos);
+    setElementQuat(elem, quat);
+}
+
+void Document::setElementPos(Element* elem, Eigen::Vector3f pos)
+{
     if (!pos.isZero(floatCompPrecision))
     {
-        body->SetAttribute("pos", toAttr(pos).c_str());
+        elem->SetAttribute("pos", toAttr(pos).c_str());
     }
     else
     {
-        body->DeleteAttribute("pos");
+        elem->DeleteAttribute("pos");
     }
-    
+}
+
+void Document::setElementQuat(Element* elem, const Eigen::Quaternionf& quat)
+{
     if (!quat.isApprox(Eigen::Quaternionf::Identity(), floatCompPrecision))
     {
-        body->SetAttribute("quat", toAttr(quat).c_str());
+        elem->SetAttribute("quat", toAttr(quat).c_str());
     }
     else
     {
-        body->DeleteAttribute("quat");
+        elem->DeleteAttribute("quat");
     }
 }
 
@@ -306,10 +311,6 @@ void Document::setFloatCompPrecision(float value)
     floatCompPrecision = value;
 }
 
-void Document::setLengthScaling(float value)
-{
-    lengthScaling = value;
-}
 
 Element* Document::robotRootBody() const
 {

VirtualRobot/XML/mjcf/MjcfDocument.h

@@ -24,7 +24,6 @@ namespace mjcf
         Document();
         
         
-        void setLengthScaling(float value);
         void setFloatCompPrecision(float value);
         void setDummyMass(float value);
         
@@ -98,7 +97,10 @@ namespace mjcf
 
         
         /// Set the pos and quat attribute of a body.
-        void setBodyPose(Element* body, const Eigen::Matrix4f& pose);
+        void setElementPose(Element* elem, const Eigen::Matrix4f& pose);
+        void setElementPos(Element* elem, Eigen::Vector3f pos);
+        void setElementQuat(Element* elem, const Eigen::Quaternionf& quat);
+        
         /// Set the axis attribute of a joint.
         void setJointAxis(Element* joint, const Eigen::Vector3f& axis);
 
@@ -126,8 +128,6 @@ namespace mjcf
         Element* section(const std::string& name);
         
         
-        /// Scaling for lengths, such as positions and translations.
-        float lengthScaling = 0.001f;
         /// Precision when comparing floats (e.g. with zero).
         float floatCompPrecision = 1e-6f;
         /// Mass used for dummy inertial elements.

VirtualRobot/XML/mjcf/MujocoIO.cpp

@@ -67,6 +67,9 @@ void MujocoIO::saveMJCF(RobotPtr robot, const std::string& filename, const std::
     std::cout << "Adding actuators ..." << std::endl;
     addActuators();
     
+    std::cout << "Scaling lengths by " << lengthScaling << " ..." << std::endl;
+    scaleLengths();
+    
     std::cout << "Done.";
     
     std::cout << std::endl;
@@ -183,7 +186,7 @@ void MujocoIO::addNodeBodyMeshes()
         
         // add asset
         std::string meshName = node->getName();
-        document->addMeshElement(meshName, dstMeshRelPath.string());
+        document->addMeshElement(meshName, (outputDirectory/dstMeshRelPath).string());
         
         // add geom to body
         Element* body = nodeBodies[node->getName()];
@@ -253,14 +256,71 @@ void MujocoIO::addActuators()
     }
 }
 
+struct ScaleLengthVisitor : public tx::XMLVisitor
+{
+    ScaleLengthVisitor(float scaling) : scaling(scaling) {}
+    
+    virtual bool VisitEnter(const tinyxml2::XMLElement& elem, const tinyxml2::XMLAttribute* attr) override;
+    void applyScaling();
+    
+    float scaling;
+    
+    std::vector<const Element*> elementsToModify;
+};
+
+bool ScaleLengthVisitor::VisitEnter(const tinyxml2::XMLElement& elem, const tinyxml2::XMLAttribute* attr)
+{
+    if (isElement(elem, "joint"))
+    {
+        if (strcmp(elem.Attribute("type"), "hinge")==0
+            && elem.Attribute("range"))
+        {
+            elementsToModify.push_back(&elem);
+        }
+    }
+    else if (elem.Attribute("pos"))
+    {
+        elementsToModify.push_back(&elem);
+    }
+    
+    return true;
+}
+
+void ScaleLengthVisitor::applyScaling()
+{
+    for (auto& elemConst : elementsToModify)
+    {
+        Element* elem = const_cast<Element*>(elemConst);
+        if (isElement(elem, "joint"))
+        {
+            if (strcmp(elem->Attribute("type"), "hinge") == 0)
+            {
+                assert(elem->Attribute("range"));
+                Eigen::Vector2f range = strToVec2(elem->Attribute("range"));
+                range *= scaling;
+                
+                elem->SetAttribute("range", toAttr(range).c_str());
+            }
+        }
+        else if (elemConst->Attribute("pos"))
+        {
+            Eigen::Vector3f pos = strToVec(elemConst->Attribute("pos"));
+            pos *= scaling;
+            elem->SetAttribute("pos", toAttr(pos).c_str());
+        }
+    }
+}
+
+void MujocoIO::scaleLengths()
+{
+    ScaleLengthVisitor visitor(lengthScaling);
+    document->robotRootBody()->Accept(&visitor);
+    visitor.applyScaling();
+}
+
 std::vector<const mjcf::Element*> MujocoIO::getAllElements(const std::string& elemName)
 {
     mjcf::ListElementsVisitor visitor(elemName);
     document->worldbody()->Accept(&visitor);
     return visitor.getFoundElements();
 }
-
-
-
-
-

VirtualRobot/XML/mjcf/MujocoIO.h

@@ -44,10 +44,16 @@ namespace mjcf
         
         void addActuators();
         
+        void scaleLengths();
         
         std::vector<const mjcf::Element*> getAllElements(const std::string& elemName);
 
         
+        // Paremeters
+        
+        /// Scaling for lengths, such as positions and translations.
+        float lengthScaling = 0.001f;
+        
         
         // Paths
         

VirtualRobot/XML/mjcf/utils.cpp

@@ -23,6 +23,13 @@ bool hasMass(const mjcf::Element* body)
     return hasElementChild(body, "geom") || hasElementChild(body, "inertial");
 }
 
+Eigen::Vector2f strToVec2(const char* string)
+{
+    Eigen::Vector2f v;
+    sscanf(string, "%f %f", &v(0), &v(1));
+    return v;
+}
+
 Eigen::Vector3f strToVec(const char* string)
 {
     Eigen::Vector3f v;
@@ -85,4 +92,5 @@ std::string toAttr(const Eigen::Quaternionf& quat)
 }
 
 
+
 }

VirtualRobot/XML/mjcf/utils.h

@@ -23,6 +23,7 @@ template <int dim>
 std::string toAttr(const Eigen::Matrix<float, dim, 1>& v);
 std::string toAttr(const Eigen::Quaternionf& v);
 
+Eigen::Vector2f strToVec2(const char* string);
 Eigen::Vector3f strToVec(const char* string);
 Eigen::Quaternionf strToQuat(const char* string);