make pybullet.getMeshData work for softbody

This commit is contained in:
Erwin Coumans
2019-06-17 21:43:38 -07:00
parent 20d9ad5f24
commit 7e76ee0ad7
10 changed files with 244 additions and 198 deletions

View File

@@ -1210,7 +1210,8 @@ B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle co
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId){
B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
@@ -1220,13 +1221,16 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClie
b3Assert(command);
command->m_type = CMD_REQUEST_MESH_DATA;
command->m_updateFlags = 0;
command->m_requestMeshDataArgs.m_startingVertex = 0;
command->m_requestMeshDataArgs.m_bodyUniqueId = bodyUniqueId;
command->m_requestMeshDataArgs.m_linkIndex = linkIndex;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData){
B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
@@ -1234,9 +1238,6 @@ B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3Mesh
}
}
B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius)
{
return b3CreateCollisionShapeAddSphere(commandHandle, radius);

View File

@@ -491,7 +491,7 @@ extern "C"
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData);

View File

@@ -54,7 +54,7 @@ struct PhysicsClientSharedMemoryInternalData
btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
b3MeshData m_cachedMeshData;
btAlignedObjectArray<double> m_cachedVertexPositions;
btAlignedObjectArray<b3MeshVertex> m_cachedVertexPositions;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
@@ -100,6 +100,8 @@ struct PhysicsClientSharedMemoryInternalData
m_verboseOutput(false),
m_timeOutInSeconds(1e30)
{
m_cachedMeshData.m_numVertices = 0;
m_cachedMeshData.m_vertices = 0;
}
void processServerStatus();
@@ -1037,6 +1039,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
}
case CMD_REQUEST_MESH_DATA_COMPLETED:
{
m_data->m_cachedVertexPositions.resize(serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied);
btVector3* verticesReceived = (btVector3*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; i++)
{
m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].x = verticesReceived[i].x();
m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].y = verticesReceived[i].y();
m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].z = verticesReceived[i].z();
m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].w = verticesReceived[i].w();
}
break;
}
case CMD_REQUEST_MESH_DATA_FAILED:
@@ -1770,7 +1781,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
}
}
if(serverCmd.m_type == CMD_REQUEST_MESH_DATA_COMPLETED)
if (serverCmd.m_type == CMD_REQUEST_MESH_DATA_COMPLETED)
{
B3_PROFILE("CMD_REQUEST_MESH_DATA_COMPLETED");
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@@ -1778,7 +1789,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
if (serverCmd.m_sendMeshDataArgs.m_numVerticesRemaining > 0 && serverCmd.m_sendMeshDataArgs.m_numVerticesCopied)
{
command.m_type = CMD_REQUEST_MESH_DATA;
command.m_requestMeshDataArgs.m_startingVertex=
command.m_requestMeshDataArgs.m_startingVertex =
serverCmd.m_sendMeshDataArgs.m_startingVertex +
serverCmd.m_sendMeshDataArgs.m_numVerticesCopied;
submitClientCommand(command);
@@ -1786,7 +1797,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
}
else
{
m_data->m_cachedMeshData.m_numVertices =serverCmd.m_sendMeshDataArgs.m_startingVertex +serverCmd.m_sendMeshDataArgs.m_numVerticesCopied;
m_data->m_cachedMeshData.m_numVertices = serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied;
}
}
@@ -1972,8 +1983,12 @@ void PhysicsClientSharedMemory::getCachedCollisionShapeInformation(struct b3Coll
collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0;
}
void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData)
{
m_data->m_cachedMeshData.m_numVertices = m_data->m_cachedVertexPositions.size();
m_data->m_cachedMeshData.m_vertices = m_data->m_cachedMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0;
void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData){
*meshData = m_data->m_cachedMeshData;
}

View File

@@ -67,7 +67,7 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
b3MeshData m_cachedMeshData;
btAlignedObjectArray<double> m_cachedVertexPositions;
btAlignedObjectArray<b3MeshVertex> m_cachedVertexPositions;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
@@ -94,6 +94,7 @@ struct PhysicsDirectInternalData
m_ownsCommandProcessor(false),
m_timeOutInSeconds(1e30)
{
memset(&m_cachedMeshData.m_numVertices, 0, sizeof(b3MeshData));
memset(&m_command, 0, sizeof(m_command));
memset(&m_serverStatus, 0, sizeof(m_serverStatus));
memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient));
@@ -589,7 +590,6 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
return m_data->m_hasStatus;
}
bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
@@ -623,20 +623,22 @@ bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand
}
const b3SendMeshDataArgs& args = serverCmd.m_sendMeshDataArgs;
int numTotalPixels =args.m_startingVertex +
int numTotalPixels = args.m_startingVertex +
args.m_numVerticesCopied + args.m_numVerticesRemaining;
double* verticesReceived =
(double*)&m_data->m_bulletStreamDataServerToClient[0];
btVector3* verticesReceived =
(btVector3*)&m_data->m_bulletStreamDataServerToClient[0];
// flattened position
const int dimension = 3;
m_data->m_cachedVertexPositions.resize((args.m_startingVertex +
args.m_numVerticesCopied)*dimension);
for (int i = 0; i < dimension*args.m_numVerticesCopied; i++)
m_data->m_cachedVertexPositions.resize(args.m_startingVertex +
args.m_numVerticesCopied);
for (int i = 0; i < args.m_numVerticesCopied; i++)
{
m_data->m_cachedVertexPositions[i + dimension*args.m_startingVertex] = verticesReceived[i];
m_data->m_cachedVertexPositions[i + args.m_startingVertex].x = verticesReceived[i].x();
m_data->m_cachedVertexPositions[i + args.m_startingVertex].y = verticesReceived[i].y();
m_data->m_cachedVertexPositions[i + args.m_startingVertex].z = verticesReceived[i].z();
m_data->m_cachedVertexPositions[i + args.m_startingVertex].w = verticesReceived[i].w();
}
if (args.m_numVerticesRemaining > 0 && args.m_numVerticesCopied)
@@ -650,7 +652,7 @@ bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand
}
else
{
m_data->m_cachedMeshData.m_numVertices=args.m_startingVertex +
m_data->m_cachedMeshData.m_numVertices = args.m_startingVertex +
args.m_numVerticesCopied;
}
}
@@ -1526,9 +1528,15 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
}
}
void PhysicsDirect::getCachedMeshData(struct b3MeshData* meshData){
void PhysicsDirect::getCachedMeshData(struct b3MeshData* meshData)
{
m_data->m_cachedMeshData.m_numVertices = m_data->m_cachedVertexPositions.size();
m_data->m_cachedMeshData.m_vertices = m_data->m_cachedMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0;
*meshData = m_data->m_cachedMeshData;
}
void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();

View File

@@ -4636,10 +4636,35 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId);
if (bodyHandle)
{
if (bodyHandle->m_multiBody)
{
//todo
}
if (bodyHandle->m_rigidBody)
{
//todo
}
if (bodyHandle->m_softBody)
{
btSoftBody* psb = bodyHandle->m_softBody;
int totalBytesPerVertex = sizeof(btVector3);
int numVertices = psb->m_nodes.size();
int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
btVector3* verticesOut = (btVector3*)bufferServerToClient;
for (int i = 0; i < verticesCopied; ++i)
{
const btSoftBody::Node& n = psb->m_nodes[i+ clientCmd.m_requestMeshDataArgs.m_startingVertex];
verticesOut[i] = n.m_x;
}
serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = 1;
serverStatusOut.m_sendMeshDataArgs.m_startingVertex = 0;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = 0;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex;
serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied;
}
}
serverStatusOut.m_numDataStreamBytes = 0;
@@ -4647,7 +4672,6 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
return hasStatus;
}
bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -11309,7 +11333,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_MESH_DATA:{
case CMD_REQUEST_MESH_DATA:
{
hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}

View File

@@ -16,7 +16,7 @@
struct RemoteGUIHelperInternalData
{
// GUIHelperInterface* m_guiHelper;
// GUIHelperInterface* m_guiHelper;
bool m_waitingForServer;
GraphicsSharedMemoryBlock* m_testBlock1;
SharedMemoryInterface* m_sharedMemory;
@@ -36,7 +36,6 @@ struct RemoteGUIHelperInternalData
m_sharedMemoryKey = GRAPHICS_SHARED_MEMORY_KEY;
m_isConnected = false;
connect();
}
virtual ~RemoteGUIHelperInternalData()
@@ -98,7 +97,6 @@ struct RemoteGUIHelperInternalData
return false;
}
const GraphicsSharedMemoryStatus* processServerStatus()
{
// SharedMemoryStatus* stat = 0;
@@ -120,7 +118,6 @@ struct RemoteGUIHelperInternalData
return &m_lastServerStatus;
}
if (m_testBlock1->m_numServerCommands >
m_testBlock1->m_numProcessedServerCommands)
{
@@ -140,7 +137,6 @@ struct RemoteGUIHelperInternalData
{
B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED");
break;
}
default:
@@ -163,7 +159,6 @@ struct RemoteGUIHelperInternalData
m_waitingForServer = true;
}
return &m_lastServerStatus;
}
return 0;
@@ -199,7 +194,6 @@ struct RemoteGUIHelperInternalData
return true;
}
void disconnect()
{
if (m_isConnected && m_sharedMemory)
@@ -208,15 +202,8 @@ struct RemoteGUIHelperInternalData
}
m_isConnected = false;
}
};
RemoteGUIHelper::RemoteGUIHelper()
{
m_data = new RemoteGUIHelperInternalData;
@@ -264,11 +251,11 @@ void RemoteGUIHelper::createCollisionObjectGraphicsObject(btCollisionObject* bod
{
// btAssert(graphicsShapeId >= 0);
//the graphics shape is already scaled
float localScaling[4] = { 1.f, 1.f, 1.f, 1.f };
float colorRGBA[4] = { (float)color[0], (float)color[1], (float)color[2], (float) color[3] };
float pos[4] = { (float)startTransform.getOrigin()[0], (float)startTransform.getOrigin()[1], (float)startTransform.getOrigin()[2], (float)startTransform.getOrigin()[3] };
float orn[4] = { (float)startTransform.getRotation()[0], (float)startTransform.getRotation()[1], (float)startTransform.getRotation()[2], (float)startTransform.getRotation()[3] };
int graphicsInstanceId = registerGraphicsInstance(graphicsShapeId, pos,orn, colorRGBA, localScaling);
float localScaling[4] = {1.f, 1.f, 1.f, 1.f};
float colorRGBA[4] = {(float)color[0], (float)color[1], (float)color[2], (float)color[3]};
float pos[4] = {(float)startTransform.getOrigin()[0], (float)startTransform.getOrigin()[1], (float)startTransform.getOrigin()[2], (float)startTransform.getOrigin()[3]};
float orn[4] = {(float)startTransform.getRotation()[0], (float)startTransform.getRotation()[1], (float)startTransform.getRotation()[2], (float)startTransform.getRotation()[3]};
int graphicsInstanceId = registerGraphicsInstance(graphicsShapeId, pos, orn, colorRGBA, localScaling);
body->setUserIndex(graphicsInstanceId);
}
}
@@ -319,12 +306,8 @@ void RemoteGUIHelper::syncPhysicsToGraphics2(const btDiscreteDynamicsWorld* rbWo
}
}
void RemoteGUIHelper::syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions)
{
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
@@ -338,8 +321,6 @@ void RemoteGUIHelper::syncPhysicsToGraphics2(const GUISyncPosition* positions, i
while ((status = m_data->processServerStatus()) == 0)
{
}
}
void RemoteGUIHelper::render(const btDiscreteDynamicsWorld* rbWorld)
@@ -364,7 +345,7 @@ int RemoteGUIHelper::uploadData(const unsigned char* data, int sizeInBytes, int
{
for (int i = 0; i < curBytes; i++)
{
m_data->m_testBlock1->m_bulletStreamData[i] = data[i+offset];
m_data->m_testBlock1->m_bulletStreamData[i] = data[i + offset];
}
cmd->m_updateFlags = 0;
@@ -391,11 +372,10 @@ int RemoteGUIHelper::registerTexture(const unsigned char* texels, int width, int
//first upload all data
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
int sizeInBytes = width*height * 3;//rgb
int sizeInBytes = width * height * 3; //rgb
uploadData(texels, sizeInBytes, 0);
cmd->m_updateFlags = 0;
cmd->m_type = GFX_CMD_REGISTER_TEXTURE;
@@ -412,7 +392,6 @@ int RemoteGUIHelper::registerTexture(const unsigned char* texels, int width, int
}
}
return textureId;
}
@@ -420,7 +399,6 @@ int RemoteGUIHelper::registerGraphicsShape(const float* vertices, int numvertice
{
int shapeId = -1;
GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
if (cmd)
{
@@ -473,7 +451,6 @@ int RemoteGUIHelper::registerGraphicsInstance(int shapeIndex, const float* posit
{
graphicsInstanceId = status->m_registerGraphicsInstanceStatus.m_graphicsInstanceId;
}
}
return graphicsInstanceId;
}

View File

@@ -1054,12 +1054,15 @@ struct AddUserDataRequestArgs
// Value data stored in m_bulletStreamDataServerToClientRefactor.
};
struct b3RequestMeshDataArgs{
struct b3RequestMeshDataArgs
{
int m_bodyUniqueId;
int m_linkIndex;
int m_startingVertex;
};
struct b3SendMeshDataArgs{
struct b3SendMeshDataArgs
{
int m_numVerticesCopied;
int m_startingVertex;
int m_numVerticesRemaining;

View File

@@ -101,7 +101,6 @@ enum EnumSharedMemoryClientCommand
CMD_ADD_USER_DATA,
CMD_REMOVE_USER_DATA,
CMD_COLLISION_FILTER,
CMD_REQUEST_MESH_DATA,
//don't go beyond this command!
@@ -411,10 +410,15 @@ struct b3CameraImageData
const int* m_segmentationMaskValues; //m_pixelWidth*m_pixelHeight ints
};
struct b3MeshVertex
{
double x, y, z, w;
};
struct b3MeshData
{
int m_numVertices;
double* m_vertices;
struct b3MeshVertex* m_vertices;
};
struct b3OpenGLVisualizerCameraInfo

View File

@@ -1,11 +1,13 @@
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
physicsClient = p.connect(p.DIRECT)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
bunnyId = p.loadSoftBody("bunny.obj")
#meshData = p.getMeshData(bunnyId)
#print("meshData=",meshData)
p.loadURDF("cube_small.urdf", [1, 0, 1])
useRealTimeSimulation = 1

View File

@@ -7358,11 +7358,10 @@ static PyObject* pybullet_createCollisionShapeArray(PyObject* self, PyObject* ar
return NULL;
}
static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId = -1;
int linkIndex = -1;
b3PhysicsClientHandle sm = 0;
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
@@ -7370,8 +7369,8 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
int statusType;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
{
return NULL;
}
@@ -7381,23 +7380,35 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
command = b3GetMeshDataCommandInit(sm, bodyUniqueId);
command = b3GetMeshDataCommandInit(sm, bodyUniqueId, linkIndex);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_MESH_DATA_COMPLETED)
{
int i;
PyObject* pyVertexData;
PyObject* pyListMeshData = PyTuple_New(2);
b3GetMeshData(sm, &meshData);
PyObject* pyListMeshData = PyTuple_New(1);
PyTuple_SetItem(pyListMeshData, 0, PyInt_FromLong(meshData.m_numVertices));
pyVertexData = PyTuple_New(meshData.m_numVertices);
PyTuple_SetItem(pyListMeshData, 1, pyVertexData);
for (i = 0; i < meshData.m_numVertices; i++)
{
PyObject* pyListVertex = PyTuple_New(3);
PyTuple_SetItem(pyListVertex, 0, PyFloat_FromDouble(meshData.m_vertices[i].x));
PyTuple_SetItem(pyListVertex, 1, PyFloat_FromDouble(meshData.m_vertices[i].y));
PyTuple_SetItem(pyListVertex, 2, PyFloat_FromDouble(meshData.m_vertices[i].z));
PyTuple_SetItem(pyVertexData, i, pyListVertex);
}
return pyListMeshData;
}
}
PyErr_SetString(SpamError, "Couldn't get body info");
PyErr_SetString(SpamError, "getMeshData failed");
return NULL;
}
static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;