Fix pybullet Windows build errors: C99 requires variables to be defined at the start of the function.

Improve CMake Windows support to build PyBullet (BUILD_PYBULLET)
Implement b3LoadSdfCommandInit in shared memory API
Implement pybullet SDF loading binding, in loadSDF API
TODO for SDF support is provide way to query object/link/joint information.

CMakeLists.txt

@@ -191,14 +191,15 @@ IF (APPLE)
 ENDIF()
 
 OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON)
+
+FIND_PACKAGE(PythonLibs)
 OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF)
 
+
 IF(BUILD_PYBULLET)
 	IF(WIN32)
-		FIND_PACKAGE(PythonLibs 3.4 REQUIRED)
 		SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE)
 	ELSE(WIN32)
-		FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
 		SET(BUILD_SHARED_LIBS ON CACHE BOOL "Shared Libs" FORCE)
 	ENDIF(WIN32)
 ENDIF(BUILD_PYBULLET)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -519,6 +519,33 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
     return CMD_INVALID_STATUS;
 }
 
+int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
+{
+    int numBodies = 0;
+    const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+    b3Assert(status);
+	
+	if (status)
+	{
+			switch (status->m_type)
+			{
+				case CMD_SDF_LOADING_COMPLETED:
+				{
+				    int i,maxBodies;
+				    numBodies = status->m_sdfLoadedArgs.m_numBodies;
+				    maxBodies = btMin(bodyIndicesCapacity, numBodies);
+				    for (i=0;i<maxBodies;i++)
+				    {
+                            bodyIndicesOut[i] = status->m_sdfLoadedArgs.m_bodyUniqueIds[i];
+				    }
+					break;
+				}
+			}
+	}
+	
+	return numBodies;
+}
+
 int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -558,6 +585,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
                            const double* jointReactionForces[]) {
     const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
     const SendActualStateArgs &args = status->m_sendActualStateArgs;
+    btAssert(status->m_type == CMD_URDF_LOADING_COMPLETED);
+    if (status->m_type != CMD_URDF_LOADING_COMPLETED)
+        return false;
+    
     if (bodyUniqueId) {
         *bodyUniqueId = args.m_bodyUniqueId;
     }

examples/SharedMemory/PhysicsClientC_API.h

@@ -40,6 +40,8 @@ b3SharedMemoryStatusHandle	b3ProcessServerStatus(b3PhysicsClientHandle physClien
 /// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
 int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
 
+int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
+
 int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
 
 int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,

examples/SharedMemory/PhysicsClientExample.cpp

@@ -77,9 +77,9 @@ protected:
     
 	virtual void resetCamera()
 	{
-		float dist = 1.1;
+		float dist = 4;
-		float pitch = 50;
+		float pitch = 193;
-		float yaw = 35;
+		float yaw = 25;
 		float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
 		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
 
@@ -223,9 +223,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
     {
         case  CMD_LOAD_URDF:
         {
-            
             b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
-            
             //setting the initial position, orientation and other arguments are optional
             double startPosX = 0;
             static double startPosY = 0;
@@ -234,7 +232,13 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
 			startPosY += 2.f;
 //            ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
             b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
-
+            break;
+        }
+        
+        case  CMD_LOAD_SDF:
+        {
+            b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");
+            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
             break;
         }
         case CMD_REQUEST_CAMERA_IMAGE_DATA:
@@ -452,6 +456,7 @@ void	PhysicsClientExample::createButtons()
 		m_guiHelper->getParameterInterface()->removeAllParameters();
 
         createButton("Load URDF",CMD_LOAD_URDF,  isTrigger);
+        createButton("Load SDF",CMD_LOAD_SDF,  isTrigger);
         createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
         createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
         createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM,  isTrigger);
@@ -649,11 +654,40 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
                 b3Warning("Camera image FAILED\n");
             }
        
-        
+            if (statusType == CMD_SDF_LOADING_COMPLETED)
-      		if (statusType == CMD_URDF_LOADING_COMPLETED)
 			{
-				int bodyIndex = b3GetStatusBodyIndex(status);
+				//int bodyIndex = b3GetStatusBodyIndex(status);
-				if (bodyIndex>=0)
+				/*if (bodyIndex>=0)
+				{
+					int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
+            
+					for (int i=0;i<numJoints;i++)
+					{
+						b3JointInfo info;
+						b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
+						b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+					}
+					ComboBoxParams comboParams;
+					comboParams.m_comboboxId = bodyIndex;
+					comboParams.m_numItems = 1;
+					comboParams.m_startItem = 0;
+					comboParams.m_callback = MyComboBoxCallback;
+					comboParams.m_userPointer = this;
+					const char* bla = "bla";
+					const char* blarray[1];
+					blarray[0] = bla;
+				
+					comboParams.m_items=blarray;//{&bla};
+					m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+				}
+				*/
+			}
+			
+        
+      		if (statusType == CMD_URDF_LOADING_COMPLETED)
+			{
+				int bodyIndex = b3GetStatusBodyIndex(status);
+				if (bodyIndex>=0)
 				{
 					int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
             
@@ -662,7 +696,6 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 						b3JointInfo info;
 						b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
 						b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
-				
 					}
 					ComboBoxParams comboParams;
 					comboParams.m_comboboxId = bodyIndex;
@@ -676,12 +709,8 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 				
 					comboParams.m_items=blarray;//{&bla};
 					m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
-		
-
 				}
-
 			}
-    
 		}
 	}
     if (b3CanSubmitCommand(m_physicsClientHandle))
@@ -734,13 +763,12 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
             if (m_numMotors)
             {
                 enqueueCommand(CMD_SEND_DESIRED_STATE);
-                enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
-				if (m_options != eCLIENTEXAMPLE_SERVER)
-				{
-					enqueueCommand(CMD_REQUEST_DEBUG_LINES);
-				}
-                //enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
             }
+            enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
+			if (m_options != eCLIENTEXAMPLE_SERVER)
+			{
+				enqueueCommand(CMD_REQUEST_DEBUG_LINES);
+			}
         }
     }
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -40,7 +40,7 @@ struct PhysicsClientSharedMemoryInternalData {
     SharedMemoryStatus m_lastServerStatus;
 
     int m_counter;
-    bool m_serverLoadUrdfOK;
+    
     bool m_isConnected;
     bool m_waitingForServer;
     bool m_hasLastServerStatus;
@@ -54,7 +54,6 @@ struct PhysicsClientSharedMemoryInternalData {
 		  m_counter(0),
 		  m_cachedCameraPixelsWidth(0),
 		  m_cachedCameraPixelsHeight(0),
-	      m_serverLoadUrdfOK(false),
           m_isConnected(false),
           m_waitingForServer(false),
           m_hasLastServerStatus(false),
@@ -204,8 +203,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                 }
                 break;
             }
+            case CMD_SDF_LOADING_COMPLETED: {
+                
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server loading the SDF OK\n");
+                }
+                break;
+            }
             case CMD_URDF_LOADING_COMPLETED: {
-                m_data->m_serverLoadUrdfOK = true;
+                
                 if (m_data->m_verboseOutput) {
                     b3Printf("Server loading the URDF OK\n");
                 }
@@ -265,7 +271,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                 if (m_data->m_verboseOutput) {
                     b3Printf("Server failed loading the URDF...\n");
                 }
-                m_data->m_serverLoadUrdfOK = false;
+                
+                break;
+            }
+            
+             case CMD_SDF_LOADING_FAILED: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server failed loading the SDF...\n");
+                }
+                
                 break;
             }
 
@@ -490,6 +504,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
             m_data->m_waitingForServer = true;
         }
 
+        /*if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
+        {
+            int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
+            if (numBodies>0)
+            {
+                SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+                //now transfer the information of the individual objects etc.
+                command.m_type = CMD_REQUEST_SDF_INFO;
+                command.m_updateFlags = SDF_REQUEST_INFO_BODY;
+                command.m_sdfRequestInfoArgs.m_infoIndex = 0;
+                submitClientCommand(command);
+                return 0;    
+            }
+        }
+        */
+        
 		if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
 		{
 			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -393,6 +393,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btMultiBodyDynamicsWorld* m_dynamicsWorld;
 	SharedMemoryDebugDrawer*		m_remoteDebugDrawer;
 	
+	btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
     
 
     
@@ -681,6 +682,128 @@ void	PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
 }
 
 
+bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes)
+{
+    btAssert(m_data->m_dynamicsWorld);
+	if (!m_data->m_dynamicsWorld)
+	{
+		b3Error("loadSdf: No valid m_dynamicsWorld");
+		return false;
+	}
+	
+	m_data->m_sdfRecentLoadedBodies.clear();
+	
+    BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
+   
+    bool useFixedBase = false;
+    bool loadOk =  u2b.loadSDF(fileName, useFixedBase);
+    if (loadOk)
+    {
+        for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
+        {
+            btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
+            m_data->m_collisionShapes.push_back(shape);
+            if (shape->isCompound())
+            {
+                btCompoundShape* compound = (btCompoundShape*) shape;
+                for (int childIndex=0;childIndex<compound->getNumChildShapes();childIndex++)
+                {
+                    m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex));
+                }
+            }
+            
+        }
+        
+        btTransform rootTrans;
+        rootTrans.setIdentity();
+        if (m_data->m_verboseOutput)
+        {
+            b3Printf("loaded %s OK!", fileName);
+        }
+        
+        for (int m =0; m<u2b.getNumModels();m++)
+        {
+
+            u2b.activateModel(m);
+            btMultiBody* mb = 0;
+
+            //get a body index
+            int bodyUniqueId = m_data->allocHandle();
+            
+            InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
+
+            {
+                btScalar mass = 0;
+                bodyHandle->m_rootLocalInertialFrame.setIdentity();
+                btVector3 localInertiaDiagonal(0,0,0);
+                int urdfLinkIndex = u2b.getRootLinkIndex();
+                u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame);
+            }
+            
+            
+
+            //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
+            int rootLinkIndex = u2b.getRootLinkIndex();
+            b3Printf("urdf root link index = %d\n",rootLinkIndex);
+            MyMultiBodyCreator creation(m_data->m_guiHelper);
+
+            u2b.getRootTransformInWorld(rootTrans);
+            bool useMultiBody = true;
+            ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true);
+            
+           
+            
+            mb = creation.getBulletMultiBody();
+            if (mb)
+            {
+                bodyHandle->m_multiBody = mb;
+				
+				m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
+				
+				createJointMotors(mb);
+
+				//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
+				
+			    UrdfLinkNameMapUtil* util2 = new UrdfLinkNameMapUtil;
+			    m_data->m_urdfLinkNameMapper.push_back(util2);
+			    util2->m_mb = mb;
+			    util2->m_memSerializer = 0;
+			    //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
+
+                bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
+			    for (int i=0;i<mb->getNumLinks();i++)
+                {
+					//disable serialization of the collision objects
+                   
+				   int urdfLinkIndex = creation.m_mb2urdfLink[i];
+				   btScalar mass;
+                   btVector3 localInertiaDiagonal(0,0,0);
+                   btTransform localInertialFrame;
+				   u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
+				   bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame);
+
+				   std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
+				   m_data->m_strings.push_back(linkName);
+				   
+				   mb->getLink(i).m_linkName = linkName->c_str();
+
+				   std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str());
+				   m_data->m_strings.push_back(jointName);
+				   
+				   mb->getLink(i).m_jointName = jointName->c_str();
+                }
+				std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
+				m_data->m_strings.push_back(baseName);
+				mb->setBaseName(baseName->c_str());
+			} else
+			{
+				b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
+			}
+            
+        }
+    }
+    return loadOk;    
+}
 
 bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
                              bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes)
@@ -731,8 +854,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
         
         ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
         
-        
-        ///todo(erwincoumans) refactor this memory allocation issue
         for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
         {
             btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
@@ -751,11 +872,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
         btMultiBody* mb = creation.getBulletMultiBody();
 		if (useMultiBody)
 		{
-            
+			
-            
+			
 			if (mb)
 			{
+			    
 				bodyHandle->m_multiBody = mb;
+				
 				createJointMotors(mb);
 
 
@@ -805,12 +928,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
                 const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
                 util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
 
-                               return true;
+                return true;
 			} else
 			{
 				b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
 				return false;
 			}
+			
 		} else
 		{
 			btAssert(0);
@@ -1047,6 +1171,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					break;
 				}
 
+                case CMD_LOAD_SDF:
+                    {
+                        const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
+                        if (m_data->m_verboseOutput)
+                        {
+                            b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName);
+                        }
+                        
+                        bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes);
+                         
+                        //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
+                        serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
+                        int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
+                        for (int i=0;i<maxBodies;i++)
+                        {
+                            serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
+                        }
+                        
+                        serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
+						hasStatus = true;
+                        break;
+                    }
                 case CMD_LOAD_URDF:
                 {
 					

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -19,6 +19,10 @@ class PhysicsServerCommandProcessor
 protected:
 
 
+    
+
+    bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes);
+
 	bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
                              bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -33,6 +33,7 @@
 #define MAX_SDF_FILENAME_LENGTH 1024
 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
 #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
+#define MAX_SDF_BODIES 1024
 
 struct TmpFloat3 
 {
@@ -298,6 +299,28 @@ struct CreateBoxShapeArgs
 	double m_colorRGBA[4];
 };
 
+struct SdfLoadedArgs
+{
+    int m_numBodies;
+    int m_bodyUniqueIds[MAX_SDF_BODIES];
+    
+    ///@todo(erwincoumans) load cameras, lights etc
+    //int m_numCameras; 
+    //int m_numLights; 
+};
+
+
+struct SdfRequestInfoArgs
+{
+    int m_infoIndex;
+};
+
+enum EnumSdfRequestInfoFlags
+{
+    SDF_REQUEST_INFO_BODY=1,
+    //SDF_REQUEST_INFO_CAMERA=2,
+};
+
 struct SharedMemoryCommand
 {
 	int m_type;
@@ -312,6 +335,7 @@ struct SharedMemoryCommand
     {
         struct UrdfArgs m_urdfArguments;
 		struct SdfArgs m_sdfArguments;
+		struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
 		struct InitPoseArgs m_initPoseArgs;
 		struct SendPhysicsSimulationParameters m_physSimParamArgs;
 		struct BulletDataStreamArgs	m_dataStreamArguments;
@@ -340,6 +364,7 @@ struct SharedMemoryStatus
 	union
 	{
 		struct BulletDataStreamArgs	m_dataStreamArguments;
+		struct SdfLoadedArgs m_sdfLoadedArgs;
 		struct SendActualStateArgs m_sendActualStateArgs;
 		struct SendDebugLinesArgs m_sendDebugLinesArgs;
 		struct SendPixelDataArgs m_sendPixelDataArguments;

examples/SharedMemory/SharedMemoryPublic.h

@@ -19,6 +19,7 @@ enum EnumSharedMemoryClientCommand
         CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
         CMD_REQUEST_ACTUAL_STATE,
         CMD_REQUEST_DEBUG_LINES,
+    CMD_REQUEST_SDF_INFO,
     CMD_STEP_FORWARD_SIMULATION,
     CMD_RESET_SIMULATION,
     CMD_PICK_BODY,

examples/pybullet/pybullet.c

@@ -182,6 +182,61 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
 	return PyLong_FromLong(bodyIndex);
 }
 
+#define MAX_SDF_BODIES 512
+
+static PyObject*
+pybullet_loadSDF(PyObject* self, PyObject* args)
+{
+	const char* sdfFileName="";
+	int size= PySequence_Size(args);
+	int numBodies  = 0;
+	int i;
+	int bodyIndicesOut[MAX_SDF_BODIES];
+	PyObject* pylist=0;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+
+	if (0==sm)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	if (size==1)
+	{
+		if (!PyArg_ParseTuple(args, "s", &sdfFileName))
+		return NULL;
+	}
+
+	command = b3LoadSdfCommandInit(sm, sdfFileName);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType!=CMD_SDF_LOADING_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Cannot load SDF file.");
+		return NULL;
+	}
+
+	numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
+	if (numBodies > MAX_SDF_BODIES)
+	{
+		PyErr_SetString(SpamError, "SDF exceeds body capacity");
+		return NULL;
+	}
+
+	pylist = PyTuple_New(numBodies);
+
+	if (numBodies >0 && numBodies <= MAX_SDF_BODIES)	
+	{
+		for (i=0;i<numBodies;i++)
+		{
+			PyTuple_SetItem(pylist,i,PyInt_FromLong(bodyIndicesOut[i]));
+		}
+	}
+	return pylist;
+}
+
 // Reset the simulation to remove all loaded objects
 static PyObject *
 pybullet_resetSimulation(PyObject* self, PyObject* args)
@@ -287,24 +342,26 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
 static PyObject *
 pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
 {
+	int bodyIndex = -1;
+	double basePosition[3];
+    double baseOrientation[4];
+	PyObject *pylistPos;
+    PyObject *pylistOrientation;
+
     if (0==sm)
     {
         PyErr_SetString(SpamError, "Not connected to physics server.");
         return NULL;
     }
     
-    int bodyIndex = -1;
     if (!PyArg_ParseTuple(args, "i", &bodyIndex ))
     {
         PyErr_SetString(SpamError, "Expected a body index (integer).");
         return NULL;
     }
     
-    double basePosition[3];
-    double baseOrientation[4];
-    
     pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation);
-    PyObject *pylistPos; 
+    
     {
     
         PyObject *item; 
@@ -318,7 +375,7 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
         }
     
     }
-    PyObject *pylistOrientation; 
+
     {
         
         PyObject *item; 
@@ -446,13 +503,6 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16])
 //  to see object based on camera position?
 static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
 {
-
-  if (0==sm)
-  {
-  	PyErr_SetString(SpamError, "Not connected to physics server.");
-  	return NULL;
-  }
-
   ///request an image from a simulated camera, using a software renderer.
   struct b3CameraImageData imageData;
   PyObject* objViewMat,* objProjMat;
@@ -463,6 +513,14 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
 
   // inialize cmd
   b3SharedMemoryCommandHandle command;
+  
+
+  if (0==sm)
+  {
+  	PyErr_SetString(SpamError, "Not connected to physics server.");
+  	return NULL;
+  }
+
   command = b3InitRequestCameraImage(sm);
 
   if (size==2) // only set camera resolution
@@ -498,6 +556,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
 		{
 			PyObject *item2;
 			PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
+			PyObject *pylistRGB;
+			PyObject* pylistDep;
+			int i, j, p;
 
 			b3GetCameraImageData(sm, &imageData);
 			//TODO(hellojas): error handling if image size is 0
@@ -505,9 +566,6 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
 			PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
 			PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
 
-			PyObject *pylistRGB;
-			PyObject* pylistDep;
-			int i, j, p;
 
 			{
 
@@ -549,6 +607,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
 static PyMethodDef SpamMethods[] = {
   {"loadURDF",  pybullet_loadURDF, METH_VARARGS,
       "Create a multibody by loading a URDF file."},
+      
+	{"loadSDF", pybullet_loadSDF, METH_VARARGS,
+		"Load multibodies from an SDF file."},
   
   {"connect",  pybullet_connectPhysicsServer, METH_VARARGS,
       "Connect to an existing physics server (using shared memory by default)."},