From 85ee4c29346a69d4beb9b4cfa9f607a7c59b3394 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 12 Feb 2019 10:36:01 -0800 Subject: [PATCH] allow batch creation of objects through PyBullet.createMultiBody, see createMultiBodyBatch.py example. 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) --- .../InProcessExampleBrowser.cpp | 5 + examples/SharedMemory/PhysicsClientC_API.cpp | 19 +++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 72 +++++++-- .../PhysicsServerCommandProcessor.h | 2 + examples/SharedMemory/SharedMemoryCommands.h | 10 +- .../b3RobotSimulatorClientAPI_NoDirect.cpp | 15 ++ .../b3RobotSimulatorClientAPI_NoDirect.h | 2 +- .../pybullet/examples/createMultiBodyBatch.py | 150 ++++++++++++++++++ examples/pybullet/pybullet.c | 56 ++++++- 10 files changed, 307 insertions(+), 27 deletions(-) create mode 100644 examples/pybullet/examples/createMultiBodyBatch.py diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index e686bfda6..024fc1ceb 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -217,6 +217,11 @@ void ExampleBrowserThreadFunc(void* userPtr, void* lsMemory) ExampleBrowserArgs* args = (ExampleBrowserArgs*)userPtr; //int workLeft = true; b3CommandLineArgs args2(args->m_argc, args->m_argv); + int minUpdateMs = 4000; + if (args2.GetCmdLineArgument("minGraphicsUpdateTimeMs", minUpdateMs)) + { + gMinUpdateTimeMicroSecs = minUpdateMs; + } b3Clock clock; ExampleEntriesPhysicsServer examples; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 094683da0..a0c3c4fd4 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2c695e0e9..01529410b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 94072b1a3..b8ae068cb 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 2e08cb713..237309b77 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -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); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index dce9cdd1a..a1581d00f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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 m_materials; - btHashMap m_links; - btHashMap m_joints; -#endif + int m_numBatchObjects; + }; struct b3CreateMultiBodyResultArgs diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index f04d43f18..284430847 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -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 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); diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 7e9c56c14..cca09d974 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -418,7 +418,7 @@ struct b3RobotSimulatorCreateMultiBodyArgs int *m_linkParentIndices; int *m_linkJointTypes; btVector3 *m_linkJointAxes; - + btAlignedObjectArray m_batchPositions; int m_useMaximalCoordinates; b3RobotSimulatorCreateMultiBodyArgs() diff --git a/examples/pybullet/examples/createMultiBodyBatch.py b/examples/pybullet/examples/createMultiBodyBatch.py new file mode 100644 index 000000000..3e8d33cab --- /dev/null +++ b/examples/pybullet/examples/createMultiBodyBatch.py @@ -0,0 +1,150 @@ +import pybullet as p +import time +import math + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI, options="--minGraphicsUpdateTimeMs=16000") + + +p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) +p.setTimeStep(1./120.) +logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") +#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) +p.loadURDF("plane100.urdf", useMaximalCoordinates=True) +#disable rendering during creation. +p.setPhysicsEngineParameter(contactBreakingThreshold=0.04) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) +#disable tinyrenderer, software (CPU) renderer, we don't use it here +p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) + +shift = [0,-0.02,0] +meshScale=[0.1,0.1,0.1] + +vertices=[ +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[1.000000,1.000000,1.000000], +[-1.000000,1.000000,1.000000], +[-1.000000,-1.000000,-1.000000], +[1.000000,-1.000000,-1.000000], +[1.000000,1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,-1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,1.000000,1.000000], +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,-1.000000], +[1.000000,1.000000,-1.000000], +[1.000000,1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[-1.000000,-1.000000,-1.000000], +[-1.000000,-1.000000,1.000000], +[1.000000,-1.000000,1.000000], +[1.000000,-1.000000,-1.000000], +[-1.000000,1.000000,-1.000000], +[-1.000000,1.000000,1.000000], +[1.000000,1.000000,1.000000], +[1.000000,1.000000,-1.000000] +] + +normals=[ +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[0.000000,0.000000,-1.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[-1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[1.000000,0.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,-1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000], +[0.000000,1.000000,0.000000] +] + +uvs=[ +[0.750000,0.250000], +[1.000000,0.250000], +[1.000000,0.000000], +[0.750000,0.000000], +[0.500000,0.250000], +[0.250000,0.250000], +[0.250000,0.000000], +[0.500000,0.000000], +[0.500000,0.000000], +[0.750000,0.000000], +[0.750000,0.250000], +[0.500000,0.250000], +[0.250000,0.500000], +[0.250000,0.250000], +[0.000000,0.250000], +[0.000000,0.500000], +[0.250000,0.500000], +[0.250000,0.250000], +[0.500000,0.250000], +[0.500000,0.500000], +[0.000000,0.000000], +[0.000000,0.250000], +[0.250000,0.250000], +[0.250000,0.000000] +] +indices=[ 0, 1, 2, 0, 2, 3, #//ground face + 6, 5, 4, 7, 6, 4, #//top face + 10, 9, 8, 11, 10, 8, + 12, 13, 14, 12, 14, 15, + 18, 17, 16, 19, 18, 16, + 20, 21, 22, 20, 22, 23] + +#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) +#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) +visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals) +#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices) + +#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj") +collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) + +texUid = p.loadTexture("tex256.png") + + +batchPositions=[] + + + +for x in range (33): + for y in range (34): + for z in range (4): + batchPositions.append([x*meshScale[0]*5.5,y*meshScale[1]*5.5,(0.5+z)*meshScale[2]*2.5]) + +bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True) +p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid) + +#p.syncBodyInfo() +#print("numBodies=",p.getNumBodies()) +p.stopStateLogging(logId) +p.setGravity(0,0,-10) + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) + +colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]] +currentColor = 0 + + +while (1): + p.stepSimulation() + #time.sleep(1./120.) + #p.getCameraImage(320,200) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 06561ab1f..f3e01e5dc 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1501,9 +1501,32 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int minimumSolverIslandSize = -1; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching", "restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "minimumSolverIslandSize", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", + "numSolverIterations", + "useSplitImpulse", + "splitImpulsePenetrationThreshold", + "numSubSteps", + "collisionFilterMode", + "contactBreakingThreshold", + "maxNumCmdPer1ms", + "enableFileCaching", + "restitutionVelocityThreshold", + "erp", + "contactERP", + "frictionERP", + "enableConeFriction", + "deterministicOverlappingPairs", + "allowedCcdPenetration", + "jointFeedbackMode", + "solverResidualThreshold", + "contactSlop", + "enableSAT", + "constraintSolverType", + "globalCFM", + "minimumSolverIslandSize", + "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId)) { return NULL; @@ -7656,18 +7679,19 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje PyObject* linkJointAxisObj = 0; PyObject* linkInertialFramePositionObj = 0; PyObject* linkInertialFrameOrientationObj = 0; + PyObject* objBatchPositions = 0; static char* kwlist[] = { "baseMass", "baseCollisionShapeIndex", "baseVisualShapeIndex", "basePosition", "baseOrientation", "baseInertialFramePosition", "baseInertialFrameOrientation", "linkMasses", "linkCollisionShapeIndices", "linkVisualShapeIndices", "linkPositions", "linkOrientations", "linkInertialFramePositions", "linkInertialFrameOrientations", "linkParentIndices", - "linkJointTypes", "linkJointAxis", "useMaximalCoordinates", "flags", "physicsClientId", NULL}; + "linkJointTypes", "linkJointAxis", "useMaximalCoordinates", "flags", "batchPositions", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOiii", kwlist, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOOOOOiiOi", kwlist, &baseMass, &baseCollisionShapeIndex, &baseVisualShapeIndex, &basePosObj, &baseOrnObj, &baseInertialFramePositionObj, &baseInertialFrameOrientationObj, &linkMassesObj, &linkCollisionShapeIndicesObj, &linkVisualShapeIndicesObj, &linkPositionsObj, &linkOrientationsObj, &linkInertialFramePositionObj, &linkInertialFrameOrientationObj, &linkParentIndicesObj, - &linkJointTypesObj, &linkJointAxisObj, &useMaximalCoordinates, &flags, &physicsClientId)) + &linkJointTypesObj, &linkJointAxisObj, &useMaximalCoordinates, &flags, &objBatchPositions, &physicsClientId)) { return NULL; } @@ -7690,6 +7714,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje int numLinkJoinAxis = linkJointAxisObj ? PySequence_Size(linkJointAxisObj) : 0; int numLinkInertialFramePositions = linkInertialFramePositionObj ? PySequence_Size(linkInertialFramePositionObj) : 0; int numLinkInertialFrameOrientations = linkInertialFrameOrientationObj ? PySequence_Size(linkInertialFrameOrientationObj) : 0; + int numBatchPositions = objBatchPositions ? PySequence_Size(objBatchPositions) : 0; PyObject* seqLinkMasses = linkMassesObj ? PySequence_Fast(linkMassesObj, "expected a sequence") : 0; PyObject* seqLinkCollisionShapes = linkCollisionShapeIndicesObj ? PySequence_Fast(linkCollisionShapeIndicesObj, "expected a sequence") : 0; @@ -7702,6 +7727,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje PyObject* seqLinkInertialFramePositions = linkInertialFramePositionObj ? PySequence_Fast(linkInertialFramePositionObj, "expected a sequence") : 0; PyObject* seqLinkInertialFrameOrientations = linkInertialFrameOrientationObj ? PySequence_Fast(linkInertialFrameOrientationObj, "expected a sequence") : 0; + PyObject* seqBatchPositions = objBatchPositions ? PySequence_Fast(objBatchPositions, "expected a sequence") : 0; + if ((numLinkMasses == numLinkCollisionShapes) && (numLinkMasses == numLinkVisualShapes) && (numLinkMasses == numLinkPositions) && @@ -7728,6 +7755,18 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje baseIndex = b3CreateMultiBodyBase(commandHandle, baseMass, baseCollisionShapeIndex, baseVisualShapeIndex, basePosition, baseOrientation, baseInertialFramePosition, baseInertialFrameOrientation); + if (numBatchPositions > 0) + { + double* batchPositions = malloc(sizeof(double) * 3 * numBatchPositions); + for (i = 0; i < numBatchPositions; i++) + { + pybullet_internalGetVector3FromSequence(seqBatchPositions, i, &batchPositions[3*i]); + } + b3CreateMultiBodySetBatchPositions(sm, commandHandle, batchPositions, numBatchPositions); + free(batchPositions); + } + + for (i = 0; i < numLinkMasses; i++) { double linkMass = pybullet_internalGetFloatFromSequence(seqLinkMasses, i); @@ -7742,7 +7781,7 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje int linkJointType; pybullet_internalGetVector3FromSequence(seqLinkInertialFramePositions, i, linkInertialFramePosition); - pybullet_internalGetVector4FromSequence(linkInertialFrameOrientationObj, i, linkInertialFrameOrientation); + pybullet_internalGetVector4FromSequence(seqLinkInertialFrameOrientations, i, linkInertialFrameOrientation); pybullet_internalGetVector3FromSequence(seqLinkPositions, i, linkPosition); pybullet_internalGetVector4FromSequence(seqLinkOrientations, i, linkOrientation); pybullet_internalGetVector3FromSequence(seqLinkJoinAxis, i, linkJointAxis); @@ -7782,6 +7821,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje Py_DECREF(seqLinkInertialFramePositions); if (seqLinkInertialFrameOrientations) Py_DECREF(seqLinkInertialFrameOrientations); + if (seqBatchPositions) + Py_DECREF(seqBatchPositions); if (useMaximalCoordinates > 0) { @@ -7822,7 +7863,8 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje Py_DECREF(seqLinkInertialFramePositions); if (seqLinkInertialFrameOrientations) Py_DECREF(seqLinkInertialFrameOrientations); - + if (seqBatchPositions) + Py_DECREF(seqBatchPositions); PyErr_SetString(SpamError, "All link arrays need to be same size."); return NULL; }