Start of a urdfEditor.py, limited support to extract a URDF from a PyBullet body.

Use btCylinderShapeZ for URDF cylinder, instead of converting it to a btConvexHullShape. Implement PyBullet.getCollisionShapeData Extend PyBullet.getDynamicsInfo / b3GetDynamicsInfo, remove flag (don't rely on API returning a fixed number of elements in a list!) Extend PyBullet.getJointInfo: add parentIndex
2018-01-03 19:17:28 -08:00
parent a1492f50fc
commit 79d78a325a
16 changed files with 858 additions and 45 deletions
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -599,8 +599,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 +620,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
 			cylZShape->optimizeConvexHull();
 			
 			//btConvexShape* cylZShape = new btConeShapeZ(cylRadius,cylLength);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-            
-            //btVector3 halfExtents(cylRadius,cylRadius,cylLength);
-            //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
-            
+#else       
+            btVector3 halfExtents(cylRadius,cylRadius, cylHalfLength);
+            btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
+#endif       

            shape = cylZShape;
            break;
--- a/examples/SharedMemory/BodyJointInfoUtility.h
+++ b/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_parentComToThisComOffset.m_floats[0];
+			info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisComOffset.m_floats[1];
+			info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisComOffset.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)) {
--- a/examples/SharedMemory/PhysicsClient.h
+++ b/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;
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -2141,13 +2141,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;
-	info->m_localInertialDiagonal[0] = dynamicsInfo.m_localInertialDiagonal[0];
-	info->m_localInertialDiagonal[1] = dynamicsInfo.m_localInertialDiagonal[1];
-	info->m_localInertialDiagonal[2] = dynamicsInfo.m_localInertialDiagonal[2];
-
-	info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff;
-	return true;
+	if (info)
+	{
+		*info = dynamicsInfo;
+		return true;
+	}
+	return false;
 }

 B3_SHARED_API	b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)
@@ -3418,6 +3417,30 @@ B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient
 }


+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA, 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 = bodyUniqueIdA;
+	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)
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/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);

--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/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()) {
--- a/examples/SharedMemory/PhysicsClientSharedMemory.h
+++ b/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);
--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/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();
--- a/examples/SharedMemory/PhysicsDirect.h
+++ b/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);
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -6237,6 +6237,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
 		{
@@ -6245,9 +6268,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
 			{
@@ -7967,7 +8012,144 @@ 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:
+	{
+		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 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:
+	{
+		//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;
@@ -8677,6 +8859,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);
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.h
+++ b/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& childTransform, 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,
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -2542,14 +2542,14 @@ void PhysicsServerExample::drawUserDebugLines()
 			}
 			
 			float colorRGBA[4] = {
-				m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
-				m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
-				m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
-				1.};
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
+				(float)1.};

-			float pos[3] = {m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
-			m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
-			m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};
+			float pos[3] = { (float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
+				(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
+				(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,
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/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
 {
@@ -993,6 +1004,7 @@ struct SharedMemoryCommand
 		struct b3SearchPathfArgs m_searchPathArgs;
 		struct b3CustomCommand m_customCommandArgs;
 		struct b3StateSerializationArguments m_loadStateArguments;
+		struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments;		
    };
 };

@@ -1068,6 +1080,7 @@ struct SharedMemoryStatus
 		struct b3CustomCommandResultArgs m_customCommandResultArgs;
 		struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
 		struct b3StateSerializationArguments m_saveStateResultArgs;
+		struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
 	};
 };

--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -80,6 +80,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 +186,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 +220,6 @@ enum JointType {
 	eGearType=6
 };

-enum b3RequestDynamicsInfoFlags
-{
-	eDYNAMICS_INFO_REPORT_INERTIA=1,
-};

 enum b3JointInfoFlags
 {
@@ -247,6 +246,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 +278,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 +538,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:
+	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,
--- a/examples/pybullet/examples/urdfEditor.py
+++ b/examples/pybullet/examples/urdfEditor.py
@@ -0,0 +1,325 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+
+door = p.loadURDF("door.urdf")
+
+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_meshfile = "meshfile"
+		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_extents = [7,8,9]
+		self.geom_meshfile = "meshfile"
+	
+class UrdfJoint(object):
+	def __init__(self):
+		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_axis_xyz = [1,2,3]
+		 
+class UrdfEditor(object):
+	def __init__(self):
+		self.initialize()
+
+	def initialize(self):
+		self.urdfLinks=[]
+		self.urdfJoints=[]
+		self.robotName = ""
+		
+	#def addLink(...)
+
+	#def createMultiBody(self):
+		
+	def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink):
+		dyn = p.getDynamicsInfo(bodyUid,linkIndex)
+		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)
+		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_meshfile = v[4].decode("utf-8")
+				if (v[2]==p.GEOM_CYLINDER):
+					urdfVisual.geom_radius=v[3][1]
+					urdfVisual.geom_length=v[3][0]
+					
+				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)
+		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_meshfile = v[4].decode("utf-8")
+			#localInertiaFrame*childTrans
+			if (v[2]==p.GEOM_CYLINDER):
+				urdfCollision.geom_radius=v[3][1]
+				urdfCollision.geom_length=v[3][0]
+
+			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):
+		self.initialize()
+
+		#always create a base link
+		baseLink = UrdfLink()
+		baseLinkIndex = -1
+		self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink)
+		baseLink.link_name = 	p.getBodyInfo(bodyUid)[0].decode("utf-8") 		
+		self.urdfLinks.append(baseLink)
+	
+		#print(visualShapes)
+		#optionally create child links and joints
+		for j in range(p.getNumJoints(bodyUid)):
+			jointInfo = p.getJointInfo(bodyUid,j)
+			urdfLink = UrdfLink()
+			self.convertLinkFromMultiBody(bodyUid, j, urdfLink)
+			urdfLink.link_name = jointInfo[12].decode("utf-8")
+			self.urdfLinks.append(urdfLink)
+	
+			urdfJoint = UrdfJoint()
+			urdfJoint.joint_name = jointInfo[1].decode("utf-8")
+			urdfJoint.joint_type = jointInfo[2]
+			urdfJoint.joint_axis_xyz = jointInfo[13]
+			parentIndex = jointInfo[16]
+			if (parentIndex<0):
+				urdfJoint.parent_name = baseLink.link_name
+			else:
+				parentJointInfo = p.getJointInfo(bodyUid,parentIndex)
+				urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
+
+			urdfJoint.child_name = urdfLink.link_name
+
+			#todo, compensate for inertia/link frame offset
+			dyn = p.getDynamicsInfo(bodyUid,parentIndex)
+			parentInertiaPos = dyn[3]
+			parentInertiaOrn = dyn[4]
+			
+			pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
+				jointInfo[14], jointInfo[15])
+			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_meshfile,\
+				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)
+
+		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_meshfile,\
+				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)
+	
+		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)
+		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 __del__(self):
+		pass
+      
+parser = UrdfEditor()
+parser.initializeFromBulletBody(door)
+parser.saveUrdf("test.urdf")
+parser=0
+
+p.setRealTimeSimulation(1)
+print("numJoints:",p.getNumJoints(door))
+
+print("base name:",p.getBodyInfo(door))
+
+for i in range(p.getNumJoints(door)):
+	print("jointInfo(",i,"):",p.getJointInfo(door,i))
+	print("linkState(",i,"):",p.getLinkState(door,i))
+
+while (p.getConnectionInfo()["isConnected"]):
+	time.sleep(0.01)
+		
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -913,13 +913,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};
-		if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex, &flags, &physicsClientId))
+		static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex,  &physicsClientId))
 		{
 			return NULL;
 		}
@@ -934,11 +933,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 +954,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,1,PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
-					PyTuple_SetItem(pyInertiaDiag,2,PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
+					PyTuple_SetItem(pyInertiaDiag, 0, PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
+					PyTuple_SetItem(pyInertiaDiag, 1, PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
+					PyTuple_SetItem(pyInertiaDiag, 2, PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
 					PyTuple_SetItem(pyDynamicsInfo, 2, pyInertiaDiag);
 				}
-				
+				{
+					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;
 			}
 		}
@@ -3067,7 +3078,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};
@@ -3138,6 +3149,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;
 			}
@@ -4773,6 +4808,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;
@@ -8088,6 +8224,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."},
 	
@@ -8306,8 +8445,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);