From 26a464bf0b1c2df89855f63e339afe1fcd20706a Mon Sep 17 00:00:00 2001
From: Jeffrey Bingham <bingjeff@gmail.com>
Date: Sat, 10 Sep 2016 18:05:14 -0700
Subject: [PATCH] [URDF] Edit loader to handle full inertia tensor.

Previous URDF loader did not handle off-diagonal elements
in the URDF. This commit adds functionality to allow
the loading of an inertia matrix that has not already
been reduced to its principal components. It diagonalizes
the inertia matrix, tests that the values are real and
updates the inertial frame.
---
 .../ImportURDFDemo/BulletUrdfImporter.cpp     | 64 +++++++++++++++----
 1 file changed, 52 insertions(+), 12 deletions(-)

diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index b2e528677..5a7ffbb86 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -295,26 +295,66 @@ void  BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
 	if (linkPtr)
 	{
 		UrdfLink* link = *linkPtr;
+		btMatrix3x3 linkInertiaBasis;
+		btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
 		if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
 		{
-			mass = 0.f;
-			localInertiaDiagonal.setValue(0,0,0);
+			linkMass = 0.f;
+			principalInertiaX = 0.f;
+			principalInertiaY = 0.f;
+			principalInertiaZ = 0.f;
+			linkInertiaBasis.setIdentity();
 		}
 		else
 		{
-			mass = link->m_inertia.m_mass;
-			localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
-										  link->m_inertia.m_izz);
+			linkMass = link->m_inertia.m_mass;
+			if (link->m_inertia.m_ixy == 0.0 &&
+			    link->m_inertia.m_ixz == 0.0 &&
+			    link->m_inertia.m_iyz == 0.0)
+			{
+				principalInertiaX = link->m_inertia.m_ixx;
+				principalInertiaY = link->m_inertia.m_iyy;
+				principalInertiaZ = link->m_inertia.m_izz;
+				linkInertiaBasis.setIdentity();
+			}
+			else
+			{
+				principalInertiaX = link->m_inertia.m_ixx;
+				btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
+							  link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
+							  link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
+				btScalar threshold = 1.0e-6;
+				int numIterations = 30;
+				inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
+				principalInertiaX = inertiaTensor[0][0];
+				principalInertiaY = inertiaTensor[1][1];
+				principalInertiaZ = inertiaTensor[2][2];
+			}
 		}
-		inertialFrame = link->m_inertia.m_linkLocalFrame;
-		
+		mass = linkMass;
+		if (principalInertiaX < 0 ||
+		    principalInertiaX > (principalInertiaY + principalInertiaZ) ||
+		    principalInertiaY < 0 ||
+		    principalInertiaY > (principalInertiaX + principalInertiaZ) ||
+		    principalInertiaZ < 0 ||
+		    principalInertiaZ > (principalInertiaX + principalInertiaY))
+		{
+			b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
+			principalInertiaX = 0.f;
+			principalInertiaY = 0.f;
+			principalInertiaZ = 0.f;
+			linkInertiaBasis.setIdentity();
+		}
+		localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
+		inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
+		inertialFrame.setBasis(linkInertiaBasis * link->m_inertia.m_linkLocalFrame.getBasis());
 	}
 	else
-    {
-        mass = 1.f;
-        localInertiaDiagonal.setValue(1,1,1);
-        inertialFrame.setIdentity();
-    }
+	{
+		mass = 1.f;
+		localInertiaDiagonal.setValue(1,1,1);
+		inertialFrame.setIdentity();
+	}
 }
     
 bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const