Merge pull request #964 from erwincoumans/master

initial implementation of state logging
bulletphysics · Feb 17, 2017 · 6784cff · 6784cff
2 parents 835dea5 + 8af6223
commit 6784cff
Show file tree

Hide file tree

Showing 20 changed files with 620 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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;
+}
+
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/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
 }

diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/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);