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

14 files changed, 240 insertions, 7 deletions

diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 68b5d6d1f..b7cf1fd06 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/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
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 5e6e41351..0c44e93bb 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/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",
diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt
index 5db17f0e5..a2c852581 100644
--- a/examples/SharedMemory/CMakeLists.txt
+++ b/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
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index f02e332a4..30e543a7f 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/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;
+}
+
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index febe08396..63a5a9847 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/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
 }
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index cd4260469..fb35cb66e 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/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()
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h
index 66dc230f9..481e46f85 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.h
+++ b/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);
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 847a0de75..a4f665abd 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/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];
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 913fe086b..11a8dc93a 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/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
diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua
index 3bb14c6b0..399cf1473 100644
--- a/examples/SharedMemory/premake4.lua
+++ b/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/*",
diff --git a/examples/SharedMemory/udp/premake4.lua b/examples/SharedMemory/udp/premake4.lua
index 7f3ef58a8..174625ed3 100644
--- a/examples/SharedMemory/udp/premake4.lua
+++ b/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",
diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt
index 0a10c0677..e2d302370 100644
--- a/examples/pybullet/CMakeLists.txt
+++ b/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
diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua
index 0b856bd4b..a47016c8b 100644
--- a/examples/pybullet/premake4.lua
+++ b/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",
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 65cc7e108..2e3fe51e7 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -2583,6 +2583,58 @@ 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;
+	}
+}
+
+static PyObject* pybullet_stopStateLogging(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	PyObject* rayFromObj=0;
+	PyObject* rayToObj=0;
+	double from[3];
+	double to[3];
+	b3PhysicsClientHandle sm = 0;
+	static char *kwlist[] = { "loggingId", "physicsClientId", NULL };
+		int physicsClientId = 0;
+		
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist,
+		&rayFromObj, &rayToObj,&physicsClientId))
+		return NULL;
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+}
+
+
 
 static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) 
 {
@@ -4960,8 +5012,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)."
 	  },
-
-
+	  {	"startRobotStateLogging", (PyCFunction)pybullet_startStateLogging, METH_VARARGS | METH_KEYWORDS,
+		"Start logging of state, such as robot base position, orientation, joint positions etc. "
+		"Specify loggingType (ROBOT_LOG_MINITAUR, ROBOT_LOG_GENERIC_ROBOT, ROBOT_LOG_VR_CONTROLLERS etc), "
+		"fileName, optional objectUniqueId. Function returns int loggingUniqueId"
+	  },
+	  {	"stopRobotStateLogging", (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"
@@ -5047,6 +5105,9 @@ initpybullet(void)
   PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS);
   PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS);
 
+  PyModule_AddIntConstant(m, "ROBOT_LOGGING_MINITAUR", ROBOT_LOGGING_MINITAUR);
+  PyModule_AddIntConstant(m, "ROBOT_LOGGING_GENERIC_ROBOT", ROBOT_LOGGING_GENERIC_ROBOT);
+  PyModule_AddIntConstant(m, "ROBOT_LOGGING_VR_CONTROLLERS", ROBOT_LOGGING_VR_CONTROLLERS);
 
   SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
   Py_INCREF(SpamError);