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

Implement rayTestBatch. At the moment, it is still testing individual rays on the physics server. We can enable multi-threaded version later. At least the python + shared-memory IPC overhead will be much lower.

Browse Source
This commit is contained in:
Erwin Coumans
2017-04-05 15:21:26 -07:00
parent e32debdca4
commit 6cbb00fd6b
6 changed files with 274 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1383,17 +1383,52 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_requestRaycastIntersections.m_rayFromPosition[0] = rayFromWorldX;
command->m_requestRaycastIntersections.m_rayFromPosition[1] = rayFromWorldY;
command->m_requestRaycastIntersections.m_rayFromPosition[2] = rayFromWorldZ;
command->m_requestRaycastIntersections.m_rayToPosition[0] = rayToWorldX;
command->m_requestRaycastIntersections.m_rayToPosition[1] = rayToWorldY;
command->m_requestRaycastIntersections.m_rayToPosition[2] = rayToWorldZ;
command->m_requestRaycastIntersections.m_numRays = 1;
command->m_requestRaycastIntersections.m_rayFromPositions[0][0] = rayFromWorldX;
command->m_requestRaycastIntersections.m_rayFromPositions[0][1] = rayFromWorldY;
command->m_requestRaycastIntersections.m_rayFromPositions[0][2] = rayFromWorldZ;
command->m_requestRaycastIntersections.m_rayToPositions[0][0] = rayToWorldX;
command->m_requestRaycastIntersections.m_rayToPositions[0][1] = rayToWorldY;
command->m_requestRaycastIntersections.m_rayToPositions[0][2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command;
}
b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_updateFlags = 0;
command->m_requestRaycastIntersections.m_numRays = 0;
return (b3SharedMemoryCommandHandle)command;
}
void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
{
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)
{
int numRays = command->m_requestRaycastIntersections.m_numRays;
if (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][0] = rayFromWorld[0];
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][1] = rayFromWorld[1];
command->m_requestRaycastIntersections.m_rayFromPositions[numRays][2] = rayFromWorld[2];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][0] = rayToWorld[0];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][1] = rayToWorld[1];
command->m_requestRaycastIntersections.m_rayToPositions[numRays][2] = rayToWorld[2];
command->m_requestRaycastIntersections.m_numRays++;
}
}
}
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -332,6 +332,9 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);

48
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2235,20 +2235,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
{
BT_PROFILE("CMD_REQUEST_RAY_CAST_INTERSECTIONS");
serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
for (int ray=0;ray<clientCmd.m_requestRaycastIntersections.m_numRays;ray++)
{
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][0],
clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][1],
clientCmd.m_requestRaycastIntersections.m_rayFromPositions[ray][2]);
btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][0],
clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][1],
clientCmd.m_requestRaycastIntersections.m_rayToPositions[ray][2]);
btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPosition[0],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[1],
clientCmd.m_requestRaycastIntersections.m_rayFromPosition[2]);
btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPosition[0],
clientCmd.m_requestRaycastIntersections.m_rayToPosition[1],
clientCmd.m_requestRaycastIntersections.m_rayToPosition[2]);
btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld);
m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback);
serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
int rayHits = serverStatusOut.m_raycastHits.m_numRaycastHits;
if (rayResultCallback.hasHit())
{
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitFraction
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction
= rayResultCallback.m_closestHitFraction;
int objectUniqueId = -1;
@@ -2268,25 +2272,37 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectUniqueId
= objectUniqueId;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitObjectLinkIndex
= linkIndex;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[0]
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0]
= rayResultCallback.m_hitPointWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[1]
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1]
= rayResultCallback.m_hitPointWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[2]
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2]
= rayResultCallback.m_hitPointWorld[2];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[0]
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0]
= rayResultCallback.m_hitNormalWorld[0];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[1]
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1]
= rayResultCallback.m_hitNormalWorld[1];
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[2]
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2]
= rayResultCallback.m_hitNormalWorld[2];
} else
{
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitFraction = 1;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId = -1;
serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex = -1;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[0] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[1] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitPositionWorld[2] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[0] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[1] = 0;
serverStatusOut.m_raycastHits.m_rayHits[rayHits].m_hitNormalWorld[2] = 0;
}
serverStatusOut.m_raycastHits.m_numRaycastHits++;
}
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;

7
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -185,14 +185,15 @@ enum EnumRequestContactDataUpdateFlags
struct RequestRaycastIntersections
{
double m_rayFromPosition[3];
double m_rayToPosition[3];
int m_numRays;
double m_rayFromPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3];
double m_rayToPositions[MAX_RAY_INTERSECTION_BATCH_SIZE][3];
};
struct SendRaycastHits
{
int m_numRaycastHits;
b3RayHitInfo m_rayHits[MAX_RAY_HITS];
b3RayHitInfo m_rayHits[MAX_RAY_INTERSECTION_BATCH_SIZE];
};
struct RequestContactDataArgs

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -257,9 +257,13 @@ enum b3VREventType
#define MAX_VR_BUTTONS 64
#define MAX_VR_CONTROLLERS 8
#define MAX_RAY_HITS 128
#define MAX_RAY_INTERSECTION_BATCH_SIZE 1024
#define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
#define MAX_KEYBOARD_EVENTS 256
enum b3VRButtonInfo
{
eButtonIsDown = 1,

165
examples/pybullet/pybullet.c
View File

@@ -400,8 +400,8 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
return Py_None;
}
void b3pybulletExitFunc()
///to avoid memory leaks, disconnect all physics servers explicitly
void b3pybulletExitFunc(void)
{
int i;
for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
@@ -2791,7 +2791,7 @@ static PyObject* pybullet_setTimeOut(PyObject* self, PyObject* args, PyObject* k
return Py_None;
}
static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keywds)
static PyObject* pybullet_rayTestObsolete(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -2875,6 +2875,154 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject* keyw
return Py_None;
}
static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* rayFromObjList = 0;
PyObject* rayToObjList = 0;
b3PhysicsClientHandle sm = 0;
int sizeFrom = 0;
int sizeTo = 0;
static char* kwlist[] = {"rayFromPositions", "rayToPositions", "physicsClientId", NULL};
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist,
&rayFromObjList, &rayToObjList, &physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
commandHandle = b3CreateRaycastBatchCommandInit(sm);
if (rayFromObjList)
{
PyObject* seqRayFromObj = PySequence_Fast(rayFromObjList, "expected a sequence of rayFrom positions");
PyObject* seqRayToObj = PySequence_Fast(rayToObjList, "expected a sequence of 'rayTo' positions");
if (seqRayFromObj && seqRayToObj)
{
int lenFrom = PySequence_Size(rayFromObjList);
int lenTo= PySequence_Size(seqRayToObj);
if (lenFrom!=lenTo)
{
PyErr_SetString(SpamError, "Size of from_positions need to be equal to size of to_positions.");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
return NULL;
} else
{
int i;
if (lenFrom >= MAX_RAY_INTERSECTION_BATCH_SIZE)
{
PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size.");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
return NULL;
}
for (i = 0; i < lenFrom; i++)
{
PyObject* rayFromObj = PySequence_GetItem(rayFromObjList,i);
PyObject* rayToObj = PySequence_GetItem(seqRayToObj,i);
double rayFromWorld[3];
double rayToWorld[3];
if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) &&
(pybullet_internalSetVectord(rayToObj, rayToWorld)))
{
b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld);
} else
{
PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles");
Py_DECREF(seqRayFromObj);
Py_DECREF(seqRayToObj);
Py_DECREF(rayFromObj);
Py_DECREF(rayToObj);
return NULL;
}
Py_DECREF(rayFromObj);
Py_DECREF(rayToObj);
}
}
} else
{
}
if (seqRayFromObj)
{
Py_DECREF(seqRayFromObj);
}
if (seqRayToObj)
{
Py_DECREF(seqRayToObj);
}
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)
{
struct b3RaycastInformation raycastInfo;
PyObject* rayHitsObj = 0;
int i;
b3GetRaycastInformation(sm, &raycastInfo);
rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits);
for (i = 0; i < raycastInfo.m_numRayHits; i++)
{
PyObject* singleHitObj = PyTuple_New(5);
{
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectUniqueId);
PyTuple_SetItem(singleHitObj, 0, ob);
}
{
PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectLinkIndex);
PyTuple_SetItem(singleHitObj, 1, ob);
}
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitFraction);
PyTuple_SetItem(singleHitObj, 2, ob);
}
{
PyObject* posObj = PyTuple_New(3);
int p;
for (p = 0; p < 3; p++)
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitPositionWorld[p]);
PyTuple_SetItem(posObj, p, ob);
}
PyTuple_SetItem(singleHitObj, 3, posObj);
}
{
PyObject* normalObj = PyTuple_New(3);
int p;
for (p = 0; p < 3; p++)
{
PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitNormalWorld[p]);
PyTuple_SetItem(normalObj, p, ob);
}
PyTuple_SetItem(singleHitObj, 4, normalObj);
}
PyTuple_SetItem(rayHitsObj, i, singleHitObj);
}
return rayHitsObj;
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args)
{
PyObject* quatObj;
@@ -5298,9 +5446,14 @@ static PyMethodDef SpamMethods[] = {
"fileName, optional objectUniqueId. Function returns int loggingUniqueId"},
{"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
"Stop logging of robot state, given a loggingUniqueId."},
{"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
{"rayTest", (PyCFunction)pybullet_rayTestObsolete, METH_VARARGS | METH_KEYWORDS,
"Cast a ray and return the first object hit, if any. "
"Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"},
"Takes two arguments (from_position [x,y,z] and to_position [x,y,z] in Cartesian world coordinates"},
{"rayTestBatch", (PyCFunction)pybullet_rayTestBatch, METH_VARARGS | METH_KEYWORDS,
"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
"Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
"Set the timeOut in seconds, used for most of the API calls."},
// todo(erwincoumans)
@@ -5437,6 +5590,8 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE);
PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);
PyModule_AddIntConstant(m, "B3G_F2", B3G_F2);
PyModule_AddIntConstant(m, "B3G_F3", B3G_F3);