From 51fba6f78d118c4a93d01222008e600735861372 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 29 Jul 2019 20:23:38 -0700 Subject: [PATCH] export btHeightfieldTerrainShape to PyBullet. Note that tinyrenderer doesn't support rendering it (it would be too slow on CPU) fix rare getKeyboardEvents threading issue change texture color to default plane.urdf blue --- .../BulletWorldImporter/btWorldImporter.cpp | 14 ++ .../BulletWorldImporter/btWorldImporter.h | 6 + examples/ExampleBrowser/OpenGLGuiHelper.cpp | 19 +- examples/Heightfield/HeightfieldExample.cpp | 33 +++- examples/SharedMemory/PhysicsClientC_API.cpp | 26 +++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 187 +++++++++++++----- .../SharedMemory/PhysicsServerExample.cpp | 9 +- examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/pybullet/examples/heightfield.py | 28 ++- .../gym/pybullet_data/heightmaps/Image8x4.png | Bin 0 -> 980 bytes examples/pybullet/pybullet.c | 17 +- .../btHeightfieldTerrainShape.cpp | 6 +- .../btHeightfieldTerrainShape.h | 9 + 14 files changed, 286 insertions(+), 71 deletions(-) create mode 100644 examples/pybullet/gym/pybullet_data/heightmaps/Image8x4.png diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp index c3fa088aa..df4106596 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp @@ -15,6 +15,7 @@ subject to the following restrictions: #include "btWorldImporter.h" #include "btBulletDynamicsCommon.h" +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" #ifdef USE_GIMPACT #include "BulletCollision/Gimpact/btGImpactShape.h" #endif @@ -1784,6 +1785,19 @@ btMultiSphereShape* btWorldImporter::createMultiSphereShape(const btVector3* pos return shape; } +class btHeightfieldTerrainShape* btWorldImporter::createHeightfieldShape(int heightStickWidth, int heightStickLength, + const void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, + int upAxis, int heightDataType, + bool flipQuadEdges) +{ + + btHeightfieldTerrainShape* shape = new btHeightfieldTerrainShape(heightStickWidth, heightStickLength, + heightfieldData, heightScale, minHeight, maxHeight, upAxis, PHY_ScalarType(heightDataType), flipQuadEdges); + m_allocatedCollisionShapes.push_back(shape); + return shape; +} + btRigidBody& btWorldImporter::getFixedBody() { static btRigidBody s_fixed(0, 0, 0); diff --git a/Extras/Serialize/BulletWorldImporter/btWorldImporter.h b/Extras/Serialize/BulletWorldImporter/btWorldImporter.h index 696064ab4..ba087fba7 100644 --- a/Extras/Serialize/BulletWorldImporter/btWorldImporter.h +++ b/Extras/Serialize/BulletWorldImporter/btWorldImporter.h @@ -197,6 +197,12 @@ public: virtual btTriangleIndexVertexArray* createMeshInterface(btStridingMeshInterfaceData& meshData); + virtual class btHeightfieldTerrainShape* createHeightfieldShape(int heightStickWidth, int heightStickLength, + const void* heightfieldData, btScalar heightScale, + btScalar minHeight, btScalar maxHeight, + int upAxis, int heightDataType, + bool flipQuadEdges); + ///acceleration and connectivity structures virtual btOptimizedBvh* createOptimizedBvh(); virtual btTriangleInfoMap* createTriangleInfoMap(); diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index f5d6dfbf0..afd810980 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -262,9 +262,10 @@ public: btAlignedObjectArray* m_pVerticesOut; btAlignedObjectArray* m_pIndicesOut; btVector3 m_aabbMin, m_aabbMax; + btScalar m_textureScaling; MyTriangleCollector2(const btVector3& aabbMin, const btVector3& aabbMax) - :m_aabbMin(aabbMin), m_aabbMax(aabbMax) + :m_aabbMin(aabbMin), m_aabbMax(aabbMax), m_textureScaling(1) { m_pVerticesOut = 0; m_pIndicesOut = 0; @@ -284,9 +285,11 @@ public: v.xyzw[l] = tris[k][l]; v.normal[l] = normal[l]; } - - v.uv[1] = 1-((v.xyzw[0] - m_aabbMin[0]) / (m_aabbMax[0] - m_aabbMin[0])); - v.uv[0] = ((v.xyzw[1] - m_aabbMin[1]) / (m_aabbMax[1] - m_aabbMin[1])); + + btVector3 extents = m_aabbMax - m_aabbMin; + + v.uv[0] = (1.-((v.xyzw[0] - m_aabbMin[0]) / (m_aabbMax[0] - m_aabbMin[0])))*m_textureScaling; + v.uv[1] = (1.-(v.xyzw[1] - m_aabbMin[1]) / (m_aabbMax[1] - m_aabbMin[1]))*m_textureScaling; m_pIndicesOut->push_back(m_pVerticesOut->size()); m_pVerticesOut->push_back(v); @@ -433,7 +436,7 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli if (m_data->m_checkedTexture < 0) { - m_data->m_checkedTexture = createCheckeredTexture(192, 192, 255); + m_data->m_checkedTexture = createCheckeredTexture(173, 199, 255); } if (m_data->m_checkedTextureGrey < 0) @@ -459,6 +462,10 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli tr.setIdentity(); heightField->getAabb(tr, aabbMin, aabbMax); MyTriangleCollector2 col(aabbMin, aabbMax); + if (heightField->getUserValue3()) + { + col.m_textureScaling = heightField->getUserValue3(); + } col.m_pVerticesOut = &gfxVertices; col.m_pIndicesOut = &indices; for (int k = 0; k < 3; k++) @@ -472,7 +479,7 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli int userImage = heightField->getUserIndex2(); if (userImage == -1) { - userImage = m_data->m_checkedTextureGrey; + userImage = m_data->m_checkedTexture; } int shapeId = m_data->m_glApp->m_renderer->registerShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(),1, userImage); collisionShape->setUserIndex(shapeId); diff --git a/examples/Heightfield/HeightfieldExample.cpp b/examples/Heightfield/HeightfieldExample.cpp index b93aa8df6..96d32c441 100644 --- a/examples/Heightfield/HeightfieldExample.cpp +++ b/examples/Heightfield/HeightfieldExample.cpp @@ -491,8 +491,22 @@ getRawHeightfieldData for (int j = 0; j < width; ++j) { float y = j * s_gridSpacing; - float z = double(image[i*3+width*j*3])*(40./256.); + float z = double(image[i*3+width*j*3])*(14./256.); convertFromFloat(p, z, type); + // update min/max + if (!i && !j) { + minHeight = z; + maxHeight = z; + } + else { + if (z < minHeight) { + minHeight = z; + } + if (z > maxHeight) { + maxHeight = z; + } + } + p += bytesPerElement; } } @@ -574,6 +588,19 @@ getRawHeightfieldData float y = j * s_gridSpacing; float z = allValues[i+width*j]; convertFromFloat(p, z, type); + // update min/max + if (!i && !j) { + minHeight = z; + maxHeight = z; + } + else { + if (z < minHeight) { + minHeight = z; + } + if (z > maxHeight) { + maxHeight = z; + } + } p += bytesPerElement; } } @@ -1159,11 +1186,11 @@ void HeightfieldExample::resetPhysics(void) // set origin to middle of heightfield btTransform tr; tr.setIdentity(); - tr.setOrigin(btVector3(0, 0, 0)); + tr.setOrigin(btVector3(0, 0, -4)); if (m_model== eImageFile) { - tr.setOrigin(btVector3(0, 0, -24)); + b3BulletDefaultFileIO fileIO; char relativeFileName[1024]; int found = fileIO.findFile("heightmaps/gimp_overlay_out.png", relativeFileName, 1024); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1b29da628..aed794cf2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1310,6 +1310,32 @@ B3_SHARED_API int b3CreateVisualShapeAddCapsule(b3SharedMemoryCommandHandle comm return b3CreateCollisionShapeAddCapsule(commandHandle, radius, height); } +B3_SHARED_API int b3CreateCollisionShapeAddHeightfield(b3SharedMemoryCommandHandle commandHandle, const char* fileName, const double meshScale[/*3*/], double textureScaling) +{ + 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; + strcpy(command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName, fileName); + 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_numUserShapes++; + return shapeIndex; + } + } + return -1; +} + B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, double radius, double height) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d3b9ecbb1..d4fb247e0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -488,6 +488,8 @@ extern "C" B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle, const double halfExtents[/*3*/]); 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 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(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double meshScale[/*3*/], const double* vertices, int numVertices); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2e0b64334..23e0bd1e8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -15,7 +15,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h" #include "../Utils/b3BulletDefaultFileIO.h" - +#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" @@ -1610,6 +1610,7 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_strings; btAlignedObjectArray m_collisionShapes; + btAlignedObjectArray m_heightfieldDatas; btAlignedObjectArray m_allocatedTextures; btHashMap m_bulletCollisionShape2UrdfCollision; btAlignedObjectArray m_meshInterfaces; @@ -2806,6 +2807,10 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() } delete shape; } + for (int j = 0; j < m_data->m_heightfieldDatas.size(); j++) + { + delete[] m_data->m_heightfieldDatas[j]; + } for (int j = 0; j < m_data->m_meshInterfaces.size(); j++) { delete m_data->m_meshInterfaces[j]; @@ -4217,18 +4222,25 @@ bool PhysicsServerCommandProcessor::processSaveWorldCommand(const struct SharedM #define MYLINELENGTH 16*32768 -static unsigned char* MyGetRawHeightfieldData(const char* fileName, int& width, int& height) +static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY_ScalarType type, const char* fileName, int& width, int& height, + btScalar& minHeight, + btScalar& maxHeight) { - int s_gridSize = width; - btScalar s_gridSpacing = 0.5; - btScalar s_gridHeightScale = 0.02; - - PHY_ScalarType type = PHY_FLOAT; - if (1)//model == eImageFile) + + std::string ext; + std::string fn(fileName); + std::string ext_ = fn.substr(fn.size() - 4); + for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i) { - b3BulletDefaultFileIO fileIO; + ext += char(tolower(*i)); + } + + + if (ext != ".txt") + { + char relativeFileName[1024]; - int found = fileIO.findFile(fileName, relativeFileName, 1024); + int found = fileIO.findResourcePath(fileName, relativeFileName, 1024); b3AlignedObjectArray buffer; buffer.reserve(1024); @@ -4256,40 +4268,59 @@ static unsigned char* MyGetRawHeightfieldData(const char* fileName, int& width, unsigned char* image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3); if (image) { - fileIO.fileClose(fileId); - long nElements = ((long)s_gridSize) * s_gridSize; + int nElements = width * height; int bytesPerElement = sizeof(btScalar); btAssert(bytesPerElement > 0 && "bad bytes per element"); - long nBytes = nElements * bytesPerElement; + int nBytes = nElements * bytesPerElement; unsigned char * raw = new unsigned char[nBytes]; btAssert(raw && "out of memory"); unsigned char * p = raw; - for (int i = 0; i < width; ++i) + for (int j = 0; j < height; ++j) + { - float x = i * s_gridSpacing; - for (int j = 0; j < width; ++j) + + for (int i = 0; i < width; ++i) { - float y = j * s_gridSpacing; - float z = double(image[i * 3 + width*j * 3])*(40. / 256.); + + float z = double(image[(width-1-i) * 3+ width*j * 3])*(1. / 255.); btScalar * pf = (btScalar *)p; *pf = z; p += bytesPerElement; + // update min/max + if (!i && !j) + { + minHeight = z; + maxHeight = z; + } + else + { + if (z < minHeight) + { + minHeight = z; + } + if (z > maxHeight) + { + maxHeight = z; + } + } } } + free (image); + return raw; } } } -#if 0 - if (model == eCSVFile) + + if (ext == ".txt") { + //read a csv file as used in DeepLoco { - b3BulletDefaultFileIO fileIO; char relativePath[1024]; - int found = fileIO.findFile("heightmaps/ground0.txt", relativePath, 1024); + int found = fileIO.findResourcePath(fileName, relativePath, 1024); char lineBuffer[MYLINELENGTH]; int slot = fileIO.fileOpen(relativePath, "r"); int rows = 0; @@ -4302,37 +4333,40 @@ static unsigned char* MyGetRawHeightfieldData(const char* fileName, int& width, while (lineChar = fileIO.readLine(slot, lineBuffer, MYLINELENGTH)) { rows = 0; - char** values = urdfStrSplit(lineChar, ","); - if (values) + std::string line(lineChar); + int pos=0; + while (pos < line.length()) { - int index = 0; - char* value; - while (value = values[index++]) + int nextPos = pos+1; + while (nextPos < line.length()) { - std::string strval(value); - double v; - if (sscanf(value, "%lf", &v) == 1) + if (line[nextPos-1] == ',') { - //printf("strlen = %d\n", strval.length()); - //printf("value[%d,%d]=%s or (%f)", cols,rows,value, v); - allValues.push_back(v); - rows++; + break; } + nextPos++; } + std::string substr = line.substr(pos, nextPos-pos-1); + + double v; + if (sscanf(substr.c_str(), "%lf", &v) == 1) + { + allValues.push_back(v); + rows++; + } + pos = nextPos; } cols++; } - printf("done, rows=%d, cols=%d\n", rows, cols); - int width = rows - 1; - s_gridSize = rows; - s_gridSpacing = 0.2; - s_gridHeightScale = 0.2; + width = rows; + height = cols; + fileIO.fileClose(slot); - long nElements = ((long)s_gridSize) * s_gridSize; + int nElements = width * height; // std::cerr << " nElements = " << nElements << "\n"; - int bytesPerElement = getByteSize(type); + int bytesPerElement = sizeof(btScalar); // std::cerr << " bytesPerElement = " << bytesPerElement << "\n"; btAssert(bytesPerElement > 0 && "bad bytes per element"); @@ -4341,23 +4375,43 @@ static unsigned char* MyGetRawHeightfieldData(const char* fileName, int& width, unsigned char * raw = new unsigned char[nBytes]; btAssert(raw && "out of memory"); - byte_t * p = raw; + unsigned char* p = raw; for (int i = 0; i < width; ++i) { - float x = i * s_gridSpacing; - for (int j = 0; j < width; ++j) + + for (int j = 0; j < height; ++j) { - float y = j * s_gridSpacing; + float z = allValues[i + width*j]; - convertFromFloat(p, z, type); + //convertFromFloat(p, z, type); + btScalar * pf = (btScalar *)p; + *pf = z; p += bytesPerElement; + // update min/max + if (!i && !j) + { + minHeight = z; + maxHeight = z; + } + else + { + if (z < minHeight) + { + minHeight = z; + } + if (z > maxHeight) + { + maxHeight = z; + } + } + } } return raw; } } } -#endif + return 0; } @@ -4474,7 +4528,43 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str } case GEOM_HEIGHTFIELD: { - + int width; + int height; + 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); + if (heightfieldData) + { + + btScalar gridSpacing = 0.5; + btScalar gridHeightScale = 1. / 256.; + + bool flipQuadEdges = false; + int upAxis = 2; + btHeightfieldTerrainShape* heightfieldShape = worldImporter->createHeightfieldShape( width, height, + heightfieldData, + gridHeightScale, + minHeight, maxHeight, + upAxis, int(scalarType), flipQuadEdges); + + heightfieldShape->setUserValue3(clientCmd.m_createUserShapeArgs.m_shapes[i].m_heightfieldTextureScaling); + shape = heightfieldShape; + if (upAxis == 2) + heightfieldShape->setFlipTriangleWinding(true); + //buildAccelerator is optional, it may not support all features. + heightfieldShape->buildAccelerator(); + + // scale the shape + btVector3 localScaling(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1], + clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]); + + heightfieldShape->setLocalScaling(localScaling); + + this->m_data->m_heightfieldDatas.push_back(heightfieldData); + + } break; } case GEOM_PLANE: @@ -4497,6 +4587,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str break; } + case GEOM_MESH: { btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0], diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index eadded409..f5e1046e1 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1499,6 +1499,7 @@ public: { //printf("key=%d, state=%d\n", key,state); { + m_args[0].m_csGUI->lock(); int keyIndex = -1; //is already there? for (int i = 0; i < m_args[0].m_keyboardEvents.size(); i++) @@ -1515,7 +1516,7 @@ public: b3KeyboardEvent ev; ev.m_keyCode = key; ev.m_keyState = eButtonIsDown + eButtonTriggered; - m_args[0].m_csGUI->lock(); + if (keyIndex >= 0) { if (0 == (m_args[0].m_keyboardEvents[keyIndex].m_keyState & eButtonIsDown)) @@ -1527,11 +1528,10 @@ public: { m_args[0].m_keyboardEvents.push_back(ev); } - m_args[0].m_csGUI->unlock(); + } else { - m_args[0].m_csGUI->lock(); b3KeyboardEvent ev; ev.m_keyCode = key; ev.m_keyState = eButtonReleased; @@ -1543,8 +1543,9 @@ public: { m_args[0].m_keyboardEvents.push_back(ev); } - m_args[0].m_csGUI->unlock(); + } + m_args[0].m_csGUI->unlock(); } /*printf("m_args[0].m_keyboardEvents.size()=%d\n", m_args[0].m_keyboardEvents.size()); for (int i=0;i`1bEam91J8DHW)Ckn_