From 9326fff75a98ee3a354d9b3dc8b577efbea80d0e Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 16 Jun 2018 11:02:04 -0700 Subject: [PATCH 1/2] PyBullet: bump up version to 2.0.8 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index e54f0e9f0..6022d0724 100644 --- a/setup.py +++ b/setup.py @@ -452,7 +452,7 @@ print("-----") setup( name = 'pybullet', - version='2.0.7', + version='2.0.8', 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', From 7924d5166572bff1653475e8b302f96cedb1996b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 16 Jun 2018 12:28:21 -0700 Subject: [PATCH 2/2] PyBullet: deal with backward compatibility of b3RaycastBatchAddRay: use b3RaycastBatchAddRays API to enable MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING num rays. Old API (b3RaycastBatchAddRay) sticks to 256 rays, MAX_RAY_INTERSECTION_BATCH_SIZE. --- examples/SharedMemory/PhysicsClientC_API.cpp | 41 +++++++++++++------ examples/SharedMemory/PhysicsClientC_API.h | 4 +- .../PhysicsClientSharedMemory.cpp | 22 +++++----- examples/SharedMemory/PhysicsDirect.cpp | 22 +++++----- .../PhysicsServerCommandProcessor.cpp | 23 +++++++---- examples/SharedMemory/SharedMemoryCommands.h | 10 +++-- examples/SharedMemory/SharedMemoryPublic.h | 5 ++- examples/pybullet/pybullet.c | 8 ++-- 8 files changed, 85 insertions(+), 50 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ab28ab411..35f487d7b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2736,13 +2736,19 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand(); b3Assert(command); command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; - command->m_requestRaycastIntersections.m_numRays = 0; + command->m_requestRaycastIntersections.m_numCommandRays = 1; + command->m_requestRaycastIntersections.m_numStreamingRays = 0; command->m_requestRaycastIntersections.m_numThreads = 1; - double rayFrom[3] = {rayFromWorldX,rayFromWorldY,rayFromWorldZ}; - double rayTo[3] = {rayToWorldX,rayToWorldY,rayToWorldZ}; - cl->uploadRaysToSharedMemory(*command, rayFrom, rayTo, 1); - + command->m_requestRaycastIntersections.m_numCommandRays = 1; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[0] = rayFromWorldX; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[1] = rayFromWorldY; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[2] = rayFromWorldZ; + + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[0] = rayToWorldX; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[1] = rayToWorldY; + command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[2] = rayToWorldZ; + return (b3SharedMemoryCommandHandle)command; } @@ -2755,7 +2761,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3Phys b3Assert(command); command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS; command->m_updateFlags = 0; - command->m_requestRaycastIntersections.m_numRays = 0; + command->m_requestRaycastIntersections.m_numCommandRays = 0; + command->m_requestRaycastIntersections.m_numStreamingRays = 0; command->m_requestRaycastIntersections.m_numThreads = 1; return (b3SharedMemoryCommandHandle)command; } @@ -2767,18 +2774,26 @@ B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle comm command->m_requestRaycastIntersections.m_numThreads = numThreads; } -B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]) +B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]) { - PhysicsClient* cl = (PhysicsClient* ) physClient; - b3Assert(cl); - struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); - if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS) { - cl->uploadRaysToSharedMemory(*command, rayFromWorld, rayToWorld, 1); + int numRays = command->m_requestRaycastIntersections.m_numCommandRays; + if (numRaysm_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[0] = rayFromWorld[0]; + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[1] = rayFromWorld[1]; + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[2] = rayFromWorld[2]; + + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[0] = rayToWorld[0]; + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[1] = rayToWorld[1]; + command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[2] = rayToWorld[2]; + + command->m_requestRaycastIntersections.m_numCommandRays++; + } } } @@ -2790,7 +2805,7 @@ B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3Sha struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS); - b3Assert(numRaysm_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS) { cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6146a452e..e57aaa510 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -544,7 +544,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient); // Sets the number of threads to use to compute the ray intersections for the batch. Specify 0 to let Bullet decide, 1 (default) for single core execution, 2 or more to select the number of threads to use. B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads); -B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]); +//max num rays for b3RaycastBatchAddRay is MAX_RAY_INTERSECTION_BATCH_SIZE +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 b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 131cc5d5f..1f3d24c35 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1719,22 +1719,22 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, void PhysicsClientSharedMemory::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) { - int curNumRays = command.m_requestRaycastIntersections.m_numRays; - int newNumRays = curNumRays + numRays; - btAssert(newNumRaysm_testBlock1->m_bulletStreamDataServerToClientRefactor; - rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; - rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; - rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; - rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; - rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; - rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; - command.m_requestRaycastIntersections.m_numRays++; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; } } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 24fb40a29..45f85d75c 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1355,22 +1355,22 @@ void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len) void PhysicsDirect::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) { - int curNumRays = command.m_requestRaycastIntersections.m_numRays; - int newNumRays = curNumRays + numRays; - btAssert(newNumRaysm_bulletStreamDataServerToClient; - rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; - rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; - rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; - rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; - rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; - rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; - command.m_requestRaycastIntersections.m_numRays++; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; } } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0f27342b8..82391b4c2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4826,21 +4826,30 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS"); serverStatusOut.m_raycastHits.m_numRaycastHits = 0; - const int numRays = clientCmd.m_requestRaycastIntersections.m_numRays; + const int numCommandRays = clientCmd.m_requestRaycastIntersections.m_numCommandRays; + const int numStreamingRays = clientCmd.m_requestRaycastIntersections.m_numStreamingRays; const int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads; + int totalRays = numCommandRays+numStreamingRays; btAlignedObjectArray rays; - rays.resize(numRays); - memcpy(&rays[0],bufferServerToClient,numRays*sizeof(b3RayData)); + rays.resize(totalRays); + if (numCommandRays) + { + memcpy(&rays[0],&clientCmd.m_requestRaycastIntersections.m_fromToRays[0],numCommandRays*sizeof(b3RayData)); + } + if (numStreamingRays) + { + memcpy(&rays[numCommandRays],bufferServerToClient,numStreamingRays*sizeof(b3RayData)); + } - BatchRayCaster batchRayCaster(m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo *)bufferServerToClient, numRays); + BatchRayCaster batchRayCaster(m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo *)bufferServerToClient, totalRays); if (numThreads == 0) { // When 0 is specified, Bullet can decide how many threads to use. // About 16 rays per thread seems to work reasonably well. - batchRayCaster.castRays(numRays / 16); + batchRayCaster.castRays(totalRays / 16); } else if (numThreads == 1) { // Sequentially trace all rays: - for (int i = 0; i < numRays; i++) { + for (int i = 0; i < totalRays; i++) { batchRayCaster.processRay(i); } } else { @@ -4849,7 +4858,7 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co batchRayCaster.castRays(numThreads); } - serverStatusOut.m_raycastHits.m_numRaycastHits = numRays; + serverStatusOut.m_raycastHits.m_numRaycastHits = totalRays; serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED; return hasStatus; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 6df03e6f4..b94ed8666 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -287,14 +287,18 @@ struct RequestRaycastIntersections // 1: Use a single thread (i.e. no multi-threading) // 2 or more: Number of threads to use. int m_numThreads; - int m_numRays; - // Actual ray data stored in m_bulletStreamDataServerToClientRefactor. + int m_numCommandRays; + //m_numCommandRays command rays are stored in m_fromToRays + b3RayData m_fromToRays[MAX_RAY_INTERSECTION_BATCH_SIZE]; + + int m_numStreamingRays; + //streaming ray data stored in shared memory streaming part. (size m_numStreamingRays ) }; struct SendRaycastHits { int m_numRaycastHits; - // Actual ray data stored in m_bulletStreamDataServerToClientRefactor. + // Actual ray result data stored in shared memory streaming part. }; struct RequestContactDataArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index f6467fb38..3023bfaf3 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -582,7 +582,10 @@ typedef union { struct b3RayData a; struct b3RayHitInfo b; } RAY_DATA_UNION; -#define MAX_RAY_INTERSECTION_BATCH_SIZE 16*1024 + + +#define MAX_RAY_INTERSECTION_BATCH_SIZE 256 +#define MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING 16*1024 #define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE #define VISUAL_SHAPE_MAX_PATH_LEN 1024 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 72a730982..71813938f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4730,7 +4730,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* { int i; - if (lenFrom > MAX_RAY_INTERSECTION_BATCH_SIZE) + if (lenFrom > MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING) { PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size."); Py_DECREF(seqRayFromObj); @@ -4748,7 +4748,9 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) && (pybullet_internalSetVectord(rayToObj, rayToWorld))) { - b3RaycastBatchAddRay(sm, commandHandle, rayFromWorld, rayToWorld); + //todo: better to upload all rays at once + //b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld); + b3RaycastBatchAddRays(sm, commandHandle, rayFromWorld, rayToWorld,1); } else { PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles"); @@ -9647,7 +9649,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS); - PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE); + PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING); PyModule_AddIntConstant(m, "B3G_F1", B3G_F1); PyModule_AddIntConstant(m, "B3G_F2", B3G_F2);