Merge pull request #964 from erwincoumans/master

initial implementation of state logging

examples/ExampleBrowser/CMakeLists.txt

@@ -317,6 +317,8 @@ SET(BulletExampleBrowser_SRCS
   ../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
   ../Importers/ImportURDFDemo/MyMultiBodyCreator.h
   ../Importers/ImportURDFDemo/UrdfParser.cpp
+  ../Utils/RobotLoggingUtil.cpp
+  ../Utils/RobotLoggingUtil.h
   ../Importers/ImportURDFDemo/urdfStringSplit.cpp
  ../Importers/ImportURDFDemo/urdfStringSplit.h
   ../Importers/ImportURDFDemo/BulletUrdfImporter.cpp

examples/ExampleBrowser/premake4.lua

@@ -97,6 +97,8 @@ project "App_BulletExampleBrowser"
 		"../BasicDemo/BasicExample.*",
 		"../Tutorial/*",
 		"../ExtendedTutorials/*",
+		"../Utils/RobotLoggingUtil.cpp",
+		"../Utils/RobotLoggingUtil.h",
 		"../Evolution/NN3DWalkers.cpp",
 		"../Evolution/NN3DWalkers.h",
 		"../Collision/*",
@@ -193,6 +195,7 @@ project "BulletExampleBrowserLib"
 		"OpenGLGuiHelper.cpp",
 		"OpenGLExampleBrowser.cpp",
 		"../Utils/b3Clock.cpp",
+		"../Utils/b3Clock.h",
 		"../Utils/ChromeTraceUtil.cpp",
 		"../Utils/ChromeTraceUtil.h",
 		"*.h",

examples/SharedMemory/CMakeLists.txt

@@ -61,6 +61,8 @@ SET(SharedMemory_SRCS
 	../Importers/ImportMJCFDemo/BulletMJCFImporter.h
 	../Utils/b3ResourcePath.cpp
 	../Utils/b3Clock.cpp	
+	../Utils/RobotLoggingUtil.cpp
+	../Utils/RobotLoggingUtil.h
 	../Utils/ChromeTraceUtil.cpp
 	../Utils/ChromeTraceUtil.h
 	../Importers/ImportURDFDemo/URDFImporterInterface.h

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2467,3 +2467,81 @@ int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int o
 	return 0;
 }
 
+b3SharedMemoryCommandHandle	b3StateLoggingCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+
+	command->m_type = CMD_STATE_LOGGING;
+	command->m_updateFlags = 0;
+	command->m_stateLoggingArguments.m_numBodyUniqueIds = 0;
+
+	return (b3SharedMemoryCommandHandle)command;
+
+}
+
+int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName)
+{
+	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_START_LOG;
+		int len = strlen(fileName);
+		if (len < MAX_FILENAME_LENGTH)
+		{
+			strcpy(command->m_stateLoggingArguments.m_fileName, fileName);
+		}
+		else
+		{
+			command->m_stateLoggingArguments.m_fileName[0] = 0;
+		}
+		command->m_stateLoggingArguments.m_logType = loggingType;
+	}	
+	return 0;
+}
+
+int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle)
+{
+	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+	b3Assert(status);
+	b3Assert(status->m_type == CMD_STATE_LOGGING_START_COMPLETED);
+	if (status && status->m_type == CMD_STATE_LOGGING_START_COMPLETED)
+	{
+		return status->m_stateLoggingResultArgs.m_loggingUniqueId;
+	}
+	return -1;
+}
+
+int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId)
+{
+	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_FILTER_OBJECT_UNIQUE_ID;
+		if (command->m_stateLoggingArguments.m_numBodyUniqueIds < MAX_SDF_BODIES)
+		{
+			command->m_stateLoggingArguments.m_bodyUniqueIds[command->m_stateLoggingArguments.m_numBodyUniqueIds++] = objectUniqueId;
+		}
+	}
+	return 0;
+}
+int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
+{
+	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_STOP_LOG;
+		command->m_stateLoggingArguments.m_loggingUniqueId = loggingUid;
+	}
+	return 0;
+}
+

examples/SharedMemory/PhysicsClientC_API.h

@@ -343,8 +343,11 @@ int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double
 int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]);
 int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
 
-
+b3SharedMemoryCommandHandle	b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
-
+int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
+int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
+int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
+int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
 
 #ifdef __cplusplus
 }

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -899,6 +899,20 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 			{
 				break;
 			}
+			case CMD_STATE_LOGGING_START_COMPLETED:
+			{
+				break;
+			};
+			case CMD_STATE_LOGGING_COMPLETED:
+			{
+				break;
+			};
+
+			case CMD_STATE_LOGGING_FAILED:
+			{
+				b3Warning("State Logging failed");
+				break;
+			}
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -17,7 +17,7 @@
 #include "BulletInverseDynamics/MultiBodyTree.hpp"
 #include "IKTrajectoryHelper.h"
 #include "btBulletDynamicsCommon.h"
-
+#include "../Utils/RobotLoggingUtil.h"
 #include "LinearMath/btTransform.h"
 #include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h"
 #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
@@ -420,6 +420,142 @@ struct MyOverlapFilterCallback : public btOverlapFilterCallback
 };
 
 
+struct InternalStateLogger
+{
+	int m_loggingUniqueId;
+	int m_loggingType;
+
+	InternalStateLogger()
+		:m_loggingUniqueId(0),
+		m_loggingType(0)
+	{
+	}
+
+	virtual void stop() = 0;
+	virtual void logState(btScalar timeStamp)=0;
+
+};
+
+struct MinitaurStateLogger : public InternalStateLogger
+{
+	int m_loggingTimeStamp;
+	std::string m_fileName;
+	int m_minitaurBodyUniqueId;
+	FILE* m_logFileHandle;
+
+	std::string m_structTypes;
+	btMultiBody* m_minitaurMultiBody;
+	btAlignedObjectArray<int> m_motorIdList;
+
+	MinitaurStateLogger(int loggingUniqueId, std::string fileName, btMultiBody* minitaurMultiBody, btAlignedObjectArray<int>& motorIdList)
+		:m_loggingTimeStamp(0),
+		m_logFileHandle(0),
+		m_minitaurMultiBody(minitaurMultiBody)
+	{
+		m_loggingType = STATE_LOGGING_MINITAUR;
+		m_motorIdList.resize(motorIdList.size());
+		for (int m=0;m<motorIdList.size();m++)
+		{
+			m_motorIdList[m] = motorIdList[m];
+		}
+
+		btAlignedObjectArray<std::string> structNames;
+		//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
+		structNames.push_back("t");
+		structNames.push_back("r");
+		structNames.push_back("p");
+		structNames.push_back("y");
+	
+		structNames.push_back("q0");
+		structNames.push_back("q1");
+		structNames.push_back("q2");
+		structNames.push_back("q3");
+		structNames.push_back("q4");
+		structNames.push_back("q5");
+		structNames.push_back("q6");
+		structNames.push_back("q7");
+
+		structNames.push_back("u0");
+		structNames.push_back("u1");
+		structNames.push_back("u2");
+		structNames.push_back("u3");
+		structNames.push_back("u4");
+		structNames.push_back("u5");
+		structNames.push_back("u6");
+		structNames.push_back("u7");
+
+		structNames.push_back("dx");
+		structNames.push_back("mo");
+
+		m_structTypes = "IffffffffffffffffffffB";
+		const char* fileNameC = fileName.c_str();
+
+		m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
+	}
+	virtual void stop()
+	{
+		if (m_logFileHandle)
+		{
+			closeMinitaurLogFile(m_logFileHandle);
+			m_logFileHandle = 0;
+		}
+	}
+
+	virtual void logState(btScalar timeStep)
+	{
+		if (m_logFileHandle)
+		{
+			
+			btVector3 pos = m_minitaurMultiBody->getBasePos();
+
+			MinitaurLogRecord logData;
+			//'t', 'r', 'p', 'y', 'q0', 'q1', 'q2', 'q3', 'q4', 'q5', 'q6', 'q7', 'u0', 'u1', 'u2', 'u3', 'u4', 'u5', 'u6', 'u7', 'xd', 'mo'
+			btScalar motorDir[8] = {1, -1, 1, -1, -1, 1, -1, 1};
+
+			btQuaternion orn = m_minitaurMultiBody->getBaseWorldTransform().getRotation();
+			btMatrix3x3 mat(orn);
+			btScalar roll=0;
+			btScalar pitch=0;
+			btScalar yaw = 0;
+
+			mat.getEulerZYX(yaw,pitch,roll);
+			
+			logData.m_values.push_back(m_loggingTimeStamp);
+			logData.m_values.push_back((float)roll);
+			logData.m_values.push_back((float)pitch);
+			logData.m_values.push_back((float)yaw);
+
+			for (int i=0;i<8;i++)
+			{
+				float jointAngle = (float)motorDir[i]*m_minitaurMultiBody->getJointPos(m_motorIdList[i]);
+				logData.m_values.push_back(jointAngle);
+			}
+			for (int i=0;i<8;i++)
+			{
+                btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_minitaurMultiBody->getLink(m_motorIdList[i]).m_userPtr;
+
+                if (motor && timeStep>btScalar(0))
+                {
+					btScalar force = motor->getAppliedImpulse(0)/timeStep;
+					logData.m_values.push_back((float)force);
+				}
+			}
+			//x is forward component, estimated speed forward
+			float xd_speed = m_minitaurMultiBody->getBaseVel()[0];
+			logData.m_values.push_back(xd_speed);
+			char mode = 6;
+			logData.m_values.push_back(mode);
+
+			//at the moment, appendMinitaurLogData will directly write to disk (potential delay)
+			//better to fill a huge memory buffer and once in a while write it to disk
+			appendMinitaurLogData(m_logFileHandle, m_structTypes, logData);
+
+			fflush(m_logFileHandle);
+		
+			m_loggingTimeStamp++;
+		}
+	}
+};
 
 
 struct PhysicsServerCommandProcessorInternalData
@@ -577,7 +713,8 @@ struct PhysicsServerCommandProcessorInternalData
 
 	btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
 
-
+	btAlignedObjectArray<InternalStateLogger*>	m_stateLoggers;
+	int m_stateLoggersUniqueId;
 
 	struct GUIHelperInterface* m_guiHelper;
 	int m_sharedMemoryKey;
@@ -622,6 +759,7 @@ struct PhysicsServerCommandProcessorInternalData
 		m_collisionConfiguration(0),
 		m_dynamicsWorld(0),
 		m_remoteDebugDrawer(0),
+		m_stateLoggersUniqueId(0),
 		m_guiHelper(0),
 		m_sharedMemoryKey(SHARED_MEMORY_KEY),
 		m_verboseOutput(false),
@@ -758,6 +896,22 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
 	delete m_data;
 }
 
+void logCallback(btDynamicsWorld *world, btScalar timeStep)
+{
+	PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
+	proc->logObjectStates(timeStep);
+
+}
+
+
+void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep)
+{
+	for (int i=0;i<m_data->m_stateLoggers.size();i++)
+	{
+		m_data->m_stateLoggers[i]->logState(timeStep);
+	}
+
+}
 
 void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 {
@@ -804,6 +958,19 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 //	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
 	//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
 
+
+	m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this);
+}
+
+void PhysicsServerCommandProcessor::deleteStateLoggers()
+{
+	for (int i=0;i<m_data->m_stateLoggers.size();i++)
+	{
+		m_data->m_stateLoggers[i]->stop();
+		delete m_data->m_stateLoggers[i];
+	}
+	m_data->m_stateLoggers.clear();
+
 }
 
 void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
@@ -838,6 +1005,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 {
 	deleteCachedInverseDynamicsBodies();
 	deleteCachedInverseKinematicsBodies();
+	deleteStateLoggers();
 
 	m_data->m_userConstraints.clear();
 	m_data->m_saveWorldBodyData.clear();
@@ -1451,7 +1619,85 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					break;
 				}
 #endif
+				case CMD_STATE_LOGGING:
+				{
+					serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED;
+                    hasStatus = true;
 
+					if (clientCmd.m_updateFlags & STATE_LOGGING_START_LOG)
+					{
+
+						
+						if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_MINITAUR)
+						{
+							
+							std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName;
+							//either provide the minitaur by object unique Id, or search for first multibody with 8 motors...
+
+							
+							if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
+							{
+								int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0];
+								InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+								if (body)
+								{
+									if (body->m_multiBody)
+									{
+										btAlignedObjectArray<std::string> motorNames;
+										motorNames.push_back("motor_front_leftR_joint");
+										motorNames.push_back("motor_front_leftL_joint");
+										motorNames.push_back("motor_back_leftR_joint");
+										motorNames.push_back("motor_back_leftL_joint");
+										motorNames.push_back("motor_front_rightL_joint");
+										motorNames.push_back("motor_front_rightR_joint");
+										motorNames.push_back("motor_back_rightL_joint");
+										motorNames.push_back("motor_back_rightR_joint");
+										
+										btAlignedObjectArray<int> motorIdList;
+										for (int m=0;m<motorNames.size();m++)
+										{
+											for (int i=0;i<body->m_multiBody->getNumLinks();i++)
+											{
+												std::string jointName;
+												if (body->m_multiBody->getLink(i).m_jointName)
+												{
+													jointName = body->m_multiBody->getLink(i).m_jointName;
+												}
+												if (motorNames[m]==jointName)
+												{
+													motorIdList.push_back(i);
+												}
+											}
+										}
+
+										if (motorIdList.size()==8)
+										{
+											int loggerUid = m_data->m_stateLoggersUniqueId++;
+											MinitaurStateLogger* logger = new MinitaurStateLogger(loggerUid,fileName,body->m_multiBody, motorIdList);
+											m_data->m_stateLoggers.push_back(logger);
+											serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
+											serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
+										}
+									}
+								}
+							}
+						}
+					}
+					if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0)
+					{
+						serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
+						for (int i=0;i<m_data->m_stateLoggers.size();i++)
+						{
+							if (m_data->m_stateLoggers[i]->m_loggingUniqueId==clientCmd.m_stateLoggingArguments.m_loggingUniqueId)
+							{
+								m_data->m_stateLoggers[i]->stop();
+								delete m_data->m_stateLoggers[i];
+								m_data->m_stateLoggers.removeAtIndex(i);
+							}
+						}
+					}
+					break;
+				}
 				case CMD_SET_VR_CAMERA_STATE:
 				{
 
@@ -4693,7 +4939,6 @@ void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
 void PhysicsServerCommandProcessor::resetSimulation()
 {
 	//clean up all data
-	deleteCachedInverseDynamicsBodies();
 
 	if (m_data && m_data->m_guiHelper)
 	{

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -20,6 +20,8 @@ class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
 
 	struct PhysicsServerCommandProcessorInternalData* m_data;
 
+	
+
 	//todo: move this to physics client side / Python
 	void createDefaultRobotAssets();
 
@@ -44,6 +46,7 @@ protected:
 	int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
 	void deleteCachedInverseDynamicsBodies();
 	void deleteCachedInverseKinematicsBodies();
+	void deleteStateLoggers();
 
 public:
 	PhysicsServerCommandProcessor();
@@ -84,10 +87,15 @@ public:
 	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
 	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
 	virtual void removePickingConstraint();
-	
+
+	//logging /playback the shared memory commands
 	void enableCommandLogging(bool enable, const char* fileName);
 	void replayFromLogFile(const char* fileName);
 	void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
+
+	//logging of object states (position etc)
+	void logObjectStates(btScalar timeStep);
+
 	void stepSimulationRealTime(double dtInSec,	const struct b3VRControllerEvent* vrEvents, int numVREvents);
 	void enableRealTimeSimulation(bool enableRealTimeSim);
 	void applyJointDamping(int bodyUniqueId);

examples/SharedMemory/SharedMemoryCommands.h

@@ -613,6 +613,13 @@ enum eVRCameraEnums
 	VR_CAMERA_ROOT_TRACKING_OBJECT=4
 };
 
+enum eStateLoggingEnums
+{
+	STATE_LOGGING_START_LOG=1,
+	STATE_LOGGING_STOP_LOG=2,
+	STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID=4,
+};
+
 struct VRCameraState
 {
 	double m_rootPosition[3];
@@ -621,6 +628,21 @@ struct VRCameraState
 };
 
 
+
+struct StateLoggingRequest
+{
+	char m_fileName[MAX_FILENAME_LENGTH];
+	int m_logType;//Minitaur, generic robot, VR states
+	int m_numBodyUniqueIds;////only if ROBOT_LOGGING_FILTER_OBJECT_UNIQUE_ID flag is set
+	int m_bodyUniqueIds[MAX_SDF_BODIES];
+	int m_loggingUniqueId;
+};
+
+struct StateLoggingResultArgs
+{
+	int m_loggingUniqueId;
+};
+
 struct SharedMemoryCommand
 {
 	int m_type;
@@ -662,6 +684,7 @@ struct SharedMemoryCommand
 		struct RequestRaycastIntersections m_requestRaycastIntersections;
         struct LoadBunnyArgs m_loadBunnyArguments;
 		struct VRCameraState m_vrCameraStateArguments;
+		struct StateLoggingRequest m_stateLoggingArguments;
     };
 };
 
@@ -722,6 +745,8 @@ struct SharedMemoryStatus
 		struct b3UserConstraint m_userConstraintResultArgs;
 		struct SendVREvents m_sendVREvents;
 		struct SendRaycastHits m_raycastHits;
+		struct StateLoggingResultArgs m_stateLoggingResultArgs;
+
 	};
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -48,6 +48,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_REQUEST_VR_EVENTS_DATA,
 	CMD_SET_VR_CAMERA_STATE,
 	CMD_SYNC_BODY_INFO,
+	CMD_STATE_LOGGING,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -122,6 +123,9 @@ enum EnumSharedMemoryServerStatus
 		CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
 		CMD_SYNC_BODY_INFO_COMPLETED,
 		CMD_SYNC_BODY_INFO_FAILED,
+		CMD_STATE_LOGGING_COMPLETED,
+		CMD_STATE_LOGGING_START_COMPLETED,
+		CMD_STATE_LOGGING_FAILED,
         //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
@@ -301,6 +305,13 @@ enum
 	CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1,
 };
 
+enum 
+{
+	STATE_LOGGING_MINITAUR = 0,
+	STATE_LOGGING_GENERIC_ROBOT = 1,
+	STATE_LOGGING_VR_CONTROLLERS = 2,
+	STATE_LOGGING_COMMANDS = 3,
+};
 
 
 struct b3ContactInformation

examples/SharedMemory/premake4.lua

@@ -76,7 +76,9 @@ myfiles =
 	"../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp",
 	"../Importers/ImportMJCFDemo/BulletMJCFImporter.h",
 	"../Utils/b3ResourcePath.cpp",
-	"../Utils/b3Clock.cpp",	
+	"../Utils/b3Clock.cpp",
+	"../Utils/RobotLoggingUtil.cpp",
+	"../Utils/RobotLoggingUtil.h",
 	"../Utils/ChromeTraceUtil.cpp",
 	"../Utils/ChromeTraceUtil.h",
 	"../../Extras/Serialize/BulletWorldImporter/*",

examples/SharedMemory/udp/premake4.lua

@@ -103,7 +103,9 @@ myfiles =
 	"../../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 	"../../Importers/ImportURDFDemo/URDF2Bullet.h",
 	"../../Utils/b3ResourcePath.cpp",
-	"../../Utils/b3Clock.cpp",	
+	"../../Utils/b3Clock.cpp",
+	"../../Utils/RobotLoggingUtil.cpp",
+	"../../Utils/RobotLoggingUtil.h",
 	"../../../Extras/Serialize/BulletWorldImporter/*",
 	"../../../Extras/Serialize/BulletFileLoader/*",	
 	"../../Importers/ImportURDFDemo/URDFImporterInterface.h",

examples/pybullet/CMakeLists.txt

@@ -54,6 +54,9 @@ SET(pybullet_SRCS
 		../../examples/SharedMemory/PosixSharedMemory.h
 		../../examples/Utils/b3ResourcePath.cpp
 		../../examples/Utils/b3ResourcePath.h
+		../../examples/Utils/RobotLoggingUtil.cpp
+		../../examples/Utils/RobotLoggingUtil.h
+		
 		../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
 		../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
 		../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp

examples/pybullet/logMinitaur.py

@@ -0,0 +1,15 @@
+import pybullet as p
+cid = p.connect(p.SHARED_MEMORY)
+if (cid < 0) :
+	p.connect(p.GUI)
+p.loadURDF("plane.urdf")
+
+quadruped = p.loadURDF("quadruped/quadruped.urdf")
+logId = p.startStateLogging(p.STATE_LOGGING_MINITAUR,"LOG00048.TXT",[quadruped])
+p.stepSimulation()
+p.stepSimulation()
+p.stepSimulation()
+p.stepSimulation()
+p.stepSimulation()
+
+p.stopStateLogging(logId)

examples/pybullet/premake4.lua

@@ -101,6 +101,8 @@ if not _OPTIONS["no-enet"] then
 			"../../examples/SharedMemory/PosixSharedMemory.h",
 			"../../examples/Utils/b3ResourcePath.cpp",
 			"../../examples/Utils/b3ResourcePath.h",
+			"../../examples/Utils/RobotLoggingUtil.cpp",
+			"../../examples/Utils/RobotLoggingUtil.h",
 			"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
 			"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
 			"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",

examples/pybullet/pybullet.c

@@ -2590,6 +2590,102 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args
 	return Py_None;
 }
 
+static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	b3PhysicsClientHandle sm = 0;
+	int loggingType = -1;
+	char* fileName = 0;
+	PyObject* objectUniqueIdsObj = 0;
+
+	static char *kwlist[] = { "loggingType", "fileName", "objectUniqueIds", "physicsClientId", NULL };
+	int physicsClientId = 0;
+		
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oi", kwlist,
+		&loggingType, &fileName, &objectUniqueIdsObj,&physicsClientId))
+		return NULL;
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	{
+		b3SharedMemoryCommandHandle commandHandle;
+		commandHandle = b3StateLoggingCommandInit(sm);
+		
+		b3StateLoggingStart(commandHandle,loggingType,fileName);
+
+		if (objectUniqueIdsObj)
+		{
+			PyObject* seq = PySequence_Fast(objectUniqueIdsObj, "expected a sequence of object unique ids");
+			if (seq)
+			{
+				int len = PySequence_Size(objectUniqueIdsObj);
+				int i;
+				for ( i=0;i<len;i++)
+				{
+					int objectUid = pybullet_internalGetFloatFromSequence(seq, i);
+					b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid);
+				}
+				Py_DECREF(seq);
+			}
+		}
+
+		
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType==CMD_STATE_LOGGING_START_COMPLETED)
+		{
+			int loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
+			PyObject* loggingUidObj = PyInt_FromLong(loggingUniqueId);
+			return loggingUidObj;
+		}
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	int loggingId=-1;
+
+	b3PhysicsClientHandle sm = 0;
+	static char *kwlist[] = { "loggingId", "physicsClientId", NULL };
+		int physicsClientId = 0;
+		
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,
+		&loggingId,&physicsClientId))
+		return NULL;
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	if (loggingId>=0)
+	{
+		b3SharedMemoryCommandHandle commandHandle;
+		commandHandle = b3StateLoggingCommandInit(sm);
+		b3StateLoggingStop(commandHandle, loggingId);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+		statusType = b3GetStatusType(statusHandle);
+
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
 
 static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) 
 {
@@ -4967,8 +5063,14 @@ static PyMethodDef SpamMethods[] = {
 		"Set properties of the VR Camera such as its root transform "
 		"for teleporting or to track objects (camera inside a vehicle for example)."
 	  },
-
+	  {	"startStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS,
-
+		"Start logging of state, such as robot base position, orientation, joint positions etc. "
+		"Specify loggingType (STATE_LOG_MINITAUR, STATE_LOG_GENERIC_ROBOT, STATE_LOG_VR_CONTROLLERS etc), "
+		"fileName, optional objectUniqueId. Function returns int loggingUniqueId"
+	  },
+	  {	"stopStateLogging", (PyCFunction)pybullet_stopStateLogging, METH_VARARGS | METH_KEYWORDS,
+		"Stop logging of robot state, given a loggingUniqueId."
+	  },
 	{	"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
 				"Cast a ray and return the first object hit, if any. "
 				"Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"
@@ -5054,6 +5156,9 @@ initpybullet(void)
   PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS);
   PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS);
 
+  PyModule_AddIntConstant(m, "STATE_LOGGING_MINITAUR", STATE_LOGGING_MINITAUR);
+  PyModule_AddIntConstant(m, "STATE_LOGGING_GENERIC_ROBOT", STATE_LOGGING_GENERIC_ROBOT);
+  PyModule_AddIntConstant(m, "STATE_LOGGING_VR_CONTROLLERS", STATE_LOGGING_VR_CONTROLLERS);
 
   SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
   Py_INCREF(SpamError);

test/CMakeLists.txt

@@ -3,5 +3,5 @@ IF(BUILD_BULLET3)
 	SUBDIRS(  InverseDynamics SharedMemory )
 ENDIF(BUILD_BULLET3)
 
-SUBDIRS(  gtest-1.7.0 collision BulletDynamics/pendulum )
+SUBDIRS(  gtest-1.7.0 collision RobotLogging BulletDynamics/pendulum )
 

test/RobotLogging/main.cpp

@@ -0,0 +1,81 @@
+#include "Utils/RobotLoggingUtil.h"
+
+#ifndef ENABLE_GTEST
+
+#include <assert.h>
+#define ASSERT_EQ(a,b) assert((a)==(b));
+#else
+#include <gtest/gtest.h>
+#define printf
+#endif
+
+
+void testMinitaurLogging()
+{
+	const char* fileName = "d:/logTest.txt";
+	btAlignedObjectArray<std::string> structNames;
+	std::string structTypes;
+	btAlignedObjectArray<MinitaurLogRecord> logRecords;
+	bool verbose = false;
+
+	int status = readMinitaurLogFile(fileName, structNames, structTypes, logRecords, verbose);
+
+	for (int i=0;i<logRecords.size();i++)
+	{
+		for (int j=0;j<structTypes.size();j++)
+		{
+			switch (structTypes[j])
+			{
+				case 'I':
+				{
+					int v = logRecords[i].m_values[j].m_intVal;
+					printf("record %d, %s = %d\n",i,structNames[j].c_str(),v);
+					break;
+				}
+				case 'f':
+				{
+					float v = logRecords[i].m_values[j].m_floatVal;
+					printf("record %d, %s = %f\n",i,structNames[j].c_str(),v);
+					break;
+
+				}
+				case 'B':
+				{
+					int v = logRecords[i].m_values[j].m_charVal;
+					printf("record %d, %s = %d\n",i,structNames[j].c_str(),v);
+					break;
+				}
+				default:
+				{
+				}
+			}
+		}
+	}
+}
+
+#ifdef ENABLE_GTEST
+TEST(RobotLoggingTest, LogMinitaur) {
+        testMinitaurLogging();
+}
+#endif
+
+int main(int argc, char* argv[])
+{
+
+//b3SetCustomPrintfFunc(myprintf);
+//b3SetCustomWarningMessageFunc(myprintf);
+
+#ifdef ENABLE_GTEST
+
+#if _MSC_VER
+        _CrtSetDbgFlag ( _CRTDBG_ALLOC_MEM_DF | _CRTDBG_LEAK_CHECK_DF );
+        //void *testWhetherMemoryLeakDetectionWorks = malloc(1);
+#endif
+        ::testing::InitGoogleTest(&argc, argv);
+        return RUN_ALL_TESTS();
+#else
+	testMinitaurLogging();
+#endif
+
+
+}

test/SharedMemory/CMakeLists.txt

@@ -56,6 +56,8 @@ ENDIF()
                         ../../examples/SharedMemory/PosixSharedMemory.h
                         ../../examples/Utils/b3ResourcePath.cpp
                         ../../examples/Utils/b3ResourcePath.h
+                        ../../examples/Utils/RobotLoggingUtil.cpp
+                        ../../examples/Utils/RobotLoggingUtil.h
 									      ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
 												../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
 												../../examples/OpenGLWindow/SimpleCamera.cpp

test/SharedMemory/premake4.lua

@@ -84,6 +84,8 @@ project ("Test_PhysicsServerLoopBack")
 			"../../examples/TinyRenderer/TinyRenderer.cpp",
 			"../../examples/Utils/b3ResourcePath.cpp",
 			"../../examples/Utils/b3ResourcePath.h",
+			"../../examples/Utils/RobotLoggingUtil.cpp",
+			"../../examples/Utils/RobotLoggingUtil.h",
 			"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
 			"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
 			"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
@@ -158,6 +160,8 @@ project ("Test_PhysicsServerLoopBack")
 			"../../examples/OpenGLWindow/SimpleCamera.h",
 			"../../examples/Utils/b3ResourcePath.cpp",
 			"../../examples/Utils/b3ResourcePath.h",
+			"../../examples/Utils/RobotLoggingUtil.cpp",
+			"../../examples/Utils/RobotLoggingUtil.h",			
 			"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
 			"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
 			"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
@@ -259,6 +263,8 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
 			"../../examples/TinyRenderer/TinyRenderer.cpp",
 			"../../examples/Utils/b3ResourcePath.cpp",
 			"../../examples/Utils/b3ResourcePath.h",
+			"../../examples/Utils/RobotLoggingUtil.cpp",
+			"../../examples/Utils/RobotLoggingUtil.h",
 			"../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp",
 			"../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp",
 			"../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",