From 4d70d54d1966cad42fec77ebe0d298264c1f91f2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Adri=C3=A0=20Garriga?= <adria.garriga@gmail.com>
Date: Sat, 23 Dec 2017 08:49:48 +0100
Subject: [PATCH] Support for reading <inertial> elements from MJCF

---
 .../ImportMJCFDemo/BulletMJCFImporter.cpp     | 24 +++++++++++++------
 .../Importers/ImportURDFDemo/URDF2Bullet.cpp  |  4 +++-
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
index 484391388..3c2a87322 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -172,10 +172,11 @@ struct BulletMJCFImporterInternalData
 
 	btAlignedObjectArray<UrdfModel*>	m_models;
 
-	//<compiler angle="radian" meshdir="mesh/" texturedir="texture/"/>
+	//<compiler angle="radian" meshdir="mesh/" texturedir="texture/" inertiafromgeom="true"/>
 	std::string m_meshDir;
 	std::string m_textureDir;
 	std::string m_angleUnits;
+	bool m_inertiaFromGeom;
 
 
 	int m_activeModel;
@@ -190,7 +191,8 @@ struct BulletMJCFImporterInternalData
 	mutable btAlignedObjectArray<btTriangleMesh*> m_allocatedMeshInterfaces;
 
 	BulletMJCFImporterInternalData()
-		:m_activeModel(-1),
+		:m_inertiaFromGeom(true),
+		m_activeModel(-1),
 		m_activeBodyUniqueId(-1)
 	{
 		m_pathPrefix[0] = 0;
@@ -248,12 +250,12 @@ struct BulletMJCFImporterInternalData
 		}
 		const char* angle = root_xml->Attribute("angle");
 		m_angleUnits = angle ? angle : "degree";  // degrees by default, http://www.mujoco.org/book/modeling.html#compiler
-#if 0
-		for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
+		const char* inertiaFromGeom = root_xml->Attribute("inertiafromgeom");
+		if(inertiaFromGeom[0] == 'f')  // false, other values assumed `true`.
 		{
-			std::string n = child_xml->Value();
+			m_inertiaFromGeom = false;
 		}
-#endif
+
 	}
 	
 	void parseAssets(TiXmlElement* root_xml, MJCFErrorLogger* logger)
@@ -1134,8 +1136,16 @@ struct BulletMJCFImporterInternalData
 				}
 				
 				massDefined = true;
-
 				handled = true;
+
+				if (!m_inertiaFromGeom) {
+					linkPtr->m_inertia.m_mass = mass;
+					linkPtr->m_inertia.m_linkLocalFrame = localInertialFrame;
+					linkPtr->m_inertia.m_ixx = localInertiaDiag[0];
+					linkPtr->m_inertia.m_iyy = localInertiaDiag[1];
+					linkPtr->m_inertia.m_izz = localInertiaDiag[2];
+				}
+
 			}
 
 			if (n=="joint")
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index d9757db66..eb6bd5cd7 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -296,7 +296,9 @@ void ConvertURDF2BulletInternal(
          */
         if (mass)
         {
-            if (!(flags & CUF_USE_URDF_INERTIA))
+            if (!(flags & CUF_USE_URDF_INERTIA) && (localInertiaDiagonal[0] == 0.0 ||
+													localInertiaDiagonal[1] == 0.0 ||
+													localInertiaDiagonal[2] == 0.0))
             {
                 compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
                 btAssert(localInertiaDiagonal[0] < 1e10);