diff options
author | erwincoumans <erwin.coumans@gmail.com> | 2019-04-04 09:47:16 -0700 |
---|---|---|
committer | GitHub <noreply@github.com> | 2019-04-04 09:47:16 -0700 |
commit | 473fd15a864cdcfe1633c4df8198ea42634135bf (patch) | |
tree | bfd303f025e363a54a61ec985e09e77e72630a13 | |
parent | 62add41ab4723707777d83c88a299b85c95f2684 (diff) | |
parent | 76918ca26dcc658323d936d12dbb247b26edfcc4 (diff) | |
download | bullet3-473fd15a864cdcfe1633c4df8198ea42634135bf.tar.gz |
Merge pull request #2182 from erwincoumans/master
implement PyBullet removeState command. Fixes Issue #2163
-rw-r--r-- | examples/SharedMemory/PhysicsClientC_API.cpp | 19 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientC_API.h | 1 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsClientSharedMemory.cpp | 2 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsDirect.cpp | 8 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 58 | ||||
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.h | 1 | ||||
-rw-r--r-- | examples/SharedMemory/SharedMemoryPublic.h | 9 | ||||
-rw-r--r-- | examples/pybullet/examples/saveRestoreState.py | 4 | ||||
-rw-r--r-- | examples/pybullet/pybullet.c | 37 |
9 files changed, 132 insertions, 7 deletions
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 59285ad68..1e515b576 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -108,6 +108,25 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadStateCommandInit(b3PhysicsClient return 0; } +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveStateCommand(b3PhysicsClientHandle physClient, int stateId) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + + if (cl->canSubmitCommand()) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_REMOVE_STATE; + command->m_updateFlags = 0; + command->m_loadStateArguments.m_fileName[0] = 0; + command->m_loadStateArguments.m_stateId = stateId; + return (b3SharedMemoryCommandHandle)command; + } + return 0; +} + B3_SHARED_API int b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6286233a9..9682486ec 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -375,6 +375,7 @@ extern "C" B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient); + B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveStateCommand(b3PhysicsClientHandle physClient, int stateId); B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle); B3_SHARED_API b3SharedMemoryCommandHandle b3LoadStateCommandInit(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 1990cc1fd..f6cd23d1c 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1429,6 +1429,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() 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: { break; } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 336f61804..dd5bbfb7f 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -1159,6 +1159,14 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } break; } + case CMD_REMOVE_STATE_FAILED: + { + break; + } + case CMD_REMOVE_STATE_COMPLETED: + { + break; + } default: { //b3Warning("Unknown server status type"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8cb562691..da65be89c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10778,7 +10778,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { - BT_PROFILE("CMD_RESTORE_STATE"); + BT_PROFILE("CMD_SAVE_STATE"); bool hasStatus = true; SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_SAVE_STATE_FAILED; @@ -10792,15 +10792,57 @@ bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedM if (bulletFile->ok()) { serverCmd.m_type = CMD_SAVE_STATE_COMPLETED; - serverCmd.m_saveStateResultArgs.m_stateId = m_data->m_savedStates.size(); + //re-use state if available + int reuseStateId = -1; + for (int i = 0; i < m_data->m_savedStates.size(); i++) + { + if (m_data->m_savedStates[i].m_bulletFile == 0) + { + reuseStateId = i; + break; + } + } SaveStateData sd; sd.m_bulletFile = bulletFile; sd.m_serializer = ser; - m_data->m_savedStates.push_back(sd); + if (reuseStateId >= 0) + { + serverCmd.m_saveStateResultArgs.m_stateId = reuseStateId; + m_data->m_savedStates[reuseStateId] = sd; + } + else + { + serverCmd.m_saveStateResultArgs.m_stateId = m_data->m_savedStates.size(); + m_data->m_savedStates.push_back(sd); + } + } + return hasStatus; +} + + +bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + BT_PROFILE("CMD_REMOVE_STATE"); + bool hasStatus = true; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_REMOVE_STATE_FAILED; + + if (clientCmd.m_loadStateArguments.m_stateId >= 0) + { + if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size()) + { + SaveStateData& sd = m_data->m_savedStates[clientCmd.m_loadStateArguments.m_stateId]; + delete sd.m_bulletFile; + delete sd.m_serializer; + sd.m_bulletFile = 0; + sd.m_serializer = 0; + serverCmd.m_type = CMD_REMOVE_STATE_COMPLETED; + } } return hasStatus; } + bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { BT_PROFILE("CMD_RESTORE_STATE"); @@ -10818,7 +10860,10 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar if (clientCmd.m_loadStateArguments.m_stateId < m_data->m_savedStates.size()) { bParse::btBulletFile* bulletFile = m_data->m_savedStates[clientCmd.m_loadStateArguments.m_stateId].m_bulletFile; - ok = importer->convertAllObjects(bulletFile); + if (bulletFile) + { + ok = importer->convertAllObjects(bulletFile); + } } } else @@ -11329,6 +11374,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } + case CMD_REMOVE_STATE: + { + hasStatus = processRemoveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_LOAD_BULLET: { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 237309b77..1b00a8af7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -81,6 +81,7 @@ protected: bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRemoveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRequestUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processAddUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 28e62700f..a569cbaf0 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,8 +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 201902120 +#define SHARED_MEMORY_MAGIC_NUMBER 201904030 +//#define SHARED_MEMORY_MAGIC_NUMBER 201902120 //#define SHARED_MEMORY_MAGIC_NUMBER 201811260 //#define SHARED_MEMORY_MAGIC_NUMBER 201810250 //#define SHARED_MEMORY_MAGIC_NUMBER 201809030 @@ -93,6 +93,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS, CMD_SAVE_STATE, CMD_RESTORE_STATE, + CMD_REMOVE_STATE, CMD_REQUEST_COLLISION_SHAPE_INFO, CMD_SYNC_USER_DATA, @@ -100,7 +101,7 @@ enum EnumSharedMemoryClientCommand CMD_ADD_USER_DATA, CMD_REMOVE_USER_DATA, CMD_COLLISION_FILTER, - + //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, }; @@ -218,6 +219,8 @@ enum EnumSharedMemoryServerStatus CMD_ADD_USER_DATA_FAILED, CMD_REMOVE_USER_DATA_COMPLETED, CMD_REMOVE_USER_DATA_FAILED, + CMD_REMOVE_STATE_COMPLETED, + CMD_REMOVE_STATE_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/pybullet/examples/saveRestoreState.py b/examples/pybullet/examples/saveRestoreState.py index dd1007527..7bac634be 100644 --- a/examples/pybullet/examples/saveRestoreState.py +++ b/examples/pybullet/examples/saveRestoreState.py @@ -80,6 +80,10 @@ setupWorld() #both restore from file or from in-memory state should work p.restoreState(fileName="state.bullet") stateId = p.saveState() +print("stateId=",stateId) +p.removeState(stateId) +stateId = p.saveState() +print("stateId=",stateId) if verbose: p.setInternalSimFlags(1) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 301d43164..fe3b18799 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1160,6 +1160,40 @@ static PyObject* pybullet_saveState(PyObject* self, PyObject* args, PyObject* ke return PyInt_FromLong(stateId); } +static PyObject* pybullet_removeState(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + int stateUniqueId = -1; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "stateUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &stateUniqueId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (stateUniqueId >= 0) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (b3CanSubmitCommand(sm)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitRemoveStateCommand(sm, stateUniqueId)); + statusType = b3GetStatusType(statusHandle); + } + } + } + + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds) { const char* mjcfFileName = ""; @@ -10329,6 +10363,9 @@ static PyMethodDef SpamMethods[] = { {"saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS, "Save the full state of the world to memory."}, + { "removeState", (PyCFunction)pybullet_removeState, METH_VARARGS | METH_KEYWORDS, + "Remove a state created using saveState by its state unique id." }, + {"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS, "Load multibodies from an MJCF file."}, |