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

pybullet: allow programmatic creation of heightfield. See https://github.com/erwincoumans/bullet3/tree/master/examples/pybullet/examples/heightfield.py

Browse Source 
premake4: allow to build example browser without C++11, re-enable stable PD control plugin using --enable_stable_pd=True
This commit is contained in:
Erwin Coumans 2019-08-11 13:59:24 -07:00
parent f5ae8bd1af
commit f9b232b153
10 changed files with 192 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/BulletRobotics/FixJointBoxes.cpp
View File

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

26
examples/ExampleBrowser/premake4.lua
View File

@ -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/*",

33
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -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*/]);

28
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

5
examples/SharedMemory/SharedMemoryBlock.h
View File

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

4
examples/SharedMemory/SharedMemoryCommands.h
View File

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

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

44
examples/pybullet/examples/heightfield.py
View File

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

79
examples/pybullet/pybullet.c
View File

@ -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;
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", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOdi", kwlist,
&shapeType, &radius, &halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &collisionFramePositionObj, &collisionFrameOrientationObj, &verticesObj, &indicesObj, &heightfieldTextureScaling, &physicsClientId))
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);