From bf9efffa4b4cfee98312dcb8bfcff764a4c64bf8 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 3 Jan 2019 16:19:28 -0800 Subject: [PATCH] Increase mesh allocation for vertices/indices in PyBullet.createCollisionShape See createMesh.py for an example. The data has to fit in shared memory, hence the limit on Mac is lower than Windows and Linux: #ifdef __APPLE__ #define B3_MAX_NUM_VERTICES 8192 #define B3_MAX_NUM_INDICES 32768 #else #define B3_MAX_NUM_VERTICES 131072 #define B3_MAX_NUM_INDICES 524288 #endif --- examples/SharedMemory/PhysicsClientC_API.cpp | 31 +++-- examples/SharedMemory/PhysicsClientC_API.h | 4 +- .../PhysicsServerCommandProcessor.cpp | 41 +++--- examples/SharedMemory/SharedMemoryCommands.h | 2 - examples/SharedMemory/SharedMemoryPublic.h | 15 ++- examples/pybullet/examples/createMesh.py | 121 ++++++++++++++++++ examples/pybullet/pybullet.c | 29 +++-- 7 files changed, 199 insertions(+), 44 deletions(-) create mode 100644 examples/pybullet/examples/createMesh.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 4fdbbdc05..73c2b5c33 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1280,8 +1280,11 @@ B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle comm return -1; } -B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices) +B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices) { + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command); b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE)); @@ -1305,12 +1308,7 @@ B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandl command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices; command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0; - for (i=0;im_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+0]=vertices[i*3+0]; - command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+1]=vertices[i*3+1]; - command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+2]=vertices[i*3+2]; - } + cl->uploadBulletFileToSharedMemory((const char*)vertices, numVertices * sizeof(double)*3); command->m_createUserShapeArgs.m_numUserShapes++; return shapeIndex; } @@ -1318,8 +1316,10 @@ B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandl return -1; } -B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices) +B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices) { + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command); b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE)); @@ -1342,11 +1342,16 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHand command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0]=0; command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices; + int totalUploadSizeInBytes = numVertices * sizeof(double) *3 + numIndices * sizeof(int); + char* data = (char*)malloc(totalUploadSizeInBytes); + double* vertexUpload = (double*)data; + int* indexUpload = (int*)(data + numVertices*sizeof(double)*3); + for (i=0;im_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+0]=vertices[i*3+0]; - command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+1]=vertices[i*3+1]; - command->m_createUserShapeArgs.m_shapes[shapeIndex].m_vertices[i*3+2]=vertices[i*3+2]; + vertexUpload[i*3+0]=vertices[i*3+0]; + vertexUpload[i*3+1]=vertices[i*3+1]; + vertexUpload[i*3+2]=vertices[i*3+2]; } if (numIndices>B3_MAX_NUM_INDICES) numIndices = B3_MAX_NUM_INDICES; @@ -1354,9 +1359,11 @@ B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHand command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices; for (i=0;im_createUserShapeArgs.m_shapes[shapeIndex].m_indices[i]=indices[i]; + indexUpload[i]=indices[i]; } command->m_createUserShapeArgs.m_numUserShapes++; + cl->uploadBulletFileToSharedMemory(data, totalUploadSizeInBytes); + free(data); return shapeIndex; } } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 18f8f81f6..a09ebfcd1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -466,8 +466,8 @@ extern "C" B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height); B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, const double planeNormal[/*3*/], double planeConstant); B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/]); - B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices); - B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices); + B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices); + B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices); B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, int flags); B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle, int shapeIndex, const double childPosition[/*3*/], const double childOrientation[/*4*/]); B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2bf9c99ec..e88276d5b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4215,6 +4215,17 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices) { + + int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices; + int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices; + + //int totalUploadSizeInBytes = numVertices * sizeof(double) * 3 + numIndices * sizeof(int); + + char* data = bufferServerToClient; + double* vertexUpload = (double*)data; + int* indexUpload = (int*)(data + numVertices * sizeof(double)*3); + + if (compound == 0) { compound = worldImporter->createCompoundShape(); @@ -4231,19 +4242,19 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str for (int j = 0; j < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices / 3; j++) { - int i0 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+0]; - int i1 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+1]; - int i2 = clientCmd.m_createUserShapeArgs.m_shapes[i].m_indices[j*3+2]; + int i0 = indexUpload[j*3+0]; + int i1 = indexUpload[j*3+1]; + int i2 = indexUpload[j*3+2]; - btVector3 v0( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i0*3+2]); - btVector3 v1( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i1*3+2]); - btVector3 v2( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[i2*3+2]); + btVector3 v0(vertexUpload[i0*3+0], + vertexUpload[i0*3+1], + vertexUpload[i0*3+2]); + btVector3 v1(vertexUpload[i1*3+0], + vertexUpload[i1*3+1], + vertexUpload[i1*3+2]); + btVector3 v2(vertexUpload[i2*3+0], + vertexUpload[i2*3+1], + vertexUpload[i2*3+2]); meshInterface->addTriangle(v0, v1, v2); } } @@ -4274,9 +4285,9 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str for (int v = 0; v < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices; v++) { - btVector3 pt( clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+0], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+1], - clientCmd.m_createUserShapeArgs.m_shapes[i].m_vertices[v*3+2]); + btVector3 pt(vertexUpload[v*3+0], + vertexUpload[v*3+1], + vertexUpload[v*3+2]); convexHull->addPoint(pt, false); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 27d65b1da..eacc049a6 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -928,9 +928,7 @@ struct b3CreateUserShapeData int m_collisionFlags; int m_visualFlags; int m_numVertices; - double m_vertices[B3_MAX_NUM_VERTICES*3]; int m_numIndices; - int m_indices[B3_MAX_NUM_INDICES]; double m_rgbaColor[4]; double m_specularColor[3]; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index cc522e754..17cef03ce 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -941,8 +941,19 @@ enum eFileIOTypes eInMemoryFileIO, }; + //limits for vertices/indices in PyBullet::createCollisionShape -#define B3_MAX_NUM_VERTICES 16 -#define B3_MAX_NUM_INDICES 16 +//Make sure the data fits in SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE +//(numVertices*sizeof(double)*3 + numIndices*sizeof(int)) < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE + + +#ifdef __APPLE__ +#define B3_MAX_NUM_VERTICES 8192 +#define B3_MAX_NUM_INDICES 32768 +#else +#define B3_MAX_NUM_VERTICES 131072 +#define B3_MAX_NUM_INDICES 524288 +#endif + #endif //SHARED_MEMORY_PUBLIC_H diff --git a/examples/pybullet/examples/createMesh.py b/examples/pybullet/examples/createMesh.py new file mode 100644 index 000000000..385312edb --- /dev/null +++ b/examples/pybullet/examples/createMesh.py @@ -0,0 +1,121 @@ +import pybullet as p +import time +import math + +p.connect(p.GUI) +#don't create a ground plane, to allow for gaps etc +p.resetSimulation() +#p.createCollisionShape(p.GEOM_PLANE) +#p.createMultiBody(0,0) +#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]); +p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]); + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) + + +sphereRadius = 0.05 +colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius) + +#a few different ways to create a mesh: + +vertices=[ [-0.246350,-0.246483,-0.000624], +[-0.151407, -0.176325, 0.172867], +[ -0.246350, 0.249205, -0.000624], +[ -0.151407, 0.129477, 0.172867], +[ 0.249338, -0.246483, -0.000624], +[ 0.154395, -0.176325, 0.172867], +[ 0.249338, 0.249205, -0.000624], +[ 0.154395, 0.129477, 0.172867] +] +indices=[0,3,2, +3,6,2, +7,4,6, +5,0,4, +6,0,2, +3,5,7, +0,1,3, +3,7,6, +7,5,4, +5,1,0, +6,4,0, +3,1,5] +#convex mesh from obj +stoneId = p.createCollisionShape(p.GEOM_MESH,vertices=vertices,indices=indices) + + + +boxHalfLength = 0.5 +boxHalfWidth = 2.5 +boxHalfHeight = 0.1 +segmentLength = 5 + +colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight]) + +mass = 1 +visualShapeId = -1 + +segmentStart = 0 + +for i in range (segmentLength): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) + segmentStart=segmentStart-1 + +for i in range (segmentLength): + height = 0 + if (i%2): + height=1 + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height]) + segmentStart=segmentStart-1 + +baseOrientation = p.getQuaternionFromEuler([math.pi/2.,0,math.pi/2.]) + +for i in range (segmentLength): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) + segmentStart=segmentStart-1 + if (i%2): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,i%3,-0.1+height+boxHalfWidth],baseOrientation=baseOrientation) + +for i in range (segmentLength): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1]) + width=4 + for j in range (width): + p.createMultiBody(baseMass=0,baseCollisionShapeIndex = stoneId,basePosition = [segmentStart,0.5*(i%2)+j-width/2.,0]) + segmentStart=segmentStart-1 + + +link_Masses=[1] +linkCollisionShapeIndices=[colBoxId] +linkVisualShapeIndices=[-1] +linkPositions=[[0,0,0]] +linkOrientations=[[0,0,0,1]] +linkInertialFramePositions=[[0,0,0]] +linkInertialFrameOrientations=[[0,0,0,1]] +indices=[0] +jointTypes=[p.JOINT_REVOLUTE] +axis=[[1,0,0]] + +baseOrientation = [0,0,0,1] +for i in range (segmentLength): + boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis) + p.changeDynamics(boxId,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) + print(p.getNumJoints(boxId)) + for joint in range (p.getNumJoints(boxId)): + targetVelocity = 10 + if (i%2): + targetVelocity =-10 + p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=100) + segmentStart=segmentStart-1.1 + + + +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +while (1): + camData = p.getDebugVisualizerCamera() + viewMat = camData[2] + projMat = camData[3] + p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL) + keys = p.getKeyboardEvents() + p.stepSimulation() + #print(keys) + time.sleep(0.01) + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e9dd48a47..b506fa183 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6696,9 +6696,12 @@ static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVe double vertex[3]; if (pybullet_internalSetVectord(vertexObj, vertex)) { - vertices[numVerticesOut*3+0]=vertex[0]; - vertices[numVerticesOut*3+1]=vertex[1]; - vertices[numVerticesOut*3+2]=vertex[2]; + if (vertices) + { + vertices[numVerticesOut * 3 + 0] = vertex[0]; + vertices[numVerticesOut * 3 + 1] = vertex[1]; + vertices[numVerticesOut * 3 + 2] = vertex[2]; + } numVerticesOut++; } } @@ -6730,7 +6733,10 @@ static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices) for (i = 0; i < numIndicesSrc; i++) { int index = pybullet_internalGetIntFromSequence(seqIndicesObj,i); - indices[numIndicesOut]=index; + if (indices) + { + indices[numIndicesOut] = index; + } numIndicesOut++; } } @@ -6806,11 +6812,10 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P } if (shapeType == GEOM_MESH && verticesObj) { - int numVertices=0; - int numIndices=0; - - double vertices[B3_MAX_NUM_VERTICES*3]; - int indices[B3_MAX_NUM_INDICES]; + int numVertices= extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES); + int numIndices= extractIndices(indicesObj, 0, B3_MAX_NUM_INDICES); + double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0; + int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0; numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES); pybullet_internalSetVectord(meshScaleObj, meshScale); @@ -6822,11 +6827,13 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P if (numIndices) { - shapeIndex = b3CreateCollisionShapeAddConcaveMesh(commandHandle, meshScale, vertices, numVertices, indices, numIndices); + shapeIndex = b3CreateCollisionShapeAddConcaveMesh(sm, commandHandle, meshScale, vertices, numVertices, indices, numIndices); } else { - shapeIndex = b3CreateCollisionShapeAddConvexMesh(commandHandle, meshScale, vertices, numVertices); + shapeIndex = b3CreateCollisionShapeAddConvexMesh(sm, commandHandle, meshScale, vertices, numVertices); } + free(vertices); + free(indices); } if (shapeType == GEOM_PLANE)