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)
This commit is contained in:
erwincoumans
150
examples/pybullet/examples/createMultiBodyBatch.py
Normal file
150
examples/pybullet/examples/createMultiBodyBatch.py
Normal 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)
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user