prepare state logging system (log state of robot, vr controllers after each stepSimulation)

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

@@ -2457,3 +2457,42 @@ int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int o
 	return 0;
 }
 
+b3SharedMemoryCommandHandle	b3RobotLoggingCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+
+	command->m_type = CMD_ROBOT_LOGGING;
+	command->m_updateFlags = 0;
+
+	return (b3SharedMemoryCommandHandle)command;
+
+}
+
+int b3RobotLoggingStartMinitaurLog(b3SharedMemoryCommandHandle commandHandle, const char* fileName, int objectUniqueId)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_ROBOT_LOGGING);
+	if (command->m_type == CMD_ROBOT_LOGGING)
+	{
+	    command->m_updateFlags |= ROBOT_LOGGING_START_MINITAUR_LOG;
+	}	
+	return 0;
+}
+
+int b3RobotLoggingStopMinitaurLog(b3SharedMemoryCommandHandle commandHandle)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_ROBOT_LOGGING);
+	if (command->m_type == CMD_ROBOT_LOGGING)
+	{
+	    command->m_updateFlags |= ROBOT_LOGGING_STOP_MINITAUR_LOG;
+	}
+	return 0;
+}
+

examples/SharedMemory/PhysicsClientC_API.h

@@ -343,8 +343,10 @@ int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double
 int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]);
 int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
 
-
-
+b3SharedMemoryCommandHandle	b3RobotLoggingCommandInit(b3PhysicsClientHandle physClient);
+int b3RobotLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
+int b3RobotLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
+int b3RobotLoggingStop(b3SharedMemoryCommandHandle commandHandle);
 
 #ifdef __cplusplus
 }

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -757,6 +757,97 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
 	delete m_data;
 }
 
+void logCallback(btDynamicsWorld *world, btScalar timeStep)
+{
+	PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
+	proc->logObjectStates(timeStep);
+
+}
+#include "../Utils/RobotLoggingUtil.h"
+
+void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep)
+{
+	//quick hack
+	static FILE* logFile = 0;
+	static btScalar logTime = 0;
+	//printf("log state at time %f\n",logTime);
+
+	btAlignedObjectArray<std::string> structNames;
+	std::string structTypes;
+	//'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");
+
+	
+
+/*	structNames.push_back("timeStamp");
+	structNames.push_back("objectId");
+	structNames.push_back("posX");
+	structNames.push_back("posY");
+	structNames.push_back("posZ");
+	*/
+
+	structTypes = "fIfff";//I = int, f = float, B char
+
+	if (logFile==0)
+	{
+		logFile = createMinitaurLogFile("d:/logTest.txt", structNames, structTypes);
+	}
+
+	if (logFile)
+	{
+		for (int i=0;i<m_data->m_dynamicsWorld->getNumMultibodies();i++)
+		{
+			btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i);
+			btVector3 pos = mb->getBasePos();
+
+			MinitaurLogRecord logData;
+			float timeStamp = logTime;
+			int objectUniqueId = mb->getUserIndex2();
+			float posX = pos[0];
+			float posY = pos[1];
+			float posZ = pos[2];
+
+			logData.m_values.push_back(timeStamp);
+			logData.m_values.push_back(objectUniqueId);
+			logData.m_values.push_back(posX);
+			logData.m_values.push_back(posY);
+			logData.m_values.push_back(posZ);
+			//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(logFile, structTypes, logData);
+
+		}
+		fflush(logFile);
+	}
+	//void closeMinitaurLogFile(FILE* f);
+
+	logTime += timeStep;
+
+}
 
 void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 {
@@ -803,6 +894,8 @@ 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::deleteCachedInverseKinematicsBodies()

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();
 
@@ -84,10 +86,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

@@ -611,6 +611,14 @@ enum eVRCameraEnums
 	VR_CAMERA_ROOT_TRACKING_OBJECT=4
 };
 
+enum eRobotLoggingEnums
+{
+	ROBOT_LOGGING_START_MINITAUR_LOG=1,
+	ROBOT_LOGGING_STOP_MINITAUR_LOG=1,
+	ROBOT_LOGGING_START_GENERIC_LOG=1,
+	ROBOT_LOGGING_STOP_GENERIC_LOG=1,
+};
+
 struct VRCameraState
 {
 	double m_rootPosition[3];

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_ROBOT_LOGGING,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -301,6 +302,12 @@ enum
 	CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS = 1,
 };
 
+enum 
+{
+	ROBOT_LOGGING_MINITAUR = 0,
+	ROBOT_LOGGING_GENERIC_ROBOT = 1,
+	ROBOT_LOGGING_VR_CONTROLLERS = 2,
+};
 
 
 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",