Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1765 from erwincoumans/master

Browse Source 
PyBullet: deal with backward compatibility of b3RaycastBatchAddRay
This commit is contained in:
erwincoumans
2018-06-16 13:09:17 -07:00
committed by GitHub
parent ec37495142 9b0845a0f1
commit 046d95b7e6
9 changed files with 86 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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 (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{
command->m_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(numRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
b3Assert(numRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
{
cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays);

4
examples/SharedMemory/PhysicsClientC_API.h
View File

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

22
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -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(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays;
int newNumRays = curNumStreamingRays + numRays;
btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING)
{
for (int i=0;i<numRays;i++)
{
b3RayData* rayDataStream = (b3RayData *)m_data->m_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++;
}
}

22
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -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(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays;
int newNumRays = curNumStreamingRays + numRays;
btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING)
{
for (int i=0;i<numRays;i++)
{
b3RayData* rayDataStream = (b3RayData *)m_data->m_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++;
}
}

23
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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<b3RayData> 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;
}

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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

8
examples/pybullet/pybullet.c
View File

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

2
setup.py
View File

@@ -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',