diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0ec231e4a..a7d23f68c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2585,6 +2585,20 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa } return 0; } + +int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_STATE_LOGGING); + if (command->m_type == CMD_STATE_LOGGING) + { + command->m_updateFlags |= STATE_LOGGING_MAX_LOG_DOF; + command->m_stateLoggingArguments.m_maxLogDof = maxLogDof; + } + return 0; +} + int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e71203700..01eb55cc1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -356,9 +356,13 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient); int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); +int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof); + int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); + + void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds); double b3GetTimeOut(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1c46d1d1d..68ca59fbf 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1856,6 +1856,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; int loggerUid = m_data->m_stateLoggersUniqueId++; + int maxLogDof = 12; + if ((clientCmd.m_updateFlags & STATE_LOGGING_MAX_LOG_DOF)) + { + maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof; + } GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0)) @@ -1863,7 +1868,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm logger->m_filterObjectUniqueId = true; for (int i = 0; i < clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds; ++i) { - logger->m_bodyIdList.push_back(clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]); + int objectUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[i]; + logger->m_bodyIdList.push_back(objectUniqueId); } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 5b0a02237..dc3390c4e 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -322,6 +322,7 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) numCmdSinceSleep1ms = 0; sleepClock.reset(); } + } if (sleepClock.getTimeMilliseconds()>1) { sleepClock.reset(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 725849d20..1c119c4d1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -626,6 +626,7 @@ enum eStateLoggingEnums STATE_LOGGING_START_LOG=1, STATE_LOGGING_STOP_LOG=2, STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4, + STATE_LOGGING_MAX_LOG_DOF=8 }; struct VRCameraState @@ -644,6 +645,7 @@ struct StateLoggingRequest int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set int m_bodyUniqueIds[MAX_SDF_BODIES]; int m_loggingUniqueId; + int m_maxLogDof; }; struct StateLoggingResultArgs diff --git a/examples/pybullet/kuka_with_cube_playback.py b/examples/pybullet/kuka_with_cube_playback.py index c3a5ad445..9975b27c5 100644 --- a/examples/pybullet/kuka_with_cube_playback.py +++ b/examples/pybullet/kuka_with_cube_playback.py @@ -16,8 +16,8 @@ def readLogFile(filename, verbose = True): print('Opened'), print(filename) - keys = f.readline().rstrip('\n').split(',') - fmt = f.readline().rstrip('\n') + keys = f.readline().decode('utf8').rstrip('\n').split(',') + fmt = f.readline().decode('utf8').rstrip('\n') # The byte number of one record sz = struct.calcsize(fmt) @@ -37,7 +37,7 @@ def readLogFile(filename, verbose = True): # Read data wholeFile = f.read() # split by alignment word - chunks = wholeFile.split('\xaa\xbb') + chunks = wholeFile.split(b'\xaa\xbb') log = list() for chunk in chunks: if len(chunk) == sz: diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f1a51a712..8f72dfe77 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2625,12 +2625,13 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb int loggingType = -1; char* fileName = 0; PyObject* objectUniqueIdsObj = 0; + int maxLogDof=-1; - static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "physicsClientId", NULL }; + static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "maxLogDof", "physicsClientId", NULL }; int physicsClientId = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oi", kwlist, - &loggingType, &fileName, &objectUniqueIdsObj,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oii", kwlist, + &loggingType, &fileName, &objectUniqueIdsObj,&maxLogDof, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -2661,6 +2662,10 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb } } + if (maxLogDof>0) + { + b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle);