implement first draft of pybullet.renderImage for synthetic camera

remove a few debug printf from tinyrenderer

examples/SharedMemory/PhysicsClientExample.cpp

@@ -612,9 +612,9 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 			}
 			if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
             {
-				static int counter=0;
+			//	static int counter=0;
-				char msg[1024];
+			//	char msg[1024];
-				sprintf(msg,"Camera image %d OK\n",counter++);
+			//	sprintf(msg,"Camera image %d OK\n",counter++);
 				b3CameraImageData imageData;
 				b3GetCameraImageData(m_physicsClientHandle,&imageData);
 				if (m_canvas && m_canvasIndex >=0)
@@ -642,11 +642,11 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 					m_canvas->refreshImageData(m_canvasIndex);
 				}
 
-                b3Printf(msg);
+               // b3Printf(msg);
             } 
             if (statusType == CMD_CAMERA_IMAGE_FAILED)
             {
-                b3Printf("Camera image FAILED\n");
+                b3Warning("Camera image FAILED\n");
             }
        
         
@@ -716,8 +716,8 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 						bool hasStatus = (status != 0);
 						if (hasStatus)
 						{
-							int statusType = b3GetStatusType(status);
+							//int statusType = b3GetStatusType(status);
-							b3Printf("Status after reset: %d",statusType);
+							//b3Printf("Status after reset: %d",statusType);
 						}
 					}
 				} else

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -448,7 +448,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                 
 				unsigned char* rgbaPixelsReceived =
                     (unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
-                printf("pixel = %d\n", rgbaPixelsReceived[0]);
+              //  printf("pixel = %d\n", rgbaPixelsReceived[0]);
                 
 				for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
 				{
@@ -461,7 +461,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
             
             case CMD_CAMERA_IMAGE_FAILED:
             {
-                b3Printf("Camera image FAILED\n");
+                b3Warning("Camera image FAILED\n");
                 break;
             }
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1003,7 +1003,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                             
                             if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0)
                             {
-                                printf("-------------------------------\nRendering\n");
+                             //   printf("-------------------------------\nRendering\n");
                                 
                                 if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0)
                                 {

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -541,7 +541,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
     
     lightDirWorld.normalize();
     
-    printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
+  //  printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
     for (int i=0;i<m_data->m_swRenderInstances.size();i++)
     {
         TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);

examples/TinyRenderer/TinyRenderer.cpp

@@ -265,7 +265,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
         Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
         Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA());
         		
-		printf("Render %d triangles.\n",model->nfaces());
+		//printf("Render %d triangles.\n",model->nfaces());
 		for (int i=0; i<model->nfaces(); i++) 
 		{
 			 

examples/pybullet/CMakeLists.txt

@@ -9,6 +9,15 @@ INCLUDE_DIRECTORIES(
 SET(pybullet_SRCS
 	pybullet.c
 	../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
+      ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+                                                                                                ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
+                                                                                                ../../examples/OpenGLWindow/SimpleCamera.cpp
+                                                                                                ../../examples/OpenGLWindow/SimpleCamera.h
+                                                                                                ../../examples/TinyRenderer/geometry.cpp
+                                                                                                ../../examples/TinyRenderer/model.cpp
+                                                                                                ../../examples/TinyRenderer/tgaimage.cpp
+                                                                                                ../../examples/TinyRenderer/our_gl.cpp
+                                                                                                ../../examples/TinyRenderer/TinyRenderer.cpp
                         ../../examples/SharedMemory/InProcessMemory.cpp
                         ../../examples/SharedMemory/PhysicsClient.cpp
                         ../../examples/SharedMemory/PhysicsClient.h
@@ -48,7 +57,6 @@ SET(pybullet_SRCS
                         ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
                         ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
                         ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
-			../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp
                         ../../examples/MultiThreading/b3PosixThreadSupport.cpp
                         ../../examples/MultiThreading/b3Win32ThreadSupport.cpp
 			../../examples/MultiThreading/b3ThreadSupportInterface.cpp

examples/pybullet/pybullet.c

@@ -134,7 +134,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
 	float startOrnY = 0;
 	float startOrnZ = 0;
 	float startOrnW = 1;
-	printf("size=%d\n", size);
+	//printf("size=%d\n", size);
 	if (0==sm)
     {
         PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -164,7 +164,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
         b3SharedMemoryStatusHandle statusHandle;
         int statusType;
         b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
-		printf("urdf filename = %s\n", urdfFileName);
+		//printf("urdf filename = %s\n", urdfFileName);
         //setting the initial position, orientation and other arguments are optional
         b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
         statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -359,6 +359,157 @@ pybullet_getNumJoints(PyObject* self, PyObject* args)
 	}
 }
 
+static PyObject*
+pybullet_setJointPositions(PyObject* self, PyObject* args)
+{
+	
+	Py_INCREF(Py_None);
+        return Py_None;
+}
+
+       // const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes
+       // const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats
+
+static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
+{
+					
+	///request an image from a simulated camera, using a software renderer.
+	struct b3CameraImageData imageData;
+	PyObject* objViewMat,* objProjMat;
+	
+	b3SharedMemoryCommandHandle command = b3InitRequestCameraImage(sm);
+	
+	if (PyArg_ParseTuple(args, "OO", &objViewMat, &objProjMat))
+	{
+    		PyObject* seq;
+    		int i, len;
+		PyObject* item;
+		float viewMatrix[16];
+		float projectionMatrix[16];
+		int valid = 1;
+		{
+    		seq = PySequence_Fast(objViewMat, "expected a sequence");
+    		len = PySequence_Size(objViewMat);
+		//printf("objViewMat size = %d\n", len);
+		if (len==16)
+		{
+				
+		if (PyList_Check(seq))
+		{
+			for (i = 0; i < len; i++) 
+			{
+            			item = PyList_GET_ITEM(seq, i);
+				viewMatrix[i] = PyFloat_AsDouble(item);
+      				float v = viewMatrix[i]; 
+	 			//printf("view %d to %f\n", i,v);
+	
+			}
+		}
+		else
+		{
+    			for (i = 0; i < len; i++)
+			{
+        			item = PyTuple_GET_ITEM(seq,i);
+				viewMatrix[i] = PyFloat_AsDouble(item);
+    			}
+		}
+		} else
+		{
+			valid = 0;
+		}
+		}
+		
+		{
+                seq = PySequence_Fast(objProjMat, "expected a sequence");
+                len = PySequence_Size(objProjMat);
+		//printf("projMat len = %d\n", len);
+                if (len==16)
+                {
+                if (PyList_Check(seq))
+                {
+                        for (i = 0; i < len; i++)
+                        {
+                                item = PyList_GET_ITEM(seq, i);
+                                projectionMatrix[i] = PyFloat_AsDouble(item);
+                        }
+                }
+                else
+                {
+                        for (i = 0; i < len; i++)
+                        {
+                                item = PyTuple_GET_ITEM(seq,i);
+                                projectionMatrix[i] = PyFloat_AsDouble(item);
+                        }
+                }
+                } else
+                {
+                        valid = 0;
+                }
+                }
+
+
+    		Py_DECREF(seq);	
+		if (valid)
+		{
+			//printf("set b3RequestCameraImageSetCameraMatrices\n");
+			b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix);
+		}
+	}
+
+	if (b3CanSubmitCommand(sm))
+        {
+	    b3SharedMemoryStatusHandle statusHandle;
+	    int statusType;
+            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+            statusType = b3GetStatusType(statusHandle);
+	    if (statusType==CMD_CAMERA_IMAGE_COMPLETED)
+		{
+			PyObject *item2;
+			PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
+
+			b3GetCameraImageData(sm, &imageData);
+			//todo: error handling if image size is 0
+			pyResultList =  PyTuple_New(4);
+			PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
+			PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));	
+	
+			PyObject *pylistPos;
+			PyObject* pylistDep;
+
+        		//printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight);
+    			{
+
+        			PyObject *item;
+				int bytesPerPixel = 3;//Red, Green, Blue, each 8 bit values
+        			int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight;
+        			pylistPos = PyTuple_New(num);
+				pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
+	
+				for (int i=0;i<imageData.m_pixelWidth;i++)
+				{
+					for (int j=0;j<imageData.m_pixelHeight;j++)
+					{
+						int depIndex = i+j*imageData.m_pixelWidth;
+						item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
+						PyTuple_SetItem(pylistDep, depIndex, item);
+						for (int p=0;p<bytesPerPixel;p++)
+						{
+							int pixelIndex = bytesPerPixel*(i+j*imageData.m_pixelWidth)+p;
+							item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]);
+            						PyTuple_SetItem(pylistPos, pixelIndex, item);
+						}
+					}
+				}	
+			}
+			PyTuple_SetItem(pyResultList, 2,pylistPos);
+			PyTuple_SetItem(pyResultList, 3,pylistDep);
+			return pyResultList;	
+		}	
+        }
+
+	Py_INCREF(Py_None);
+        return Py_None;
+}
 
 static PyMethodDef SpamMethods[] = {
     {"loadURDF",  pybullet_loadURDF, METH_VARARGS,
@@ -379,6 +530,12 @@ static PyMethodDef SpamMethods[] = {
 	{"setGravity",  pybullet_setGravity, METH_VARARGS,
         "Set the gravity acceleration (x,y,z)."},
 
+	{"initializeJointPositions", pybullet_setJointPositions, METH_VARARGS,
+	"Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."},
+	
+	{"renderImage", pybullet_renderImage, METH_VARARGS,
+	"Render an image (given the camera view & projection matrices and resolution), and return the 8-8-8bit RGB pixel data and floating point depth values"},	
+	
 	{"getBasePositionAndOrientation",  pybullet_getBasePositionAndOrientation, METH_VARARGS,
         "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},