From 2257ec05ad8eb31fa59ed862b24f752ec3f43fac Mon Sep 17 00:00:00 2001
From: = <=>
Date: Thu, 10 Sep 2015 17:36:19 -0700
Subject: [PATCH] don't compensate for COLLADA up axis in collision element for
 now (todo: let user override COLLADA up axis, some URDF files are
 inconsistent)

---
 .../Importers/ImportURDFDemo/BulletUrdfImporter.cpp    | 10 ++++------
 examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp  |  4 ++--
 2 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 18433a900..05dc23d90 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -742,7 +742,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 			
 									//compensate upAxisTrans and unitMeterScaling here
 									btMatrix4x4 upAxisMat;
-									upAxisMat.setPureRotation(upAxisTrans.getRotation());
+                                    upAxisMat.setIdentity();
+									//upAxisMat.setPureRotation(upAxisTrans.getRotation());
 									btMatrix4x4 unitMeterScalingMat;
 									unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
 									btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
@@ -851,10 +852,8 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
 		for (int v = 0; v < link->m_visualArray.size();v++)
 		{
 			const UrdfVisual& vis = link->m_visualArray[v];
-			btTransform childTrans = vis.m_linkLocalFrame;
-		
+            		btTransform childTrans = vis.m_linkLocalFrame;
 			convertURDFToVisualShape(&vis, pathPrefix, inertialFrame.inverse()*childTrans, vertices, indices);
-			
 		}
 	}
 #endif
@@ -909,8 +908,7 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
 				btTransform childTrans = col.m_linkLocalFrame;
 				
 				compoundShape->addChildShape(localInertiaFrame.inverse()*childTrans,childShape);
-				
-			}
+           		}
 		}
 	}
 
diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
index c75a6e2da..d6ef58531 100644
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
@@ -185,7 +185,7 @@ void ImportUrdfSetup::initPhysics()
 {
 
 	int upAxis = 2;
-	m_guiHelper->setUpAxis(2);
+	m_guiHelper->setUpAxis(upAxis);
 
 	this->createEmptyDynamicsWorld();
 	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
@@ -356,7 +356,7 @@ void ImportUrdfSetup::initPhysics()
 
 
 
-		bool createGround=false;
+		bool createGround=true;
 		if (createGround)
 		{
 			btVector3 groundHalfExtents(20,20,20);