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

implement resetMeshData for deformable bodies, this allows better reset for reinforcement learning algorithms.

Browse Source
This commit is contained in:
Erwin Coumans 2021-09-02 17:27:05 -07:00
parent 3577ef8108
commit b6df08b553
9 changed files with 148 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -1484,6 +1484,28 @@ B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle co
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int numVertices, const double* vertices)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl)
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_RESET_MESH_DATA;
command->m_updateFlags = 0;
command->m_resetMeshDataArgs.m_numVertices = numVertices;
command->m_resetMeshDataArgs.m_bodyUniqueId = bodyUniqueId;
command->m_resetMeshDataArgs.m_flags = 0;
int totalUploadSizeInBytes = numVertices * sizeof(double) *3;
cl->uploadBulletFileToSharedMemory((const char*)vertices, totalUploadSizeInBytes);
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -534,6 +534,7 @@ extern "C"
B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData);
B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int num_vertices, const double* vertices);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius);

6
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -1521,12 +1521,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
b3Warning("Removing user data failed");
break;
}
case CMD_RESET_MESH_DATA_FAILED:
{
b3Warning("resetMeshData failed");
break;
}
case CMD_REQUEST_USER_DATA_COMPLETED:
case CMD_SYNC_USER_DATA_COMPLETED:
case CMD_REMOVE_USER_DATA_COMPLETED:
case CMD_ADD_USER_DATA_COMPLETED:
case CMD_REMOVE_STATE_FAILED:
case CMD_REMOVE_STATE_COMPLETED:
case CMD_RESET_MESH_DATA_COMPLETED:
{
break;
}

8
examples/SharedMemory/PhysicsDirect.cpp
View File

@ -1306,6 +1306,14 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
break;
}
case CMD_RESET_MESH_DATA_COMPLETED:
{
break;
}
case CMD_RESET_MESH_DATA_FAILED:
{
break;
}
case CMD_REMOVE_STATE_FAILED:
{
break;

44
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -5485,6 +5485,44 @@ static void gatherVertices(const btTransform& trans, const btCollisionShape* col
}
}
}
bool PhysicsServerCommandProcessor::processResetMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_REQUEST_MESH_DATA");
serverStatusOut.m_type = CMD_RESET_MESH_DATA_FAILED;
int sizeInBytes = 0;
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId);
if (bodyHandle)
{
int totalBytesPerVertex = sizeof(btVector3);
double* vertexUpload = (double*)bufferServerToClient;
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (bodyHandle->m_softBody)
{
btSoftBody* psb = bodyHandle->m_softBody;
int numVertices = psb->m_nodes.size();
if (clientCmd.m_resetMeshDataArgs.m_numVertices == numVertices)
{
for (int i = 0; i < numVertices; ++i)
{
btSoftBody::Node& n = psb->m_nodes[i];
n.m_x.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]);
}
serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED;
}
}
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
}
serverStatusOut.m_numDataStreamBytes = 0;
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@ -14218,6 +14256,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_RESET_MESH_DATA:
{
hasStatus = processResetMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_CREATE_MULTI_BODY:
{
hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@ -32,6 +32,7 @@ protected:
bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processResetMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSetVRCameraStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);

9
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -1141,6 +1141,13 @@ struct b3RequestMeshDataArgs
int m_flags;
};
struct b3ResetMeshDataArgs
{
int m_bodyUniqueId;
int m_numVertices;
int m_flags;
};
struct b3SendMeshDataArgs
{
int m_numVerticesCopied;
@ -1209,6 +1216,8 @@ struct SharedMemoryCommand
struct UserDataRequestArgs m_removeUserDataRequestArgs;
struct b3CollisionFilterArgs m_collisionFilterArgs;
struct b3RequestMeshDataArgs m_requestMeshDataArgs;
struct b3ResetMeshDataArgs m_resetMeshDataArgs;
};
};

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -117,6 +117,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_MESH_DATA,
CMD_PERFORM_COLLISION_DETECTION,
CMD_RESET_MESH_DATA,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
};
@ -241,6 +242,8 @@ enum EnumSharedMemoryServerStatus
CMD_REQUEST_MESH_DATA_FAILED,
CMD_PERFORM_COLLISION_DETECTION_COMPLETED,
CMD_RESET_MESH_DATA_COMPLETED,
CMD_RESET_MESH_DATA_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};

56
examples/pybullet/pybullet.c
View File

@ -1,4 +1,4 @@
//#include "D:/dev/visual leak detector/include/vld.h"
//#include "D:/dev/VisualLeakDetector/include/vld.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/PhysicsDirectC_API.h"
@ -9116,6 +9116,55 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
return NULL;
}
static PyObject* pybullet_resetMeshData(PyObject* self, PyObject* args, PyObject* keywds)
{
int bodyUniqueId = -1;
b3PhysicsClientHandle sm = 0;
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
struct b3MeshData meshData;
int statusType;
PyObject* verticesObj = 0;
int physicsClientId = 0;
int numVertices = 0;
static char* kwlist[] = { "bodyUniqueId", "vertices", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist, &bodyUniqueId, &verticesObj, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
numVertices = extractVertices(verticesObj, 0, B3_MAX_NUM_VERTICES);
if (numVertices)
{
double* vertices = numVertices ? malloc(numVertices * 3 * sizeof(double)) : 0;
numVertices = extractVertices(verticesObj, vertices, B3_MAX_NUM_VERTICES);
command = b3ResetMeshDataCommandInit(sm, bodyUniqueId, numVertices, vertices);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
free(vertices);
if (statusType == CMD_RESET_MESH_DATA_COMPLETED)
{
Py_INCREF(Py_None);
return Py_None;
}
}
PyErr_SetString(SpamError, "resetMeshData failed");
return NULL;
}
static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
@ -12522,9 +12571,12 @@ static PyMethodDef SpamMethods[] = {
{"removeCollisionShape", (PyCFunction)pybullet_removeCollisionShape, METH_VARARGS | METH_KEYWORDS,
"Remove a collision shape. Only useful when the collision shape is not used in a body (to perform a getClosestPoint query)."},
{"getMeshData", (PyCFunction)pybullet_getMeshData, METH_VARARGS | METH_KEYWORDS,
{"getMeshData", (PyCFunction)pybullet_getMeshData, METH_VARARGS | METH_KEYWORDS,
"Get mesh data. Returns vertices etc from the mesh."},
{"resetMeshData", (PyCFunction)pybullet_resetMeshData, METH_VARARGS | METH_KEYWORDS,
"Reset mesh data. Only implemented for deformable bodies."},
{"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS,
"Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},