Improved URDF support for btMultiBody and separate graphics/collision/inertial frames and shapes

Fix WinXP GetTickCount64 with a typedef
Expose debug drawing mode/flags in UI (hot keys A,D,L,W for now, buttons later)
GLInstancingRenderer: tweak near/far planes to allow closer approach of camera
btDiscreteDynamicsWorld: enable debug drawing for btGeneric6DofSpring2Constraint
btMultiBodyDynamicsWorld: enable basic debug drawing for btMultiBody
btMultibody: allow center-of-mass shift for prismatic and fixed constraint

Demos3/AllBullet2Demos/main.cpp

@@ -234,9 +234,10 @@ extern bool useShadowMap;
 static bool visualWireframe=false;
 static bool renderVisualGeometry=true;
 static bool renderGrid = true;
-
+int gDebugDrawFlags = 0;
-static bool pauseSimulation=false;//true;
+static bool pauseSimulation=true;
 int midiBaseIndex = 176;
+extern bool gDisableDeactivation;
 
 //#include <float.h>
 //unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM);
@@ -268,9 +269,28 @@ void MyKeyboardCallback(int key, int state)
 	//if (handled)
 	//	return;
 
+	if (key=='a' && state)
+	{
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawAabb;
+	}
+	if (key=='c' && state)
+	{
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints;
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawContactPoints;
+	}
+	if (key == 'd' && state)
+	{
+		gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation;
+		gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0);
+	}
+	if (key=='l' && state)
+	{
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraintLimits;
+	}
 	if (key=='w' && state)
 	{
 		visualWireframe=!visualWireframe;
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawWireframe;
 	}
 	if (key=='v' && state)
 	{
@@ -792,7 +812,7 @@ int main(int argc, char* argv[])
             }
             {
 				glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
-                sCurrentDemo->physicsDebugDraw();
+                sCurrentDemo->physicsDebugDraw(gDebugDrawFlags);
             }
 		}
 

Demos3/FiniteElementMethod/FiniteElementDemo.cpp

@@ -317,7 +317,7 @@ void	FiniteElementDemo::renderScene()
 	m_app->m_renderer->renderScene();
 }
 
-void	FiniteElementDemo::physicsDebugDraw()
+void	FiniteElementDemo::physicsDebugDraw(int debugDrawFlags)
 {
     {
 		btAlignedObjectArray<btVector3FloatData> m_linePoints;

Demos3/FiniteElementMethod/FiniteElementDemo.h

@@ -50,7 +50,7 @@ public:
     virtual void	renderScene();
 
 	
-    virtual void	physicsDebugDraw();
+    virtual void	physicsDebugDraw(int debugDrawFlags);
     virtual bool	mouseMoveCallback(float x,float y);
     virtual bool	mouseButtonCallback(int button, int state, float x, float y);
     virtual bool	keyboardCallback(int key, int state);

Demos3/Geometry/DistributePoints.h

@@ -181,7 +181,7 @@ public:
 		
         
     }
-    virtual void	physicsDebugDraw()
+    virtual void	physicsDebugDraw(int debugDrawFlags)
     {
    	
 		int lineWidth = 1;

Demos3/Geometry/SphereCreation.h

@@ -168,7 +168,7 @@ public:
         m_app->m_renderer->renderScene();
         
     }
-    virtual void	physicsDebugDraw()
+    virtual void	physicsDebugDraw(int debugDrawFlags)
     {
    		int lineWidth = 1;
 		int pointSize = 2;

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -12,9 +12,10 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr
 static bool enableConstraints = true;//false;
 
 
+
 ImportUrdfSetup::ImportUrdfSetup()
 {
-    sprintf(m_fileName,"r2d2.urdf");//sphere2.urdf");//
+    sprintf(m_fileName,"r2d2.urdf");
 }
 
 ImportUrdfSetup::~ImportUrdfSetup()
@@ -86,12 +87,26 @@ struct URDF_LinkInformation
 {
     const Link* m_thisLink;
 	int m_linkIndex;
-	int m_parentIndex;
+	//int m_parentIndex;
 
     btTransform m_localInertialFrame;
-    btTransform m_localVisualFrame;
+    //btTransform m_localVisualFrame;
+	btTransform m_bodyWorldTransform;
+	btVector3 m_localInertiaDiagonal;
+	btScalar m_mass;
 
+	btCollisionShape* m_collisionShape;
     btRigidBody* m_bulletRigidBody;
+
+	URDF_LinkInformation()
+		:m_thisLink(0),
+		m_linkIndex(-2),
+		//m_parentIndex(-2),
+		m_collisionShape(0),
+		m_bulletRigidBody(0)
+	{
+
+	}
 	virtual ~URDF_LinkInformation()
 	{
         printf("~\n");
@@ -107,17 +122,30 @@ struct URDF_JointInformation
 struct URDF2BulletMappings
 {
     btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
-    btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
+    //btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
 
-	btAlignedObjectArray<btTransform>		m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
+	//btAlignedObjectArray<btTransform>		m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
 	btAlignedObjectArray<btScalar>			m_linkMasses;
-	btAlignedObjectArray<btVector3>			m_linkLocalDiagonalInertiaTensors;
+	//btAlignedObjectArray<btVector3>			m_linkLocalDiagonalInertiaTensors;
-	btAlignedObjectArray<btTransform>		m_jointTransforms;//for root, it is identity
+	
-	btAlignedObjectArray<int>				m_parentIndices;//for root, it is identity
+	//btAlignedObjectArray<int>				m_parentIndices;//for root, it is identity
-	btAlignedObjectArray<btVector3>			m_jointAxisArray;
+	//btAlignedObjectArray<btVector3>			m_jointAxisArray;
-	btAlignedObjectArray<btTransform>			m_jointOffsetInParent;
+	//btAlignedObjectArray<btTransform>			m_jointOffsetInParent;
-	btAlignedObjectArray<btTransform>			m_jointOffsetInChild;
+	//btAlignedObjectArray<btTransform>			m_jointOffsetInChild;
-	btAlignedObjectArray<int>				m_jointTypeArray;
+	//btAlignedObjectArray<int>				m_jointTypeArray;
+
+
+	
+	bool m_createMultiBody;
+	int m_totalNumJoints;
+	btMultiBody*	m_bulletMultiBody;
+
+	URDF2BulletMappings()
+		:m_createMultiBody(false),
+		m_totalNumJoints(0),
+		m_bulletMultiBody(0)
+	{
+	}
 
 };
 enum MyFileType
@@ -126,7 +154,8 @@ enum MyFileType
 	FILE_COLLADA=2
 };
 
-btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const char* pathPrefix)
+template <typename T>
+btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathPrefix)
 {
 	btCollisionShape* shape = 0;
 
@@ -151,6 +180,8 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
             }
             btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
             cylZShape->initializePolyhedralFeatures();
+			//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+            
             //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
             //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
             cylZShape->setMargin(0.001);
@@ -164,6 +195,7 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
             urdf::Box* box = (urdf::Box*)visual->geometry.get();
             btVector3 extents(box->dim.x,box->dim.y,box->dim.z);
             btBoxShape* boxShape = new btBoxShape(extents*0.5f);
+			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
             shape = boxShape;
 			shape ->setMargin(0.001);
             break;
@@ -348,32 +380,46 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
     }
 	return shape;
 }
-void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix)
+void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix)
 {
-    btCollisionShape* shape = 0;
+    //btCollisionShape* shape = 0;
 
 	btTransform linkTransformInWorldSpace;
 	linkTransformInWorldSpace.setIdentity();
 
-	btScalar mass = 1;
+	btScalar mass = 0;
 	btTransform inertialFrame;
 	inertialFrame.setIdentity();
     const Link* parentLink = (*link).getParent();
 	URDF_LinkInformation* pp = 0;
+	int linkIndex =  mappings.m_linkMasses.size();
+	btVector3 localInertiaDiagonal(0,0,0);
+
+	int parentIndex = -1;
+
+	
+	if (parentLink)
+	{
+		parentIndex = parentLink->m_link_index;
+		btAssert(parentIndex>=0);
+	}
 
     {
 		URDF_LinkInformation** ppRigidBody = mappings.m_link2rigidbody.find(parentLink);
 		if (ppRigidBody)
 		{
 			pp = (*ppRigidBody);
-			btRigidBody* parentRigidBody = pp->m_bulletRigidBody;
+			btTransform tr = pp->m_bodyWorldTransform;
-			btTransform tr = parentRigidBody->getWorldTransform();
 			printf("rigidbody origin (COM) of link(%s) parent(%s): %f,%f,%f\n",(*link).name.c_str(), parentLink->name.c_str(), tr.getOrigin().x(), tr.getOrigin().y(), tr.getOrigin().z());
 		}
 	}
+
+	(*link).m_link_index = linkIndex;
+
 	if ((*link).inertial)
 	{
 		mass = (*link).inertial->mass;
+		localInertiaDiagonal.setValue((*link).inertial->ixx,(*link).inertial->iyy,(*link).inertial->izz);
 		inertialFrame.setOrigin(btVector3((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z));
 		inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w));
 	}
@@ -394,20 +440,73 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 	} else
 	{
 		linkTransformInWorldSpace = parentTransformInWorldSpace;
+
+		
+		
 	}
 
 
     {
         printf("converting visuals of link %s",link->name.c_str());
+		
+
+       
+        {
+
+			
+			btCompoundShape* tmpGfxShape = new btCompoundShape();
+
+			for (int v=0;v<(int)link->visual_array.size();v++)
+			{
+				const Visual* vis = link->visual_array[v].get();
+				btCollisionShape* childShape = convertURDFToCollisionShape(vis,pathPrefix);
+				if (childShape)
+				{
+					btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
+					btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
+					btTransform childTrans;
+					childTrans.setOrigin(childPos);
+					childTrans.setRotation(childOrn);
+					if (!mappings.m_createMultiBody)
+					{
+						tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
+					} else
+					{
+						tmpGfxShape->addChildShape(childTrans,childShape);
+					}
+				}
+			}
+
+			btCompoundShape* compoundShape = new btCompoundShape();
 			for (int v=0;v<(int)link->collision_array.size();v++)
 			{
-            const Collision* visual = link->collision_array[v].get();
+				const Collision* col = link->collision_array[v].get();
-
+				btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
-			shape = convertVisualToCollisionShape(visual,pathPrefix);
+				if (childShape)
-			
-            if (shape)
 				{
-                gfxBridge.createCollisionShapeGraphicsObject(shape);
+					btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
+					btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
+					btTransform childTrans;
+					childTrans.setOrigin(childPos);
+					childTrans.setRotation(childOrn);
+					if (!mappings.m_createMultiBody)
+					{
+						compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
+					} else
+					{
+						compoundShape->addChildShape(childTrans,childShape);
+					}
+					
+				}
+			}
+
+			
+
+            
+			
+			if (compoundShape)
+			{
+				
 
 				btVector3 color = selectColor();
 				/*                if (visual->material.get())
@@ -415,33 +514,49 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 								color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
 								}
 								*/
-                btVector3 localInertia(0,0,0);
+				//btVector3 localInertiaDiagonal(0, 0, 0);
-                if (mass)
+				//if (mass)
+				//{
+				//	shape->calculateLocalInertia(mass, localInertiaDiagonal);
+				//}
+
+				
+				//btTransform visualFrameInWorldSpace = linkTransformInWorldSpace*visual_frame;
+				btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*inertialFrame;
+				URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
+				
+				if (!mappings.m_createMultiBody)
 				{
-                    shape->calculateLocalInertia(mass,localInertia);
+					btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal);
-                }
+					rbci.m_startWorldTransform = inertialFrameInWorldSpace;
-                btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
+					linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
-
+					//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
-
-				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 visualFrameInWorldSpace =linkTransformInWorldSpace*visual_frame;
-				rbci.m_startWorldTransform = visualFrameInWorldSpace;//linkCenterOfMass;
-
-
 					btRigidBody* body = new btRigidBody(rbci);
-
 					world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
-    //            body->setFriction(0);
+					gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
+					//hack-> transfer user inder from visual to collision shape
+					compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
 
 					gfxBridge.createRigidBodyGraphicsObject(body, color);
-                URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
 					linkInfo->m_bulletRigidBody = body;
-                linkInfo->m_localVisualFrame =visual_frame;
+				} else
+				{
+					if (mappings.m_bulletMultiBody==0)
+					{
+						bool multiDof = true;
+						bool canSleep = false;
+						bool isFixedBase = (mass==0);//todo: figure out when base is fixed
+						int totalNumJoints = mappings.m_totalNumJoints;
+						mappings.m_bulletMultiBody = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
+					}
+
+				}
+
+
+				linkInfo->m_collisionShape = compoundShape;
+				linkInfo->m_localInertiaDiagonal = localInertiaDiagonal;
+				linkInfo->m_mass = mass;
+                //linkInfo->m_localVisualFrame =visual_frame;
                 linkInfo->m_localInertialFrame =inertialFrame;
                 linkInfo->m_thisLink = link.get();
                 const Link* p = link.get();
@@ -452,23 +567,60 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                 {
 					btAssert(pp);
 
-                    btRigidBody* parentBody =pp->m_bulletRigidBody;
+                    
 
                     const Joint* pj = (*link).parent_joint.get();
                     btTransform offsetInA,offsetInB;
+                    static bool once = true;
 
                     offsetInA.setIdentity();
+					static bool toggle=false;
+					
+					//offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
+					offsetInA = pp->m_localInertialFrame.inverse()*parent2joint;
 					
-                    offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
                     offsetInB.setIdentity();
-                    offsetInB = visual_frame.inverse();
+                    //offsetInB = visual_frame.inverse();
+					offsetInB = inertialFrame.inverse();
 					
+					
+					bool disableParentCollision = true;
+					btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z);
                     switch (pj->type)
                     {
                         case Joint::FIXED:
+                        {
+							if (mappings.m_createMultiBody)
+							{
+								//todo: adjust the center of mass transform and pivot axis properly
+
+								printf("Fixed joint (btMultiBody)\n");
+								//btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin());
+								btQuaternion rot = parent2joint.inverse().getRotation();
+								//toggle=!toggle;
+								mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 
+									rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
+								btMatrix3x3 rm(rot);
+								btScalar y,p,r;
+								rm.getEulerZYX(y,p,r);
+								//parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
+								//linkInfo->m_localVisualFrame.setIdentity();
+								printf("y=%f,p=%f,r=%f\n", y,p,r);
+								
+
+
+							} else
 							{
 								printf("Fixed joint\n");
-                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+								
+								btMatrix3x3 rm(offsetInA.getBasis());
+								btScalar y,p,r;
+								rm.getEulerZYX(y,p,r);
+								//parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
+								//linkInfo->m_localVisualFrame.setIdentity();
+								printf("y=%f,p=%f,r=%f\n", y,p,r);
+
+								btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
 								//                            btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
 								dof6->setLinearLowerLimit(btVector3(0,0,0));
 								dof6->setLinearUpperLimit(btVector3(0,0,0));
@@ -481,29 +633,96 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 
 								//                            btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
 								//                          world->addConstraint(fixed,true);
+								}
                             break;
                         }
                         case Joint::CONTINUOUS:
                         case Joint::REVOLUTE:
                         {
-                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+							if (mappings.m_createMultiBody)
-//                            btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
+							{
+								//todo: adjust the center of mass transform and pivot axis properly
+								mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 
+									parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(), 
+									btVector3(0,0,0),//offsetInB.getOrigin(), 
+									disableParentCollision);
+
+								/*
+								
+								mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
+								parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),
+								-offsetInB.getOrigin(),
+								disableParentCollision);
+								linkInfo->m_localVisualFrame.setIdentity();
+								*/
+							} else
+							{
+								//only handle principle axis at the moment, 
+								//@todo(erwincoumans) orient the constraint for non-principal axis
+								btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
+								int principleAxis = axis.closestAxis();
+								switch (principleAxis)
+								{
+								case 0:
+									{
+										btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_ZYX);
 										dof6->setLinearLowerLimit(btVector3(0,0,0));
 										dof6->setLinearUpperLimit(btVector3(0,0,0));
 								
-                            dof6->setAngularLowerLimit(btVector3(0,0,1000));
+										dof6->setAngularUpperLimit(btVector3(-1,0,0));
-                           dof6->setAngularUpperLimit(btVector3(0,0,-1000));
+										dof6->setAngularLowerLimit(btVector3(1,0,0));
 								
 										if (enableConstraints)
 											world1->addConstraint(dof6,true);
+										break;
+									}
+								case 1:
+									{
+										btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XZY);
+										dof6->setLinearLowerLimit(btVector3(0,0,0));
+										dof6->setLinearUpperLimit(btVector3(0,0,0));
 								
+										dof6->setAngularUpperLimit(btVector3(0,-1,0));
+										dof6->setAngularLowerLimit(btVector3(0,1,0));
+								
+										if (enableConstraints)
+											world1->addConstraint(dof6,true);
+										break;
+									}
+								case 2:
+								default:
+									{
+										btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XYZ);
+										dof6->setLinearLowerLimit(btVector3(0,0,0));
+										dof6->setLinearUpperLimit(btVector3(0,0,0));
+								
+										dof6->setAngularUpperLimit(btVector3(0,0,-1));
+										dof6->setAngularLowerLimit(btVector3(0,0,0));
+								
+										if (enableConstraints)
+											world1->addConstraint(dof6,true);
+									}
+								};
 								printf("Revolute/Continuous joint\n");
+							}
                             break;
                         }
                         case Joint::PRISMATIC:
                         {
-                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+							if (mappings.m_createMultiBody)
+							{
+								mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
+									parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
 
+								//mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 
+								//	parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(), 
+								//	offsetInB.getOrigin(), 
+								//	disableParentCollision);
+
+							} else
+							{
+								
+								btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
 								dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
 								dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
 
@@ -514,6 +733,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 									world1->addConstraint(dof6,true);
 
 								printf("Prismatic\n");
+							}
                             break;
                         }
                         default:
@@ -523,10 +743,63 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                     }
 
                 }
+
+				if (mappings.m_createMultiBody)
+				{
+					if (compoundShape->getNumChildShapes()>0)
+					{
+						btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mappings.m_bulletMultiBody, linkIndex-1);
+						
+						//btCompoundShape* comp = new btCompoundShape();
+						//comp->addChildShape(linkInfo->m_localVisualFrame,shape);
+						
+						gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
+						compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
+						col->setCollisionShape(compoundShape);
+
+						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);
+		
+						world1->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)
+						{
+							mappings.m_bulletMultiBody->getLink(linkIndex-1).m_collider=col;
+						} else
+						{
+							mappings.m_bulletMultiBody->setBaseCollider(col);
 						}
 					}
 				}
 				
+				//mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal);
+				//mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform);
+            }
+
+			
+        }
+    }
+
+	mappings.m_linkMasses.push_back(mass);
+
     for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
     {
         if (*child)
@@ -546,257 +819,6 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 }
 
 
-
-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)
 {
 
@@ -874,27 +896,22 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	int numJoints = (*robot).m_numJoints;
 
 	static bool useFeatherstone = false;
-    if (!useFeatherstone)
     {
         URDF2BulletMappings mappings;
+		mappings.m_createMultiBody = useFeatherstone;
+		mappings.m_totalNumJoints = numJoints;
         URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
-    }
+		if (useFeatherstone)
-
-	//the btMultiBody support is work-in-progress :-)
-#if 1
-	else
 		{
-        URDF2BulletMappings mappings;
+			btMultiBody* mb = mappings.m_bulletMultiBody;
-		
-		btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints);
-		
 			mb->setHasSelfCollision(false);
 			mb->finalizeMultiDof();
 			m_dynamicsWorld->addMultiBody(mb);
-		//m_dynamicsWorld->integrateTransforms(0.f);
-
 		}
-#endif//
+    }
+
+	//the btMultiBody support is work-in-progress :-)
+
 	useFeatherstone = !useFeatherstone;
 	printf("numJoints/DOFS = %d\n", numJoints);
 
@@ -923,7 +940,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         gfxBridge.createCollisionShapeGraphicsObject(box);
         btTransform start; start.setIdentity();
         btVector3 groundOrigin(0,0,0);
-        groundOrigin[upAxis]=-2.5;
+        groundOrigin[upAxis]=-1.5;
         start.setOrigin(groundOrigin);
         btRigidBody* body =  createRigidBody(0,start,box);
         //m_dynamicsWorld->removeRigidBody(body);
@@ -932,7 +949,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         gfxBridge.createRigidBodyGraphicsObject(body,color);
     }
 
-
+	m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
 }
 
 void ImportUrdfSetup::stepSimulation(float deltaTime)

Demos3/ImportURDFDemo/urdf_samples.h

@@ -402,7 +402,7 @@ const char* urdf_char_r2d2 = MSTRINGIFY(
 const char* urdf_char = MSTRINGIFY(
 
 								<?xml version="1.0"?>
-								   <robot name="flexible">
+<robot name="physics">
   <link name="base_link">
     <visual>
       <geometry>
@@ -412,6 +412,15 @@ const char* urdf_char = MSTRINGIFY(
         <color rgba="0 0 .8 1"/>
       </material>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.6" radius="0.2"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <link name="right_leg">
@@ -424,6 +433,17 @@ const char* urdf_char = MSTRINGIFY(
         <color rgba="1 1 1 1"/>
       </material>
     </visual>
+    <collision>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+    </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>
 
   <joint name="base_to_right_leg" type="fixed">
@@ -439,6 +459,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="white"/>
     </visual>
+    <collision>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_base_joint" type="fixed">
@@ -456,6 +485,15 @@ const char* urdf_char = MSTRINGIFY(
         <color rgba="0 0 0 1"/>
       </material>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_front_wheel_joint" type="continuous">
@@ -474,6 +512,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="black"/>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_back_wheel_joint" type="continuous">
@@ -493,6 +540,17 @@ const char* urdf_char = MSTRINGIFY(
       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
       <material name="white"/>
     </visual>
+    <collision>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+    </collision>
+    <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>
 
   <joint name="base_to_left_leg" type="fixed">
@@ -508,6 +566,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="white"/>
     </visual>
+    <collision>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_base_joint" type="fixed">
@@ -523,6 +590,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="black"/>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_front_wheel_joint" type="continuous">
@@ -541,6 +617,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="black"/>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_back_wheel_joint" type="continuous">
@@ -568,6 +653,16 @@ const char* urdf_char = MSTRINGIFY(
         <color rgba=".7 .7 .7 1"/>
       </material>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.2" radius=".01"/>
+      </geometry>
+      <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_gripper_joint" type="revolute">
@@ -582,9 +677,19 @@ const char* urdf_char = MSTRINGIFY(
     <visual>
       <origin rpy="0.0 0 0" xyz="0 0 0"/>
       <geometry>
-								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="l_finger.stl"/>
       </geometry>
     </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_tip_joint" type="fixed">
@@ -596,9 +701,19 @@ const char* urdf_char = MSTRINGIFY(
     <visual>
       <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
       <geometry>
-								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="l_finger_tip.stl"/>
       </geometry>
     </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger_tip.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_gripper_joint" type="revolute">
@@ -613,9 +728,19 @@ const char* urdf_char = MSTRINGIFY(
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
       <geometry>
-								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="l_finger.stl"/>
       </geometry>
     </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger.stl"/>
+      </geometry>
+      <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_tip_joint" type="fixed">
@@ -627,9 +752,19 @@ const char* urdf_char = MSTRINGIFY(
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
       <geometry>
-								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="l_finger_tip.stl"/>
       </geometry>
     </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger_tip.stl"/>
+      </geometry>
+      <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <link name="head">
@@ -639,6 +774,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="white"/>
     </visual>
+    <collision>
+      <geometry>
+        <sphere radius="0.2"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="head_swivel" type="continuous">
@@ -655,6 +799,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="blue"/>
     </visual>
+    <collision>
+      <geometry>
+        <box size=".08 .08 .08"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="tobox" type="fixed">
@@ -665,6 +818,7 @@ const char* urdf_char = MSTRINGIFY(
 
 </robot>
 
+
 );
 
 #endif //URDF_SAMPLES_H

Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h

@@ -89,7 +89,7 @@ public:
 		}
 	}
 
-    virtual void	physicsDebugDraw()
+    virtual void	physicsDebugDraw(int debugDrawFlags)
     {
       
 		btVector3 xUnit(1,0,0);

Demos3/bullet2/CollisionDetection/SupportFuncDemo.h

@@ -74,7 +74,7 @@ public:
         m_app->m_renderer->renderScene();
         
     }
-    virtual void	physicsDebugDraw()
+    virtual void	physicsDebugDraw(int debugDrawFlags)
     {
         int width=3;
         btVector3 from(0,0,0);

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp

@@ -612,10 +612,11 @@ void	FeatherstoneDemo1::renderScene()
 	m_glApp->m_renderer->renderScene();
 }
 
-void	FeatherstoneDemo1::physicsDebugDraw()
+void	FeatherstoneDemo1::physicsDebugDraw(int debugDrawFlags)
 {
 	if (m_dynamicsWorld)
 	{
+		m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
 		m_dynamicsWorld->debugDrawWorld();	
 	}
 }

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h

@@ -89,7 +89,7 @@ public:
 	virtual void	initPhysics();
 	virtual void	exitPhysics();
 	virtual void	renderScene();
-        virtual void    physicsDebugDraw();
+    virtual void    physicsDebugDraw(int debugDrawFlags);
 
 	virtual void	stepSimulation(float deltaTime);
 };

Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp

@@ -413,10 +413,16 @@ void LuaPhysicsSetup::stepSimulation(float deltaTime)
 
 }
 
-void    LuaPhysicsSetup::debugDraw()
+void    LuaPhysicsSetup::debugDraw(int debugDrawFlags)
 {
     if (m_dynamicsWorld)
+	{
+		if (m_dynamicsWorld->getDebugDrawer())
+		{
+			m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
+		}
         m_dynamicsWorld->debugDrawWorld();
+	}
 
 }
 

Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h

@@ -25,7 +25,7 @@ struct LuaPhysicsSetup : public CommonPhysicsSetup
 
 	virtual void stepSimulation(float deltaTime);
 
-    virtual void    debugDraw();
+    virtual void    debugDraw(int debugDrawFlags);
 
 	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
 	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);

btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp

@@ -226,9 +226,9 @@ void Bullet2RigidBodyDemo::renderScene()
 
 }
 
-void	Bullet2RigidBodyDemo::physicsDebugDraw()
+void	Bullet2RigidBodyDemo::physicsDebugDraw(int debugDrawFlags)
 {
-    m_physicsSetup->debugDraw();
+    m_physicsSetup->debugDraw(debugDrawFlags);
 }
 
 Bullet2RigidBodyDemo::~Bullet2RigidBodyDemo()

btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.h

@@ -25,7 +25,7 @@ public:
 	virtual void initPhysics();
 	virtual void exitPhysics();
 	virtual void	renderScene();
-    virtual void	physicsDebugDraw();
+    virtual void	physicsDebugDraw(int debugDrawFlags);
 	virtual void	stepSimulation(float dt);
 	virtual CommonPhysicsSetup* getPhysicsSetup()
 	{

btgui/Bullet3AppSupport/BulletDemoInterface.h

@@ -3,6 +3,7 @@
 
 struct CommonGraphicsApp;
 
+
 class BulletDemoInterface
 {
 public:
@@ -17,7 +18,7 @@ public:
 	virtual void    exitPhysics()=0;
 	virtual void	stepSimulation(float deltaTime)=0;
 	virtual void	renderScene()=0;
-	virtual void	physicsDebugDraw()=0;
+	virtual void	physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h
 	virtual bool	mouseMoveCallback(float x,float y)=0;
 	virtual bool	mouseButtonCallback(int button, int state, float x, float y)=0;
 	virtual bool	keyboardCallback(int key, int state)=0;
@@ -44,7 +45,7 @@ public:
 	virtual void	renderScene()
 	{
 	}
-	virtual void	physicsDebugDraw()
+	virtual void	physicsDebugDraw(int debugFlags)
 	{
 	}
 	virtual bool	mouseMoveCallback(float x,float y)

btgui/Bullet3AppSupport/CommonMultiBodySetup.h

@@ -125,10 +125,14 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
 		}
 	}
 
-    virtual void    debugDraw()
+    virtual void    debugDraw(int debugDrawFlags)
     {
      	if (m_dynamicsWorld)
         {
+			if (m_dynamicsWorld->getDebugDrawer())
+			{
+				m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
+			}
             m_dynamicsWorld->debugDrawWorld();
         }
 

btgui/Bullet3AppSupport/CommonPhysicsSetup.h

@@ -57,7 +57,7 @@ public:
 
 	virtual void stepSimulation(float deltaTime)=0;
 
-    virtual void    debugDraw()=0;
+    virtual void    debugDraw(int debugDrawFlags)=0;
 
 	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) = 0;
 	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;

btgui/Bullet3AppSupport/CommonRigidBodySetup.h

@@ -118,10 +118,14 @@ struct CommonRigidBodySetup : public CommonPhysicsSetup
 		}
 	}
 
-    virtual void    debugDraw()
+    virtual void    debugDraw(int debugDrawFlags)
     {
      	if (m_dynamicsWorld)
         {
+			if (m_dynamicsWorld->getDebugDrawer())
+			{
+				m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
+			}
             m_dynamicsWorld->debugDrawWorld();
         }
 

btgui/OpenGLWindow/GLInstancingRenderer.cpp

@@ -1091,8 +1091,8 @@ void GLInstancingRenderer::updateCamera(int upAxis)
 	};
 
 
-    float m_frustumZNear=1;
+    float m_frustumZNear=0.1;
-    float m_frustumZFar=10000.f;
+    float m_frustumZFar=1000.f;
 
 
 //    m_azi=m_azi+0.01;

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -34,6 +34,7 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
@@ -1306,6 +1307,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				}
 			}
 			break;
+		///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
+		case D6_SPRING_2_CONSTRAINT_TYPE:
+		{
+			{
+				btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
+				btTransform tr = p6DOF->getCalculatedTransformA();
+				if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = p6DOF->getCalculatedTransformB();
+				if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				if (drawLimits)
+				{
+					tr = p6DOF->getCalculatedTransformA();
+					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
+					btVector3 up = tr.getBasis().getColumn(2);
+					btVector3 axis = tr.getBasis().getColumn(0);
+					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
+					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
+					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					axis = tr.getBasis().getColumn(1);
+					btScalar ay = p6DOF->getAngle(1);
+					btScalar az = p6DOF->getAngle(2);
+					btScalar cy = btCos(ay);
+					btScalar sy = btSin(ay);
+					btScalar cz = btCos(az);
+					btScalar sz = btSin(az);
+					btVector3 ref;
+					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
+					ref[1] = -sz*axis[0] + cz*axis[1];
+					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
+					tr = p6DOF->getCalculatedTransformB();
+					btVector3 normal = -tr.getBasis().getColumn(0);
+					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+					btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+					if (minFi > maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
+					}
+					else if (minFi < maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
+					}
+					tr = p6DOF->getCalculatedTransformA();
+					btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
+					btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
+					getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
+				}
+			}
+			break;
+		}
 		case SLIDER_CONSTRAINT_TYPE:
 			{
 				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -151,7 +151,7 @@ public:
 	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
 
 
-	void	debugDrawConstraint(btTypedConstraint* constraint);
+	virtual void	debugDrawConstraint(btTypedConstraint* constraint);
 
 	virtual void	debugDrawWorld();
 

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -137,16 +137,18 @@ void btMultiBody::setupFixed(int i,
 						   const btVector3 &inertia,
 						   int parent,
 						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisComOffset,
+						   const btVector3 &parentComToThisPivotOffset,
+                           const btVector3 &thisPivotToThisComOffset,
 						   bool disableParentCollision)
 {
 	btAssert(m_isMultiDof);
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;
-    m_links[i].m_eVector = parentComToThisComOffset;    
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+    m_links[i].m_eVector = parentComToThisPivotOffset;    
 
 	m_links[i].m_jointType = btMultibodyLink::eFixed;
 	m_links[i].m_dofCount = 0;
@@ -156,6 +158,7 @@ void btMultiBody::setupFixed(int i,
 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
 	//
 	m_links[i].updateCacheMultiDof();
+
 	//
 	//if(m_isMultiDof)
 	//	resizeInternalMultiDofBuffers();
@@ -180,7 +183,7 @@ void btMultiBody::setupPrismatic(int i,
 	}
 
     m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;
     m_links[i].setAxisTop(0, 0., 0., 0.);
@@ -223,7 +226,7 @@ void btMultiBody::setupRevolute(int i,
 	}
 
     m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;
     m_links[i].setAxisTop(0, jointAxis);
@@ -265,7 +268,7 @@ void btMultiBody::setupSpherical(int i,
 	m_posVarCnt += 4;
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;    
     m_links[i].m_dVector = thisPivotToThisComOffset;
@@ -307,7 +310,7 @@ void btMultiBody::setupPlanar(int i,
 	m_posVarCnt += 3;
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;    
 	m_links[i].m_dVector.setZero();
@@ -365,7 +368,7 @@ btScalar btMultiBody::getLinkMass(int i) const
 
 const btVector3 & btMultiBody::getLinkInertia(int i) const
 {
-    return m_links[i].m_inertia;
+    return m_links[i].m_inertiaLocal;
 }
 
 btScalar btMultiBody::getJointPos(int i) const
@@ -506,7 +509,7 @@ btScalar btMultiBody::getKineticEnergy() const
     
     for (int i = 0; i < num_links; ++i) {
         result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
-        result += omega[i+1].dot(m_links[i].m_inertia * omega[i+1]);
+        result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
     }
 
     return 0.5f * result;
@@ -526,7 +529,7 @@ btVector3 btMultiBody::getAngularMomentum() const
 
     for (int i = 0; i < num_links; ++i) {
         rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
-        result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertia * omega[i+1])));
+        result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
     }
 
     return result;
@@ -1241,7 +1244,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 		//
 		//adding damping terms (only)
 		btScalar linDampMult = 1., angDampMult = 1.;
-		zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
+		zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
 									 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
 		
         // calculate Ihat_i^A
@@ -1252,9 +1255,9 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 												0, m_links[i].m_mass, 0,
 												0, 0, m_links[i].m_mass),
 									//
-									btMatrix3x3(m_links[i].m_inertia[0], 0, 0,
+									btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
-												0, m_links[i].m_inertia[1], 0,
+												0, m_links[i].m_inertiaLocal[1], 0,
-												0, 0, m_links[i].m_inertia[2])
+												0, 0, m_links[i].m_inertiaLocal[2])
 								);
 		//
 		//p += vhat x Ihat vhat - done in a simpler way
@@ -1694,19 +1697,19 @@ void btMultiBody::stepVelocities(btScalar dt,
             - (rot_from_world[i+1] * m_links[i].m_appliedTorque);
 		if (m_useGyroTerm)
 		{
-			zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
+			zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
 		}
 
-        zero_acc_bottom_linear[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
+        zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
 
         // calculate Ihat_i^A
         inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
         inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
                                   0, m_links[i].m_mass, 0,
                                   0, 0, m_links[i].m_mass);
-        inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
+        inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
-                                    0, m_links[i].m_inertia[1], 0,
+                                    0, m_links[i].m_inertiaLocal[1], 0,
-                                    0, 0, m_links[i].m_inertia[2]);
+                                    0, 0, m_links[i].m_inertiaLocal[2]);
     }
 
 

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -61,7 +61,8 @@ public:
 						   const btVector3 &inertia,
 						   int parent,
 						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisComOffset,
+						   const btVector3 &parentComToThisPivotOffset,
+                           const btVector3 &thisPivotToThisComOffset,
 						   bool disableParentCollision);
 
 

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -20,7 +20,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 #include "LinearMath/btQuickprof.h"
 #include "btMultiBodyConstraint.h"
-
+#include "LinearMath/btIDebugDraw.h"
 	
 
 
@@ -755,3 +755,81 @@ void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint*
 {
 	m_multiBodyConstraints.remove(constraint);
 }
+
+
+void	btMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
+
+	bool drawConstraints = false;
+	if (getDebugDrawer())
+	{
+		int mode = getDebugDrawer()->getDebugMode();
+		if (mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		{
+			drawConstraints = true;
+		}
+
+		if (drawConstraints)
+		{
+			BT_PROFILE("btMultiBody stepPositions");
+			//integrate and update the Featherstone hierarchies
+			btAlignedObjectArray<btQuaternion> world_to_local;
+			btAlignedObjectArray<btVector3> local_origin;
+
+			for (int b = 0; b<m_multiBodies.size(); b++)
+			{
+				btMultiBody* bod = m_multiBodies[b];
+				int nLinks = bod->getNumLinks();
+
+				///base + num m_links
+				world_to_local.resize(nLinks + 1);
+				local_origin.resize(nLinks + 1);
+
+					
+				world_to_local[0] = bod->getWorldToBaseRot();
+				local_origin[0] = bod->getBasePos();
+
+				
+				{
+					btVector3 posr = local_origin[0];
+					//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posr);
+					tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
+					getDebugDrawer()->drawTransform(tr, 0.1);
+
+				}
+
+				for (int k = 0; k<bod->getNumLinks(); k++)
+				{
+					const int parent = bod->getParent(k);
+					world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
+					local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
+				}
+
+
+				for (int m = 0; m<bod->getNumLinks(); m++)
+				{
+					int link = m;
+					int index = link + 1;
+
+					btVector3 posr = local_origin[index];
+					//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posr);
+					tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
+					getDebugDrawer()->drawTransform(tr, 0.1);
+				}
+			}
+		}
+	}
+
+	btDiscreteDynamicsWorld::debugDrawWorld();
+}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -54,5 +54,7 @@ public:
 	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
 	virtual void	integrateTransforms(btScalar timeStep);
+
+	virtual void	debugDrawWorld();
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -355,7 +355,7 @@ struct btMultibodyLink
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
     btScalar m_mass;         // mass of link
-    btVector3 m_inertia;   // inertia of link (local frame; diagonal)
+    btVector3 m_inertiaLocal;   // inertia of link (local frame; diagonal)
 
     int m_parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
 
@@ -440,15 +440,15 @@ struct btMultibodyLink
 	btMultibodyLink()
 		: 	m_mass(1),
 			m_parent(-1),
-			m_zeroRotParentToThis(1, 0, 0, 0),			
+			m_zeroRotParentToThis(0, 0, 0, 1),
-			m_cachedRotParentToThis(1, 0, 0, 0),			
+			m_cachedRotParentToThis(0, 0, 0, 1),
 			m_collider(0),
 			m_flags(0),
 			m_dofCount(0),
 			m_posVarCount(0),
 			m_jointType(btMultibodyLink::eInvalid)
 	{
-		m_inertia.setValue(1, 1, 1);
+		m_inertiaLocal.setValue(1, 1, 1);
 		setAxisTop(0, 0., 0., 0.);
 		setAxisBottom(0, 1., 0., 0.);
 		m_dVector.setValue(0, 0, 0);
@@ -493,7 +493,7 @@ struct btMultibodyLink
 			case ePrismatic:
 			{
 				// m_cachedRotParentToThis never changes, so no need to update
-				m_cachedRVector = m_eVector + pJointPos[0] * getAxisBottom(0);
+				m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
 
 				break;
 			}
@@ -516,7 +516,7 @@ struct btMultibodyLink
 			case eFixed:
 			{
 				m_cachedRotParentToThis = m_zeroRotParentToThis;
-				m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector);
+				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
 
 				break;
 			}

src/LinearMath/btQuickprof.cpp

@@ -43,6 +43,11 @@ static btClock gProfileClock;
 	#include <Xtl.h>
 #else //_XBOX
 	#include <windows.h>
+
+#if WINVER < 0x0600
+ULONGLONG GetTickCount64() { return GetTickCount(); }
+#endif
+
 #endif //_XBOX
 
 #include <time.h>