Merge pull request #780 from erwincoumans/master

pybullet: add option to use NumPy to speed up, thanks to moof2k

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)

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)

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);

examples/SharedMemory/RobotControlExample.cpp

@@ -15,7 +15,6 @@
 #include <string>
 
 //const char* blaatnaam = "basename";
-#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE 1024
 
 struct MyMotorInfo
 {

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"
 

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

examples/pybullet/pybullet.c

@@ -2,12 +2,17 @@
 #include "../SharedMemory/PhysicsDirectC_API.h"
 #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
 
+
 #ifdef __APPLE__
 #include <Python/Python.h>
 #else
 #include <Python.h>
 #endif
 
+#ifdef PYBULLET_USE_NUMPY
+#include <numpy/arrayobject.h>
+#endif
+
 #if PY_MAJOR_VERSION >= 3
 #define PyInt_FromLong PyLong_FromLong
 #define PyString_FromString PyBytes_FromString
@@ -1275,6 +1280,42 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) {
       PyObject* item2;
       PyObject* pyResultList;  // store 4 elements in this result: width,
                                // height, rgbData, depth
+
+#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

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()