From bb305c6ebcb921125773af32a0d773a084d9d571 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 10 Oct 2018 23:31:50 -0700 Subject: [PATCH 1/5] allow to provide rayCastBatch in local 'from'/'to' with a parent/link index, b3RaycastBatchSetParentObject If parentObjectUniqueId provided, convert local from/to into world space coordinates AddUserDebugLins: don't block when replacing an item Fix examples/pybullet/examples/inverse_kinematics.py --- examples/SharedMemory/PhysicsClientC_API.cpp | 12 +++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 47 +++++++++++++++++++ .../SharedMemory/PhysicsServerExample.cpp | 33 +++++++++++-- examples/SharedMemory/SharedMemoryCommands.h | 3 ++ .../pybullet/examples/inverse_kinematics.py | 2 +- examples/pybullet/pybullet.c | 15 ++++-- 7 files changed, 104 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index fb17341c3..4b557bcd4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2898,6 +2898,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys command->m_requestRaycastIntersections.m_numCommandRays = 0; command->m_requestRaycastIntersections.m_numStreamingRays = 0; command->m_requestRaycastIntersections.m_numThreads = 1; + command->m_requestRaycastIntersections.m_parentObjectUniqueId = -1; + command->m_requestRaycastIntersections.m_parentLinkIndex=-1; return (b3SharedMemoryCommandHandle)command; } @@ -2947,6 +2949,16 @@ B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3Sha } } +B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); + command->m_requestRaycastIntersections.m_parentObjectUniqueId = parentObjectUniqueId; + command->m_requestRaycastIntersections.m_parentLinkIndex = parentLinkIndex; +} + + B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 998bf7677..795013e43 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -564,6 +564,7 @@ extern "C" B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]); //max num rays for b3RaycastBatchAddRays is MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays); + B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex); B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index eb2b4e747..12c1aa7be 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5050,6 +5050,53 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co memcpy(&rays[numCommandRays], bufferServerToClient, numStreamingRays * sizeof(b3RayData)); } + if (clientCmd.m_requestRaycastIntersections.m_parentObjectUniqueId>=0) + { + btTransform tr; + tr.setIdentity(); + + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestRaycastIntersections.m_parentObjectUniqueId); + if (bodyHandle) + { + int linkIndex = -1; + if (bodyHandle->m_multiBody) + { + int linkIndex = clientCmd.m_userDebugDrawArgs.m_parentLinkIndex; + if (linkIndex == -1) + { + tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + } + else + { + if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks()) + { + tr = bodyHandle->m_multiBody->getLink(linkIndex).m_cachedWorldTransform; + } + } + } + if (bodyHandle->m_rigidBody) + { + tr = bodyHandle->m_rigidBody->getWorldTransform(); + } + //convert all rays into world space + for (int i=0;im_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays); batchRayCaster.castRays(numThreads); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index d12475707..93292ee89 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1239,10 +1239,35 @@ public: m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex; m_tmpLine.m_replaceItemUid = replaceItemUid; - m_cs->lock(); - m_cs->setSharedParam(1, eGUIUserDebugAddLine); - m_resultDebugLineUid = -1; - workerThreadWait(); + + //don't block when replacing an item + if (replaceItemUid>=0 && replaceItemUid=0) + { + m_userDebugLines[slot] = m_tmpLine; + } + m_resultDebugLineUid = replaceItemUid; + } + else + { + + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugAddLine); + m_resultDebugLineUid = -1; + workerThreadWait(); + } return m_resultDebugLineUid; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 45b9c7bf2..1504a69f6 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -293,6 +293,9 @@ struct RequestRaycastIntersections b3RayData m_fromToRays[MAX_RAY_INTERSECTION_BATCH_SIZE]; int m_numStreamingRays; + //optional m_parentObjectUniqueId (-1 for unused) + int m_parentObjectUniqueId; + int m_parentLinkIndex; //streaming ray data stored in shared memory streaming part. (size m_numStreamingRays ) }; diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index c311c3ca1..514829a31 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -48,7 +48,7 @@ p.setRealTimeSimulation(useRealTimeSimulation) trailDuration = 15 while 1: - p.getCameraImage(320,200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_HARDWARE_OPENGL_RENDERER) + p.getCameraImage(320,200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX, renderer=p.ER_BULLET_HARDWARE_OPENGL) if (useRealTimeSimulation): dt = datetime.now() t = (dt.second/60.)*2.*math.pi diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 798131c46..e1f37edda 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4742,12 +4742,14 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* b3PhysicsClientHandle sm = 0; int sizeFrom = 0; int sizeTo = 0; - - static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "physicsClientId", NULL}; + int parentObjectUniqueId = -1; + int parentLinkIndex = -1; + + static char* kwlist[] = {"rayFromPositions", "rayToPositions", "numThreads", "parentObjectUniqueId", "parentLinkIndex", "physicsClientId", NULL}; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|ii", kwlist, - &rayFromObjList, &rayToObjList, &numThreads, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|iiii", kwlist, + &rayFromObjList, &rayToObjList, &numThreads, &parentObjectUniqueId, &parentLinkIndex, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -4831,6 +4833,11 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* } } + if (parentObjectUniqueId>=0) + { + b3RaycastBatchSetParentObject(commandHandle, parentObjectUniqueId,parentLinkIndex); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED) From f792a5951a9ccb498ca8c29d23572be5a2c4f4f7 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 11 Oct 2018 10:58:14 -0700 Subject: [PATCH 2/5] move zipfFileIO into own header route loadTextureFile from fileIO plugin fix B3_ENABLE_FILEIO_PLUGIN logic --- .../ImportURDFDemo/UrdfRenderingInterface.h | 5 +- .../PhysicsServerCommandProcessor.cpp | 38 ++- .../eglRendererVisualShapeConverter.cpp | 32 ++- .../eglRendererVisualShapeConverter.h | 2 +- .../plugins/fileIOPlugin/fileIOPlugin.cpp | 242 +----------------- .../plugins/fileIOPlugin/zipFileIO.h | 238 +++++++++++++++++ .../TinyRendererVisualShapeConverter.cpp | 31 ++- .../TinyRendererVisualShapeConverter.h | 2 +- 8 files changed, 339 insertions(+), 251 deletions(-) create mode 100644 examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h diff --git a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h index a9c1ba57d..f84e6f551 100644 --- a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h +++ b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h @@ -87,8 +87,9 @@ struct UrdfRenderingInterface virtual void render(const float viewMat[16], const float projMat[16]) = 0; ///load a texture from file, in png or other popular/supported format - virtual int loadTextureFile(const char* filename) = 0; - + //virtual int loadTextureFile(const char* filename) = 0; + virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO)=0; + ///register a texture using an in-memory pixel buffer of a given width and height virtual int registerTexture(unsigned char* texels, int width, int height) = 0; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 12c1aa7be..e7bcb6b07 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -77,7 +77,8 @@ #include "plugins/tinyRendererPlugin/tinyRendererPlugin.h" #endif -#ifndef B3_ENABLE_FILEIO_PLUGIN + +#ifdef B3_ENABLE_FILEIO_PLUGIN #include "plugins/fileIOPlugin/fileIOPlugin.h" #endif//B3_DISABLE_FILEIO_PLUGIN @@ -9911,7 +9912,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share int uid = -1; if (m_data->m_pluginManager.getRenderInterface()) { - uid = m_data->m_pluginManager.getRenderInterface()->loadTextureFile(relativeFileName); + uid = m_data->m_pluginManager.getRenderInterface()->loadTextureFile(relativeFileName, fileIO); } if (uid >= 0) { @@ -9920,8 +9921,37 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share { int width, height, n; - unsigned char* imageData = stbi_load(relativeFileName, &width, &height, &n, 3); - + unsigned char* imageData = 0; + + CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface(); + if (fileIO) + { + b3AlignedObjectArray buffer; + buffer.reserve(1024); + int fileId = fileIO->fileOpen(relativeFileName,"rb"); + if (fileId>=0) + { + int size = fileIO->getFileSize(fileId); + if (size>0) + { + buffer.resize(size); + int actual = fileIO->fileRead(fileId,&buffer[0],size); + if (actual != size) + { + b3Warning("image filesize mismatch!\n"); + buffer.resize(0); + } + } + } + if (buffer.size()) + { + imageData = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3); + } + } else + { + imageData = stbi_load(relativeFileName, &width, &height, &n, 3); + } + if (imageData) { texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData, width, height); diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp index 367fb3b50..4b06f4894 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp @@ -1317,12 +1317,40 @@ int EGLRendererVisualShapeConverter::registerTexture(unsigned char* texels, int return m_data->m_textures.size() - 1; } -int EGLRendererVisualShapeConverter::loadTextureFile(const char* filename) +int EGLRendererVisualShapeConverter::loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO) { B3_PROFILE("loadTextureFile"); int width, height, n; unsigned char* image = 0; - image = stbi_load(filename, &width, &height, &n, 3); + + if (fileIO) + { + b3AlignedObjectArray buffer; + buffer.reserve(1024); + int fileId = fileIO->fileOpen(filename,"rb"); + if (fileId>=0) + { + int size = fileIO->getFileSize(fileId); + if (size>0) + { + buffer.resize(size); + int actual = fileIO->fileRead(fileId,&buffer[0],size); + if (actual != size) + { + b3Warning("image filesize mismatch!\n"); + buffer.resize(0); + } + } + } + if (buffer.size()) + { + image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3); + } + } else + { + image = stbi_load(filename, &width, &height, &n, 3); + } + if (image && (width >= 0) && (height >= 0)) { return registerTexture(image, width, height); diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h index 1d74a8a09..a6caea855 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h @@ -48,7 +48,7 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface virtual void render(); virtual void render(const float viewMat[16], const float projMat[16]); - virtual int loadTextureFile(const char* filename); + virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO); virtual int registerTexture(unsigned char* texels, int width, int height); virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]); diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index 80ebafb69..9b39ae469 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -9,247 +9,11 @@ #include "../../../Utils/b3BulletDefaultFileIO.h" +#include "zipFileIO.h" + //#define B3_USE_ZLIB #ifdef B3_USE_ZLIB - -#include "minizip/unzip.h" - -struct MyFileIO : public CommonFileIOInterface -{ - std::string m_zipfileName; - - unzFile m_fileHandles[FILEIO_MAX_FILES]; - int m_numFileHandles; - - MyFileIO(const char* zipfileName) - :m_zipfileName(zipfileName), - m_numFileHandles(0) - { - for (int i=0;ifindFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen); - } - - virtual ~MyFileIO() - { - } - virtual int fileOpen(const char* fileName, const char* mode) - { - //search a free slot - int slot = -1; - for (int i=0;i=0) - { - unzFile zipfile; - unz_global_info m_global_info; - zipfile = unzOpen(m_zipfileName.c_str()); - if (zipfile == NULL) - { - printf("%s: not found\n", m_zipfileName.c_str()); - slot = -1; - } else - { - int result = 0; - result = unzGetGlobalInfo(zipfile, &m_global_info ); - if (result != UNZ_OK) - { - printf("could not read file global info from %s\n", m_zipfileName.c_str()); - unzClose(zipfile); - zipfile = 0; - slot = -1; - } else - { - m_fileHandles[slot] = zipfile; - } - } - if (slot >=0) - { - int result = unzLocateFile(zipfile, fileName, 0); - if (result == UNZ_OK) - { - unz_file_info info; - result = unzGetCurrentFileInfo(zipfile, &info, NULL, 0, NULL, 0, NULL, 0); - if (result != UNZ_OK) - { - printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result); - slot=-1; - } - else - { - result = unzOpenCurrentFile(zipfile); - if (result == UNZ_OK) - { - } else - { - slot=-1; - } - } - } - } - } - return slot; - } - virtual int fileRead(int fileHandle, char* destBuffer, int numBytes) - { - int result = -1; - if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) - { - unzFile f = m_fileHandles[fileHandle]; - if (f) - { - result = unzReadCurrentFile(f, destBuffer,numBytes); - //::fread(destBuffer, 1, numBytes, f); - } - } - return result; - - } - virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes) - { -#if 0 - if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) - { - FILE* f = m_fileHandles[fileHandle]; - if (f) - { - return ::fwrite(sourceBuffer, 1, numBytes,m_fileHandles[fileHandle]); - } - } -#endif - return -1; - } - virtual void fileClose(int fileHandle) - { - if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) - { - unzFile f = m_fileHandles[fileHandle]; - if (f) - { - unzClose(f); - m_fileHandles[fileHandle]=0; - } - } - } - - virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes) - { - return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, MyFileIO::FileIOPluginFindFile, this); - } - - - virtual bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) - { - int fileHandle = -1; - fileHandle = fileOpen(orgFileName, "rb"); - if (fileHandle>=0) - { - //printf("original file found: [%s]\n", orgFileName); - sprintf(relativeFileName, "%s", orgFileName); - fileClose(fileHandle); - return true; - } - - //printf("Trying various directories, relative to current working directory\n"); - const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"}; - int numPrefixes = sizeof(prefix) / sizeof(const char*); - - int f = 0; - bool fileFound = false; - - for (int i = 0; !f && i < numPrefixes; i++) - { -#ifdef _MSC_VER - sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName); -#else - sprintf(relativeFileName, "%s%s", prefix[i], orgFileName); -#endif - f = fileOpen(relativeFileName, "rb"); - if (f>=0) - { - fileFound = true; - break; - } - } - if (f>=0) - { - fileClose(f); - } - - return fileFound; - } - virtual char* readLine(int fileHandle, char* destBuffer, int numBytes) - { - int numRead = 0; - - if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) - { - unzFile f = m_fileHandles[fileHandle]; - if (f) - { - //return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); - char c = 0; - do - { - fileRead(fileHandle,&c,1); - if (c && c!='\n') - { - if (c!=13) - { - destBuffer[numRead++]=c; - } else - { - destBuffer[numRead++]=0; - } - } - } while (c != 0 && c != '\n' && numRead<(numBytes-1)); - } - } - if (numRead0) - { - destBuffer[numRead]=0; - return &destBuffer[0]; - } - return 0; - } - virtual int getFileSize(int fileHandle) - { - int size=0; - - if (fileHandle>=0 && fileHandle < FILEIO_MAX_FILES) - { - unzFile f = m_fileHandles[fileHandle]; - if (f) - { - unz_file_info info; - int result = unzGetCurrentFileInfo(f, &info, NULL, 0, NULL, 0, NULL, 0); - if (result == UNZ_OK) - { - size = info.uncompressed_size; - } - } - } - return size; - } - -}; - - - +typedef ZipFileIO MyFileIO; #else typedef b3BulletDefaultFileIO MyFileIO; #endif diff --git a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h new file mode 100644 index 000000000..e83c06c63 --- /dev/null +++ b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h @@ -0,0 +1,238 @@ + +#include "minizip/unzip.h" + +#define B3_ZIP_FILEIO_MAX_FILES 1024 + +struct ZipFileIO : public CommonFileIOInterface +{ + std::string m_zipfileName; + + unzFile m_fileHandles[B3_ZIP_FILEIO_MAX_FILES ]; + int m_numFileHandles; + + ZipFileIO(const char* zipfileName) + :m_zipfileName(zipfileName), + m_numFileHandles(0) + { + for (int i=0;ifindFile(orgFileName, relativeFileName, maxRelativeFileNameMaxLen); + } + + virtual ~ZipFileIO() + { + } + virtual int fileOpen(const char* fileName, const char* mode) + { + //search a free slot + int slot = -1; + for (int i=0;i=0) + { + unzFile zipfile; + unz_global_info m_global_info; + zipfile = unzOpen(m_zipfileName.c_str()); + if (zipfile == NULL) + { + printf("%s: not found\n", m_zipfileName.c_str()); + slot = -1; + } else + { + int result = 0; + result = unzGetGlobalInfo(zipfile, &m_global_info ); + if (result != UNZ_OK) + { + printf("could not read file global info from %s\n", m_zipfileName.c_str()); + unzClose(zipfile); + zipfile = 0; + slot = -1; + } else + { + m_fileHandles[slot] = zipfile; + } + } + if (slot >=0) + { + int result = unzLocateFile(zipfile, fileName, 0); + if (result == UNZ_OK) + { + unz_file_info info; + result = unzGetCurrentFileInfo(zipfile, &info, NULL, 0, NULL, 0, NULL, 0); + if (result != UNZ_OK) + { + printf("unzGetCurrentFileInfo() != UNZ_OK (%d)\n", result); + slot=-1; + } + else + { + result = unzOpenCurrentFile(zipfile); + if (result == UNZ_OK) + { + } else + { + slot=-1; + } + } + } + } + } + return slot; + } + virtual int fileRead(int fileHandle, char* destBuffer, int numBytes) + { + int result = -1; + if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES ) + { + unzFile f = m_fileHandles[fileHandle]; + if (f) + { + result = unzReadCurrentFile(f, destBuffer,numBytes); + //::fread(destBuffer, 1, numBytes, f); + } + } + return result; + + } + virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes) + { +#if 0 + if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES ) + { + FILE* f = m_fileHandles[fileHandle]; + if (f) + { + return ::fwrite(sourceBuffer, 1, numBytes,m_fileHandles[fileHandle]); + } + } +#endif + return -1; + } + virtual void fileClose(int fileHandle) + { + if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES ) + { + unzFile f = m_fileHandles[fileHandle]; + if (f) + { + unzClose(f); + m_fileHandles[fileHandle]=0; + } + } + } + + virtual bool findResourcePath(const char* fileName, char* relativeFileName, int relativeFileNameSizeInBytes) + { + return b3ResourcePath::findResourcePath(fileName, relativeFileName, relativeFileNameSizeInBytes, ZipFileIO::FileIOPluginFindFile, this); + } + + + virtual bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen) + { + int fileHandle = -1; + fileHandle = fileOpen(orgFileName, "rb"); + if (fileHandle>=0) + { + //printf("original file found: [%s]\n", orgFileName); + sprintf(relativeFileName, "%s", orgFileName); + fileClose(fileHandle); + return true; + } + + //printf("Trying various directories, relative to current working directory\n"); + const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"}; + int numPrefixes = sizeof(prefix) / sizeof(const char*); + + int f = 0; + bool fileFound = false; + + for (int i = 0; !f && i < numPrefixes; i++) + { +#ifdef _MSC_VER + sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName); +#else + sprintf(relativeFileName, "%s%s", prefix[i], orgFileName); +#endif + f = fileOpen(relativeFileName, "rb"); + if (f>=0) + { + fileFound = true; + break; + } + } + if (f>=0) + { + fileClose(f); + } + + return fileFound; + } + virtual char* readLine(int fileHandle, char* destBuffer, int numBytes) + { + int numRead = 0; + + if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES ) + { + unzFile f = m_fileHandles[fileHandle]; + if (f) + { + //return ::fgets(destBuffer, numBytes, m_fileHandles[fileHandle]); + char c = 0; + do + { + fileRead(fileHandle,&c,1); + if (c && c!='\n') + { + if (c!=13) + { + destBuffer[numRead++]=c; + } else + { + destBuffer[numRead++]=0; + } + } + } while (c != 0 && c != '\n' && numRead<(numBytes-1)); + } + } + if (numRead0) + { + destBuffer[numRead]=0; + return &destBuffer[0]; + } + return 0; + } + virtual int getFileSize(int fileHandle) + { + int size=0; + + if (fileHandle>=0 && fileHandle < B3_ZIP_FILEIO_MAX_FILES ) + { + unzFile f = m_fileHandles[fileHandle]; + if (f) + { + unz_file_info info; + int result = unzGetCurrentFileInfo(f, &info, NULL, 0, NULL, 0, NULL, 0); + if (result == UNZ_OK) + { + size = info.uncompressed_size; + } + } + } + return size; + } + +}; diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 12bc80cb7..32cbd8fd4 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -1170,12 +1170,39 @@ int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int return m_data->m_textures.size() - 1; } -int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) +int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO) { B3_PROFILE("loadTextureFile"); int width, height, n; unsigned char* image = 0; - image = stbi_load(filename, &width, &height, &n, 3); + if (fileIO) + { + b3AlignedObjectArray buffer; + buffer.reserve(1024); + int fileId = fileIO->fileOpen(filename,"rb"); + if (fileId>=0) + { + int size = fileIO->getFileSize(fileId); + if (size>0) + { + buffer.resize(size); + int actual = fileIO->fileRead(fileId,&buffer[0],size); + if (actual != size) + { + b3Warning("image filesize mismatch!\n"); + buffer.resize(0); + } + } + } + if (buffer.size()) + { + image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3); + } + } else + { + image = stbi_load(filename, &width, &height, &n, 3); + } + if (image && (width >= 0) && (height >= 0)) { return registerTexture(image, width, height); diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h index fcd009c3e..5ada574be 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h @@ -47,7 +47,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface virtual void render(); virtual void render(const float viewMat[16], const float projMat[16]); - virtual int loadTextureFile(const char* filename); + virtual int loadTextureFile(const char* filename, struct CommonFileIOInterface* fileIO); virtual int registerTexture(unsigned char* texels, int width, int height); virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling); From a24c1436afc0b9b12bf9b0e8cf7b61e39ae03e4f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 11 Oct 2018 14:39:31 -0700 Subject: [PATCH 3/5] state 2 of FileIO plugin: adding/removing FileIO types, search through all registered FileIO types. (not enabled by default yet) Example: fileIO = p.loadPlugin("fileIOPlugin") print("fileIO=",fileIO) p.executePluginCommand(fileIO,"e:/develop/bullet3/data/plane.zip", [p.AddFileIOAction,p.ZipFileIO]) p.executePluginCommand(fileIO,"e:/develop/bullet3/data/test2.zip", [p.AddFileIOAction,p.ZipFileIO]) planeId = p.loadURDF("plane.urdf") duckId = p.loadURDF("duck_vhacd.urdf",[0,0,1]) --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 7 +- examples/SharedMemory/SharedMemoryPublic.h | 14 + .../plugins/fileIOPlugin/fileIOPlugin.cpp | 311 +++++++++++++++--- .../plugins/fileIOPlugin/zipFileIO.h | 8 + examples/pybullet/pybullet.c | 7 + 5 files changed, 306 insertions(+), 41 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index e1190bb40..80f86b5e4 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -189,7 +189,12 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) BulletErrorLogger loggie; m_data->m_urdfParser.setParseSDF(false); - bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS)); + bool result = false; + + if (xml_string.length()) + { + result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS)); + } return result; } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index e2c27ede1..1cab59bf8 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -909,4 +909,18 @@ enum eConstraintSolverTypes eConstraintSolverLCP_BLOCK_PGS, }; +enum eFileIOActions +{ + eAddFileIOAction = 1024,//avoid collision with eFileIOTypes + eRemoveFileIOAction, +}; + + +enum eFileIOTypes +{ + ePosixFileIO = 1, + eZipFileIO, + eCNSFileIO, +}; + #endif //SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index 9b39ae469..d592ce48e 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -6,27 +6,209 @@ #include #include "../../../CommonInterfaces/CommonFileIOInterface.h" #include "../../../Utils/b3ResourcePath.h" + + +#ifndef B3_EXCLUDE_DEFAULT_FILEIO #include "../../../Utils/b3BulletDefaultFileIO.h" +#endif //B3_EXCLUDE_DEFAULT_FILEIO +#ifdef B3_USE_ZIPFILE_FILEIO #include "zipFileIO.h" +#endif //B3_USE_ZIPFILE_FILEIO + + +#ifdef B3_USE_CNS_FILEIO +#include "CNSFileIO.h" +#endif //B3_USE_CNS_FILEIO + +#define B3_MAX_FILEIO_INTERFACES 1024 + +struct WrapperFileHandle +{ + CommonFileIOInterface* childFileIO; + int m_childFileHandle; +}; + +struct WrapperFileIO : public CommonFileIOInterface +{ + CommonFileIOInterface* m_availableFileIOInterfaces[B3_MAX_FILEIO_INTERFACES]; + int m_numWrapperInterfaces; + + WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES]; + + + WrapperFileIO() + :m_numWrapperInterfaces(0) + { + for (int i=0;i=0 && fileIOIndex=0) + { + //figure out what wrapper interface to use + //use the first one that can open the file + for (int i=0;ifileOpen(fileName, mode); + if (childHandle>=0) + { + m_wrapperFileHandles[wrapperFileHandle].childFileIO = childFileIO; + m_wrapperFileHandles[wrapperFileHandle].m_childFileHandle = childHandle; + break; + } + } + } + } + return wrapperFileHandle; + } + + virtual int fileRead(int fileHandle, char* destBuffer, int numBytes) + { + int fileReadResult=-1; + if (fileHandle>=0 && fileHandlefileRead( + m_wrapperFileHandles[fileHandle].m_childFileHandle, destBuffer, numBytes); + } + } + return fileReadResult; + } + + virtual int fileWrite(int fileHandle,const char* sourceBuffer, int numBytes) + { + //todo + return -1; + } + virtual void fileClose(int fileHandle) + { + int fileReadResult=-1; + if (fileHandle>=0 && fileHandlefileClose( + m_wrapperFileHandles[fileHandle].m_childFileHandle); + m_wrapperFileHandles[fileHandle].childFileIO = 0; + m_wrapperFileHandles[fileHandle].m_childFileHandle = -1; + } + } + } + virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes) + { + bool found = false; + for (int i=0;ifindResourcePath(fileName, resourcePathOut, resourcePathMaxNumBytes); + } + if (found) + break; + } + return found; + } + virtual char* readLine(int fileHandle, char* destBuffer, int numBytes) + { + char* result = 0; + + int fileReadResult=-1; + if (fileHandle>=0 && fileHandlereadLine( + m_wrapperFileHandles[fileHandle].m_childFileHandle, + destBuffer, numBytes); + } + } + return result; + } + virtual int getFileSize(int fileHandle) + { + int numBytes = 0; + + int fileReadResult=-1; + if (fileHandle>=0 && fileHandlegetFileSize( + m_wrapperFileHandles[fileHandle].m_childFileHandle); + } + } + return numBytes; + } + +}; -//#define B3_USE_ZLIB -#ifdef B3_USE_ZLIB -typedef ZipFileIO MyFileIO; -#else -typedef b3BulletDefaultFileIO MyFileIO; -#endif struct FileIOClass { int m_testData; - MyFileIO m_fileIO; + WrapperFileIO m_fileIO; FileIOClass() : m_testData(42), - m_fileIO()//"e:/develop/bullet3/data/plane.zip") + m_fileIO() { } virtual ~FileIOClass() @@ -38,49 +220,98 @@ B3_SHARED_API int initPlugin_fileIOPlugin(struct b3PluginContext* context) { FileIOClass* obj = new FileIOClass(); context->m_userPointer = obj; + +#ifndef B3_EXCLUDE_DEFAULT_FILEIO + obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO()); +#endif //B3_EXCLUDE_DEFAULT_FILEIO + + return SHARED_MEMORY_MAGIC_NUMBER; } B3_SHARED_API int executePluginCommand_fileIOPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) { - printf("text argument:%s\n", arguments->m_text); - printf("int args: ["); - for (int i = 0; i < arguments->m_numInts; i++) - { - printf("%d", arguments->m_ints[i]); - if ((i + 1) < arguments->m_numInts) - { - printf(","); - } - } - printf("]\nfloat args: ["); - for (int i = 0; i < arguments->m_numFloats; i++) - { - printf("%f", arguments->m_floats[i]); - if ((i + 1) < arguments->m_numFloats) - { - printf(","); - } - } - printf("]\n"); + int result=-1; FileIOClass* obj = (FileIOClass*)context->m_userPointer; - b3SharedMemoryStatusHandle statusHandle; - int statusType = -1; - int bodyUniqueId = -1; - - b3SharedMemoryCommandHandle command = - b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text); - - statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_URDF_LOADING_COMPLETED) + printf("text argument:%s\n", arguments->m_text); + printf("int args: ["); + + //remove a fileIO type + if (arguments->m_numInts==1) { - bodyUniqueId = b3GetStatusBodyIndex(statusHandle); + int fileIOIndex = arguments->m_ints[0]; + obj->m_fileIO.removeFileIOInterface(fileIOIndex); } - return bodyUniqueId; + + if (arguments->m_numInts==2) + { + int action = arguments->m_ints[0]; + switch (action) + { + case eAddFileIOAction: + { + //create new fileIO interface + int fileIOType = arguments->m_ints[1]; + switch (fileIOType) + { + case ePosixFileIO: + { +#ifdef B3_EXCLUDE_DEFAULT_FILEIO + printf("ePosixFileIO is not enabled in this build.\n"); +#else + obj->m_fileIO.addFileIOInterface(new b3BulletDefaultFileIO()); +#endif + break; + } + case eZipFileIO: + { +#ifdef B3_USE_ZIPFILE_FILEIO + if (arguments->m_text) + { + obj->m_fileIO.addFileIOInterface(new ZipFileIO(arguments->m_text)); + } +#else + printf("eZipFileIO is not enabled in this build.\n"); +#endif + break; + } + case eCNSFileIO: + { +#ifdef B3_USE_CNS_FILEIO + B3_USE_ZIPFILE_FILEIO + if (arguments->m_text) + { + obj->m_fileIO.addFileIOInterface(new CNSFileIO(arguments->m_text)); + } +#else//B3_USE_CNS_FILEIO + printf("CNSFileIO is not enabled in this build.\n"); +#endif //B3_USE_CNS_FILEIO + break; + } + default: + { + } + } + break; + } + case eRemoveFileIOAction: + + { + //remove fileIO interface + int fileIOIndex = arguments->m_ints[1]; + obj->m_fileIO.removeFileIOInterface(fileIOIndex); + break; + } + default: + { + printf("executePluginCommand_fileIOPlugin: unknown action\n"); + } + } + } + return result; } B3_SHARED_API struct CommonFileIOInterface* getFileIOFunc_fileIOPlugin(struct b3PluginContext* context) diff --git a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h index e83c06c63..0df577052 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h +++ b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h @@ -29,7 +29,12 @@ struct ZipFileIO : public CommonFileIOInterface virtual ~ZipFileIO() { + for (int i=0;i Date: Thu, 11 Oct 2018 17:00:17 -0700 Subject: [PATCH 4/5] more initial work on fileIOPlugin --- .../SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index d592ce48e..84e522cf0 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -88,15 +88,16 @@ struct WrapperFileIO : public CommonFileIOInterface { //find an available wrapperFileHandle slot int wrapperFileHandle=-1; + int slot = -1; for (int i=0;i=0) + if (slot>=0) { //figure out what wrapper interface to use //use the first one that can open the file @@ -108,8 +109,9 @@ struct WrapperFileIO : public CommonFileIOInterface int childHandle = childFileIO->fileOpen(fileName, mode); if (childHandle>=0) { - m_wrapperFileHandles[wrapperFileHandle].childFileIO = childFileIO; - m_wrapperFileHandles[wrapperFileHandle].m_childFileHandle = childHandle; + wrapperFileHandle = slot; + m_wrapperFileHandles[slot].childFileIO = childFileIO; + m_wrapperFileHandles[slot].m_childFileHandle = childHandle; break; } } From c441a9469cfca6c40f75cb7a49e58aa3875d9df3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 11 Oct 2018 17:44:54 -0700 Subject: [PATCH 5/5] enable fileIOPlugin and loading from zipfile in PyBullet, bump up to version 2.3.0 --- setup.py | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index ae9ec4f66..738c9339d 100644 --- a/setup.py +++ b/setup.py @@ -43,6 +43,9 @@ CXX_FLAGS += '-DBT_ENABLE_ENET ' CXX_FLAGS += '-DBT_ENABLE_CLSOCKET ' CXX_FLAGS += '-DB3_DUMP_PYTHON_VERSION ' CXX_FLAGS += '-DEGL_ADD_PYTHON_INIT ' +CXX_FLAGS += '-DB3_ENABLE_FILEIO_PLUGIN ' +CXX_FLAGS += '-DB3_USE_ZIPFILE_FILEIO ' + EGL_CXX_FLAGS = '' @@ -73,6 +76,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/TinyRenderer/TinyRenderer.cpp"]\ +["examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp"]\ +["examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp"]\ ++["examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp"]\ +["examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp"]\ +["examples/SharedMemory/IKTrajectoryHelper.cpp"]\ +["examples/SharedMemory/InProcessMemory.cpp"]\ @@ -106,6 +110,24 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp"]\ +["examples/ThirdPartyLibs/stb_image/stb_image.cpp"]\ +["examples/ThirdPartyLibs/stb_image/stb_image_write.cpp"]\ ++["examples/ThirdPartyLibs/minizip/ioapi.c"]\ ++["examples/ThirdPartyLibs/minizip/unzip.c"]\ ++["examples/ThirdPartyLibs/minizip/zip.c"]\ ++["examples/ThirdPartyLibs/zlib/adler32.c"]\ ++["examples/ThirdPartyLibs/zlib/compress.c"]\ ++["examples/ThirdPartyLibs/zlib/crc32.c"]\ ++["examples/ThirdPartyLibs/zlib/deflate.c"]\ ++["examples/ThirdPartyLibs/zlib/gzclose.c"]\ ++["examples/ThirdPartyLibs/zlib/gzlib.c"]\ ++["examples/ThirdPartyLibs/zlib/gzread.c"]\ ++["examples/ThirdPartyLibs/zlib/gzwrite.c"]\ ++["examples/ThirdPartyLibs/zlib/infback.c"]\ ++["examples/ThirdPartyLibs/zlib/inffast.c"]\ ++["examples/ThirdPartyLibs/zlib/inflate.c"]\ ++["examples/ThirdPartyLibs/zlib/inftrees.c"]\ ++["examples/ThirdPartyLibs/zlib/trees.c"]\ ++["examples/ThirdPartyLibs/zlib/uncompr.c"]\ ++["examples/ThirdPartyLibs/zlib/zutil.c"]\ +["examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp"]\ +["examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp"]\ +["examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp"]\ @@ -559,7 +581,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.2.9', + version='2.3.0', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3',