Merge branch 'master' into master

Extras/Serialize/BulletFileLoader/autogenerated/bullet.h

@@ -556,8 +556,8 @@ typedef struct bInvalidHandle {
         double m_contactBreakingThreshold;
         double m_contactProcessingThreshold;
         int m_padding;
-        void *m_body0;
+        btCollisionObjectDoubleData *m_body0;
-        void *m_body1;
+        btCollisionObjectDoubleData *m_body1;
     };
 
 
@@ -570,8 +570,8 @@ typedef struct bInvalidHandle {
         btVector3FloatData m_pointCachePositionWorldOnA[4];
         btVector3FloatData m_pointCachePositionWorldOnB[4];
         btVector3FloatData m_pointCacheNormalWorldOnB[4];
-        btVector3FloatData m_pointCacheLateralFrictionDir1;
+        btVector3FloatData m_pointCacheLateralFrictionDir1[4];
-        btVector3FloatData m_pointCacheLateralFrictionDir2;
+        btVector3FloatData m_pointCacheLateralFrictionDir2[4];
         float m_pointCacheDistance[4];
         float m_pointCacheAppliedImpulse[4];
         float m_pointCacheCombinedFriction[4];
@@ -601,8 +601,8 @@ typedef struct bInvalidHandle {
         float m_contactBreakingThreshold;
         float m_contactProcessingThreshold;
         int m_padding;
-        void *m_body0;
+        btCollisionObjectFloatData *m_body0;
-        void *m_body1;
+        btCollisionObjectFloatData *m_body1;
     };
 
 
@@ -1451,7 +1451,7 @@ typedef struct bInvalidHandle {
     {
     public:
         btQuaternionDoubleData m_zeroRotParentToThis;
-        btVector3DoubleData m_parentComToThisComOffset;
+        btVector3DoubleData m_parentComToThisPivotOffset;
         btVector3DoubleData m_thisPivotToThisComOffset;
         btVector3DoubleData m_jointAxisTop[6];
         btVector3DoubleData m_jointAxisBottom[6];
@@ -1482,7 +1482,7 @@ typedef struct bInvalidHandle {
     {
     public:
         btQuaternionFloatData m_zeroRotParentToThis;
-        btVector3FloatData m_parentComToThisComOffset;
+        btVector3FloatData m_parentComToThisPivotOffset;
         btVector3FloatData m_thisPivotToThisComOffset;
         btVector3FloatData m_jointAxisTop[6];
         btVector3FloatData m_jointAxisBottom[6];
@@ -1548,7 +1548,7 @@ typedef struct bInvalidHandle {
     {
     public:
         btCollisionObjectFloatData m_colObjData;
-        void *m_multiBody;
+        btMultiBodyFloatData *m_multiBody;
         int m_link;
         char m_padding[4];
     };
@@ -1559,7 +1559,7 @@ typedef struct bInvalidHandle {
     {
     public:
         btCollisionObjectDoubleData m_colObjData;
-        void *m_multiBody;
+        btMultiBodyDoubleData *m_multiBody;
         int m_link;
         char m_padding[4];
     };

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -194,7 +194,7 @@ template<class T>  void convertMultiBody(T* mbd, btMultiBodyWorldImporterInterna
 		btQuaternion parentRotToThis;
 		parentRotToThis.deSerialize(mbd->m_links[i].m_zeroRotParentToThis);
 		btVector3 parentComToThisPivotOffset;
-		parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisComOffset);
+		parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisPivotOffset);
 		btVector3 thisPivotToThisComOffset;
 		thisPivotToThisComOffset.deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset);
 
@@ -333,7 +333,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
 					if (ptr)
 					{
-						manifoldData->m_body0 = ptr;
+						manifoldData->m_body0 = (btCollisionObjectDoubleData*)ptr;
 					}
 				}
 
@@ -341,7 +341,7 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
 					if (ptr)
 					{
-						manifoldData->m_body1 = ptr;
+						manifoldData->m_body1 = (btCollisionObjectDoubleData*)ptr;
 					}
 				}
 			}
@@ -371,14 +371,14 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
 					if (ptr)
 					{
-						manifoldData->m_body0 = ptr;
+						manifoldData->m_body0 = (btCollisionObjectFloatData*)ptr;
 					}
 				}
 				{
 					void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
 					if (ptr)
 					{
-						manifoldData->m_body1 = ptr;
+						manifoldData->m_body1 = (btCollisionObjectFloatData*)ptr;
 					}
 				}
 			}

data/plane_transparent.urdf

@@ -12,10 +12,10 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-				<mesh filename="plane_transparent.obj" scale="1 1 1"/>
+				<mesh filename="plane.obj" scale="1 1 1"/>
       </geometry>
        <material name="white">
-        <color rgba="1 1 1 0.4"/>
+        <color rgba="1 1 1 .9"/>
       </material>
     </visual>
     <collision>

examples/CommonInterfaces/CommonRenderInterface.h

@@ -15,6 +15,7 @@ enum
 	//B3_WIREFRAME_RENDERMODE,
 	B3_CREATE_SHADOWMAP_RENDERMODE,
 	B3_USE_SHADOWMAP_RENDERMODE,
+	B3_USE_SHADOWMAP_RENDERMODE_REFLECTION,
 };
 
 

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -251,7 +251,7 @@ 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
 		const char* inertiaFromGeom = root_xml->Attribute("inertiafromgeom");
-		if(inertiaFromGeom[0] == 'f')  // false, other values assumed `true`.
+		if(inertiaFromGeom && inertiaFromGeom[0] == 'f')  // false, other values assumed `true`.
 		{
 			m_inertiaFromGeom = false;
 		}

examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp

@@ -119,9 +119,10 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gMCFJFileNameArray.size()==0)
 		{
+			gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
+
 			gMCFJFileNameArray.push_back("MPL/MPL.xml");
 
-			gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
 			gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
 			gMCFJFileNameArray.push_back("mjcf/ant.xml");
 			gMCFJFileNameArray.push_back("mjcf/hello_mjcf.xml");
@@ -198,7 +199,7 @@ void ImportMJCFSetup::initPhysics()
 	//@todo also use the modified collision filter for raycast and other collision related queries
 	m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
 	
-	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
+	//m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 	m_dynamicsWorld->getDebugDrawer()->setDebugMode(
 		btIDebugDraw::DBG_DrawConstraints
@@ -255,6 +256,8 @@ void ImportMJCFSetup::initPhysics()
 				s->registerNameForPointer(name->c_str(),name->c_str());
 #endif//TEST_MULTIBODY_SERIALIZATION
 				mb->setBaseName(name->c_str());
+				mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
+
 				//create motors for each btMultiBody joint
 				int numLinks = mb->getNumLinks();
 				for (int i=0;i<numLinks;i++)
@@ -270,10 +273,11 @@ void ImportMJCFSetup::initPhysics()
 #endif//TEST_MULTIBODY_SERIALIZATION
 					m_nameMemory.push_back(jointName);
 					m_nameMemory.push_back(linkName);
+					mb->getLinkCollider(i)->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
 
 					mb->getLink(i).m_linkName = linkName->c_str();
 					mb->getLink(i).m_jointName = jointName->c_str();
-							
+					m_data->m_mb = mb;
 					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
 					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
 					)
@@ -363,7 +367,16 @@ void ImportMJCFSetup::stepSimulation(float deltaTime)
         {
 			if (m_data->m_jointMotors[i])
 			{
-				m_data->m_jointMotors[i]->setPositionTarget(m_data->m_motorTargetPositions[i]);
+				btScalar pos = m_data->m_motorTargetPositions[i];
+
+				int link = m_data->m_jointMotors[i]->getLinkA();
+				btScalar lowerLimit = m_data->m_mb->getLink(link).m_jointLowerLimit;
+				btScalar upperLimit = m_data->m_mb->getLink(link).m_jointUpperLimit;
+				if (lowerLimit < upperLimit)
+				{
+					btClamp(pos, lowerLimit, upperLimit);
+				}
+				m_data->m_jointMotors[i]->setPositionTarget(pos);
 			}
 			if (m_data->m_generic6DofJointMotors[i])
 			{

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -49,7 +49,8 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
 	btHashMap<btHashInt,UrdfMaterialColor> m_linkColors;
     btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
 	mutable btAlignedObjectArray<btTriangleMesh*> m_allocatedMeshInterfaces;
-	
+	btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
+
 	LinkVisualShapesConverter* m_customVisualShapesConverter;
 	bool m_enableTinyRenderer;
 
@@ -569,6 +570,17 @@ bool findExistingMeshFile(
 	}
 }
 
+int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const
+{
+	UrdfCollision* col = m_data->m_bulletCollisionShape2UrdfCollision.find(collisionShape);
+	if (col)
+	{
+		collision = *col;
+		return 1;
+	}
+	return 0;
+}
+
 btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) const
 {
 	BT_PROFILE("convertURDFToCollisionShape");
@@ -599,8 +611,8 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
         case URDF_GEOM_CYLINDER:
         {
 			btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
-			btScalar cylLength = collision->m_geometry.m_capsuleHeight;
+			btScalar cylHalfLength = 0.5*collision->m_geometry.m_capsuleHeight;
-			
+#if 0
             btAlignedObjectArray<btVector3> vertices;
             //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
             int numSteps = 32;
@@ -620,10 +632,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
 			cylZShape->optimizeConvexHull();
 			
 			//btConvexShape* cylZShape = new btConeShapeZ(cylRadius,cylLength);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-            
+#else       
-            //btVector3 halfExtents(cylRadius,cylRadius,cylLength);
+            btVector3 halfExtents(cylRadius,cylRadius, cylHalfLength);
-            //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+            btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
-            
+#endif       
 
             shape = cylZShape;
             break;
@@ -671,6 +683,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
 				//create a convex hull for each shape, and store it in a btCompoundShape
 
 				shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
+				m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
 				return shape;
 			}
 			break;
@@ -812,6 +825,10 @@ upAxisMat.setIdentity();
 		b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
 		
 	}
+	if (shape && collision->m_geometry.m_type==URDF_GEOM_MESH)
+	{
+		m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
+	}
 	return shape;
 }
 

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -72,6 +72,8 @@ public:
 
 	class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const;
 
+	virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const;
+
     ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
     
 	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -75,6 +75,10 @@ public:
 
    //default implementation for backward compatibility 
 	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const  = 0;
+	virtual int getUrdfFromCollisionShape(const class btCollisionShape* collisionShape, struct UrdfCollision& collision) const
+	{
+		return 0;
+	}
 
 	virtual int getNumAllocatedCollisionShapes() const { return 0;}
     virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/ ) {return 0;}

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -1527,6 +1527,7 @@ void GLInstancingRenderer::renderScene()
 
 		renderSceneInternal(B3_CREATE_SHADOWMAP_RENDERMODE);
 		//glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
+		//renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE_REFLECTION);
 		renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
 
 	} else
@@ -1960,8 +1961,15 @@ struct TransparentDistanceSortPredicate
 	}
 };
 
-void GLInstancingRenderer::renderSceneInternal(int renderMode)
+void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
 {
+	int renderMode=orgRenderMode;
+	bool reflectionPass = false;
+	if (orgRenderMode==B3_USE_SHADOWMAP_RENDERMODE_REFLECTION)
+	{
+		reflectionPass = true;
+		renderMode = B3_USE_SHADOWMAP_RENDERMODE;
+	}
 
 	if (!useShadowMap)
 	{
@@ -2385,9 +2393,10 @@ b3Assert(glGetError() ==GL_NO_ERROR);
                             
 								glUseProgram(useShadowMapInstancingShader);
 								glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
-								glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
+								//glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
-								glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]);
+								//glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrix[0]);
-								glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
+								//glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]);
+								//glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
 
 								glUniform3f(useShadow_lightSpecularIntensity, m_data->m_lightSpecularIntensity[0],m_data->m_lightSpecularIntensity[1],m_data->m_lightSpecularIntensity[2]);
 								glUniform3f(useShadow_materialSpecularColor, gfxObj->m_materialSpecularColor[0],gfxObj->m_materialSpecularColor[1],gfxObj->m_materialSpecularColor[2]);
@@ -2395,7 +2404,22 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 							
 
 								float MVP[16];
-								b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
+								if (reflectionPass)
+								{
+									float tmp[16];
+									float reflectionMatrix[16] = {1,0,0,0,
+										0,1,0,0,
+										0,0,-1,0,
+										0,0,0,1};
+									glCullFace(GL_FRONT);
+									b3Matrix4x4Mul16(m_data->m_viewMatrix,reflectionMatrix,tmp);
+									b3Matrix4x4Mul16(m_data->m_projectionMatrix,tmp,MVP);
+								} else
+								{
+									b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
+									glCullFace(GL_BACK);
+								}
+								
 								glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]);
 								//gLightDir.normalize();
 								glUniform3f(useShadow_lightPosIn,m_data->m_lightPos[0],m_data->m_lightPos[1],m_data->m_lightPos[2]);

examples/OpenGLWindow/GLInstancingRenderer.h

@@ -52,7 +52,7 @@ public:
 	virtual void init();
 
 	virtual void renderScene();
-	virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
+	virtual void renderSceneInternal(int orgRenderMode=B3_DEFAULT_RENDERMODE);
 
 	void InitShaders();
 	void CleanupShaders();

examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl

@@ -11,8 +11,6 @@ layout (location = 5) in vec4 instance_color;
 layout (location = 6) in vec3 instance_scale;
 
 
-uniform mat4 ModelViewMatrix;
-uniform mat4 ProjectionMatrix;
 uniform mat4 DepthBiasModelViewProjectionMatrix;
 uniform mat4 MVP;
 uniform vec3 lightPosIn;

examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h

@@ -9,8 +9,6 @@ static const char* useShadowMapInstancingVertexShader= \
 "layout (location = 4) in vec3 vertexnormal;\n"
 "layout (location = 5) in vec4 instance_color;\n"
 "layout (location = 6) in vec3 instance_scale;\n"
-"uniform mat4 ModelViewMatrix;\n"
-"uniform mat4 ProjectionMatrix;\n"
 "uniform mat4 DepthBiasModelViewProjectionMatrix;\n"
 "uniform mat4 MVP;\n"
 "uniform vec3 lightPosIn;\n"

examples/SharedMemory/BodyJointInfoUtility.h

@@ -67,6 +67,32 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
 			info.m_jointUpperLimit = mb->m_links[link].m_jointUpperLimit;
 			info.m_jointMaxForce = mb->m_links[link].m_jointMaxForce;
 			info.m_jointMaxVelocity = mb->m_links[link].m_jointMaxVelocity;
+			
+			info.m_parentFrame[0] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[0];
+			info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[1];
+			info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[2];
+			info.m_parentFrame[3] = mb->m_links[link].m_zeroRotParentToThis.m_floats[0];
+			info.m_parentFrame[4] = mb->m_links[link].m_zeroRotParentToThis.m_floats[1];
+			info.m_parentFrame[5] = mb->m_links[link].m_zeroRotParentToThis.m_floats[2];
+			info.m_parentFrame[6] = mb->m_links[link].m_zeroRotParentToThis.m_floats[3];
+
+			info.m_jointAxis[0] = 0;
+			info.m_jointAxis[1] = 0;
+			info.m_jointAxis[2] = 0;
+			info.m_parentIndex = mb->m_links[link].m_parentIndex;
+
+			if (info.m_jointType == eRevoluteType)
+			{
+				info.m_jointAxis[0] = mb->m_links[link].m_jointAxisTop[0].m_floats[0];
+				info.m_jointAxis[1] = mb->m_links[link].m_jointAxisTop[0].m_floats[1];
+				info.m_jointAxis[2] = mb->m_links[link].m_jointAxisTop[0].m_floats[2];
+			}
+			if (info.m_jointType == ePrismaticType)
+			{
+				info.m_jointAxis[0] = mb->m_links[link].m_jointAxisBottom[0].m_floats[0];
+				info.m_jointAxis[1] = mb->m_links[link].m_jointAxisBottom[0].m_floats[1];
+				info.m_jointAxis[2] = mb->m_links[link].m_jointAxisBottom[0].m_floats[2];
+			}
 
 			if ((mb->m_links[link].m_jointType == eRevoluteType) ||
 				(mb->m_links[link].m_jointType == ePrismaticType)) {

examples/SharedMemory/PhysicsClient.h

@@ -58,6 +58,8 @@ public:
 
 	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
 
+	virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) = 0;
+
 	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData) = 0;
 
 	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) = 0;

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2154,13 +2154,12 @@ B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, str
 	if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
 		return false;
 
-	info->m_mass = dynamicsInfo.m_mass;
+	if (info)
-	info->m_localInertialDiagonal[0] = dynamicsInfo.m_localInertialDiagonal[0];
+	{
-	info->m_localInertialDiagonal[1] = dynamicsInfo.m_localInertialDiagonal[1];
+		*info = dynamicsInfo;
-	info->m_localInertialDiagonal[2] = dynamicsInfo.m_localInertialDiagonal[2];
+		return true;
-
+	}
-	info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff;
+	return false;
-	return true;
 }
 
 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)
@@ -3431,6 +3430,30 @@ B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient
 }
 
 
+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+	command->m_type = CMD_REQUEST_COLLISION_SHAPE_INFO;
+	command->m_requestCollisionShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
+	command->m_requestCollisionShapeDataArguments.m_linkIndex = linkIndex;
+
+	command->m_updateFlags = 0;
+	return (b3SharedMemoryCommandHandle)command;
+}
+B3_SHARED_API	void b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient, struct b3CollisionShapeInformation* collisionShapeInfo)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	if (cl)
+	{
+		cl->getCachedCollisionShapeInformation(collisionShapeInfo);
+	}
+}
+
+
 
 //request visual shape information
 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA)

examples/SharedMemory/PhysicsClientC_API.h

@@ -259,6 +259,11 @@ B3_SHARED_API	void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, str
 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
 B3_SHARED_API	void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
 
+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
+B3_SHARED_API	void b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient, struct b3CollisionShapeInformation* collisionShapeInfo);
+
+
+
 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
 B3_SHARED_API	int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle);
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -44,6 +44,8 @@ struct PhysicsClientSharedMemoryInternalData {
     btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
 	btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
 	btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
+	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
+
 	btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
 	btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
 	btAlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
@@ -1253,6 +1255,27 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 			{
 				break;
 			}
+			case CMD_COLLISION_SHAPE_INFO_FAILED:
+			{
+				b3Warning("getCollisionShapeData failed");
+				break;
+			}
+			case CMD_COLLISION_SHAPE_INFO_COMPLETED:
+			{
+				B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED");
+				if (m_data->m_verboseOutput)
+				{
+					b3Printf("Collision Shape Information Request OK\n");
+				}
+				int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes;
+				m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied);
+				b3CollisionShapeData* shapeData = (b3CollisionShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
+				for (int i = 0; i < numCollisionShapesCopied; i++)
+				{
+					m_data->m_cachedCollisionShapes[i] = shapeData[i];
+				}
+				break;
+			}
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);
@@ -1595,6 +1618,13 @@ void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualS
 	visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
 }
 
+void PhysicsClientSharedMemory::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
+{
+	collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size();
+	collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes? &m_data->m_cachedCollisionShapes[0] : 0;
+}
+
+
 
 const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
     if (m_data->m_debugLinesFrom.size()) {

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -68,6 +68,8 @@ public:
 
 	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
 
+	virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
+
 	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
 
 	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

examples/SharedMemory/PhysicsDirect.cpp

@@ -54,6 +54,8 @@ struct PhysicsDirectInternalData
 	btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
 
 	btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
+	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
+
     btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
 
 	btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
@@ -1011,6 +1013,27 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 	{
 		break;
 	}
+	case CMD_COLLISION_SHAPE_INFO_FAILED:
+	{
+		b3Warning("getCollisionShapeData failed");
+		break;
+	}
+	case CMD_COLLISION_SHAPE_INFO_COMPLETED:
+	{
+		B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED");
+		if (m_data->m_verboseOutput)
+		{
+			b3Printf("Collision Shape Information Request OK\n");
+		}
+		int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes;
+		m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied);
+		b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0];
+		for (int i = 0; i < numCollisionShapesCopied; i++)
+		{
+			m_data->m_cachedCollisionShapes[i] = shapeData[i];
+		}
+		break;
+	}
 	case CMD_RESTORE_STATE_FAILED:
 	{
 		b3Warning("restoreState failed");
@@ -1242,6 +1265,14 @@ void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformat
 	visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
 }
 
+void PhysicsDirect::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
+{
+	collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size();
+	collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0;
+}
+
+
+
 void PhysicsDirect::getCachedVREvents(struct b3VREventsData* vrEventsData)
 {
 	vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();

examples/SharedMemory/PhysicsDirect.h

@@ -91,6 +91,8 @@ public:
 
 	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
 	
+	virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
+
 	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
 
 	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -180,6 +180,12 @@ void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInform
 	return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
 }
 
+void PhysicsLoopBack::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
+{
+	return m_data->m_physicsClient->getCachedCollisionShapeInformation(collisionShapesInfo);
+}
+
+
 void PhysicsLoopBack::getCachedVREvents(struct b3VREventsData* vrEventsData)
 {
 	return m_data->m_physicsClient->getCachedVREvents(vrEventsData);

examples/SharedMemory/PhysicsLoopBack.h

@@ -72,6 +72,8 @@ public:
 
 	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
 
+	virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
+
 	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
 
 	virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -29,6 +29,7 @@
 #include "LinearMath/btTransform.h"
 #include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h"
 #include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
+#include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
 #include "../Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h"
 #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
 #include "LinearMath/btSerializer.h"
@@ -1516,6 +1517,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btAlignedObjectArray<std::string*> m_strings;
 
 	btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;
+	btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
 	btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
 
 	MyOverlapFilterCallback* m_broadphaseCollisionFilterCallback;
@@ -2138,11 +2140,25 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 		if (colShapeUniqueId>=0)
 		{
 			InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
-			if (handle)
+			if (handle && handle->m_collisionShape)
 			{
-				btTransform childTrans;
+				if (handle->m_collisionShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
-				childTrans.setIdentity();				
+				{
-				compound->addChildShape(localInertiaFrame.inverse()*childTrans,handle->m_collisionShape);
+					btCompoundShape* childCompound = (btCompoundShape*)handle->m_collisionShape;
+					for (int c = 0; c < childCompound->getNumChildShapes(); c++)
+					{
+						btTransform childTrans = childCompound->getChildTransform(c);
+						btCollisionShape* childShape = childCompound->getChildShape(c);
+						btTransform tr = localInertiaFrame.inverse()*childTrans;
+						compound->addChildShape(tr, childShape);
+					}
+				}
+				else
+				{
+					btTransform childTrans;
+					childTrans.setIdentity();
+					compound->addChildShape(localInertiaFrame.inverse()*childTrans, handle->m_collisionShape);
+				}
 			}
 		}
 		m_allocatedCollisionShapes.push_back(compound);
@@ -2392,6 +2408,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 	}
 	m_data->m_meshInterfaces.clear();
 	m_data->m_collisionShapes.clear();
+	m_data->m_bulletCollisionShape2UrdfCollision.clear();
 
 	delete m_data->m_dynamicsWorld;
 	m_data->m_dynamicsWorld=0;
@@ -2634,6 +2651,25 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
     {
         btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
         m_data->m_collisionShapes.push_back(shape);
+		UrdfCollision  urdfCollision;
+		if (u2b.getUrdfFromCollisionShape(shape, urdfCollision))
+		{
+			m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, urdfCollision);
+		}
+		if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+		{
+			btCompoundShape* compound = (btCompoundShape*)shape;
+			for (int c = 0; c < compound->getNumChildShapes(); c++)
+			{
+				btCollisionShape* childShape = compound->getChildShape(c);
+				if (u2b.getUrdfFromCollisionShape(childShape, urdfCollision))
+				{
+					m_data->m_bulletCollisionShape2UrdfCollision.insert(childShape, urdfCollision);
+				}
+			}
+		}
+
+
     }
 
 	m_data->m_saveWorldBodyData.push_back(sd);
@@ -3488,7 +3524,8 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 {
 	bool hasStatus = true;
 	serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
-					
+	btScalar defaultCollisionMargin = 0.001;
+
 	btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
 
 	btCollisionShape* shape = 0;
@@ -3500,8 +3537,11 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 	{
 		compound = worldImporter->createCompoundShape();
 	}
-	for (int i=0;i<clientCmd.m_createUserShapeArgs.m_numUserShapes;i++)
+	for (int i = 0; i < clientCmd.m_createUserShapeArgs.m_numUserShapes; i++)
 	{
+		GLInstanceGraphicsShape* glmesh = 0;
+		char pathPrefix[1024] = "";
+		char relativeFileName[1024] = "";
 		UrdfCollision urdfColObj;
 
 		btTransform childTransform;
@@ -3517,7 +3557,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 				clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[2],
 				clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[3]
 			));
-			if (compound==0)
+			if (compound == 0)
 			{
 				compound = worldImporter->createCompoundShape();
 			}
@@ -3530,227 +3570,274 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 
 		switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type)
 		{
-			case GEOM_SPHERE:
+		case GEOM_SPHERE:
+		{
+			double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
+			shape = worldImporter->createSphereShape(radius);
+			if (compound)
 			{
-				double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
+				compound->addChildShape(childTransform, shape);
-				shape = worldImporter->createSphereShape(radius);
-				if (compound)
-				{
-					compound->addChildShape(childTransform,shape);
-				}
-				urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
-				urdfColObj.m_geometry.m_sphereRadius = radius;
-				break;
 			}
-			case GEOM_BOX:
+			urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
+			urdfColObj.m_geometry.m_sphereRadius = radius;
+			break;
+		}
+		case GEOM_BOX:
+		{
+			//double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
+			btVector3 halfExtents(
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0],
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1],
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
+			shape = worldImporter->createBoxShape(halfExtents);
+			if (compound)
 			{
-				//double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
+				compound->addChildShape(childTransform, shape);
-				btVector3 halfExtents(
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0],
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1],
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
-				shape = worldImporter->createBoxShape(halfExtents);
-				if (compound)
-				{
-					compound->addChildShape(childTransform,shape);
-				}
-				urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
-				urdfColObj.m_geometry.m_boxSize = 2.*halfExtents;
-				break;
 			}
-			case GEOM_CAPSULE:
+			urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
+			urdfColObj.m_geometry.m_boxSize = 2.*halfExtents;
+			break;
+		}
+		case GEOM_CAPSULE:
+		{
+			shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
+			if (compound)
 			{
-				shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
+				compound->addChildShape(childTransform, shape);
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
-				if (compound)
-				{
-					compound->addChildShape(childTransform,shape);
-				}
-				urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
-				urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
-				urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
-
-				break;
 			}
-			case GEOM_CYLINDER:
+			urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
-			{
+			urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
-				shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
+			urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
-				if (compound)
-				{
-					compound->addChildShape(childTransform,shape);
-				}
-				urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
-				urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
-				urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
 
-				break;
+			break;
+		}
+		case GEOM_CYLINDER:
+		{
+			shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
+				0.5*clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
+			if (compound)
+			{
+				compound->addChildShape(childTransform, shape);
 			}
-			case GEOM_PLANE:
+			urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
-			{
+			urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
-				btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
+			urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
 
-				shape = worldImporter->createPlaneShape(planeNormal,0);
+			break;
-				if (compound)
+		}
-				{
+		case GEOM_PLANE:
-					compound->addChildShape(childTransform,shape);
+		{
-				}
+			btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
-				urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
-				urdfColObj.m_geometry.m_planeNormal.setValue(
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
+
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
+			shape = worldImporter->createPlaneShape(planeNormal, 0);
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
+			if (compound)
-									
+			{
-				break;
+				compound->addChildShape(childTransform, shape);
 			}
-			case GEOM_MESH:
+			urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
-			{
+			urdfColObj.m_geometry.m_planeNormal.setValue(
-				btScalar defaultCollisionMargin = 0.001;
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
 
-				btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
+			break;
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
+		}
-					clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
+		case GEOM_MESH:
+		{
 
-				const std::string& urdf_path="";
+			btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
 
-				std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName;
+			const std::string& urdf_path = "";
-				urdfColObj.m_geometry.m_type = URDF_GEOM_MESH;
-				urdfColObj.m_geometry.m_meshFileName = fileName;
-							
-				urdfColObj.m_geometry.m_meshScale = meshScale;
-				char relativeFileName[1024];
-				char pathPrefix[1024];
-				pathPrefix[0] = 0;
-				if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
-				{
 
-					b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
+			std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName;
-				}
+			urdfColObj.m_geometry.m_type = URDF_GEOM_MESH;
-								
+			urdfColObj.m_geometry.m_meshFileName = fileName;
-				const std::string& error_message_prefix="";
-				std::string out_found_filename; 
-				int out_type;
 
-				bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); 
+			urdfColObj.m_geometry.m_meshScale = meshScale;
-				if (foundFile)
-				{
-					urdfColObj.m_geometry.m_meshFileType = out_type;
-
-					if (out_type==UrdfGeometry::FILE_OBJ)
-					{
-						//create a convex hull for each shape, and store it in a btCompoundShape
-
-						if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
-						{
-							GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
-											
-							if (!glmesh || glmesh->m_numvertices<=0)
-							{
-								b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
-								delete glmesh;
-								break;
-							}
-							btAlignedObjectArray<btVector3> convertedVerts;
-							convertedVerts.reserve(glmesh->m_numvertices);
-											
-							for (int i=0; i<glmesh->m_numvertices; i++)
-							{
-								convertedVerts.push_back(btVector3(
-									glmesh->m_vertices->at(i).xyzw[0]*meshScale[0],
-									glmesh->m_vertices->at(i).xyzw[1]*meshScale[1],
-									glmesh->m_vertices->at(i).xyzw[2]*meshScale[2]));
-							}
-
-							BT_PROFILE("convert trimesh");
-							btTriangleMesh* meshInterface = new btTriangleMesh();
-							this->m_data->m_meshInterfaces.push_back(meshInterface);
-							{
-								BT_PROFILE("convert vertices");
-
-								for (int i=0; i<glmesh->m_numIndices/3; i++)
-								{
-									const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)];
-									const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)];
-									const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)];
-									meshInterface->addTriangle(v0,v1,v2);
-								}
-							}
-							{
-								BT_PROFILE("create btBvhTriangleMeshShape");
-								btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
-								m_data->m_collisionShapes.push_back(trimesh);
-								//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
-								shape = trimesh;
-								if (compound)
-								{
-									compound->addChildShape(childTransform,shape);
-								}
-							}
-							delete glmesh;
-						} else
-						{
-
-							std::vector<tinyobj::shape_t> shapes;
-							std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str());
-
-							//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
-							//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
-							B3_PROFILE("createConvexHullFromShapes");
-							if (compound==0)
-							{
-								compound = worldImporter->createCompoundShape();
-							}
-							compound->setMargin(defaultCollisionMargin);
-
-							for (int s = 0; s<(int)shapes.size(); s++)
-							{
-								btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
-								convexHull->setMargin(defaultCollisionMargin);
-								tinyobj::shape_t& shape = shapes[s];
-								int faceCount = shape.mesh.indices.size();
-
-								for (int f = 0; f<faceCount; f += 3)
-								{
-
-									btVector3 pt;
-									pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
-										shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
-										shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
 			
-									convexHull->addPoint(pt*meshScale,false);
+			
+			pathPrefix[0] = 0;
+			if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
+			{
 
-									pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
+				b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
-												shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
+			}
-												shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
-									convexHull->addPoint(pt*meshScale, false);
 
-									pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
+			const std::string& error_message_prefix = "";
-												shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
+			std::string out_found_filename;
-												shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
+			int out_type;
-									convexHull->addPoint(pt*meshScale, false);
-								}
 
-								convexHull->recalcLocalAabb();
+			bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
-								convexHull->optimizeConvexHull();
+			if (foundFile)
-								compound->addChildShape(childTransform,convexHull);
+			{
+				urdfColObj.m_geometry.m_meshFileType = out_type;
+
+				if (out_type == UrdfGeometry::FILE_STL)
+				{
+					glmesh = LoadMeshFromSTL(relativeFileName);
+
+				}
+				if (out_type == UrdfGeometry::FILE_OBJ)
+				{
+					//create a convex hull for each shape, and store it in a btCompoundShape
+
+					if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
+					{
+						glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
+
+					}
+					else
+					{
+
+						std::vector<tinyobj::shape_t> shapes;
+						std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str());
+
+						//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
+						//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
+						B3_PROFILE("createConvexHullFromShapes");
+						if (compound == 0)
+						{
+							compound = worldImporter->createCompoundShape();
+						}
+						compound->setMargin(defaultCollisionMargin);
+
+						for (int s = 0; s < (int)shapes.size(); s++)
+						{
+							btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
+							convexHull->setMargin(defaultCollisionMargin);
+							tinyobj::shape_t& shape = shapes[s];
+							int faceCount = shape.mesh.indices.size();
+
+							for (int f = 0; f < faceCount; f += 3)
+							{
+
+								btVector3 pt;
+								pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
+									shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
+									shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
+
+								convexHull->addPoint(pt*meshScale, false);
+
+								pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
+									shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
+									shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
+								convexHull->addPoint(pt*meshScale, false);
+
+								pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
+									shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
+									shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
+								convexHull->addPoint(pt*meshScale, false);
 							}
+
+							convexHull->recalcLocalAabb();
+							convexHull->optimizeConvexHull();
+							compound->addChildShape(childTransform, convexHull);
 						}
 					}
 				}
-				break;
-			}
-			default:
-			{
 			}
+			break;
+		}
+		default:
+		{
+		}
 		}
 		if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN)
 		{
 			urdfCollisionObjects.push_back(urdfColObj);
 		}
-	}
 
+		if (glmesh)
+		{
+			btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
+				clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
+
+
+			if (!glmesh || glmesh->m_numvertices <= 0)
+			{
+				b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
+				delete glmesh;
+			}
+			else
+			{
+
+				btAlignedObjectArray<btVector3> convertedVerts;
+				convertedVerts.reserve(glmesh->m_numvertices);
+
+				for (int i = 0; i < glmesh->m_numvertices; i++)
+				{
+					convertedVerts.push_back(btVector3(
+						glmesh->m_vertices->at(i).xyzw[0] * meshScale[0],
+						glmesh->m_vertices->at(i).xyzw[1] * meshScale[1],
+						glmesh->m_vertices->at(i).xyzw[2] * meshScale[2]));
+				}
+
+				if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
+				{
+					
+
+					BT_PROFILE("convert trimesh");
+					btTriangleMesh* meshInterface = new btTriangleMesh();
+					this->m_data->m_meshInterfaces.push_back(meshInterface);
+					{
+						BT_PROFILE("convert vertices");
+
+						for (int i = 0; i < glmesh->m_numIndices / 3; i++)
+						{
+							const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i * 3)];
+							const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i * 3 + 1)];
+							const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i * 3 + 2)];
+							meshInterface->addTriangle(v0, v1, v2);
+						}
+					}
+					{
+						BT_PROFILE("create btBvhTriangleMeshShape");
+						btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
+						m_data->m_collisionShapes.push_back(trimesh);
+						//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
+						shape = trimesh;
+						if (compound)
+						{
+							compound->addChildShape(childTransform, shape);
+						}
+					}
+					delete glmesh;
+				}
+				else
+				{
+					//convex mesh
+
+					if (compound == 0)
+					{
+						compound = worldImporter->createCompoundShape();
+					}
+					compound->setMargin(defaultCollisionMargin);
+
+					{
+						btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
+						convexHull->setMargin(defaultCollisionMargin);
+
+						for (int v = 0; v < convertedVerts.size(); v++)
+						{
+
+							btVector3 pt = convertedVerts[v];
+							convexHull->addPoint(pt, false);
+						}
+
+						convexHull->recalcLocalAabb();
+						convexHull->optimizeConvexHull();
+						compound->addChildShape(childTransform, convexHull);
+					}
+				}
+			}
+		}	
+	}
 	if (compound && compound->getNumChildShapes())
 	{
 		shape = compound;
@@ -4647,6 +4734,14 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 									}
 
 									motor->setVelocityTarget(desiredVelocity,kd);
+									//todo: instead of clamping, combine the motor and limit
+									//and combine handling of limit force and motor force.
+									
+									//clamp position
+									if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
+									{
+										btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
+									}
 									motor->setPositionTarget(desiredPosition,kp);
 
 									btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
@@ -6296,6 +6391,29 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
 			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();
+
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[1] = body->m_rootLocalInertialFrame.getOrigin()[1];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[2] = body->m_rootLocalInertialFrame.getOrigin()[2];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[3] = body->m_rootLocalInertialFrame.getRotation()[0];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[4] = body->m_rootLocalInertialFrame.getRotation()[1];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_rootLocalInertialFrame.getRotation()[2];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_rootLocalInertialFrame.getRotation()[3];
+		
+			serverCmd.m_dynamicsInfo.m_restitution = mb->getBaseCollider()->getRestitution();
+			serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getBaseCollider()->getRollingFriction();
+			serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getBaseCollider()->getSpinningFriction();
+
+			if (mb->getBaseCollider()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)
+			{
+				serverCmd.m_dynamicsInfo.m_contactStiffness = mb->getBaseCollider()->getContactStiffness();
+				serverCmd.m_dynamicsInfo.m_contactDamping = mb->getBaseCollider()->getContactDamping();
+			}
+			else
+			{
+				serverCmd.m_dynamicsInfo.m_contactStiffness = -1;
+				serverCmd.m_dynamicsInfo.m_contactDamping = -1;
+			}
 		}
 		else
 		{
@@ -6304,9 +6422,31 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
 			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getLinkInertia(linkIndex)[1];
 			serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getLinkInertia(linkIndex)[2];
 
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[0] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[0];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[1] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[1];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[2] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[2];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[3] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[0];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[4] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[1];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[2];
+			serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[3];
+
 			if (mb->getLinkCollider(linkIndex))
 			{
 				serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction();
+				serverCmd.m_dynamicsInfo.m_restitution = mb->getLinkCollider(linkIndex)->getRestitution();
+				serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getLinkCollider(linkIndex)->getRollingFriction();
+				serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getLinkCollider(linkIndex)->getSpinningFriction();
+
+				if (mb->getLinkCollider(linkIndex)->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)
+				{
+					serverCmd.m_dynamicsInfo.m_contactStiffness = mb->getLinkCollider(linkIndex)->getContactStiffness();
+					serverCmd.m_dynamicsInfo.m_contactDamping = mb->getLinkCollider(linkIndex)->getContactDamping();
+				}
+				else
+				{
+					serverCmd.m_dynamicsInfo.m_contactStiffness = -1;
+					serverCmd.m_dynamicsInfo.m_contactDamping = -1;
+				}
 			}
 			else
 			{
@@ -8026,7 +8166,183 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
 	return hasStatus;
 }
 
+//		PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
+//		PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
+//		PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
+//		PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH);
+//		PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
+//		PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
 
+int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape* colShape, const btTransform& transform, b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes)
+{
+	if (maxCollisionShapes <= 0)
+	{
+		b3Warning("No space in buffer");
+		return 0;
+	}
+
+	int numConverted = 0;
+
+	collisionShapeBuffer[0].m_localCollisionFrame[0] = transform.getOrigin()[0];
+	collisionShapeBuffer[0].m_localCollisionFrame[1] = transform.getOrigin()[1];
+	collisionShapeBuffer[0].m_localCollisionFrame[2] = transform.getOrigin()[2];
+	collisionShapeBuffer[0].m_localCollisionFrame[3] = transform.getRotation()[0];
+	collisionShapeBuffer[0].m_localCollisionFrame[4] = transform.getRotation()[1];
+	collisionShapeBuffer[0].m_localCollisionFrame[5] = transform.getRotation()[2];
+	collisionShapeBuffer[0].m_localCollisionFrame[6] = transform.getRotation()[3];
+	collisionShapeBuffer[0].m_meshAssetFileName[0] = 0;
+
+	switch (colShape->getShapeType())
+	{
+	case CONVEX_HULL_SHAPE_PROXYTYPE:
+	{
+		UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
+		if (urdfCol && (urdfCol->m_geometry.m_type == GEOM_MESH))
+		{
+			collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
+			collisionShapeBuffer[0].m_dimensions[0] = urdfCol->m_geometry.m_meshScale[0];
+			collisionShapeBuffer[0].m_dimensions[1] = urdfCol->m_geometry.m_meshScale[1];
+			collisionShapeBuffer[0].m_dimensions[2] = urdfCol->m_geometry.m_meshScale[2];
+			strcpy(collisionShapeBuffer[0].m_meshAssetFileName, urdfCol->m_geometry.m_meshFileName.c_str());
+			numConverted += 1;
+		}
+		else
+		{
+			collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
+			sprintf(collisionShapeBuffer[0].m_meshAssetFileName, "unknown_file");
+			collisionShapeBuffer[0].m_dimensions[0] = 1;
+			collisionShapeBuffer[0].m_dimensions[1] = 1;
+			collisionShapeBuffer[0].m_dimensions[2] = 1;
+			numConverted++;
+		}
+		
+		break;
+	}
+	case CAPSULE_SHAPE_PROXYTYPE:
+	{
+		btCapsuleShapeZ* capsule = (btCapsuleShapeZ*)colShape;
+		collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CAPSULE;
+		collisionShapeBuffer[0].m_dimensions[0] = 2.*capsule->getHalfHeight();
+		collisionShapeBuffer[0].m_dimensions[1] = capsule->getRadius();
+		collisionShapeBuffer[0].m_dimensions[2] = 0;
+		numConverted++;
+		break;
+	}
+	case CYLINDER_SHAPE_PROXYTYPE:
+	{
+		btCylinderShapeZ* cyl = (btCylinderShapeZ*)colShape;
+		collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CYLINDER;
+		collisionShapeBuffer[0].m_dimensions[0] = 2.*cyl->getHalfExtentsWithMargin().getZ(); 
+		collisionShapeBuffer[0].m_dimensions[1] = cyl->getHalfExtentsWithMargin().getX();
+		collisionShapeBuffer[0].m_dimensions[2] = 0;
+		numConverted++;
+		break;
+	}
+	case BOX_SHAPE_PROXYTYPE:
+	{
+		btBoxShape* box = (btBoxShape*)colShape;
+		btVector3 halfExtents = box->getHalfExtentsWithMargin();
+		collisionShapeBuffer[0].m_collisionGeometryType = GEOM_BOX;
+		collisionShapeBuffer[0].m_dimensions[0] = 2.*halfExtents[0];
+		collisionShapeBuffer[0].m_dimensions[1] = 2.*halfExtents[1];
+		collisionShapeBuffer[0].m_dimensions[2] = 2.*halfExtents[2];
+		numConverted++;
+		break;
+	}
+	case SPHERE_SHAPE_PROXYTYPE:
+	{
+		btSphereShape* sphere = (btSphereShape*)colShape;
+		collisionShapeBuffer[0].m_collisionGeometryType = GEOM_SPHERE;
+		collisionShapeBuffer[0].m_dimensions[0] = sphere->getRadius();
+		collisionShapeBuffer[0].m_dimensions[1] = sphere->getRadius();
+		collisionShapeBuffer[0].m_dimensions[2] = sphere->getRadius();
+		numConverted++;
+		break;
+	}
+	case COMPOUND_SHAPE_PROXYTYPE:
+	{
+		//it could be a compound mesh from a wavefront OBJ, check it
+		UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
+		if (urdfCol && (urdfCol->m_geometry.m_type == GEOM_MESH))
+		{
+			collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
+			collisionShapeBuffer[0].m_dimensions[0] = urdfCol->m_geometry.m_meshScale[0];
+			collisionShapeBuffer[0].m_dimensions[1] = urdfCol->m_geometry.m_meshScale[1];
+			collisionShapeBuffer[0].m_dimensions[2] = urdfCol->m_geometry.m_meshScale[2];
+			strcpy(collisionShapeBuffer[0].m_meshAssetFileName, urdfCol->m_geometry.m_meshFileName.c_str());
+			numConverted += 1;
+		}
+		else
+		{
+
+			//recurse, accumulate childTransform
+			btCompoundShape* compound = (btCompoundShape*)colShape;
+			for (int i = 0; i < compound->getNumChildShapes(); i++)
+			{
+				btTransform childTrans = transform*compound->getChildTransform(i);
+				int remain = maxCollisionShapes - numConverted;
+				int converted = extractCollisionShapes(compound->getChildShape(i), childTrans, &collisionShapeBuffer[numConverted], remain);
+				numConverted += converted;
+			}
+		}
+		break;
+	}
+	default:
+	{
+		b3Warning("Unexpected collision shape type in PhysicsServerCommandProcessor::extractCollisionShapes");
+	}
+	};
+	
+	return numConverted;
+}
+
+
+bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
+{
+	bool hasStatus = true;
+
+	BT_PROFILE("CMD_REQUEST_COLLISION_SHAPE_INFO");
+	SharedMemoryStatus& serverCmd = serverStatusOut;
+	serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_FAILED;
+	int bodyUniqueId = clientCmd.m_requestCollisionShapeDataArguments.m_bodyUniqueId;
+	int linkIndex = clientCmd.m_requestCollisionShapeDataArguments.m_linkIndex;
+	InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+	if (bodyHandle)
+	{
+		if (bodyHandle->m_multiBody)
+		{
+			b3CollisionShapeData* collisionShapeStoragePtr = (b3CollisionShapeData*)bufferServerToClient;
+			int totalBytesPerObject = sizeof(b3CollisionShapeData);
+			int maxNumColObjects = bufferSizeInBytes / totalBytesPerObject - 1;
+			btTransform childTrans;
+			childTrans.setIdentity();
+			serverCmd.m_sendCollisionShapeArgs.m_bodyUniqueId = bodyUniqueId;
+			serverCmd.m_sendCollisionShapeArgs.m_linkIndex = linkIndex;
+
+			if (linkIndex == -1)
+			{
+				if (bodyHandle->m_multiBody->getBaseCollider())
+				{
+					//extract shape info from base collider
+					int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
+					serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
+					serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
+				}
+			}
+			else
+			{
+				if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks() && bodyHandle->m_multiBody->getLinkCollider(linkIndex))
+				{
+					int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getLinkCollider(linkIndex)->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
+					serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
+					serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
+				}
+			}
+		}
+	}
+
+	return hasStatus;
+}
 bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	bool hasStatus = true;
@@ -8736,6 +9052,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
+	case CMD_REQUEST_COLLISION_SHAPE_INFO:
+	{
+		hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
+		break;
+	}
 	case CMD_UPDATE_VISUAL_SHAPE:
 		{
 			hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -74,6 +74,7 @@ protected:
 	bool processCreateUserConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processRequestCollisionShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processUpdateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processLoadTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -83,6 +84,8 @@ protected:
 	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);
 
+	int extractCollisionShapes(const class btCollisionShape* colShape, const class btTransform& transform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes);
+
 	bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);
 
 	bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,

examples/SharedMemory/PhysicsServerExample.cpp

@@ -2542,14 +2542,14 @@ void PhysicsServerExample::drawUserDebugLines()
 			}
 			
 			float colorRGBA[4] = {
-				m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
-				m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
-				m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
-				1.};
+				(float)1.};
 
-			float pos[3] = {m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
+			float pos[3] = { (float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
-			m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
-			m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};
 
 			int graphicsIndex = m_multiThreadedHelper->m_userDebugText[i].m_trackingVisualShapeIndex;
 			if (graphicsIndex>=0)
@@ -2604,7 +2604,9 @@ void PhysicsServerExample::drawUserDebugLines()
 					offset.setIdentity();
 					offset.setOrigin(btVector3(0,-float(t)*sz,0));
 					btTransform result = tr*offset;
-					float newpos[3] = {result.getOrigin()[0],result.getOrigin()[1],result.getOrigin()[2]};
+					float newpos[3] = { (float)result.getOrigin()[0],
+						(float)result.getOrigin()[1],
+						(float)result.getOrigin()[2]};
 					
 					m_guiHelper->getAppInterface()->drawText3D(pieces[t].c_str(),
 						newpos,orientation,colorRGBA,

examples/SharedMemory/SharedMemoryCommands.h

@@ -303,6 +303,12 @@ struct RequestVisualShapeDataArgs
 	int m_startingVisualShapeIndex;
 };
 
+struct RequestCollisionShapeDataArgs
+{
+	int m_bodyUniqueId;
+	int m_linkIndex;
+};
+
 enum EnumUpdateVisualShapeData
 {
 	CMD_UPDATE_VISUAL_SHAPE_TEXTURE=1,
@@ -338,7 +344,12 @@ struct SendVisualShapeDataArgs
     int m_numRemainingVisualShapes;
 };
 
-
+struct SendCollisionShapeDataArgs
+{
+	int m_bodyUniqueId;
+	int m_linkIndex;
+	int m_numCollisionShapes;
+};
 
 struct SendDebugLinesArgs
 {
@@ -1000,6 +1011,7 @@ struct SharedMemoryCommand
 		struct b3SearchPathfArgs m_searchPathArgs;
 		struct b3CustomCommand m_customCommandArgs;
 		struct b3StateSerializationArguments m_loadStateArguments;
+		struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments;		
     };
 };
 
@@ -1076,6 +1088,7 @@ struct SharedMemoryStatus
 		struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
 		struct b3StateSerializationArguments m_saveStateResultArgs;
 		struct b3LoadSoftBodyResultArgs m_loadSoftBodyResultArguments;
+		struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
 	};
 };
 

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 201801010
+#define SHARED_MEMORY_MAGIC_NUMBER 201801080
+//#define SHARED_MEMORY_MAGIC_NUMBER 201801010
 //#define SHARED_MEMORY_MAGIC_NUMBER 201710180
 //#define SHARED_MEMORY_MAGIC_NUMBER 201710050
 //#define SHARED_MEMORY_MAGIC_NUMBER 201708270
@@ -80,6 +81,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
 	CMD_SAVE_STATE,
 	CMD_RESTORE_STATE,
+	CMD_REQUEST_COLLISION_SHAPE_INFO,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -185,6 +187,8 @@ enum EnumSharedMemoryServerStatus
 		CMD_SAVE_STATE_COMPLETED,
 		CMD_RESTORE_STATE_FAILED,
 		CMD_RESTORE_STATE_COMPLETED,
+		CMD_COLLISION_SHAPE_INFO_COMPLETED,
+		CMD_COLLISION_SHAPE_INFO_FAILED,
 		//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
@@ -217,10 +221,6 @@ enum JointType {
 	eGearType=6
 };
 
-enum b3RequestDynamicsInfoFlags
-{
-	eDYNAMICS_INFO_REPORT_INERTIA=1,
-};
 
 enum b3JointInfoFlags
 {
@@ -247,6 +247,7 @@ struct b3JointInfo
 		double m_parentFrame[7]; // position and orientation (quaternion)
 		double m_childFrame[7]; // ^^^
 		double m_jointAxis[3]; // joint axis in parent local frame
+		int m_parentIndex;
 };
 
 
@@ -278,7 +279,14 @@ struct b3DynamicsInfo
 {
 	double m_mass;
 	double m_localInertialDiagonal[3];
+	double m_localInertialFrame[7];
 	double m_lateralFrictionCoeff;
+
+	double m_rollingFrictionCoeff;
+	double m_spinningFrictionCoeff;
+	double m_restitution;
+	double m_contactStiffness;
+	double m_contactDamping;
 };
 
 // copied from btMultiBodyLink.h
@@ -531,6 +539,23 @@ struct b3VisualShapeInformation
 	struct b3VisualShapeData* m_visualShapeData;
 };
 
+
+struct b3CollisionShapeData
+{
+	int m_objectUniqueId;
+	int m_linkIndex;
+	int m_collisionGeometryType;//GEOM_BOX, GEOM_SPHERE etc
+	double m_dimensions[3];//meaning depends on m_visualGeometryType GEOM_BOX: extents, GEOM_SPHERE: radius, GEOM_CAPSULE+GEOM_CYLINDER:length, radius, GEOM_MESH: mesh scale 
+	double m_localCollisionFrame[7];//pos[3], orn[4]
+	char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
+};
+
+struct b3CollisionShapeInformation
+{
+	int m_numCollisionShapes;
+	struct b3CollisionShapeData* m_collisionShapeData;
+};
+
 enum eLinkStateFlags
 {
 	ACTUAL_STATE_COMPUTE_LINKVELOCITY=1,

examples/pybullet/examples/humanoid_manual_control.py

@@ -0,0 +1,32 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+obUids = p.loadMJCF("mjcf/humanoid.xml")
+humanoid = obUids[1]
+
+gravId = p.addUserDebugParameter("gravity",-10,10,-10)
+jointIds=[]
+paramIds=[]
+
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.changeDynamics(humanoid,-1,linearDamping=0, angularDamping=0)
+
+for j in range (p.getNumJoints(humanoid)):
+	p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
+	info = p.getJointInfo(humanoid,j)
+	#print(info)
+	jointName = info[1]
+	jointType = info[2]
+	if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
+		jointIds.append(j)
+		paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
+
+p.setRealTimeSimulation(1)
+while(1):
+	p.setGravity(0,0,p.readUserDebugParameter(gravId))
+	for i in range(len(paramIds)):
+		c = paramIds[i]
+		targetPos = p.readUserDebugParameter(c)
+		p.setJointMotorControl2(humanoid,jointIds[i],p.POSITION_CONTROL,targetPos, force=5*240.)
+	time.sleep(0.01)

examples/pybullet/examples/jointFrictionAndMotor.py

@@ -0,0 +1,26 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+door=p.loadURDF("door.urdf")
+
+#linear/angular damping for base and all children=0
+p.changeDynamics(door,-1,linearDamping=0, angularDamping=0)
+for j in range (p.getNumJoints(door)):
+	p.changeDynamics(door,j,linearDamping=0, angularDamping=0)
+	print(p.getJointInfo(door,j))
+
+
+frictionId = p.addUserDebugParameter("jointFriction",0,20,10)
+torqueId = p.addUserDebugParameter("joint torque",0,20,5)
+
+while (1):
+	frictionForce = p.readUserDebugParameter(frictionId)
+	jointTorque = p.readUserDebugParameter(torqueId)
+	#set the joint friction	
+	p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce)
+	#apply a joint torque
+	p.setJointMotorControl2(door,1,p.TORQUE_CONTROL,force=jointTorque)
+	p.stepSimulation()
+	time.sleep(0.01)
+	

examples/pybullet/examples/urdfEditor.py

@@ -0,0 +1,567 @@
+import pybullet as p
+import time
+
+
+
+class UrdfInertial(object):
+	def __init__(self):
+		self.mass = 1
+		self.inertia_xxyyzz=[7,8,9]
+		self.origin_rpy=[1,2,3]
+		self.origin_xyz=[4,5,6]
+
+class UrdfContact(object):
+	def __init__(self):
+		self.lateral_friction = 1
+		self.rolling_friction = 0
+		self.spinning_friction = 0
+
+class UrdfLink(object):
+	def __init__(self):
+		self.link_name = "dummy"
+		self.urdf_inertial = UrdfInertial()
+		self.urdf_visual_shapes=[]
+		self.urdf_collision_shapes=[]
+		
+class UrdfVisual(object):
+	def __init__(self):
+		self.origin_rpy = [1,2,3]
+		self.origin_xyz = [4,5,6]
+		self.geom_type = p.GEOM_BOX
+		self.geom_radius = 1
+		self.geom_extents = [7,8,9]
+		self.geom_length=[10]
+		self.geom_meshfilename = "meshfile"
+		self.geom_meshscale=[1,1,1]
+		self.material_rgba = [1,0,0,1]
+		self.material_name = ""
+		
+class UrdfCollision(object):
+	def __init__(self):
+		self.origin_rpy = [1,2,3]
+		self.origin_xyz = [4,5,6]
+		self.geom_type = p.GEOM_BOX
+		self.geom_radius = 1
+		self.geom_length = 2
+		self.geom_extents = [7,8,9]
+		self.geom_meshfilename = "meshfile"
+		self.geom_meshscale = [1,1,1]
+class UrdfJoint(object):
+	def __init__(self):
+		self.link = UrdfLink()
+		self.joint_name = "joint_dummy"
+		self.joint_type = p.JOINT_REVOLUTE
+		self.joint_lower_limit = 0
+		self.joint_upper_limit = -1
+		self.parent_name = "parentName"
+		self.child_name = "childName"
+		self.joint_origin_xyz = [1,2,3]
+		self.joint_origin_rpy = [1,2,3]
+		self.joint_axis_xyz = [1,2,3]
+		 
+class UrdfEditor(object):
+	def __init__(self):
+		self.initialize()
+
+	def initialize(self):
+		self.urdfLinks=[]
+		self.urdfJoints=[]
+		self.robotName = ""
+		self.linkNameToIndex={}
+		self.jointTypeToName={p.JOINT_FIXED:"JOINT_FIXED" ,\
+													p.JOINT_REVOLUTE:"JOINT_REVOLUTE",\
+													p.JOINT_PRISMATIC:"JOINT_PRISMATIC" }
+		
+		
+	def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
+		dyn = p.getDynamicsInfo(bodyUid,linkIndex,physicsClientId=physicsClientId)
+		urdfLink.urdf_inertial.mass = dyn[0]
+		urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
+		#todo
+		urdfLink.urdf_inertial.origin_xyz = dyn[3]
+		rpy = p.getEulerFromQuaternion(dyn[4])
+		urdfLink.urdf_inertial.origin_rpy = rpy
+		
+		visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId)
+		matIndex = 0
+		for v in visualShapes:
+			if (v[1]==linkIndex):
+				#print("visualShape base:",v)
+				urdfVisual = UrdfVisual()
+				urdfVisual.geom_type = v[2]
+				if (v[2]==p.GEOM_BOX):
+					urdfVisual.geom_extents = v[3]
+				if (v[2]==p.GEOM_SPHERE):
+					urdfVisual.geom_radius = v[3][0]	
+				if (v[2]==p.GEOM_MESH):
+					urdfVisual.geom_meshfilename = v[4].decode("utf-8")
+					urdfVisual.geom_meshscale = v[3]
+				if (v[2]==p.GEOM_CYLINDER):
+					urdfVisual.geom_length=v[3][0]
+					urdfVisual.geom_radius=v[3][1]
+				if (v[2]==p.GEOM_CAPSULE):
+					urdfVisual.geom_length=v[3][0]
+					urdfVisual.geom_radius=v[3][1]
+					#print("Capsule length=",urdfVisual.geom_length)
+					#print("Capsule radius=",urdfVisual.geom_radius)
+					
+				urdfVisual.origin_xyz = v[5]
+				urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
+				urdfVisual.material_rgba = v[7]
+				name = 'mat_{}_{}'.format(linkIndex,matIndex)
+				urdfVisual.material_name = name
+				
+				urdfLink.urdf_visual_shapes.append(urdfVisual)
+				matIndex=matIndex+1
+				
+		collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId)
+		for v in collisionShapes:
+			#print("collisionShape base:",v)
+			urdfCollision = UrdfCollision()
+			print("geom type=",v[0])
+			urdfCollision.geom_type = v[2]
+			if (v[2]==p.GEOM_BOX):
+				urdfCollision.geom_extents = v[3]
+			if (v[2]==p.GEOM_SPHERE):
+				urdfCollision.geom_radius = v[3][0]
+			if (v[2]==p.GEOM_MESH):
+					urdfCollision.geom_meshfilename = v[4].decode("utf-8")
+					urdfCollision.geom_meshscale = v[3]
+					print("p.GEOM_MESH filename=",urdfCollision.geom_meshfilename)
+			if (v[2]==p.GEOM_CYLINDER):
+				urdfCollision.geom_length=v[3][0]
+				urdfCollision.geom_radius=v[3][1]
+			if (v[2]==p.GEOM_CAPSULE):
+					urdfCollision.geom_length=v[3][0]
+					urdfCollision.geom_radius=v[3][1]
+					print("Capsule length=",urdfCollision.geom_length)
+					print("Capsule radius=",urdfCollision.geom_radius)
+			pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
+				v[5], v[6])
+			urdfCollision.origin_xyz = pos
+			urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
+			urdfLink.urdf_collision_shapes.append(urdfCollision)
+
+	def initializeFromBulletBody(self, bodyUid, physicsClientId):
+		self.initialize()
+
+		#always create a base link
+		baseLink = UrdfLink()
+		baseLinkIndex = -1
+		self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
+		baseLink.link_name = 	p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8") 		
+		self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
+		self.urdfLinks.append(baseLink)
+		
+	
+		#print(visualShapes)
+		#optionally create child links and joints
+		for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
+			jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
+			urdfLink = UrdfLink()
+			self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
+			urdfLink.link_name = jointInfo[12].decode("utf-8")
+			self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
+			self.urdfLinks.append(urdfLink)
+	
+			urdfJoint = UrdfJoint()
+			urdfJoint.link = urdfLink
+			urdfJoint.joint_name = jointInfo[1].decode("utf-8")
+			urdfJoint.joint_type = jointInfo[2]
+			urdfJoint.joint_axis_xyz = jointInfo[13]
+			orgParentIndex = jointInfo[16]
+			if (orgParentIndex<0):
+				urdfJoint.parent_name = baseLink.link_name
+			else:
+				parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
+				urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
+			urdfJoint.child_name = urdfLink.link_name
+
+			#todo, compensate for inertia/link frame offset
+			
+			dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
+			childInertiaPos = dynChild[3]
+			childInertiaOrn = dynChild[4]
+			parentCom2JointPos=jointInfo[14] 
+			parentCom2JointOrn=jointInfo[15]
+			tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
+			tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
+			dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
+			parentInertiaPos = dynParent[3]
+			parentInertiaOrn = dynParent[4]
+			
+			pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
+			pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
+			
+			urdfJoint.joint_origin_xyz = pos
+			urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
+			
+			self.urdfJoints.append(urdfJoint)
+			
+	def writeInertial(self,file,urdfInertial, precision=5):
+		file.write("\t\t<inertial>\n")
+		str = '\t\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(\
+		urdfInertial.origin_rpy[0],urdfInertial.origin_rpy[1],urdfInertial.origin_rpy[2],\
+		urdfInertial.origin_xyz[0],urdfInertial.origin_xyz[1],urdfInertial.origin_xyz[2], prec=precision)
+		file.write(str)
+		str = '\t\t\t<mass value=\"{:.{prec}f}\"/>\n'.format(urdfInertial.mass,prec=precision)
+		file.write(str)
+		str = '\t\t\t<inertia ixx=\"{:.{prec}f}\" ixy=\"0\" ixz=\"0\" iyy=\"{:.{prec}f}\" iyz=\"0\" izz=\"{:.{prec}f}\"/>\n'.format(\
+		urdfInertial.inertia_xxyyzz[0],\
+		urdfInertial.inertia_xxyyzz[1],\
+		urdfInertial.inertia_xxyyzz[2],prec=precision)
+		file.write(str)
+		file.write("\t\t</inertial>\n")
+  
+	def writeVisualShape(self,file,urdfVisual, precision=5):
+		file.write("\t\t<visual>\n")
+		str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
+			urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2],
+			urdfVisual.origin_xyz[0],urdfVisual.origin_xyz[1],urdfVisual.origin_xyz[2], prec=precision)
+		file.write(str)
+		file.write("\t\t\t<geometry>\n")
+		if urdfVisual.geom_type == p.GEOM_BOX:
+			str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfVisual.geom_extents[0],\
+				urdfVisual.geom_extents[1],urdfVisual.geom_extents[2], prec=precision)
+			file.write(str)
+		if urdfVisual.geom_type == p.GEOM_SPHERE:
+			str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfVisual.geom_radius,\
+				prec=precision)
+			file.write(str)
+		if urdfVisual.geom_type == p.GEOM_MESH:
+			str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfVisual.geom_meshfilename,\
+				prec=precision)
+			file.write(str)
+		if urdfVisual.geom_type == p.GEOM_CYLINDER:
+			str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
+				urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
+			file.write(str)
+		if urdfVisual.geom_type == p.GEOM_CAPSULE:
+			str = '\t\t\t\t<capsule length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
+				urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
+			file.write(str)
+
+		file.write("\t\t\t</geometry>\n")
+		str = '\t\t\t<material name=\"{}\">\n'.format(urdfVisual.material_name)
+		file.write(str)
+		str = '\t\t\t\t<color rgba="{:.{prec}f} {:.{prec}f} {:.{prec}f} {:.{prec}f}" />\n'.format(urdfVisual.material_rgba[0],\
+			urdfVisual.material_rgba[1],urdfVisual.material_rgba[2],urdfVisual.material_rgba[3],prec=precision)
+		file.write(str)
+		file.write("\t\t\t</material>\n")
+		file.write("\t\t</visual>\n")
+
+	def writeCollisionShape(self,file,urdfCollision, precision=5):
+		file.write("\t\t<collision>\n")
+		str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
+			urdfCollision.origin_rpy[0],urdfCollision.origin_rpy[1],urdfCollision.origin_rpy[2],
+			urdfCollision.origin_xyz[0],urdfCollision.origin_xyz[1],urdfCollision.origin_xyz[2], prec=precision)
+		file.write(str)
+		file.write("\t\t\t<geometry>\n")
+		if urdfCollision.geom_type == p.GEOM_BOX:
+			str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfCollision.geom_extents[0],\
+				urdfCollision.geom_extents[1],urdfCollision.geom_extents[2], prec=precision)
+			file.write(str)
+		if urdfCollision.geom_type == p.GEOM_SPHERE:
+			str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfCollision.geom_radius,\
+				prec=precision)
+			file.write(str)
+		if urdfCollision.geom_type == p.GEOM_MESH:
+			str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfCollision.geom_meshfilename,\
+				prec=precision)
+			file.write(str)
+		if urdfCollision.geom_type == p.GEOM_CYLINDER:
+			str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
+				urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
+			file.write(str)
+		if urdfCollision.geom_type == p.GEOM_CAPSULE:
+			str = '\t\t\t\t<capsule length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
+				urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
+			file.write(str)
+		file.write("\t\t\t</geometry>\n")
+		file.write("\t\t</collision>\n")
+		
+	  
+	def writeLink(self, file, urdfLink):
+		file.write("\t<link name=\"")
+		file.write(urdfLink.link_name)
+		file.write("\">\n")
+		
+		self.writeInertial(file,urdfLink.urdf_inertial)
+		for v in urdfLink.urdf_visual_shapes:
+			self.writeVisualShape(file,v)
+		for c in urdfLink.urdf_collision_shapes:
+			self.writeCollisionShape(file,c)
+		file.write("\t</link>\n")
+
+	def writeJoint(self, file, urdfJoint, precision=5):
+		jointTypeStr = "invalid"
+		if urdfJoint.joint_type == p.JOINT_REVOLUTE:
+			if urdfJoint.joint_upper_limit < urdfJoint.joint_lower_limit:
+				jointTypeStr = "continuous"
+			else:
+				jointTypeStr = "revolute"
+		if urdfJoint.joint_type == p.JOINT_FIXED:
+			jointTypeStr  = "fixed"
+		if urdfJoint.joint_type == p.JOINT_PRISMATIC:
+			jointTypeStr  = "prismatic"
+		str = '\t<joint name=\"{}\" type=\"{}\">\n'.format(urdfJoint.joint_name, jointTypeStr)
+		file.write(str)
+		str = '\t\t<parent link=\"{}\"/>\n'.format(urdfJoint.parent_name)
+		file.write(str)
+		str = '\t\t<child link=\"{}\"/>\n'.format(urdfJoint.child_name)
+		file.write(str)
+
+		if urdfJoint.joint_type == p.JOINT_PRISMATIC:
+			#todo: handle limits
+			lowerLimit=-0.5
+			upperLimit=0.5
+			str='<limit effort="1000.0" lower="{:.{prec}f}" upper="{:.{prec}f}" velocity="0.5"/>'.format(lowerLimit,upperLimit,prec=precision)
+			file.write(str)
+	
+		file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
+		str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
+			urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
+		file.write(str)
+		str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
+			urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
+		file.write(str)
+		file.write("\t</joint>\n")
+  
+	def saveUrdf(self, fileName):
+		file = open(fileName,"w")
+		file.write("<?xml version=\"0.0\" ?>\n")
+		file.write("<robot name=\"")
+		file.write(self.robotName)
+		file.write("\">\n")
+
+		for link in self.urdfLinks:
+			self.writeLink(file,link)
+	
+		for joint in self.urdfJoints:
+			self.writeJoint(file,joint)
+		
+		file.write("</robot>\n")
+		file.close()
+  	
+  	#def addLink(...)
+
+	def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
+		#assume link[0] is base
+		if (len(self.urdfLinks)==0):
+			return -1
+			
+		base = self.urdfLinks[0]
+		
+		#v.tmp_collision_shape_ids=[]
+		baseMass = base.urdf_inertial.mass
+		print("baseMass=",baseMass)
+		baseCollisionShapeIndex = -1
+
+		baseShapeTypeArray=[]
+		baseRadiusArray=[]
+		baseHalfExtentsArray=[]
+		lengthsArray=[]
+		fileNameArray=[]
+		meshScaleArray=[]
+		basePositionsArray=[]
+		baseOrientationsArray=[]
+		
+		for v in base.urdf_collision_shapes:
+			print("base v.origin_xyz=",v.origin_xyz)
+			print("base v.origin_rpy=",v.origin_rpy)
+			shapeType = v.geom_type
+			baseShapeTypeArray.append(shapeType)
+			baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
+			baseRadiusArray.append(v.geom_radius)
+			lengthsArray.append(v.geom_length)
+			fileNameArray.append(v.geom_meshfilename)
+			meshScaleArray.append(v.geom_meshscale)
+			basePositionsArray.append(v.origin_xyz)
+			print("v.origin_rpy=",v.origin_rpy)
+			orn=p.getQuaternionFromEuler(v.origin_rpy)
+			baseOrientationsArray.append(orn)
+			
+		print("baseHalfExtentsArray=",baseHalfExtentsArray)
+		if (len(baseShapeTypeArray)):
+			baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
+					radii=baseRadiusArray,
+					halfExtents=baseHalfExtentsArray,
+					lengths=lengthsArray,
+					fileNames=fileNameArray,
+					meshScales=meshScaleArray,
+					collisionFramePositions=basePositionsArray,
+					collisionFrameOrientations=baseOrientationsArray,
+					physicsClientId=physicsClientId)
+
+
+		print("baseCollisionShapeIndex=",baseCollisionShapeIndex)
+			
+			
+		linkMasses=[]
+		linkCollisionShapeIndices=[]
+		linkVisualShapeIndices=[]
+		linkPositions=[]
+		linkOrientations=[]
+		linkMeshScaleArray=[]
+		linkInertialFramePositions=[]
+		linkInertialFrameOrientations=[]
+		linkParentIndices=[]
+		linkJointTypes=[]
+		linkJointAxis=[]
+		
+		for joint in self.urdfJoints:
+			link = joint.link
+			linkMass = link.urdf_inertial.mass
+			
+			print("linkMass=",linkMass)
+
+			linkCollisionShapeIndex=-1
+			linkVisualShapeIndex=-1
+			linkPosition=[0,0,0]
+			linkOrientation=[0,0,0]
+			linkInertialFramePosition=[0,0,0]
+			linkInertialFrameOrientation=[0,0,0]
+			linkParentIndex=self.linkNameToIndex[joint.parent_name]
+			print("parentId=",linkParentIndex)
+			
+			linkJointType=joint.joint_type
+			print("linkJointType=",linkJointType, self.jointTypeToName[linkJointType]	)
+			
+			linkJointAx = joint.joint_axis_xyz
+			
+			linkShapeTypeArray=[]
+			linkRadiusArray=[]
+			linkHalfExtentsArray=[]
+			lengthsArray=[]
+			fileNameArray=[]
+			linkPositionsArray=[]
+			linkOrientationsArray=[]
+			
+			for v in link.urdf_collision_shapes:
+				print("link v.origin_xyz=",v.origin_xyz)
+				print("link v.origin_rpy=",v.origin_rpy)
+				shapeType = v.geom_type
+				linkShapeTypeArray.append(shapeType)
+				linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
+				linkRadiusArray.append(v.geom_radius)
+				lengthsArray.append(v.geom_length)
+				fileNameArray.append(v.geom_meshfilename)
+				linkMeshScaleArray.append(v.geom_meshscale)
+				linkPositionsArray.append(v.origin_xyz)
+				linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
+			
+			print("linkHalfExtentsArray=",linkHalfExtentsArray)
+			if (len(linkShapeTypeArray)):
+				linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray,
+					radii=linkRadiusArray,
+					halfExtents=linkHalfExtentsArray,
+					lengths=lengthsArray,
+					fileNames=fileNameArray,
+					meshScales=linkMeshScaleArray,
+					collisionFramePositions=linkPositionsArray,
+					collisionFrameOrientations=linkOrientationsArray,
+					physicsClientId=physicsClientId)
+			
+			linkMasses.append(linkMass)
+			linkCollisionShapeIndices.append(linkCollisionShapeIndex)
+			linkVisualShapeIndices.append(linkVisualShapeIndex)
+			linkPositions.append(joint.joint_origin_xyz)
+			linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
+			linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
+			linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
+			linkParentIndices.append(linkParentIndex)
+			linkJointTypes.append(joint.joint_type)
+			linkJointAxis.append(joint.joint_axis_xyz)
+		print("\n\n\nlinkMasses=",linkMasses)
+		obUid = p.createMultiBody(baseMass,\
+					baseCollisionShapeIndex= baseCollisionShapeIndex,
+					basePosition=basePosition,
+					baseInertialFramePosition=base.urdf_inertial.origin_xyz,
+					baseInertialFrameOrientation=base.urdf_inertial.origin_rpy,
+					linkMasses=linkMasses,
+					linkCollisionShapeIndices=linkCollisionShapeIndices,
+					linkVisualShapeIndices=linkVisualShapeIndices,
+					linkPositions=linkPositions,
+					linkOrientations=linkOrientations,
+					linkInertialFramePositions=linkInertialFramePositions,
+					linkInertialFrameOrientations=linkInertialFrameOrientations,
+					linkParentIndices=linkParentIndices,
+					linkJointTypes=linkJointTypes,
+					linkJointAxis=linkJointAxis,
+					physicsClientId=physicsClientId)
+		return obUid
+		#create visual and collision shapes
+		#for link in self.urdfLinks:
+		#	for v in link.urdf_visual_shapes:
+		#		v.tmp_visual_shape_ids=[]
+		#		shapeType = v[0]
+		#		if shapeType==p.GEOM_BOX:
+		#			p.createVisualShape(physicsClientId=physId)
+		
+			
+		#for joint in self.urdfJoints:
+		#	self.writeJoint(file,joint)
+			
+		
+		
+	def __del__(self):
+		pass
+      
+      
+      
+##########################################
+org2 = p.connect(p.DIRECT)
+org = p.connect(p.SHARED_MEMORY)
+if (org<0):
+	org = p.connect(p.DIRECT)
+	
+gui = p.connect(p.GUI)
+
+p.resetSimulation(physicsClientId=org)
+
+#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf
+
+
+
+mb = p.loadURDF("r2d2.urdf", physicsClientId=org)
+for i in range(p.getNumJoints(mb,physicsClientId=org)):
+	p.setJointMotorControl2(mb,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
+	
+#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
+
+#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
+
+#for i in range(p.getNumJoints(mb,physicsClientId=org)):
+#	print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org))
+#	print("linkState(",i,"):",p.getLinkState(mb,i,physicsClientId=org))
+
+parser = UrdfEditor()
+parser.initializeFromBulletBody(mb,physicsClientId=org)
+parser.saveUrdf("test.urdf")
+
+if (1):
+	print("\ncreatingMultiBody...................n")
+
+	obUid = parser.createMultiBody(physicsClientId=gui)
+
+	parser2 = UrdfEditor()
+	print("\n###########################################\n")
+	
+	parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
+	parser2.saveUrdf("test2.urdf")
+
+
+	for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
+		p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui)
+		print(p.getJointInfo(obUid,i,physicsClientId=gui))
+
+
+	parser=0
+
+p.setRealTimeSimulation(1,physicsClientId=gui)
+
+
+while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
+	p.stepSimulation(physicsClientId=gui)
+	time.sleep(0.01)
+		

examples/pybullet/pybullet.c

@@ -66,6 +66,7 @@ b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
 	return 0;
 }
 
+
 static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index)
 {
 	double v = 0.0;
@@ -153,6 +154,7 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3])
 	if (seq)
 	{
 		len = PySequence_Size(objVec);
+		assert(len == 3);
 		if (len == 3)
 		{
 			for (i = 0; i < len; i++)
@@ -180,6 +182,7 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3])
 	if (seq)
 	{
 		len = PySequence_Size(obVec);
+		assert(len == 3);
 		if (len == 3)
 		{
 			for (i = 0; i < len; i++)
@@ -913,13 +916,12 @@ 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", "flags", "physicsClientId", NULL};
+		static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
-		if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex, &flags, &physicsClientId))
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex,  &physicsClientId))
 		{
 			return NULL;
 		}
@@ -934,11 +936,7 @@ 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)
 			{
@@ -959,25 +957,41 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
 				return NULL;
 			}
 			
-			
-			
-
 			if (b3GetDynamicsInfo(status_handle, &info))
 			{
 			
+				int numFields = 10;
 				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, 0, PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
-					PyTuple_SetItem(pyInertiaDiag,1,PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
+					PyTuple_SetItem(pyInertiaDiag, 1, PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
-					PyTuple_SetItem(pyInertiaDiag,2,PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
+					PyTuple_SetItem(pyInertiaDiag, 2, PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
 					PyTuple_SetItem(pyDynamicsInfo, 2, pyInertiaDiag);
 				}
-				
+				{
+					PyObject* pyInertiaPos= PyTuple_New(3);
+					PyTuple_SetItem(pyInertiaPos, 0, PyFloat_FromDouble(info.m_localInertialFrame[0]));
+					PyTuple_SetItem(pyInertiaPos, 1, PyFloat_FromDouble(info.m_localInertialFrame[1]));
+					PyTuple_SetItem(pyInertiaPos, 2, PyFloat_FromDouble(info.m_localInertialFrame[2]));
+					PyTuple_SetItem(pyDynamicsInfo, 3, pyInertiaPos);
+				}
+				{
+					PyObject* pyInertiaOrn= PyTuple_New(4);
+					PyTuple_SetItem(pyInertiaOrn, 0, PyFloat_FromDouble(info.m_localInertialFrame[3]));
+					PyTuple_SetItem(pyInertiaOrn, 1, PyFloat_FromDouble(info.m_localInertialFrame[4]));
+					PyTuple_SetItem(pyInertiaOrn, 2, PyFloat_FromDouble(info.m_localInertialFrame[5]));
+					PyTuple_SetItem(pyInertiaOrn, 3, PyFloat_FromDouble(info.m_localInertialFrame[6]));
+					PyTuple_SetItem(pyDynamicsInfo, 4, pyInertiaOrn);
+				}
+				PyTuple_SetItem(pyDynamicsInfo, 5, PyFloat_FromDouble(info.m_restitution));
+				PyTuple_SetItem(pyDynamicsInfo, 6, PyFloat_FromDouble(info.m_rollingFrictionCoeff));
+				PyTuple_SetItem(pyDynamicsInfo, 7, PyFloat_FromDouble(info.m_spinningFrictionCoeff));
+				PyTuple_SetItem(pyDynamicsInfo, 8, PyFloat_FromDouble(info.m_contactDamping));
+				PyTuple_SetItem(pyDynamicsInfo, 9, PyFloat_FromDouble(info.m_contactStiffness));
 				return pyDynamicsInfo;
 			}
 		}
@@ -3128,7 +3142,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
 
 	int bodyUniqueId = -1;
 	int jointIndex = -1;
-	int jointInfoSize = 13;  // size of struct b3JointInfo
+	int jointInfoSize = 17;  // size of struct b3JointInfo
 	b3PhysicsClientHandle sm = 0;
 	int physicsClientId = 0;
 	static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL};
@@ -3199,6 +3213,30 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
 					PyTuple_SetItem(pyListJointInfo, 12,
 									PyString_FromString("not available"));
 				}
+				{
+					PyObject* axis = PyTuple_New(3);
+					PyTuple_SetItem(axis, 0, PyFloat_FromDouble(info.m_jointAxis[0]));
+					PyTuple_SetItem(axis, 1, PyFloat_FromDouble(info.m_jointAxis[1]));
+					PyTuple_SetItem(axis, 2, PyFloat_FromDouble(info.m_jointAxis[2]));
+					PyTuple_SetItem(pyListJointInfo, 13, axis);
+				}
+				{
+					PyObject* pos = PyTuple_New(3);
+					PyTuple_SetItem(pos, 0, PyFloat_FromDouble(info.m_parentFrame[0]));
+					PyTuple_SetItem(pos, 1, PyFloat_FromDouble(info.m_parentFrame[1]));
+					PyTuple_SetItem(pos, 2, PyFloat_FromDouble(info.m_parentFrame[2]));
+					PyTuple_SetItem(pyListJointInfo, 14, pos);
+				}
+				{
+					PyObject* orn = PyTuple_New(4);
+					PyTuple_SetItem(orn, 0, PyFloat_FromDouble(info.m_parentFrame[3]));
+					PyTuple_SetItem(orn, 1, PyFloat_FromDouble(info.m_parentFrame[4]));
+					PyTuple_SetItem(orn, 2, PyFloat_FromDouble(info.m_parentFrame[5]));
+					PyTuple_SetItem(orn, 3, PyFloat_FromDouble(info.m_parentFrame[6]));
+					PyTuple_SetItem(pyListJointInfo, 15, orn);
+				}
+				PyTuple_SetItem(pyListJointInfo, 16, PyInt_FromLong(info.m_parentIndex));
+
 
 				return pyListJointInfo;
 			}
@@ -4834,6 +4872,107 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py
 	return Py_None;
 }
 
+static PyObject* pybullet_getCollisionShapeData(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int objectUniqueId = -1;
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	struct b3CollisionShapeInformation collisionShapeInfo;
+	int statusType;
+	int i;
+	int linkIndex;
+	PyObject* pyResultList = 0;
+	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+	static char* kwlist[] = { "objectUniqueId", "linkIndex", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &objectUniqueId, &linkIndex, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	{
+		commandHandle = b3InitRequestCollisionShapeInformation(sm, objectUniqueId, linkIndex);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED)
+		{
+			b3GetCollisionShapeInformation(sm, &collisionShapeInfo);
+			pyResultList = PyTuple_New(collisionShapeInfo.m_numCollisionShapes);
+			for (i = 0; i < collisionShapeInfo.m_numCollisionShapes; i++)
+			{
+				PyObject* collisionShapeObList = PyTuple_New(7);
+				PyObject* item;
+
+				item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_objectUniqueId);
+				PyTuple_SetItem(collisionShapeObList, 0, item);
+
+				item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_linkIndex);
+				PyTuple_SetItem(collisionShapeObList, 1, item);
+
+				item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_collisionGeometryType);
+				PyTuple_SetItem(collisionShapeObList, 2, item);
+
+				{
+					PyObject* vec = PyTuple_New(3);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[0]);
+					PyTuple_SetItem(vec, 0, item);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[1]);
+					PyTuple_SetItem(vec, 1, item);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[2]);
+					PyTuple_SetItem(vec, 2, item);
+					PyTuple_SetItem(collisionShapeObList, 3, vec);
+				}
+
+				item = PyString_FromString(collisionShapeInfo.m_collisionShapeData[i].m_meshAssetFileName);
+				PyTuple_SetItem(collisionShapeObList, 4, item);
+
+				{
+					PyObject* vec = PyTuple_New(3);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[0]);
+					PyTuple_SetItem(vec, 0, item);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[1]);
+					PyTuple_SetItem(vec, 1, item);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[2]);
+					PyTuple_SetItem(vec, 2, item);
+					PyTuple_SetItem(collisionShapeObList, 5, vec);
+				}
+
+				{
+					PyObject* vec = PyTuple_New(4);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[3]);
+					PyTuple_SetItem(vec, 0, item);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[4]);
+					PyTuple_SetItem(vec, 1, item);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[5]);
+					PyTuple_SetItem(vec, 2, item);
+					item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[6]);
+					PyTuple_SetItem(vec, 3, item);
+					PyTuple_SetItem(collisionShapeObList, 6, vec);
+				}
+
+				
+				PyTuple_SetItem(pyResultList, i, collisionShapeObList);
+			}
+			return pyResultList;
+		}
+		else
+		{
+			PyErr_SetString(SpamError, "Error receiving collision shape info");
+			return NULL;
+		}
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
 static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int objectUniqueId = -1;
@@ -5564,7 +5703,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 
 			if (collisionFrameOrientationObj)
 			{
-				pybullet_internalSetVectord(collisionFrameOrientationObj,collisionFrameOrientation);
+				pybullet_internalSetVector4d(collisionFrameOrientationObj,collisionFrameOrientation);
 			}
 			b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition,collisionFrameOrientation);
 
@@ -5582,6 +5721,233 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 	return NULL;
 }
 
+static PyObject* pybullet_createCollisionShapeArray(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	PyObject* shapeTypeArray = 0;
+	PyObject* radiusArray = 0;
+	PyObject* halfExtentsObjArray = 0;
+	PyObject* lengthArray = 0;
+	PyObject* fileNameArray = 0;
+	PyObject* meshScaleObjArray = 0;
+	PyObject* planeNormalObjArray = 0;
+	PyObject* flagsArray = 0;
+	PyObject* collisionFramePositionObjArray = 0;
+	PyObject* collisionFrameOrientationObjArray = 0;
+	
+	static char* kwlist[] = { "shapeTypes", "radii", "halfExtents", "lengths", "fileNames", "meshScales", "planeNormals", 
+								"flags", "collisionFramePositions", "collisionFrameOrientations", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|OOOOOOOOOi", kwlist,
+		&shapeTypeArray, &radiusArray, &halfExtentsObjArray, &lengthArray, &fileNameArray, &meshScaleObjArray, &planeNormalObjArray, &flagsArray, &collisionFramePositionObjArray, &collisionFrameOrientationObjArray, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+
+	b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm);
+
+	{
+		int numShapeTypes = 0;
+		int numRadius = 0;
+		int numHalfExtents = 0;
+		int numLengths = 0;
+		int numFileNames = 0;
+		int numMeshScales = 0;
+		int numPlaneNormals = 0;
+		int numFlags = 0;
+		int numPositions = 0;
+		int numOrientations = 0;
+
+		int s;
+		PyObject* shapeTypeArraySeq = shapeTypeArray?PySequence_Fast(shapeTypeArray, "expected a sequence of shape types"):0;
+		PyObject* radiusArraySeq = radiusArray?PySequence_Fast(radiusArray, "expected a sequence of radii"):0;
+		PyObject* halfExtentsArraySeq = halfExtentsObjArray?PySequence_Fast(halfExtentsObjArray, "expected a sequence of half extents"):0;
+		PyObject* lengthArraySeq = lengthArray ?PySequence_Fast(lengthArray, "expected a sequence of lengths"):0;
+		PyObject* fileNameArraySeq = fileNameArray?PySequence_Fast(fileNameArray, "expected a sequence of filename"):0;
+		PyObject* meshScaleArraySeq = meshScaleObjArray?PySequence_Fast(meshScaleObjArray, "expected a sequence of mesh scale"):0;
+		PyObject* planeNormalArraySeq = planeNormalObjArray?PySequence_Fast(planeNormalObjArray, "expected a sequence of plane normal"):0;
+		PyObject* flagsArraySeq = flagsArray?PySequence_Fast(flagsArray, "expected a sequence of flags"):0;
+		PyObject* positionArraySeq = collisionFramePositionObjArray?PySequence_Fast(collisionFramePositionObjArray, "expected a sequence of collision frame positions"):0;
+		PyObject* orientationArraySeq = collisionFrameOrientationObjArray?PySequence_Fast(collisionFrameOrientationObjArray, "expected a sequence of collision frame orientations"):0;
+
+		if (shapeTypeArraySeq == 0)
+		{
+			PyErr_SetString(SpamError, "expected a sequence of shape types");
+			return NULL;
+		}
+
+		numShapeTypes = shapeTypeArray?PySequence_Size(shapeTypeArray):0;
+		numRadius = radiusArraySeq?PySequence_Size(radiusArraySeq):0;
+		numHalfExtents = halfExtentsArraySeq?PySequence_Size(halfExtentsArraySeq):0;
+		numLengths = lengthArraySeq?PySequence_Size(lengthArraySeq):0;
+		numFileNames = fileNameArraySeq?PySequence_Size(fileNameArraySeq):0;
+		numMeshScales = meshScaleArraySeq?PySequence_Size(meshScaleArraySeq):0;
+		numPlaneNormals = planeNormalArraySeq?PySequence_Size(planeNormalArraySeq):0;
+
+		for (s=0;s<numShapeTypes;s++)
+		{
+			int shapeType = pybullet_internalGetIntFromSequence(shapeTypeArraySeq, s);
+			if (shapeType >= GEOM_SPHERE)
+			{
+
+				int shapeIndex = -1;
+
+				if (shapeType == GEOM_SPHERE && s <= numRadius)
+				{
+					double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
+					if (radius > 0)
+					{
+						shapeIndex = b3CreateCollisionShapeAddSphere(commandHandle, radius);
+					}
+				}
+				if (shapeType == GEOM_BOX)
+				{
+					PyObject* halfExtentsObj = 0;
+					double halfExtents[3] = { 1, 1, 1 };
+
+					if (halfExtentsArraySeq && s<= numHalfExtents)
+					{
+						if (PyList_Check(halfExtentsArraySeq))
+						{
+							halfExtentsObj = PyList_GET_ITEM(halfExtentsArraySeq, s);
+						}
+						else
+						{
+							halfExtentsObj = PyTuple_GET_ITEM(halfExtentsArraySeq, s);
+						}
+					}
+					pybullet_internalSetVectord(halfExtentsObj, halfExtents);
+					shapeIndex = b3CreateCollisionShapeAddBox(commandHandle, halfExtents);
+				}
+				if (shapeType == GEOM_CAPSULE && s<=numRadius)
+				{
+					double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
+					double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
+					if (radius > 0 && height >= 0)
+					{
+						shapeIndex = b3CreateCollisionShapeAddCapsule(commandHandle, radius, height);
+					}
+				}
+				if (shapeType == GEOM_CYLINDER  && s <= numRadius && s<numLengths)
+				{
+					double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
+					double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
+					if (radius > 0 && height >= 0)
+					{
+						shapeIndex = b3CreateCollisionShapeAddCylinder(commandHandle, radius, height);
+					}
+				}
+				if (shapeType == GEOM_MESH)
+				{
+					double meshScale[3] = { 1, 1, 1 };
+
+					PyObject* meshScaleObj = meshScaleArraySeq?PyList_GET_ITEM(meshScaleArraySeq, s):0;
+					PyObject* fileNameObj = fileNameArraySeq?PyList_GET_ITEM(fileNameArraySeq, s):0;
+					const char* fileName = 0;
+
+					if (fileNameObj)
+					{
+#if PY_MAJOR_VERSION >= 3
+						PyObject* ob = PyUnicode_AsASCIIString(fileNameObj);
+						fileName = PyBytes_AS_STRING(ob);
+#else
+						fileName = PyString_AsString(fileNameObj);
+#endif
+					}
+					if (meshScaleObj)
+					{
+						pybullet_internalSetVectord(meshScaleObj, meshScale);
+					}
+					if (fileName)
+					{
+						shapeIndex = b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale);
+					}
+					
+				}
+				if (shapeType == GEOM_PLANE)
+				{
+					PyObject* planeNormalObj = planeNormalArraySeq?PyList_GET_ITEM(planeNormalArraySeq, s):0;
+					double planeNormal[3];
+					double planeConstant = 0;
+					pybullet_internalSetVectord(planeNormalObj, planeNormal);
+					shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant);
+				}
+				if (flagsArraySeq)
+				{
+					int flags = pybullet_internalGetIntFromSequence(flagsArraySeq, s);
+					b3CreateCollisionSetFlag(commandHandle, shapeIndex, flags);
+				}
+				if (positionArraySeq || orientationArraySeq)
+				{
+					PyObject* collisionFramePositionObj = positionArraySeq?PyList_GET_ITEM(positionArraySeq, s):0;
+					PyObject* collisionFrameOrientationObj = orientationArraySeq?PyList_GET_ITEM(orientationArraySeq, s):0;
+					double collisionFramePosition[3] = { 0, 0, 0 };
+					double collisionFrameOrientation[4] = { 0, 0, 0, 1 };
+					if (collisionFramePositionObj)
+					{
+						pybullet_internalSetVectord(collisionFramePositionObj, collisionFramePosition);
+					}
+
+					if (collisionFrameOrientationObj)
+					{
+						pybullet_internalSetVector4d(collisionFrameOrientationObj, collisionFrameOrientation);
+					}
+					if (shapeIndex >= 0)
+					{
+						b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation);
+					}
+
+				}
+			}
+
+
+
+		}
+
+		if (shapeTypeArraySeq)
+			Py_DECREF(shapeTypeArraySeq);
+		if (radiusArraySeq)
+			Py_DECREF(radiusArraySeq);
+		if (halfExtentsArraySeq)
+			Py_DECREF(halfExtentsArraySeq);
+		if (lengthArraySeq)
+			Py_DECREF(lengthArraySeq);
+		if (fileNameArraySeq)
+			Py_DECREF(fileNameArraySeq);
+		if (meshScaleArraySeq)
+			Py_DECREF(meshScaleArraySeq);
+		if (planeNormalArraySeq)
+			Py_DECREF(planeNormalArraySeq);
+		if (flagsArraySeq)
+			Py_DECREF(flagsArraySeq);
+		if (positionArraySeq)
+			Py_DECREF(positionArraySeq);
+		if (orientationArraySeq)
+			Py_DECREF(orientationArraySeq);
+
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
+		{
+			int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
+			PyObject* ob = PyLong_FromLong(uid);
+			return ob;
+		}
+	}
+
+	PyErr_SetString(SpamError, "createCollisionShapeArray failed.");
+	return NULL;
+}
 
 static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
 {
@@ -5591,7 +5957,7 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
 
 	int shapeType=-1;
 	double radius=0.5;
-	double height = 1;
+	double length = 1;
 	PyObject* meshScaleObj=0;
 	double meshScale[3] = {1,1,1};
 	PyObject* planeNormalObj=0;
@@ -5613,9 +5979,9 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
 	
 	PyObject* halfExtentsObj=0;
 
-	static char* kwlist[] = {"shapeType","radius","halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL};
+	static char* kwlist[] = {"shapeType","radius","halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist, 
-		&shapeType, &radius,&halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId))
+		&shapeType, &radius,&halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -5645,13 +6011,13 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
 			shapeIndex = b3CreateVisualShapeAddBox(commandHandle,halfExtents);
 		}
 	
-		if (shapeType==GEOM_CAPSULE && radius>0 && height>=0)
+		if (shapeType==GEOM_CAPSULE && radius>0 && length>=0)
 		{
-			shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle,radius,height);
+			shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle,radius,length);
 		}
-		if (shapeType==GEOM_CYLINDER && radius>0 && height>=0)
+		if (shapeType==GEOM_CYLINDER && radius>0 && length>=0)
 		{
-			shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle,radius,height);
+			shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle,radius,length);
 		}
 		if (shapeType==GEOM_MESH && fileName)
 		{
@@ -5692,7 +6058,7 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
 
 			if (visualFrameOrientationObj)
 			{
-				pybullet_internalSetVectord(visualFrameOrientationObj,visualFrameOrientation);
+				pybullet_internalSetVector4d(visualFrameOrientationObj,visualFrameOrientation);
 			}
 			b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, visualFramePosition,visualFrameOrientation);
 
@@ -7944,6 +8310,9 @@ static PyMethodDef SpamMethods[] = {
 	{"createCollisionShape", (PyCFunction)pybullet_createCollisionShape, METH_VARARGS | METH_KEYWORDS,
 	 "Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
 
+	{ "createCollisionShapeArray", (PyCFunction)pybullet_createCollisionShapeArray, METH_VARARGS | METH_KEYWORDS,
+	"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise." },
+
 	{"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS,
 	 "Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
 
@@ -8152,6 +8521,9 @@ static PyMethodDef SpamMethods[] = {
 	{"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS | METH_KEYWORDS,
 	 "Return the visual shape information for one object."},
 
+	{ "getCollisionShapeData", (PyCFunction)pybullet_getCollisionShapeData, METH_VARARGS | METH_KEYWORDS,
+		"Return the collision shape information for one object." },
+
 	{"changeVisualShape", (PyCFunction)pybullet_changeVisualShape, METH_VARARGS | METH_KEYWORDS,
 	 "Change part of the visual shape information for one object."},
 	
@@ -8370,8 +8742,7 @@ 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);

setup.py

@@ -443,7 +443,7 @@ print("-----")
 
 setup(
 	name = 'pybullet',
-	version='1.7.6',
+	version='1.8.0',
 	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/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -18,6 +18,11 @@ subject to the following restrictions:
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btSerializer.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define btCollisionObjectData btCollisionObjectDoubleData
+#else
+#define btCollisionObjectData btCollisionObjectFloatData
+#endif
 
 btScalar					gContactBreakingThreshold = btScalar(0.02);
 ContactDestroyedCallback	gContactDestroyedCallback = 0;
@@ -316,8 +321,8 @@ const char*	btPersistentManifold::serialize(const class btPersistentManifold* ma
 	btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
 	memset(dataOut, 0, sizeof(btPersistentManifoldData));
 
-	dataOut->m_body0 = serializer->getUniquePointer((void*)manifold->getBody0());
+	dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0());
-	dataOut->m_body1 = serializer->getUniquePointer((void*)manifold->getBody1());
+	dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1());
 	dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
 	dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
 	dataOut->m_numCachedPoints = manifold->getNumContacts();

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -24,6 +24,8 @@ class btCollisionObject;
 #include "LinearMath/btAlignedAllocator.h"
 
 struct btCollisionResult;
+struct btCollisionObjectDoubleData;
+struct btCollisionObjectFloatData;
 
 ///maximum contact breaking and merging threshold
 extern btScalar gContactBreakingThreshold;
@@ -310,8 +312,8 @@ struct btPersistentManifoldDoubleData
 	double	m_contactProcessingThreshold;
 	int m_padding;
 
-	void *m_body0;
+	btCollisionObjectDoubleData *m_body0;
-	void *m_body1;
+	btCollisionObjectDoubleData *m_body1;
 };
 
 
@@ -356,8 +358,8 @@ struct btPersistentManifoldFloatData
 	float	m_contactProcessingThreshold;
 	int m_padding;
 
-	void *m_body0;
+	btCollisionObjectFloatData *m_body0;
-	void *m_body1;
+	btCollisionObjectFloatData *m_body1;
 };
 
 #ifdef BT_USE_DOUBLE_PRECISION

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -2033,7 +2033,7 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 				memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
 				memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
 
-				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
+				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
 				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
 				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
 				btAssert(memPtr->m_dofCount<=3);

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -718,7 +718,7 @@ private:
 struct btMultiBodyLinkDoubleData
 {
 	btQuaternionDoubleData	m_zeroRotParentToThis;
-	btVector3DoubleData		m_parentComToThisComOffset;
+	btVector3DoubleData		m_parentComToThisPivotOffset;
 	btVector3DoubleData		m_thisPivotToThisComOffset;
 	btVector3DoubleData		m_jointAxisTop[6];
 	btVector3DoubleData		m_jointAxisBottom[6];
@@ -751,7 +751,7 @@ struct btMultiBodyLinkDoubleData
 struct btMultiBodyLinkFloatData
 {
 	btQuaternionFloatData	m_zeroRotParentToThis;
-	btVector3FloatData		m_parentComToThisComOffset;
+	btVector3FloatData		m_parentComToThisPivotOffset;
 	btVector3FloatData		m_thisPivotToThisComOffset;
 	btVector3FloatData		m_jointAxisTop[6];
 	btVector3FloatData		m_jointAxisBottom[6];

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -119,6 +119,14 @@ public:
 		return m_bodyB;
 	}
 
+	int getLinkA() const
+	{
+		return m_linkA;
+	}
+	int getLinkB() const
+	{
+		return m_linkB;
+	}
 	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
 	{
 		btAssert(dof>=0);

src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h

@@ -141,7 +141,7 @@ public:
 struct	btMultiBodyLinkColliderFloatData
 {
 	btCollisionObjectFloatData m_colObjData;
-	void		*m_multiBody;
+	btMultiBodyFloatData	*m_multiBody;
 	int			m_link;
 	char		m_padding[4];
 };
@@ -149,7 +149,7 @@ struct	btMultiBodyLinkColliderFloatData
 struct	btMultiBodyLinkColliderDoubleData
 {
 	btCollisionObjectDoubleData m_colObjData;
-	void		*m_multiBody;
+	btMultiBodyDoubleData		*m_multiBody;
 	int			m_link;
 	char		m_padding[4];
 };
@@ -165,7 +165,7 @@ SIMD_FORCE_INLINE	const char*	btMultiBodyLinkCollider::serialize(void* dataBuffe
 	btCollisionObject::serialize(&dataOut->m_colObjData,serializer);
 	
 	dataOut->m_link = this->m_link;
-	dataOut->m_multiBody = serializer->getUniquePointer(m_multiBody);
+	dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
 
 	// Fill padding with zeros to appease msan.
 	memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));