From 1c7f87aff100b472c2ecc1cb383d9d12556ecd60 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 6 Jun 2016 18:54:05 -0700 Subject: [PATCH] implement first draft of pybullet.renderImage for synthetic camera remove a few debug printf from tinyrenderer --- .../SharedMemory/PhysicsClientExample.cpp | 14 +- .../PhysicsClientSharedMemory.cpp | 4 +- .../PhysicsServerCommandProcessor.cpp | 2 +- .../TinyRendererVisualShapeConverter.cpp | 2 +- examples/TinyRenderer/TinyRenderer.cpp | 2 +- examples/pybullet/CMakeLists.txt | 10 +- examples/pybullet/pybullet.c | 161 +++++++++++++++++- 7 files changed, 180 insertions(+), 15 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 15c90748a..30da039b3 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -612,9 +612,9 @@ void PhysicsClientExample::stepSimulation(float deltaTime) } if (statusType ==CMD_CAMERA_IMAGE_COMPLETED) { - static int counter=0; - char msg[1024]; - sprintf(msg,"Camera image %d OK\n",counter++); + // static int counter=0; + // char msg[1024]; + // 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); - b3Printf("Status after reset: %d",statusType); + //int statusType = b3GetStatusType(status); + //b3Printf("Status after reset: %d",statusType); } } } else diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c0ecc0a19..82062b0c8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/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;im_swRenderInstances.size()); + // printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); for (int i=0;im_swRenderInstances.size();i++) { TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index e7dfb8f05..6b9fdbb35 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/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; infaces(); i++) { diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index 5fd6b106a..672187557 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/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 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 3286e5cf2..43ac64db9 100644 --- a/examples/pybullet/pybullet.c +++ b/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