diff --git a/CMakeLists.txt b/CMakeLists.txt index a0cd0484d..a73cad8e1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -194,11 +194,28 @@ 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) + + FIND_PACKAGE(PythonLibs) + + OPTION(BUILD_PYBULLET_NUMPY "Set when you want to build pybullet with NumPy support" OFF) + + IF(BUILD_PYBULLET_NUMPY) + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_LIST_DIR}/build3/cmake) + #include(FindNumPy) + FIND_PACKAGE(NumPy) + if (PYTHON_NUMPY_FOUND) + message("NumPy found") + add_definitions(-DPYBULLET_USE_NUMPY) + else() + message("NumPy not found") + endif() + ENDIF() + OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF) + IF(WIN32) SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE) ELSE(WIN32) diff --git a/build3/cmake/FindNumPy.cmake b/build3/cmake/FindNumPy.cmake new file mode 100644 index 000000000..4d729b2a3 --- /dev/null +++ b/build3/cmake/FindNumPy.cmake @@ -0,0 +1,41 @@ +# Find the Python NumPy package +# PYTHON_NUMPY_INCLUDE_DIR +# PYTHON_NUMPY_FOUND +# will be set by this script + +cmake_minimum_required(VERSION 2.6) + +if(NOT PYTHON_EXECUTABLE) + if(NumPy_FIND_QUIETLY) + find_package(PythonInterp QUIET) + else() + find_package(PythonInterp) + set(__numpy_out 1) + endif() +endif() + +if (PYTHON_EXECUTABLE) + # Find out the include path + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c + "from __future__ import print_function\ntry: import numpy; print(numpy.get_include(), end='')\nexcept:pass\n" + OUTPUT_VARIABLE __numpy_path) + # And the version + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c + "from __future__ import print_function\ntry: import numpy; print(numpy.__version__, end='')\nexcept:pass\n" + OUTPUT_VARIABLE __numpy_version) +elseif(__numpy_out) + message(STATUS "Python executable not found.") +endif(PYTHON_EXECUTABLE) + +find_path(PYTHON_NUMPY_INCLUDE_DIR numpy/arrayobject.h + HINTS "${__numpy_path}" "${PYTHON_INCLUDE_PATH}" NO_DEFAULT_PATH) + +if(PYTHON_NUMPY_INCLUDE_DIR) + set(PYTHON_NUMPY_FOUND 1 CACHE INTERNAL "Python numpy found") +endif(PYTHON_NUMPY_INCLUDE_DIR) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(NumPy REQUIRED_VARS PYTHON_NUMPY_INCLUDE_DIR + VERSION_VAR __numpy_version) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index f84e7356c..d75b8e4ff 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1131,7 +1131,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt else { - if ((button == 33)) + if (button == 33) { m_args[0].m_isVrControllerPicking[controllerId] = (state != 0); m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0); diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index 1a0dad330..ad5d5f8e1 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -15,7 +15,6 @@ #include //const char* blaatnaam = "basename"; -#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE 1024 struct MyMotorInfo { diff --git a/examples/SharedMemory/SharedMemoryBlock.h b/examples/SharedMemory/SharedMemoryBlock.h index b737765ea..f266db496 100644 --- a/examples/SharedMemory/SharedMemoryBlock.h +++ b/examples/SharedMemory/SharedMemoryBlock.h @@ -3,7 +3,6 @@ #define SHARED_MEMORY_MAGIC_NUMBER 64738 #define SHARED_MEMORY_MAX_COMMANDS 4 -#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) #include "SharedMemoryCommands.h" diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index f49c97b25..dbac56750 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -5,6 +5,11 @@ INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs ${PYTHON_INCLUDE_DIRS} ) +IF(BUILD_PYBULLET_NUMPY) + INCLUDE_DIRECTORIES( + ${PYTHON_NUMPY_INCLUDE_DIR} + ) +ENDIF() SET(pybullet_SRCS pybullet.c diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 69df944ce..221f310b9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2,12 +2,17 @@ #include "../SharedMemory/PhysicsDirectC_API.h" #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" + #ifdef __APPLE__ #include #else #include #endif +#ifdef PYBULLET_USE_NUMPY +#include +#endif + #if PY_MAJOR_VERSION >= 3 #define PyInt_FromLong PyLong_FromLong #define PyString_FromString PyBytes_FromString @@ -1275,7 +1280,43 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { PyObject* item2; PyObject* pyResultList; // store 4 elements in this result: width, // height, rgbData, depth - PyObject* pylistRGB; + +#ifdef PYBULLET_USE_NUMPY + PyObject* pyRGB; + PyObject* pyDep; + PyObject* pySeg; + + int i, j, p; + + b3GetCameraImageData(sm, &imageData); + // TODO(hellojas): error handling if image size is 0 + pyResultList = PyTuple_New(5); + PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth)); + PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight)); + + int bytesPerPixel = 4; // Red, Green, Blue, and Alpha each 8 bit values + + npy_intp rgb_dims[3] = {imageData.m_pixelHeight, imageData.m_pixelWidth, + bytesPerPixel}; + npy_intp dep_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; + npy_intp seg_dims[2] = {imageData.m_pixelHeight, imageData.m_pixelWidth}; + + pyRGB = PyArray_SimpleNew(3, rgb_dims, NPY_UINT8); + pyDep = PyArray_SimpleNew(2, dep_dims, NPY_FLOAT32); + pySeg = PyArray_SimpleNew(2, seg_dims, NPY_INT32); + + memcpy(PyArray_DATA(pyRGB), imageData.m_rgbColorData, + imageData.m_pixelHeight * imageData.m_pixelWidth * bytesPerPixel); + memcpy(PyArray_DATA(pyDep), imageData.m_depthValues, + imageData.m_pixelHeight * imageData.m_pixelWidth); + memcpy(PyArray_DATA(pySeg), imageData.m_segmentationMaskValues, + imageData.m_pixelHeight * imageData.m_pixelWidth); + + PyTuple_SetItem(pyResultList, 2, pyRGB); + PyTuple_SetItem(pyResultList, 3, pyDep); + PyTuple_SetItem(pyResultList, 4, pySeg); +#else//PYBULLET_USE_NUMPY + PyObject* pylistRGB; PyObject* pylistDep; PyObject* pylistSeg; @@ -1324,6 +1365,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) { PyTuple_SetItem(pyResultList, 2, pylistRGB); PyTuple_SetItem(pyResultList, 3, pylistDep); PyTuple_SetItem(pyResultList, 4, pylistSeg); + return pyResultList; +#endif//PYBULLET_USE_NUMPY + return pyResultList; } } @@ -1777,7 +1821,11 @@ static PyMethodDef SpamMethods[] = { {"renderImage", pybullet_renderImage, METH_VARARGS, "Render an image (given the pixel resolution width, height, camera view " "matrix, projection matrix, near, and far values), and return the " - "8-8-8bit RGB pixel data and floating point depth values"}, + "8-8-8bit RGB pixel data and floating point depth values" +#ifdef PYBULLET_USE_NUMPY + " as NumPy arrays" +#endif + }, {"getContactPointData", pybullet_getContactPointData, METH_VARARGS, "Return the contact point information for all or some of pairwise " @@ -1862,6 +1910,13 @@ initpybullet(void) SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); PyModule_AddObject(m, "error", SpamError); + +#ifdef PYBULLET_USE_NUMPY + // Initialize numpy array. + import_array(); +#endif //PYBULLET_USE_NUMPY + + #if PY_MAJOR_VERSION >= 3 return m; #endif diff --git a/examples/pybullet/testrender_np.py b/examples/pybullet/testrender_np.py new file mode 100644 index 000000000..56439dde6 --- /dev/null +++ b/examples/pybullet/testrender_np.py @@ -0,0 +1,50 @@ +import numpy as np +import matplotlib.pyplot as plt +import pybullet +import time + +pybullet.connect(pybullet.DIRECT) +pybullet.loadURDF("r2d2.urdf") + +camTargetPos = [0,0,0] +cameraUp = [0,0,1] +cameraPos = [1,1,1] +yaw = 40 +pitch = 10.0 + +roll=0 +upAxisIndex = 2 +camDistance = 4 +pixelWidth = 1920 +pixelHeight = 1080 +nearPlane = 0.01 +farPlane = 1000 + +fov = 60 + +main_start = time.time() +#img_arr = pybullet.renderImage(pixelWidth, pixelHeight) +#renderImage(w, h, view[16], projection[16]) +#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane) +for pitch in range (0,360,10) : + start = time.time() + img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex, nearPlane, farPlane, fov) + stop = time.time() + print "renderImage %f" % (stop - start) + + w=img_arr[0] #width of the image, in pixels + h=img_arr[1] #height of the image, in pixels + rgb=img_arr[2] #color data RGB + dep=img_arr[3] #depth data + + #print 'width = %d height = %d' % (w,h) + + #show + plt.imshow(rgb,interpolation='none') + #plt.show() + plt.pause(0.01) + +main_stop = time.time() +print "Total time %f" % (main_stop - main_start) + +pybullet.resetSimulation()