add 'fixed' joint for btMultiBody

improve btMultiBody version of URDF reader (still work-in-progress)
enabled planar joint for btMultiBody (untested)
enable loading from relative path for .stl meshes

Demos/CommonMultiBodySetup.h

@@ -7,6 +7,8 @@
 #include "CommonPhysicsSetup.h"
 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 
 struct CommonMultiBodySetup : public CommonPhysicsSetup
 {
@@ -21,9 +23,12 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
 	//data for picking objects
 	class btRigidBody*	m_pickedBody;
 	class btTypedConstraint* m_pickedConstraint;
+	class btMultiBodyPoint2Point*		m_pickingMultiBodyPoint2Point;
+
 	btVector3 m_oldPickingPos;
 	btVector3 m_hitPos;
 	btScalar m_oldPickingDist;
+	bool m_prevCanSleep;
 
 	CommonMultiBodySetup()
 	:m_broadphase(0),
@@ -32,7 +37,9 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
 		m_collisionConfiguration(0),
 		m_dynamicsWorld(0),
 		m_pickedBody(0),
-		m_pickedConstraint(0)
+		m_pickedConstraint(0),
+		m_pickingMultiBodyPoint2Point(0),
+		m_prevCanSleep(false)
 	{
 	}
 
@@ -157,9 +164,33 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
 					//very weak constraint for picking
 					p2p->m_setting.m_tau = 0.001f;
 				}
+			} else
+			{
+				btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
+				if (multiCol && multiCol->m_multiBody)
+				{
+						
+					m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
+					multiCol->m_multiBody->setCanSleep(false);
+
+					btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
+
+					btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos);
+					//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
+					//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
+					//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
+					//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
+					btScalar scaling=1;
+					p2p->setMaxAppliedImpulse(2*scaling);
+		
+					btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
+					world->addMultiBodyConstraint(p2p);
+					m_pickingMultiBodyPoint2Point =p2p; 
+				}
 			}
 
 
+
 			//					pickObject(pickPos, rayCallback.m_collisionObject);
 			m_oldPickingPos = rayToWorld;
 			m_hitPos = pickPos;
@@ -177,18 +208,30 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
 			if (pickCon)
 			{
 				//keep it at the same picking distance
-
+		
-				btVector3 newPivotB;
+				btVector3 dir = rayToWorld-rayFromWorld;
-
-				btVector3 dir = rayToWorld - rayFromWorld;
 				dir.normalize();
 				dir *= m_oldPickingDist;
 
-				newPivotB = rayFromWorld + dir;
+				btVector3 newPivotB = rayFromWorld + dir;
 				pickCon->setPivotB(newPivotB);
-				return true;
 			}
 		}
+		
+		if (m_pickingMultiBodyPoint2Point)
+		{
+			//keep it at the same picking distance
+
+		
+			btVector3 dir = rayToWorld-rayFromWorld;
+			dir.normalize();
+			dir *= m_oldPickingDist;
+
+			btVector3 newPivotB = rayFromWorld + dir;
+			
+			m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
+		}
+		
 		return false;
 	}
 	virtual void removePickingConstraint()
@@ -200,6 +243,14 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
 			m_pickedConstraint = 0;
 			m_pickedBody = 0;
 		}
+		if (m_pickingMultiBodyPoint2Point)
+		{
+			m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_prevCanSleep);
+			btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
+			world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
+			delete m_pickingMultiBodyPoint2Point;
+			m_pickingMultiBodyPoint2Point = 0;
+		}
 	}
 
 

Demos3/AllBullet2Demos/main.cpp

@@ -24,9 +24,11 @@ static b3AlignedObjectArray<const char*> allNames;
 bool drawGUI=true;
 extern bool useShadowMap;
 static bool wireframe=false;
-static bool pauseSimulation=false;
+static bool pauseSimulation=false;//true;
 int midiBaseIndex = 176;
 
+//#include <float.h>
+//unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM);
 
 #ifdef B3_USE_MIDI
 #include "../../btgui/MidiTest/RtMidi.h"

Demos3/ImportSTLDemo/ImportSTLSetup.cpp

@@ -4,6 +4,7 @@
 #include "OpenGLWindow/GLInstanceGraphicsShape.h"
 #include "btBulletDynamicsCommon.h"
 #include "OpenGLWindow/SimpleOpenGL3App.h"
+#include "LoadMeshFromSTL.h"
 
 ImportSTLDemo::ImportSTLDemo(SimpleOpenGL3App* app)
 :m_app(app)
@@ -16,97 +17,6 @@ ImportSTLDemo::~ImportSTLDemo()
     
 }
 
-struct MySTLTriangle
-{
-	float normal[3];
-	float vertex0[3];
-	float vertex1[3];
-	float vertex2[3];
-};
-
-GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
-{
-	GLInstanceGraphicsShape* shape = 0;
-	
-	FILE* file = fopen(relativeFileName,"rb");
-	if (file)
-	{
-		int size=0;
-		if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
-		{
-			printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
-		} else
-		{
-			if (size)
-			{
-				printf("Open STL file of %d bytes\n",size);
-				char* memoryBuffer = new char[size+1];
-				int actualBytesRead = fread(memoryBuffer,1,size,file);
-				if (actualBytesRead!=size)
-				{
-					printf("Error reading from file %s",relativeFileName);
-				} else
-				{
-					int numTriangles = *(int*)&memoryBuffer[80];
-					
-					if (numTriangles)
-					{
-						
-						shape = new GLInstanceGraphicsShape;
-//						b3AlignedObjectArray<GLInstanceVertex>*	m_vertices;
-//						int				m_numvertices;
-//						b3AlignedObjectArray<int>* 		m_indices;
-//						int				m_numIndices;
-//						float			m_scaling[4];
-						shape->m_scaling[0] = 1;
-						shape->m_scaling[1] = 1;
-						shape->m_scaling[2] = 1;
-						shape->m_scaling[3] = 1;
-						int index = 0;
-						shape->m_indices = new b3AlignedObjectArray<int>();
-						shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
-						for (int i=0;i<numTriangles;i++)
-						{
-							char* curPtr = &memoryBuffer[84+i*50];
-							MySTLTriangle* tri = (MySTLTriangle*) curPtr;
-							
-							GLInstanceVertex v0,v1,v2;
-							if (i==numTriangles-2)
-							{
-								printf("!\n");
-							}
-							v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
-							v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
-							for (int v=0;v<3;v++)
-							{
-								v0.xyzw[v] = tri->vertex0[v];
-								v1.xyzw[v] = tri->vertex1[v];
-								v2.xyzw[v] = tri->vertex2[v];
-								v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
-							}
-							v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
-							
-							shape->m_vertices->push_back(v0);
-							shape->m_vertices->push_back(v1);
-							shape->m_vertices->push_back(v2);
-							
-							shape->m_indices->push_back(index++);
-							shape->m_indices->push_back(index++);
-							shape->m_indices->push_back(index++);
-							
-						}
-					}
-				}
-				
-				delete[] memoryBuffer;
-			}
-		}
-		fclose(file);
-	}
-	shape->m_numIndices = shape->m_indices->size();
-	shape->m_numvertices = shape->m_vertices->size();
-	return shape;
-}
 
 
 void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)

Demos3/ImportSTLDemo/LoadMeshFromSTL.h

@@ -0,0 +1,101 @@
+
+#ifndef LOAD_MESH_FROM_STL_H
+#define LOAD_MESH_FROM_STL_H
+
+#include "OpenGLWindow/GLInstanceGraphicsShape.h"
+#include <stdio.h> //fopen
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+struct MySTLTriangle
+{
+	float normal[3];
+	float vertex0[3];
+	float vertex1[3];
+	float vertex2[3];
+};
+
+static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
+{
+	GLInstanceGraphicsShape* shape = 0;
+	
+	FILE* file = fopen(relativeFileName,"rb");
+	if (file)
+	{
+		int size=0;
+		if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
+		{
+			printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
+		} else
+		{
+			if (size)
+			{
+				printf("Open STL file of %d bytes\n",size);
+				char* memoryBuffer = new char[size+1];
+				int actualBytesRead = fread(memoryBuffer,1,size,file);
+				if (actualBytesRead!=size)
+				{
+					printf("Error reading from file %s",relativeFileName);
+				} else
+				{
+					int numTriangles = *(int*)&memoryBuffer[80];
+					
+					if (numTriangles)
+					{
+						
+						shape = new GLInstanceGraphicsShape;
+//						b3AlignedObjectArray<GLInstanceVertex>*	m_vertices;
+//						int				m_numvertices;
+//						b3AlignedObjectArray<int>* 		m_indices;
+//						int				m_numIndices;
+//						float			m_scaling[4];
+						shape->m_scaling[0] = 1;
+						shape->m_scaling[1] = 1;
+						shape->m_scaling[2] = 1;
+						shape->m_scaling[3] = 1;
+						int index = 0;
+						shape->m_indices = new b3AlignedObjectArray<int>();
+						shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+						for (int i=0;i<numTriangles;i++)
+						{
+							char* curPtr = &memoryBuffer[84+i*50];
+							MySTLTriangle* tri = (MySTLTriangle*) curPtr;
+							
+							GLInstanceVertex v0,v1,v2;
+							if (i==numTriangles-2)
+							{
+								printf("!\n");
+							}
+							v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
+							v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
+							for (int v=0;v<3;v++)
+							{
+								v0.xyzw[v] = tri->vertex0[v];
+								v1.xyzw[v] = tri->vertex1[v];
+								v2.xyzw[v] = tri->vertex2[v];
+								v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
+							}
+							v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
+							
+							shape->m_vertices->push_back(v0);
+							shape->m_vertices->push_back(v1);
+							shape->m_vertices->push_back(v2);
+							
+							shape->m_indices->push_back(index++);
+							shape->m_indices->push_back(index++);
+							shape->m_indices->push_back(index++);
+							
+						}
+					}
+				}
+				
+				delete[] memoryBuffer;
+			}
+		}
+		fclose(file);
+	}
+	shape->m_numIndices = shape->m_indices->size();
+	shape->m_numvertices = shape->m_vertices->size();
+	return shape;
+}
+
+#endif //LOAD_MESH_FROM_STL_H

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -1,6 +1,9 @@
 
 #include "ImportURDFSetup.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include "../ImportSTLDemo/LoadMeshFromSTL.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 
 static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
 static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
@@ -59,6 +62,8 @@ void printTree(my_shared_ptr<const Link> link,int level = 0)
 struct URDF_LinkInformation
 {
     const Link* m_thisLink;
+	int m_linkIndex;
+	int m_parentIndex;
 
     btTransform m_localInertialFrame;
     btTransform m_localVisualFrame;
@@ -80,9 +85,367 @@ struct URDF2BulletMappings
 {
     btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
     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<btScalar>			m_linkMasses;
+	btAlignedObjectArray<btVector3>			m_linkLocalDiagonalInertiaTensors;
+	btAlignedObjectArray<btTransform>		m_jointTransforms;//for root, it is identity
+	btAlignedObjectArray<int>				m_parentIndices;//for root, it is identity
+	btAlignedObjectArray<btVector3>			m_jointAxisArray;
+	btAlignedObjectArray<btTransform>			m_jointOffsetInParent;
+	btAlignedObjectArray<btTransform>			m_jointOffsetInChild;
+	btAlignedObjectArray<int>				m_jointTypeArray;
+
 };
 
-void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings)
+btCollisionShape* convertVisualToCollisionShape(const Visual* visual, const char* pathPrefix)
+{
+	btCollisionShape* shape = 0;
+
+    switch (visual->geometry->type)
+    {
+        case Geometry::CYLINDER:
+        {
+            printf("processing a cylinder\n");
+            urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
+
+            btAlignedObjectArray<btVector3> vertices;
+            //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+            int numSteps = 32;
+            for (int i=0;i<numSteps;i++)
+            {
+
+                btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.);
+                vertices.push_back(vert);
+                vert[2] = -cyl->length/2.;
+                vertices.push_back(vert);
+
+            }
+            btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+            cylZShape->initializePolyhedralFeatures();
+            //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+            //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+            cylZShape->setMargin(0.001);
+
+            shape = cylZShape;
+            break;
+        }
+        case Geometry::BOX:
+        {
+            printf("processing a box\n");
+            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);
+            shape = boxShape;
+			shape ->setMargin(0.001);
+            break;
+        }
+        case Geometry::SPHERE:
+        {
+			printf("processing a sphere\n");
+            urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
+            btScalar radius = sphere->radius;
+			btSphereShape* sphereShape = new btSphereShape(radius);
+            shape = sphereShape;
+			shape ->setMargin(0.001);
+            break;
+
+            break;
+        }
+        case Geometry::MESH:
+        {
+			if (visual->name.length())
+			{
+				printf("visual->name=%s\n",visual->name.c_str());
+			}
+			if (visual->geometry)
+			{
+				const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();
+				if (mesh->filename.length())
+				{
+					const char* filename = mesh->filename.c_str();
+					printf("mesh->filename=%s\n",filename);
+					char fullPath[1024];
+					sprintf(fullPath,"%s%s",pathPrefix,filename);
+					FILE* f = fopen(fullPath,"rb");
+					if (f)
+					{
+						fclose(f);
+						GLInstanceGraphicsShape* glmesh = LoadMeshFromSTL(fullPath);
+						if (glmesh && (glmesh->m_numvertices>0))
+						{
+							printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
+							//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
+							//convex->setUserIndex(shapeId);
+							btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
+							//cylZShape->initializePolyhedralFeatures();
+							//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+							//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+							cylZShape->setMargin(0.001);
+							shape = cylZShape;
+						} else
+						{
+							printf("issue extracting mesh from STL file %s\n", fullPath);
+						}
+
+					} else
+					{
+						printf("mesh geometry not found %s\n",fullPath);
+					}
+							
+							
+				}
+			}
+
+					
+            break;
+        }
+        default:
+        {
+            printf("Error: unknown visual geometry type\n");
+        }
+    }
+	return shape;
+}
+
+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);
+			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->visual_array.size();v++)
+	{
+		const Visual* visual = link->visual_array[v].get();
+
+		shape = convertVisualToCollisionShape(visual,pathPrefix);
+			
+		if (shape)//childShape)
+		{
+			gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape);
+
+			btVector3 color(0,0,1);
+			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(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 URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix)
 {
     btCollisionShape* shape = 0;
 
@@ -112,11 +475,13 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 		inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w));
 	}
 
-    btTransform parent2joint;
+    
+	btTransform parent2joint;
+	parent2joint.setIdentity();
 
 	if ((*link).parent_joint)
 	{
-		btTransform p2j;
+		
 		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;
 
@@ -130,72 +495,13 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 
 
     {
-        printf("converting link %s",link->name.c_str());
+        printf("converting visuals of link %s",link->name.c_str());
         for (int v=0;v<(int)link->visual_array.size();v++)
         {
             const Visual* visual = link->visual_array[v].get();
 
-
+			shape = convertVisualToCollisionShape(visual,pathPrefix);
-            switch (visual->geometry->type)
+			
-            {
-                case Geometry::CYLINDER:
-                {
-                    printf("processing a cylinder\n");
-                    urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
-
-                    btAlignedObjectArray<btVector3> vertices;
-                    //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
-                    int numSteps = 32;
-                    for (int i=0;i<numSteps;i++)
-                    {
-
-                        btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i)/numSteps)),cyl->radius*btCos(SIMD_2_PI*(float(i)/numSteps)),cyl->length/2.);
-                        vertices.push_back(vert);
-                        vert[2] = -cyl->length/2.;
-                        vertices.push_back(vert);
-
-                    }
-                    btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-                    cylZShape->initializePolyhedralFeatures();
-                    //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
-                    //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
-                    cylZShape->setMargin(0.001);
-
-                    shape = cylZShape;
-                    break;
-                }
-                case Geometry::BOX:
-                {
-                    printf("processing a box\n");
-                    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);
-                    shape = boxShape;
-                    break;
-                }
-                case Geometry::SPHERE:
-                {
-					printf("processing a sphere\n");
-                    urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
-                    btScalar radius = sphere->radius*0.8;
-					btSphereShape* sphereShape = new btSphereShape(radius);
-                    shape = sphereShape;
-                    break;
-
-                    break;
-                }
-                case Geometry::MESH:
-                {
-                    break;
-                }
-                default:
-                {
-                    printf("Error: unknown visual geometry type\n");
-                }
-            }
-
-
-
             if (shape)
             {
                 gfxBridge.createCollisionShapeGraphicsObject(shape);
@@ -245,21 +551,10 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 
                     const Joint* pj = (*link).parent_joint.get();
                     btTransform offsetInA,offsetInB;
-                    btTransform p2j; p2j.setIdentity();
+                    
-                    btVector3 p2jPos; p2jPos.setValue(pj->parent_to_joint_origin_transform.position.x,
-                                                      pj->parent_to_joint_origin_transform.position.y,
-                                                      pj->parent_to_joint_origin_transform.position.z);
-                    btQuaternion p2jOrn;p2jOrn.setValue(pj->parent_to_joint_origin_transform.rotation.x,
-                                                        pj->parent_to_joint_origin_transform.rotation.y,
-                                                        pj->parent_to_joint_origin_transform.rotation.z,
-                                                        pj->parent_to_joint_origin_transform.rotation.w);
-
-                    p2j.setOrigin(p2jPos);
-                    p2j.setRotation(p2jOrn);
-
                     offsetInA.setIdentity();
 
-                    offsetInA = pp->m_localVisualFrame.inverse()*p2j;
+                    offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
                     offsetInB.setIdentity();
                     offsetInB = visual_frame.inverse();
 
@@ -331,7 +626,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
     {
         if (*child)
         {
-            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings);
+            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix);
 
         }
         else
@@ -353,9 +648,9 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	this->createEmptyDynamicsWorld();
     gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
     m_dynamicsWorld->getDebugDrawer()->setDebugMode(
-    //btIDebugDraw::DBG_DrawConstraints
+    btIDebugDraw::DBG_DrawConstraints
     +btIDebugDraw::DBG_DrawContactPoints
-    //+btIDebugDraw::DBG_DrawAabb
+    +btIDebugDraw::DBG_DrawAabb
         );//+btIDebugDraw::DBG_DrawConstraintLimits);
 
 
@@ -365,38 +660,29 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 
 	m_dynamicsWorld->setGravity(gravity);
     //int argc=0;
-    const char* someFileName="r2d2.urdf";
+	const char* someFileName="r2d2.urdf";
+	char relativeFileName[1024];
+	
+	b3FileUtils fu;
+	bool fileFound = fu.findFile(someFileName, relativeFileName, 1024);
 
-    std::string xml_string;
-
-    const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
-	int numPrefixes = sizeof(prefix)/sizeof(const char*);
-	char relativeFileName[1024];
-	FILE* f=0;
-	bool fileFound = false;
-	int result = 0;
-
-	for (int i=0;!f && i<numPrefixes;i++)
-	{
-		sprintf(relativeFileName,"%s%s",prefix[i],someFileName);
-		f = fopen(relativeFileName,"rb");
-		if (f)
-		{
-		    fileFound = true;
-		    break;
-		}
-	}
-	if (f)
-	{
-		fclose(f);
-	}
 
+
+	std::string xml_string;
+	char pathPrefix[1024];
+	pathPrefix[0] = 0;
+	
     if (!fileFound){
         std::cerr << "URDF file not found, using a dummy test URDF" << std::endl;
         xml_string = std::string(urdf_char);
 
     } else
     {
+		
+		int maxPathLen = 1024;
+		fu.extractPath(relativeFileName,pathPrefix,maxPathLen);
+
+
         std::fstream xml_file(relativeFileName, std::fstream::in);
         while ( xml_file.good() )
         {
@@ -426,11 +712,46 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
     printTree(root_link);
     btTransform worldTrans;
 	worldTrans.setIdentity();
-
+	
+	int numJoints = (*robot).m_numJoints;
+
     if (1)
     {
         URDF2BulletMappings mappings;
-        URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings);
+        URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings,pathPrefix);
+    }
+
+	//the btMultiBody support is work-in-progress :-)
+	if (0)
+    {
+        URDF2BulletMappings mappings;
+		
+		btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints);
+		
+		mb->setHasSelfCollision(false);
+		mb->finalizeMultiDof();
+		m_dynamicsWorld->addMultiBody(mb);
+		//m_dynamicsWorld->integrateTransforms(0.f);
+
+    }
+
+	
+	printf("numJoints/DOFS = %d\n", numJoints);
+
+	if (0)
+	{
+        btVector3 halfExtents(1,1,1);
+        btBoxShape* box = new btBoxShape(halfExtents);
+        box->initializePolyhedralFeatures();
+
+        gfxBridge.createCollisionShapeGraphicsObject(box);
+        btTransform start; start.setIdentity();
+        btVector3 origin(0,0,0);
+        origin[upAxis]=5;
+        start.setOrigin(origin);
+        btRigidBody* body =  createRigidBody(1,start,box);
+        btVector3 color(0.5,0.5,0.5);
+        gfxBridge.createRigidBodyGraphicsObject(body,color);
     }
 
     {
@@ -442,7 +763,7 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         gfxBridge.createCollisionShapeGraphicsObject(box);
         btTransform start; start.setIdentity();
         btVector3 groundOrigin(0,0,0);
-        groundOrigin[upAxis]=-1.5;
+        groundOrigin[upAxis]=-2.5;
         start.setOrigin(groundOrigin);
         btRigidBody* body =  createRigidBody(0,start,box);
         btVector3 color(0.5,0.5,0.5);
@@ -451,3 +772,12 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 
 
 }
+
+void ImportUrdfDemo::stepSimulation(float deltaTime)
+{
+	if (m_dynamicsWorld)
+	{
+		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
+		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
+	}
+}

Demos3/ImportURDFDemo/ImportURDFSetup.h

@@ -11,6 +11,7 @@ public:
     virtual ~ImportUrdfDemo();
 
 	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+	virtual void stepSimulation(float deltaTime);
 };
 
 #endif //IMPORT_URDF_SETUP_H

Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp

@@ -36,6 +36,10 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 	}
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
 	{
+		//already has a graphics object?
+		if (collisionShape->getUserIndex()>=0)
+			return;
+
 		//todo: support all collision shape types
 		switch (collisionShape->getShapeType())
 		{

Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp

@@ -184,6 +184,9 @@ void TestJointTorqueSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 
                 btTransform tr;
                 tr.setIdentity();
+//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);

btgui/Gwen/Controls/ScrollControl.cpp

@@ -134,8 +134,14 @@ void ScrollControl::UpdateScrollBars()
 
 	m_InnerPanel->SetSize( Utility::Max(Width(), childrenWidth), Utility::Max(Height(), childrenHeight));
  
-	float wPercent = (float)Width()  / (float)(childrenWidth + (m_VerticalScrollBar->Hidden() ? 0 : m_VerticalScrollBar->Width()));
+	float hg = (float)(childrenWidth + (m_VerticalScrollBar->Hidden() ? 0 : m_VerticalScrollBar->Width()));
-	float hPercent = (float)Height() / (float)(childrenHeight + (m_HorizontalScrollBar->Hidden() ? 0 : m_HorizontalScrollBar->Height()));
+	if (hg==0.f)
+		hg = 0.00001f;
+	float wPercent = (float)Width()  / hg;
+	hg = (float)(childrenHeight + (m_HorizontalScrollBar->Hidden() ? 0 : m_HorizontalScrollBar->Height()));
+	if (hg==0.f)
+		hg = 0.00001f;
+	float hPercent = (float)Height() / hg;
 
 	if ( m_bCanScrollV )
 		SetVScrollRequired( hPercent >= 1 );

btgui/Gwen/Controls/TreeControl.cpp

@@ -130,20 +130,23 @@ bool TreeControl::OnKeyUp( bool bDown )
 			//float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
 			float minCoordSelectedItem = curItem*16.f;
 	//		float maxCoordSelectedItem = (curItem+1)*16.f;
+			if (contSize!=viewSize)
 			{
-				float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
-				if (newAmount<curAmount)
 				{
-					m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
+					if (newAmount<curAmount)
+					{
+						m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					}
 				}
-			}
-			{
-				int numItems = (viewSize)/16-1;
-				float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
-
-				if (newAmount>curAmount)
 				{
-					m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					int numItems = (viewSize)/16-1;
+					float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
+
+					if (newAmount>curAmount)
+					{
+						m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					}
 				}
 			}
 		}
@@ -186,20 +189,23 @@ bool TreeControl::OnKeyDown( bool bDown )
 			//float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
 			float minCoordSelectedItem = curItem*16.f;
 			//float maxCoordSelectedItem = (curItem+1)*16.f;
+			if (contSize!=viewSize)
 			{
-				float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
-				if (newAmount<curAmount)
 				{
-					m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
+					if (newAmount<curAmount)
+					{
+						m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					}
 				}
-			}
-			{
-				int numItems = (viewSize)/16-1;
-				float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
-
-				if (newAmount>curAmount)
 				{
-					m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					int numItems = (viewSize)/16-1;
+					float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
+
+					if (newAmount>curAmount)
+					{
+						m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					}
 				}
 			}
 		}
@@ -228,20 +234,23 @@ bool TreeControl::OnKeyRight( bool bDown )
 //		float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
 		float minCoordSelectedItem = curItem*16.f;
 	//	float maxCoordSelectedItem = (curItem+1)*16.f;
+		if (contSize!=viewSize)
 		{
-			float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
-			if (newAmount<curAmount)
 			{
-				m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+				float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
+				if (newAmount<curAmount)
+				{
+					m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+				}
 			}
-		}
-		{
-			int numItems = (viewSize)/16-1;
-			float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
-
-			if (newAmount>curAmount)
 			{
-				m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+				int numItems = (viewSize)/16-1;
+				float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
+
+				if (newAmount>curAmount)
+				{
+					m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+				}
 			}
 		}
 		Invalidate();
@@ -273,23 +282,25 @@ bool TreeControl::OnKeyLeft( bool bDown )
 	//	float maxCoordViewableWindow = minCoordViewableWindow+viewSize;
 		float minCoordSelectedItem = curItem*16.f;
 	//	float maxCoordSelectedItem = (curItem+1)*16.f;
-		
+		if (contSize!=viewSize)
 		{
-			float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
-			if (newAmount<curAmount)
 			{
-				m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+				float newAmount = float(minCoordSelectedItem)/(contSize-viewSize);
+				if (newAmount<curAmount)
+				{
+					m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+				}
 			}
-		}
+			{
-		{
+				int numItems = (viewSize)/16-1;
-			int numItems = (viewSize)/16-1;
+				float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
-			float newAmount = float((curItem-numItems)*16)/(contSize-viewSize);
 
-			if (newAmount>curAmount)
+				if (newAmount>curAmount)
-			{
+				{
-				m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+					m_ScrollControl->m_VerticalScrollBar->SetScrolledAmount(newAmount,true);
+				}
+				Invalidate();
 			}
-			Invalidate();
 		}
 		//viewSize/contSize
 

btgui/urdf/urdfdom/urdf_parser/src/model.cpp

@@ -116,6 +116,7 @@ my_shared_ptr<ModelInterface>  parseURDF(const std::string &xml_string)
   {
     my_shared_ptr<Link> link;
     link.reset(new Link);
+	model->m_numLinks++;
 
     try {
       parseLink(*link, link_xml);
@@ -176,6 +177,7 @@ my_shared_ptr<ModelInterface>  parseURDF(const std::string &xml_string)
   {
     my_shared_ptr<Joint> joint;
     joint.reset(new Joint);
+	model->m_numJoints++;
 
     if (parseJoint(*joint, joint_xml))
     {

btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/link.h

@@ -223,6 +223,8 @@ public:
   std::vector<my_shared_ptr<Joint> > child_joints;
   std::vector<my_shared_ptr<Link> > child_links;
 
+  mutable int m_link_index;
+
   const Link* getParent() const
   {return parent_link_;}
 
@@ -242,6 +244,8 @@ public:
     this->child_links.clear();
     this->collision_array.clear();
     this->visual_array.clear();
+	this->m_link_index=-1;
+	this->parent_link_ = NULL;
   };
 
 private:

btgui/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h

@@ -82,6 +82,8 @@ public:
   
   void clear()
   {
+	m_numLinks=0;
+	m_numJoints = 0;
     name_.clear();
     this->links_.clear();
     this->joints_.clear();
@@ -132,6 +134,7 @@ public:
         this->getLink(child_link_name, child_link);
         if (!child_link)
         {
+			printf("Error: child link [%s] of joint [%s] not found\n", child_link_name,joint->first );
 			assert(0);
 //          throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
         }
@@ -206,6 +209,8 @@ public:
   /// \brief The root is always a link (the parent of the tree describing the robot)
   my_shared_ptr<Link> root_link_;
 
+  int m_numLinks;//includes parent
+  int m_numJoints;
 
 
 };

data/r2d2.urdf

@@ -0,0 +1,414 @@
+<?xml version="1.0"?>
+<robot name="physics">
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <cylinder length="0.6" radius="0.2"/>
+      </geometry>
+      <material name="blue">
+        <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">
+    <visual>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+      <material name="white">
+        <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"/>
+      <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">
+    <parent link="base_link"/>
+    <child link="right_leg"/>
+    <origin xyz="0.22 0 .25"/>
+  </joint>
+
+  <link name="right_base">
+    <visual>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </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">
+    <parent link="right_leg"/>
+    <child link="right_base"/>
+    <origin xyz="0 0 -0.6"/>
+  </joint>
+
+  <link name="right_front_wheel">
+    <visual>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+      <material name="black">
+        <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">
+    <axis xyz="0 0 1"/>
+    <parent link="right_base"/>
+    <child link="right_front_wheel"/>
+    <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+
+  <link name="right_back_wheel">
+    <visual>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </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">
+    <axis xyz="0 0 1"/>
+    <parent link="right_base"/>
+    <child link="right_back_wheel"/>
+    <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+
+  <link name="left_leg">
+    <visual>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <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"/>
+    </inertial>
+  </link>
+
+  <joint name="base_to_left_leg" type="fixed">
+    <parent link="base_link"/>
+    <child link="left_leg"/>
+    <origin xyz="-0.22 0 .25"/>
+  </joint>
+
+  <link name="left_base">
+    <visual>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </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">
+    <parent link="left_leg"/>
+    <child link="left_base"/>
+    <origin xyz="0 0 -0.6"/>
+  </joint>
+
+  <link name="left_front_wheel">
+    <visual>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </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">
+    <axis xyz="0 0 1"/>
+    <parent link="left_base"/>
+    <child link="left_front_wheel"/>
+    <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+
+  <link name="left_back_wheel">
+    <visual>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </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">
+    <axis xyz="0 0 1"/>
+    <parent link="left_base"/>
+    <child link="left_back_wheel"/>
+    <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+    <limit effort="100" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+  </joint>
+  <joint name="gripper_extension" type="prismatic">
+    <parent link="base_link"/>
+    <child link="gripper_pole"/>
+    <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
+    <origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
+  </joint>
+
+  <link name="gripper_pole">
+    <visual>
+      <geometry>
+        <cylinder length="0.2" radius=".01"/>
+      </geometry>
+      <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+      <material name="Gray">
+        <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">
+    <axis xyz="0 0 1"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
+    <parent link="gripper_pole"/>
+    <child link="left_gripper"/>
+  </joint>
+
+  <link name="left_gripper">
+    <visual>
+      <origin rpy="0.0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <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">
+    <parent link="left_gripper"/>
+    <child link="left_tip"/>
+  </joint>
+
+  <link name="left_tip">
+    <visual>
+      <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+      <geometry>
+        <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">
+    <axis xyz="0 0 -1"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
+    <parent link="gripper_pole"/>
+    <child link="right_gripper"/>
+  </joint>
+
+  <link name="right_gripper">
+    <visual>
+      <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+      <geometry>
+        <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">
+    <parent link="right_gripper"/>
+    <child link="right_tip"/>
+  </joint>
+
+  <link name="right_tip">
+    <visual>
+      <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+      <geometry>
+        <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">
+    <visual>
+      <geometry>
+        <sphere radius="0.2"/>
+      </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">
+    <parent link="base_link"/>
+    <child link="head"/>
+    <axis xyz="0 0 1"/>
+    <origin xyz="0 0 0.3"/>
+  </joint>
+
+  <link name="box">
+    <visual>
+      <geometry>
+        <box size=".08 .08 .08"/>
+      </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">
+    <parent link="head"/>
+    <child link="box"/>
+    <origin xyz="0 0.1414 0.1414"/>
+  </joint>
+
+</robot>

src/Bullet3Common/b3FileUtils.h

@@ -0,0 +1,105 @@
+#ifndef B3_FILE_UTILS_H
+#define B3_FILE_UTILS_H
+
+#include <stdio.h>
+#include "b3Scalar.h"
+
+struct b3FileUtils
+{
+	b3FileUtils()
+	{
+	}
+	virtual ~b3FileUtils()
+	{
+	}
+
+	bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
+	{
+		
+			const char* prefix[]={"","./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
+			int numPrefixes = sizeof(prefix)/sizeof(const char*);
+	
+			FILE* f=0;
+			bool fileFound = false;
+			int result = 0;
+
+			for (int i=0;!f && i<numPrefixes;i++)
+			{
+#ifdef _WIN32
+				sprintf_s(relativeFileName,maxRelativeFileNameMaxLen,"%s%s",prefix[i],orgFileName);
+#else
+				sprintf(relativeFileName,"%s%s",prefix[i],orgFileName);
+#endif
+				f = fopen(relativeFileName,"rb");
+				if (f)
+				{
+					fileFound = true;
+					break;
+				}
+			}
+			if (f)
+			{
+				fclose(f);
+			}
+	
+		return fileFound;
+	}
+
+	static const char* strip2(const char* name, const char* pattern)
+	{
+		size_t const patlen = strlen(pattern);
+		size_t patcnt = 0;
+		const char * oriptr;
+		const char * patloc;
+		// find how many times the pattern occurs in the original string
+		for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
+		{
+			patcnt++;
+		}
+		return oriptr;
+	}
+
+
+	void extractPath(const char* fileName, char* path, int maxPathLength)
+	{
+		const char* stripped = strip2(fileName, "/");
+		stripped = strip2(stripped, "\\");
+
+		ptrdiff_t len = stripped-fileName;
+		b3Assert((len+1)<maxPathLength);
+
+		if (len && ((len+1)<maxPathLength))
+		{
+
+			for (int i=0;i<len;i++)
+			{
+				path[i] = fileName[i];
+			}
+			path[len]=0;
+		} else
+		{
+#ifdef _WIN32
+			sprintf_s(path, maxPathLength,"");
+#else
+			sprintf(path, maxPathLength,"");
+#endif
+		}
+	}
+
+	/*static const char* strip2(const char* name, const char* pattern)
+	{
+		size_t const patlen = strlen(pattern);
+		size_t patcnt = 0;
+		const char * oriptr;
+		const char * patloc;
+		// find how many times the pattern occurs in the original string
+		for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
+		{
+			patcnt++;
+		}
+		return oriptr;
+	}
+	*/
+
+};
+#endif //B3_FILE_UTILS_H

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -132,6 +132,38 @@ btMultiBody::~btMultiBody()
 {
 }
 
+void btMultiBody::setupFixed(int i,
+						   btScalar mass,
+						   const btVector3 &inertia,
+						   int parent,
+						   const btQuaternion &rotParentToThis,
+						   const btVector3 &parentComToThisComOffset,
+						   bool disableParentCollision)
+{
+	btAssert(m_isMultiDof);
+
+	m_links[i].m_mass = mass;
+    m_links[i].m_inertia = inertia;
+    m_links[i].m_parent = parent;
+    m_links[i].m_zeroRotParentToThis = rotParentToThis;
+    m_links[i].m_eVector = parentComToThisComOffset;    
+
+	m_links[i].m_jointType = btMultibodyLink::eFixed;
+	m_links[i].m_dofCount = 0;
+	m_links[i].m_posVarCount = 0;
+
+	if (disableParentCollision)
+		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
+	//
+	m_links[i].updateCacheMultiDof();
+	//
+	//if(m_isMultiDof)
+	//	resizeInternalMultiDofBuffers();
+	//
+	updateLinksDofOffsets();
+
+}
+
 void btMultiBody::setupPrismatic(int i,
                                btScalar mass,
                                const btVector3 &inertia,
@@ -290,12 +322,15 @@ void btMultiBody::setupPlanar(int i,
 	m_links[i].m_jointType = btMultibodyLink::ePlanar;
 	m_links[i].m_dofCount = 3;
 	m_links[i].m_posVarCount = 3;
-	m_links[i].getAxisTop(0) = rotationAxis.normalized();
+	btVector3 n=rotationAxis.normalized();
-	m_links[i].getAxisTop(1).setZero();
+	m_links[i].setAxisTop(0, n[0],n[1],n[2]);
-	m_links[i].getAxisTop(2).setZero();
+	m_links[i].setAxisTop(1,0,0,0);
-	m_links[i].getAxisBottom(0).setZero();
+	m_links[i].setAxisTop(2,0,0,0);
-	m_links[i].getAxisBottom(1) = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
+	m_links[i].setAxisBottom(0,0,0,0);
-	m_links[i].getAxisBottom(2) = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
+	btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
+	m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
+	cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
+	m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
 	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
 	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
 
@@ -1171,41 +1206,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 
 		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
 		fromWorld.m_rotMat = rot_from_world[i+1];
-
+		fromParent.transform(spatVel[parent+1], spatVel[i+1]);
-		////clamp parent's omega
-		//btScalar parOmegaMod = spatVel[parent+1].getAngular().length();
-		//btScalar parOmegaModMax = 0.1;
-		//if(parOmegaMod > parOmegaModMax)
-		//{
-		//	//btSpatialMotionVector clampedParVel(spatVel[parent+1].getAngular() * parOmegaModMax / parOmegaMod, spatVel[parent+1].getLinear());
-		//	btSpatialMotionVector clampedParVel; clampedParVel = spatVel[parent+1] * (parOmegaModMax / parOmegaMod);
-		//	fromParent.transform(clampedParVel, spatVel[i+1]);
-		//	spatVel[parent+1] *= (parOmegaModMax / parOmegaMod);
-		//}
-		//else
-		{
-			// vhat_i = i_xhat_p(i) * vhat_p(i)		
-			fromParent.transform(spatVel[parent+1], spatVel[i+1]);
-			//nice alternative below (using operator *) but it generates temps
-		}
-		//////////////////////////////////////////////////////////////		
-        
-		//if(m_links[i].m_jointType == btMultibodyLink::eRevolute || m_links[i].m_jointType == btMultibodyLink::eSpherical)
-		//{
-		//	btScalar mod2 = 0;
-		//	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-		//		mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof];
-
-		//	btScalar angvel = sqrt(mod2);
-		//	btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075;
-		//	btScalar step = 1; //dt
-		//	if (angvel*step > maxAngVel)
-		//	{
-		//		btScalar * qd =	getJointVelMultiDof(i);
-		//		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)		
-		//			qd[dof] *= (maxAngVel/step) /angvel;			
-		//	}
-		//}
 
 		// now set vhat_i to its true value by doing
         // vhat_i += qidot * shat_i			
@@ -1717,13 +1718,14 @@ void btMultiBody::stepVelocities(btScalar dt,
         h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
 		btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
         D[i] = val;
+		
 		Y[i] = m_links[i].m_jointTorque[0]
             - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
             - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
 
         const int parent = m_links[i].m_parent;
 
-        
+        btAssert(D[i]!=0.f);
         // Ip += pXi * (Ii - hi hi' / Di) * iXp
         const btScalar one_over_di = 1.0f / D[i];
 
@@ -2637,7 +2639,7 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
     btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
 
     scratch_r.resize(m_dofCount);
-    btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
+    btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
 
     btMatrix3x3 * rot_from_world = &scratch_m[0];
 
@@ -2717,9 +2719,9 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
 				case btMultibodyLink::ePlanar:
 				{
-					results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
+					results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
-					results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisBottom(1));
+					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
-					results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisBottom(2));
+					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
 
 					break;
 				}

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -56,7 +56,16 @@ public:
 
     ~btMultiBody();	
     
-    void setupPrismatic(int i,             // 0 to num_links-1
+	void btMultiBody::setupFixed(int linkIndex,
+						   btScalar mass,
+						   const btVector3 &inertia,
+						   int parent,
+						   const btQuaternion &rotParentToThis,
+						   const btVector3 &parentComToThisComOffset,
+						   bool disableParentCollision);
+
+
+    void setupPrismatic(int linkIndex,             // 0 to num_links-1
                         btScalar mass,
                         const btVector3 &inertia,       // in my frame; assumed diagonal
                         int parent,
@@ -66,17 +75,17 @@ public:
 						bool disableParentCollision=false
 						);
 
-    void setupRevolute(int i,            // 0 to num_links-1
+    void setupRevolute(int linkIndex,            // 0 to num_links-1
                        btScalar mass,
                        const btVector3 &inertia,
-                       int parent,
+                       int parentIndex,
                        const btQuaternion &rotParentToThis,  // rotate points in parent frame to this frame, when q = 0
                        const btVector3 &jointAxis,    // in my frame
                        const btVector3 &parentComToThisPivotOffset,    // vector from parent COM to joint axis, in PARENT frame
                        const btVector3 &thisPivotToThisComOffset,       // vector from joint axis to my COM, in MY frame
 					   bool disableParentCollision=false);
 
-	void setupSpherical(int i,											// 0 to num_links-1
+	void setupSpherical(int linkIndex,											// 0 to num_links-1
                        btScalar mass,
                        const btVector3 &inertia,
                        int parent,

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -38,7 +38,7 @@ protected:
 	virtual void	calculateSimulationIslands();
 	virtual void	updateActivationState(btScalar timeStep);
 	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
-	virtual void	integrateTransforms(btScalar timeStep);
+	
 public:
 
 	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
@@ -52,5 +52,7 @@ public:
 	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
 	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
+
+	virtual void	integrateTransforms(btScalar timeStep);
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -25,7 +25,7 @@ enum	btMultiBodyLinkFlags
 	BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
 };
 
-//#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
+#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
 #define TEST_SPATIAL_ALGEBRA_LAYER
 
 //
@@ -368,7 +368,9 @@ struct btMultibodyLink
     // revolute: vector from parent's COM to the pivot point, in PARENT's frame.
     btVector3 m_eVector;
 
+#ifdef TEST_SPATIAL_ALGEBRA_LAYER
 	btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
+#endif
 
 	enum eFeatherstoneJointType
 	{
@@ -378,6 +380,7 @@ struct btMultibodyLink
 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
 		ePlanar = 3,
 #endif
+		eFixed = 4,
 		eInvalid
 	};
 
@@ -505,11 +508,18 @@ struct btMultibodyLink
 			case ePlanar:
 			{
 				m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;				
-				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * m_axesBottom[1] + pJointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector);				
+				m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);				
 
 				break;
 			}
 #endif
+			case eFixed:
+			{
+				m_cachedRotParentToThis = m_zeroRotParentToThis;
+				m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector);
+
+				break;
+			}
 			default:
 			{
 				//invalid type