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

5
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

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

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

62
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,9 +7121,16 @@ 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;
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;
@@ -7096,6 +7139,7 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
}
}
}
//ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags);
}
@@ -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()

150
examples/pybullet/examples/createMultiBodyBatch.py Normal file
View File

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

56
examples/pybullet/pybullet.c
View File

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