Merge pull request #2364 from erwincoumans/master

pybullet: allow programmatic creation of heightfield. See https://git…

10 files changed, 194 insertions, 36 deletions

diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp
index 6e33cbfc9..51cd1842d 100644
--- a/examples/BulletRobotics/FixJointBoxes.cpp
+++ b/examples/BulletRobotics/FixJointBoxes.cpp
@@ -24,13 +24,14 @@ class FixJointBoxes : public CommonExampleInterface
 	b3RobotSimulatorSetPhysicsEngineParameters physicsArgs;
 	int solver;
 
-	const size_t numCubes = 30;
+	const size_t numCubes;
 	std::vector<int> cubeIds;
 
 public:
 	FixJointBoxes(GUIHelperInterface* helper, int options)
 		: m_guiHelper(helper),
 		  m_options(options),
+		  numCubes(30),
 		  cubeIds(numCubes, 0),
 		  solver(solverId)
 	{
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index b976136df..58b67a3bf 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -145,12 +145,6 @@ project "App_BulletExampleBrowser"
 		"../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
 		"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
 		"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
-		"../SharedMemory/plugins/stablePDPlugin/SpAlg.cpp",
-		"../SharedMemory/plugins/stablePDPlugin/Shape.cpp",
-		"../SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp",
-		"../SharedMemory/plugins/stablePDPlugin/RBDModel.cpp",
-		"../SharedMemory/plugins/stablePDPlugin/MathUtil.cpp",
-		"../SharedMemory/plugins/stablePDPlugin/KinTree.cpp",
 		"../SharedMemory/SharedMemoryCommands.h",
 		"../SharedMemory/SharedMemoryPublic.h",
 		"../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",
@@ -211,6 +205,26 @@ project "App_BulletExampleBrowser"
     "../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
     "../ThirdPartyLibs/tinyxml2/tinyxml2.h",
         }
+        
+  if _OPTIONS["enable_stable_pd"] then
+		defines {"STATIC_LINK_SPD_PLUGIN"}
+		files {
+			"../SharedMemory/plugins/stablePDPlugin/SpAlg.cpp",
+			"../SharedMemory/plugins/stablePDPlugin/SpAlg.h",
+			"../SharedMemory/plugins/stablePDPlugin/Shape.cpp",
+			"../SharedMemory/plugins/stablePDPlugin/Shape.h",
+			"../SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp",
+			"../SharedMemory/plugins/stablePDPlugin/RBDUtil.h",
+			"../SharedMemory/plugins/stablePDPlugin/RBDModel.cpp",
+			"../SharedMemory/plugins/stablePDPlugin/RBDModel.h",
+			"../SharedMemory/plugins/stablePDPlugin/MathUtil.cpp",
+			"../SharedMemory/plugins/stablePDPlugin/MathUtil.h",
+			"../SharedMemory/plugins/stablePDPlugin/KinTree.cpp",
+			"../SharedMemory/plugins/stablePDPlugin/KinTree.h",
+			"../SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp",
+			"../SharedMemory/plugins/stablePDPlugin/BulletConversion.h",
+			}
+		end
 if (hasCL and findOpenGL3()) then
 			files {
 				"../OpenCL/broadphase/*",
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 3fe405ba1..646f03985 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -1338,6 +1338,39 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHand
 			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_heightfieldTextureScaling = textureScaling;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = -1;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = -1;
+			command->m_createUserShapeArgs.m_numUserShapes++;
+			return shapeIndex;
+		}
+	}
+	return -1;
+}
+
+B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns)
+{
+	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));
+	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)
+		{
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_HEIGHTFIELD;
+			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_meshFileName[0] = 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_heightfieldTextureScaling = textureScaling;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = numHeightfieldRows;
+			command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = numHeightfieldColumns;
+			cl->uploadBulletFileToSharedMemory((const char*)heightfieldData, numHeightfieldRows*numHeightfieldColumns* sizeof(float));
 			command->m_createUserShapeArgs.m_numUserShapes++;
 			return shapeIndex;
 		}
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index f11f7a872..ce1baef0c 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -490,6 +490,7 @@ extern "C"
 	B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
 	B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height);
 	B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/], double textureScaling);
+	B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], double textureScaling, float* heightfieldData, int numHeightfieldRows, int numHeightfieldColumns);
 	
 	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*/]);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 351541cad..b8e0275cd 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1610,7 +1610,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btAlignedObjectArray<std::string*> m_strings;
 
 	btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
-	btAlignedObjectArray<unsigned char*> m_heightfieldDatas;
+	btAlignedObjectArray<const unsigned char*> m_heightfieldDatas;
 	btAlignedObjectArray<int> m_allocatedTextures;
 	btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
 	btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
@@ -4394,7 +4394,7 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
 					for (int j = 0; j < height; ++j)
 					{
 						
-						float z = allValues[i + width*j];
+						double z = allValues[i + width*j];
 						//convertFromFloat(p, z, type);
 						btScalar * pf = (btScalar *)p;
 						*pf = z;
@@ -4545,7 +4545,29 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 				btScalar minHeight, maxHeight;
 				PHY_ScalarType scalarType = PHY_FLOAT;
 				CommonFileIOInterface* fileIO = m_data->m_pluginManager.getFileIOInterface();
-				unsigned char* heightfieldData = MyGetRawHeightfieldData(*fileIO, scalarType, clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName, width, height, minHeight, maxHeight);
+				const unsigned char* heightfieldData = 0;
+				if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldColumns > 0 &&
+					clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldRows > 0)
+				{
+					
+					width = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldRows;
+					height = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldColumns;
+					float* heightfieldDataSrc = (float*)bufferServerToClient;
+					heightfieldData = new unsigned char[width*height * sizeof(btScalar)];
+					btScalar* datafl = (btScalar*)heightfieldData;
+					minHeight = heightfieldDataSrc[0];
+					maxHeight = heightfieldDataSrc[0];
+					for (int i = 0; i < width*height; i++)
+					{
+						datafl[i] = heightfieldDataSrc[i];
+						minHeight = btMin(minHeight, (btScalar)datafl[i]);
+						maxHeight = btMax(maxHeight, (btScalar)datafl[i]);
+					}
+				}
+				else
+				{
+					heightfieldData = MyGetRawHeightfieldData(*fileIO, scalarType, clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName, width, height, minHeight, maxHeight);
+				}
 				if (heightfieldData)
 				{
 					
diff --git a/examples/SharedMemory/SharedMemoryBlock.h b/examples/SharedMemory/SharedMemoryBlock.h
index 16c7a2fad..54d896986 100644
--- a/examples/SharedMemory/SharedMemoryBlock.h
+++ b/examples/SharedMemory/SharedMemoryBlock.h
@@ -17,10 +17,7 @@ struct SharedMemoryBlock
 	int m_numServerCommands;
 	int m_numProcessedServerCommands;
 
-	//m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints
-	//the Bullet data structures are more general purpose than the capabilities of a URDF file.
-	char m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
-
+	
 	//m_bulletStreamDataServerToClient is used to send (debug) data from server to client, for
 	//example to provide all details of a multibody including joint/link names, after loading a URDF file.
 	char m_bulletStreamDataServerToClientRefactor[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 9aa7a74a0..f429bee59 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -24,7 +24,7 @@ typedef unsigned long long int smUint64_t;
 #endif
 
 #ifdef __APPLE__
-#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512 * 1024)
+#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (1024 * 1024)
 #else
 #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8 * 1024 * 1024)
 #endif
@@ -961,6 +961,8 @@ struct b3CreateUserShapeData
 	int m_numUVs;
 	int m_numNormals;
 	double m_heightfieldTextureScaling;
+	int m_numHeightfieldRows;
+	int m_numHeightfieldColumns;
 	double m_rgbaColor[4];
 	double m_specularColor[3];
 };
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 680de9481..c5b32b05a 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/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 201908050
+#define SHARED_MEMORY_MAGIC_NUMBER 201908110
+//#define SHARED_MEMORY_MAGIC_NUMBER 201908050
 //#define SHARED_MEMORY_MAGIC_NUMBER 2019060190
 //#define SHARED_MEMORY_MAGIC_NUMBER 201904030
 //#define SHARED_MEMORY_MAGIC_NUMBER 201902120
diff --git a/examples/pybullet/examples/heightfield.py b/examples/pybullet/examples/heightfield.py
index 4f67d35f4..f8ffefee5 100644
--- a/examples/pybullet/examples/heightfield.py
+++ b/examples/pybullet/examples/heightfield.py
@@ -7,13 +7,39 @@ p.connect(p.GUI)
 p.setAdditionalSearchPath(pd.getDataPath())
 
 textureId = -1
-useDeepLocoCSV = False
 
-if useDeepLocoCSV:
-  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.5,.5,2.5],fileName = "heightmaps/ground0.txt", heightfieldTextureScaling=128)#Image8x4.png")#wm_height_out.png")
+useProgrammatic = 0
+useTerrainFromPNG = 1
+useDeepLocoCSV = 2
+
+heightfieldSource = useProgrammatic
+import random
+random.seed(10)
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+heightPerturbationRange = 0.05
+if heightfieldSource==useProgrammatic:
+  numHeightfieldRows = 256
+  numHeightfieldColumns = 256
+  heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns 
+  for j in range (int(numHeightfieldColumns/2)):
+    for i in range (int(numHeightfieldRows/2) ):
+      height = random.uniform(0,heightPerturbationRange)
+      heightfieldData[2*i+2*j*numHeightfieldRows]=height
+      heightfieldData[2*i+1+2*j*numHeightfieldRows]=height
+      heightfieldData[2*i+(2*j+1)*numHeightfieldRows]=height
+      heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows]=height
+      
+      
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.05,.05,1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns, )
   terrain  = p.createMultiBody(0, terrainShape)
-  p.resetBasePositionAndOrientation(terrain,[-8,0,-2], [0,0,0,1])
-else:
+  p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useDeepLocoCSV:
+  terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.5,.5,2.5],fileName = "heightmaps/ground0.txt", heightfieldTextureScaling=128)
+  terrain  = p.createMultiBody(0, terrainShape)
+  p.resetBasePositionAndOrientation(terrain,[0,0,0], [0,0,0,1])
+
+if heightfieldSource==useTerrainFromPNG:
   terrainShape = p.createCollisionShape(shapeType = p.GEOM_HEIGHTFIELD, meshScale=[.1,.1,24],fileName = "heightmaps/wm_height_out.png")
   textureId = p.loadTexture("heightmaps/gimp_overlay_out.png")
   terrain  = p.createMultiBody(0, terrainShape)
@@ -78,6 +104,7 @@ for i in range(3):
       for joint in range(p.getNumJoints(sphereUid)):
         p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10)
 
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
 p.setGravity(0, 0, -10)
 p.setRealTimeSimulation(1)
 
@@ -85,9 +112,10 @@ p.getNumJoints(sphereUid)
 for i in range(p.getNumJoints(sphereUid)):
   p.getJointInfo(sphereUid, i)
 
-while (1):
-  keys = p.getKeyboardEvents()
-  print(keys)
+
+while (p.isConnected()):
+  #keys = p.getKeyboardEvents()
+  #print(keys)
   #getCameraImage note: software/TinyRenderer doesn't render/support heightfields!
   #p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL)
   time.sleep(0.01)
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index c6894629a..c3fcdbb45 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -1,3 +1,4 @@
+
 #include "../SharedMemory/PhysicsClientC_API.h"
 #include "../SharedMemory/PhysicsDirectC_API.h"
 #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
@@ -336,18 +337,19 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec
 				struct b3ForwardDynamicsAnalyticsArgs analyticsData;
 				int numIslands = 0;
 				int i;
-                                PyObject* val = 0;
+				PyObject* val = 0;
+				PyObject* pyAnalyticsData;
                                 
 				numIslands = b3GetStatusForwardDynamicsAnalyticsData(statusHandle, &analyticsData);
-				PyObject* pyAnalyticsData = PyTuple_New(numIslands);
+				pyAnalyticsData = PyTuple_New(numIslands);
                                 
 				for (i=0;i<numIslands;i++)
 				{
-                                        val = Py_BuildValue("{s:i, s:i, s:i, s:d}",
-                                                            "islandId", analyticsData.m_islandData[i].m_islandId,
-                                                            "numBodies", analyticsData.m_islandData[i].m_numBodies,
-                                                            "numIterationsUsed", analyticsData.m_islandData[i].m_numIterationsUsed,
-                                                            "remainingResidual", analyticsData.m_islandData[i].m_remainingLeastSquaresResidual);
+					val = Py_BuildValue("{s:i, s:i, s:i, s:d}",
+					"islandId", analyticsData.m_islandData[i].m_islandId,
+					"numBodies", analyticsData.m_islandData[i].m_numBodies,
+					"numIterationsUsed", analyticsData.m_islandData[i].m_numIterationsUsed,
+					"remainingResidual", analyticsData.m_islandData[i].m_remainingLeastSquaresResidual);
 					PyTuple_SetItem(pyAnalyticsData, i, val);
 				}
 
@@ -7960,6 +7962,7 @@ static int extractIndices(PyObject* indicesObj, int* indices, int maxNumIndices)
 	return numIndicesOut;
 }
 
+
 static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int physicsClientId = 0;
@@ -7981,10 +7984,29 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 	PyObject* halfExtentsObj = 0;
 	PyObject* verticesObj = 0;
 	PyObject* indicesObj = 0;
-
-	static char* kwlist[] = {"shapeType", "radius", "halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "collisionFramePosition", "collisionFrameOrientation", "vertices", "indices", "heightfieldTextureScaling", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOdi", kwlist,
-									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &heightfieldTextureScaling, &physicsClientId))
+	PyObject* heightfieldDataObj = 0;
+	int numHeightfieldRows = -1;
+	int numHeightfieldColumns = -1;
+
+	static char* kwlist[] = {"shapeType",
+							 "radius",
+							 "halfExtents",
+							 "height",
+							 "fileName",
+							 "meshScale",
+							 "planeNormal",
+							 "flags",
+							 "collisionFramePosition",
+							 "collisionFrameOrientation",
+							 "vertices",
+							 "indices",
+							 "heightfieldTextureScaling",
+							 "heightfieldData",
+							 "numHeightfieldRows",
+							 "numHeightfieldColumns",
+							 "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOdOiii", kwlist,
+									 &shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &heightfieldTextureScaling, &heightfieldDataObj, &numHeightfieldRows, &numHeightfieldColumns, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -8029,6 +8051,43 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 			shapeIndex = b3CreateCollisionShapeAddHeightfield(commandHandle, fileName, meshScale, heightfieldTextureScaling);
 
 		}
+		if (shapeType == GEOM_HEIGHTFIELD && fileName==0 && heightfieldDataObj && numHeightfieldColumns>0 && numHeightfieldRows > 0)
+		{
+			if (meshScaleObj)
+			{
+				pybullet_internalSetVectord(meshScaleObj, meshScale);
+			}
+			PyObject* seqPoints = PySequence_Fast(heightfieldDataObj, "expected a sequence");
+			int numHeightfieldPoints = PySequence_Size(heightfieldDataObj);
+			if (numHeightfieldPoints != numHeightfieldColumns*numHeightfieldRows)
+			{
+				PyErr_SetString(SpamError, "Size of heightfieldData doesn't match numHeightfieldColumns*numHeightfieldRows");
+				return NULL;
+			}
+			PyObject* item;
+			int i;
+			float* pointBuffer = (float*)malloc(numHeightfieldPoints*sizeof(float));
+			if (PyList_Check(seqPoints))
+			{
+				for (i = 0; i < numHeightfieldPoints; i++)
+				{
+					item = PyList_GET_ITEM(seqPoints, i);
+					pointBuffer[i] = (float)PyFloat_AsDouble(item);
+				}
+			}
+			else
+			{
+				for (i = 0; i < numHeightfieldPoints; i++)
+				{
+					item = PyTuple_GET_ITEM(seqPoints, i);
+					pointBuffer[i] = (float)PyFloat_AsDouble(item);
+				}
+			}
+			shapeIndex = b3CreateCollisionShapeAddHeightfield2(sm, commandHandle, meshScale, heightfieldTextureScaling, pointBuffer, numHeightfieldRows, numHeightfieldColumns);
+			free(pointBuffer);
+			if (seqPoints)
+				Py_DECREF(seqPoints);
+		}
 		if (shapeType == GEOM_MESH && fileName)
 		{
 			pybullet_internalSetVectord(meshScaleObj, meshScale);
@@ -10969,7 +11028,7 @@ static PyObject* pybullet_calculateInverseKinematics2(PyObject* self,
 	double residualThreshold = -1;
 
 	static char* kwlist[] = { "bodyUniqueId", "endEffectorLinkIndices", "targetPositions",  "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "currentPositions", "maxNumIterations", "residualThreshold", "physicsClientId", NULL };
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|OOOOOOiOidi", kwlist, &bodyUniqueId, &endEffectorLinkIndicesObj, &targetPosObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &currentPositionsObj, &maxNumIterations, &residualThreshold, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|OOOOOiOidi", kwlist, &bodyUniqueId, &endEffectorLinkIndicesObj, &targetPosObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &currentPositionsObj, &maxNumIterations, &residualThreshold, &physicsClientId))
 	{
 			return NULL;
 	}