Merge pull request #1332 from erwincoumans/master

create a C/C++ plugin system for pybullet / C-API.

Extras/BulletRobotics/CMakeLists.txt

@@ -32,6 +32,9 @@ SET(BulletRobotics_SRCS
 		../../examples/SharedMemory/PhysicsDirectC_API.h
 		../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
 		../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+		../../examples/SharedMemory/b3PluginManager.cpp
+		../../examples/SharedMemory/b3PluginManager.h
+				
 		../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
 		../../examples/SharedMemory/PhysicsClientSharedMemory.h
 		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp

examples/ExampleBrowser/CMakeLists.txt

@@ -171,6 +171,7 @@ SET(BulletExampleBrowser_SRCS
 	../SharedMemory/PhysicsServerCommandProcessor.h
 	../SharedMemory/SharedMemoryCommands.h
 	../SharedMemory/SharedMemoryPublic.h
+	../SharedMemory/b3PluginManager.cpp
 	../RobotSimulator/b3RobotSimulatorClientAPI.cpp
 	../RobotSimulator/b3RobotSimulatorClientAPI.h
 	../BasicDemo/BasicExample.cpp

examples/ExampleBrowser/premake4.lua

@@ -109,6 +109,7 @@ project "App_BulletExampleBrowser"
 		"../SharedMemory/PhysicsLoopBackC_API.h",
 		"../SharedMemory/PhysicsServerCommandProcessor.cpp",
 		"../SharedMemory/PhysicsServerCommandProcessor.h",
+		"../SharedMemory/b3PluginManager.cpp",		
 		"../SharedMemory/TinyRendererVisualShapeConverter.cpp",
 		"../SharedMemory/TinyRendererVisualShapeConverter.h",
 		"../SharedMemory/SharedMemoryCommands.h",

examples/RobotSimulator/CMakeLists.txt

@@ -42,6 +42,8 @@ SET(RobotSimulator_SRCS
 		../../examples/SharedMemory/PhysicsDirectC_API.h
 		../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
 		../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+		../../examples/SharedMemory/b3PluginManager.cpp
+		
 		../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
 		../../examples/SharedMemory/PhysicsClientSharedMemory.h
 		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp

examples/RobotSimulator/premake4.lua

@@ -20,6 +20,8 @@ myfiles =
 			"../../examples/SharedMemory/PhysicsDirectC_API.h",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+			"../../examples/SharedMemory/b3PluginManager.cpp",
+
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",

examples/SharedMemory/CMakeLists.txt

@@ -1,5 +1,4 @@
 
-
 SET(SharedMemory_SRCS
 	IKTrajectoryHelper.cpp
 	IKTrajectoryHelper.h
@@ -41,6 +40,7 @@ SET(SharedMemory_SRCS
 	TinyRendererVisualShapeConverter.h
 	SharedMemoryCommands.h
 	SharedMemoryPublic.h
+	b3PluginManager.cpp
 	../TinyRenderer/geometry.cpp
 	../TinyRenderer/model.cpp
 	../TinyRenderer/tgaimage.cpp

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1631,6 +1631,7 @@ B3_SHARED_API	int	b3SubmitClientCommand(b3PhysicsClientHandle physClient, const
 #include "../Utils/b3Clock.h"
 
 
+
 B3_SHARED_API b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
 {
 	B3_PROFILE("b3SubmitClientCommandAndWaitStatus");
@@ -1733,6 +1734,137 @@ B3_SHARED_API	int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex
 	return cl->getJointInfo(bodyIndex, jointIndex, *info);
 }
 
+
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+	command->m_type = CMD_CUSTOM_COMMAND;
+	command->m_updateFlags = 0;
+	return (b3SharedMemoryCommandHandle)command;
+}
+
+B3_SHARED_API	void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
+	if (command->m_type == CMD_CUSTOM_COMMAND)
+	{
+		command->m_updateFlags |= CMD_CUSTOM_COMMAND_LOAD_PLUGIN;
+		command->m_customCommandArgs.m_pluginPath[0] = 0;
+		
+		int len = strlen(pluginPath);
+		if (len<MAX_FILENAME_LENGTH)
+		{
+			strcpy(command->m_customCommandArgs.m_pluginPath, pluginPath);
+		}
+	}
+}
+
+
+
+B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle)
+{
+	int statusUniqueId = -1;
+
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	b3Assert(status);
+	b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
+	if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
+	{
+		statusUniqueId = status->m_customCommandResultArgs.m_executeCommandResult;
+	}
+	return statusUniqueId;
+}
+
+B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle)
+{
+	int statusUniqueId = -1;
+
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	b3Assert(status);
+	b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
+	if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
+	{
+		statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId;
+	}
+	return statusUniqueId;
+}
+
+B3_SHARED_API	void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId)
+{
+
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
+	if (command->m_type == CMD_CUSTOM_COMMAND)
+	{
+		command->m_updateFlags |= CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN;
+		command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId;
+	}
+}
+B3_SHARED_API	void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments)
+{
+
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
+	if (command->m_type == CMD_CUSTOM_COMMAND)
+	{
+		command->m_updateFlags |= CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND;
+		command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId;
+		
+		command->m_customCommandArgs.m_arguments.m_numInts = 0;
+		command->m_customCommandArgs.m_arguments.m_numFloats = 0;
+		command->m_customCommandArgs.m_arguments.m_text[0] = 0;
+
+		int len = strlen(textArguments);
+
+		if (len<MAX_FILENAME_LENGTH)
+		{
+			strcpy(command->m_customCommandArgs.m_arguments.m_text, textArguments);
+		}
+	}
+}
+
+B3_SHARED_API	void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
+	b3Assert(command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND);
+	if (command->m_type == CMD_CUSTOM_COMMAND && (command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND))
+	{
+		int numInts = command->m_customCommandArgs.m_arguments.m_numInts;
+		if (numInts<B3_MAX_PLUGIN_ARG_SIZE)
+		{
+			command->m_customCommandArgs.m_arguments.m_ints[numInts]=intVal;
+			command->m_customCommandArgs.m_arguments.m_numInts++;
+		}
+	}
+}
+
+B3_SHARED_API	void b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle, float floatVal)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
+	b3Assert(command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND);
+	if (command->m_type == CMD_CUSTOM_COMMAND && (command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND))
+	{
+		int numFloats = command->m_customCommandArgs.m_arguments.m_numFloats;
+		if (numFloats<B3_MAX_PLUGIN_ARG_SIZE)
+		{
+			command->m_customCommandArgs.m_arguments.m_floats[numFloats]=floatVal;
+			command->m_customCommandArgs.m_arguments.m_numFloats++;
+		}
+	}
+}
+
+
+
+
+
 B3_SHARED_API	b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -60,6 +60,18 @@ B3_SHARED_API	b3SharedMemoryStatusHandle	b3ProcessServerStatus(b3PhysicsClientHa
 /// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
 B3_SHARED_API	int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
 
+///Plugin system, load and unload a plugin, execute a command
+B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient);
+B3_SHARED_API	void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath);
+B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle);
+B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle);
+
+B3_SHARED_API	void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId);
+B3_SHARED_API	void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments);
+B3_SHARED_API	void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal);
+B3_SHARED_API	void b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle, float floatVal);
+
+
 B3_SHARED_API	int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
 
 B3_SHARED_API	int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -78,7 +78,7 @@ struct PhysicsClientSharedMemoryInternalData {
           m_hasLastServerStatus(false),
           m_sharedMemoryKey(SHARED_MEMORY_KEY),
           m_verboseOutput(false),
-		  m_timeOutInSeconds(5)
+		  m_timeOutInSeconds(30)
 	{}
 
     void processServerStatus();
@@ -1200,6 +1200,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 				b3Warning("Request getCollisionInfo failed");
 				break;
 			}
+			case CMD_CUSTOM_COMMAND_COMPLETED:
+			{
+				break;
+			}
+			case CMD_CUSTOM_COMMAND_FAILED:
+			{
+				b3Warning("custom plugin command failed");
+				break;
+			}
 
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);

examples/SharedMemory/PhysicsDirect.cpp

@@ -940,6 +940,16 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		break;
 	}
 
+	case CMD_CUSTOM_COMMAND_COMPLETED:
+	{
+		break;
+	}
+	case CMD_CUSTOM_COMMAND_FAILED:
+	{
+		b3Warning("custom plugin command failed");
+		break;
+	}
+
 	default:
 	{
 		//b3Warning("Unknown server status type");

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1,5 +1,6 @@
 #include "PhysicsServerCommandProcessor.h"
 
+
 #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
 #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
 #include "../Importers/ImportURDFDemo/URDF2Bullet.h"
@@ -37,6 +38,8 @@
 #include "LinearMath/btRandom.h"
 #include "Bullet3Common/b3ResizablePool.h"
 #include "../Utils/b3Clock.h"
+#include "b3PluginManager.h"
+
 #ifdef B3_ENABLE_TINY_AUDIO
 #include "../TinyAudio/b3SoundEngine.h"
 #endif
@@ -1053,13 +1056,13 @@ struct GenericRobotStateLogger : public InternalStateLogger
     std::string m_fileName;
     FILE* m_logFileHandle;
     std::string m_structTypes;
-    btMultiBodyDynamicsWorld* m_dynamicsWorld;
+    const btMultiBodyDynamicsWorld* m_dynamicsWorld;
     btAlignedObjectArray<int> m_bodyIdList;
     bool m_filterObjectUniqueId;
     int m_maxLogDof;
 	int m_logFlags;
 
-    GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
+    GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, const btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
     :m_loggingTimeStamp(0),
     m_logFileHandle(0),
     m_dynamicsWorld(dynamicsWorld),
@@ -1137,7 +1140,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
         {
             for (int i=0;i<m_dynamicsWorld->getNumMultibodies();i++)
             {
-                btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
+                const btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
                 int objectUniqueId = mb->getUserIndex2();
                 if (m_filterObjectUniqueId && m_bodyIdList.findLinearSearch2(objectUniqueId) < 0)
                 {
@@ -1430,6 +1433,8 @@ struct PhysicsServerCommandProcessorInternalData
 	b3ResizablePool< InternalBodyHandle > m_bodyHandles;
 	b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
 
+	b3PluginManager m_pluginManager;
+
 	bool m_allowRealTimeSimulation;
 	
 
@@ -1512,8 +1517,8 @@ struct PhysicsServerCommandProcessorInternalData
 
 	b3HashMap<b3HashString,  char*> m_profileEvents;
 
-	PhysicsServerCommandProcessorInternalData()
+	PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
-		:
+		:m_pluginManager(proc),
 		m_allowRealTimeSimulation(false),
 		m_commandLogger(0),
 		m_logPlayback(0),
@@ -1537,6 +1542,16 @@ struct PhysicsServerCommandProcessorInternalData
 		m_pickedConstraint(0),
 		m_pickingMultiBodyPoint2Point(0)
 	{
+
+		
+
+		{
+			//test to statically link a plugin
+			//#include "plugins/testPlugin/testplugin.h"
+			//register static plugins:
+			//m_pluginManager.registerStaticLinkedPlugin("path", initPlugin, exitPlugin, executePluginCommand);
+		}
+
 		m_vrControllerEvents.init();
 
 		m_bodyHandles.exitHandles();
@@ -1643,8 +1658,9 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH
 
 
 PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
+	:m_data(0)
 {
-	m_data = new PhysicsServerCommandProcessorInternalData();
+	m_data = new PhysicsServerCommandProcessorInternalData(this);
 
 	createEmptyDynamicsWorld();
 
@@ -1666,14 +1682,24 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
 	delete m_data;
 }
 
+
+void preTickCallback(btDynamicsWorld *world, btScalar timeStep)
+{
+	PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
+	bool isPreTick = true;
+	proc->tickPlugins(timeStep, isPreTick);
+}
+
 void logCallback(btDynamicsWorld *world, btScalar timeStep)
 {
 	//handle the logging and playing sounds
 	PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
 	proc->processCollisionForces(timeStep);
-	
 	proc->logObjectStates(timeStep);
 	
+	bool isPreTick = false;
+	proc->tickPlugins(timeStep, isPreTick);
+
 }
 
 bool MyContactAddedCallback(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
@@ -1781,6 +1807,12 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep)
 #endif//B3_ENABLE_TINY_AUDIO
 }
 
+void PhysicsServerCommandProcessor::tickPlugins(btScalar timeStep, bool isPreTick)
+{
+	m_data->m_pluginManager.tickPlugins(timeStep, isPreTick);
+}
+
+
 void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep)
 {
 	for (int i=0;i<m_data->m_stateLoggers.size();i++)
@@ -2144,7 +2176,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	{
 		m_data->m_guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld);
 	}
-	m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this);
+	bool isPreTick=false;
+	m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this,isPreTick);
+	isPreTick = true;
+	m_data->m_dynamicsWorld->setInternalTickCallback(preTickCallback,this,isPreTick);
+
 
 #ifdef B3_ENABLE_TINY_AUDIO
 	m_data->m_soundEngine.init(16,true);
@@ -2961,27 +2997,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 	bool hasStatus = false;
 
     {
-        ///we ignore overflow of integer for now
-
         {
-
-            //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
-
-
-			//const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
-#if 1
 			if (m_data->m_commandLogger)
 			{
                 m_data->m_commandLogger->logCommand(clientCmd);
 			}
-#endif
-
-			//m_data->m_testBlock1->m_numProcessedClientCommands++;
-
-			//no timestamp yet
-            //int timeStamp = 0;
-			
-			//catch uninitialized cases
 			serverStatusOut.m_type = CMD_INVALID_STATUS;
 			serverStatusOut.m_numDataStreamBytes = 0;
 			serverStatusOut.m_dataStream = 0;
@@ -2989,32 +3009,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
             //consume the command
 			switch (clientCmd.m_type)
             {
-#if 0
-				case CMD_SEND_BULLET_DATA_STREAM:
-                {
-					if (m_data->m_verboseOutput)
-					{
-						b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
-					}
 
-					btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
-					m_data->m_worldImporters.push_back(worldImporter);
-					bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
-
-                    if (completedOk)
-                    {
-						SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
-						m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
-						m_data->submitServerStatus(status);
-                    } else
-                    {
-						SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp);
-                        m_data->submitServerStatus(status);
-                    }
-
-					break;
-				}
-#endif
 				case CMD_STATE_LOGGING:
 				{
 					BT_PROFILE("CMD_STATE_LOGGING");
@@ -7975,6 +7970,39 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 						break;        
 					}
+					case CMD_CUSTOM_COMMAND:
+					{
+						SharedMemoryStatus& serverCmd = serverStatusOut;
+						serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED;
+						serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1;
+
+						if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN)
+						{
+							//pluginPath could be registered or load from disk
+							int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath);
+							if (pluginUniqueId>=0)
+							{
+								serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId;
+								serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
+							}
+						}
+						if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN)
+						{
+							m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId);
+							serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
+						}
+						if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND)
+						{
+							
+							int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments);
+							serverCmd.m_customCommandResultArgs.m_executeCommandResult = result;
+							serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
+
+						}
+
+						hasStatus = true;
+						break;
+					}
                 default:
                 {
 					BT_PROFILE("CMD_UNKNOWN");
@@ -8193,12 +8221,6 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
 }
 
 
-btVector3 gVRGripperPos(0.7, 0.3, 0.7);
-btQuaternion gVRGripperOrn(0,0,0,1);
-btVector3 gVRController2Pos(0,0,0.2);
-btQuaternion gVRController2Orn(0,0,0,1);
-btScalar gVRGripper2Analog = 0;
-btScalar gVRGripperAnalog = 0;
 
 
 
@@ -8221,6 +8243,8 @@ bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
 void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
 {
 	m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents);
+
+
 	for (int i=0;i<m_data->m_stateLoggers.size();i++)
 	{
 		if (m_data->m_stateLoggers[i]->m_loggingType==STATE_LOGGING_VR_CONTROLLERS)

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -91,6 +91,7 @@ public:
 	virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
 
 	//logging of object states (position etc)
+	void tickPlugins(btScalar timeStep, bool isPreTick);
 	void logObjectStates(btScalar timeStep);
 	void processCollisionForces(btScalar timeStep);
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -109,6 +109,26 @@ struct b3SearchPathfArgs
 	char m_path[MAX_FILENAME_LENGTH];
 };
 
+enum CustomCommandEnum
+{
+	CMD_CUSTOM_COMMAND_LOAD_PLUGIN=1,
+	CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN=2,
+	CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND=4,
+};
+
+struct b3CustomCommand
+{
+	int m_pluginUniqueId;
+	b3PluginArguments m_arguments;
+	char m_pluginPath[MAX_FILENAME_LENGTH];
+};
+
+struct b3CustomCommandResultArgs
+{
+	int m_pluginUniqueId;
+	int m_executeCommandResult;
+
+};
 
 struct BulletDataStreamArgs
 {
@@ -968,6 +988,7 @@ struct SharedMemoryCommand
 		struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs;
 		struct b3ChangeTextureArgs m_changeTextureArgs;
 		struct b3SearchPathfArgs m_searchPathArgs;
+		struct b3CustomCommand m_customCommandArgs;
     };
 };
 
@@ -1039,6 +1060,7 @@ struct SharedMemoryStatus
 		struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs;
 		struct SendMouseEvents m_sendMouseEvents;
 		struct b3LoadTextureResultArgs m_loadTextureResultArguments;
+		struct b3CustomCommandResultArgs m_customCommandResultArgs;
 
 	};
 };

examples/SharedMemory/SharedMemoryPublic.h

@@ -72,6 +72,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_REQUEST_MOUSE_EVENTS_DATA,
 	CMD_CHANGE_TEXTURE,
 	CMD_SET_ADDITIONAL_SEARCH_PATH,
+	CMD_CUSTOM_COMMAND,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -167,6 +168,9 @@ enum EnumSharedMemoryServerStatus
 		CMD_REQUEST_COLLISION_INFO_FAILED,
 		CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED,
 		CMD_CHANGE_TEXTURE_COMMAND_FAILED,
+		CMD_CUSTOM_COMMAND_COMPLETED,
+		CMD_CUSTOM_COMMAND_FAILED,
+
         //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
@@ -616,6 +620,19 @@ enum eStateLoggingFlags
 	STATE_LOG_JOINT_TORQUES = STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES,
 };
 
+#define B3_MAX_PLUGIN_ARG_SIZE 128
+#define B3_MAX_PLUGIN_ARG_TEXT_LEN 1024
+
+struct b3PluginArguments
+{
+	char m_text[B3_MAX_PLUGIN_ARG_TEXT_LEN];
+	int m_numInts;
+	int m_ints[B3_MAX_PLUGIN_ARG_SIZE];
+	int m_numFloats;
+	int m_floats[B3_MAX_PLUGIN_ARG_SIZE];
+
+
+};
 
 
 #endif//SHARED_MEMORY_PUBLIC_H

examples/SharedMemory/b3PluginManager.cpp

@@ -0,0 +1,255 @@
+
+#include "b3PluginManager.h"
+#include "Bullet3Common/b3HashMap.h"
+#include "Bullet3Common/b3ResizablePool.h"
+#include "SharedMemoryPublic.h"
+#include "PhysicsDirect.h"
+#include "plugins/b3PluginContext.h"
+
+#ifdef _WIN32
+    #define WIN32_LEAN_AND_MEAN
+    #define VC_EXTRALEAN
+    #include <windows.h>
+
+    typedef HMODULE             B3_DYNLIB_HANDLE;
+
+    #define B3_DYNLIB_OPEN    LoadLibrary
+    #define B3_DYNLIB_CLOSE   FreeLibrary
+    #define B3_DYNLIB_IMPORT  GetProcAddress
+#else
+    #include <dlfcn.h>
+    
+    typedef void*                   B3_DYNLIB_HANDLE;
+
+    #define B3_DYNLIB_OPEN(path)  dlopen(path, RTLD_NOW | RTLD_GLOBAL)
+    #define B3_DYNLIB_CLOSE       dlclose
+    #define B3_DYNLIB_IMPORT      dlsym
+#endif
+
+struct b3Plugin
+{
+	B3_DYNLIB_HANDLE m_pluginHandle;
+	bool m_ownsPluginHandle;
+	std::string m_pluginPath;
+	int m_pluginUniqueId;
+	PFN_INIT m_initFunc;
+	PFN_EXIT m_exitFunc;
+	PFN_EXECUTE m_executeCommandFunc;
+	
+	PFN_TICK m_preTickFunc;
+	PFN_TICK m_postTickFunc;
+
+	void* m_userPointer;
+
+	b3Plugin()
+		:m_pluginHandle(0),
+		m_ownsPluginHandle(false),
+		m_pluginUniqueId(-1),
+		m_initFunc(0),
+		m_exitFunc(0),
+		m_executeCommandFunc(0),
+		m_postTickFunc(0),
+		m_preTickFunc(0),
+		m_userPointer(0)
+	{
+	}
+	void clear()
+	{
+		if (m_ownsPluginHandle)
+		{
+			B3_DYNLIB_CLOSE(m_pluginHandle);
+		}
+		m_pluginHandle = 0;
+		m_initFunc = 0;
+		m_exitFunc = 0;
+		m_executeCommandFunc = 0;
+		m_preTickFunc = 0;
+		m_postTickFunc = 0;
+		m_userPointer = 0;
+	}
+};
+
+typedef b3PoolBodyHandle<b3Plugin> b3PluginHandle;
+
+struct b3PluginManagerInternalData
+{
+	b3ResizablePool<b3PluginHandle> m_plugins;
+	b3HashMap<b3HashString, b3PluginHandle> m_pluginMap;
+	PhysicsDirect* m_physicsDirect;
+};
+
+b3PluginManager::b3PluginManager(class PhysicsCommandProcessorInterface* physSdk)
+{
+	m_data = new b3PluginManagerInternalData;
+	m_data->m_physicsDirect = new PhysicsDirect(physSdk,false);
+
+}
+
+b3PluginManager::~b3PluginManager()
+{
+	while (m_data->m_pluginMap.size())
+	{
+		b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(0);
+		unloadPlugin(plugin->m_pluginUniqueId);
+	}
+	delete m_data->m_physicsDirect;
+	m_data->m_pluginMap.clear();
+	m_data->m_plugins.exitHandles();
+	delete m_data;
+}
+
+
+int b3PluginManager::loadPlugin(const char* pluginPath)
+{
+	int pluginUniqueId = -1;
+
+	b3Plugin* pluginOrg = m_data->m_pluginMap.find(pluginPath);
+	if (pluginOrg)
+	{
+		//already loaded
+		pluginUniqueId = pluginOrg->m_pluginUniqueId;
+	}
+	else
+	{
+		pluginUniqueId = m_data->m_plugins.allocHandle();
+		b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
+		plugin->m_pluginUniqueId = pluginUniqueId;
+		B3_DYNLIB_HANDLE pluginHandle = B3_DYNLIB_OPEN(pluginPath);
+		bool ok = false;
+		if (pluginHandle)
+		{
+
+			plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, "initPlugin");
+			plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, "exitPlugin");
+			plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, "executePluginCommand");
+			plugin->m_preTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "preTickPluginCallback");
+			plugin->m_postTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "postTickPluginCallback");
+			
+			if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc)
+			{
+
+				b3PluginContext context;
+				context.m_userPointer = plugin->m_userPointer;
+				context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
+				int version = plugin->m_initFunc(&context);
+				//keep the user pointer persistent
+				plugin->m_userPointer = context.m_userPointer;
+				if (version == SHARED_MEMORY_MAGIC_NUMBER)
+				{
+					ok = true;
+					plugin->m_ownsPluginHandle = true;
+					plugin->m_pluginHandle = pluginHandle;
+					plugin->m_pluginPath = pluginPath;
+					m_data->m_pluginMap.insert(pluginPath, *plugin);
+				}
+				else
+				{
+					int expect = SHARED_MEMORY_MAGIC_NUMBER;
+					b3Warning("Warning: plugin is wrong version: expected %d, got %d\n", expect, version);
+				}
+			}
+			else
+			{
+				b3Warning("Loaded plugin but couldn't bind functions");
+			}
+
+			if (!ok)
+			{
+				B3_DYNLIB_CLOSE(pluginHandle);
+			}
+		}
+		else
+		{
+			b3Warning("Warning: couldn't load plugin %s\n", pluginPath);
+		}
+		if (!ok)
+		{
+			m_data->m_plugins.freeHandle(pluginUniqueId);
+			pluginUniqueId = -1;
+		}
+	}
+	return pluginUniqueId;
+}
+
+void b3PluginManager::unloadPlugin(int pluginUniqueId)
+{
+	b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
+	if (plugin)
+	{
+		b3PluginContext context;
+		context.m_userPointer = plugin->m_userPointer;
+		context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
+
+		plugin->m_exitFunc(&context);
+		m_data->m_pluginMap.remove(plugin->m_pluginPath.c_str());
+		m_data->m_plugins.freeHandle(pluginUniqueId);
+	}
+}
+		
+void b3PluginManager::tickPlugins(double timeStep, bool isPreTick)
+{
+	for (int i=0;i<m_data->m_pluginMap.size();i++)
+	{
+		b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(i);
+
+		PFN_TICK  tick = isPreTick? plugin->m_preTickFunc : plugin->m_postTickFunc;
+		if (tick)
+		{
+			b3PluginContext context;
+			context.m_userPointer = plugin->m_userPointer;
+			context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
+			int result = tick(&context);
+			plugin->m_userPointer = context.m_userPointer;
+		}
+	}
+}
+
+int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArguments* arguments)
+{
+	int result = -1;
+
+	b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
+	if (plugin)
+	{
+		b3PluginContext context;
+		context.m_userPointer = plugin->m_userPointer;
+		context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
+		
+		result = plugin->m_executeCommandFunc(&context, arguments);
+		plugin->m_userPointer = context.m_userPointer;
+	}
+	return result;
+}
+
+
+int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc)
+{
+
+	b3Plugin orgPlugin;
+
+	
+	int pluginUniqueId = m_data->m_plugins.allocHandle();
+	b3PluginHandle* pluginHandle = m_data->m_plugins.getHandle(pluginUniqueId);
+	pluginHandle->m_pluginHandle = 0;
+	pluginHandle->m_ownsPluginHandle =false;
+	pluginHandle->m_pluginUniqueId = pluginUniqueId;
+	pluginHandle->m_executeCommandFunc = executeCommandFunc;
+	pluginHandle->m_exitFunc = exitFunc;
+	pluginHandle->m_initFunc = initFunc;
+	pluginHandle->m_preTickFunc = preTickFunc;
+	pluginHandle->m_postTickFunc = postTickFunc;
+	pluginHandle->m_pluginHandle = 0;
+	pluginHandle->m_pluginPath = pluginPath;
+	pluginHandle->m_userPointer = 0;
+	
+	m_data->m_pluginMap.insert(pluginPath, *pluginHandle);
+
+	{
+		b3PluginContext context;
+		context.m_userPointer = 0;
+		context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
+		int result = pluginHandle->m_initFunc(&context);
+		pluginHandle->m_userPointer = context.m_userPointer;
+	}
+	return pluginUniqueId;
+}

examples/SharedMemory/b3PluginManager.h

@@ -0,0 +1,23 @@
+#ifndef B3_PLUGIN_MANAGER_H
+#define B3_PLUGIN_MANAGER_H
+
+#include "plugins/b3PluginAPI.h"
+
+class b3PluginManager
+{
+	struct b3PluginManagerInternalData* m_data;
+
+	public:
+		
+		b3PluginManager(class PhysicsCommandProcessorInterface* physSdk);
+		virtual ~b3PluginManager();
+		
+		int loadPlugin(const char* pluginPath);
+		void unloadPlugin(int pluginUniqueId);
+		int executePluginCommand(int pluginUniqueId, const struct b3PluginArguments* arguments);
+		void tickPlugins(double timeStep, bool isPreTick);
+		int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE m_executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc);
+	
+};
+
+#endif //B3_PLUGIN_MANAGER_H

examples/SharedMemory/plugins/b3PluginAPI.h

@@ -0,0 +1,37 @@
+#ifndef B3_PLUGIN_API_H
+#define B3_PLUGIN_API_H
+
+#ifdef _WIN32
+#define B3_SHARED_API __declspec(dllexport)
+#elif defined (__GNUC__)
+#define B3_SHARED_API __attribute__((visibility("default")))
+#else
+#define B3_SHARED_API
+#endif
+
+
+#if defined(_WIN32)
+#define B3_API_ENTRY
+#define B3_API_CALL     __cdecl 
+#define B3_CALLBACK     __cdecl 
+#else
+#define B3_API_ENTRY
+#define B3_API_CALL
+#define B3_CALLBACK
+#endif
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+	/* Plugin API */
+	typedef B3_API_ENTRY int (B3_API_CALL * PFN_INIT)(struct b3PluginContext* context);
+	typedef B3_API_ENTRY void (B3_API_CALL * PFN_EXIT)(struct b3PluginContext* context);
+	typedef B3_API_ENTRY int (B3_API_CALL * PFN_EXECUTE)(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+	typedef B3_API_ENTRY int (B3_API_CALL * PFN_TICK)(struct b3PluginContext* context);
+#ifdef __cplusplus
+}
+#endif
+
+#endif //B3_PLUGIN_API_H

examples/SharedMemory/plugins/b3PluginContext.h

@@ -0,0 +1,21 @@
+#ifndef B3_PLUGIN_CONTEXT_H
+#define B3_PLUGIN_CONTEXT_H
+
+#include "../PhysicsClientC_API.h"
+
+struct b3PluginContext
+{
+
+	b3PhysicsClientHandle	m_physClient;
+
+	//plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc)
+	void* m_userPointer;
+
+};
+
+
+
+
+
+
+#endif //B3_PLUGIN_CONTEXT_H

examples/SharedMemory/plugins/testPlugin/premake4.lua

@@ -0,0 +1,42 @@
+		
+
+project ("pybullet_testplugin")
+		language "C++"
+		kind "SharedLib"
+		
+		includedirs {".","../../../../src", "../../../../examples",
+		"../../../ThirdPartyLibs"}
+		defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
+	hasCL = findOpenCL("clew")
+
+	links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
+
+
+	if os.is("MacOSX") then
+--		targetextension {"so"}
+		links{"Cocoa.framework","Python"}
+	end
+
+
+		files {
+			"testplugin.cpp",
+			"../../PhysicsClient.cpp",
+			"../../PhysicsClient.h",
+			"../../PhysicsClientSharedMemory.cpp",
+			"../../PhysicsClientSharedMemory.h",
+			"../../PhysicsClientSharedMemory_C_API.cpp",
+			"../../PhysicsClientSharedMemory_C_API.h",
+			"../../PhysicsClientC_API.cpp",
+			"../../PhysicsClientC_API.h",
+			"../../Win32SharedMemory.cpp",
+			"../../Win32SharedMemory.h",
+			"../../PosixSharedMemory.cpp",
+			"../../PosixSharedMemory.h",
+			"../../../Utils/b3Clock.cpp",
+			"../../../Utils/b3Clock.h",
+			"../../../Utils/b3ResourcePath.cpp",
+			"../../../Utils/b3ResourcePath.h",
+				}
+	
+	
+	

examples/SharedMemory/plugins/testPlugin/testplugin.cpp

@@ -0,0 +1,150 @@
+
+//test plugin, can load a URDF file, example usage on a Windows machine:
+
+/*
+import pybullet as p
+p.connect(p.GUI)
+pluginUid = p.loadPlugin("E:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll")
+commandUid = 0
+argument = "plane.urdf"
+p.executePluginCommand(pluginUid,commandUid,argument)
+p.unloadPlugin(pluginUid)
+*/
+
+#include "testplugin.h"
+#include "../../SharedMemoryPublic.h"
+#include "../b3PluginContext.h"
+#include <stdio.h>
+
+struct MyClass
+{
+	int m_testData;
+
+	MyClass()
+		:m_testData(42)
+	{
+	}
+	virtual ~MyClass()
+	{
+	}
+};
+
+B3_SHARED_API int initPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = new MyClass();
+	context->m_userPointer = obj;
+
+	printf("hi!\n");
+	return SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+
+B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass* )context->m_userPointer;
+	
+	{
+		b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(context->m_physClient);
+		int deviceTypeFilter = VR_DEVICE_CONTROLLER;
+		b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
+
+		b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
+		int statusType = b3GetStatusType(statusHandle);
+		if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
+		{
+			struct b3VREventsData vrEvents;
+		
+			int i = 0;
+			b3GetVREventsData(context->m_physClient, &vrEvents);
+			if (vrEvents.m_numControllerEvents)
+			{
+				//this is only for a test, normally you wouldn't print to the console at each simulation substep!
+				printf("got %d VR controller events!\n", vrEvents.m_numControllerEvents);
+			}
+		}
+	}
+	{
+		b3KeyboardEventsData keyboardEventsData;
+		b3SharedMemoryCommandHandle	commandHandle = b3RequestKeyboardEventsCommandInit(context->m_physClient);
+		b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
+		b3GetKeyboardEventsData(context->m_physClient, &keyboardEventsData);
+		if (keyboardEventsData.m_numKeyboardEvents)
+		{
+			//this is only for a test, normally you wouldn't print to the console at each simulation substep!
+			printf("got %d keyboard events\n", keyboardEventsData.m_numKeyboardEvents);
+		}
+	}
+
+	{
+		b3MouseEventsData mouseEventsData;
+		b3SharedMemoryCommandHandle commandHandle = b3RequestMouseEventsCommandInit(context->m_physClient);
+		b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
+		b3GetMouseEventsData(context->m_physClient, &mouseEventsData);
+		if (mouseEventsData.m_numMouseEvents)
+		{
+			//this is only for a test, normally you wouldn't print to the console at each simulation substep!
+			printf("got %d mouse events\n", mouseEventsData.m_numMouseEvents);
+		}
+	}
+
+	return 0;
+}
+
+
+B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass* )context->m_userPointer;
+	obj->m_testData++;
+	return 0;
+}
+
+B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
+{
+	printf("text argument:%s\n",arguments->m_text);
+	printf("int args: [");
+	for (int i=0;i<arguments->m_numInts;i++)
+	{
+		printf("%d", arguments->m_ints[i]);
+		if ((i+1)<arguments->m_numInts)
+		{
+			printf(",");
+		}
+	}
+	printf("]\nfloat args: [");
+	for (int i=0;i<arguments->m_numFloats;i++)
+	{
+		printf("%f", arguments->m_floats[i]);
+		if ((i+1)<arguments->m_numFloats)
+		{
+			printf(",");
+		}
+	}
+	printf("]\n");
+
+	MyClass* obj = (MyClass*) context->m_userPointer;
+	
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType = -1;
+	int bodyUniqueId = -1;
+
+	b3SharedMemoryCommandHandle command =
+		b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_URDF_LOADING_COMPLETED)
+	{
+		bodyUniqueId = b3GetStatusBodyIndex(statusHandle);	
+	}
+	return bodyUniqueId;
+}
+
+
+B3_SHARED_API void exitPlugin(struct b3PluginContext* context)
+{
+	MyClass* obj = (MyClass*) context->m_userPointer;
+	delete obj;
+	context->m_userPointer = 0;
+
+	printf("bye!\n");
+}

examples/SharedMemory/plugins/testPlugin/testplugin.h

@@ -0,0 +1,26 @@
+#ifndef TEST_PLUGIN_H
+#define TEST_PLUGIN_H
+
+#include "../b3PluginAPI.h"
+
+#ifdef __cplusplus
+extern "C" 
+{
+#endif
+
+//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
+B3_SHARED_API int initPlugin(struct b3PluginContext* context);
+B3_SHARED_API void exitPlugin(struct b3PluginContext* context);
+B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
+
+//preTickPluginCallback and postTickPluginCallback are optional.
+B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context);
+B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context);
+
+
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif//#define TEST_PLUGIN_H

examples/SharedMemory/premake4.lua

@@ -7,7 +7,7 @@ else
 	kind "ConsoleApp"
 end
 
-includedirs {".","../../src", "../ThirdPartyLibs",}
+includedirs {".","../../src", "../ThirdPartyLibs"}
 
 links {
 	"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics",	"BulletDynamics","BulletCollision", "LinearMath", "BussIK"
@@ -53,6 +53,8 @@ myfiles =
 	"SharedMemoryCommandProcessor.h",
 	"PhysicsServerCommandProcessor.cpp",
 	"PhysicsServerCommandProcessor.h",
+	"b3PluginManager.cpp",
+	"b3PluginManager.h",
 	"TinyRendererVisualShapeConverter.cpp",
 	"TinyRendererVisualShapeConverter.h",
 	"../TinyRenderer/geometry.cpp",
@@ -99,6 +101,7 @@ myfiles =
 	"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
 	"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
 	"../ThirdPartyLibs/stb_image/stb_image.cpp",     
+
 }
 
 files {
@@ -405,4 +408,4 @@ end
 
 include "udp"
 include "tcp"
-
+include "plugins/testPlugin"

examples/SharedMemory/tcp/premake4.lua

@@ -88,6 +88,7 @@ myfiles =
 	"../SharedMemoryPublic.h",
 	"../PhysicsServerCommandProcessor.cpp",
 	"../PhysicsServerCommandProcessor.h",
+	"../b3PluginManager.cpp",
 	"../TinyRendererVisualShapeConverter.cpp",
 	"../TinyRendererVisualShapeConverter.h",
 	"../../TinyRenderer/geometry.cpp",

examples/SharedMemory/udp/premake4.lua

@@ -79,6 +79,7 @@ myfiles =
 	"../SharedMemoryPublic.h",
 	"../PhysicsServerCommandProcessor.cpp",
 	"../PhysicsServerCommandProcessor.h",
+	"../b3PluginManager.cpp",
 	"../TinyRendererVisualShapeConverter.cpp",
 	"../TinyRendererVisualShapeConverter.h",
 	"../../TinyRenderer/geometry.cpp",

examples/pybullet/CMakeLists.txt

@@ -43,6 +43,9 @@ SET(pybullet_SRCS
 		../../examples/SharedMemory/PhysicsDirectC_API.h
 		../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
 		../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+		../../examples/SharedMemory/b3PluginManager.cpp
+		../../examples/SharedMemory/b3PluginManager.h
+		
 		../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
 		../../examples/SharedMemory/PhysicsClientSharedMemory.h
 		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp

examples/pybullet/premake4.lua

@@ -116,6 +116,8 @@ if not _OPTIONS["no-enet"] then
 			"../../examples/SharedMemory/PhysicsDirectC_API.h",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+			"../../examples/SharedMemory/b3PluginManager.cpp",
+			"../../examples/SharedMemory/b3PluginManager.h",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",

examples/pybullet/pybullet.c

@@ -6653,6 +6653,131 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self,
 	return Py_None;
 }
 
+
+static PyObject* pybullet_loadPlugin(PyObject* self,
+	PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	
+	char* pluginPath = 0;
+	b3SharedMemoryCommandHandle command = 0;
+	b3SharedMemoryStatusHandle 	statusHandle = 0;
+	int statusType = -1;
+
+	b3PhysicsClientHandle sm = 0;
+	static char* kwlist[] = { "pluginPath",  "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &pluginPath, &physicsClientId))
+	{
+		return NULL;
+	}
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	command = b3CreateCustomCommand(sm);
+	b3CustomCommandLoadPlugin(command, pluginPath);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusPluginUniqueId(statusHandle);
+	return PyInt_FromLong(statusType);
+}
+
+
+static PyObject* pybullet_unloadPlugin(PyObject* self,
+	PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	int pluginUniqueId = -1;
+
+	b3SharedMemoryCommandHandle command = 0;
+	b3SharedMemoryStatusHandle 	statusHandle = 0;
+	int statusType = -1;
+
+	b3PhysicsClientHandle sm = 0;
+	static char* kwlist[] = { "pluginUniqueId",  "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &pluginUniqueId,&physicsClientId))
+	{
+		return NULL;
+	}
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	command = b3CreateCustomCommand(sm);
+	b3CustomCommandUnloadPlugin(command, pluginUniqueId);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+
+	Py_INCREF(Py_None);
+	return Py_None;;
+}
+
+//createCustomCommand for executing commands implemented in a plugin system
+static PyObject* pybullet_executePluginCommand(PyObject* self,
+	PyObject* args, PyObject* keywds)
+{
+	int physicsClientId = 0;
+	int pluginUniqueId = -1;
+	char* textArgument = 0;
+	b3SharedMemoryCommandHandle command=0;
+	b3SharedMemoryStatusHandle 	statusHandle=0;
+	int statusType = -1;
+	PyObject* intArgs=0;
+	PyObject* floatArgs=0;
+
+	b3PhysicsClientHandle sm = 0;
+	static char* kwlist[] = { "pluginUniqueId", "textArgument", "intArgs", "floatArgs", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|sOOi", kwlist, &pluginUniqueId, &textArgument, &intArgs, &floatArgs, &physicsClientId))
+	{
+		return NULL;
+	}
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+
+	command = b3CreateCustomCommand(sm);
+	b3CustomCommandExecutePluginCommand(command, pluginUniqueId, textArgument);
+
+	{
+		PyObject* seqIntArgs = intArgs?PySequence_Fast(intArgs, "expected a sequence"):0;
+		PyObject* seqFloatArgs = floatArgs?PySequence_Fast(floatArgs, "expected a sequence"):0;
+		int numIntArgs = seqIntArgs?PySequence_Size(intArgs):0;
+		int numFloatArgs = seqIntArgs?PySequence_Size(floatArgs):0;
+		int i;
+		for (i=0;i<numIntArgs;i++)
+		{
+			int val = pybullet_internalGetIntFromSequence(seqIntArgs,i);
+			b3CustomCommandExecuteAddIntArgument(command, val);
+		}
+		
+
+		for (i=0;i<numFloatArgs;i++)
+		{
+			float val = pybullet_internalGetFloatFromSequence(seqFloatArgs,i);
+			b3CustomCommandExecuteAddFloatArgument(command, val);
+		}
+		
+	}
+	
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusPluginCommandResult(statusHandle);
+	return PyInt_FromLong(statusType);
+}
+
+
+
+
 ///Inverse Kinematics binding
 static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 													 PyObject* args, PyObject* keywds)
@@ -6954,8 +7079,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 }
 
 /// Given an object id, joint positions, joint velocities and joint
-/// accelerations,
+/// accelerations, compute the Jacobian
-/// compute the joint forces using Inverse Dynamics
 static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	{
@@ -7431,6 +7555,16 @@ static PyMethodDef SpamMethods[] = {
 		"Cast a batch of rays and return the result for each of the rays (first object hit, if any. or -1) "
 	 "Takes two arguments (list of from_positions [x,y,z] and a list of to_positions [x,y,z] in Cartesian world coordinates"},
 
+	 { "loadPlugin", (PyCFunction)pybullet_loadPlugin, METH_VARARGS | METH_KEYWORDS,
+		 "Load a plugin, could implement custom commands etc." },
+
+	{ "unloadPlugin", (PyCFunction)pybullet_unloadPlugin, METH_VARARGS | METH_KEYWORDS,
+		"Unload a plugin, given the pluginUniqueId." },
+
+	 { "executePluginCommand", (PyCFunction)pybullet_executePluginCommand, METH_VARARGS | METH_KEYWORDS,
+		"Execute a command, implemented in a plugin." },
+
+
 	{"submitProfileTiming", (PyCFunction)pybullet_submitProfileTiming, METH_VARARGS | METH_KEYWORDS,
 	 "Add a custom profile timing that will be visible in performance profile recordings on the physics server."
 	 "On the physics server (in GUI and VR mode) you can press 'p' to start and/or stop profile recordings" },

setup.py

@@ -70,6 +70,7 @@ sources = ["examples/pybullet/pybullet.c"]\
 +["examples/SharedMemory/PhysicsClientUDP_C_API.cpp"]\
 +["examples/SharedMemory/PhysicsClientTCP.cpp"]\
 +["examples/SharedMemory/PhysicsClientTCP_C_API.cpp"]\
++["examples/SharedMemory/b3PluginManager.cpp"]\
 +["examples/Utils/b3ResourcePath.cpp"]\
 +["examples/Utils/RobotLoggingUtil.cpp"]\
 +["examples/Utils/ChromeTraceUtil.cpp"]\

src/BulletDynamics/Dynamics/btDynamicsWorld.h

@@ -135,6 +135,11 @@ public:
 			return m_solverInfo;
 		}
 
+		const btContactSolverInfo& getSolverInfo() const
+		{
+			return m_solverInfo;
+		}
+
 
 		///obsolete, use addAction instead.
 		virtual void	addVehicle(btActionInterface* vehicle) {(void)vehicle;}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -72,6 +72,11 @@ public:
 		return m_multiBodies[mbIndex];
 	}
 
+	const btMultiBody*	getMultiBody(int mbIndex) const
+	{
+		return m_multiBodies[mbIndex];
+	}
+
 	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
 	virtual int     getNumMultiBodyConstraints() const

test/SharedMemory/CMakeLists.txt

@@ -21,6 +21,9 @@ ENDIF()
 
 IF (NOT WIN32)
 	LINK_LIBRARIES(		pthread	)
+	IF (NOT APPLE)
+		LINK_LIBRARIES(${DL})
+	ENDIF()
 ENDIF()
 
 	ADD_EXECUTABLE(Test_PhysicsClientServer
@@ -39,6 +42,7 @@ ENDIF()
                         ../../examples/SharedMemory/PhysicsDirectC_API.h
                         ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
                         ../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+                        ../../examples/SharedMemory/b3PluginManager.cpp
                         ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
                         ../../examples/SharedMemory/PhysicsClientSharedMemory.h
 						../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp

test/SharedMemory/premake4.lua

@@ -177,6 +177,7 @@ project ("Test_PhysicsServerLoopBack")
 			"../../examples/SharedMemory/PhysicsServerSharedMemory.h",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+			"../../examples/SharedMemory/b3PluginManager.cpp",
 			"../../examples/SharedMemory/PhysicsLoopBack.cpp",
 			"../../examples/SharedMemory/PhysicsLoopBack.h",
 			"../../examples/SharedMemory/PhysicsLoopBackC_API.cpp",
@@ -261,6 +262,7 @@ project ("Test_PhysicsServerLoopBack")
 			"../../examples/SharedMemory/PhysicsDirectC_API.h",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+			"../../examples/SharedMemory/b3PluginManager.cpp",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
 			"../../examples/SharedMemory/PhysicsClientC_API.cpp",
@@ -370,6 +372,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
 			"../../examples/SharedMemory/PhysicsDirectC_API.h",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp",
 			"../../examples/SharedMemory/PhysicsServerCommandProcessor.h",
+			"../../examples/SharedMemory/b3PluginManager.cpp",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
 			"../../examples/SharedMemory/PhysicsClientC_API.cpp",