prepare to create coordinate frame demo, minor cleanup for create funcs in demo entries,

fix r2d2.urdf inertia

Demos3/AllBullet2Demos/BulletDemoEntries.h

@@ -4,6 +4,9 @@
 
 #include "Bullet3AppSupport/BulletDemoInterface.h"
 #include "../bullet2/BasicDemo/BasicDemo.h"
+
+#include "../bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.h"
+
 #include "../bullet2/BasicDemo/HingeDemo.h"
 #include "../bullet2/BasicDemo/HingeDemo.h"
 
@@ -29,59 +32,33 @@
 #include "../../Demos3/FiniteElementMethod/FiniteElementDemo.h"
 //#include "../../Demos3/bullet2/SoftDemo/SoftDemo.h"
 
-static BulletDemoInterface* TestJointTorqueCreateFunc(CommonGraphicsApp* app)
+
-{
+#define MYCREATEFUNC(func) \
-       CommonPhysicsSetup* physicsSetup = new TestJointTorqueSetup();
+static BulletDemoInterface* func##CreateFunc(CommonGraphicsApp* app)\
-       return new BasicDemo(app, physicsSetup);
+{\
-}
+	CommonPhysicsSetup* physicsSetup = new func##Setup();\
-static BulletDemoInterface* MultiBodyVehicleCreateFunc(CommonGraphicsApp* app)
+       return new BasicDemo(app, physicsSetup);\
-{
-       CommonPhysicsSetup* physicsSetup = new MultiBodyVehicleSetup();
-       return new BasicDemo(app, physicsSetup);
-}
-static BulletDemoInterface* LuaDemoCreateFunc(CommonGraphicsApp* app)
-{
-       CommonPhysicsSetup* physicsSetup = new LuaPhysicsSetup(app);
-       return new BasicDemo(app, physicsSetup);
 }
 
-static BulletDemoInterface* MyCcdPhysicsDemoCreateFunc(CommonGraphicsApp* app)
+#define MYCREATEFUNC2(func,setup) \
-{
+static BulletDemoInterface* func(CommonGraphicsApp* app)\
-	CommonPhysicsSetup* physicsSetup = new CcdPhysicsSetup();
+{\
-	return new BasicDemo(app, physicsSetup);
+	CommonPhysicsSetup* physicsSetup = new setup(app);\
+       return new BasicDemo(app, physicsSetup);\
 }
 
-static BulletDemoInterface* MyKinematicObjectCreateFunc(CommonGraphicsApp* app)
+MYCREATEFUNC(TestJointTorque);
-{
+MYCREATEFUNC(MultiBodyVehicle);
-	CommonPhysicsSetup* physicsSetup = new KinematicObjectSetup();
+MYCREATEFUNC2(LuaDemoCreateFunc,LuaPhysicsSetup);
-	return new BasicDemo(app, physicsSetup);
+MYCREATEFUNC(Serialize);
-}
+MYCREATEFUNC(CcdPhysics);
-static BulletDemoInterface* MySerializeCreateFunc(CommonGraphicsApp* app)
+MYCREATEFUNC(KinematicObject);
-{
+MYCREATEFUNC(ConstraintPhysics);
-    CommonPhysicsSetup* physicsSetup = new SerializeSetup();
+MYCREATEFUNC(ImportUrdf);
-	return new BasicDemo(app, physicsSetup);
+MYCREATEFUNC2(ImportObjCreateFunc,ImportObjSetup);
-}
+MYCREATEFUNC2(ImportSTLCreateFunc,ImportSTLSetup);
-static BulletDemoInterface* MyConstraintCreateFunc(CommonGraphicsApp* app)
+MYCREATEFUNC(CoordinateFrameDemoPhysics);
-{
-	CommonPhysicsSetup* physicsSetup = new ConstraintPhysicsSetup();
-	return new BasicDemo(app, physicsSetup);
-}
 
-static BulletDemoInterface* MyImportUrdfCreateFunc(CommonGraphicsApp* app)
-{
-    CommonPhysicsSetup* physicsSetup = new ImportUrdfDemo();
-	return new BasicDemo(app, physicsSetup);
-}
-static BulletDemoInterface* MyImportObjCreateFunc(CommonGraphicsApp* app)
-{
-    CommonPhysicsSetup* physicsSetup = new ImportObjDemo(app);
-	return new BasicDemo(app, physicsSetup);
-}
-static BulletDemoInterface* MyImportSTLCreateFunc(CommonGraphicsApp* app)
-{
-    CommonPhysicsSetup* physicsSetup = new ImportSTLDemo(app);
-	return new BasicDemo(app, physicsSetup);
-}
 
 static BulletDemoInterface* MyImportColladaCreateFunc(CommonGraphicsApp* app)
 {
@@ -92,7 +69,6 @@ static BulletDemoInterface* MyImportColladaCreateFunc(CommonGraphicsApp* app)
 
 
 
-
 struct BulletDemoEntry
 {
 	int									m_menuLevel;
@@ -107,21 +83,22 @@ static BulletDemoEntry allDemos[]=
     {0,"Basic Concepts",0},
     {1,"Basis Frame", &CoordinateSystemDemo::CreateFunc},
 	{1,"SupportFunc", &MySupportFuncDemo::CreateFunc},
+	{1,"Coordinate Frames", CoordinateFrameDemoPhysicsCreateFunc},
 	//{"emptydemo",EmptyBulletDemo::MyCreateFunc},
 	{0,"API Demos", 0},
 
 	{1,"BasicDemo",BasicDemo::MyCreateFunc},
-	{ 1, "CcdDemo", MyCcdPhysicsDemoCreateFunc },
+	{ 1, "CcdDemo", CcdPhysicsCreateFunc },
-	{ 1, "Kinematic", MyKinematicObjectCreateFunc },
+	{ 1, "Kinematic", KinematicObjectCreateFunc },
-	{ 1, "Constraints", MyConstraintCreateFunc },
+	{ 1, "Constraints", ConstraintPhysicsCreateFunc },
 	{ 1, "LuaDemo",LuaDemoCreateFunc},
 
 	{0,"File Formats", 0},
 
-    { 1, ".bullet",MySerializeCreateFunc},
+    { 1, ".bullet",SerializeCreateFunc},
-	{ 1, "Wavefront Obj", MyImportObjCreateFunc},
+	{ 1, "Wavefront Obj", ImportObjCreateFunc},
-    { 1, "URDF", MyImportUrdfCreateFunc },
+    { 1, "URDF", ImportUrdfCreateFunc },
-	{ 1, "STL", MyImportSTLCreateFunc},
+	{ 1, "STL", ImportSTLCreateFunc},
 	{ 1, "COLLADA", MyImportColladaCreateFunc},
 	{0,"Experiments", 0},
 	{1, "Finite Element Demo", FiniteElementDemo::CreateFunc},

Demos3/AllBullet2Demos/main.cpp

@@ -321,7 +321,7 @@ void openURDFDemo(const char* filename)
     
     s_parameterInterface->removeAllParameters();
    
-    ImportUrdfDemo* physicsSetup = new ImportUrdfDemo();
+    ImportUrdfSetup* physicsSetup = new ImportUrdfSetup();
     physicsSetup->setFileName(filename);
     
     sCurrentDemo = new BasicDemo(app, physicsSetup);

Demos3/ImportObjDemo/ImportObjSetup.cpp

@@ -7,13 +7,13 @@
 #include "OpenGLWindow/SimpleOpenGL3App.h"
 #include "Wavefront2GLInstanceGraphicsShape.h"
 
-ImportObjDemo::ImportObjDemo(CommonGraphicsApp* app)
+ImportObjSetup::ImportObjSetup(CommonGraphicsApp* app)
 :m_app(app)
 {
     
 }
 
-ImportObjDemo::~ImportObjDemo()
+ImportObjSetup::~ImportObjSetup()
 {
     
 }
@@ -24,7 +24,7 @@ ImportObjDemo::~ImportObjDemo()
 
 
 
-void ImportObjDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+void ImportObjSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
 	gfxBridge.setUpAxis(2);
 	this->createEmptyDynamicsWorld();

Demos3/ImportObjDemo/ImportObjSetup.h

@@ -4,12 +4,12 @@
 
 #include "Bullet3AppSupport/CommonRigidBodySetup.h"
 
-class ImportObjDemo : public CommonRigidBodySetup
+class ImportObjSetup : public CommonRigidBodySetup
 {
 	struct CommonGraphicsApp* m_app;
 public:
-    ImportObjDemo(CommonGraphicsApp* app);
+    ImportObjSetup(CommonGraphicsApp* app);
-    virtual ~ImportObjDemo();
+    virtual ~ImportObjSetup();
     
 	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
 };

Demos3/ImportSTLDemo/ImportSTLSetup.cpp

@@ -6,20 +6,20 @@
 #include "OpenGLWindow/SimpleOpenGL3App.h"
 #include "LoadMeshFromSTL.h"
 
-ImportSTLDemo::ImportSTLDemo(CommonGraphicsApp* app)
+ImportSTLSetup::ImportSTLSetup(CommonGraphicsApp* app)
 :m_app(app)
 {
     
 }
 
-ImportSTLDemo::~ImportSTLDemo()
+ImportSTLSetup::~ImportSTLSetup()
 {
     
 }
 
 
 
-void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+void ImportSTLSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
 	gfxBridge.setUpAxis(2);
 	this->createEmptyDynamicsWorld();

Demos3/ImportSTLDemo/ImportSTLSetup.h

@@ -4,12 +4,12 @@
 
 #include "Bullet3AppSupport/CommonRigidBodySetup.h"
 
-class ImportSTLDemo : public CommonRigidBodySetup
+class ImportSTLSetup : public CommonRigidBodySetup
 {
 	struct CommonGraphicsApp* m_app;
 public:
-    ImportSTLDemo(CommonGraphicsApp* app);
+    ImportSTLSetup(CommonGraphicsApp* app);
-    virtual ~ImportSTLDemo();
+    virtual ~ImportSTLSetup();
     
 	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
 };

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -12,12 +12,12 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr
 static bool enableConstraints = true;//false;
 
 
-ImportUrdfDemo::ImportUrdfDemo()
+ImportUrdfSetup::ImportUrdfSetup()
 {
-    sprintf(m_fileName,"r2d2.urdf");
+    sprintf(m_fileName,"r2d2.urdf");//sphere2.urdf");//
 }
 
-ImportUrdfDemo::~ImportUrdfDemo()
+ImportUrdfSetup::~ImportUrdfSetup()
 {
 
 }
@@ -41,7 +41,7 @@ btVector3 selectColor()
 	return color;
 }
 
-void ImportUrdfDemo::setFileName(const char* urdfFileName)
+void ImportUrdfSetup::setFileName(const char* urdfFileName)
 {
     memcpy(m_fileName,urdfFileName,strlen(urdfFileName)+1);
 }
@@ -544,7 +544,260 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 
 
 }
-void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+
+
+
+btMultiBody* URDF2BulletMultiBody(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix, btMultiBody* mb, int totalNumJoints)
+{
+
+	btScalar mass = 0.f;
+	btTransform localInertialTransform; localInertialTransform.setIdentity();
+	btVector3 localInertiaDiagonal(0,0,0);
+
+	{
+		
+		if ((*link).inertial)
+		{
+			mass = (*link).inertial->mass;
+			btMatrix3x3 inertiaMat;
+			inertiaMat.setIdentity();
+			inertiaMat.setValue(
+				(*link).inertial->ixx,(*link).inertial->ixy,(*link).inertial->ixz,
+				(*link).inertial->ixy,(*link).inertial->iyy,(*link).inertial->iyz,
+				(*link).inertial->ixz,(*link).inertial->iyz,(*link).inertial->izz);
+			
+			btScalar threshold = 0.00001f;
+			int maxSteps=20;
+			btMatrix3x3 inertia2PrincipalAxis;
+			inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps);
+			localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]);
+            
+            btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z);
+            localInertialTransform.setOrigin(inertiaLocalCOM);
+            btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w);
+            btMatrix3x3 inertiaOrnMat(inertiaOrn);
+            
+            if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f
+				|| localInertiaDiagonal[2] == 0.f))
+            {
+				b3Warning("Error: inertia should not be zero if mass is positive\n");
+                localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1));
+                localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat);
+            }
+            else
+            {
+                localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis);
+            }
+		}
+	}
+	btTransform linkTransformInWorldSpace;
+	int parentIndex = -1;
+
+	const Link* parentLink = (*link).getParent();
+	if (parentLink)
+	{
+		parentIndex = parentLink->m_link_index;
+		btAssert(parentIndex>=0);
+	}
+	int linkIndex =  mappings.m_linkMasses.size();
+
+	btTransform parent2joint;
+
+	if ((*link).parent_joint)
+	{
+		const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position;
+		const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation;
+		parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
+		parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
+		linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
+	} else
+	{
+		linkTransformInWorldSpace = parentTransformInWorldSpace;
+
+		btAssert(mb==0);
+
+		bool multiDof = true;
+		bool canSleep = false;
+		bool isFixedBase = (mass==0);//todo: figure out when base is fixed
+
+		mb = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
+
+
+	}
+
+	btAssert(mb);
+	
+	(*link).m_link_index = linkIndex;
+
+	//compute this links center of mass transform, aligned with the principal axis of inertia
+
+
+	{
+		//btTransform rigidBodyFrameInWorldSpace =linkTransformInWorldSpace*inertialFrame;
+			
+		mappings.m_linkMasses.push_back(mass);
+		mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal);
+		mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform);
+		
+			
+		
+		if ((*link).parent_joint)
+		{
+			btTransform offsetInA,offsetInB;
+        offsetInA.setIdentity();
+		//offsetInA = mappings.m_linkLocalInertiaTransforms[parentIndex].inverse()*parent2joint;
+		offsetInA = parent2joint;
+        offsetInB.setIdentity();
+        //offsetInB = localInertialTransform.inverse();
+
+			const Joint* pj = (*link).parent_joint.get();
+			//btVector3 jointAxis(0,0,1);//pj->axis.x,pj->axis.y,pj->axis.z);
+			btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z);
+			mappings.m_jointAxisArray.push_back(jointAxis);
+			mappings.m_jointOffsetInParent.push_back(offsetInA);
+			mappings.m_jointOffsetInChild.push_back(offsetInB);
+			mappings.m_jointTypeArray.push_back(pj->type);
+
+			switch (pj->type)
+            {
+                case Joint::FIXED:
+                {
+                    printf("Fixed joint\n");
+					mb->setupFixed(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),offsetInA.getOrigin(),offsetInB.getOrigin());
+					
+					break;
+				}
+				case Joint::CONTINUOUS:
+                case Joint::REVOLUTE:
+                {
+                    printf("Revolute joint\n");
+					mb->setupRevolute(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInA.getOrigin(),offsetInB.getOrigin(),true);
+					mb->finalizeMultiDof();
+					//mb->setJointVel(linkIndex-1,1);
+
+					break;
+				}
+				case Joint::PRISMATIC:
+				{
+					mb->setupPrismatic(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInB.getOrigin(),true);
+					printf("Prismatic joint\n");
+					break;
+				}
+				default:
+				{
+					printf("Unknown joint\n");
+					btAssert(0);
+				}
+			};
+			
+			
+
+		
+		} else
+		{
+			mappings.m_jointAxisArray.push_back(btVector3(0,0,0));
+			btTransform ident;
+			ident.setIdentity();
+			mappings.m_jointOffsetInParent.push_back(ident);
+			mappings.m_jointOffsetInChild.push_back(ident);
+			mappings.m_jointTypeArray.push_back(-1);
+			
+
+		}
+	}
+
+	//btCompoundShape* compoundShape = new btCompoundShape();
+	btCollisionShape* shape = 0;
+
+	for (int v=0;v<(int)link->collision_array.size();v++)
+	{
+		const Collision* visual = link->collision_array[v].get();
+
+		shape = convertVisualToCollisionShape(visual,pathPrefix);
+			
+		if (shape)//childShape)
+		{
+			gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape);
+
+			//btVector3 color = selectColor();
+			/*
+			if (visual->material.get())
+			{
+				color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
+			}
+			*/
+			btVector3 localInertia(0,0,0);
+			if (mass)
+			{
+				shape->calculateLocalInertia(mass,localInertia);
+			}
+			//btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
+
+
+			btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z);
+			btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w);
+			btTransform visual_frame;
+			visual_frame.setOrigin(visual_pos);
+			visual_frame.setRotation(visual_orn);
+			btTransform childTransform;
+			childTransform.setIdentity();//TODO(erwincoumans): compute relative visual/inertial transform
+		//	compoundShape->addChildShape(childTransform,childShape);
+		}
+	}
+	
+	if (shape)//compoundShape->getNumChildShapes()>0)
+	{
+		btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1);
+        col->setCollisionShape(shape);
+
+        btTransform tr;
+        tr.setIdentity();
+		tr = linkTransformInWorldSpace;
+		//if we don't set the initial pose of the btCollisionObject, the simulator will do this 
+		//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
+                
+		//tr.setOrigin(local_origin[0]);
+        //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+        col->setWorldTransform(tr);
+
+		bool isDynamic = true;
+		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+		
+        world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
+
+        btVector3 color = selectColor();//(0.0,0.0,0.5);
+        gfxBridge.createCollisionObjectGraphicsObject(col,color);
+		btScalar friction = 0.5f;
+
+        col->setFriction(friction);
+        
+		if (parentIndex>=0)
+		{
+			mb->getLink(linkIndex-1).m_collider=col;
+		} else
+		{
+			mb->setBaseCollider(col);
+		}
+	}
+
+	for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
+    {
+        if (*child)
+        {
+            URDF2BulletMultiBody(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix,mb,totalNumJoints);
+
+        }
+        else
+        {
+            std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
+        }
+    }
+	return mb;
+}
+
+
+void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
 
 	int upAxis = 2;
@@ -620,15 +873,16 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	
 	int numJoints = (*robot).m_numJoints;
 
-    if (1)
+	static bool useFeatherstone = false;
+    if (!useFeatherstone)
     {
         URDF2BulletMappings mappings;
         URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
     }
 
 	//the btMultiBody support is work-in-progress :-)
-#if 0
+#if 1
-	if (0)
+	else
     {
         URDF2BulletMappings mappings;
 		
@@ -641,7 +895,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 
     }
 #endif//
-	
+	useFeatherstone = !useFeatherstone;
 	printf("numJoints/DOFS = %d\n", numJoints);
 
 	if (0)
@@ -681,7 +935,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 
 }
 
-void ImportUrdfDemo::stepSimulation(float deltaTime)
+void ImportUrdfSetup::stepSimulation(float deltaTime)
 {
 	if (m_dynamicsWorld)
 	{

Demos3/ImportURDFDemo/ImportURDFSetup.h

@@ -4,13 +4,13 @@
 
 #include "Bullet3AppSupport/CommonMultiBodySetup.h"
 
-class ImportUrdfDemo : public CommonMultiBodySetup
+class ImportUrdfSetup : public CommonMultiBodySetup
 {
     char m_fileName[1024];
     
 public:
-    ImportUrdfDemo();
+    ImportUrdfSetup();
-    virtual ~ImportUrdfDemo();
+    virtual ~ImportUrdfSetup();
 
 	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
 	virtual void stepSimulation(float deltaTime);

Demos3/bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.cpp

@@ -0,0 +1,90 @@
+
+
+#include "CoordinateFrameDemoPhysicsSetup.h"
+#include "btBulletDynamicsCommon.h"
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Z 5
+
+void CoordinateFrameDemoPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+	createEmptyDynamicsWorld();
+	gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
+
+	///create a few basic rigid bodies
+	btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+	gfxBridge.createCollisionShapeGraphicsObject(groundShape);
+
+	//groundShape->initializePolyhedralFeatures();
+//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
+	
+	m_collisionShapes.push_back(groundShape);
+
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0,-50,0));
+
+	{
+		btScalar mass(0.);
+		btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
+		gfxBridge.createRigidBodyGraphicsObject(body, btVector3(0, 1, 0));
+	}
+
+
+	{
+		//create a few dynamic rigidbodies
+		// Re-using the same collision is better for memory usage and performance
+
+		btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
+		gfxBridge.createCollisionShapeGraphicsObject(colShape);
+
+		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+		m_collisionShapes.push_back(colShape);
+
+		/// Create Dynamic Objects
+		btTransform startTransform;
+		startTransform.setIdentity();
+
+		btScalar	mass(1.f);
+
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0,0,0);
+		if (isDynamic)
+			colShape->calculateLocalInertia(mass,localInertia);
+
+
+		for (int k=0;k<ARRAY_SIZE_Y;k++)
+		{
+			for (int i=0;i<ARRAY_SIZE_X;i++)
+			{
+				for(int j = 0;j<ARRAY_SIZE_Z;j++)
+				{
+					startTransform.setOrigin(btVector3(
+										btScalar(2.0*i),
+										btScalar(20+2.0*k),
+										btScalar(2.0*j)));
+
+			
+					btRigidBody* body = createRigidBody(mass,startTransform,colShape);
+					gfxBridge.createRigidBodyGraphicsObject(body, btVector3(1, 1, 0));
+
+				}
+			}
+		}
+	}
+
+
+}
+
+
+
+
+
+
+
+
+
+

Demos3/bullet2/CoordinateFramesDemo/CoordinateFrameDemoPhysicsSetup.h

@@ -0,0 +1,28 @@
+#ifndef COORDINATE_FRAME_DEMO_PHYSICS_SETUP_H
+#define COORDINATE_FRAME_DEMO_PHYSICS_SETUP_H
+
+class btRigidBody;
+class btCollisionShape;
+class btBroadphaseInterface;
+class btConstraintSolver;
+class btCollisionDispatcher;
+class btDefaultCollisionConfiguration;
+class btDiscreteDynamicsWorld;
+class btTransform;
+class btVector3;
+class btBoxShape;
+#include "LinearMath/btVector3.h"
+
+#include "LinearMath/btAlignedObjectArray.h"
+
+
+#include "Bullet3AppSupport/CommonRigidBodySetup.h"
+
+struct CoordinateFrameDemoPhysicsSetup : public CommonRigidBodySetup
+{
+	
+	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+
+};
+
+#endif //COORDINATE_FRAME_DEMO_PHYSICS_SETUP_H

data/r2d2.urdf

@@ -38,6 +38,7 @@
     </collision>
     <inertial>
       <mass value="10"/>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
@@ -145,6 +146,7 @@
     <inertial>
       <mass value="10"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
     </inertial>
   </link>