fix quadruped, allow user to pick the maximum number of dofs to log

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;

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

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

examples/SharedMemory/PhysicsServerExample.cpp

@@ -322,6 +322,7 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 					numCmdSinceSleep1ms = 0;
 					sleepClock.reset();
 				}
+			}
 			if (sleepClock.getTimeMilliseconds()>1)
 			{
 				sleepClock.reset();

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

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(',')
+  keys = f.readline().decode('utf8').rstrip('\n').split(',')
-  fmt = f.readline().rstrip('\n')
+  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:

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,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oii", kwlist,
-		&loggingType, &fileName, &objectUniqueIdsObj,&physicsClientId))
+		&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);