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

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

@@ -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",

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/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

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

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",