Merge remote-tracking branch 'bp/master' into pullRequest

CMakeLists.txt

@@ -108,7 +108,7 @@ IF(MSVC)
 	IF (CMAKE_CL_64)
 	  ADD_DEFINITIONS(-D_WIN64)
 	ELSE()
-	  OPTION(USE_MSVC_SSE "Use MSVC /arch:sse option"	ON)
+	  OPTION(USE_MSVC_SSE "Use MSVC /arch:sse option"	OFF)
 	  option(USE_MSVC_SSE2 "Compile your program with SSE2 instructions" ON)
 
 	  IF (USE_MSVC_SSE)

Extras/Serialize/BulletFileLoader/autogenerated/bullet.h

@@ -63,6 +63,8 @@ typedef struct bInvalidHandle {
     class btCapsuleShapeData;
     class btTriangleInfoData;
     class btTriangleInfoMapData;
+    class btPersistentManifoldDoubleData;
+    class btPersistentManifoldFloatData;
     class btGImpactMeshShapeData;
     class btConvexHullShapeData;
     class btCollisionObjectDoubleData;
@@ -514,6 +516,96 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btPersistentManifoldDoubleData
+    {
+    public:
+        btVector3DoubleData m_pointCacheLocalPointA[4];
+        btVector3DoubleData m_pointCacheLocalPointB[4];
+        btVector3DoubleData m_pointCachePositionWorldOnA[4];
+        btVector3DoubleData m_pointCachePositionWorldOnB[4];
+        btVector3DoubleData m_pointCacheNormalWorldOnB[4];
+        btVector3DoubleData m_pointCacheLateralFrictionDir1[4];
+        btVector3DoubleData m_pointCacheLateralFrictionDir2[4];
+        double m_pointCacheDistance[4];
+        double m_pointCacheAppliedImpulse[4];
+        double m_pointCacheCombinedFriction[4];
+        double m_pointCacheCombinedRollingFriction[4];
+        double m_pointCacheCombinedSpinningFriction[4];
+        double m_pointCacheCombinedRestitution[4];
+        int m_pointCachePartId0[4];
+        int m_pointCachePartId1[4];
+        int m_pointCacheIndex0[4];
+        int m_pointCacheIndex1[4];
+        int m_pointCacheContactPointFlags[4];
+        double m_pointCacheAppliedImpulseLateral1[4];
+        double m_pointCacheAppliedImpulseLateral2[4];
+        double m_pointCacheContactMotion1[4];
+        double m_pointCacheContactMotion2[4];
+        double m_pointCacheContactCFM[4];
+        double m_pointCacheCombinedContactStiffness1[4];
+        double m_pointCacheContactERP[4];
+        double m_pointCacheCombinedContactDamping1[4];
+        double m_pointCacheFrictionCFM[4];
+        int m_pointCacheLifeTime[4];
+        int m_numCachedPoints;
+        int m_companionIdA;
+        int m_companionIdB;
+        int m_index1a;
+        int m_objectType;
+        double m_contactBreakingThreshold;
+        double m_contactProcessingThreshold;
+        int m_padding;
+        void *m_body0;
+        void *m_body1;
+    };
+
+
+// -------------------------------------------------- //
+    class btPersistentManifoldFloatData
+    {
+    public:
+        btVector3FloatData m_pointCacheLocalPointA[4];
+        btVector3FloatData m_pointCacheLocalPointB[4];
+        btVector3FloatData m_pointCachePositionWorldOnA[4];
+        btVector3FloatData m_pointCachePositionWorldOnB[4];
+        btVector3FloatData m_pointCacheNormalWorldOnB[4];
+        btVector3FloatData m_pointCacheLateralFrictionDir1;
+        btVector3FloatData m_pointCacheLateralFrictionDir2;
+        float m_pointCacheDistance[4];
+        float m_pointCacheAppliedImpulse[4];
+        float m_pointCacheCombinedFriction[4];
+        float m_pointCacheCombinedRollingFriction[4];
+        float m_pointCacheCombinedSpinningFriction[4];
+        float m_pointCacheCombinedRestitution[4];
+        int m_pointCachePartId0[4];
+        int m_pointCachePartId1[4];
+        int m_pointCacheIndex0[4];
+        int m_pointCacheIndex1[4];
+        int m_pointCacheContactPointFlags[4];
+        float m_pointCacheAppliedImpulseLateral1[4];
+        float m_pointCacheAppliedImpulseLateral2[4];
+        float m_pointCacheContactMotion1[4];
+        float m_pointCacheContactMotion2[4];
+        float m_pointCacheContactCFM[4];
+        float m_pointCacheCombinedContactStiffness1[4];
+        float m_pointCacheContactERP[4];
+        float m_pointCacheCombinedContactDamping1[4];
+        float m_pointCacheFrictionCFM[4];
+        int m_pointCacheLifeTime[4];
+        int m_numCachedPoints;
+        int m_companionIdA;
+        int m_companionIdB;
+        int m_index1a;
+        int m_objectType;
+        float m_contactBreakingThreshold;
+        float m_contactProcessingThreshold;
+        int m_padding;
+        void *m_body0;
+        void *m_body1;
+    };
+
+
 // -------------------------------------------------- //
     class btGImpactMeshShapeData
     {
@@ -568,7 +660,9 @@ typedef struct bInvalidHandle {
         int m_activationState1;
         int m_internalType;
         int m_checkCollideWith;
-        char m_padding[4];
+        int m_collisionFilterGroup;
+        int m_collisionFilterMask;
+        int m_uniqueId;
     };
 
 
@@ -602,7 +696,9 @@ typedef struct bInvalidHandle {
         int m_activationState1;
         int m_internalType;
         int m_checkCollideWith;
-        char m_padding[4];
+        int m_collisionFilterGroup;
+        int m_collisionFilterMask;
+        int m_uniqueId;
     };
 
 
@@ -1416,15 +1512,17 @@ typedef struct bInvalidHandle {
     class btMultiBodyDoubleData
     {
     public:
-        btTransformDoubleData m_baseWorldTransform;
+        btVector3DoubleData m_baseWorldPosition;
+        btQuaternionDoubleData m_baseWorldOrientation;
+        btVector3DoubleData m_baseLinearVelocity;
+        btVector3DoubleData m_baseAngularVelocity;
         btVector3DoubleData m_baseInertia;
         double m_baseMass;
+        int m_numLinks;
+        char m_padding[4];
         char *m_baseName;
         btMultiBodyLinkDoubleData *m_links;
         btCollisionObjectDoubleData *m_baseCollider;
-        char *m_paddingPtr;
-        int m_numLinks;
-        char m_padding[4];
     };
 
 
@@ -1432,13 +1530,16 @@ typedef struct bInvalidHandle {
     class btMultiBodyFloatData
     {
     public:
-        char *m_baseName;
+        btVector3FloatData m_baseWorldPosition;
-        btMultiBodyLinkFloatData *m_links;
+        btQuaternionFloatData m_baseWorldOrientation;
-        btCollisionObjectFloatData *m_baseCollider;
+        btVector3FloatData m_baseLinearVelocity;
-        btTransformFloatData m_baseWorldTransform;
+        btVector3FloatData m_baseAngularVelocity;
         btVector3FloatData m_baseInertia;
         float m_baseMass;
         int m_numLinks;
+        char *m_baseName;
+        btMultiBodyLinkFloatData *m_links;
+        btCollisionObjectFloatData *m_baseCollider;
     };
 
 

Extras/Serialize/BulletFileLoader/btBulletFile.cpp

@@ -176,6 +176,10 @@ void btBulletFile::parseData()
 					//	listID->push_back((bStructHandle*)id);
 				}
 
+				if (dataChunk.code == BT_CONTACTMANIFOLD_CODE)
+				{
+					m_contactManifolds.push_back((bStructHandle*)id);
+				}
 				if (dataChunk.code == BT_MULTIBODY_CODE)
 				{
 					m_multiBodies.push_back((bStructHandle*)id);

Extras/Serialize/BulletFileLoader/btBulletFile.h

@@ -59,6 +59,8 @@ namespace bParse {
 
 		btAlignedObjectArray<bStructHandle*>	m_dynamicsWorldInfo;
 
+		btAlignedObjectArray<bStructHandle*>	m_contactManifolds;
+
 		btAlignedObjectArray<char*>				m_dataBlocks;
 		btBulletFile();
 

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -33,171 +33,484 @@ void btMultiBodyWorldImporter::deleteAllData()
 }
 
 
+static btCollisionObjectDoubleData* getBody0FromContactManifold(btPersistentManifoldDoubleData* manifold)
+{
+	return (btCollisionObjectDoubleData*)manifold->m_body0;
+}
+static btCollisionObjectDoubleData* getBody1FromContactManifold(btPersistentManifoldDoubleData* manifold)
+{
+	return (btCollisionObjectDoubleData*)manifold->m_body1;
+}
+static btCollisionObjectFloatData* getBody0FromContactManifold(btPersistentManifoldFloatData* manifold)
+{
+	return (btCollisionObjectFloatData*)manifold->m_body0;
+}
+static btCollisionObjectFloatData* getBody1FromContactManifold(btPersistentManifoldFloatData* manifold)
+{
+	return (btCollisionObjectFloatData*)manifold->m_body1;
+}
+
+
+template<class T> void syncContactManifolds(T** contactManifolds, int numContactManifolds, btMultiBodyWorldImporterInternalData* m_data)
+{
+	m_data->m_mbDynamicsWorld->updateAabbs();
+	m_data->m_mbDynamicsWorld->computeOverlappingPairs();
+	btDispatcher* dispatcher = m_data->m_mbDynamicsWorld->getDispatcher();
+
+	btDispatcherInfo& dispatchInfo = m_data->m_mbDynamicsWorld->getDispatchInfo();
+
+
+	if (dispatcher)
+	{
+		btOverlappingPairCache* pairCache = m_data->m_mbDynamicsWorld->getBroadphase()->getOverlappingPairCache();
+		if (dispatcher)
+		{
+			dispatcher->dispatchAllCollisionPairs(pairCache, dispatchInfo, dispatcher);
+		}
+		int numExistingManifolds = m_data->m_mbDynamicsWorld->getDispatcher()->getNumManifolds();
+		btManifoldArray manifoldArray;
+		for (int i = 0; i < pairCache->getNumOverlappingPairs(); i++)
+		{
+			btBroadphasePair& pair = pairCache->getOverlappingPairArray()[i];
+			if (pair.m_algorithm)
+			{
+				pair.m_algorithm->getAllContactManifolds(manifoldArray);
+				//for each existing manifold, search a matching manifoldData and reconstruct
+				for (int m = 0; m < manifoldArray.size(); m++)
+				{
+					btPersistentManifold* existingManifold = manifoldArray[m];
+					int uid0 = existingManifold->getBody0()->getBroadphaseHandle()->m_uniqueId;
+					int uid1 = existingManifold->getBody1()->getBroadphaseHandle()->m_uniqueId;
+					int matchingManifoldIndex = -1;
+					for (int q = 0; q < numContactManifolds; q++)
+					{
+						if (uid0 == getBody0FromContactManifold(contactManifolds[q])->m_uniqueId && uid1 == getBody1FromContactManifold(contactManifolds[q])->m_uniqueId)
+						{
+							matchingManifoldIndex = q;
+						}
+					}
+					if (matchingManifoldIndex >= 0)
+					{
+						existingManifold->deSerialize(contactManifolds[matchingManifoldIndex]);
+					}
+					else
+					{
+						existingManifold->setNumContacts(0);
+						//printf("Issue: cannot find maching contact manifold (%d, %d), may cause issues in determinism.\n", uid0, uid1);
+					}
+
+					manifoldArray.clear();
+				}
+			}
+		}
+	}
+
+}
+
+template<class T>  void syncMultiBody(T* mbd, btMultiBody* mb, btMultiBodyWorldImporterInternalData* m_data, btAlignedObjectArray<btQuaternion>& scratchQ, btAlignedObjectArray<btVector3>& scratchM)
+{
+	bool isFixedBase = mbd->m_baseMass == 0;
+	bool canSleep = false;
+	btVector3 baseInertia;
+	baseInertia.deSerialize(mbd->m_baseInertia);
+
+	btVector3 baseWorldPos;
+	baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
+	mb->setBasePos(baseWorldPos);
+	btQuaternion baseWorldRot;
+	baseWorldRot.deSerialize(mbd->m_baseWorldOrientation);
+	mb->setWorldToBaseRot(baseWorldRot.inverse());
+	btVector3 baseLinVal;
+	baseLinVal.deSerialize(mbd->m_baseLinearVelocity);
+	btVector3 baseAngVel;
+	baseAngVel.deSerialize(mbd->m_baseAngularVelocity);
+	mb->setBaseVel(baseLinVal);
+	mb->setBaseOmega(baseAngVel);
+
+	for (int i = 0; i < mbd->m_numLinks; i++)
+	{
+		switch (mbd->m_links[i].m_jointType)
+		{
+		case btMultibodyLink::eFixed:
+		{
+			break;
+		}
+		case btMultibodyLink::ePrismatic:
+		{
+			mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+			mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+			break;
+		}
+		case btMultibodyLink::eRevolute:
+		{
+			mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+			mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+			break;
+		}
+		case btMultibodyLink::eSpherical:
+		{
+			btScalar jointPos[3] = { (btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2] };
+			btScalar jointVel[3] = { (btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2] };
+			mb->setJointPosMultiDof(i, jointPos);
+			mb->setJointVelMultiDof(i, jointVel);
+
+			break;
+		}
+		case btMultibodyLink::ePlanar:
+		{
+			break;
+		}
+		default:
+		{
+		}
+		}
+	}
+	mb->forwardKinematics(scratchQ, scratchM);
+	mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
+}
+
+template<class T>  void convertMultiBody(T* mbd, btMultiBodyWorldImporterInternalData* m_data)
+{
+	bool isFixedBase = mbd->m_baseMass == 0;
+	bool canSleep = false;
+	btVector3 baseInertia;
+	baseInertia.deSerialize(mbd->m_baseInertia);
+	btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
+	mb->setHasSelfCollision(false);
+
+	btVector3 baseWorldPos;
+	baseWorldPos.deSerialize(mbd->m_baseWorldPosition);
+
+	btQuaternion baseWorldOrn;
+	baseWorldOrn.deSerialize(mbd->m_baseWorldOrientation);
+	mb->setBasePos(baseWorldPos);
+	mb->setWorldToBaseRot(baseWorldOrn.inverse());
+
+	m_data->m_mbMap.insert(mbd, mb);
+	for (int i = 0; i < mbd->m_numLinks; i++)
+	{
+		btVector3 localInertiaDiagonal;
+		localInertiaDiagonal.deSerialize(mbd->m_links[i].m_linkInertia);
+		btQuaternion parentRotToThis;
+		parentRotToThis.deSerialize(mbd->m_links[i].m_zeroRotParentToThis);
+		btVector3 parentComToThisPivotOffset;
+		parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisComOffset);
+		btVector3 thisPivotToThisComOffset;
+		thisPivotToThisComOffset.deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset);
+
+		switch (mbd->m_links[i].m_jointType)
+		{
+		case btMultibodyLink::eFixed:
+		{
+
+
+			mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
+			//search for the collider
+			//mbd->m_links[i].m_linkCollider
+			break;
+		}
+		case btMultibodyLink::ePrismatic:
+		{
+			btVector3 jointAxis;
+			jointAxis.deSerialize(mbd->m_links[i].m_jointAxisBottom[0]);
+			bool disableParentCollision = true;//todo
+			mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+			mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+			mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+			break;
+		}
+		case btMultibodyLink::eRevolute:
+		{
+			btVector3 jointAxis;
+			jointAxis.deSerialize(mbd->m_links[i].m_jointAxisTop[0]);
+			bool disableParentCollision = true;//todo
+			mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+			mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+			mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
+			break;
+		}
+		case btMultibodyLink::eSpherical:
+		{
+			btAssert(0);
+			bool disableParentCollision = true;//todo
+			mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+			btScalar jointPos[3] = { (btScalar)mbd->m_links[i].m_jointPos[0], (btScalar)mbd->m_links[i].m_jointPos[1], (btScalar)mbd->m_links[i].m_jointPos[2] };
+			btScalar jointVel[3] = { (btScalar)mbd->m_links[i].m_jointVel[0], (btScalar)mbd->m_links[i].m_jointVel[1], (btScalar)mbd->m_links[i].m_jointVel[2] };
+			mb->setJointPosMultiDof(i, jointPos);
+			mb->setJointVelMultiDof(i, jointVel);
+
+			break;
+		}
+		case btMultibodyLink::ePlanar:
+		{
+			btAssert(0);
+			break;
+		}
+		default:
+		{
+			btAssert(0);
+		}
+		}
+	}
+}
 
 bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletFile2)
 {
-	bool result = btBulletWorldImporter::convertAllObjects(bulletFile2);
+	bool result = false;
+	btAlignedObjectArray<btQuaternion> scratchQ;
+	btAlignedObjectArray<btVector3> scratchM;
 
-	
+	if (m_importerFlags&eRESTORE_EXISTING_OBJECTS)
-	//convert all multibodies
-	for (int i=0;i<bulletFile2->m_multiBodies.size();i++)
 	{
+		//check if the snapshot is valid for the existing world
+		//equal number of objects, # links etc
+		if ((bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies()))
+		{
+			result = false;
+			return result;
+		}
+		result = true;
 
+		//convert all multibodies
 		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
 		{
-			btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*) bulletFile2->m_multiBodies[i];
-			bool isFixedBase = mbd->m_baseMass==0;
-			bool canSleep = false;
-			btVector3 baseInertia;
-			baseInertia.deSerializeDouble(mbd->m_baseInertia);
-			btMultiBody* mb = new btMultiBody(mbd->m_numLinks,mbd->m_baseMass,baseInertia,isFixedBase,canSleep);
-			mb->setHasSelfCollision(false);
 
-			m_data->m_mbMap.insert(mbd,mb);
+			//for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
-			for (int i=0;i<mbd->m_numLinks;i++)
+			for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--)
 			{
-				btVector3 localInertiaDiagonal;
+				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
-				localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
+				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
-				btQuaternion parentRotToThis;
+				syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
-				parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
+			}
-				btVector3 parentComToThisPivotOffset;
-				parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
-				btVector3 thisPivotToThisComOffset;
-				thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
 
-				switch (mbd->m_links[i].m_jointType)
+			for (int i = bulletFile2->m_rigidBodies.size() - 1; i >= 0; i--)
+			{
+				btRigidBodyDoubleData* rbd = (btRigidBodyDoubleData*)bulletFile2->m_rigidBodies[i];
+				int foundRb = -1;
+				int uid = rbd->m_collisionObjectData.m_uniqueId;
+				for (int i = 0; i < m_data->m_mbDynamicsWorld->getNumCollisionObjects(); i++)
 				{
-					case btMultibodyLink::eFixed:
+					if (uid == m_data->m_mbDynamicsWorld->getCollisionObjectArray()[i]->getBroadphaseHandle()->m_uniqueId)
 					{
+						foundRb = i;
+						break;
+					}
+				}
+				if (foundRb >= 0)
+				{
+					btRigidBody* rb = btRigidBody::upcast(m_data->m_mbDynamicsWorld->getCollisionObjectArray()[foundRb]);
+					if (rb)
+					{
+						btTransform tr;
+						tr.deSerializeDouble(rbd->m_collisionObjectData.m_worldTransform);
+						rb->setWorldTransform(tr);
+						btVector3 linVel, angVel;
+						linVel.deSerializeDouble(rbd->m_linearVelocity);
+						angVel.deSerializeDouble(rbd->m_angularVelocity);
+						rb->setLinearVelocity(linVel);
+						rb->setAngularVelocity(angVel);
+					}
+					else
+					{
+						result = false;
+					}
+				}
+				else
+				{
+					result = false;
+				}
+			}
 
+			//todo: check why body1 pointer is not properly deserialized
+			for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)
+			{
+				btPersistentManifoldDoubleData* manifoldData = (btPersistentManifoldDoubleData*)bulletFile2->m_contactManifolds[i];
+				{
+					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
+					if (ptr)
+					{
+						manifoldData->m_body0 = ptr;
+					}
+				}
 
-						mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
+				{
-                                                            parentRotToThis, parentComToThisPivotOffset,thisPivotToThisComOffset);
+					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
-						//search for the collider
+					if (ptr)
-						//mbd->m_links[i].m_linkCollider
-						break;
-					}
-					case btMultibodyLink::ePrismatic:
 					{
-						btVector3 jointAxis;
+						manifoldData->m_body1 = ptr;
-						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisBottom[0]);
-						bool disableParentCollision = true;//todo
-						mb->setupPrismatic(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
-							parentRotToThis,jointAxis, parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
-						break;
-					}
-					case btMultibodyLink::eRevolute:
-					{
-						btVector3 jointAxis;
-						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisTop[0]);
-						bool disableParentCollision = true;//todo
-						mb->setupRevolute(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
-							parentRotToThis, jointAxis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
-						break;
-					}
-					case btMultibodyLink::eSpherical:
-					{
-						btAssert(0);
-						bool disableParentCollision = true;//todo
-						mb->setupSpherical(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
-							parentRotToThis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
-						break;
-					}
-					case btMultibodyLink::ePlanar:
-					{
-						btAssert(0);
-						break;
-					}
-					default:
-					{
-						btAssert(0);
 					}
 				}
 			}
+
+
+			if (bulletFile2->m_contactManifolds.size())
+			{
+				syncContactManifolds((btPersistentManifoldDoubleData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
+			}
+		}
+		else
+		{
+			//single precision version
+			//for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+			for (int i = bulletFile2->m_multiBodies.size() - 1; i >= 0; i--)
+			{
+				btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
+				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
+				syncMultiBody(mbd, mb, m_data, scratchQ, scratchM);
+			}
+
+			//todo: check why body1 pointer is not properly deserialized
+			for (int i = 0; i < bulletFile2->m_contactManifolds.size(); i++)
+			{
+				btPersistentManifoldFloatData* manifoldData = (btPersistentManifoldFloatData*)bulletFile2->m_contactManifolds[i];
+				{
+					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
+					if (ptr)
+					{
+						manifoldData->m_body0 = ptr;
+					}
+				}
+				{
+					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
+					if (ptr)
+					{
+						manifoldData->m_body1 = ptr;
+					}
+				}
+			}
+
+
+			if (bulletFile2->m_contactManifolds.size())
+			{
+				syncContactManifolds((btPersistentManifoldFloatData**)&bulletFile2->m_contactManifolds[0], bulletFile2->m_contactManifolds.size(), m_data);
+			}
+
 		}
 	}
-	
+	else
-	//convert all multibody link colliders
-	for (int i=0;i<bulletFile2->m_multiBodyLinkColliders.size();i++)
 	{
-		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
+		result = btBulletWorldImporter::convertAllObjects(bulletFile2);
-		{
-			btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*) bulletFile2->m_multiBodyLinkColliders[i];
 
-			btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
+
+		//convert all multibodies
+		for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+		{
+
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
+			{
+				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
+				convertMultiBody(mbd, m_data);
+			}
+			else
+			{
+				btMultiBodyFloatData* mbd = (btMultiBodyFloatData*)bulletFile2->m_multiBodies[i];
+				convertMultiBody(mbd, m_data);
+			}
+		}
+
+		//forward kinematics, so that the link world transforms are valid, for collision detection
+		for (int i = 0; i < m_data->m_mbMap.size(); i++)
+		{
+			btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
 			if (ptr)
 			{
-				btMultiBody* multiBody = *ptr;
+				btMultiBody* mb = *ptr;
-				
+				mb->finalizeMultiDof();
-
+				btVector3 linvel = mb->getBaseVel();
-				btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
+				btVector3 angvel = mb->getBaseOmega();
-				if (shapePtr && *shapePtr)
+				mb->forwardKinematics(scratchQ, scratchM);
-				{
-					btTransform startTransform;
-					mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
-					startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
-				
-					btCollisionShape* shape = (btCollisionShape*)*shapePtr;
-					if (shape)
-					{
-						btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
-						col->setCollisionShape( shape );
-
-						//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
-						col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
-						col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
-						//m_bodyMap.insert(colObjData,body);
-						if (mblcd->m_link==-1)
-						{
-							multiBody->setBaseCollider(col);
-						} else
-						{
-							multiBody->getLink(mblcd->m_link).m_collider = col;
-						}
-						int mbLinkIndex = mblcd->m_link;
-
-						bool isDynamic = (mbLinkIndex<0 && multiBody->hasFixedBase())? false : true;
-						int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
-						int collisionFilterMask = isDynamic? 	int(btBroadphaseProxy::AllFilter) : 	int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
-
-	#if 0
-						int colGroup=0, colMask=0;
-						int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
-						if (collisionFlags & URDF_HAS_COLLISION_GROUP)
-						{
-							collisionFilterGroup = colGroup;
-						}
-						if (collisionFlags & URDF_HAS_COLLISION_MASK)
-						{
-							collisionFilterMask = colMask;
-						}
-	#endif
-						m_data->m_mbDynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
-					}
-					
-				} else
-				{
-					printf("error: no shape found\n");
-				}
-#if 0
-				//base and fixed? -> static, otherwise flag as dynamic
-				
-				world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
-#endif
 			}
-
 		}
-	}
 
-	for (int i=0;i<m_data->m_mbMap.size();i++)
+		//convert all multibody link colliders
-	{
+		for (int i = 0; i < bulletFile2->m_multiBodyLinkColliders.size(); i++)
-		btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
-		if (ptr)
 		{
-			btMultiBody* mb = *ptr;
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
-			mb->finalizeMultiDof();
+			{
+				btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*)bulletFile2->m_multiBodyLinkColliders[i];
 
-			m_data->m_mbDynamicsWorld->addMultiBody(mb);
+				btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
+				if (ptr)
+				{
+					btMultiBody* multiBody = *ptr;
+
+
+					btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
+					if (shapePtr && *shapePtr)
+					{
+						btTransform startTransform;
+						mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
+						startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
+
+						btCollisionShape* shape = (btCollisionShape*)*shapePtr;
+						if (shape)
+						{
+							btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
+							col->setCollisionShape(shape);
+							//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
+							col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
+							col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
+							//m_bodyMap.insert(colObjData,body);
+							if (mblcd->m_link == -1)
+							{
+								col->setWorldTransform(multiBody->getBaseWorldTransform());
+								multiBody->setBaseCollider(col);
+							}
+							else
+							{
+								col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
+								multiBody->getLink(mblcd->m_link).m_collider = col;
+							}
+							int mbLinkIndex = mblcd->m_link;
+
+							bool isDynamic = (mbLinkIndex < 0 && multiBody->hasFixedBase()) ? false : true;
+							int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+							int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+#if 0
+							int colGroup = 0, colMask = 0;
+							int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
+							if (collisionFlags & URDF_HAS_COLLISION_GROUP)
+							{
+								collisionFilterGroup = colGroup;
+							}
+							if (collisionFlags & URDF_HAS_COLLISION_MASK)
+							{
+								collisionFilterMask = colMask;
+							}
+#endif
+							m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
+						}
+
+					}
+					else
+					{
+						printf("error: no shape found\n");
+					}
+#if 0
+					//base and fixed? -> static, otherwise flag as dynamic
+
+					world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
+#endif
+				}
+
+			}
+		}
+
+		for (int i = 0; i < m_data->m_mbMap.size(); i++)
+		{
+			btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
+			if (ptr)
+			{
+				btMultiBody* mb = *ptr;
+				mb->finalizeMultiDof();
+
+				m_data->m_mbDynamicsWorld->addMultiBody(mb);
+			}
 		}
 	}
 	return result;

Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp

@@ -20,7 +20,8 @@ subject to the following restrictions:
 #endif
 btWorldImporter::btWorldImporter(btDynamicsWorld* world)
 :m_dynamicsWorld(world),
-m_verboseMode(0)
+m_verboseMode(0),
+m_importerFlags(0)
 {
 
 }

Extras/Serialize/BulletWorldImporter/btWorldImporter.h

@@ -59,6 +59,10 @@ struct btRigidBodyFloatData;
 #define btRigidBodyData btRigidBodyFloatData
 #endif//BT_USE_DOUBLE_PRECISION
 
+enum btWorldImporterFlags
+{
+	eRESTORE_EXISTING_OBJECTS=1,//don't create new objects
+};
 
 class btWorldImporter
 {
@@ -66,6 +70,7 @@ protected:
 	btDynamicsWorld* m_dynamicsWorld;
 	
 	int m_verboseMode;
+	int m_importerFlags;
 
 	btAlignedObjectArray<btCollisionShape*>  m_allocatedCollisionShapes;
 	btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
@@ -131,6 +136,18 @@ public:
 		return m_verboseMode;
 	}
 
+	void	setImporterFlags(int importerFlags)
+	{
+		m_importerFlags = importerFlags;
+	}
+
+	int getImporterFlags() const
+	{
+		return m_importerFlags;
+	}
+
+	
+
 		// query for data
 	int	getNumCollisionShapes() const;
 	btCollisionShape* getCollisionShapeByIndex(int index);

Extras/Serialize/makesdna/makesdna.cpp

@@ -132,6 +132,7 @@ typedef unsigned __int64 uint64_t;
 #include "BulletCollision/CollisionShapes/btConeShape.h"
 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleInfoMap.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "BulletCollision/Gimpact/btGImpactShape.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
@@ -181,6 +182,7 @@ char *includefiles[] = {
 	"../../../src/BulletCollision/CollisionShapes/btConeShape.h",
 	"../../../src/BulletCollision/CollisionShapes/btCapsuleShape.h",
 	"../../../src/BulletCollision/CollisionShapes/btTriangleInfoMap.h",
+	"../../../src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h",
 	"../../../src/BulletCollision/Gimpact/btGImpactShape.h",
 	"../../../src/BulletCollision/CollisionShapes/btConvexHullShape.h",
 	"../../../src/BulletCollision/CollisionDispatch/btCollisionObject.h",

data/cube_small.urdf

@@ -2,8 +2,8 @@
 <robot name="cube.urdf">
   <link name="baseLink">
     <contact>
-      <lateral_friction value="1.0"/>
+    	<friction_anchor/>
-      <inertia_scaling value="3.0"/>
+    	   <lateral_friction value="1.0"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

data/planeMesh.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<mesh filename="plane.obj" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/quadruped/minitaur_rainbow_dash_v1.urdf

@@ -0,0 +1,980 @@
+<?xml version="1.0"?>
+<robot name="quadruped">
+	
+	<link name="base_chassis_link">
+		<visual>
+			<geometry>
+				<box size=".3 .13 .087"/>
+			</geometry>
+			<material name="black">
+				<color rgba=".3 .3 .3 1"/>
+			</material>
+		</visual>
+		
+		<collision>
+			<geometry>
+				<box size=".3 .13 .087"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz=".05 0 0"/>
+			<mass value="3.353"/>
+			<inertia ixx=".006837" ixy=".0" ixz=".0" iyy=".027262" iyz=".0" izz=".029870"/>
+		</inertial>
+	</link>
+	
+	
+	<link name="chassis_right">
+		
+		<visual>
+			<origin rpy="0 0 0" xyz=".2375 .054 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+			<material name="blue">
+				<color rgba="0 0 1 1"/>
+			</material>
+		</visual>
+		
+		<visual>
+			<origin rpy="0 0 0" xyz="-.2375 .054 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+			<material name="yellow">
+				<color rgba="1 1 0 1"/>
+			</material>
+		</visual>
+		
+		<collision>
+			<origin rpy="0 0 0" xyz=".2375 .054 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+		</collision>
+		<collision>
+			<origin rpy="0 0 0" xyz="-.2375 .054 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value="0.322"/>
+			<inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/>
+		</inertial>
+	</link>
+	<joint name="chassis_right_center" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="base_chassis_link"/>
+		<child link="chassis_right"/>
+		<origin rpy="0 0 0" xyz="0 -.1265 -.0185"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="chassis_left">
+		<visual>
+			<origin rpy="0 0 0" xyz=".2375 -.054 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+			<material name="green">
+				<color rgba="0 1 0 1"/>
+			</material>
+		</visual>
+		
+		<visual>
+			<origin rpy="0 0 0" xyz="-.2375 -.054 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+			<material name="green"/>
+		</visual>
+		
+		<collision>
+			<origin rpy="0 0 0" xyz=".2375 -.054 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+		</collision>
+	
+		<collision>
+			<origin rpy="0 0 0" xyz="-.2375 -.054 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+		</collision>
+		
+		<inertial>
+			<mass value="0.322"/>
+			<inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/>
+		</inertial>
+	</link>
+	<joint name="chassis_left_center" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="base_chassis_link"/>
+		<child link="chassis_left"/>
+		<origin rpy="0 0 0" xyz="0 .1265 -.0185"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="motor_front_rightR_bracket_link">
+	<visual>
+			<origin rpy="0 0 0" xyz="0 -0.02 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+			<material name="red">
+				<color rgba="1 0 0 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 -0.02 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+		</collision>
+		
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 -0.03 0"/>
+			<mass value=".16"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_front_rightR_bracket_joint" type="prismatic">
+	  <axis xyz="0 0 1"/>
+		<parent link="base_chassis_link"/>
+		<child link="motor_front_rightR_bracket_link"/>
+		<origin rpy="0 0 0" xyz=".2375 -0.154 -.0185"/>
+		<limit effort="100" lower="-0.02" upper="0.02" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="motor_front_rightR_link">
+		<visual>
+			<geometry>
+				<mesh filename="tmotor3.obj" scale="1 1 1"/>
+			</geometry>
+			<material name="white">
+				<color rgba="1 1 1 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<cylinder length=".021" radius=".0425"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".241"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_front_rightR_joint" type="continuous">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_front_rightR_bracket_link"/>
+		<child link="motor_front_rightR_link"/>
+		<origin rpy="1.57075 0 0" xyz="0 0 0"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="motor_front_rightL_link">
+	
+		<visual>
+			<geometry>
+				<mesh filename="tmotor3.obj" scale="1 1 1"/>
+			</geometry>
+			<material name="white">
+				<color rgba="1 1 1 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<cylinder length=".021" radius=".0425"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".241"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_front_rightL_joint" type="continuous">
+		<axis xyz="0 0 1"/>
+		<parent link="chassis_right"/>
+		<child link="motor_front_rightL_link"/>
+		<origin rpy="1.57075 0 3.141592" xyz=".2375 .0275 0"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	
+	<link name="motor_front_leftL_bracket_link">
+	<visual>
+			<origin rpy="0 0 0" xyz="0 0.02 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+			<material name="red">
+				<color rgba="1 0 0 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0.02 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+		</collision>
+		
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0.02 0"/>
+			<mass value=".16"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_front_leftL_bracket_joint" type="prismatic">
+	  <axis xyz="0 0 1"/>
+		<parent link="base_chassis_link"/>
+		<child link="motor_front_leftL_bracket_link"/>
+		<origin rpy="0 0 0" xyz=".2375 0.154 -.0185"/>
+		<limit effort="100" lower="-0.02" upper="0.02" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	
+	<link name="motor_front_leftL_link">
+		<visual>
+			<geometry>
+				<mesh filename="tmotor3.obj" scale="1 1 1"/>
+			</geometry>
+			<material name="white">
+				<color rgba="1 1 1 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<cylinder length=".021" radius=".0425"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".241"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_front_leftL_joint" type="continuous">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_front_leftL_bracket_link"/>
+		<child link="motor_front_leftL_link"/>
+		<origin rpy="1.57075 0 3.141592" xyz="0 0 0"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="motor_front_leftR_link">
+		<visual>
+			<geometry>
+				<mesh filename="tmotor3.obj" scale="1 1 1"/>
+			</geometry>
+			<material name="white">
+				<color rgba="1 1 1 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<cylinder length=".021" radius=".0425"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".241"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_front_leftR_joint" type="continuous">
+		<axis xyz="0 0 1"/>
+		<parent link="chassis_left"/>
+		<child link="motor_front_leftR_link"/>
+		<origin rpy="1.57075 0 0" xyz=".2375 -.0275 0"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	
+	<link name="motor_back_rightR_bracket_link">
+	<visual>
+			<origin rpy="0 0 0" xyz="0 -0.02 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+			<material name="red">
+				<color rgba="1 0 0 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 -0.02 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+		</collision>
+		
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 -0.03 0"/>
+			<mass value=".16"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_back_rightR_bracket_joint" type="prismatic">
+	  <axis xyz="0 0 1"/>
+		<parent link="base_chassis_link"/>
+		<child link="motor_back_rightR_bracket_link"/>
+		<origin rpy="0 0 0" xyz="-.2375 -0.154 -.0185"/>
+		<limit effort="100" lower="-0.02" upper="0.02" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	
+	<link name="motor_back_rightR_link">
+		<visual>
+			<geometry>
+				<mesh filename="tmotor3.obj" scale="1 1 1"/>
+			</geometry>
+			<material name="white1">
+				<color rgba="1 0 0 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<cylinder length=".021" radius=".0425"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".241"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_back_rightR_joint" type="continuous">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_back_rightR_bracket_link"/>
+		<child link="motor_back_rightR_link"/>
+		<origin rpy="1.57075 0 0" xyz="0 0 0"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="motor_back_rightL_link">
+		<visual>
+			<geometry>
+				<mesh filename="tmotor3.obj" scale="1 1 1"/>
+			</geometry>
+			<material name="white">
+				<color rgba="1 1 1 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<cylinder length=".021" radius=".0425"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".241"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_back_rightL_joint" type="continuous">
+		<axis xyz="0 0 1"/>
+		<parent link="chassis_right"/>
+		<child link="motor_back_rightL_link"/>
+		<origin rpy="1.57075 0 3.141592" xyz="-.2375 .0275 0"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	
+	<link name="motor_back_leftL_bracket_link">
+	<visual>
+			<origin rpy="0 0 0" xyz="0 0.02 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+			<material name="red">
+				<color rgba="1 0 0 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0.02 0"/>
+			<geometry>
+				<box size=".068 .032 .050"/>
+			</geometry>
+		</collision>
+		
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0.02 0"/>
+			<mass value=".16"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_back_leftL_bracket_joint" type="prismatic">
+	  <axis xyz="0 0 1"/>
+		<parent link="base_chassis_link"/>
+		<child link="motor_back_leftL_bracket_link"/>
+		<origin rpy="0 0 0" xyz="-.2375 0.154 -.0185"/>
+		<limit effort="100" lower="-0.02" upper="0.02" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	
+	
+	<link name="motor_back_leftL_link">
+		<visual>
+			<geometry>
+				<mesh filename="tmotor3.obj" scale="1 1 1"/>
+			</geometry>
+			<material name="white">
+				<color rgba="1 1 1 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<cylinder length=".021" radius=".0425"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".241"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_back_leftL_joint" type="continuous">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_back_leftL_bracket_link"/>
+		<child link="motor_back_leftL_link"/>
+		<origin rpy="1.57075 0 3.141592" xyz="0 0 0"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="motor_back_leftR_link">
+		<visual>
+			<geometry>
+				<mesh filename="tmotor3.obj" scale="1 1 1"/>
+			</geometry>
+			<material name="white">
+				<color rgba="1 1 1 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<cylinder length=".021" radius=".0425"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".241"/>
+			<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
+		</inertial>
+	</link>
+	<joint name="motor_back_leftR_joint" type="continuous">
+		<axis xyz="0 0 1"/>
+		<parent link="chassis_left"/>
+		<child link="motor_back_leftR_link"/>
+		<origin rpy="1.57075 0 0" xyz="-.2375 -.0275 0"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="upper_leg_front_rightR_link">
+		<visual>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".034"/>
+			<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
+		</inertial>
+	</link>
+	<joint name="hip_front_rightR_joint" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_front_rightR_link"/>
+		<child link="upper_leg_front_rightR_link"/>
+		<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="lower_leg_front_rightR_link">
+		<contact>
+			<friction_anchor/>
+			<stiffness value="3000.0"/>
+			<damping value="100.0"/>
+			<spinning_friction value=".05"/>
+			<lateral_friction value="1"/>
+		</contact>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<geometry>
+				<box size=".017 .009 .240"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<geometry>
+				<box size=".017 .009 .240"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<mass value=".086"/>
+			<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
+		</inertial>
+	</link>
+	<joint name="knee_front_rightR_joint" type="continuous">
+		<axis xyz="0 1 0"/>
+		<parent link="upper_leg_front_rightR_link"/>
+		<child link="lower_leg_front_rightR_link"/>
+		<origin rpy="0 0 0" xyz="0 .0085 .056"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="upper_leg_front_rightL_link">
+		<visual>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".034"/>
+			<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
+		</inertial>
+	</link>
+	<joint name="hip_front_rightL_joint" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_front_rightL_link"/>
+		<child link="upper_leg_front_rightL_link"/>
+		<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="lower_leg_front_rightL_link">
+		<contact>
+			<friction_anchor/>
+			<stiffness value="3000.0"/>
+			<damping value="100.0"/>
+			<spinning_friction value=".05"/>
+			<lateral_friction value="1"/>
+		</contact>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 .108"/>
+			<geometry>
+				<box size=".017 .009 .216"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 .108"/>
+			<geometry>
+				<box size=".017 .009 .216"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 .1"/>
+			<mass value=".072"/>
+			<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
+		</inertial>
+	</link>
+	<joint name="knee_front_rightL_joint" type="continuous">
+		<axis xyz="0 1 0"/>
+		<parent link="upper_leg_front_rightL_link"/>
+		<child link="lower_leg_front_rightL_link"/>
+		<origin rpy="0 0 0" xyz="0 .0085 .056"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="upper_leg_front_leftR_link">
+		<visual>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".034"/>
+			<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
+		</inertial>
+	</link>
+	<joint name="hip_front_leftR_joint" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_front_leftR_link"/>
+		<child link="upper_leg_front_leftR_link"/>
+		<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="lower_leg_front_leftR_link">
+		<contact>
+			<friction_anchor/>
+			<stiffness value="3000.0"/>
+			<damping value="100.0"/>
+			<spinning_friction value=".05"/>
+			<lateral_friction value="1"/>
+		</contact>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 .108"/>
+			<geometry>
+				<box size=".017 .009 .216"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 .108"/>
+			<geometry>
+				<box size=".017 .009 .216"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 .1"/>
+			<mass value=".072"/>
+			<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
+		</inertial>
+	</link>
+	<joint name="knee_front_leftR_joint" type="continuous">
+		<axis xyz="0 1 0"/>
+		<parent link="upper_leg_front_leftR_link"/>
+		<child link="lower_leg_front_leftR_link"/>
+		<origin rpy="0 0 0" xyz="0 .0085 .056"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="upper_leg_front_leftL_link">
+		<visual>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".034"/>
+			<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
+		</inertial>
+	</link>
+	<joint name="hip_front_leftL_joint" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_front_leftL_link"/>
+		<child link="upper_leg_front_leftL_link"/>
+		<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="lower_leg_front_leftL_link">
+		<contact>
+			<friction_anchor/>
+			<stiffness value="3000.0"/>
+			<damping value="100.0"/>
+			<spinning_friction value=".05"/>
+			<lateral_friction value="1"/>
+		</contact>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<geometry>
+				<box size=".017 .009 .240"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<geometry>
+				<box size=".017 .009 .240"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<mass value=".086"/>
+			<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
+		</inertial>
+	</link>
+	<joint name="knee_front_leftL_joint" type="continuous">
+		<axis xyz="0 1 0"/>
+		<parent link="upper_leg_front_leftL_link"/>
+		<child link="lower_leg_front_leftL_link"/>
+		<origin rpy="0 0 0" xyz="0 .0085 .056"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="upper_leg_back_rightR_link">
+		<visual>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".034"/>
+			<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
+		</inertial>
+	</link>
+	<joint name="hip_back_rightR_joint" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_back_rightR_link"/>
+		<child link="upper_leg_back_rightR_link"/>
+		<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="lower_leg_back_rightR_link">
+		<contact>
+			<friction_anchor/>
+			<stiffness value="3000.0"/>
+			<damping value="100.0"/>
+			<spinning_friction value=".05"/>
+			<lateral_friction value="1"/>
+		</contact>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<geometry>
+				<box size=".017 .009 .240"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<geometry>
+				<box size=".017 .009 .240"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<mass value=".086"/>
+			<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
+		</inertial>
+	</link>
+	<joint name="knee_back_rightR_joint" type="continuous">
+		<axis xyz="0 1 0"/>
+		<parent link="upper_leg_back_rightR_link"/>
+		<child link="lower_leg_back_rightR_link"/>
+		<origin rpy="0 0 0" xyz="0 .0085 .056"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="upper_leg_back_rightL_link">
+		<visual>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".034"/>
+			<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
+		</inertial>
+	</link>
+	<joint name="hip_back_rightL_joint" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_back_rightL_link"/>
+		<child link="upper_leg_back_rightL_link"/>
+		<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="lower_leg_back_rightL_link">
+		<contact>
+			<friction_anchor/>
+			<stiffness value="3000.0"/>
+			<damping value="100.0"/>
+			<spinning_friction value=".05"/>
+			<lateral_friction value="1"/>
+		</contact>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 .108"/>
+			<geometry>
+				<box size=".017 .009 .216"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 .108"/>
+			<geometry>
+				<box size=".017 .009 .216"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 .1"/>
+			<mass value=".072"/>
+			<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
+		</inertial>
+	</link>
+	<joint name="knee_back_rightL_joint" type="continuous">
+		<axis xyz="0 1 0"/>
+		<parent link="upper_leg_back_rightL_link"/>
+		<child link="lower_leg_back_rightL_link"/>
+		<origin rpy="0 0 0" xyz="0 .0085 .056"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="upper_leg_back_leftR_link">
+		<visual>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".034"/>
+			<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
+		</inertial>
+	</link>
+	<joint name="hip_back_leftR_joint" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_back_leftR_link"/>
+		<child link="upper_leg_back_leftR_link"/>
+		<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="lower_leg_back_leftR_link">
+		<contact>
+			<friction_anchor/>
+			<stiffness value="3000.0"/>
+			<damping value="100.0"/>
+			<spinning_friction value=".05"/>
+			<lateral_friction value="1"/>
+		</contact>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 .108"/>
+			<geometry>
+				<box size=".017 .009 .216"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 .108"/>
+			<geometry>
+				<box size=".017 .009 .216"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 .1"/>
+			<mass value=".072"/>
+			<inertia ixx=".00023166" ixy=".0" ixz=".0" iyy=".00023325" iyz=".0" izz=".00000198"/>
+		</inertial>
+	</link>
+	<joint name="knee_back_leftR_joint" type="continuous">
+		<axis xyz="0 1 0"/>
+		<parent link="upper_leg_back_leftR_link"/>
+		<child link="lower_leg_back_leftR_link"/>
+		<origin rpy="0 0 0" xyz="0 .0085 .056"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="upper_leg_back_leftL_link">
+		<visual>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<geometry>
+				<box size=".039 .008 .129"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<mass value=".034"/>
+			<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
+		</inertial>
+	</link>
+	<joint name="hip_back_leftL_joint" type="fixed">
+		<axis xyz="0 0 1"/>
+		<parent link="motor_back_leftL_link"/>
+		<child link="upper_leg_back_leftL_link"/>
+		<origin rpy="-1.57075 0 0" xyz="0 .045 -.0145"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+	<link name="lower_leg_back_leftL_link">
+		<contact>
+			<friction_anchor/>
+			<stiffness value="3000.0"/>
+			<damping value="100.0"/>
+			<spinning_friction value=".05"/>
+			<lateral_friction value="1"/>
+		</contact>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<geometry>
+				<box size=".017 .009 .240"/>
+			</geometry>
+			<material name="grey">
+				<color rgba=".65 .65 .75 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<geometry>
+				<box size=".017 .009 .240"/>
+			</geometry>
+		</collision>
+		<inertial>
+			<origin rpy="0 0 0" xyz="0 0 .120"/>
+			<mass value=".086"/>
+			<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
+		</inertial>
+	</link>
+	<joint name="knee_back_leftL_joint" type="continuous">
+		<axis xyz="0 1 0"/>
+		<parent link="upper_leg_back_leftL_link"/>
+		<child link="lower_leg_back_leftL_link"/>
+		<origin rpy="0 0 0" xyz="0 .0085 .056"/>
+		<limit effort="100" velocity="100"/>
+		<joint_properties damping=".0" friction=".0"/>
+	</joint>
+</robot>

docs/pybullet_quickstart_guide/PlainPreview/Article.md.html

@@ -0,0 +1,10 @@
+<meta charset="utf-8" lang="en">
+Hello Markdeep!
+
+- Hello
+- lists!
+
+Hello MathJax:
+\[f(0)=\frac{1}{2\cdot\pi\cdot i}\cdot \oint_{|z|=1} \frac{f(z)}{z} \textrm{d}z\]
+
+<script src="MarkdeepUtility.js"></script>

docs/pybullet_quickstart_guide/PlainPreview/BuildMarkdeepUtility.py

@@ -0,0 +1,43 @@
+import re
+
+if(__name__=="__main__"):
+    # Assemble the script which embeds the Markdeep page into the preview blog
+    PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8");
+    HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL);
+    HeadAttributes=HeadMatch.group(1);
+    FullDocumentHead=HeadMatch.group(2);
+    BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL);
+    BodyAttributes=BodyMatch.group(1);
+    FullPreviewBody=BodyMatch.group(2);
+    ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)";
+    iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro);
+    DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro];
+    DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):];
+    FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8");
+    ReplacementList=[
+        ("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
+        ("$(DOCUMENT_BODY_PREFIX)",DocumentBodyPrefix),
+        ("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix)
+    ];
+    for Macro,Replacement in ReplacementList:
+        FullPrepareHTMLCode=FullPrepareHTMLCode.replace(Macro,Replacement.replace("\r\n","\\r\\n\\\r\n").replace("'","\\'"));
+    # Generate code which sets body and head attributes appropriately
+    for Element,AttributeCode in [("head",HeadAttributes),("body",BodyAttributes)]:
+        FullPrepareHTMLCode+="\r\n// Setting "+Element+" attributes\r\n";
+        for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"",AttributeCode):
+            FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n";
+    open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8"));
+
+    # Concatenate all the scripts together
+    SourceFileList=[
+                    "PrepareHTML.full.js",
+                    "SetMarkdeepMode.js",
+                    "markdeep.min.js",
+                    "DisplayMarkdeepOutput.js",
+                    "InvokeMathJax.js"
+    ];
+    OutputCode="\r\n\r\n".join(["// "+SourceFile+"\r\n\r\n"+open(SourceFile,"rb").read().decode("utf-8") for SourceFile in SourceFileList]);
+    OutputFile=open("MarkdeepUtility.js","wb");
+    OutputFile.write(OutputCode.encode("utf-8"));
+    OutputFile.close();
+    print("Done.");

docs/pybullet_quickstart_guide/PlainPreview/DisplayMarkdeepOutput.js

@@ -0,0 +1,6 @@
+BodyHTML=document.body.innerHTML;
+BeginTag="<!-- MARKDEEP_BEGIN -->";
+EndTag="<!-- MARKDEEP_END -->";
+BodyHTML=BodyHTML.slice(BodyHTML.indexOf(BeginTag)+BeginTag.length,BodyHTML.lastIndexOf(EndTag));
+document.getElementById("BodyDisplayBox").textContent=BodyHTML;
+document.head.innerHTML=FullDocumentHead;

docs/pybullet_quickstart_guide/PlainPreview/InvokeMathJax.js

@@ -0,0 +1,4 @@
+var MathjaxScript=document.createElement("script");
+MathjaxScript.type="text/javascript";
+MathjaxScript.src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML";
+document.head.appendChild(MathjaxScript);

docs/pybullet_quickstart_guide/PlainPreview/PrepareHTML.full.js

@@ -0,0 +1,117 @@
+/** Converts <>&" to their HTML escape sequences */
+function escapeHTMLEntities(str) {
+    return String(str).replace(/&/g, '&amp;').replace(/</g, '&lt;').replace(/>/g, '&gt;').replace(/"/g, '&quot;');
+}
+
+
+/** Restores the original source string's '<' and '>' as entered in
+    the document, before the browser processed it as HTML. There is no
+    way in an HTML document to distinguish an entity that was entered
+    as an entity.*/
+function unescapeHTMLEntities(str) {
+    // Process &amp; last so that we don't recursively unescape
+    // escaped escape sequences.
+    return str.
+        replace(/&lt;/g, '<').
+        replace(/&gt;/g, '>').
+        replace(/&quot;/g, '"').
+        replace(/&#39;/g, "'").
+        replace(/&ndash;/g, '--').
+        replace(/&mdash;/g, '---').
+        replace(/&amp;/g, '&');
+}
+
+
+/**
+   \param node  A node from an HTML DOM
+
+   \return A String that is a very good reconstruction of what the
+   original source looked like before the browser tried to correct
+   it to legal HTML.
+ */
+function nodeToMarkdeepSource(node, leaveEscapes) {
+    var source = node.innerHTML;
+
+    // Markdown uses <john@bar.com> e-mail syntax, which HTML parsing
+    // will try to close by inserting the matching close tags at the end of the
+    // document. Remove anything that looks like that and comes *after*
+    // the first fallback style.
+    source = source.replace(/(?:<style class="fallback">[\s\S]*?<\/style>[\s\S]*)<\/\S+@\S+\.\S+?>/gim, '');
+    
+    // Remove artificially inserted close tags
+    source = source.replace(/<\/h?ttps?:.*>/gi, '');
+    
+    // Now try to fix the URLs themselves, which will be 
+    // transformed like this: <http: casual-effects.com="" markdeep="">
+    source = source.replace(/<(https?): (.*?)>/gi, function (match, protocol, list) {
+
+        // Remove any quotes--they wouldn't have been legal in the URL anyway
+        var s = '<' + protocol + '://' + list.replace(/=""\s/g, '/');
+
+        if (s.substring(s.length - 3) === '=""') {
+            s = s.substring(0, s.length - 3);
+        }
+
+        // Remove any lingering quotes (since they
+        // wouldn't have been legal in the URL)
+        s = s.replace(/"/g, '');
+
+        return s + '>';
+    });
+
+    // Remove the "fallback" style tags
+    source = source.replace(/<style class=["']fallback["']>.*?<\/style>/gmi, '');
+
+    source = unescapeHTMLEntities(source);
+
+    return source;
+}
+
+// $ (FULL_DOCUMENT_HEAD) is replaced by the contents of the <head> found in 
+// PreviewBlogPage.htm. This document head will overwrite whatever Markdeep does to 
+// the head at the very end.
+FullDocumentHead='\
+\r\n\
+		<meta http-equiv="content-type" content="text/html; charset=UTF-8">\r\n\
+		<meta charset="UTF-8">\r\n\
+\r\n\
+		<!-- Markdeep styles -->\r\n\
+		<style>body{counter-reset: h1 h2 h3 h4 h5 h6}.md code,pre{font-family:Menlo,Consolas,monospace;font-size:15.018802542857143px;line-height:140%}.md div.title{font-size:26px;font-weight:800;line-height:120%;text-align:center}.md div.afterTitles{height:10px}.md div.subtitle{text-align:center}.md .image{display:inline-block}.md div.imagecaption,.md div.tablecaption,.md div.listingcaption{margin:0.2em 5px 10px 5px;text-align: justify;font-style:italic}.md div.imagecaption{margin-bottom:0}.md img{max-width:100%;page-break-inside:avoid}li{text-align:left};.md div.tilde{margin:20px 0 -10px;text-align:center}.md blockquote.fancyquote{margin:25px 0 25px;text-align:left;line-height:160%}.md blockquote.fancyquote::before{content:"“";color:#DDD;font-family:Times New Roman;font-size:45px;line-height:0;margin-right:6px;vertical-align:-0.3em}.md span.fancyquote{font-size:118%;color:#777;font-style:italic}.md span.fancyquote::after{content:"”";font-style:normal;color:#DDD;font-family:Times New Roman;font-size:45px;line-height:0;margin-left:6px;vertical-align:-0.3em}.md blockquote.fancyquote .author{width:100%;margin-top:10px;display:inline-block;text-align:right}.md small{font-size:60%}.md div.title,contents,.md .tocHeader,h1,h2,h3,h4,h5,h6,.md .shortTOC,.md .mediumTOC,.nonumberh1,.nonumberh2,.nonumberh3,.nonumberh4,.nonumberh5,.nonumberh6{font-family:Verdana,Helvetica,Arial,sans-serif;margin:13.4px 0 13.4px;padding:15px 0 3px;border-top:none;clear:both}.md svg.diagram{display:block;font-family:Menlo,Consolas,monospace;font-size:15.018802542857143px;text-align:center;stroke-linecap:round;stroke-width:2px;page-break-inside:avoid;stroke:#000;fill:#000}.md svg.diagram .opendot{fill:#FFF}.md svg.diagram text{stroke:none}.md h1,.tocHeader,.nonumberh1{border-bottom:3px solid;font-size:20px;font-weight:bold;}h1,.nonumberh1{counter-reset: h2 h3 h4 h5 h6}h2,.nonumberh2{counter-reset: h3 h4 h5 h6;border-bottom:2px solid #999;color:#555;font-size:18px;}h3,h4,h5,h6,.nonumberh3,.nonumberh4,.nonumberh5,.nonumberh6{font-family:Helvetica,Arial,sans-serif;color:#555;font-size:16px;}h3{counter-reset:h4 h5 h6}h4{counter-reset:h5 h6}h5{counter-reset:h6}.md table{border-collapse:collapse;line-height:140%;page-break-inside:avoid}.md table.table{margin:auto}.md table.calendar{width:100%;margin:auto;font-size:11px;font-family:Helvetica,Arial,sans-serif}.md table.calendar th{font-size:16px}.md .today{background:#ECF8FA}.md .calendar .parenthesized{color:#999;font-style:italic}.md div.tablecaption{text-align:center}.md table.table th{color:#FFF;background-color:#AAA;border:1px solid #888;padding:8px 15px 8px 15px}.md table.table td{padding:5px 15px 5px 15px;border:1px solid #888}.md table.table tr:nth-child(even){background:#EEE}.md pre.tilde{border-top: 1px solid #CCC;border-bottom: 1px solid #CCC;padding: 5px 0 5px 20px;margin:0 0 30px 0;background:#FCFCFC;page-break-inside:avoid}.md a:link, .md a:visited{color:#38A;text-decoration:none}.md a:link:hover{text-decoration:underline}.md dt{font-weight:700}dl>.md dd{padding:0 0 18px}.md dl>table{margin:35px 0 30px}.md code{white-space:pre;page-break-inside:avoid}.md .endnote{font-size:13px;line-height:15px;padding-left:10px;text-indent:-10px}.md .bib{padding-left:80px;text-indent:-80px;text-align:left}.markdeepFooter{font-size:9px;text-align:right;padding-top:80px;color:#999}.md .mediumTOC{float:right;font-size:12px;line-height:15px;border-left:1px solid #CCC;padding-left:15px;margin:15px 0px 15px 25px}.md .mediumTOC .level1{font-weight:600}.md .longTOC .level1{font-weight:600;display:block;padding-top:12px;margin:0 0 -20px}.md .shortTOC{text-align:center;font-weight:bold;margin-top:15px;font-size:14px}</style>\r\n\
+\r\n\
+		<!-- hljs styles -->\r\n\
+		<style>.hljs{display:block;overflow-x:auto;padding:0.5em;background:#fff;color:#000;-webkit-text-size-adjust:none}.hljs-comment{color:#006a00}.hljs-keyword{color:#02E}.hljs-literal,.nginx .hljs-title{color:#aa0d91}.method,.hljs-list .hljs-title,.hljs-tag .hljs-title,.setting .hljs-value,.hljs-winutils,.tex .hljs-command,.http .hljs-title,.hljs-request,.hljs-status,.hljs-name{color:#008}.hljs-envvar,.tex .hljs-special{color:#660}.hljs-string{color:#c41a16}.hljs-tag .hljs-value,.hljs-cdata,.hljs-filter .hljs-argument,.hljs-attr_selector,.apache .hljs-cbracket,.hljs-date,.hljs-regexp{color:#080}.hljs-sub .hljs-identifier,.hljs-pi,.hljs-tag,.hljs-tag .hljs-keyword,.hljs-decorator,.ini .hljs-title,.hljs-shebang,.hljs-prompt,.hljs-hexcolor,.hljs-rule .hljs-value,.hljs-symbol,.hljs-symbol .hljs-string,.hljs-number,.css .hljs-function,.hljs-function .hljs-title,.coffeescript .hljs-attribute{color:#A0C}.hljs-function .hljs-title{font-weight:bold;color:#000}.hljs-class .hljs-title,.smalltalk .hljs-class,.hljs-type,.hljs-typename,.hljs-tag .hljs-attribute,.hljs-doctype,.hljs-class .hljs-id,.hljs-built_in,.setting,.hljs-params,.clojure .hljs-attribute{color:#5c2699}.hljs-variable{color:#3f6e74}.css .hljs-tag,.hljs-rule .hljs-property,.hljs-pseudo,.hljs-subst{color:#000}.css .hljs-class,.css .hljs-id{color:#9b703f}.hljs-value .hljs-important{color:#ff7700;font-weight:bold}.hljs-rule .hljs-keyword{color:#c5af75}.hljs-annotation,.apache .hljs-sqbracket,.nginx .hljs-built_in{color:#9b859d}.hljs-preprocessor,.hljs-preprocessor *,.hljs-pragma{color:#643820}.tex .hljs-formula{background-color:#eee;font-style:italic}.diff .hljs-header,.hljs-chunk{color:#808080;font-weight:bold}.diff .hljs-change{background-color:#bccff9}.hljs-addition{background-color:#baeeba}.hljs-deletion{background-color:#ffc8bd}.hljs-comment .hljs-doctag{font-weight:bold}.method .hljs-id{color:#000}</style>\r\n\
+	\
+';
+
+// This code is placed at the beginning of the body before the Markdeep code. 
+// $ (DOCUMENT_BODY_PREFIX) is everything in the body of PreviewBlogPage.htm up to 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodyPrefix='\
+\r\n\
+\
+	<!-- MARKDEEP_BEGIN -->\
+	<pre class="markdeep">\
+';
+// This code is placed at the end of the body after the Markdeep code. 
+// $ (DOCUMENT_BODY_SUFFIX) is everything in the body of PreviewBlogPage.htm after 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodySuffix='\
+	</pre>\
+	<!-- MARKDEEP_END -->\
+	<div>Document &lt;body&gt; code:<br/>\
+	<textarea cols="40" rows="10" id="BodyDisplayBox"></textarea></div>\
+\r\n\
+	\
+';
+
+// Get the full Markdeep code from the .md.html file without the script invocation
+MarkdeepCode=nodeToMarkdeepSource(document.body);
+MarkdeepCode=MarkdeepCode.slice(0,MarkdeepCode.lastIndexOf("<script"));
+// Bring it into a form where it can be pasted into an HTML document
+SanitizedMarkdeepCode=escapeHTMLEntities(MarkdeepCode);
+// Surround it by the prefix and suffix code and set that as body code
+document.body.innerHTML=DocumentBodyPrefix+SanitizedMarkdeepCode+DocumentBodySuffix;
+
+// Setting head attributes
+
+// Setting body attributes

docs/pybullet_quickstart_guide/PlainPreview/PrepareHTML.js

@@ -0,0 +1,102 @@
+/** Converts <>&" to their HTML escape sequences */
+function escapeHTMLEntities(str) {
+    return String(str).replace(/&/g, '&amp;').replace(/</g, '&lt;').replace(/>/g, '&gt;').replace(/"/g, '&quot;');
+}
+
+
+/** Restores the original source string's '<' and '>' as entered in
+    the document, before the browser processed it as HTML. There is no
+    way in an HTML document to distinguish an entity that was entered
+    as an entity.*/
+function unescapeHTMLEntities(str) {
+    // Process &amp; last so that we don't recursively unescape
+    // escaped escape sequences.
+    return str.
+        replace(/&lt;/g, '<').
+        replace(/&gt;/g, '>').
+        replace(/&quot;/g, '"').
+        replace(/&#39;/g, "'").
+        replace(/&ndash;/g, '--').
+        replace(/&mdash;/g, '---').
+        replace(/&amp;/g, '&');
+}
+
+
+/**
+   \param node  A node from an HTML DOM
+
+   \return A String that is a very good reconstruction of what the
+   original source looked like before the browser tried to correct
+   it to legal HTML.
+ */
+function nodeToMarkdeepSource(node, leaveEscapes) {
+    var source = node.innerHTML;
+
+    // Markdown uses <john@bar.com> e-mail syntax, which HTML parsing
+    // will try to close by inserting the matching close tags at the end of the
+    // document. Remove anything that looks like that and comes *after*
+    // the first fallback style.
+    source = source.replace(/(?:<style class="fallback">[\s\S]*?<\/style>[\s\S]*)<\/\S+@\S+\.\S+?>/gim, '');
+    
+    // Remove artificially inserted close tags
+    source = source.replace(/<\/h?ttps?:.*>/gi, '');
+    
+    // Now try to fix the URLs themselves, which will be 
+    // transformed like this: <http: casual-effects.com="" markdeep="">
+    source = source.replace(/<(https?): (.*?)>/gi, function (match, protocol, list) {
+
+        // Remove any quotes--they wouldn't have been legal in the URL anyway
+        var s = '<' + protocol + '://' + list.replace(/=""\s/g, '/');
+
+        if (s.substring(s.length - 3) === '=""') {
+            s = s.substring(0, s.length - 3);
+        }
+
+        // Remove any lingering quotes (since they
+        // wouldn't have been legal in the URL)
+        s = s.replace(/"/g, '');
+
+        return s + '>';
+    });
+
+    // Remove the "fallback" style tags
+    source = source.replace(/<style class=["']fallback["']>.*?<\/style>/gmi, '');
+
+    source = unescapeHTMLEntities(source);
+
+    return source;
+}
+
+// $ (FULL_DOCUMENT_HEAD) is replaced by the contents of the <head> found in 
+// PreviewBlogPage.htm. This document head will overwrite whatever Markdeep does to 
+// the head at the very end.
+FullDocumentHead='\
+$(FULL_DOCUMENT_HEAD)\
+';
+
+// This code is placed at the beginning of the body before the Markdeep code. 
+// $ (DOCUMENT_BODY_PREFIX) is everything in the body of PreviewBlogPage.htm up to 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodyPrefix='\
+$(DOCUMENT_BODY_PREFIX)\
+	<!-- MARKDEEP_BEGIN -->\
+	<pre class="markdeep">\
+';
+// This code is placed at the end of the body after the Markdeep code. 
+// $ (DOCUMENT_BODY_SUFFIX) is everything in the body of PreviewBlogPage.htm after 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodySuffix='\
+	</pre>\
+	<!-- MARKDEEP_END -->\
+	<div>Document &lt;body&gt; code:<br/>\
+	<textarea cols="40" rows="10" id="BodyDisplayBox"></textarea></div>\
+$(DOCUMENT_BODY_SUFFIX)\
+';
+
+// Get the full Markdeep code from the .md.html file without the script invocation
+MarkdeepCode=nodeToMarkdeepSource(document.body);
+MarkdeepCode=MarkdeepCode.slice(0,MarkdeepCode.lastIndexOf("<script"));
+// Bring it into a form where it can be pasted into an HTML document
+SanitizedMarkdeepCode=escapeHTMLEntities(MarkdeepCode);
+// Surround it by the prefix and suffix code and set that as body code
+document.body.innerHTML=DocumentBodyPrefix+SanitizedMarkdeepCode+DocumentBodySuffix;

docs/pybullet_quickstart_guide/PlainPreview/PreviewBlogPage.htm

@@ -0,0 +1,17 @@
+<!DOCTYPE html>
+<html lang="en-US">
+	<head>
+		<meta http-equiv="content-type" content="text/html; charset=UTF-8">
+		<meta charset="UTF-8">
+
+		<!-- Markdeep styles -->
+		<style>body{counter-reset: h1 h2 h3 h4 h5 h6}.md code,pre{font-family:Menlo,Consolas,monospace;font-size:15.018802542857143px;line-height:140%}.md div.title{font-size:26px;font-weight:800;line-height:120%;text-align:center}.md div.afterTitles{height:10px}.md div.subtitle{text-align:center}.md .image{display:inline-block}.md div.imagecaption,.md div.tablecaption,.md div.listingcaption{margin:0.2em 5px 10px 5px;text-align: justify;font-style:italic}.md div.imagecaption{margin-bottom:0}.md img{max-width:100%;page-break-inside:avoid}li{text-align:left};.md div.tilde{margin:20px 0 -10px;text-align:center}.md blockquote.fancyquote{margin:25px 0 25px;text-align:left;line-height:160%}.md blockquote.fancyquote::before{content:"“";color:#DDD;font-family:Times New Roman;font-size:45px;line-height:0;margin-right:6px;vertical-align:-0.3em}.md span.fancyquote{font-size:118%;color:#777;font-style:italic}.md span.fancyquote::after{content:"”";font-style:normal;color:#DDD;font-family:Times New Roman;font-size:45px;line-height:0;margin-left:6px;vertical-align:-0.3em}.md blockquote.fancyquote .author{width:100%;margin-top:10px;display:inline-block;text-align:right}.md small{font-size:60%}.md div.title,contents,.md .tocHeader,h1,h2,h3,h4,h5,h6,.md .shortTOC,.md .mediumTOC,.nonumberh1,.nonumberh2,.nonumberh3,.nonumberh4,.nonumberh5,.nonumberh6{font-family:Verdana,Helvetica,Arial,sans-serif;margin:13.4px 0 13.4px;padding:15px 0 3px;border-top:none;clear:both}.md svg.diagram{display:block;font-family:Menlo,Consolas,monospace;font-size:15.018802542857143px;text-align:center;stroke-linecap:round;stroke-width:2px;page-break-inside:avoid;stroke:#000;fill:#000}.md svg.diagram .opendot{fill:#FFF}.md svg.diagram text{stroke:none}.md h1,.tocHeader,.nonumberh1{border-bottom:3px solid;font-size:20px;font-weight:bold;}h1,.nonumberh1{counter-reset: h2 h3 h4 h5 h6}h2,.nonumberh2{counter-reset: h3 h4 h5 h6;border-bottom:2px solid #999;color:#555;font-size:18px;}h3,h4,h5,h6,.nonumberh3,.nonumberh4,.nonumberh5,.nonumberh6{font-family:Helvetica,Arial,sans-serif;color:#555;font-size:16px;}h3{counter-reset:h4 h5 h6}h4{counter-reset:h5 h6}h5{counter-reset:h6}.md table{border-collapse:collapse;line-height:140%;page-break-inside:avoid}.md table.table{margin:auto}.md table.calendar{width:100%;margin:auto;font-size:11px;font-family:Helvetica,Arial,sans-serif}.md table.calendar th{font-size:16px}.md .today{background:#ECF8FA}.md .calendar .parenthesized{color:#999;font-style:italic}.md div.tablecaption{text-align:center}.md table.table th{color:#FFF;background-color:#AAA;border:1px solid #888;padding:8px 15px 8px 15px}.md table.table td{padding:5px 15px 5px 15px;border:1px solid #888}.md table.table tr:nth-child(even){background:#EEE}.md pre.tilde{border-top: 1px solid #CCC;border-bottom: 1px solid #CCC;padding: 5px 0 5px 20px;margin:0 0 30px 0;background:#FCFCFC;page-break-inside:avoid}.md a:link, .md a:visited{color:#38A;text-decoration:none}.md a:link:hover{text-decoration:underline}.md dt{font-weight:700}dl>.md dd{padding:0 0 18px}.md dl>table{margin:35px 0 30px}.md code{white-space:pre;page-break-inside:avoid}.md .endnote{font-size:13px;line-height:15px;padding-left:10px;text-indent:-10px}.md .bib{padding-left:80px;text-indent:-80px;text-align:left}.markdeepFooter{font-size:9px;text-align:right;padding-top:80px;color:#999}.md .mediumTOC{float:right;font-size:12px;line-height:15px;border-left:1px solid #CCC;padding-left:15px;margin:15px 0px 15px 25px}.md .mediumTOC .level1{font-weight:600}.md .longTOC .level1{font-weight:600;display:block;padding-top:12px;margin:0 0 -20px}.md .shortTOC{text-align:center;font-weight:bold;margin-top:15px;font-size:14px}</style>
+
+		<!-- hljs styles -->
+		<style>.hljs{display:block;overflow-x:auto;padding:0.5em;background:#fff;color:#000;-webkit-text-size-adjust:none}.hljs-comment{color:#006a00}.hljs-keyword{color:#02E}.hljs-literal,.nginx .hljs-title{color:#aa0d91}.method,.hljs-list .hljs-title,.hljs-tag .hljs-title,.setting .hljs-value,.hljs-winutils,.tex .hljs-command,.http .hljs-title,.hljs-request,.hljs-status,.hljs-name{color:#008}.hljs-envvar,.tex .hljs-special{color:#660}.hljs-string{color:#c41a16}.hljs-tag .hljs-value,.hljs-cdata,.hljs-filter .hljs-argument,.hljs-attr_selector,.apache .hljs-cbracket,.hljs-date,.hljs-regexp{color:#080}.hljs-sub .hljs-identifier,.hljs-pi,.hljs-tag,.hljs-tag .hljs-keyword,.hljs-decorator,.ini .hljs-title,.hljs-shebang,.hljs-prompt,.hljs-hexcolor,.hljs-rule .hljs-value,.hljs-symbol,.hljs-symbol .hljs-string,.hljs-number,.css .hljs-function,.hljs-function .hljs-title,.coffeescript .hljs-attribute{color:#A0C}.hljs-function .hljs-title{font-weight:bold;color:#000}.hljs-class .hljs-title,.smalltalk .hljs-class,.hljs-type,.hljs-typename,.hljs-tag .hljs-attribute,.hljs-doctype,.hljs-class .hljs-id,.hljs-built_in,.setting,.hljs-params,.clojure .hljs-attribute{color:#5c2699}.hljs-variable{color:#3f6e74}.css .hljs-tag,.hljs-rule .hljs-property,.hljs-pseudo,.hljs-subst{color:#000}.css .hljs-class,.css .hljs-id{color:#9b703f}.hljs-value .hljs-important{color:#ff7700;font-weight:bold}.hljs-rule .hljs-keyword{color:#c5af75}.hljs-annotation,.apache .hljs-sqbracket,.nginx .hljs-built_in{color:#9b859d}.hljs-preprocessor,.hljs-preprocessor *,.hljs-pragma{color:#643820}.tex .hljs-formula{background-color:#eee;font-style:italic}.diff .hljs-header,.hljs-chunk{color:#808080;font-weight:bold}.diff .hljs-change{background-color:#bccff9}.hljs-addition{background-color:#baeeba}.hljs-deletion{background-color:#ffc8bd}.hljs-comment .hljs-doctag{font-weight:bold}.method .hljs-id{color:#000}</style>
+	</head>
+
+	<body>
+$(ARTICLE_HTML_CODE)
+	</body>
+</html>

docs/pybullet_quickstart_guide/PlainPreview/SetMarkdeepMode.js

@@ -0,0 +1 @@
+window.markdeepOptions={mode:"html"};

docs/pybullet_quickstart_guide/PlainPreview/ShowHeadAdditions.html

@@ -0,0 +1,29 @@
+<!DOCTYPE html>
+<html lang="en-US">
+	<head>
+		<meta http-equiv="content-type" content="text/html; charset=UTF-8">
+		<meta charset="UTF-8">
+	<body>
+		<script>window.markdeepOptions={mode:"script"};</script>
+		<script src="./markdeep.min.js"></script>
+		<div>Document &lt;head&gt; additions:<br/>
+		<textarea cols="80" rows="40" id="HeadDisplayBox"></textarea></div>
+		<script>
+			// Get the complete Markdeep and hljs stylesheet
+			FullStylesheet=window.markdeep.stylesheet();
+			// It should consist of three <style> tags which we pick apart
+			StyleTagList=FullStylesheet.split("<style>");
+			// The second one defines section number, which we do not want, so we 
+			// just drop it. The other two are Markdeep and hljs styles.
+			MarkdeepStylesheet="<style>"+StyleTagList[1];
+			HLJSStylesheet="<style>"+StyleTagList[3];
+			// Now lets show the user what we found
+			document.getElementById("HeadDisplayBox").textContent=
+				 "<!-- Markdeep styles -->\r\n"
+				+MarkdeepStylesheet
+				+"\r\n\r\n<!-- hljs styles -->\r\n"
+				+HLJSStylesheet
+				+"\r\n\r\n<!-- MathJax invocation -->\r\n<script type=\"text/javascript\" async=\"\" src=\"https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML\">\r\n";
+		</script>
+	</body>
+</html>

docs/pybullet_quickstart_guide/PyBulletQuickstartGuide.md.html

@@ -0,0 +1,466 @@
+<meta charset="utf-8"><!-- -*- markdown -*- -->
+
+
+            **PyBullet Quickstart Guide**
+   [Erwin Coumans](http://twitter.com/erwincoumans), [Google Brain Robotics](https://research.google.com/teams/brain/robotics), <erwincoumans@google.com>
+   [Yunfei Bai](http://yunfei-bai.com), [X](https://x.company), <yunfeibai@x.team>
+
+
+
+Introduction
+======================================================================================
+
+pybullet is an easy to use Python module for physics simulation for robotics, games, visual effects and machine learning. With pybullet you can load articulated bodies from URDF, SDF, MJCF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics, collision detection and ray intersection queries. The Bullet Physics SDK includes pybullet robotic examples such as a simulated Minitaur quadruped, humanoids running using TensorFlow inference and KUKA arms grasping objects.
+
+![<small>[CoRL VR BotLab demo](https://github.com/erwincoumans/pybullet_robots/tree/master/corl_demo)
+  </small>](images/CoRL_VR_demo.png width="80%" border="2")
+
+Aside from physics simulation, there are bindings to rendering, with a CPU renderer (TinyRenderer) and OpenGL visualization and support for Virtual Reality headsets such as HTC Vive and Oculus Rift. pybullet also has functionality to perform collision detection queries (closest points, overlapping pairs, ray intersection test etc) and to add debug rendering (debug lines and text). pybullet has cross-platform built-in client-server support for shared memory, UDP and TCP networking. So you can run pybullet on Linux connecting to a Windows VR server.
+
+pybullet wraps the new Bullet C-API, which is designed to be independent from the underlying physics engine and render engine, so we can easily migrate to newer versions of Bullet, or use a different physics engine or render engine. By default, pybullet uses the Bullet 2.x API on the CPU. We will expose Bullet 3.x running on GPU using OpenCL as well. There is also a C++ API similar to pybullet, see b3RobotSimulatorClientAPI.
+
+pybullet can be easily used with TensorFlow and frameworks such as OpenAI Gym. Researchers from Google Brain [1,2], X, Stanford AI Lab and OpenAI use pybullet/Bullet C-API.
+
+The installation of pybullet is as simple as (sudo) pip install pybullet (Python 2.x), pip3 install pybullet. This will expose the pybullet module as well as pybullet_envs Gym environments.
+
+Hello pybullet World
+
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ python
+import pybullet as p
+import pybullet_data
+physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
+p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
+p.setGravity(0,0,-10)
+planeId = p.loadURDF("plane.urdf")
+cubeStartPos = [0,0,1]
+cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
+boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
+p.stepSimulation()
+cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
+print(cubePos,cubeOrn)
+p.disconnect()
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+connect, disconnect
+--------------------------------------------------------------------------------------------
+After importing the pybullet module, the first thing to do is 'connecting' to the physics simulation. pybullet is designed around a client-server driven API, with a client sending commands and a physics server returning the status. pybullet has some built-in physics servers: DIRECT and GUI. Both GUI and DIRECT connections will execute the physics simulation and rendering in the same process as pybullet.
+
+Note that in DIRECT mode you cannot access the OpenGL and VR hardware features, as described in the "Virtual Reality" and "Debug GUI, Lines, Text, Parameters" chapters. DIRECT mode does allow rendering of images using the built-in software renderer through the 'getCameraImage' API. This can be useful for running simulations in the cloud on servers without GPU.
+
+You can provide your own data files, or you can use the pybullet_data package that ships with pybullet. For this, import pybullet_data and register the directory using pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()).
+
+getConnectionInfo
+--------------------------------------------------------------------------------------------
+Given a physicsClientId will return the list [isConnected, connectionMethod]
+
+connect using DIRECT, GUI
+--------------------------------------------------------------------------------------------
+
+The DIRECT connection sends the commands directly to the physics engine, without using any transport layer and no graphics visualization window, and directly returns the status after executing the command.
+
+The GUI connection will create a new graphical user interface (GUI) with 3D OpenGL rendering, within the same process space as pybullet. On Linux and Windows this GUI runs in a separate thread, while on OSX it runs in the same thread due to operating system limitations. On Mac OSX you may see a spinning wheel in the OpenGL Window, until you run a 'stepSimulation' or other pybullet command.
+The commands and status messages are sent between pybullet client and the GUI physics simulation server using an ordinary memory buffer.
+
+It is also possible to connect to a physics server in a different process on the same machine or on a remote machine using SHARED_MEMORY, UDP or TCP networking. See the section about Shared Memory, UDP and TCP for details.
+
+Unlike almost all other methods, this method doesn't parse keyword arguments, due to backward compatibility.
+
+The connect input arguments are:
+
+
+| field    | name                            | type                                          | description                                                                                                                                                                                                                                                                                                                                                                            |
+|----------|---------------------------------|-----------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| required | connection mode                 | integer: DIRECT, GUI, SHARED_MEMORY, UDP, TCP | DIRECT mode create a new physics engine and directly communicates with it. GUI will create a physics engine with graphical GUI frontend and communicates with it. SHARED_MEMORY will connect to an existing physics engine process on the same machine, and communicates with it over shared memory. TCP or UDP will connect to an existing physics server over TCP or UDP networking. |
+| optional | key                             | int                                           | in SHARED_MEMORY mode, optional shared memory key. When starting ExampleBrowser or SharedMemoryPhysics_* you can use optional command-line --shared_memory_key to set the key. This allows to run multiple servers on the same machine.                                                                                                                                                |
+| optional | UdpNetworkAddress (UDP and TCP) | string                                        | IP address or host name, for example "127.0.0.1" or "localhost" or "mymachine.domain.com"                                                                                                                                                                                                                                                                                              |
+| optional | UdpNetworkPort (UDP and TCP)    | integer                                       | UDP port number. Default UDP port is 1234, default TCP port is 6667 (matching the defaults in the server)                                                                                                                                                                                                                                                                              |
+| optional | options                         | string                                        | command-line option passed into the GUI server. At the moment, only the --opengl2 flag is enabled: by default, Bullet uses OpenGL3, but some environments such as virtual machines or remote desktop clients only support OpenGL2. Only one command-line argument can be passed on at the moment.                                                                                      |
+
+connect returns a physics client id or -1 if not connected. The physics client Id is an optional argument to most of the other pybullet commands. If you don't provide it, it will assume physics client id = 0. You can connect to multiple different physics servers, except for GUI.
+
+For example:
+
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ python
+pybullet.connect(pybullet.DIRECT)
+pybullet.connect(pybullet.GUI, options="--opengl2")
+pybullet.connect(pybullet.SHARED_MEMORY,1234)
+pybullet.connect(pybullet.UDP,"192.168.0.1")
+pybullet.connect(pybullet.UDP,"localhost", 1234)
+pybullet.connect(pybullet.TCP,"localhost", 6667)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+connect using Shared Memory
+--------------------------------------------------------------------------------------------
+There are a few physics servers that allow shared memory connection: the App_SharedMemoryPhysics, App_SharedMemoryPhysics_GUI and the Bullet Example Browser has one example under Experimental/Physics Server that allows shared memory connection. This will let you execute the physics simulation and rendering in a separate process.
+
+You can also connect over shared memory to the App_SharedMemoryPhysics_VR, the Virtual Reality application with support for head-mounted display and 6-dof tracked controllers such as HTC Vive and Oculus Rift with Touch controllers. Since the Valve OpenVR SDK only works properly under Windows, the App_SharedMemoryPhysics_VR can only be build under Windows using premake (preferably) or cmake.
+
+connect using UDP or TCP networking
+--------------------------------------------------------------------------------------------
+For UDP networking, there is a App_PhysicsServerUDP that listens to a certain UDP port. It uses the open source enet library for reliable UDP networking. This allows you to execute the physics simulation and rendering on a separate machine. For TCP pybullet uses the clsocket library. This can be useful when using SSH tunneling from a machine behind a firewall to a robot simulation. For example you can run a control stack or machine learning using pybullet on Linux, while running the physics server on Windows in Virtual Reality using HTC Vive or Rift.
+
+One more UDP application is the App_PhysicsServerSharedMemoryBridgeUDP application that acts as a bridge to an existing physics server: you can connect over UDP to this bridge, and the bridge connects to a physics server using shared memory: the bridge passes messages between client and server. In a similar way there is a TCP version (replace UDP by TCP).
+
+Note: at the moment, both client and server need to be either 32bit or 64bit builds!
+
+disconnect
+--------------------------------------------------------------------------------------------
+You can disconnect from a physics server, using the physics client Id returned by the connect call (if non-negative). A 'DIRECT' or 'GUI' physics server will shutdown. A separate (out-of-process) physics server will keep on running. See also 'resetSimulation' to remove all items.
+
+Parameters of disconnect:
+
+| field    | name            | type | description                                                         |
+|----------|-----------------|------|---------------------------------------------------------------------|
+| optional | physicsClientId | int  | if you connect to multiple physics servers, you can pick which one. |
+
+setGravity
+--------------------------------------------------------------------------------------------
+By default, there is no gravitational force enabled. setGravity lets you set the default gravity force for all objects.
+The setGravity input parameters are: (no return value)
+
+| field    | name            | type  | description                                                         |
+|----------|-----------------|-------|---------------------------------------------------------------------|
+| required | gravityX        | float | gravity force along the X world axis                                |
+| required | gravityY        | float | gravity force along the Y world axis                                |
+| required | gravityZ        | float | gravity force along the Z world axis                                |
+| required | physicsClientId | int   | if you connect to multiple physics servers, you can pick which one. |
+
+
+loadURDF
+--------------------------------------------------------------------------------------------
+The loadURDF will send a command to the physics server to load a physics model from a Universal Robot Description File (URDF). The URDF file is used by the ROS project (Robot Operating System) to describe robots and other objects, it was created by the WillowGarage and the Open Source Robotics Foundation (OSRF). Many robots have public URDF files, you can find a description and tutorial here: http://wiki.ros.org/urdf/Tutorials
+
+Important note: most joints (slider, revolute, continuous) have motors enabled by default that prevent free motion. This is similar to a robot joint with a very high-friction harmonic drive. You should set the joint motor control mode and target settings using pybullet.setJointMotorControl2. See the setJointMotorControl2 API for more information.
+
+Warning: by default, pybullet will cache some files to speed up loading. You can disable file caching using setPhysicsEngineParameter(enableFileCaching=0).
+
+The loadURDF arguments are:
+
+
+| option   | name                  | type   | description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     |
+|----------|-----------------------|--------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| required | fileName              | string | a relative or absolute path to the URDF file on the file system of the physics server.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          |
+| optional | basePosition          | vec3   | create the base of the object at the specified position in world space coordinates [X,Y,Z]                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      |
+| optional | baseOrientation       | vec4   | create the base of the object at the specified orientation as world space quaternion [X,Y,Z,W]                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  |
+| optional | useMaximalCoordinates | int    | Experimental. By default, the joints in the URDF file are created using the reduced coordinate method: the joints are simulated using the Featherstone Articulated Body algorithm (btMultiBody in Bullet 2.x). The useMaximalCoordinates option will create a 6 degree of freedom rigid body for each link, and constraints between those rigid bodies are used to model joints.                                                                                                                                                                                                                                                                                                |
+| optional | useFixedBase          | int    | force the base of the loaded object to be static                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                |
+| optional | flags                 | int    | URDF_USE_INERTIA_FROM_FILE: by default, Bullet recomputed the inertia tensor based on mass and volume of the collision shape. If you can provide more accurate inertia tensor, use this flag.URDF_USE_SELF_COLLISION: by default, Bullet disables self-collision. This flag let's you enable it. You can customize the self-collision behavior using the following flags: URDF_USE_SELF_COLLISION_EXCLUDE_PARENT will discard self-collision between links that are directly connected (parent and child).URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS will discard self-collisions between a child link and any of its ancestors (parents, parents of parents, up to the base). |
+| optional | globalScaling         | float  | globalScaling will apply a scale factor to the URDF model.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      |
+| optional | physicsClientId       | int    | if you are connected to multiple servers, you can pick one.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     |
+loadURDF returns a body unique id, a non-negative integer value. If the URDF file cannot be loaded, this integer will be negative and not a valid body unique id.
+
+loadBullet, loadSDF, loadMJCF
+--------------------------------------------------------------------------------------------
+You can also load objects from other file formats, such as .bullet, .sdf and .mjcf. Those file formats support multiple objects, so the return value is a list of object unique ids. The SDF format is explained in detail at http://sdformat.org. The loadSDF command only extracts some essential parts of the SDF related to the robot models and geometry, and ignores many elements related to cameras, lights and so on. The loadMJCF command performs basic import of MuJoCo MJCF xml files, used in OpenAI Gym. See also the Important note under loadURDF related to default joint motor settings, and make sure to use setJointMotorControl2.
+
+
+| option   | name                  | type   | description                                                                                                                   |
+|----------|-----------------------|--------|-------------------------------------------------------------------------------------------------------------------------------|
+| required | fileName              | string | a relative or absolute path to the URDF file on the file system of the physics server.                                        |
+| optional | useMaximalCoordinates | int    | Experimental. See loadURDF for more details.                                                                                  |
+| optional | globalScaling         | float  | every object will be scaled using this scale factor (including links, link frames, joint attachments and linear joint limits) |
+| optional | physicsClientId       | int    | if you are connected to multiple servers, you can pick one.                                                                   |
+
+loadBullet, loadSDF and loadMJCF will return an array of object unique ids:
+
+| name            | type        | description                                                    |
+|-----------------|-------------|----------------------------------------------------------------|
+| objectUniqueIds | list of int | the list includes the object unique id for each object loaded. |
+
+
+createCollisionShape/VisualShape
+--------------------------------------------------------------------------------------------
+Although the recommended and easiest way to create stuff in the world is using the loading functions (loadURDF/SDF/MJCF/Bullet), you can also create collision and visual shapes programmatically and use them to create a multi body using createMultiBody. See the createMultiBodyLinks.py and createVisualShape.py example in the Bullet Physics SDK.
+
+The input parameters for createCollisionShape are
+
+| option   | name            | type                  | description                                                                                                                                                                            |
+|----------|-----------------|-----------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| required | shapeType       | int                   | GEOM_SPHERE, GEOM_BOX, GEOM_CAPSULE, GEOM_CYLINDER, GEOM_PLANE, GEOM_MESH                                                                                                              |
+| optional | radius          | float                 | default 0.5: GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER                                                                                                                                  |
+| optional | halfExtents     | vec3 list of 3 floats | default [1,1,1]: for GEOM_BOX                                                                                                                                                          |
+| optional | height          | float                 | default: 1: for GEOM_CAPSULE, GEOM_CYLINDER                                                                                                                                            |
+| optional | fileName        | string                | Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each object (marked as 'o') in the .obj file.                                                      |
+| optional | meshScale       | vec3 list of 3 floats | default: [1,1,1], for GEOM_MESH                                                                                                                                                        |
+| optional | planeNormal     | vec3 list of 3 floats | default: [0,0,1] for GEOM_PLANE                                                                                                                                                        |
+| optional | flags           | int                   | GEOM_FORCE_CONCAVE_TRIMESH: for GEOM_MESH, this will create a concave static triangle mesh. This should not be used with dynamic / moving objects, only for static (mass = 0) terrain. |
+| optional | physicsClientId | int                   | If you are connected to multiple servers, you can pick one.                                                                                                                            |
+
+The return value is a non-negative int unique id for the collision shape or -1 if the call failed.
+
+createVisualShape
+--------------------------------------------------------------------------------------------
+You can create a visual shape in a similar way to creating a collision shape, with some additional arguments to control the visual appearance, such as diffuse and specular color. When you use the GEOM_MESH type, you can point to a Wavefront OBJ file, and the visual shape will parse some parameters from the material file (.mtl) and load a texture. Note that large textures (above 1024x1024 pixels) can slow down the loading and run-time performance.
+
+The input parameters are
+
+| option   | name                   | type                   | description                                                                                                                       |
+|----------|------------------------|------------------------|-----------------------------------------------------------------------------------------------------------------------------------|
+| required | shapeType              | int                    | GEOM_SPHERE, GEOM_BOX, GEOM_CAPSULE, GEOM_CYLINDER, GEOM_PLANE, GEOM_MESH                                                         |
+| optional | radius                 | float                  | default 0.5: GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER                                                                             |
+| optional | halfExtents            | vec3 list of 3 floats  | default [1,1,1]: for GEOM_BOX                                                                                                     |
+| optional | height                 | float                  | default: 1: for GEOM_CAPSULE, GEOM_CYLINDER                                                                                       |
+| optional | fileName               | string                 | Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each object (marked as 'o') in the .obj file. |
+| optional | meshScale              | vec3 list of 3 floats  | default: [1,1,1], for GEOM_MESH                                                                                                   |
+| optional | planeNormal            | vec3 list of 3 floats  | default: [0,0,1] for GEOM_PLANE                                                                                                   |
+| optional | flags                  | int                    | unused at the moment                                                                                                              |
+| optional | rgbaColor              | vec4, list of 4 floats | color components for red, green, blue and alpha, each in range [0..1].                                                            |
+| optional | specularColor          | vec3, list of 3 floats | specular reflection color, red, green, blue components in range [0..1]                                                            |
+| optional | visualFramePosition    | vec3, list of 3 floats | translational offset of the visual shape with respect to the link frame                                                           |
+| optional | visualFrameOrientation | vec4, list of 4 floats | rotational offset (quaternion x,y,z,w) of the visual shape with respect to the link frame                                         |
+| optional | physicsClientId        | int                    | If you are connected to multiple servers, you can pick one.                                                                       |
+
+The return value is a non-negative int unique id for the visual shape or -1 if the call failed.
+
+createMultiBody
+--------------------------------------------------------------------------------------------
+Although the easiest way to create stuff in the world is using the loading functions (loadURDF/SDF/MJCF/Bullet), you can create a multi body using createMultiBody. 
+See the createMultiBodyLinks.py example in the Bullet Physics SDK. The parameters of createMultiBody are very similar to URDF and SDF parameters.
+
+You can create a multi body with only a single base without joints/child links or you can create a multi body with joints/child links. If you provide links, make sure the size of every list is the same (len(linkMasses) == len(linkCollisionShapeIndices) etc). The input parameters for createMultiBody are:
+
+| option   | name                          | type                   | description                                                                                                               |
+|----------|-------------------------------|------------------------|---------------------------------------------------------------------------------------------------------------------------|
+| optional | baseMass                      | float                  | mass of the base, in kg (if using SI units)                                                                               |
+| optional | baseCollisionShapeIndex       | int                    | unique id from createCollisionShape or -1. You can re-use the collision shape for multiple multibodies (instancing)       |
+| optional | baseVisualShapeIndex          | int                    | unique id from createVisualShape or -1. You can reuse the visual shape (instancing)                                       |
+| optional | basePosition                  | vec3, list of 3 floats | Cartesian world position of the base                                                                                      |
+| optional | baseOrientation               | vec4, list of 4 floats | Orientation of base as quaternion [x,y,z,w]                                                                               |
+| optional | baseInertialFramePosition     | vec3, list of 3 floats | Local position of inertial frame                                                                                          |
+| optional | baseInertialFrameOrientation  | vec4, list of 4 floats | Local orientation of inertial frame, [x,y,z,w]                                                                            |
+| optional | linkMasses                    | list of float          | List of the mass values, one for each link.                                                                               |
+| optional | linkCollisionShapeIndices     | list of int            | List of the unique id, one for each link.                                                                                 |
+| optional | linkVisualShapeIndices        | list of int            | list of the visual shape unique id for each link                                                                          |
+| optional | linkPositions                 | list of vec3           | list of local link positions, with respect to parent                                                                      |
+| optional | linkOrientations              | list of vec4           | list of local link orientations, w.r.t. parent                                                                            |
+| optional | linkInertialFramePositions    | list of vec3           | list of local inertial frame pos. in link frame                                                                           |
+| optional | linkInertialFrameOrientations | list of vec4           | list of local inertial frame orn. in link frame                                                                           |
+| optional | linkParentIndices             | list of int            | Link index of the parent link or 0 for the base.                                                                          |
+| optional | linkJointTypes                | list of int            | list of joint types, one for each link. Only JOINT_REVOLUTE, JOINT_PRISMATIC, and JOINT_FIXED is supported at the moment. |
+| optional | linkJointAxis                 | list of vec3           | Joint axis in local frame                                                                                                 |
+| optional | useMaximalCoordinates         | int                    | experimental, best to leave it 0/false.                                                                                   |
+| optional | physicsClientId               | int                    | If you are connected to multiple servers, you can pick one.                                                               |
+
+
+The return value of createMultiBody is a non-negative unique id or -1 for failure. Example:
+
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ python
+cuid = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents = [1, 1, 1])
+mass= 0 #static box
+pybullet.createMultiBody(mass,cuid)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+See also createMultiBodyLinks.py, createObstacleCourse.py and createVisualShape.py in the Bullet/examples/pybullet/examples folder.
+
+saveWorld
+--------------------------------------------------------------------------------------------
+You can create a snapshot of the current world as a pybullet Python file, stored on the server. saveWorld can be useful as a basic editing feature, setting up the robot, joint angles, object positions and environment for example in VR. Later you can just load the pybullet Python file to re-create the world. Note that not all settings are stored in the world file at the moment.
+The input arguments are:
+
+| option   | name           | type   | description                                                |
+|----------|----------------|--------|------------------------------------------------------------|
+| required | fileName       | string | filename of the pybullet file.                             |
+| optional | clientServerId | int    | if you are connected to multiple servers, you can pick one |
+
+stepSimulation
+--------------------------------------------------------------------------------------------
+stepSimulation will perform all the actions in a single forward dynamics simulation step such as collision detection, constraint solving and integration. 
+
+stepSimulation input arguments are optional:
+
+| option   | name            | type | description                                                 |
+|----------|-----------------|------|-------------------------------------------------------------|
+| optional | physicsClientId | int  | if you are connected to multiple servers, you can pick one. |
+
+
+stepSimulation has no return values.
+
+See also setRealTimeSimulation to automatically let the physics server run forward dynamics simulation based on its real-time clock.
+
+setRealTimeSimulation
+--------------------------------------------------------------------------------------------
+
+By default, the physics server will not step the simulation, unless you explicitly send a 'stepSimulation' command. This way you can maintain control determinism of the simulation. It is possible to run the simulation in real-time by letting the physics server automatically step the simulation according to its real-time-clock (RTC) using the setRealTimeSimulation command. If you enable the real-time simulation, you don't need to call 'stepSimulation'.
+
+Note that setRealTimeSimulation has no effect in DIRECT mode: in DIRECT mode the physics server and client happen in the same thread and you trigger every command. In GUI mode and in Virtual Reality mode, and TCP/UDP mode, the physics server runs in a separate thread from the client (pybullet), and setRealTimeSimulation allows the physicsserver  thread to add additional calls to stepSimulation.
+
+
+The input parameters are:
+
+| option   | name                     | type | description                                                 |
+|----------|--------------------------|------|-------------------------------------------------------------|
+| required | enableRealTimeSimulation | int  | 0 to disable real-time simulation, 1 to enable              |
+| optional | physicsClientId          | int  | if you are connected to multiple servers, you can pick one. |
+
+
+getBasePositionAndOrientation
+--------------------------------------------------------------------------------------------
+getBasePositionAndOrientation reports the current position and orientation of the base (or root link) of the body in Cartesian world coordinates. The orientation is a quaternion in [x,y,z,w] format.
+
+The getBasePositionAndOrientation input parameters are:
+
+| option   | name                     | type | description                                                 |
+|----------|--------------------------|------|-------------------------------------------------------------|
+| required | objectUniqueId           | int  | object unique id, as returned from loadURDF.                |
+| optional | physicsClientId          | int  | if you are connected to multiple servers, you can pick one. |
+
+
+getBasePositionAndOrientation returns the position list of 3 floats and orientation as list of 4 floats in [x,y,z,w] order. Use getEulerFromQuaternion to convert the quaternion to Euler if needed.
+
+See also resetBasePositionAndOrientation to reset the position and orientation of the object.
+
+This completes the first pybullet script. Bullet ships with several URDF files in the Bullet/data folder.
+
+resetBasePositionAndOrientation
+--------------------------------------------------------------------------------------------
+You can reset the position and orientation of the base (root) of each object. It is best only to do this at the start, and not during a running simulation, since the command will override the effect of all physics simulation. The linear and angular velocity is set to zero. You can use resetBaseVelocity to reset to a non-zero linear and/or angular velocity.
+
+The input arguments to resetBasePositionAndOrientation are:
+
+| required | objectUniqueId  | int  | object unique id, as returned from loadURDF.                                                  |
+|----------|-----------------|------|-----------------------------------------------------------------------------------------------|
+| required | posObj          | vec3 | reset the base of the object at the specified position in world space coordinates [X,Y,Z]     |
+| required | ornObj          | vec4 | reset the base of the object at the specified orientation as world space quaternion [X,Y,Z,W] |
+| optional | physicsClientId | int  | if you are connected to multiple servers, you can pick one.                                   |
+
+There are no return arguments.
+
+Transforms: Position and Orientation
+--------------------------------------------------------------------------------------------
+The position of objects can be expressed in Cartesian world space coordinates [x,y,z]. The orientation (or rotation) of objects can be expressed using quaternions [x,y,z,w], euler angles [yaw, pitch, roll] or 3x3 matrices. pybullet provides a few helper functions to convert between quaternions, euler angles and 3x3 matrices. In additions there are some functions to multiply and invert transforms.
+
+getQuaternionFromEuler and getEulerFromQuaternion
+--------------------------------------------------------------------------------------------
+The pybullet API uses quaternions to represent orientations. Since quaternions are not very intuitive for people, there are two APIs to convert between quaternions and Euler angles.
+The getQuaternionFromEuler input arguments are:
+
+
+| option   | name            | type                   | description                                                                                                                                 |
+|----------|-----------------|------------------------|---------------------------------------------------------------------------------------------------------------------------------------------|
+| required | eulerAngle      | vec3: list of 3 floats | The X,Y,Z Euler angles are in radians, accumulating 3 rotations expressing the roll around the X, pitch around Y and yaw around the Z axis. |
+| optional | physicsClientId | int                    | unused, added for API consistency.                                                                                                          |
+
+
+getQuaternionFromEuler returns a quaternion, vec4 list of 4 floating point values [X,Y,Z,W].
+
+getEulerFromQuaternion
+--------------------------------------------------------------------------------------------
+The getEulerFromQuaternion input arguments are:
+
+| option   | name            | type                   | description                        |
+|----------|-----------------|------------------------|------------------------------------|
+| required | quaternion      | vec4: list of 4 floats | The quaternion format is [x,y,z,w] |
+| optional | physicsClientId | int                    | unused, added for API consistency. |
+
+getEulerFromQuaternion returns alist of 3 floating point values, a vec3.
+
+getMatrixFromQuaternion 
+--------------------------------------------------------------------------------------------
+getMatrixFromQuaternion is a utility API to create a 3x3 matrix from a quaternion. The input is a quaternion and output a list of 9 floats, representing the matrix.
+
+multiplyTransforms, invertTransform
+--------------------------------------------------------------------------------------------
+pybullet provides a few helper functions to multiply and inverse transforms. This can be helpful to transform coordinates from one to the other coordinate system.
+
+The input parameters of multiplyTransforms are:
+
+| option   | name            | type                   | description                        |
+|----------|-----------------|------------------------|------------------------------------|
+| required | positionA       | vec3, list of 3 floats | position A                         |
+| required | orientationA    | vec4, list of 4 floats | quaternion [x,y,z,w]               |
+| required | positionB       | vec3, list of 3 floats | position B                         |
+| required | orientationB    | vec4, list of 4 floats | quaternion [x,y,z,w]               |
+| optional | physicsClientId | int                    | unused, added for API consistency. |
+
+The return value is a list of position (vec3) and orientation (vec4, quaternion x,y,x,w).
+
+The input and output parameters of invertTransform are:
+
+| option   | name        | type                   | description          |
+|----------|-------------|------------------------|----------------------|
+| required | position    | vec3, list of 3 floats |                      |
+| required | orientation | vec4, list of 4 floats | quaternion [x,y,z,w] |
+
+
+The output of invertTransform is a position (vec3) and orientation (vec4, quaternion x,y,x,w).
+getAPIVersion
+You can query for the API version in a year-month-0-day format. You can only connect between physics client/server of the same API version, with the same number of bits (32-bit / 64bit). There is a optional unused argument physicsClientId, added for API consistency.
+
+| option   | name            | type | description                        |
+|----------|-----------------|------|------------------------------------|
+| optional | physicsClientId | int  | unused, added for API consistency. |
+
+Controlling a robot
+======================================================================================
+
+In the Introduction we already showed how to initialize pybullet and load some objects. If you replace the file name in the loadURDF command with "r2d2.urdf" you can simulate a R2D2 robot from the ROS tutorial. Let's control this R2D2 robot to move, look around and control the gripper. For this we need to know how to access its joint motors.
+
+Base, Joints, Links
+--------------------------------------------------------------------------------------------
+
+![<small>Base, Joints and Links</small>](images/joints_and_links.png width="80%" border="0")
+
+A simulated robot as described in a URDF file has a base, and optionally links connected by joints. Each joint connects one parent link to a child link. At the root of the hierarchy there is a single root parent that we call base. The base can be either fully fixed, 0 degrees of freedom, or fully free, with 6 degrees of freedom. Since each link is connected to a parent with a single joint, the number of joints is equal to the number of links. Regular links have link indices in the range [0..getNumJoints()] Since the base is not a regular 'link', we use the convention of -1 as its link index. We use the convention that joint frames are expressed relative to the parents center of mass inertial frame, which is aligned with the principle axis of inertia.
+
+getNumJoints, getJointInfo
+--------------------------------------------------------------------------------------------
+After you load a robot you can query the number of joints using the getNumJoints API. For the r2d2.urdf this should return 15.
+
+getNumJoints input parameters:
+
+| option   | name            | type | description                                                 |
+|----------|-----------------|------|-------------------------------------------------------------|
+| required | bodyUniqueId    | int  | the body unique id, as returned by loadURDF etc.            |
+| optional | physicsClientId | int  | if you are connected to multiple servers, you can pick one. |
+
+getNumJoints returns an integer value representing the number of joints.
+
+getJointInfo
+--------------------------------------------------------------------------------------------
+For each joint we can query some information, such as its name and type.
+
+getJointInfo input parameters:
+
+| option   | name            | type | description                                                 |
+|----------|-----------------|------|-------------------------------------------------------------|
+| required | bodyUniqueId    | int  | the body unique id, as returned by loadURDF etc.            |
+| required | jointIndex      | int  | an index in the range [0 .. getNumJoints(bodyUniqueId))     |
+| optional | physicsClientId | int  | if you are connected to multiple servers, you can pick one. |
+
+getJointInfo returns a list of information:
+
+| name             | type   | description                                                                                                                                                                                                                   |
+|------------------|--------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| jointIndex       | int    | the same joint index as the input parameter                                                                                                                                                                                   |
+| jointName        | string | the name of the joint, as specified in the URDF (or SDF etc) file                                                                                                                                                             |
+| jointType        | int    | "type of the joint, this also implies the number of position and velocity variables. JOINT_REVOLUTE, JOINT_PRISMATIC, JOINT_SPHERICAL, JOINT_PLANAR, JOINT_FIXED. See the section on Base, Joint and Links for more details." |
+| qIndex           | int    | the first position index in the positional state variables for this body                                                                                                                                                      |
+| uIndex           | int    | the first velocity index in the velocity state variables for this body                                                                                                                                                        |
+| flags            | int    | reserved                                                                                                                                                                                                                      |
+| jointDamping     | float  | the joint damping value, as specified in the URDF file                                                                                                                                                                        |
+| jointFriction    | float  | the joint friction value, as specified in the URDF file                                                                                                                                                                       |
+| jointLowerLimit  | float  | Positional lower limit for slider and revolute (hinge) joints.                                                                                                                                                                |
+| jointUpperLimit  | float  | Positional upper limit for slider and revolute joints. Values ignored in case upper limit smaller than lower limit..                                                                                                                       |
+| jointMaxForce    | float  | Maximum force specified in URDF (possibly other file formats) Note that this value is not automatically used. You can use maxForce in 'setJointMotorControl2'.                                                                |
+| jointMaxVelocity | float  | Maximum velocity specified in URDF. Note that the maximum velocity is not used in actual motor control commands at the moment.                                                                                                |
+| linkName         | string | the name of the link, as specified in the URDF (or SDF etc.) file                                                                                                                                                             |
+
+setJointMotorControl2/Array
+--------------------------------------------------------------------------------------------
+Note: setJointMotorControl is obsolete and replaced by setJointMotorControl2 API. (Or even better use setJointMotorControlArray).
+
+We can control a robot by setting a desired control mode for one or more joint motors. During the stepSimulation the physics engine will simulate the motors to reach the given target value that can be reached within the maximum motor forces and other constraints. Each revolute joint and prismatic joint is motorized by default. There are 3 different motor control modes: position control, velocity control and torque control.
+
+You can effectively disable the motor by using a force of 0. You need to disable motor in order to use direct torque control. For example:
+
+
+
+
+<!--Hello MathJax:
+\[f(0)=\frac{1}{2\cdot\pi\cdot i}\cdot \oint_{|z|=1} \frac{f(z)}{z} \textrm{d}z\]
+-->
+
+
+<script window.markdeepOptions={};
+window.markdeepOptions.tocStyle="long"; src="cs371-common.js"></script>
+

docs/pybullet_quickstart_guide/WordpressPreview/Article.md.html

@@ -0,0 +1,10 @@
+<meta charset="utf-8" lang="en">
+Hello Markdeep!
+
+- Hello
+- lists!
+
+Hello MathJax:
+\[f(0)=\frac{1}{2\cdot\pi\cdot i}\cdot \oint_{|z|=1} \frac{f(z)}{z} \textrm{d}z\]
+
+<script src="MarkdeepUtility.js"></script>

docs/pybullet_quickstart_guide/WordpressPreview/BuildMarkdeepUtility.py

@@ -0,0 +1,43 @@
+import re
+
+if(__name__=="__main__"):
+    # Assemble the script which embeds the Markdeep page into the preview blog
+    PreviewBlogPage=open("PreviewBlogPage.htm","rb").read().decode("utf-8");
+    HeadMatch=re.search("<head(.*?)>(.*?)</head>",PreviewBlogPage,re.DOTALL);
+    HeadAttributes=HeadMatch.group(1);
+    FullDocumentHead=HeadMatch.group(2);
+    BodyMatch=re.search("<body(.*?)>(.*?)</body>",PreviewBlogPage,re.DOTALL);
+    BodyAttributes=BodyMatch.group(1);
+    FullPreviewBody=BodyMatch.group(2);
+    ArticleHTMLCodeMacro="$(ARTICLE_HTML_CODE)";
+    iArticleHTMLCodeMacro=FullPreviewBody.find(ArticleHTMLCodeMacro);
+    DocumentBodyPrefix=FullPreviewBody[0:iArticleHTMLCodeMacro];
+    DocumentBodySuffix=FullPreviewBody[iArticleHTMLCodeMacro+len(ArticleHTMLCodeMacro):];
+    FullPrepareHTMLCode=open("PrepareHTML.js","rb").read().decode("utf-8");
+    ReplacementList=[
+        ("$(FULL_DOCUMENT_HEAD)",FullDocumentHead),
+        ("$(DOCUMENT_BODY_PREFIX)",DocumentBodyPrefix),
+        ("$(DOCUMENT_BODY_SUFFIX)",DocumentBodySuffix)
+    ];
+    for Macro,Replacement in ReplacementList:
+        FullPrepareHTMLCode=FullPrepareHTMLCode.replace(Macro,Replacement.replace("\r\n","\\r\\n\\\r\n").replace("'","\\'"));
+    # Generate code which sets body and head attributes appropriately
+    for Element,AttributeCode in [("head",HeadAttributes),("body",BodyAttributes)]:
+        FullPrepareHTMLCode+="\r\n// Setting "+Element+" attributes\r\n";
+        for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"",AttributeCode):
+            FullPrepareHTMLCode+="document."+Element+".setAttribute(\""+Match.group(1)+"\",\""+Match.group(2)+"\");\r\n";
+    open("PrepareHTML.full.js","wb").write(FullPrepareHTMLCode.encode("utf-8"));
+
+    # Concatenate all the scripts together
+    SourceFileList=[
+                    "PrepareHTML.full.js",
+                    "SetMarkdeepMode.js",
+                    "markdeep.min.js",
+                    "DisplayMarkdeepOutput.js",
+                    "InvokeMathJax.js"
+    ];
+    OutputCode="\r\n\r\n".join(["// "+SourceFile+"\r\n\r\n"+open(SourceFile,"rb").read().decode("utf-8") for SourceFile in SourceFileList]);
+    OutputFile=open("MarkdeepUtility.js","wb");
+    OutputFile.write(OutputCode.encode("utf-8"));
+    OutputFile.close();
+    print("Done.");

docs/pybullet_quickstart_guide/WordpressPreview/DisplayMarkdeepOutput.js

@@ -0,0 +1,6 @@
+BodyHTML=document.body.innerHTML;
+BeginTag="<!-- MARKDEEP_BEGIN -->";
+EndTag="<!-- MARKDEEP_END -->";
+BodyHTML=BodyHTML.slice(BodyHTML.indexOf(BeginTag)+BeginTag.length,BodyHTML.lastIndexOf(EndTag));
+document.getElementById("BodyDisplayBox").textContent=BodyHTML;
+document.head.innerHTML=FullDocumentHead;

docs/pybullet_quickstart_guide/WordpressPreview/InvokeMathJax.js

@@ -0,0 +1,4 @@
+var MathjaxScript=document.createElement("script");
+MathjaxScript.type="text/javascript";
+MathjaxScript.src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML";
+document.head.appendChild(MathjaxScript);

docs/pybullet_quickstart_guide/WordpressPreview/PrepareHTML.js

@@ -0,0 +1,102 @@
+/** Converts <>&" to their HTML escape sequences */
+function escapeHTMLEntities(str) {
+    return String(str).replace(/&/g, '&amp;').replace(/</g, '&lt;').replace(/>/g, '&gt;').replace(/"/g, '&quot;');
+}
+
+
+/** Restores the original source string's '<' and '>' as entered in
+    the document, before the browser processed it as HTML. There is no
+    way in an HTML document to distinguish an entity that was entered
+    as an entity.*/
+function unescapeHTMLEntities(str) {
+    // Process &amp; last so that we don't recursively unescape
+    // escaped escape sequences.
+    return str.
+        replace(/&lt;/g, '<').
+        replace(/&gt;/g, '>').
+        replace(/&quot;/g, '"').
+        replace(/&#39;/g, "'").
+        replace(/&ndash;/g, '--').
+        replace(/&mdash;/g, '---').
+        replace(/&amp;/g, '&');
+}
+
+
+/**
+   \param node  A node from an HTML DOM
+
+   \return A String that is a very good reconstruction of what the
+   original source looked like before the browser tried to correct
+   it to legal HTML.
+ */
+function nodeToMarkdeepSource(node, leaveEscapes) {
+    var source = node.innerHTML;
+
+    // Markdown uses <john@bar.com> e-mail syntax, which HTML parsing
+    // will try to close by inserting the matching close tags at the end of the
+    // document. Remove anything that looks like that and comes *after*
+    // the first fallback style.
+    source = source.replace(/(?:<style class="fallback">[\s\S]*?<\/style>[\s\S]*)<\/\S+@\S+\.\S+?>/gim, '');
+    
+    // Remove artificially inserted close tags
+    source = source.replace(/<\/h?ttps?:.*>/gi, '');
+    
+    // Now try to fix the URLs themselves, which will be 
+    // transformed like this: <http: casual-effects.com="" markdeep="">
+    source = source.replace(/<(https?): (.*?)>/gi, function (match, protocol, list) {
+
+        // Remove any quotes--they wouldn't have been legal in the URL anyway
+        var s = '<' + protocol + '://' + list.replace(/=""\s/g, '/');
+
+        if (s.substring(s.length - 3) === '=""') {
+            s = s.substring(0, s.length - 3);
+        }
+
+        // Remove any lingering quotes (since they
+        // wouldn't have been legal in the URL)
+        s = s.replace(/"/g, '');
+
+        return s + '>';
+    });
+
+    // Remove the "fallback" style tags
+    source = source.replace(/<style class=["']fallback["']>.*?<\/style>/gmi, '');
+
+    source = unescapeHTMLEntities(source);
+
+    return source;
+}
+
+// $ (FULL_DOCUMENT_HEAD) is replaced by the contents of the <head> found in 
+// PreviewBlogPage.htm. This document head will overwrite whatever Markdeep does to 
+// the head at the very end.
+FullDocumentHead='\
+$(FULL_DOCUMENT_HEAD)\
+';
+
+// This code is placed at the beginning of the body before the Markdeep code. 
+// $ (DOCUMENT_BODY_PREFIX) is everything in the body of PreviewBlogPage.htm up to 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodyPrefix='\
+$(DOCUMENT_BODY_PREFIX)\
+	<!-- MARKDEEP_BEGIN -->\
+	<pre class="markdeep">\
+';
+// This code is placed at the end of the body after the Markdeep code. 
+// $ (DOCUMENT_BODY_SUFFIX) is everything in the body of PreviewBlogPage.htm after 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodySuffix='\
+	</pre>\
+	<!-- MARKDEEP_END -->\
+	<div>Document &lt;body&gt; code:<br/>\
+	<textarea cols="40" rows="10" id="BodyDisplayBox"></textarea></div>\
+$(DOCUMENT_BODY_SUFFIX)\
+';
+
+// Get the full Markdeep code from the .md.html file without the script invocation
+MarkdeepCode=nodeToMarkdeepSource(document.body);
+MarkdeepCode=MarkdeepCode.slice(0,MarkdeepCode.lastIndexOf("<script"));
+// Bring it into a form where it can be pasted into an HTML document
+SanitizedMarkdeepCode=escapeHTMLEntities(MarkdeepCode);
+// Surround it by the prefix and suffix code and set that as body code
+document.body.innerHTML=DocumentBodyPrefix+SanitizedMarkdeepCode+DocumentBodySuffix;

docs/pybullet_quickstart_guide/WordpressPreview/SetMarkdeepMode.js

@@ -0,0 +1 @@
+window.markdeepOptions={mode:"html"};

docs/pybullet_quickstart_guide/WordpressPreview/ShowHeadAdditions.html

@@ -0,0 +1,29 @@
+<!DOCTYPE html>
+<html lang="en-US">
+	<head>
+		<meta http-equiv="content-type" content="text/html; charset=UTF-8">
+		<meta charset="UTF-8">
+	<body>
+		<script>window.markdeepOptions={mode:"script"};</script>
+		<script src="./markdeep.min.js"></script>
+		<div>Document &lt;head&gt; additions:<br/>
+		<textarea cols="80" rows="40" id="HeadDisplayBox"></textarea></div>
+		<script>
+			// Get the complete Markdeep and hljs stylesheet
+			FullStylesheet=window.markdeep.stylesheet();
+			// It should consist of three <style> tags which we pick apart
+			StyleTagList=FullStylesheet.split("<style>");
+			// The second one defines section number, which we do not want, so we 
+			// just drop it. The other two are Markdeep and hljs styles.
+			MarkdeepStylesheet="<style>"+StyleTagList[1];
+			HLJSStylesheet="<style>"+StyleTagList[3];
+			// Now lets show the user what we found
+			document.getElementById("HeadDisplayBox").textContent=
+				 "<!-- Markdeep styles -->\r\n"
+				+MarkdeepStylesheet
+				+"\r\n\r\n<!-- hljs styles -->\r\n"
+				+HLJSStylesheet
+				+"\r\n\r\n<!-- MathJax invocation -->\r\n<script type=\"text/javascript\" async=\"\" src=\"https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML\">\r\n";
+		</script>
+	</body>
+</html>

docs/pybullet_quickstart_guide/cs371-common.js

@@ -0,0 +1,24 @@
+
+// Common code for styling all CS371 web pages
+
+if (location.href.indexOf('?') == -1) {
+    // Force a reload by making a bogus query.
+    var i = ((location.href.indexOf('#') + 1) || (location.href.length + 1)) - 1;
+    location = location.href.substring(0, i) + '?' + location.href.substring(i);
+}
+
+markdeepOptions = {tocStyle: 'long'};
+
+document.write("<link href='https://fonts.googleapis.com/css?family=Roboto:400,300,100,100italic,300italic,400italic,700' rel='stylesheet' type='text/css'>" +
+               "<link rel='stylesheet' type='text/css' href='cs371.css'>");
+
+// Conceal list items that use a '+', except when viewed locally
+if (/^file:\/{3}/i.test(window.location.href)) {
+    document.write('<style>li.plus { color: #D88; }</style>');
+} else {
+    document.write('<style>li.plus { display: none; }</style>');
+}
+
+document.write('<!-- Markdeep: --><style class="fallback">body{visibility:hidden;white-space:pre;font-family:monospace}</style><script src="markdeep.js"></script><script>window.alreadyProcessedMarkdeep||(document.body.style.visibility="visible")</script>');
+
+

docs/pybullet_quickstart_guide/cs371.css

@@ -0,0 +1,21 @@
+.md table  {
+  font-size: 6px;
+}
+
+body {
+    font-family: "Roboto", sans-serif;
+    background: #FFFDF9;
+}
+
+body, table {
+    font-weight: 300;
+    color: #555;
+}
+
+
+em.asterisk { 
+    font-style: normal; 
+    font-weight: 500;
+   /* color: #333;  */
+}
+

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -1326,7 +1326,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
 				float camPos[3];
 				s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPosition(camPos);
 				s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraTargetPosition(camTarget);
-				sprintf(msg,"camTargetPos=%2.2f,%2.2f,%2.2f, dist=%2.2f, pitch=%2.2f, yaw=%2.2f", camPos[0],camPos[1],camPos[2],camDist,pitch,yaw);
+				sprintf(msg,"camTargetPos=%2.2f,%2.2f,%2.2f, dist=%2.2f, pitch=%2.2f, yaw=%2.2f", camTarget[0],camTarget[1],camTarget[2],camDist,pitch,yaw);
 				gui2->setStatusBarMessage(msg, true);	
 			}
 			

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -172,10 +172,11 @@ struct BulletMJCFImporterInternalData
 
 	btAlignedObjectArray<UrdfModel*>	m_models;
 
-	//<compiler angle="radian" meshdir="mesh/" texturedir="texture/"/>
+	//<compiler angle="radian" meshdir="mesh/" texturedir="texture/" inertiafromgeom="true"/>
 	std::string m_meshDir;
 	std::string m_textureDir;
 	std::string m_angleUnits;
+	bool m_inertiaFromGeom;
 
 
 	int m_activeModel;
@@ -190,7 +191,8 @@ struct BulletMJCFImporterInternalData
 	mutable btAlignedObjectArray<btTriangleMesh*> m_allocatedMeshInterfaces;
 
 	BulletMJCFImporterInternalData()
-		:m_activeModel(-1),
+		:m_inertiaFromGeom(true),
+		m_activeModel(-1),
 		m_activeBodyUniqueId(-1)
 	{
 		m_pathPrefix[0] = 0;
@@ -248,12 +250,12 @@ struct BulletMJCFImporterInternalData
 		}
 		const char* angle = root_xml->Attribute("angle");
 		m_angleUnits = angle ? angle : "degree";  // degrees by default, http://www.mujoco.org/book/modeling.html#compiler
-#if 0
+		const char* inertiaFromGeom = root_xml->Attribute("inertiafromgeom");
-		for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
+		if(inertiaFromGeom[0] == 'f')  // false, other values assumed `true`.
 		{
-			std::string n = child_xml->Value();
+			m_inertiaFromGeom = false;
 		}
-#endif
+
 	}
 	
 	void parseAssets(TiXmlElement* root_xml, MJCFErrorLogger* logger)
@@ -1134,8 +1136,16 @@ struct BulletMJCFImporterInternalData
 				}
 				
 				massDefined = true;
-
 				handled = true;
+
+				if (!m_inertiaFromGeom) {
+					linkPtr->m_inertia.m_mass = mass;
+					linkPtr->m_inertia.m_linkLocalFrame = localInertialFrame;
+					linkPtr->m_inertia.m_ixx = localInertiaDiag[0];
+					linkPtr->m_inertia.m_iyy = localInertiaDiag[1];
+					linkPtr->m_inertia.m_izz = localInertiaDiag[2];
+				}
+
 			}
 
 			if (n=="joint")

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -613,18 +613,19 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
                 vertices.push_back(vert);
 
             }
-            btConvexHullShape* convexHull = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+            btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-            convexHull->setMargin(gUrdfDefaultCollisionMargin);
+            cylZShape->setMargin(gUrdfDefaultCollisionMargin);
-			convexHull->initializePolyhedralFeatures();
+			cylZShape->recalcLocalAabb();
-			convexHull->optimizeConvexHull();
+			cylZShape->initializePolyhedralFeatures();
+			cylZShape->optimizeConvexHull();
 			
-			//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+			//btConvexShape* cylZShape = new btConeShapeZ(cylRadius,cylLength);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
             
-            //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
+            //btVector3 halfExtents(cylRadius,cylRadius,cylLength);
             //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
             
 
-            shape = convexHull;
+            shape = cylZShape;
             break;
         }
         case URDF_GEOM_BOX:
@@ -798,6 +799,7 @@ upAxisMat.setIdentity();
 			convexHull->optimizeConvexHull();
 			//convexHull->initializePolyhedralFeatures();
 			convexHull->setMargin(gUrdfDefaultCollisionMargin);
+			convexHull->recalcLocalAabb();
 			//convexHull->setLocalScaling(collision->m_geometry.m_meshScale);
 			shape = convexHull;
 		}
@@ -845,6 +847,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
 
 			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
 			cylZShape->setMargin(gUrdfDefaultCollisionMargin);
+			cylZShape->recalcLocalAabb();
 			convexColShape = cylZShape;
 			break;
 		}

examples/Importers/ImportURDFDemo/urdfStringSplit.h

@@ -2,14 +2,20 @@
 #ifndef STRING_SPLIT_H
 #define STRING_SPLIT_H
 
+#ifdef __cplusplus
+
 #include <cstring>
 #include <string>
 #include "LinearMath/btAlignedObjectArray.h"
 
-
 void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
 
 void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 ///The string split C code is by Lars Wirzenius
 ///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
@@ -26,5 +32,9 @@ void urdfStrArrayFree(char** array);
 /* Return length of a NULL-delimited array of strings. */
 size_t urdfStrArrayLen(char** array);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif //STRING_SPLIT_H
 

examples/RenderingExamples/TinyRendererSetup.cpp

@@ -195,7 +195,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
 					m_internalData->m_depthBuffer,
                     &m_internalData->m_shadowBuffer,
 					&m_internalData->m_segmentationMaskBuffer,
-					m_internalData->m_renderObjects.size());
+					m_internalData->m_renderObjects.size(), -1);
                 
                 meshData.m_gfxShape->m_scaling[0] = scaling[0];
                 meshData.m_gfxShape->m_scaling[1] = scaling[1];

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -107,6 +107,91 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClien
 	return 0;
 }
 
+
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadStateCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_RESTORE_STATE;
+		command->m_updateFlags = 0;
+		command->m_loadStateArguments.m_fileName[0] = 0;
+		command->m_loadStateArguments.m_stateId = -1;
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+B3_SHARED_API int	b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_RESTORE_STATE);
+	if (command->m_type == CMD_RESTORE_STATE)
+	{
+		command->m_loadStateArguments.m_stateId = stateId;
+		command->m_updateFlags |= CMD_LOAD_STATE_HAS_STATEID;
+	}
+	return 0;
+
+}
+
+B3_SHARED_API int	b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_RESTORE_STATE);
+	if (command->m_type == CMD_RESTORE_STATE)
+	{
+		int len = strlen(fileName);
+		if (len < MAX_URDF_FILENAME_LENGTH)
+		{
+			strcpy(command->m_loadStateArguments.m_fileName, fileName);
+		}
+		else
+		{
+			command->m_loadStateArguments.m_fileName[0] = 0;
+		}
+		command->m_updateFlags |= CMD_LOAD_STATE_HAS_FILENAME;
+	}
+	return 0;
+}
+
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_SAVE_STATE;
+		command->m_updateFlags = 0;
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle)
+{
+	int stateId = -1;
+
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	b3Assert(status);
+	b3Assert(status->m_type == CMD_SAVE_STATE_COMPLETED);
+	if (status && status->m_type == CMD_SAVE_STATE_COMPLETED)
+	{
+		stateId =  status->m_saveStateResultArgs.m_stateId;
+	}
+	return stateId;
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -2052,11 +2137,15 @@ B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, str
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
 	const b3DynamicsInfo &dynamicsInfo = status->m_dynamicsInfo;
-	btAssert(status->m_type == CMD_GET_DYNAMICS_INFO);
+	btAssert(status->m_type == CMD_GET_DYNAMICS_INFO_COMPLETED);
 	if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
 		return false;
 
 	info->m_mass = dynamicsInfo.m_mass;
+	info->m_localInertialDiagonal[0] = dynamicsInfo.m_localInertialDiagonal[0];
+	info->m_localInertialDiagonal[1] = dynamicsInfo.m_localInertialDiagonal[1];
+	info->m_localInertialDiagonal[2] = dynamicsInfo.m_localInertialDiagonal[2];
+
 	info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff;
 	return true;
 }
@@ -2088,6 +2177,21 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle comman
 	return 0;
 }
 
+B3_SHARED_API	int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double localInertiaDiagonal[3])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
+	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
+	command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0] = localInertiaDiagonal[0];
+	command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1] = localInertiaDiagonal[1];
+	command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2] = localInertiaDiagonal[2];
+
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL;
+	return 0;
+}
+
+
 B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -2836,6 +2940,20 @@ B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandl
 	}
 }
 
+B3_SHARED_API	void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+	if(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA)
+	{
+		command->m_requestPixelDataArguments.m_flags = flags;
+		command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_FLAGS;
+	}
+}
+
+
+
 B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -116,6 +116,8 @@ B3_SHARED_API	int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, str
 	
 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
 B3_SHARED_API	int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
+B3_SHARED_API	int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double localInertiaDiagonal[3]);
+
 B3_SHARED_API	int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
 B3_SHARED_API	int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
 B3_SHARED_API	int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
@@ -207,6 +209,9 @@ B3_SHARED_API	void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryComman
 B3_SHARED_API	void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff);
 B3_SHARED_API	void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow);
 B3_SHARED_API	void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
+B3_SHARED_API	void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
+
+
 B3_SHARED_API	void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
 
 ///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices
@@ -311,7 +316,16 @@ B3_SHARED_API int	b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle
 
 
 
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient);
+B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle);
+
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadStateCommandInit(b3PhysicsClientHandle physClient);
+B3_SHARED_API int	b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId);
+B3_SHARED_API int	b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName);
+
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
+
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
 B3_SHARED_API	void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);

examples/SharedMemory/PhysicsClientExample.cpp

@@ -940,8 +940,11 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
                                                         btVector4(32,255,255,255)};
                                 if (segmentationMask>=0)
                                 {
-                                    btVector4 rgb = palette[segmentationMask&3];
+									int obIndex = segmentationMask&((1<<24)-1);
-                                     m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
+									int linkIndex = (segmentationMask>>24)-1;
+									
+									btVector4 rgb = palette[(obIndex+linkIndex)&3];
+									m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
                                         rgb.x(),
                                         rgb.y(),
                                         rgb.z(), 255); //alpha set to 255

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -1236,6 +1236,23 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 			{
 				break;
 			}
+			case CMD_SAVE_STATE_COMPLETED:
+			{
+				break;
+			}
+			case CMD_RESTORE_STATE_FAILED:
+			{
+				b3Warning("restoreState failed");
+				break;
+			}
+			case CMD_RESTORE_STATE_COMPLETED:
+			{
+				break;
+			}
+			case CMD_BULLET_SAVING_COMPLETED:
+			{
+				break;
+			}
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);

examples/SharedMemory/PhysicsDirect.cpp

@@ -1007,6 +1007,23 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		{
 			break;
 		}
+	case CMD_SAVE_STATE_COMPLETED:
+	{
+		break;
+	}
+	case CMD_RESTORE_STATE_FAILED:
+	{
+		b3Warning("restoreState failed");
+		break;
+	}
+	case CMD_RESTORE_STATE_COMPLETED:
+	{
+		break;
+	}
+	case CMD_BULLET_SAVING_COMPLETED:
+	{
+		break;
+	}
 	default:
 	{
 		//b3Warning("Unknown server status type");

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -39,6 +39,7 @@
 #include "Bullet3Common/b3ResizablePool.h"
 #include "../Utils/b3Clock.h"
 #include "b3PluginManager.h"
+#include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
 
 
 #ifdef STATIC_LINK_VR_PLUGIN
@@ -1459,6 +1460,12 @@ struct ContactPointsStateLogger : public InternalStateLogger
 	}
 };
 
+struct SaveStateData
+{
+	bParse::btBulletFile* m_bulletFile;
+	btSerializer* m_serializer;
+};
+
 struct PhysicsServerCommandProcessorInternalData
 {
 	///handle management
@@ -1476,6 +1483,8 @@ struct PhysicsServerCommandProcessorInternalData
 
 	b3VRControllerEvents m_vrControllerEvents;
 
+	btAlignedObjectArray<SaveStateData> m_savedStates;
+
 
 	btAlignedObjectArray<b3KeyboardEvent> m_keyboardEvents;
 	btAlignedObjectArray<b3MouseEvent> m_mouseEvents;
@@ -2173,8 +2182,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	
 	m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
 
+	//int maxProxies = 32768;
+	//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
     m_data->m_broadphase = new btDbvtBroadphase(m_data->m_pairCache);
     
+
     m_data->m_solver = new btMultiBodyConstraintSolver;
     
 #ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -2183,7 +2195,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
 #endif
     
-    //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
+    //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
     m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(32768);
     
     m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
@@ -3017,7 +3029,12 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc
 			m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
 														clientCmd.m_requestPixelDataArguments.m_pixelHeight);
 	}
-
+	int flags = 0;
+	if (clientCmd.m_updateFlags&REQUEST_PIXEL_ARGS_HAS_FLAGS)
+	{
+		flags = clientCmd.m_requestPixelDataArguments.m_flags;
+	}
+	m_data->m_visualConverter.setFlags(flags);
 
 	int numTotalPixels = width*height;
 	int numRemainingPixels = numTotalPixels - startPixelIndex;
@@ -3768,29 +3785,78 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
 		char pathPrefix[1024];
 		pathPrefix[0] = 0;
 
-		if (visualShape.m_geometry.m_type == URDF_GEOM_MESH)
+		const b3CreateUserShapeData& visShape = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex];
-		{
 
-			std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
+		switch (visualShape.m_geometry.m_type)
-			const std::string& error_message_prefix="";
+		{
-			std::string out_found_filename; 
+			case URDF_GEOM_CYLINDER:
-			int out_type;
-			if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
 			{
-				b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+				visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight;
+				visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius;
+				break;
+			}
+			case URDF_GEOM_BOX:
+			{
+				visualShape.m_geometry.m_boxSize.setValue(2.*visShape.m_boxHalfExtents[0],
+					2.*visShape.m_boxHalfExtents[1],
+					2.*visShape.m_boxHalfExtents[2]);
+				break;
+			}
+			case URDF_GEOM_SPHERE:
+			{
+				visualShape.m_geometry.m_sphereRadius = visShape.m_sphereRadius;
+				break;
+
+			}
+			case URDF_GEOM_CAPSULE:
+			{
+				visualShape.m_geometry.m_hasFromTo = visShape.m_hasFromTo;
+				if (visualShape.m_geometry.m_hasFromTo)
+				{
+					visualShape.m_geometry.m_capsuleFrom.setValue(visShape.m_capsuleFrom[0],
+						visShape.m_capsuleFrom[1],
+						visShape.m_capsuleFrom[2]);				
+					visualShape.m_geometry.m_capsuleTo.setValue(visShape.m_capsuleTo[0],
+						visShape.m_capsuleTo[1],
+						visShape.m_capsuleTo[2]);
+				}
+				else
+				{
+					visualShape.m_geometry.m_capsuleHeight = visShape.m_capsuleHeight;
+					visualShape.m_geometry.m_capsuleRadius = visShape.m_capsuleRadius;
+				}
+				break;
+			}
+			case URDF_GEOM_MESH:
+			{
+
+				std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshFileName;
+				const std::string& error_message_prefix = "";
+				std::string out_found_filename;
+				int out_type;
+				if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
+				{
+					b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+				}
+
+				bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
+				visualShape.m_geometry.m_meshFileType = out_type;
+				visualShape.m_geometry.m_meshFileName = fileName;
+
+				visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
+					clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
+					clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);
+				break;
+
 			}
 
-			bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); 
+			default:
-			visualShape.m_geometry.m_meshFileType = out_type;
+			{
-			visualShape.m_geometry.m_meshFileName=fileName;
+			}
-
+		};
-			visualShape.m_geometry.m_meshScale.setValue(clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[0],
+		visualShape.m_name = "in_memory";
-				clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[1],
-				clientCmd.m_createUserShapeArgs.m_shapes[userShapeIndex].m_meshScale[2]);
-		}
-		visualShape.m_name = "bla";
 		visualShape.m_materialName="";
-		visualShape.m_sourceFileLocation="blaat_line_10";
+		visualShape.m_sourceFileLocation="in_memory_unknown_line";
 		visualShape.m_linkLocalFrame.setIdentity();
 		visualShape.m_geometry.m_hasLocalMaterial = false;
 							
@@ -3840,7 +3906,6 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
 		}
 
 							
-
 		u2b.convertURDFToVisualShapeInternal(&visualShape, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
 					
 		if (vertices.size() && indices.size())
@@ -5933,6 +5998,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 	double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction;
 	double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
 	double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
+	btVector3 newLocalInertiaDiagonal(clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0],
+						clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1],
+						clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2]);
+
 	btAssert(bodyUniqueId >= 0);
 						
 	InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
@@ -5998,6 +6067,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 					mb->setBaseInertia(localInertia);
 				}
 			}
+			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
+			{
+				mb->setBaseInertia(newLocalInertiaDiagonal);
+			}
 		}
 		else
 		{
@@ -6052,6 +6125,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 						mb->getLink(linkIndex).m_inertiaLocal = localInertia;
 					}
 				}
+				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
+				{
+					mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal;
+				}
 			}
 		}
 	} else
@@ -6110,6 +6187,14 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
 				}
 				body->m_rigidBody->setMassProps(mass,localInertia);
 			}
+			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
+			{
+				btScalar orgMass = body->m_rigidBody->getInvMass();
+				if (orgMass>0)
+				{
+					body->m_rigidBody->setMassProps(mass,newLocalInertiaDiagonal);
+				}
+			}
 		}
 	}
 					
@@ -6148,11 +6233,18 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
 		if (linkIndex == -1)
 		{
 			serverCmd.m_dynamicsInfo.m_mass = mb->getBaseMass();
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getBaseInertia()[0];
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getBaseInertia()[1];
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getBaseInertia()[2];
 			serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction();
 		}
 		else
 		{
 			serverCmd.m_dynamicsInfo.m_mass = mb->getLinkMass(linkIndex);
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[0] = mb->getLinkInertia(linkIndex)[0];
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getLinkInertia(linkIndex)[1];
+			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getLinkInertia(linkIndex)[2];
+
 			if (mb->getLinkCollider(linkIndex))
 			{
 				serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction();
@@ -8094,6 +8186,89 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
 	return hasStatus;
 }
 
+bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
+{
+	BT_PROFILE("CMD_RESTORE_STATE");
+	bool hasStatus = true;
+	SharedMemoryStatus& serverCmd = serverStatusOut;
+	serverCmd.m_type = CMD_SAVE_STATE_FAILED;
+
+	btDefaultSerializer* ser = new btDefaultSerializer();
+	int currentFlags = ser->getSerializationFlags();
+	ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
+	m_data->m_dynamicsWorld->serialize(ser);
+	bParse::btBulletFile* bulletFile = new bParse::btBulletFile((char*)ser->getBufferPointer(), ser->getCurrentBufferSize());
+	bulletFile->parse(false);
+	if (bulletFile->ok())
+	{
+		serverCmd.m_type = CMD_SAVE_STATE_COMPLETED;
+		serverCmd.m_saveStateResultArgs.m_stateId = m_data->m_savedStates.size();
+		SaveStateData sd;
+		sd.m_bulletFile = bulletFile;
+		sd.m_serializer = ser;
+		m_data->m_savedStates.push_back(sd);
+	}
+	return hasStatus;
+}
+
+
+bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
+{
+	BT_PROFILE("CMD_RESTORE_STATE");
+	bool hasStatus = true;
+	SharedMemoryStatus& serverCmd = serverStatusOut;
+	serverCmd.m_type = CMD_RESTORE_STATE_FAILED;
+
+	btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
+	importer->setImporterFlags(eRESTORE_EXISTING_OBJECTS);
+
+	bool ok = false;
+
+	if (clientCmd.m_loadStateArguments.m_stateId >= 0)
+	{
+		if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size())
+		{
+			bParse::btBulletFile* bulletFile = m_data->m_savedStates[clientCmd.m_loadStateArguments.m_stateId].m_bulletFile;
+			ok = importer->convertAllObjects(bulletFile);
+		}
+	}
+	else
+	{
+		const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" };
+		int numPrefixes = sizeof(prefix) / sizeof(const char*);
+		char relativeFileName[1024];
+		FILE* f = 0;
+		bool found = false;
+
+		for (int i = 0; !f && i<numPrefixes; i++)
+		{
+			sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
+			f = fopen(relativeFileName, "rb");
+			if (f)
+			{
+				found = true;
+				break;
+			}
+		}
+		if (f)
+		{
+			fclose(f);
+		}
+
+		if (found)
+		{
+			ok = importer->loadFile(relativeFileName);
+		}
+	}
+
+	if (ok)
+	{
+		serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
+	}	
+
+	return hasStatus;
+}
+
 bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	BT_PROFILE("CMD_LOAD_BULLET");
@@ -8217,6 +8392,9 @@ bool PhysicsServerCommandProcessor::processSaveBulletCommand(const struct Shared
 	if (f)
 	{
 		btDefaultSerializer* ser = new btDefaultSerializer();
+		int currentFlags = ser->getSerializationFlags();
+		ser->setSerializationFlags(currentFlags | BT_SERIALIZE_CONTACT_MANIFOLDS);
+
 		m_data->m_dynamicsWorld->serialize(ser);
 		fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
 		fclose(f);
@@ -8514,6 +8692,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
+	case CMD_RESTORE_STATE:
+	{
+		hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
+		break;
+	}
+
+	case CMD_SAVE_STATE:
+	{
+		hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
+		break;
+	}
+
 	case CMD_LOAD_BULLET:
 		{
 			hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
@@ -8912,6 +9102,12 @@ void PhysicsServerCommandProcessor::resetSimulation()
 	if (m_data)
 	{
 		m_data->m_visualConverter.resetAll();
+		for (int i = 0; i < m_data->m_savedStates.size(); i++)
+		{
+			delete m_data->m_savedStates[i].m_bulletFile;
+			delete m_data->m_savedStates[i].m_serializer;
+		}
+		m_data->m_savedStates.clear();
 	}
 
 	removePickingConstraint();

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -80,7 +80,8 @@ protected:
 	bool processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
-
+	bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 
 	bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);
 

examples/SharedMemory/PhysicsServerExample.cpp

@@ -2169,7 +2169,11 @@ void	PhysicsServerExample::updateGraphics()
 															btVector4(32,255,255,255)};
 									if (segmentationMask>=0)
 									{
-										btVector4 rgb = palette[segmentationMask&3];
+										int obIndex = segmentationMask&((1<<24)-1);
+										int linkIndex = (segmentationMask>>24)-1;
+										
+										btVector4 rgb = palette[(obIndex+linkIndex)&3];
+										
 										 m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
 											rgb.x(),
 											rgb.y(),

examples/SharedMemory/SharedMemoryCommands.h

@@ -69,6 +69,13 @@ struct SdfArgs
 struct FileArgs
 {
 	char m_fileName[MAX_URDF_FILENAME_LENGTH];
+	int m_stateId;
+};
+
+enum EnumLoadStateArgsUpdateFlags
+{
+	CMD_LOAD_STATE_HAS_STATEID=1,
+	CMD_LOAD_STATE_HAS_FILENAME=2,
 };
 
 enum EnumUrdfArgsUpdateFlags
@@ -151,6 +158,7 @@ enum EnumChangeDynamicsInfoFlags
 	CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING=128,
 	CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING=256,
 	CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512,
+	CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024,
 
 };
 
@@ -168,6 +176,7 @@ struct ChangeDynamicsInfoArgs
 	double m_angularDamping;
 	double m_contactStiffness;
 	double m_contactDamping;
+	double m_localInertiaDiagonal[3];
 	int m_frictionAnchor;
 };
 
@@ -229,6 +238,7 @@ struct RequestPixelDataArgs
     float m_lightDiffuseCoeff;
     float m_lightSpecularCoeff;
     int m_hasShadow;
+	int m_flags;
 };
 
 enum EnumRequestPixelDataUpdateFlags
@@ -242,6 +252,8 @@ enum EnumRequestPixelDataUpdateFlags
     REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF=64,
     REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF=128,
     REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF=256,
+	REQUEST_PIXEL_ARGS_HAS_FLAGS = 512,
+
 	//don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h
 	
 };
@@ -920,6 +932,12 @@ struct b3ChangeTextureArgs
 	int m_height;
 };
 
+struct b3StateSerializationArguments
+{
+	char m_fileName[MAX_URDF_FILENAME_LENGTH];
+	int m_stateId;
+};
+
 struct SharedMemoryCommand
 {
 	int m_type;
@@ -974,6 +992,7 @@ struct SharedMemoryCommand
 		struct b3ChangeTextureArgs m_changeTextureArgs;
 		struct b3SearchPathfArgs m_searchPathArgs;
 		struct b3CustomCommand m_customCommandArgs;
+		struct b3StateSerializationArguments m_loadStateArguments;
     };
 };
 
@@ -1048,6 +1067,7 @@ struct SharedMemoryStatus
 		struct b3LoadTextureResultArgs m_loadTextureResultArguments;
 		struct b3CustomCommandResultArgs m_customCommandResultArgs;
 		struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
+		struct b3StateSerializationArguments m_saveStateResultArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -112,12 +112,13 @@ public:
 	{
 		int newargc = argc+2;
 		m_newargv = (char**)malloc(sizeof(void*)*newargc);
-		for (int i=0;i<argc;i++)
+		char* t0 = (char*)"--unused";
-			m_newargv[i] = argv[i];
+		m_newargv[0] = t0;
+
+		for (int i=0;i<argc;i++)
+			m_newargv[i+1] = argv[i];
 		
-		char* t0 = (char*)"--logtostderr";
 		char* t1 = (char*)"--start_demo_name=Physics Server";
-		m_newargv[argc] = t0;
 		m_newargv[argc+1] = t1;
 		m_data = btCreateInProcessExampleBrowser(newargc,m_newargv, useInProcessMemory);
 		SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);

examples/SharedMemory/SharedMemoryPublic.h

@@ -5,7 +5,8 @@
 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
 ///my convention is year/month/day/rev
 
-#define SHARED_MEMORY_MAGIC_NUMBER 201710180
+#define SHARED_MEMORY_MAGIC_NUMBER 201801010
+//#define SHARED_MEMORY_MAGIC_NUMBER 201710180
 //#define SHARED_MEMORY_MAGIC_NUMBER 201710050
 //#define SHARED_MEMORY_MAGIC_NUMBER 201708270
 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140
@@ -77,6 +78,8 @@ enum EnumSharedMemoryClientCommand
 	CMD_SET_ADDITIONAL_SEARCH_PATH,
 	CMD_CUSTOM_COMMAND,
 	CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
+	CMD_SAVE_STATE,
+	CMD_RESTORE_STATE,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -178,7 +181,11 @@ enum EnumSharedMemoryServerStatus
 		CMD_CUSTOM_COMMAND_COMPLETED,
 		CMD_CUSTOM_COMMAND_FAILED,
 		CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED,
-        //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
+		CMD_SAVE_STATE_FAILED,
+		CMD_SAVE_STATE_COMPLETED,
+		CMD_RESTORE_STATE_FAILED,
+		CMD_RESTORE_STATE_COMPLETED,
+		//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
 
@@ -210,6 +217,10 @@ enum JointType {
 	eGearType=6
 };
 
+enum b3RequestDynamicsInfoFlags
+{
+	eDYNAMICS_INFO_REPORT_INERTIA=1,
+};
 
 enum b3JointInfoFlags
 {
@@ -266,7 +277,7 @@ struct b3BodyInfo
 struct b3DynamicsInfo
 {
 	double m_mass;
-	double m_localInertialPosition[3];
+	double m_localInertialDiagonal[3];
 	double m_lateralFrictionCoeff;
 };
 
@@ -574,6 +585,10 @@ enum EnumRenderer
     //ER_FIRE_RAYS=(1<<18),
 };
 
+enum EnumRendererAuxFlags
+{
+	ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX=1,
+};
 ///flags to pick the IK solver and other options
 enum EnumCalculateInverseKinematicsFlags
 {

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -82,6 +82,7 @@ struct TinyRendererVisualShapeConverterInternalData
     float m_lightSpecularCoeff;
     bool m_hasLightSpecularCoeff;
     bool m_hasShadow;
+	int m_flags;
 	SimpleCamera m_camera;
 	
 	
@@ -102,7 +103,8 @@ struct TinyRendererVisualShapeConverterInternalData
     m_hasLightDiffuseCoeff(false),
 	m_lightSpecularCoeff(0.05),
     m_hasLightSpecularCoeff(false),
-	m_hasShadow(false)
+	m_hasShadow(false),
+	m_flags(0)
 	{
 	    m_depthBuffer.resize(m_swWidth*m_swHeight);
         m_shadowBuffer.resize(m_swWidth*m_swHeight);
@@ -154,6 +156,11 @@ void TinyRendererVisualShapeConverter::setShadow(bool hasShadow)
 {
     m_data->m_hasShadow = hasShadow;
 }
+void TinyRendererVisualShapeConverter::setFlags(int flags)
+{
+	m_data->m_flags = flags;
+}
+
 
 void TinyRendererVisualShapeConverter::setLightAmbientCoeff(float ambientCoeff)
 {
@@ -629,7 +636,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 
             if (vertices.size() && indices.size())
             {
-                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId);
+                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex);
 				unsigned char* textureImage1=0;
 				int textureWidth=0;
 				int textureHeight=0;
@@ -1043,7 +1050,17 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
 			}
 			if (segmentationMaskBuffer)
             {
-                segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex];
+				int segMask = m_data->m_segmentationMaskBuffer[i + startPixelIndex];
+				if ((m_data->m_flags & ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)==0)
+				{
+					//if we don't explicitly request link index, clear it out
+					//object index are the lower 24bits
+					if (segMask >= 0)
+					{
+						segMask &= ((1 << 24) - 1);
+					}
+				}
+				segmentationMaskBuffer[i] = segMask;
             }
 			
             if (pixelsRGBA)

examples/SharedMemory/TinyRendererVisualShapeConverter.h

@@ -43,6 +43,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
     void setLightDiffuseCoeff(float diffuseCoeff);
     void setLightSpecularCoeff(float specularCoeff);
     void setShadow(bool hasShadow);
+	void setFlags(int flags);
 
     void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels,  int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
     

examples/TinyRenderer/TinyRenderer.cpp

@@ -199,7 +199,7 @@ m_objectIndex(-1)
     
 }
 
-TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
+TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex, int linkIndex)
 :m_model(0),
 m_rgbColorBuffer(rgbColorBuffer),
 m_depthBuffer(depthBuffer),
@@ -207,7 +207,8 @@ m_shadowBuffer(shadowBuffer),
 m_segmentationMaskBufferPtr(segmentationMaskBuffer),
 m_userData(0),
 m_userIndex(-1),
-m_objectIndex(objectIndex)
+m_objectIndex(objectIndex),
+m_linkIndex(linkIndex)
 {
     Vec3f       eye(1,1,3);
     Vec3f    center(0,0,0);
@@ -561,12 +562,12 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
 			{
 				for (int t=0;t<clippedTriangles.size();t++)
 				{
-					triangleClipped(clippedTriangles[t], shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
+					triangleClipped(clippedTriangles[t], shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex+((renderData.m_linkIndex + 1) << 24));
 				}
 			}
 			else
 			{
-				triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
+				triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex+((renderData.m_linkIndex + 1) << 24));
 			}
         }
 		}

examples/TinyRenderer/TinyRenderer.h

@@ -39,7 +39,7 @@ struct TinyRenderObjectData
     TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer);
     TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
     TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer);
-    TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
+    TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex, int linkIndex);
     virtual ~TinyRenderObjectData();
     
     void loadModel(const char* fileName);
@@ -52,6 +52,7 @@ struct TinyRenderObjectData
     void* m_userData;
     int m_userIndex;
     int m_objectIndex;
+    int m_linkIndex;
 };
 
 

examples/TinyRenderer/our_gl.cpp

@@ -75,7 +75,7 @@ void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &s
     triangleClipped(clipc, orgClipc,shader,image,zbuffer,0,viewPortMatrix,0);
 }
 
-void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) 
+void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectAndLinkIndex)
 {
 
 	mat<3,4,float> screenSpacePts  = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
@@ -135,7 +135,7 @@ void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &s
                 zbuffer[P.x+P.y*image.get_width()] = frag_depth;
                 if (segmentationMaskBuffer)
                 {
-                    segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex;
+                    segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex;
                 }
                 image.set(P.x, P.y, color);
             }
@@ -149,7 +149,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
     triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0);
 }
 
-void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) {
+void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectAndLinkIndex) {
 	mat<3,4,float> pts  = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
     
 	
@@ -189,7 +189,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
                 zbuffer[P.x+P.y*image.get_width()] = frag_depth;
                 if (segmentationMaskBuffer)
                 {
-                    segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex;
+						segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex;
                 }
                 image.set(P.x, P.y, color);
             }

examples/pybullet/examples/quadruped.py

@@ -2,6 +2,44 @@ import pybullet as p
 import time
 import math
 
+
+def drawInertiaBox(parentUid, parentLinkIndex, color):
+	mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA)
+	if (mass>0):
+		Ixx = inertia[0]
+		Iyy = inertia[1]
+		Izz = inertia[2]
+		boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass);
+		boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass);
+		boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass);
+  
+		halfExtents = [boxScaleX,boxScaleY,boxScaleZ]
+		pts = [[halfExtents[0],halfExtents[1],halfExtents[2]],
+				 [-halfExtents[0],halfExtents[1],halfExtents[2]],
+				 [halfExtents[0],-halfExtents[1],halfExtents[2]],
+				 [-halfExtents[0],-halfExtents[1],halfExtents[2]],
+				 [halfExtents[0],halfExtents[1],-halfExtents[2]],
+				 [-halfExtents[0],halfExtents[1],-halfExtents[2]],
+				 [halfExtents[0],-halfExtents[1],-halfExtents[2]],
+				 [-halfExtents[0],-halfExtents[1],-halfExtents[2]]]
+	
+		
+		p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+		p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+		p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+		p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+
+
 toeConstraint = True
 useMaximalCoordinates = False
 useRealTime = 1
@@ -44,7 +82,6 @@ p.setRealTimeSimulation(0)
 quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates)
 nJoints = p.getNumJoints(quadruped)
 
-
 jointNameToId = {}
 for i in range(nJoints):
 	jointInfo = p.getJointInfo(quadruped, i)
@@ -76,6 +113,9 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
 motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
 knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
 
+
+
+
 #fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
 
 motordir=[-1,-1,-1,-1,1,1,1,1]
@@ -83,6 +123,22 @@ halfpi = 1.57079632679
 twopi = 4*halfpi
 kneeangle = -2.1834
 
+mass, friction, localInertiaDiagonal = p.getDynamicsInfo(quadruped,-1, flags=p.DYNAMICS_INFO_REPORT_INERTIA )
+print("localInertiaDiagonal",localInertiaDiagonal)
+
+#this is a no-op, just to show the API
+p.changeDynamics(quadruped,-1,localInertiaDiagonal=localInertiaDiagonal)
+
+#for i in range (nJoints):
+#	p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
+
+
+drawInertiaBox(quadruped,-1, [1,0,0])
+#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
+
+for i in range (nJoints):
+	drawInertiaBox(quadruped,i, [0,1,0])
+
 
 if (useMaximalCoordinates):
 	steps = 400

examples/pybullet/examples/reset_dynamic_info.py

@@ -4,20 +4,63 @@ import math
 
 p.connect(p.GUI)
 planeId = p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
-p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2])
+p.loadURDF(fileName="cube.urdf",basePosition=[0,0,2])
 cubeId = p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
 #p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=0.1)
-p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=100.0)
+p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=1.0)
 p.setGravity(0,0,-10)
 p.setRealTimeSimulation(0)
+
+def drawInertiaBox(parentUid, parentLinkIndex):
+	mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA)
+	Ixx = inertia[0]
+	Iyy = inertia[1]
+	Izz = inertia[2]
+	boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass);
+	boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass);
+	boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass);
+  
+	halfExtents = [boxScaleX,boxScaleY,boxScaleZ]
+	pts = [[halfExtents[0],halfExtents[1],halfExtents[2]],
+				 [-halfExtents[0],halfExtents[1],halfExtents[2]],
+				 [halfExtents[0],-halfExtents[1],halfExtents[2]],
+				 [-halfExtents[0],-halfExtents[1],halfExtents[2]],
+				 [halfExtents[0],halfExtents[1],-halfExtents[2]],
+				 [-halfExtents[0],halfExtents[1],-halfExtents[2]],
+				 [halfExtents[0],-halfExtents[1],-halfExtents[2]],
+				 [-halfExtents[0],-halfExtents[1],-halfExtents[2]]]
+	
+	color=[1,0,0]
+	p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+	p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+	p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
+	
+	
+	
+  
+drawInertiaBox(cubeId,-1)
+
 t=0
 while 1:
 	t=t+1
 	if t > 400:
 		p.changeDynamics(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
-	mass1,frictionCoeff1=p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1)
+	mass1,frictionCoeff1 =p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1)
-	mass2,frictionCoeff2=p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1)
+	mass2,frictionCoeff2 =p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1)
-	print mass1,frictionCoeff1
+	
-	print mass2,frictionCoeff2
+			
-	time.sleep(.01)
+	print (mass1,frictionCoeff1)
+	print (mass2,frictionCoeff2)
+	time.sleep(1./240.)
 	p.stepSimulation()

examples/pybullet/examples/saveRestoreState.py

@@ -0,0 +1,132 @@
+import pybullet as p
+import math, time
+import difflib,sys
+
+numSteps = 500
+numSteps2 = 30
+p.connect(p.GUI, options="--width=1024 --height=768")
+numObjects = 50
+verbose = 0
+
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings.log")
+
+def setupWorld():
+	p.resetSimulation()
+	p.loadURDF("planeMesh.urdf")
+	kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
+	for i in range (p.getNumJoints(kukaId)):
+		p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
+	for i in range (numObjects):
+		cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
+		#p.changeDynamics(cube,-1,mass=100)
+	p.stepSimulation()
+	p.setGravity(0,0,-10)
+
+
+def dumpStateToFile(file):
+	for i in  range (p.getNumBodies()):
+		pos,orn = p.getBasePositionAndOrientation(i)
+		linVel,angVel = p.getBaseVelocity(i)
+		txtPos = "pos="+str(pos)+"\n"
+		txtOrn = "orn="+str(orn)+"\n"
+		txtLinVel = "linVel"+str(linVel)+"\n"
+		txtAngVel = "angVel"+str(angVel)+"\n"
+		file.write(txtPos)
+		file.write(txtOrn)
+		file.write(txtLinVel)
+		file.write(txtAngVel)
+		
+def compareFiles(file1,file2):
+	diff = difflib.unified_diff(
+            file1.readlines(),
+            file2.readlines(),
+            fromfile='saveFile.txt',
+            tofile='restoreFile.txt',
+        )
+	numDifferences = 0
+	for line in diff:
+		numDifferences = numDifferences+1
+		sys.stdout.write(line)
+	if (numDifferences>0):
+		print("Error:", numDifferences, " lines are different between files.")
+	else:
+		print("OK, files are identical")
+	
+setupWorld()
+for i in range (numSteps):
+	p.stepSimulation()	
+p.saveBullet("state.bullet")
+if verbose:
+	p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+	p.setInternalSimFlags(0)
+	print("contact points=")
+	for q in p.getContactPoints():
+		print(q)
+
+for i in range (numSteps2):
+	p.stepSimulation()
+
+
+file = open("saveFile.txt","w") 
+dumpStateToFile(file)
+file.close() 
+
+#################################
+setupWorld()
+
+#both restore from file or from in-memory state should work
+p.restoreState(fileName="state.bullet")
+stateId = p.saveState()
+
+if verbose:
+	p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+	p.setInternalSimFlags(0)
+	print("contact points restored=")
+	for q in p.getContactPoints():
+		print(q)
+for i in range (numSteps2):
+	p.stepSimulation()
+
+	
+file = open("restoreFile.txt","w")
+dumpStateToFile(file)
+file.close() 
+
+p.restoreState(stateId)
+if verbose:
+	p.setInternalSimFlags(1)
+p.stepSimulation()
+if verbose:
+	p.setInternalSimFlags(0)
+	print("contact points restored=")
+	for q in p.getContactPoints():
+		print(q)
+for i in range (numSteps2):
+	p.stepSimulation()
+
+	
+file = open("restoreFile2.txt","w")
+dumpStateToFile(file)
+file.close() 
+
+file1 = open("saveFile.txt","r") 
+file2 = open("restoreFile.txt","r") 
+compareFiles(file1,file2)
+file1.close()
+file2.close()
+
+file1 = open("saveFile.txt","r") 
+file2 = open("restoreFile2.txt","r") 
+compareFiles(file1,file2)
+file1.close()
+file2.close()        
+
+p.stopStateLogging(logId)
+
+#while (p.getConnectionInfo()["isConnected"]):
+#	time.sleep(1)
+

examples/pybullet/examples/segmask_linkindex.py

@@ -0,0 +1,42 @@
+import pybullet as p
+p.connect(p.GUI)
+r2d2 = p.loadURDF("r2d2.urdf",[0,0,1])
+for l in range (p.getNumJoints(r2d2)):
+	print(p.getJointInfo(r2d2,l))
+
+p.loadURDF("r2d2.urdf",[2,0,1])
+p.loadURDF("r2d2.urdf",[4,0,1])
+
+p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
+segLinkIndex=1
+verbose = 0
+
+while (1):
+	keys = p.getKeyboardEvents()
+	#for k in keys:
+	#	print("key=",k,"state=", keys[k])
+	if ord('1') in keys:
+		state = keys[ord('1')]
+		if (state & p.KEY_WAS_RELEASED):
+			verbose = 1-verbose
+	if ord('s') in keys:
+		state = keys[ord('s')]
+		if (state & p.KEY_WAS_RELEASED):
+			segLinkIndex = 1-segLinkIndex
+			#print("segLinkIndex=",segLinkIndex)
+	flags = 0
+	if (segLinkIndex):
+		flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX
+		
+	img = p.getCameraImage(320,200,flags=flags)
+	#print(img[0],img[1])
+	seg=img[4]
+	if (verbose):
+		for pixel in seg:
+			if (pixel>=0):
+				obUid = pixel & ((1<<24)-1)
+				linkIndex = (pixel >> 24)-1
+				print("obUid=",obUid,"linkIndex=",linkIndex)
+	
+	
+	p.stepSimulation()

examples/pybullet/pybullet.c

@@ -15,6 +15,8 @@
 #include <Python.h>
 #endif
 
+#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
+
 #ifdef B3_DUMP_PYTHON_VERSION
 #define B3_VALUE_TO_STRING(x) #x
 #define B3_VALUE(x) B3_VALUE_TO_STRING(x)
@@ -298,7 +300,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 	int freeIndex = -1;
 	int method = eCONNECT_GUI;
 	int i;
-	char* options="";
+	char* options=0;
 
 	b3PhysicsClientHandle sm = 0;
 
@@ -352,12 +354,23 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 			}
 		}
 
+		int argc = 0;
+		char** argv=0;
+		if (options)
+		{
+			int i;
+			argv = urdfStrSplit(options, " ");
+			argc = urdfStrArrayLen(argv);
+			for (i = 0; i < argc; i++)
+			{
+				printf("argv[%d]=%s\n", i, argv[i]);
+			}
+		}
 		switch (method)
 		{
 			case eCONNECT_GUI:
 			{
-				int argc = 2;
+				
-				char* argv[2] = {"unused",options};
 
 #ifdef __APPLE__
 				sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
@@ -368,8 +381,6 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 			}
 			case eCONNECT_GUI_SERVER:
 			{
-				int argc = 2;
-				char* argv[2] = {"unused",options};
 				
 #ifdef __APPLE__
 				sm = b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(argc, argv);
@@ -419,6 +430,11 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 				return NULL;
 			}
 		};
+
+		if (options)
+		{
+			urdfStrArrayFree(argv);
+		}
 	}
 
 	if (sm && b3CanSubmitCommand(sm))
@@ -588,7 +604,7 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args, PyObject* k
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL};
+	static char* kwlist[] = {"bulletFileName",  "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName, &physicsClientId))
 	{
 		return NULL;
@@ -662,6 +678,88 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k
 	return Py_None;
 }
 
+
+static PyObject* pybullet_restoreState(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	const char* fileName = "";
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	int stateId = -1;
+	b3SharedMemoryCommandHandle command;
+
+	PyObject* pylist = 0;
+	b3PhysicsClientHandle sm = 0;
+
+	int physicsClientId = 0;
+	static char* kwlist[] = { "stateId", "fileName", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|isi", kwlist, &stateId, &fileName, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	command = b3LoadStateCommandInit(sm);
+	if (stateId >= 0)
+	{
+		b3LoadStateSetStateId(command, stateId);
+	}
+	if (fileName)
+	{
+		b3LoadStateSetFileName(command, fileName);
+	}
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_RESTORE_STATE_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't restore state.");
+		return NULL;
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+
+}
+
+static PyObject* pybullet_saveState(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+	b3PhysicsClientHandle sm = 0;
+	int stateId = -1;
+
+	int physicsClientId = 0;
+	static char* kwlist[] = { "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	command = b3SaveStateCommandInit(sm);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType != CMD_SAVE_STATE_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't save state");
+		return NULL;
+	}
+
+	stateId = b3GetStatusGetStateId(statusHandle);
+	return PyInt_FromLong(stateId);		
+}
+
 static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	const char* mjcfFileName = "";
@@ -737,12 +835,13 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 	double contactStiffness = -1;
 	double contactDamping = -1;
 	int frictionAnchor = -1;
+	PyObject* localInertiaDiagonalObj=0;
 
 	b3PhysicsClientHandle sm = 0;
 	
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "physicsClientId", NULL};
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddii", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -762,6 +861,12 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
 		{
 			b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass);
 		}
+		if (localInertiaDiagonalObj)
+		{
+			double localInertiaDiagonal[3];
+			pybullet_internalSetVectord(localInertiaDiagonalObj, localInertiaDiagonal);
+			b3ChangeDynamicsInfoSetLocalInertiaDiagonal(command, bodyUniqueId, linkIndex, localInertiaDiagonal);
+		}
 		if (lateralFriction >= 0)
 		{
 			b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction);
@@ -808,11 +913,13 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
 	{
 		int bodyUniqueId = -1;
 		int linkIndex = -2;
+		int flags = 0;
+
 		b3PhysicsClientHandle sm = 0;
 		
 		int physicsClientId = 0;
-		static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
+		static char* kwlist[] = {"bodyUniqueId", "linkIndex", "flags", "physicsClientId", NULL};
-		if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex, &flags, &physicsClientId))
 		{
 			return NULL;
 		}
@@ -827,6 +934,12 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
 			b3SharedMemoryCommandHandle cmd_handle;
 			b3SharedMemoryStatusHandle status_handle;
 			struct b3DynamicsInfo info;
+			int numFields = 2;
+			if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
+			{
+				numFields++;
+			}
+
 			if (bodyUniqueId < 0)
 			{
 				PyErr_SetString(SpamError, "getDynamicsInfo failed; invalid bodyUniqueId");
@@ -846,11 +959,25 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
 				return NULL;
 			}
 			
+			
+			
+
 			if (b3GetDynamicsInfo(status_handle, &info))
 			{
-				PyObject* pyDynamicsInfo = PyTuple_New(2);
+			
+				PyObject* pyDynamicsInfo = PyTuple_New(numFields);
 				PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass));
 				PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff));
+				
+				if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
+				{
+					PyObject* pyInertiaDiag = PyTuple_New(3);
+					PyTuple_SetItem(pyInertiaDiag,0,PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
+					PyTuple_SetItem(pyInertiaDiag,1,PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
+					PyTuple_SetItem(pyInertiaDiag,2,PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
+					PyTuple_SetItem(pyDynamicsInfo, 2, pyInertiaDiag);
+				}
+				
 				return pyDynamicsInfo;
 			}
 		}
@@ -5915,16 +6042,16 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
 	float lightAmbientCoeff = -1;
 	float lightDiffuseCoeff = -1;
 	float lightSpecularCoeff = -1;
-
+	int flags = -1;
 	int renderer = -1;
 	// inialize cmd
 	b3SharedMemoryCommandHandle command;
 	int physicsClientId = 0;
 	b3PhysicsClientHandle sm = 0;
 	// set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow
-	static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "physicsClientId", NULL};
+	static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "flags", "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffiii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &flags, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -5976,6 +6103,10 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
 		b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff);
 	}
 
+	if (flags >= 0)
+	{
+		b3RequestCameraImageSetFlags(command, flags);
+	}
 	if (renderer>=0)
 	{
 		b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL
@@ -7732,11 +7863,17 @@ static PyMethodDef SpamMethods[] = {
 	 "Load multibodies from an SDF file."},
 
 	{"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS,
-	 "Restore the full state of the world from a .bullet file."},
+	 "Load a world from a .bullet file."},
 
 	{"saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS | METH_KEYWORDS,
 	 "Save the full state of the world to a .bullet file."},
 
+	 { "restoreState", (PyCFunction)pybullet_restoreState, METH_VARARGS | METH_KEYWORDS,
+	"Restore the full state of an existing world." },
+
+	{ "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
+	"Save the full state of the world to memory." },
+
 	{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
 	 "Load multibodies from an MJCF file."},
 
@@ -8169,6 +8306,8 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType);  // user read
 	PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType);  // user read
 	
+	PyModule_AddIntConstant(m, "DYNAMICS_INFO_REPORT_INERTIA", eDYNAMICS_INFO_REPORT_INERTIA);  // report local inertia in 'getDynamicsInfo'
+
 	PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType);  // user read
 
 	PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
@@ -8226,6 +8365,7 @@ initpybullet(void)
 
 	PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER);
 	PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL);
+	PyModule_AddIntConstant(m, "ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX", ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX);
 
 	PyModule_AddIntConstant(m, "IK_DLS", IK_DLS);
 	PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS);

setup.py

@@ -410,6 +410,7 @@ elif _platform == "darwin":
     +["examples/OpenGLWindow/MacOpenGLWindowObjC.m"]
 else:
     print("bsd!")
+    libraries = ['GL','GLEW','pthread']
     os.environ['LDFLAGS'] = '-L/usr/X11R6/lib'
     CXX_FLAGS += '-D_BSD '
     CXX_FLAGS += '-I/usr/X11R6/include '
@@ -442,7 +443,7 @@ print("-----")
 
 setup(
 	name = 'pybullet',
-	version='1.7.4',
+	version='1.7.6',
 	description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
 	long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
 	url='https://github.com/bulletphysics/bullet3',

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -16,6 +16,7 @@ subject to the following restrictions:
 
 #include "btCollisionObject.h"
 #include "LinearMath/btSerializer.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 
 btCollisionObject::btCollisionObject()
 	:	m_interpolationLinearVelocity(0.f, 0.f, 0.f),
@@ -114,10 +115,18 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius;
 	dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold;
 	dataOut->m_checkCollideWith = m_checkCollideWith;
-
+	if (m_broadphaseHandle)
-	// Fill padding with zeros to appease msan.
+	{
-	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));
+		dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup;
-
+		dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask;
+		dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId;
+	}
+	else
+	{
+		dataOut->m_collisionFilterGroup = 0;
+		dataOut->m_collisionFilterMask = 0;
+		dataOut->m_uniqueId = -1;
+	}
 	return btCollisionObjectDataName;
 }
 

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -621,7 +621,6 @@ struct	btCollisionObjectDoubleData
 	double					m_hitFraction; 
 	double					m_ccdSweptSphereRadius;
 	double					m_ccdMotionThreshold;
-
 	int						m_hasAnisotropicFriction;
 	int						m_collisionFlags;
 	int						m_islandTag1;
@@ -629,8 +628,9 @@ struct	btCollisionObjectDoubleData
 	int						m_activationState1;
 	int						m_internalType;
 	int						m_checkCollideWith;
-
+	int						m_collisionFilterGroup;
-	char	m_padding[4];
+	int						m_collisionFilterMask;
+	int						m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
@@ -650,13 +650,12 @@ struct	btCollisionObjectFloatData
 	float					m_deactivationTime;
 	float					m_friction;
 	float					m_rollingFriction;
-    float                   m_contactDamping;
+	float                   m_contactDamping;
     float                   m_contactStiffness;
 	float					m_restitution;
 	float					m_hitFraction; 
 	float					m_ccdSweptSphereRadius;
 	float					m_ccdMotionThreshold;
-
 	int						m_hasAnisotropicFriction;
 	int						m_collisionFlags;
 	int						m_islandTag1;
@@ -664,7 +663,9 @@ struct	btCollisionObjectFloatData
 	int						m_activationState1;
 	int						m_internalType;
 	int						m_checkCollideWith;
-	char					m_padding[4];
+	int						m_collisionFilterGroup;
+	int						m_collisionFilterMask;
+	int						m_uniqueId;
 };
 
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -1654,6 +1654,28 @@ void	btCollisionWorld::serializeCollisionObjects(btSerializer* serializer)
 }
 
 
+
+void btCollisionWorld::serializeContactManifolds(btSerializer* serializer)
+{
+	if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		for (int i = 0; i < numManifolds; i++)
+		{
+			const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i];
+			//don't serialize empty manifolds, they just take space 
+			//(may have to do it anyway if it destroys determinism)
+			if (manifold->getNumContacts() == 0)
+				continue;
+
+			btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1);
+			const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold);
+		}
+	}
+}
+
+
 void	btCollisionWorld::serialize(btSerializer* serializer)
 {
 
@@ -1661,6 +1683,8 @@ void	btCollisionWorld::serialize(btSerializer* serializer)
 	
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+	
 	serializer->finishSerialization();
 }
 

src/BulletCollision/CollisionDispatch/btCollisionWorld.h

@@ -107,6 +107,9 @@ protected:
 
 	void	serializeCollisionObjects(btSerializer* serializer);
 
+	void serializeContactManifolds(btSerializer* serializer);
+
+
 public:
 
 	//this constructor doesn't own the dispatcher and paircache/broadphase

src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp

@@ -199,6 +199,22 @@ class btPersistentManifoldSortPredicate
 		}
 };
 
+class btPersistentManifoldSortPredicateDeterministic
+{
+public:
+
+	SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
+	{
+		return (
+			(getIslandId(lhs) < getIslandId(rhs))
+							|| ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) 
+						||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) &&
+					(lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))
+			);
+
+	}
+};
+
 
 void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
 {
@@ -318,6 +334,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
 	for (i=0;i<maxNumManifolds ;i++)
 	{
 		 btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+		 if (manifold->getNumContacts() == 0)
+			 continue;
 
 		 const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
 		 const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
@@ -379,7 +397,13 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
 
 		//tried a radix sort, but quicksort/heapsort seems still faster
 		//@todo rewrite island management
-		m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+
+		//m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+
+		//btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid,
+		//but also based on object0 unique id and object1 unique id
+		m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
+
 		//m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
 
 		//now process all active islands (sets of manifolds for now)

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -16,6 +16,7 @@ subject to the following restrictions:
 
 #include "btPersistentManifold.h"
 #include "LinearMath/btTransform.h"
+#include "LinearMath/btSerializer.h"
 
 
 btScalar					gContactBreakingThreshold = btScalar(0.02);
@@ -33,6 +34,8 @@ btPersistentManifold::btPersistentManifold()
 m_body0(0),
 m_body1(0),
 m_cachedPoints (0),
+m_companionIdA(0),
+m_companionIdB(0),
 m_index1a(0)
 {
 }
@@ -303,6 +306,149 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
 }
 
 
+int	btPersistentManifold::calculateSerializeBufferSize()	const
+{
+	return sizeof(btPersistentManifoldData);
+}
 
+const char*	btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const
+{
+	btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
+	memset(dataOut, 0, sizeof(btPersistentManifoldData));
 
+	dataOut->m_body0 = serializer->getUniquePointer((void*)manifold->getBody0());
+	dataOut->m_body1 = serializer->getUniquePointer((void*)manifold->getBody1());
+	dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
+	dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
+	dataOut->m_numCachedPoints = manifold->getNumContacts();
+	dataOut->m_companionIdA = manifold->m_companionIdA;
+	dataOut->m_companionIdB = manifold->m_companionIdB;
+	dataOut->m_index1a = manifold->m_index1a;
+	dataOut->m_objectType = manifold->m_objectType;
 
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		const btManifoldPoint& pt = manifold->getContactPoint(i);
+		dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
+		dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
+		dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
+		pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]);
+		dataOut->m_pointCacheDistance[i] = pt.m_distance1;
+		dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1;
+		dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1;
+		dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime;
+		dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
+		dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
+		dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
+		dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
+		dataOut->m_pointCacheIndex0[i] = pt.m_index0;
+		dataOut->m_pointCacheIndex1[i] = pt.m_index1;
+		dataOut->m_pointCachePartId0[i] = pt.m_partId0;
+		dataOut->m_pointCachePartId1[i] = pt.m_partId1;
+		pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]);
+		dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction;
+		pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]);
+		dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction;
+		dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction;
+		dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution;
+		dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1;
+		dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2;
+	}
+	return btPersistentManifoldDataName;
+}
+
+void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr)
+{
+	m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
+	m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
+	m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
+	m_companionIdA = manifoldDataPtr->m_companionIdA;
+	m_companionIdB = manifoldDataPtr->m_companionIdB;
+	//m_index1a = manifoldDataPtr->m_index1a;
+	m_objectType = manifoldDataPtr->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		btManifoldPoint& pt = m_pointCache[i];
+		
+		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
+		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
+		pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
+		pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
+		pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
+		pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
+		pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
+		pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
+		pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
+		pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
+		pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
+		pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
+		pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
+		pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
+		pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
+		pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
+		pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
+		pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
+		pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
+		pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
+		pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
+		pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
+		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
+	}
+
+}
+
+void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr)
+{
+	m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold;
+	m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold;
+	m_cachedPoints = manifoldDataPtr->m_numCachedPoints;
+	m_companionIdA = manifoldDataPtr->m_companionIdA;
+	m_companionIdB = manifoldDataPtr->m_companionIdB;
+	//m_index1a = manifoldDataPtr->m_index1a;
+	m_objectType = manifoldDataPtr->m_objectType;
+
+	for (int i = 0; i < this->getNumContacts(); i++)
+	{
+		btManifoldPoint& pt = m_pointCache[i];
+
+		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
+		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
+		pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
+		pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]);
+		pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]);
+		pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i];
+		pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i];
+		pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i];
+		pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i];
+		pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i];
+		pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i];
+		pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i];
+		pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i];
+		pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i];
+		pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i];
+		pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i];
+		pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i];
+		pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]);
+		pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]);
+		pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i];
+		pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]);
+		pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]);
+		pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i];
+		pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i];
+		pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i];
+		pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
+		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
+	}
+
+}

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -95,7 +95,10 @@ public:
 		: btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE),
 	m_body0(body0),m_body1(body1),m_cachedPoints(0),
 		m_contactBreakingThreshold(contactBreakingThreshold),
-		m_contactProcessingThreshold(contactProcessingThreshold)
+		m_contactProcessingThreshold(contactProcessingThreshold),
+		m_companionIdA(0),
+		m_companionIdB(0),
+		m_index1a(0)
 	{
 	}
 
@@ -256,10 +259,115 @@ public:
 		m_cachedPoints = 0;
 	}
 
+	int	calculateSerializeBufferSize()	const;
+	const char*	serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const;
+	void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr);
+	void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr);
 
 
-}
+};
-;
+
+
+
+struct btPersistentManifoldDoubleData
+{
+	btVector3DoubleData m_pointCacheLocalPointA[4];
+	btVector3DoubleData m_pointCacheLocalPointB[4];
+	btVector3DoubleData m_pointCachePositionWorldOnA[4];
+	btVector3DoubleData m_pointCachePositionWorldOnB[4];
+	btVector3DoubleData m_pointCacheNormalWorldOnB[4];
+	btVector3DoubleData	m_pointCacheLateralFrictionDir1[4];
+	btVector3DoubleData	m_pointCacheLateralFrictionDir2[4];
+	double m_pointCacheDistance[4];
+	double m_pointCacheAppliedImpulse[4];
+	double m_pointCacheCombinedFriction[4];
+	double m_pointCacheCombinedRollingFriction[4];
+	double m_pointCacheCombinedSpinningFriction[4];
+	double m_pointCacheCombinedRestitution[4];
+	int	m_pointCachePartId0[4];
+	int	m_pointCachePartId1[4];
+	int	m_pointCacheIndex0[4];
+	int	m_pointCacheIndex1[4];
+	int m_pointCacheContactPointFlags[4];
+	double m_pointCacheAppliedImpulseLateral1[4];
+	double m_pointCacheAppliedImpulseLateral2[4];
+	double m_pointCacheContactMotion1[4];
+	double m_pointCacheContactMotion2[4];
+	double m_pointCacheContactCFM[4];
+	double m_pointCacheCombinedContactStiffness1[4];
+	double m_pointCacheContactERP[4];
+	double m_pointCacheCombinedContactDamping1[4];
+	double m_pointCacheFrictionCFM[4];
+	int m_pointCacheLifeTime[4];
+
+	int m_numCachedPoints;
+	int m_companionIdA;
+	int m_companionIdB;
+	int m_index1a;
+
+	int m_objectType;
+	double	m_contactBreakingThreshold;
+	double	m_contactProcessingThreshold;
+	int m_padding;
+
+	void *m_body0;
+	void *m_body1;
+};
+
+
+struct btPersistentManifoldFloatData
+{
+	btVector3FloatData m_pointCacheLocalPointA[4];
+	btVector3FloatData m_pointCacheLocalPointB[4];
+	btVector3FloatData m_pointCachePositionWorldOnA[4];
+	btVector3FloatData m_pointCachePositionWorldOnB[4];
+	btVector3FloatData m_pointCacheNormalWorldOnB[4];
+	btVector3FloatData	m_pointCacheLateralFrictionDir1[4];
+	btVector3FloatData	m_pointCacheLateralFrictionDir2[4];
+	float m_pointCacheDistance[4];
+	float m_pointCacheAppliedImpulse[4];
+	float m_pointCacheCombinedFriction[4];
+	float m_pointCacheCombinedRollingFriction[4];
+	float m_pointCacheCombinedSpinningFriction[4];
+	float m_pointCacheCombinedRestitution[4];
+	int	m_pointCachePartId0[4];
+	int	m_pointCachePartId1[4];
+	int	m_pointCacheIndex0[4];
+	int	m_pointCacheIndex1[4];
+	int m_pointCacheContactPointFlags[4];
+	float m_pointCacheAppliedImpulseLateral1[4];
+	float m_pointCacheAppliedImpulseLateral2[4];
+	float m_pointCacheContactMotion1[4];
+	float m_pointCacheContactMotion2[4];
+	float m_pointCacheContactCFM[4];
+	float m_pointCacheCombinedContactStiffness1[4];
+	float m_pointCacheContactERP[4];
+	float m_pointCacheCombinedContactDamping1[4];
+	float m_pointCacheFrictionCFM[4];
+	int m_pointCacheLifeTime[4];
+
+	int m_numCachedPoints;
+	int m_companionIdA;
+	int m_companionIdB;
+	int m_index1a;
+
+	int m_objectType;
+	float	m_contactBreakingThreshold;
+	float	m_contactProcessingThreshold;
+	int m_padding;
+
+	void *m_body0;
+	void *m_body1;
+};
+
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btPersistentManifoldData	btPersistentManifoldDoubleData
+#define btPersistentManifoldDataName	"btPersistentManifoldDoubleData"
+#else
+#define btPersistentManifoldData	btPersistentManifoldFloatData
+#define btPersistentManifoldDataName	"btPersistentManifoldFloatData"
+#endif //BT_USE_DOUBLE_PRECISION
+
 
 
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -1533,6 +1533,8 @@ void	btDiscreteDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeRigidBodies(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -1994,7 +1994,11 @@ int	btMultiBody::calculateSerializeBufferSize()	const
 const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
 {
 		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
-		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
+		getBasePos().serialize(mbd->m_baseWorldPosition);
+		getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
+		getBaseVel().serialize(mbd->m_baseLinearVelocity);
+		getBaseOmega().serialize(mbd->m_baseAngularVelocity);
+
 		mbd->m_baseMass = this->getBaseMass();
 		getBaseInertia().serialize(mbd->m_baseInertia);
 		{

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -702,13 +702,13 @@ private:
 	int	m_companionId;
 	btScalar	m_linearDamping;
 	btScalar	m_angularDamping;
-	bool	m_useGyroTerm;
+	bool		m_useGyroTerm;
 	btScalar	m_maxAppliedImpulse;
 	btScalar	m_maxCoordinateVelocity;
 	bool		m_hasSelfCollision;
 	
-		bool __posUpdated;
+	bool __posUpdated;
-		int m_dofCount, m_posVarCnt;
+	int m_dofCount, m_posVarCnt;
 	bool m_useRK4, m_useGlobalVelocities;
 	
 	///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
@@ -784,29 +784,38 @@ struct btMultiBodyLinkFloatData
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyDoubleData
 {
-	btTransformDoubleData m_baseWorldTransform;
+	btVector3DoubleData m_baseWorldPosition;
+	btQuaternionDoubleData m_baseWorldOrientation;
+	btVector3DoubleData m_baseLinearVelocity;
+	btVector3DoubleData m_baseAngularVelocity;
 	btVector3DoubleData m_baseInertia;   // inertia of the base (in local frame; diagonal)
 	double	m_baseMass;
+	int		m_numLinks;
+	char	m_padding[4];
 
 	char	*m_baseName;
 	btMultiBodyLinkDoubleData	*m_links;
 	btCollisionObjectDoubleData	*m_baseCollider;
-	char	*m_paddingPtr;
+	
-	int		m_numLinks;
+	
-	char	m_padding[4];
 };
 
 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 struct	btMultiBodyFloatData
 {
+	btVector3FloatData m_baseWorldPosition;
+	btQuaternionFloatData m_baseWorldOrientation;
+	btVector3FloatData m_baseLinearVelocity;
+	btVector3FloatData m_baseAngularVelocity;
+
+	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
+	float	m_baseMass;
+	int		m_numLinks;
+
 	char	*m_baseName;
 	btMultiBodyLinkFloatData	*m_links;
 	btCollisionObjectFloatData	*m_baseCollider;
-	btTransformFloatData m_baseWorldTransform;
-	btVector3FloatData m_baseInertia;   // inertia of the base (in local frame; diagonal)
 
-	float	m_baseMass;
-	int		m_numLinks;
 };
 
 

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -114,7 +114,14 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 					frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
 					frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse);
 					frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse;
-					leastSquaredResidual += resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
+					btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
+					leastSquaredResidual += residual*residual;
+					
+					if (frictionConstraintB.m_multiBodyA)
+						frictionConstraintB.m_multiBodyA->setPosUpdated(false);
+					if (frictionConstraintB.m_multiBodyB)
+						frictionConstraintB.m_multiBodyB->setPosUpdated(false);
+				
 					if (frictionConstraint.m_multiBodyA)
 						frictionConstraint.m_multiBodyA->setPosUpdated(false);
 					if (frictionConstraint.m_multiBodyB)

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -972,6 +972,8 @@ void	btMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 

src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp

@@ -361,6 +361,8 @@ void	btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
 
 	serializeCollisionObjects(serializer);
 
+	serializeContactManifolds(serializer);
+
 	serializer->finishSerialization();
 }
 

src/LinearMath/btQuaternion.h

@@ -602,7 +602,9 @@ public:
 
 	SIMD_FORCE_INLINE	void	serialize(struct	btQuaternionData& dataOut) const;
 
-	SIMD_FORCE_INLINE	void	deSerialize(const struct	btQuaternionData& dataIn);
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btQuaternionFloatData& dataIn);
+
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btQuaternionDoubleData& dataIn);
 
 	SIMD_FORCE_INLINE	void	serializeFloat(struct	btQuaternionFloatData& dataOut) const;
 
@@ -1003,10 +1005,16 @@ SIMD_FORCE_INLINE	void	btQuaternion::serialize(struct	btQuaternionData& dataOut)
 		dataOut.m_floats[i] = m_floats[i];
 }
 
-SIMD_FORCE_INLINE void	btQuaternion::deSerialize(const struct	btQuaternionData& dataIn)
+SIMD_FORCE_INLINE void	btQuaternion::deSerialize(const struct	btQuaternionFloatData& dataIn)
+{
+	for (int i = 0; i<4; i++)
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
+}
+
+SIMD_FORCE_INLINE void	btQuaternion::deSerialize(const struct	btQuaternionDoubleData& dataIn)
 {
 	for (int i=0;i<4;i++)
-		m_floats[i] = dataIn.m_floats[i];
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
 }
 
 

src/LinearMath/btSerializer.h

@@ -62,7 +62,8 @@ enum	btSerializationFlags
 {
 	BT_SERIALIZE_NO_BVH = 1,
 	BT_SERIALIZE_NO_TRIANGLEINFOMAP = 2,
-	BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4
+	BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4,
+	BT_SERIALIZE_CONTACT_MANIFOLDS = 8,
 };
 
 class	btSerializer
@@ -128,9 +129,9 @@ public:
 #define BT_SBMATERIAL_CODE		BT_MAKE_ID('S','B','M','T')
 #define BT_SBNODE_CODE			BT_MAKE_ID('S','B','N','D')
 #define BT_DYNAMICSWORLD_CODE	BT_MAKE_ID('D','W','L','D')
+#define BT_CONTACTMANIFOLD_CODE BT_MAKE_ID('C','O','N','T')
 #define BT_DNA_CODE				BT_MAKE_ID('D','N','A','1')
 
-
 struct	btPointerUid
 {
 	union

src/LinearMath/btVector3.h

@@ -705,7 +705,9 @@ public:
 
 	SIMD_FORCE_INLINE	void	serialize(struct	btVector3Data& dataOut) const;
 
-	SIMD_FORCE_INLINE	void	deSerialize(const struct	btVector3Data& dataIn);
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btVector3DoubleData& dataIn);
+
+	SIMD_FORCE_INLINE	void	deSerialize(const struct	btVector3FloatData& dataIn);
 
 	SIMD_FORCE_INLINE	void	serializeFloat(struct	btVector3FloatData& dataOut) const;
 
@@ -1354,10 +1356,18 @@ SIMD_FORCE_INLINE	void	btVector3::serialize(struct	btVector3Data& dataOut) const
 		dataOut.m_floats[i] = m_floats[i];
 }
 
-SIMD_FORCE_INLINE void	btVector3::deSerialize(const struct	btVector3Data& dataIn)
+
+SIMD_FORCE_INLINE void	btVector3::deSerialize(const struct	btVector3FloatData& dataIn)
+{
+	for (int i = 0; i<4; i++)
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
+}
+
+
+SIMD_FORCE_INLINE void	btVector3::deSerialize(const struct	btVector3DoubleData& dataIn)
 {
 	for (int i=0;i<4;i++)
-		m_floats[i] = dataIn.m_floats[i];
+		m_floats[i] = (btScalar)dataIn.m_floats[i];
 }
 
 #endif //BT_VECTOR3_H