Merge pull request #1955 from erwincoumans/master

PyBullet: allow to pass vertices (for convex) and vertices+indices (f…

examples/Importers/ImportURDFDemo/UrdfParser.h

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

examples/SharedMemory/PhysicsClientC_API.cpp

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

examples/SharedMemory/PhysicsClientC_API.h

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

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

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

examples/SharedMemory/SharedMemoryCommands.h

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

examples/SharedMemory/SharedMemoryPublic.h

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

examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp

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

examples/pybullet/examples/createObstacleCourse.py

@@ -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
+			targetVelocity =-10
-		p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=10)
+		p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=100)
 	segmentStart=segmentStart-1.1
 
 

examples/pybullet/pybullet.c

@@ -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};
+	static char* kwlist[] = {"shapeType", "radius", "halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "collisionFramePosition", "collisionFrameOrientation", "vertices", "indices", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOi", kwlist,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist,
-									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &physicsClientId))
+									 &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;