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

allow batch creation of objects through PyBullet.createMultiBody, see createMultiBodyBatch.py example.

Browse Source 
expose minGraphicsUpdateTimeMs through PyBullet.connect(p.GUI, options="minGraphicsUpdateTimeMs=32000"), by default OpenGL rendering runs at 4000microseconds intervals.
allow a maximum of 128k objects
fix meshScale for PyBullet.createCollisionShape for custom mesh
expose Pybullet.setPhysicsEngineParameter(minimumSolverIslandSize=...), larger minimum batches group solver constraints together in the same island, to reduce calling overhead (even if they are not related)
This commit is contained in:
erwincoumans
2019-02-12 10:36:01 -08:00
parent a94a24959f
commit 85ee4c2934
10 changed files with 307 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1604,11 +1604,30 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3Physics
command->m_createMultiBodyArgs.m_bodyName[0] = 0;
command->m_createMultiBodyArgs.m_baseLinkIndex = -1;
command->m_createMultiBodyArgs.m_numLinks = 0;
command->m_createMultiBodyArgs.m_numBatchObjects = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
//batch creation is an performance feature to create a large number of multi bodies in one command
B3_SHARED_API int b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, double* batchPositions, int numBatchObjects)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl && command->m_type == CMD_CREATE_MULTI_BODY)
{
command->m_createMultiBodyArgs.m_numBatchObjects = numBatchObjects;
cl->uploadBulletFileToSharedMemory((const char*)batchPositions, sizeof(double) * 3 * numBatchObjects);
}
return 0;
}
B3_SHARED_API int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, const double basePosition[3], const double baseOrientation[4], const double baseInertialFramePosition[3], const double baseInertialFrameOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -511,6 +511,9 @@ extern "C"
int linkJointType,
const double linkJointAxis[/*3*/]);
//batch creation is an performance feature to create a large number of multi bodies in one command
B3_SHARED_API int b3CreateMultiBodySetBatchPositions(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, double* batchPositions, int numBatchObjects);
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API void b3CreateMultiBodySetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);

72
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2621,7 +2621,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
#endif
//Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(32768);
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
@@ -4401,7 +4401,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
btVector3 v2(vertexUpload[i2*3+0],
vertexUpload[i2*3+1],
vertexUpload[i2*3+2]);
meshInterface->addTriangle(v0, v1, v2);
meshInterface->addTriangle(v0*meshScale, v1*meshScale, v2*meshScale);
}
}
@@ -4434,7 +4434,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
btVector3 pt(vertexUpload[v*3+0],
vertexUpload[v*3+1],
vertexUpload[v*3+2]);
convexHull->addPoint(pt, false);
convexHull->addPoint(pt*meshScale, false);
}
convexHull->recalcLocalAabb();
@@ -7048,6 +7048,36 @@ bool PhysicsServerCommandProcessor::processLoadSDFCommand(const struct SharedMem
bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
if (clientCmd.m_createMultiBodyArgs.m_numBatchObjects > 0)
{
//batch of objects, to speed up creation time
bool result = false;
SharedMemoryCommand clientCmd2 = clientCmd;
int baseLinkIndex = clientCmd.m_createMultiBodyArgs.m_baseLinkIndex;
double* basePositionAndOrientations = (double*)bufferServerToClient;
for (int i = 0; i < clientCmd2.m_createMultiBodyArgs.m_numBatchObjects; i++)
{
clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 0] = basePositionAndOrientations[0 + i * 3];
clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 1] = basePositionAndOrientations[1 + i * 3];
clientCmd2.m_createMultiBodyArgs.m_linkPositions[baseLinkIndex * 3 + 2] = basePositionAndOrientations[2 + i * 3];
if (i == (clientCmd2.m_createMultiBodyArgs.m_numBatchObjects - 1))
{
result = processCreateMultiBodyCommandSingle(clientCmd2, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
}
else
{
result = processCreateMultiBodyCommandSingle(clientCmd2, serverStatusOut, 0, 0);
}
}
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
return result;
}
return processCreateMultiBodyCommandSingle(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
}
bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("processCreateMultiBodyCommand2");
bool hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED;
@@ -7072,10 +7102,16 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
bool ok = 0;
{
BT_PROFILE("processImportedObjects");
ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
}
if (ok)
{
BT_PROFILE("post process");
int bodyUniqueId = -1;
if (m_data->m_sdfRecentLoadedBodies.size() == 1)
@@ -7085,15 +7121,23 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
m_data->m_sdfRecentLoadedBodies.clear();
if (bodyUniqueId >= 0)
{
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
if (bufferSizeInBytes>0 && serverStatusOut.m_numDataStreamBytes==0)
{
{
BT_PROFILE("autogenerateGraphicsObjects");
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
}
BT_PROFILE("createBodyInfoStream");
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
}
}
}
@@ -8199,6 +8243,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
}
}
if (clientCmd.m_updateFlags & SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE)
{
m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = clientCmd.m_physSimParamArgs.m_minimumSolverIslandSize;
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
{
m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
@@ -10967,6 +11016,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_MULTI_BODY:
{
hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -41,6 +41,8 @@ protected:
bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCreateMultiBodyCommandSingle(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -981,14 +981,8 @@ struct b3CreateMultiBodyArgs
int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS];
double m_linkJointAxis[3 * MAX_CREATE_MULTI_BODY_LINKS];
int m_flags;
#if 0
std::string m_name;
std::string m_sourceFile;
btTransform m_rootTransformInWorld;
btHashMap<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
#endif
int m_numBatchObjects;
};
struct b3CreateMultiBodyResultArgs

15
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -2184,6 +2184,21 @@ int b3RobotSimulatorClientAPI_NoDirect::createMultiBody(struct b3RobotSimulatorC
command = b3CreateMultiBodyCommandInit(sm);
if (args.m_useMaximalCoordinates)
{
b3CreateMultiBodyUseMaximalCoordinates(command);
}
if (args.m_batchPositions.size())
{
btAlignedObjectArray<double> positionArray;
for (int i = 0; i < args.m_batchPositions.size(); i++)
{
positionArray.push_back(args.m_batchPositions[i][0]);
positionArray.push_back(args.m_batchPositions[i][1]);
positionArray.push_back(args.m_batchPositions[i][2]);
}
b3CreateMultiBodySetBatchPositions(sm, command, &positionArray[0], args.m_batchPositions.size());
}
baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex,
doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation);

2
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -418,7 +418,7 @@ struct b3RobotSimulatorCreateMultiBodyArgs
int *m_linkParentIndices;
int *m_linkJointTypes;
btVector3 *m_linkJointAxes;
btAlignedObjectArray<btVector3> m_batchPositions;
int m_useMaximalCoordinates;
b3RobotSimulatorCreateMultiBodyArgs()