AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

PyBullet: allow to pass vertices (for convex) and vertices+indices (for concave) to createCollisionShape

Browse Source 
See createObstacleCourse.py for an example use. At the moment a limit of 1024 vertices and 1024 indices.
Will be lifted once we implement the streaming version (soon).
This commit is contained in:
erwincoumans 2018-10-26 10:18:51 -07:00
parent 9c1db5fb34
commit a44df2b0a6
9 changed files with 318 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -80,6 +80,7 @@ struct UrdfGeometry
FILE_COLLADA = 2,
FILE_OBJ = 3,
FILE_CDF = 4,
MEMORY_VERTICES = 5,
};
int m_meshFileType;

86
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -1192,6 +1192,92 @@ B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle comm
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateCollisionShapeAddConvexMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0)
{
int i=0;
if (numVertices>B3_MAX_NUM_VERTICES)
numVertices=B3_MAX_NUM_VERTICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0]=0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = 0;
for (i=0;i<numVertices;i++)
{
command->m_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];
}
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateCollisionShapeAddConcaveMesh(b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices, const int* indices, int numIndices)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE) || (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex < MAX_COMPOUND_COLLISION_SHAPES && numVertices >= 0 && numIndices >=0)
{
int i=0;
if (numVertices>B3_MAX_NUM_VERTICES)
numVertices=B3_MAX_NUM_VERTICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = GEOM_FORCE_CONCAVE_TRIMESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName[0]=0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numVertices = numVertices;
for (i=0;i<numVertices;i++)
{
command->m_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];
}
if (numIndices>B3_MAX_NUM_INDICES)
numIndices = B3_MAX_NUM_INDICES;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numIndices = numIndices;
for (i=0;i<numIndices;i++)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_indices[i]=indices[i];
}
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -460,6 +460,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 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);

74
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -4193,6 +4193,80 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
std::string out_found_filename;
int out_type;
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices)
{
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(m_data->m_defaultCollisionMargin);
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices)
{
BT_PROFILE("convert trimesh2");
btTriangleMesh* meshInterface = new btTriangleMesh();
this->m_data->m_meshInterfaces.push_back(meshInterface);
{
BT_PROFILE("convert vertices2");
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];
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]);
meshInterface->addTriangle(v0, v1, v2);
}
}
{
BT_PROFILE("create btBvhTriangleMeshShape");
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
m_data->m_collisionShapes.push_back(trimesh);
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE)
{
btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap();
btGenerateInternalEdgeInfo(trimesh, triangleInfoMap);
}
shape = trimesh;
if (compound)
{
compound->addChildShape(childTransform, shape);
shape->setMargin(m_data->m_defaultCollisionMargin);
}
}
}
else
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(m_data->m_defaultCollisionMargin);
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]);
convexHull->addPoint(pt, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(childTransform, convexHull);
}
urdfColObj.m_geometry.m_meshFileType = UrdfGeometry::MEMORY_VERTICES;
break;
}
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (foundFile)

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -38,6 +38,7 @@ typedef unsigned long long int smUint64_t;
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
#define MAX_USER_DATA_KEY_LENGTH MAX_URDF_FILENAME_LENGTH
struct TmpFloat3
{
float m_x;
@ -925,7 +926,10 @@ struct b3CreateUserShapeData
double m_meshScale[3];
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];
};

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -7,7 +7,8 @@
//Please don't replace an existing magic number:
//instead, only ADD a new one at the top, comment-out previous one
#define SHARED_MEMORY_MAGIC_NUMBER 2018090300
#define SHARED_MEMORY_MAGIC_NUMBER 201810250
//#define SHARED_MEMORY_MAGIC_NUMBER 201809030
//#define SHARED_MEMORY_MAGIC_NUMBER 201809010
//#define SHARED_MEMORY_MAGIC_NUMBER 201807040
//#define SHARED_MEMORY_MAGIC_NUMBER 201806150
@ -925,4 +926,8 @@ enum eFileIOTypes
eCNSFileIO,
};
//limits for vertices/indices in PyBullet::createCollisionShape
#define B3_MAX_NUM_VERTICES 1024
#define B3_MAX_NUM_INDICES 1024
#endif //SHARED_MEMORY_PUBLIC_H

15
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
View File

@ -320,6 +320,10 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa
switch (visual->m_geometry.m_meshFileType)
{
case UrdfGeometry::MEMORY_VERTICES:
{
break;
}
case UrdfGeometry::FILE_OBJ:
{
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
@ -442,8 +446,10 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa
}
default:
// should never get here (findExistingMeshFile returns false if it doesn't recognize extension)
btAssert(0);
{
// should never get here (findExistingMeshFile returns false if it doesn't recognize extension)
btAssert(0);
}
}
if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices > 0))
@ -458,7 +464,10 @@ static void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPa
}
else
{
b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str());
if (visual->m_geometry.m_meshFileType !=UrdfGeometry::MEMORY_VERTICES)
{
b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str());
}
}
break;
} // case mesh

35
examples/pybullet/examples/createObstacleCourse.py
View File

@ -12,10 +12,39 @@ 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:
#convex mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj")
#concave mesh from obj
stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj", flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
verts=[[-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]]
#convex mesh from vertices
stoneConvexId = p.createCollisionShape(p.GEOM_MESH,vertices=verts)
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]
#concave mesh from vertices+indices
stoneConcaveId = p.createCollisionShape(p.GEOM_MESH,vertices=verts, indices=indices)
stoneId = stoneConvexId
#stoneId = stoneConcaveId
boxHalfLength = 0.5
boxHalfWidth = 2.5
boxHalfHeight = 0.1
@ -72,10 +101,10 @@ for i in range (segmentLength):
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 = 1
targetVelocity = 10
if (i%2):
targetVelocity =-1
p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=10)
targetVelocity =-10
p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=100)
segmentStart=segmentStart-1.1

103
examples/pybullet/pybullet.c
View File

@ -6284,6 +6284,75 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
return NULL;
}
static int extractVertices(PyObject* verticesObj, double* vertices, int maxNumVertices)
{
int numVerticesOut=0;
if (verticesObj)
{
PyObject* seqVerticesObj= PySequence_Fast(verticesObj, "expected a sequence of vertex positions");
if (seqVerticesObj)
{
int numVerticesSrc = PySequence_Size(seqVerticesObj);
{
int i;
if (numVerticesSrc > B3_MAX_NUM_VERTICES)
{
PyErr_SetString(SpamError, "Number of vertices exceeds the maximum.");
Py_DECREF(seqVerticesObj);
return 0;
}
for (i = 0; i < numVerticesSrc; i++)
{
PyObject* vertexObj = PySequence_GetItem(seqVerticesObj, i);
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];
numVerticesOut++;
}
}
}
}
}
return numVerticesOut;
}
static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices)
{
int numIndicesOut=0;
if (indicesObj)
{
PyObject* seqIndicesObj= PySequence_Fast(indicesObj, "expected a sequence of indices");
if (seqIndicesObj)
{
int numIndicesSrc = PySequence_Size(seqIndicesObj);
{
int i;
if (numIndicesSrc > B3_MAX_NUM_INDICES)
{
PyErr_SetString(SpamError, "Number of indices exceeds the maximum.");
Py_DECREF(seqIndicesObj);
return 0;
}
for (i = 0; i < numIndicesSrc; i++)
{
int index = pybullet_internalGetIntFromSequence(seqIndicesObj,i);
indices[numIndicesOut]=index;
numIndicesOut++;
}
}
}
}
return numIndicesOut;
}
static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
@ -6303,10 +6372,12 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
int flags = 0;
PyObject* halfExtentsObj = 0;
PyObject* verticesObj = 0;
PyObject* indicesObj = 0;
static char* kwlist[] = {"shapeType", "radius", "halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "collisionFramePosition", "collisionFrameOrientation", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOi", kwlist,
&shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &physicsClientId))
static char* kwlist[] = {"shapeType", "radius", "halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "collisionFramePosition", "collisionFrameOrientation", "vertices", "indices", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist,
&shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &physicsClientId))
{
return NULL;
}
@ -6316,6 +6387,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (shapeType >= GEOM_SPHERE)
{
b3SharedMemoryStatusHandle statusHandle;
@ -6346,6 +6418,31 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
pybullet_internalSetVectord(meshScaleObj, meshScale);
shapeIndex = b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale);
}
if (shapeType == GEOM_MESH && verticesObj)
{
int numVertices=0;
int numIndices=0;
double vertices[B3_MAX_NUM_VERTICES*3];
int indices[B3_MAX_NUM_INDICES];
numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES);
pybullet_internalSetVectord(meshScaleObj, meshScale);
if (indicesObj)
{
numIndices = extractIndices(indicesObj, indices,B3_MAX_NUM_INDICES);
}
if (numIndices)
{
shapeIndex = b3CreateCollisionShapeAddConcaveMesh(commandHandle, meshScale, vertices, numVertices, indices, numIndices);
} else
{
shapeIndex = b3CreateCollisionShapeAddConvexMesh(commandHandle, meshScale, vertices, numVertices);
}
}
if (shapeType == GEOM_PLANE)
{
double planeConstant = 0;