Merge pull request #1850 from erwincoumans/master

Add preliminary GRPC server for PyBullet and BulletRobotics.

build3/premake4.lua

@@ -70,6 +70,13 @@
 		description = "Use Midi controller to control parameters"
 	}
 	
+	newoption
+	{
+		trigger = "grpc",
+		description = "Build GRPC server/client features for PyBullet/BulletRobotics"
+	}
+	
+
 -- _OPTIONS["midi"] = "1";
 
 	newoption

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -53,32 +53,6 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClient
 	return (b3SharedMemoryCommandHandle) command;
 }
 
-B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
-{
-    PhysicsClient* cl = (PhysicsClient* ) physClient;
-    b3Assert(cl);
-    b3Assert(cl->canSubmitCommand());
-    
-	if (cl->canSubmitCommand())
-	{
-		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
-		b3Assert(command);
-		command->m_type = CMD_LOAD_URDF;
-		int len = strlen(urdfFileName);
-		if (len < MAX_URDF_FILENAME_LENGTH)
-		{
-			strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
-		}
-		else
-		{
-			command->m_urdfArguments.m_urdfFileName[0] = 0;
-		}
-		command->m_updateFlags = URDF_ARGS_FILE_NAME;
-
-		return (b3SharedMemoryCommandHandle)command;
-	}
-	return 0;
-}
 
 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
 {
@@ -310,6 +284,56 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle c
     return 0;
 }
 
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_LOAD_URDF;
+		int len = strlen(urdfFileName);
+		if (len < MAX_URDF_FILENAME_LENGTH)
+		{
+			strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
+		}
+		else
+		{
+			command->m_urdfArguments.m_urdfFileName[0] = 0;
+		}
+		command->m_updateFlags = URDF_ARGS_FILE_NAME;
+
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* urdfFileName)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	
+	command->m_type = CMD_LOAD_URDF;
+	int len = strlen(urdfFileName);
+	if (len < MAX_URDF_FILENAME_LENGTH)
+	{
+		strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
+	}
+	else
+	{
+		command->m_urdfArguments.m_urdfFileName[0] = 0;
+	}
+	command->m_updateFlags = URDF_ARGS_FILE_NAME;
+
+	return (b3SharedMemoryCommandHandle)command;
+}
+
+
+
 B3_SHARED_API int	b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -720,11 +744,18 @@ B3_SHARED_API  b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3Physics
     b3Assert(cl->canSubmitCommand());
     struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
     b3Assert(command);
+	return b3InitStepSimulationCommand2((b3SharedMemoryCommandHandle)command);
+}
+
+B3_SHARED_API b3SharedMemoryCommandHandle	b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
 	command->m_type = CMD_STEP_FORWARD_SIMULATION;
 	command->m_updateFlags = 0;
 	return (b3SharedMemoryCommandHandle)command;
 }
 
+
 B3_SHARED_API b3SharedMemoryCommandHandle     b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -338,13 +338,14 @@ B3_SHARED_API	int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle
 
 
 B3_SHARED_API b3SharedMemoryCommandHandle	b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
+B3_SHARED_API b3SharedMemoryCommandHandle	b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
 
 B3_SHARED_API b3SharedMemoryCommandHandle	b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
 
 ///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
 ///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
 B3_SHARED_API b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
-
+B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* urdfFileName);
 B3_SHARED_API int	b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
 B3_SHARED_API int	b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
 B3_SHARED_API int	b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -3008,8 +3008,10 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 
 
 bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
-                             bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling)
+                             bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int orgFlags, btScalar globalScaling)
 {
+	//clear the LOAD_SDF_FILE=1 bit, which is reserved for internal use of loadSDF command.
+	int flags = orgFlags & ~1;
 	m_data->m_sdfRecentLoadedBodies.clear();
 	*bodyUniqueIdPtr = -1;
 

examples/SharedMemory/grpc/ConvertGRPCBullet.cpp

@@ -0,0 +1,98 @@
+#include "ConvertGRPCBullet.h"
+#include "PhysicsClientC_API.h"
+#include "SharedMemoryCommands.h"
+#include <memory>
+#include <iostream>
+#include <string>
+#include <thread>
+#include <grpc++/grpc++.h>
+#include <grpc/support/log.h>
+#include "pybullet.grpc.pb.h"
+
+using grpc::Server;
+using grpc::ServerAsyncResponseWriter;
+using grpc::ServerBuilder;
+using grpc::ServerContext;
+using grpc::ServerCompletionQueue;
+using grpc::Status;
+using pybullet_grpc::PyBulletCommand;
+using pybullet_grpc::PyBulletStatus;
+using pybullet_grpc::PyBulletAPI;
+
+
+SharedMemoryCommand* convertGRPCAndSubmitCommand(PyBulletCommand& grpcCommand, SharedMemoryCommand& cmd)
+{
+	SharedMemoryCommand* cmdPtr = 0;
+
+	if (grpcCommand.has_loadurdfcommand())
+	{
+		auto grpcCmd = grpcCommand.loadurdfcommand();
+
+		std::string fileName = grpcCmd.urdffilename();
+		if (fileName.length())
+		{
+			cmdPtr = &cmd;
+			b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
+			b3LoadUrdfCommandInit2(commandHandle, fileName.c_str());
+
+			if (grpcCmd.has_initialposition())
+			{
+				const ::pybullet_grpc::vec3& pos = grpcCmd.initialposition();
+				b3LoadUrdfCommandSetStartPosition(commandHandle, pos.x(), pos.y(), pos.z());
+			}
+			if (grpcCmd.has_initialorientation())
+			{
+				const ::pybullet_grpc::quat4& orn = grpcCmd.initialorientation();
+				b3LoadUrdfCommandSetStartOrientation(commandHandle, orn.x(), orn.y(), orn.z(), orn.w());
+			}
+			if (grpcCmd.hasUseMultiBody_case()== ::pybullet_grpc::LoadUrdfCommand::HasUseMultiBodyCase::kUseMultiBody)
+			{
+				b3LoadUrdfCommandSetUseMultiBody( commandHandle, grpcCmd.usemultibody());
+			}
+			if (grpcCmd.hasGlobalScaling_case() == ::pybullet_grpc::LoadUrdfCommand::HasGlobalScalingCase::kGlobalScaling)
+			{
+				b3LoadUrdfCommandSetGlobalScaling(commandHandle, grpcCmd.globalscaling());
+			}
+			if (grpcCmd.hasUseFixedBase_case() == ::pybullet_grpc::LoadUrdfCommand::HasUseFixedBaseCase::kUseFixedBase)
+			{
+				b3LoadUrdfCommandSetUseFixedBase(commandHandle, grpcCmd.usefixedbase());
+			}
+			if (grpcCmd.urdfflags())
+			{
+				b3LoadUrdfCommandSetFlags(commandHandle, grpcCmd.urdfflags());
+			}
+
+		}
+	}
+
+	if (grpcCommand.has_stepsimulationcommand())
+	{
+		cmdPtr = &cmd;
+		b3SharedMemoryCommandHandle commandHandle = (b3SharedMemoryCommandHandle)cmdPtr;
+		b3InitStepSimulationCommand2(commandHandle);
+	}
+
+	return cmdPtr;
+}
+
+bool convertStatusToGRPC(const SharedMemoryStatus& serverStatus, char* bufferServerToClient, int bufferSizeInBytes, PyBulletStatus& grpcReply)
+{
+	bool converted = false;
+	grpcReply.set_statustype(serverStatus.m_type);
+
+	switch (serverStatus.m_type)
+	{ 
+	case CMD_URDF_LOADING_COMPLETED:
+	{
+		::pybullet_grpc::LoadUrdfStatus* stat = grpcReply.mutable_urdfstatus();
+		b3SharedMemoryStatusHandle statusHandle = (b3SharedMemoryStatusHandle)&serverStatus;
+		int objectUniqueId = b3GetStatusBodyIndex(statusHandle);
+		stat->set_objectuniqueid(objectUniqueId);
+	}
+
+	break;
+	
+	}
+	
+	return converted;
+}

examples/SharedMemory/grpc/ConvertGRPCBullet.h

@@ -0,0 +1,17 @@
+
+#ifndef BT_CONVERT_GRPC_BULLET_H
+#define BT_CONVERT_GRPC_BULLET_H
+
+#include "../PhysicsClientC_API.h"
+
+namespace pybullet_grpc
+{
+	class PyBulletCommand;
+	class PyBulletStatus;
+};
+
+struct SharedMemoryCommand* convertGRPCAndSubmitCommand(pybullet_grpc::PyBulletCommand& grpcCommand, struct SharedMemoryCommand& cmd);
+
+bool convertStatusToGRPC(const struct SharedMemoryStatus& serverStatus, char* bufferServerToClient, int bufferSizeInBytes, pybullet_grpc::PyBulletStatus& grpcReply);
+
+#endif //BT_CONVERT_GRPC_BULLET_H

examples/SharedMemory/grpc/main.cpp

@@ -0,0 +1,278 @@
+///PyBullet / BulletRobotics GRPC server.
+///works as standalone GRPC server as as a GRPC server bridge,
+///connecting to a local physics server using shared memory
+
+#include <stdio.h>
+#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "Bullet3Common/b3CommandLineArgs.h"
+#include "PhysicsClientC_API.h"
+#ifdef NO_SHARED_MEMORY
+#include "PhysicsServerCommandProcessor.h"
+typedef PhysicsServerCommandProcessor MyCommandProcessor;
+#else
+#include "SharedMemoryCommandProcessor.h"
+typedef SharedMemoryCommandProcessor MyCommandProcessor;
+#endif //NO_SHARED_MEMORY
+
+#include "SharedMemoryCommands.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "PhysicsServerCommandProcessor.h"
+#include "../Utils/b3Clock.h"
+
+
+#include <memory>
+#include <iostream>
+#include <string>
+#include <thread>
+
+#include <grpc++/grpc++.h>
+#include <grpc/support/log.h>
+
+#include "pybullet.grpc.pb.h"
+
+
+using grpc::Server;
+using grpc::ServerAsyncResponseWriter;
+using grpc::ServerBuilder;
+using grpc::ServerContext;
+using grpc::ServerCompletionQueue;
+using grpc::Status;
+using pybullet_grpc::PyBulletCommand;
+using pybullet_grpc::PyBulletStatus;
+using pybullet_grpc::PyBulletAPI;
+
+bool gVerboseNetworkMessagesServer = true;
+#include "ConvertGRPCBullet.h"
+
+class ServerImpl final {
+public:
+	~ServerImpl() {
+		server_->Shutdown();
+		// Always shutdown the completion queue after the server.
+		cq_->Shutdown();
+	}
+
+	
+	void Run(MyCommandProcessor* comProc) {
+		std::string server_address("0.0.0.0:50051");
+
+		ServerBuilder builder;
+		// Listen on the given address without any authentication mechanism.
+		builder.AddListeningPort(server_address, grpc::InsecureServerCredentials());
+		// Register "service_" as the instance through which we'll communicate with
+		// clients. In this case it corresponds to an *asynchronous* service.
+		builder.RegisterService(&service_);
+		// Get hold of the completion queue used for the asynchronous communication
+		// with the gRPC runtime.
+		cq_ = builder.AddCompletionQueue();
+		// Finally assemble the server.
+		server_ = builder.BuildAndStart();
+		std::cout << "Server listening on " << server_address << std::endl;
+
+		// Proceed to the server's main loop.
+		HandleRpcs(comProc);
+	}
+
+private:
+	// Class encompasing the state and logic needed to serve a request.
+	class CallData {
+	public:
+		// Take in the "service" instance (in this case representing an asynchronous
+		// server) and the completion queue "cq" used for asynchronous communication
+		// with the gRPC runtime.
+		CallData(PyBulletAPI::AsyncService* service, ServerCompletionQueue* cq, MyCommandProcessor* comProc)
+			: service_(service), cq_(cq), responder_(&ctx_), status_(CREATE) , m_finished(false), m_comProc(comProc){
+			// Invoke the serving logic right away.
+			Proceed();
+		}
+
+		enum CallStatus { CREATE, PROCESS, FINISH, TERMINATE };
+
+		CallStatus Proceed() {
+			if (status_ == CREATE) {
+				// Make this instance progress to the PROCESS state.
+				status_ = PROCESS;
+
+				// As part of the initial CREATE state, we *request* that the system
+				// start processing SayHello requests. In this request, "this" acts are
+				// the tag uniquely identifying the request (so that different CallData
+				// instances can serve different requests concurrently), in this case
+				// the memory address of this CallData instance.
+				
+				
+				service_->RequestSubmitCommand(&ctx_, &m_command, &responder_, cq_, cq_,
+					this);
+			}
+			else if (status_ == PROCESS) {
+				// Spawn a new CallData instance to serve new clients while we process
+				// the one for this CallData. The instance will deallocate itself as
+				// part of its FINISH state.
+				new CallData(service_, cq_, m_comProc);
+				status_ = FINISH;
+
+				std::string replyString;
+				// The actual processing.
+
+				SharedMemoryStatus serverStatus;
+				b3AlignedObjectArray<char> buffer;
+				buffer.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+				SharedMemoryCommand cmd;
+				SharedMemoryCommand* cmdPtr = 0;
+				
+				m_status.set_statustype(CMD_UNKNOWN_COMMAND_FLUSHED);
+				
+				cmdPtr = convertGRPCAndSubmitCommand(m_command, cmd);
+								
+
+				if (cmdPtr)
+				{
+					bool hasStatus = m_comProc->processCommand(*cmdPtr, serverStatus, &buffer[0], buffer.size());
+					double timeOutInSeconds = 10;
+					b3Clock clock;
+					double startTimeSeconds = clock.getTimeInSeconds();
+					double curTimeSeconds = clock.getTimeInSeconds();
+
+					while ((!hasStatus) && ((curTimeSeconds - startTimeSeconds) <timeOutInSeconds))
+					{
+						hasStatus = m_comProc->receiveStatus(serverStatus, &buffer[0], buffer.size());
+						curTimeSeconds = clock.getTimeInSeconds();
+					}
+					if (gVerboseNetworkMessagesServer)
+					{
+						//printf("buffer.size = %d\n", buffer.size());
+						printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes);
+					}
+					if (hasStatus)
+					{
+						b3AlignedObjectArray<unsigned char> packetData;
+						unsigned char* statBytes = (unsigned char*)&serverStatus;
+						
+						convertStatusToGRPC(serverStatus, &buffer[0], buffer.size(), m_status);
+					}
+				}
+
+				if (m_command.has_terminateservercommand())
+				{
+					status_ = TERMINATE;
+				}
+
+				
+				// And we are done! Let the gRPC runtime know we've finished, using the
+				// memory address of this instance as the uniquely identifying tag for
+				// the event.
+				
+				responder_.Finish(m_status, Status::OK, this);
+			}
+			else {
+				GPR_ASSERT(status_ == FINISH);
+				// Once in the FINISH state, deallocate ourselves (CallData).
+				delete this;
+			}
+			return status_;
+		}
+
+	private:
+		// The means of communication with the gRPC runtime for an asynchronous
+		// server.
+		PyBulletAPI::AsyncService* service_;
+		// The producer-consumer queue where for asynchronous server notifications.
+		ServerCompletionQueue* cq_;
+		// Context for the rpc, allowing to tweak aspects of it such as the use
+		// of compression, authentication, as well as to send metadata back to the
+		// client.
+		ServerContext ctx_;
+
+		// What we get from the client.
+		PyBulletCommand m_command;
+		// What we send back to the client.
+		PyBulletStatus m_status;
+
+		// The means to get back to the client.
+		ServerAsyncResponseWriter<PyBulletStatus> responder_;
+
+		// Let's implement a tiny state machine with the following states.
+		
+		CallStatus status_;  // The current serving state.
+		
+		bool m_finished;
+		
+		MyCommandProcessor* m_comProc; //physics server command processor
+	};
+
+	// This can be run in multiple threads if needed.
+	void HandleRpcs(MyCommandProcessor* comProc) {
+		// Spawn a new CallData instance to serve new clients.
+		new CallData(&service_, cq_.get(), comProc);
+		void* tag;  // uniquely identifies a request.
+		bool ok;
+		bool finished = false;
+
+		CallData::CallStatus status = CallData::CallStatus::CREATE;
+
+		while (status!= CallData::CallStatus::TERMINATE) {
+			// Block waiting to read the next event from the completion queue. The
+			// event is uniquely identified by its tag, which in this case is the
+			// memory address of a CallData instance.
+			// The return value of Next should always be checked. This return value
+			// tells us whether there is any kind of event or cq_ is shutting down.
+			
+			grpc::CompletionQueue::NextStatus nextStatus = cq_->AsyncNext(&tag, &ok, gpr_now(GPR_CLOCK_MONOTONIC));
+			if (nextStatus == grpc::CompletionQueue::NextStatus::GOT_EVENT)
+			{
+				//GPR_ASSERT(cq_->Next(&tag, &ok));
+				GPR_ASSERT(ok);
+				status = static_cast<CallData*>(tag)->Proceed();
+			}
+		}
+	}
+
+	
+	std::unique_ptr<ServerCompletionQueue> cq_;
+	PyBulletAPI::AsyncService service_;
+	std::unique_ptr<Server> server_;
+};
+
+int main(int argc, char** argv) 
+{
+
+	b3CommandLineArgs parseArgs(argc, argv);
+	b3Clock clock;
+	double timeOutInSeconds = 10;
+
+	DummyGUIHelper guiHelper;
+	MyCommandProcessor* sm = new MyCommandProcessor;
+	sm->setGuiHelper(&guiHelper);
+
+	int port = 6667;
+	parseArgs.GetCmdLineArgument("port", port);
+
+	gVerboseNetworkMessagesServer = parseArgs.CheckCmdLineFlag("verbose");
+
+#ifndef NO_SHARED_MEMORY
+	int key = 0;
+	if (parseArgs.GetCmdLineArgument("sharedMemoryKey", key))
+	{
+		sm->setSharedMemoryKey(key);
+	}
+#endif//NO_SHARED_MEMORY
+
+	bool isPhysicsClientConnected = sm->connect();
+	bool exitRequested = false;
+
+	if (isPhysicsClientConnected)
+	{
+		ServerImpl server;
+
+		server.Run(sm);
+	}
+	else
+	{
+		printf("Couldn't connect to physics server\n");
+	}
+
+	delete sm;
+
+	return 0;
+}
+
+

examples/SharedMemory/grpc/premake4.lua

@@ -0,0 +1,150 @@
+
+project ("App_PhysicsServerSharedMemoryBridgeGRPC")
+
+	language "C++"
+			
+	kind "ConsoleApp"
+	
+	includedirs {"../../ThirdPartyLibs/clsocket/src","../../../src",".."}
+		
+
+	if os.is("Windows") then
+          defines { "WIN32", "_WIN32_WINNT=0x0600" }
+          links {"grpc","grpc++","grpc++_reflection","gpr",
+          "libprotobuf","crypto","ssl","zlibstaticd","Ws2_32","Winmm"  }
+        end
+        if os.is("Linux") then
+                defines {"_LINUX"}
+        end
+        if os.is("MacOSX") then
+                defines {"_DARWIN"}
+        end
+			
+		links {
+			"BulletFileLoader",
+			"Bullet3Common", 
+			"LinearMath"
+		}
+	
+	files {
+		"main.cpp",
+		"../PhysicsClient.cpp",
+		"../PhysicsClient.h",
+		"../PhysicsDirect.cpp",
+		"../PhysicsDirect.h",
+		"../PhysicsCommandProcessorInterface.h",
+		"../SharedMemoryCommandProcessor.cpp",
+		"../SharedMemoryCommandProcessor.h",
+		"../PhysicsClientC_API.cpp",
+		"../PhysicsClientC_API.h",
+		"../Win32SharedMemory.cpp",
+		"../Win32SharedMemory.h",
+		"../PosixSharedMemory.cpp",
+		"../PosixSharedMemory.h",
+		"../../Utils/b3ResourcePath.cpp",
+		"../../Utils/b3ResourcePath.h",
+		"../../Utils/b3Clock.cpp",
+		"../../Utils/b3Clock.h",		
+	}
+
+
+project "App_PhysicsServerGRPC"
+
+if _OPTIONS["ios"] then
+	kind "WindowedApp"
+else	
+	kind "ConsoleApp"
+end
+
+defines { "NO_SHARED_MEMORY" }
+						
+includedirs {"..","../../../src", "../../ThirdPartyLibs","../../ThirdPartyLibs/clsocket/src"}
+
+links {
+	"clsocket","Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics",	"BulletSoftBody", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
+}
+
+
+
+	if os.is("Windows") then
+          defines { "WIN32", "_WIN32_WINNT=0x0600" }
+          links {"grpc","grpc++","grpc++_reflection","gpr",
+          "libprotobuf","crypto","ssl","zlibstaticd","Ws2_32","Winmm"  }
+        end
+        if os.is("Linux") then
+                defines {"_LINUX"}
+                links{"dl"}
+        end
+        if os.is("MacOSX") then
+                defines {"_DARWIN"}
+        end
+
+language "C++"
+
+myfiles = 
+{
+	"../IKTrajectoryHelper.cpp",
+	"../IKTrajectoryHelper.h",
+	"../SharedMemoryCommands.h",
+	"../SharedMemoryPublic.h",
+	"../PhysicsServerCommandProcessor.cpp",
+	"../PhysicsServerCommandProcessor.h",
+	"../b3PluginManager.cpp",
+	"../PhysicsDirect.cpp",
+	"../PhysicsClientC_API.cpp",
+	"../PhysicsClient.cpp",
+	"../plugins/collisionFilterPlugin/collisionFilterPlugin.cpp",
+	"../plugins/pdControlPlugin/pdControlPlugin.cpp",
+	"../plugins/pdControlPlugin/pdControlPlugin.h",
+	"../b3RobotSimulatorClientAPI_NoDirect.cpp",
+	"../b3RobotSimulatorClientAPI_NoDirect.h",
+  "../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
+	"../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
+	"../../TinyRenderer/geometry.cpp",
+	"../../TinyRenderer/model.cpp",
+	"../../TinyRenderer/tgaimage.cpp",
+	"../../TinyRenderer/our_gl.cpp",
+	"../../TinyRenderer/TinyRenderer.cpp",
+	"../../OpenGLWindow/SimpleCamera.cpp",
+	"../../OpenGLWindow/SimpleCamera.h",
+	"../../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
+	"../../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
+	"../../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+	"../../Importers/ImportURDFDemo/MyMultiBodyCreator.h",
+	"../../Importers/ImportMJCFDemo/BulletMJCFImporter.cpp",
+	"../../Importers/ImportMJCFDemo/BulletMJCFImporter.h",
+	"../../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+	"../../Importers/ImportURDFDemo/BulletUrdfImporter.h",
+	"../../Importers/ImportURDFDemo/UrdfParser.cpp",
+ 	"../../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+	"../../Importers/ImportURDFDemo/UrdfParser.cpp",
+	"../../Importers/ImportURDFDemo/UrdfParser.h",
+	"../../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+	"../../Importers/ImportURDFDemo/URDF2Bullet.h",
+	"../../Utils/b3ResourcePath.cpp",
+	"../../Utils/b3Clock.cpp",
+	"../../Utils/ChromeTraceUtil.cpp",
+	"../../Utils/ChromeTraceUtil.h",
+	"../../Utils/RobotLoggingUtil.cpp",
+	"../../Utils/RobotLoggingUtil.h",
+	"../../../Extras/Serialize/BulletWorldImporter/*",
+	"../../../Extras/Serialize/BulletFileLoader/*",	
+	"../../Importers/ImportURDFDemo/URDFImporterInterface.h",
+	"../../Importers/ImportURDFDemo/URDFJointTypes.h",
+	"../../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+	"../../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+	"../../Importers/ImportSTLDemo/ImportSTLSetup.h",
+	"../../Importers/ImportSTLDemo/LoadMeshFromSTL.h",
+	"../../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+	"../../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
+	"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",	
+	"../../ThirdPartyLibs/tinyxml2/tinyxml2.cpp",
+	"../../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+	"../../ThirdPartyLibs/stb_image/stb_image.cpp",     
+}
+
+files {
+	myfiles,
+	"main.cpp",
+}
+

examples/SharedMemory/grpc/proto/createProtobufs.bat

@@ -0,0 +1,19 @@
+del ..\pybullet.pb.cpp
+del ..\pybullet.pb.h
+del ..\pybullet.grpc.pb.cpp
+del ..\pybullet.grpc.pb.h
+
+..\..\..\ThirdPartyLibs\grpc\lib\win32\protoc --proto_path=. --cpp_out=. pybullet.proto
+..\..\..\ThirdPartyLibs\grpc\lib\win32\protoc.exe --plugin=protoc-gen-grpc="..\..\..\ThirdPartyLibs\grpc\lib\win32\grpc_cpp_plugin.exe" --grpc_out=. pybullet.proto
+move pybullet.grpc.pb.cc ..\pybullet.grpc.pb.cpp
+move pybullet.grpc.pb.h ..\pybullet.grpc.pb.h
+move pybullet.pb.cc ..\pybullet.pb.cpp
+move pybullet.pb.h ..\pybullet.pb.h
+
+del ..\pybullet_pb2.py
+del ..\pybullet_pb2_grpc.py
+
+..\..\..\ThirdPartyLibs\grpc\lib\win32\protoc --proto_path=. --python_out=. pybullet.proto
+python -m grpc_tools.protoc -I. --python_out=. --grpc_python_out=. pybullet.proto
+move pybullet_pb2.py ..\pybullet_pb2.py
+move pybullet_pb2_grpc.py ..\pybullet_pb2_grpc.py 

examples/SharedMemory/grpc/proto/createProtobufs.sh

@@ -0,0 +1,19 @@
+rm ../pybullet.pb.cpp
+rm ../pybullet.pb.h
+rm ../pybullet.grpc.pb.cpp
+rm ../pybullet.grpc.pb.h
+
+protoc --proto_path=. --cpp_out=. pybullet.proto
+protoc --plugin=protoc-gen-grpc=`which grpc_cpp_plugin` --grpc_out=. pybullet.proto
+mv pybullet.grpc.pb.cc ../pybullet.grpc.pb.cpp
+mv pybullet.grpc.pb.h ../pybullet.grpc.pb.h
+mv pybullet.pb.cc ../pybullet.pb.cpp
+mv pybullet.pb.h ../pybullet.pb.h
+
+rm ../pybullet_pb2.py
+rm ../pybullet_pb2_grpc.py
+
+protoc --proto_path=. --python_out=. pybullet.proto
+python -m grpc_tools.protoc -I. --python_out=. --grpc_python_out=. pybullet.proto
+mv pybullet_pb2.py ../pybullet_pb2.py
+mv pybullet_pb2_grpc.py ../pybullet_pb2_grpc.py 

examples/SharedMemory/grpc/proto/pybullet.proto

@@ -0,0 +1,85 @@
+syntax = "proto3";
+
+option java_multiple_files = true;
+option java_package = "io.grpc.pybullet_grpc";
+option java_outer_classname = "PyBulletProto";
+option objc_class_prefix = "PBG";
+
+
+package pybullet_grpc;
+
+service PyBulletAPI {
+  // Sends a greeting
+  rpc SubmitCommand (PyBulletCommand) returns (PyBulletStatus) {}
+}
+
+
+message vec3
+{
+	double x=1;
+	double y=2;
+	double z=3;
+};
+
+message quat4
+{
+	double x=1;
+	double y=2;
+	double z=3;
+	double w=4;
+};
+
+
+message TerminateServerCommand
+{
+	string exitReason=1;
+};
+
+message StepSimulationCommand
+{
+};
+
+message LoadUrdfCommand {
+	string urdfFileName=1;
+	vec3 initialPosition=2;
+	quat4 initialOrientation=3;
+	//for why oneof here, see the sad decision here:
+	//https://github.com/protocolbuffers/protobuf/issues/1606
+	oneof hasUseMultiBody	{		int32  useMultiBody=4;	}
+	oneof hasUseFixedBase{	bool useFixedBase=5;	}
+	int32 urdfFlags=6;
+	oneof hasGlobalScaling {	double globalScaling=7;
+	}
+};
+
+message LoadUrdfStatus {
+	int32 objectUniqueId=1;
+}
+
+
+
+
+// The request message containing the command
+message PyBulletCommand {
+  int32 commandType=1;
+  
+  oneof commands {
+    LoadUrdfCommand loadUrdfCommand = 3;
+    TerminateServerCommand terminateServerCommand=4;
+		StepSimulationCommand stepSimulationCommand= 5;
+  }
+
+  
+}
+
+// The response message containing the status
+message PyBulletStatus {
+  int32 statusType=1;
+  
+  oneof status
+  {
+  	LoadUrdfStatus urdfStatus = 2;
+  }
+}
+
+

examples/SharedMemory/grpc/pybullet.grpc.pb.cpp

@@ -0,0 +1,59 @@
+// Generated by the gRPC C++ plugin.
+// If you make any local change, they will be lost.
+// source: pybullet.proto
+
+#include "pybullet.pb.h"
+#include "pybullet.grpc.pb.h"
+
+#include <grpc++/impl/codegen/async_stream.h>
+#include <grpc++/impl/codegen/async_unary_call.h>
+#include <grpc++/impl/codegen/channel_interface.h>
+#include <grpc++/impl/codegen/client_unary_call.h>
+#include <grpc++/impl/codegen/method_handler_impl.h>
+#include <grpc++/impl/codegen/rpc_service_method.h>
+#include <grpc++/impl/codegen/service_type.h>
+#include <grpc++/impl/codegen/sync_stream.h>
+namespace pybullet_grpc {
+
+static const char* PyBulletAPI_method_names[] = {
+  "/pybullet_grpc.PyBulletAPI/SubmitCommand",
+};
+
+std::unique_ptr< PyBulletAPI::Stub> PyBulletAPI::NewStub(const std::shared_ptr< ::grpc::ChannelInterface>& channel, const ::grpc::StubOptions& options) {
+  std::unique_ptr< PyBulletAPI::Stub> stub(new PyBulletAPI::Stub(channel));
+  return stub;
+}
+
+PyBulletAPI::Stub::Stub(const std::shared_ptr< ::grpc::ChannelInterface>& channel)
+  : channel_(channel), rpcmethod_SubmitCommand_(PyBulletAPI_method_names[0], ::grpc::RpcMethod::NORMAL_RPC, channel)
+  {}
+
+::grpc::Status PyBulletAPI::Stub::SubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::pybullet_grpc::PyBulletStatus* response) {
+  return ::grpc::BlockingUnaryCall(channel_.get(), rpcmethod_SubmitCommand_, context, request, response);
+}
+
+::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>* PyBulletAPI::Stub::AsyncSubmitCommandRaw(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) {
+  return new ::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>(channel_.get(), cq, rpcmethod_SubmitCommand_, context, request);
+}
+
+PyBulletAPI::Service::Service() {
+  AddMethod(new ::grpc::RpcServiceMethod(
+      PyBulletAPI_method_names[0],
+      ::grpc::RpcMethod::NORMAL_RPC,
+      new ::grpc::RpcMethodHandler< PyBulletAPI::Service, ::pybullet_grpc::PyBulletCommand, ::pybullet_grpc::PyBulletStatus>(
+          std::mem_fn(&PyBulletAPI::Service::SubmitCommand), this)));
+}
+
+PyBulletAPI::Service::~Service() {
+}
+
+::grpc::Status PyBulletAPI::Service::SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response) {
+  (void) context;
+  (void) request;
+  (void) response;
+  return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
+}
+
+
+}  // namespace pybullet_grpc
+

examples/SharedMemory/grpc/pybullet.grpc.pb.h

@@ -0,0 +1,130 @@
+// Generated by the gRPC C++ plugin.
+// If you make any local change, they will be lost.
+// source: pybullet.proto
+#ifndef GRPC_pybullet_2eproto__INCLUDED
+#define GRPC_pybullet_2eproto__INCLUDED
+
+#include "pybullet.pb.h"
+
+#include <grpc++/impl/codegen/async_stream.h>
+#include <grpc++/impl/codegen/async_unary_call.h>
+#include <grpc++/impl/codegen/method_handler_impl.h>
+#include <grpc++/impl/codegen/proto_utils.h>
+#include <grpc++/impl/codegen/rpc_method.h>
+#include <grpc++/impl/codegen/service_type.h>
+#include <grpc++/impl/codegen/status.h>
+#include <grpc++/impl/codegen/stub_options.h>
+#include <grpc++/impl/codegen/sync_stream.h>
+
+namespace grpc {
+class CompletionQueue;
+class Channel;
+class RpcService;
+class ServerCompletionQueue;
+class ServerContext;
+}  // namespace grpc
+
+namespace pybullet_grpc {
+
+class PyBulletAPI final {
+ public:
+  class StubInterface {
+   public:
+    virtual ~StubInterface() {}
+    // Sends a greeting
+    virtual ::grpc::Status SubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::pybullet_grpc::PyBulletStatus* response) = 0;
+    std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::pybullet_grpc::PyBulletStatus>> AsyncSubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) {
+      return std::unique_ptr< ::grpc::ClientAsyncResponseReaderInterface< ::pybullet_grpc::PyBulletStatus>>(AsyncSubmitCommandRaw(context, request, cq));
+    }
+  private:
+    virtual ::grpc::ClientAsyncResponseReaderInterface< ::pybullet_grpc::PyBulletStatus>* AsyncSubmitCommandRaw(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) = 0;
+  };
+  class Stub final : public StubInterface {
+   public:
+    Stub(const std::shared_ptr< ::grpc::ChannelInterface>& channel);
+    ::grpc::Status SubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::pybullet_grpc::PyBulletStatus* response) override;
+    std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>> AsyncSubmitCommand(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) {
+      return std::unique_ptr< ::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>>(AsyncSubmitCommandRaw(context, request, cq));
+    }
+
+   private:
+    std::shared_ptr< ::grpc::ChannelInterface> channel_;
+    ::grpc::ClientAsyncResponseReader< ::pybullet_grpc::PyBulletStatus>* AsyncSubmitCommandRaw(::grpc::ClientContext* context, const ::pybullet_grpc::PyBulletCommand& request, ::grpc::CompletionQueue* cq) override;
+    const ::grpc::RpcMethod rpcmethod_SubmitCommand_;
+  };
+  static std::unique_ptr<Stub> NewStub(const std::shared_ptr< ::grpc::ChannelInterface>& channel, const ::grpc::StubOptions& options = ::grpc::StubOptions());
+
+  class Service : public ::grpc::Service {
+   public:
+    Service();
+    virtual ~Service();
+    // Sends a greeting
+    virtual ::grpc::Status SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response);
+  };
+  template <class BaseClass>
+  class WithAsyncMethod_SubmitCommand : public BaseClass {
+   private:
+    void BaseClassMustBeDerivedFromService(const Service *service) {}
+   public:
+    WithAsyncMethod_SubmitCommand() {
+      ::grpc::Service::MarkMethodAsync(0);
+    }
+    ~WithAsyncMethod_SubmitCommand() override {
+      BaseClassMustBeDerivedFromService(this);
+    }
+    // disable synchronous version of this method
+    ::grpc::Status SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response) final override {
+      abort();
+      return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
+    }
+    void RequestSubmitCommand(::grpc::ServerContext* context, ::pybullet_grpc::PyBulletCommand* request, ::grpc::ServerAsyncResponseWriter< ::pybullet_grpc::PyBulletStatus>* response, ::grpc::CompletionQueue* new_call_cq, ::grpc::ServerCompletionQueue* notification_cq, void *tag) {
+      ::grpc::Service::RequestAsyncUnary(0, context, request, response, new_call_cq, notification_cq, tag);
+    }
+  };
+  typedef WithAsyncMethod_SubmitCommand<Service > AsyncService;
+  template <class BaseClass>
+  class WithGenericMethod_SubmitCommand : public BaseClass {
+   private:
+    void BaseClassMustBeDerivedFromService(const Service *service) {}
+   public:
+    WithGenericMethod_SubmitCommand() {
+      ::grpc::Service::MarkMethodGeneric(0);
+    }
+    ~WithGenericMethod_SubmitCommand() override {
+      BaseClassMustBeDerivedFromService(this);
+    }
+    // disable synchronous version of this method
+    ::grpc::Status SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response) final override {
+      abort();
+      return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
+    }
+  };
+  template <class BaseClass>
+  class WithStreamedUnaryMethod_SubmitCommand : public BaseClass {
+   private:
+    void BaseClassMustBeDerivedFromService(const Service *service) {}
+   public:
+    WithStreamedUnaryMethod_SubmitCommand() {
+      ::grpc::Service::MarkMethodStreamed(0,
+        new ::grpc::StreamedUnaryHandler< ::pybullet_grpc::PyBulletCommand, ::pybullet_grpc::PyBulletStatus>(std::bind(&WithStreamedUnaryMethod_SubmitCommand<BaseClass>::StreamedSubmitCommand, this, std::placeholders::_1, std::placeholders::_2)));
+    }
+    ~WithStreamedUnaryMethod_SubmitCommand() override {
+      BaseClassMustBeDerivedFromService(this);
+    }
+    // disable regular version of this method
+    ::grpc::Status SubmitCommand(::grpc::ServerContext* context, const ::pybullet_grpc::PyBulletCommand* request, ::pybullet_grpc::PyBulletStatus* response) final override {
+      abort();
+      return ::grpc::Status(::grpc::StatusCode::UNIMPLEMENTED, "");
+    }
+    // replace default version of method with streamed unary
+    virtual ::grpc::Status StreamedSubmitCommand(::grpc::ServerContext* context, ::grpc::ServerUnaryStreamer< ::pybullet_grpc::PyBulletCommand,::pybullet_grpc::PyBulletStatus>* server_unary_streamer) = 0;
+  };
+  typedef WithStreamedUnaryMethod_SubmitCommand<Service > StreamedUnaryService;
+  typedef Service SplitStreamedService;
+  typedef WithStreamedUnaryMethod_SubmitCommand<Service > StreamedService;
+};
+
+}  // namespace pybullet_grpc
+
+
+#endif  // GRPC_pybullet_2eproto__INCLUDED

examples/SharedMemory/grpc/pybullet_client.py

@@ -0,0 +1,28 @@
+"""The Python implementation of the PyBullet GRPC client."""
+
+from __future__ import print_function
+
+import grpc
+
+import pybullet_pb2
+import pybullet_pb2_grpc
+
+
+def run():
+  channel = grpc.insecure_channel('localhost:50051')
+  stub = pybullet_pb2_grpc.PyBulletAPIStub(channel)
+  response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(urdfFileName="plane.urdf", initialPosition=pybullet_pb2.vec3(x=0,y=0,z=0), useMultiBody=False, useFixedBase=True, globalScaling=2, urdfFlags = 1)))
+  print("PyBullet client received: " , response.statusType)
+  print("URDF objectid =", response.urdfStatus.objectUniqueId)
+  
+	
+  response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(stepSimulationCommand=pybullet_pb2.StepSimulationCommand()))
+  print("PyBullet client received: " , response.statusType)
+  
+  
+  #response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(terminateServerCommand=pybullet_pb2.TerminateServerCommand()))
+  #print("PyBullet client received: " , response.statusType)
+
+
+if __name__ == '__main__':
+  run()

examples/SharedMemory/grpc/pybullet_pb2.py

@@ -0,0 +1,508 @@
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: pybullet.proto
+
+import sys
+_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1'))
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+from google.protobuf import descriptor_pb2
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+  name='pybullet.proto',
+  package='pybullet_grpc',
+  syntax='proto3',
+  serialized_pb=_b('\n\x0epybullet.proto\x12\rpybullet_grpc\"\'\n\x04vec3\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\"3\n\x05quat4\x12\t\n\x01x\x18\x01 \x01(\x01\x12\t\n\x01y\x18\x02 \x01(\x01\x12\t\n\x01z\x18\x03 \x01(\x01\x12\t\n\x01w\x18\x04 \x01(\x01\",\n\x16TerminateServerCommand\x12\x12\n\nexitReason\x18\x01 \x01(\t\"\x17\n\x15StepSimulationCommand\"\x9d\x02\n\x0fLoadUrdfCommand\x12\x14\n\x0curdfFileName\x18\x01 \x01(\t\x12,\n\x0finitialPosition\x18\x02 \x01(\x0b\x32\x13.pybullet_grpc.vec3\x12\x30\n\x12initialOrientation\x18\x03 \x01(\x0b\x32\x14.pybullet_grpc.quat4\x12\x16\n\x0cuseMultiBody\x18\x04 \x01(\x05H\x00\x12\x16\n\x0cuseFixedBase\x18\x05 \x01(\x08H\x01\x12\x11\n\turdfFlags\x18\x06 \x01(\x05\x12\x17\n\rglobalScaling\x18\x07 \x01(\x01H\x02\x42\x11\n\x0fhasUseMultiBodyB\x11\n\x0fhasUseFixedBaseB\x12\n\x10hasGlobalScaling\"(\n\x0eLoadUrdfStatus\x12\x16\n\x0eobjectUniqueId\x18\x01 \x01(\x05\"\xfd\x01\n\x0fPyBulletCommand\x12\x13\n\x0b\x63ommandType\x18\x01 \x01(\x05\x12\x39\n\x0floadUrdfCommand\x18\x03 \x01(\x0b\x32\x1e.pybullet_grpc.LoadUrdfCommandH\x00\x12G\n\x16terminateServerCommand\x18\x04 \x01(\x0b\x32%.pybullet_grpc.TerminateServerCommandH\x00\x12\x45\n\x15stepSimulationCommand\x18\x05 \x01(\x0b\x32$.pybullet_grpc.StepSimulationCommandH\x00\x42\n\n\x08\x63ommands\"c\n\x0ePyBulletStatus\x12\x12\n\nstatusType\x18\x01 \x01(\x05\x12\x33\n\nurdfStatus\x18\x02 \x01(\x0b\x32\x1d.pybullet_grpc.LoadUrdfStatusH\x00\x42\x08\n\x06status2_\n\x0bPyBulletAPI\x12P\n\rSubmitCommand\x12\x1e.pybullet_grpc.PyBulletCommand\x1a\x1d.pybullet_grpc.PyBulletStatus\"\x00\x42.\n\x15io.grpc.pybullet_grpcB\rPyBulletProtoP\x01\xa2\x02\x03PBGb\x06proto3')
+)
+
+
+
+
+_VEC3 = _descriptor.Descriptor(
+  name='vec3',
+  full_name='pybullet_grpc.vec3',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='x', full_name='pybullet_grpc.vec3.x', index=0,
+      number=1, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='y', full_name='pybullet_grpc.vec3.y', index=1,
+      number=2, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='z', full_name='pybullet_grpc.vec3.z', index=2,
+      number=3, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=33,
+  serialized_end=72,
+)
+
+
+_QUAT4 = _descriptor.Descriptor(
+  name='quat4',
+  full_name='pybullet_grpc.quat4',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='x', full_name='pybullet_grpc.quat4.x', index=0,
+      number=1, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='y', full_name='pybullet_grpc.quat4.y', index=1,
+      number=2, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='z', full_name='pybullet_grpc.quat4.z', index=2,
+      number=3, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='w', full_name='pybullet_grpc.quat4.w', index=3,
+      number=4, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=74,
+  serialized_end=125,
+)
+
+
+_TERMINATESERVERCOMMAND = _descriptor.Descriptor(
+  name='TerminateServerCommand',
+  full_name='pybullet_grpc.TerminateServerCommand',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='exitReason', full_name='pybullet_grpc.TerminateServerCommand.exitReason', index=0,
+      number=1, type=9, cpp_type=9, label=1,
+      has_default_value=False, default_value=_b("").decode('utf-8'),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=127,
+  serialized_end=171,
+)
+
+
+_STEPSIMULATIONCOMMAND = _descriptor.Descriptor(
+  name='StepSimulationCommand',
+  full_name='pybullet_grpc.StepSimulationCommand',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=173,
+  serialized_end=196,
+)
+
+
+_LOADURDFCOMMAND = _descriptor.Descriptor(
+  name='LoadUrdfCommand',
+  full_name='pybullet_grpc.LoadUrdfCommand',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='urdfFileName', full_name='pybullet_grpc.LoadUrdfCommand.urdfFileName', index=0,
+      number=1, type=9, cpp_type=9, label=1,
+      has_default_value=False, default_value=_b("").decode('utf-8'),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='initialPosition', full_name='pybullet_grpc.LoadUrdfCommand.initialPosition', index=1,
+      number=2, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='initialOrientation', full_name='pybullet_grpc.LoadUrdfCommand.initialOrientation', index=2,
+      number=3, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='useMultiBody', full_name='pybullet_grpc.LoadUrdfCommand.useMultiBody', index=3,
+      number=4, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='useFixedBase', full_name='pybullet_grpc.LoadUrdfCommand.useFixedBase', index=4,
+      number=5, type=8, cpp_type=7, label=1,
+      has_default_value=False, default_value=False,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='urdfFlags', full_name='pybullet_grpc.LoadUrdfCommand.urdfFlags', index=5,
+      number=6, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='globalScaling', full_name='pybullet_grpc.LoadUrdfCommand.globalScaling', index=6,
+      number=7, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+    _descriptor.OneofDescriptor(
+      name='hasUseMultiBody', full_name='pybullet_grpc.LoadUrdfCommand.hasUseMultiBody',
+      index=0, containing_type=None, fields=[]),
+    _descriptor.OneofDescriptor(
+      name='hasUseFixedBase', full_name='pybullet_grpc.LoadUrdfCommand.hasUseFixedBase',
+      index=1, containing_type=None, fields=[]),
+    _descriptor.OneofDescriptor(
+      name='hasGlobalScaling', full_name='pybullet_grpc.LoadUrdfCommand.hasGlobalScaling',
+      index=2, containing_type=None, fields=[]),
+  ],
+  serialized_start=199,
+  serialized_end=484,
+)
+
+
+_LOADURDFSTATUS = _descriptor.Descriptor(
+  name='LoadUrdfStatus',
+  full_name='pybullet_grpc.LoadUrdfStatus',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='objectUniqueId', full_name='pybullet_grpc.LoadUrdfStatus.objectUniqueId', index=0,
+      number=1, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=486,
+  serialized_end=526,
+)
+
+
+_PYBULLETCOMMAND = _descriptor.Descriptor(
+  name='PyBulletCommand',
+  full_name='pybullet_grpc.PyBulletCommand',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='commandType', full_name='pybullet_grpc.PyBulletCommand.commandType', index=0,
+      number=1, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='loadUrdfCommand', full_name='pybullet_grpc.PyBulletCommand.loadUrdfCommand', index=1,
+      number=3, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='terminateServerCommand', full_name='pybullet_grpc.PyBulletCommand.terminateServerCommand', index=2,
+      number=4, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='stepSimulationCommand', full_name='pybullet_grpc.PyBulletCommand.stepSimulationCommand', index=3,
+      number=5, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+    _descriptor.OneofDescriptor(
+      name='commands', full_name='pybullet_grpc.PyBulletCommand.commands',
+      index=0, containing_type=None, fields=[]),
+  ],
+  serialized_start=529,
+  serialized_end=782,
+)
+
+
+_PYBULLETSTATUS = _descriptor.Descriptor(
+  name='PyBulletStatus',
+  full_name='pybullet_grpc.PyBulletStatus',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='statusType', full_name='pybullet_grpc.PyBulletStatus.statusType', index=0,
+      number=1, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='urdfStatus', full_name='pybullet_grpc.PyBulletStatus.urdfStatus', index=1,
+      number=2, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+    _descriptor.OneofDescriptor(
+      name='status', full_name='pybullet_grpc.PyBulletStatus.status',
+      index=0, containing_type=None, fields=[]),
+  ],
+  serialized_start=784,
+  serialized_end=883,
+)
+
+_LOADURDFCOMMAND.fields_by_name['initialPosition'].message_type = _VEC3
+_LOADURDFCOMMAND.fields_by_name['initialOrientation'].message_type = _QUAT4
+_LOADURDFCOMMAND.oneofs_by_name['hasUseMultiBody'].fields.append(
+  _LOADURDFCOMMAND.fields_by_name['useMultiBody'])
+_LOADURDFCOMMAND.fields_by_name['useMultiBody'].containing_oneof = _LOADURDFCOMMAND.oneofs_by_name['hasUseMultiBody']
+_LOADURDFCOMMAND.oneofs_by_name['hasUseFixedBase'].fields.append(
+  _LOADURDFCOMMAND.fields_by_name['useFixedBase'])
+_LOADURDFCOMMAND.fields_by_name['useFixedBase'].containing_oneof = _LOADURDFCOMMAND.oneofs_by_name['hasUseFixedBase']
+_LOADURDFCOMMAND.oneofs_by_name['hasGlobalScaling'].fields.append(
+  _LOADURDFCOMMAND.fields_by_name['globalScaling'])
+_LOADURDFCOMMAND.fields_by_name['globalScaling'].containing_oneof = _LOADURDFCOMMAND.oneofs_by_name['hasGlobalScaling']
+_PYBULLETCOMMAND.fields_by_name['loadUrdfCommand'].message_type = _LOADURDFCOMMAND
+_PYBULLETCOMMAND.fields_by_name['terminateServerCommand'].message_type = _TERMINATESERVERCOMMAND
+_PYBULLETCOMMAND.fields_by_name['stepSimulationCommand'].message_type = _STEPSIMULATIONCOMMAND
+_PYBULLETCOMMAND.oneofs_by_name['commands'].fields.append(
+  _PYBULLETCOMMAND.fields_by_name['loadUrdfCommand'])
+_PYBULLETCOMMAND.fields_by_name['loadUrdfCommand'].containing_oneof = _PYBULLETCOMMAND.oneofs_by_name['commands']
+_PYBULLETCOMMAND.oneofs_by_name['commands'].fields.append(
+  _PYBULLETCOMMAND.fields_by_name['terminateServerCommand'])
+_PYBULLETCOMMAND.fields_by_name['terminateServerCommand'].containing_oneof = _PYBULLETCOMMAND.oneofs_by_name['commands']
+_PYBULLETCOMMAND.oneofs_by_name['commands'].fields.append(
+  _PYBULLETCOMMAND.fields_by_name['stepSimulationCommand'])
+_PYBULLETCOMMAND.fields_by_name['stepSimulationCommand'].containing_oneof = _PYBULLETCOMMAND.oneofs_by_name['commands']
+_PYBULLETSTATUS.fields_by_name['urdfStatus'].message_type = _LOADURDFSTATUS
+_PYBULLETSTATUS.oneofs_by_name['status'].fields.append(
+  _PYBULLETSTATUS.fields_by_name['urdfStatus'])
+_PYBULLETSTATUS.fields_by_name['urdfStatus'].containing_oneof = _PYBULLETSTATUS.oneofs_by_name['status']
+DESCRIPTOR.message_types_by_name['vec3'] = _VEC3
+DESCRIPTOR.message_types_by_name['quat4'] = _QUAT4
+DESCRIPTOR.message_types_by_name['TerminateServerCommand'] = _TERMINATESERVERCOMMAND
+DESCRIPTOR.message_types_by_name['StepSimulationCommand'] = _STEPSIMULATIONCOMMAND
+DESCRIPTOR.message_types_by_name['LoadUrdfCommand'] = _LOADURDFCOMMAND
+DESCRIPTOR.message_types_by_name['LoadUrdfStatus'] = _LOADURDFSTATUS
+DESCRIPTOR.message_types_by_name['PyBulletCommand'] = _PYBULLETCOMMAND
+DESCRIPTOR.message_types_by_name['PyBulletStatus'] = _PYBULLETSTATUS
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+vec3 = _reflection.GeneratedProtocolMessageType('vec3', (_message.Message,), dict(
+  DESCRIPTOR = _VEC3,
+  __module__ = 'pybullet_pb2'
+  # @@protoc_insertion_point(class_scope:pybullet_grpc.vec3)
+  ))
+_sym_db.RegisterMessage(vec3)
+
+quat4 = _reflection.GeneratedProtocolMessageType('quat4', (_message.Message,), dict(
+  DESCRIPTOR = _QUAT4,
+  __module__ = 'pybullet_pb2'
+  # @@protoc_insertion_point(class_scope:pybullet_grpc.quat4)
+  ))
+_sym_db.RegisterMessage(quat4)
+
+TerminateServerCommand = _reflection.GeneratedProtocolMessageType('TerminateServerCommand', (_message.Message,), dict(
+  DESCRIPTOR = _TERMINATESERVERCOMMAND,
+  __module__ = 'pybullet_pb2'
+  # @@protoc_insertion_point(class_scope:pybullet_grpc.TerminateServerCommand)
+  ))
+_sym_db.RegisterMessage(TerminateServerCommand)
+
+StepSimulationCommand = _reflection.GeneratedProtocolMessageType('StepSimulationCommand', (_message.Message,), dict(
+  DESCRIPTOR = _STEPSIMULATIONCOMMAND,
+  __module__ = 'pybullet_pb2'
+  # @@protoc_insertion_point(class_scope:pybullet_grpc.StepSimulationCommand)
+  ))
+_sym_db.RegisterMessage(StepSimulationCommand)
+
+LoadUrdfCommand = _reflection.GeneratedProtocolMessageType('LoadUrdfCommand', (_message.Message,), dict(
+  DESCRIPTOR = _LOADURDFCOMMAND,
+  __module__ = 'pybullet_pb2'
+  # @@protoc_insertion_point(class_scope:pybullet_grpc.LoadUrdfCommand)
+  ))
+_sym_db.RegisterMessage(LoadUrdfCommand)
+
+LoadUrdfStatus = _reflection.GeneratedProtocolMessageType('LoadUrdfStatus', (_message.Message,), dict(
+  DESCRIPTOR = _LOADURDFSTATUS,
+  __module__ = 'pybullet_pb2'
+  # @@protoc_insertion_point(class_scope:pybullet_grpc.LoadUrdfStatus)
+  ))
+_sym_db.RegisterMessage(LoadUrdfStatus)
+
+PyBulletCommand = _reflection.GeneratedProtocolMessageType('PyBulletCommand', (_message.Message,), dict(
+  DESCRIPTOR = _PYBULLETCOMMAND,
+  __module__ = 'pybullet_pb2'
+  # @@protoc_insertion_point(class_scope:pybullet_grpc.PyBulletCommand)
+  ))
+_sym_db.RegisterMessage(PyBulletCommand)
+
+PyBulletStatus = _reflection.GeneratedProtocolMessageType('PyBulletStatus', (_message.Message,), dict(
+  DESCRIPTOR = _PYBULLETSTATUS,
+  __module__ = 'pybullet_pb2'
+  # @@protoc_insertion_point(class_scope:pybullet_grpc.PyBulletStatus)
+  ))
+_sym_db.RegisterMessage(PyBulletStatus)
+
+
+DESCRIPTOR.has_options = True
+DESCRIPTOR._options = _descriptor._ParseOptions(descriptor_pb2.FileOptions(), _b('\n\025io.grpc.pybullet_grpcB\rPyBulletProtoP\001\242\002\003PBG'))
+
+_PYBULLETAPI = _descriptor.ServiceDescriptor(
+  name='PyBulletAPI',
+  full_name='pybullet_grpc.PyBulletAPI',
+  file=DESCRIPTOR,
+  index=0,
+  options=None,
+  serialized_start=885,
+  serialized_end=980,
+  methods=[
+  _descriptor.MethodDescriptor(
+    name='SubmitCommand',
+    full_name='pybullet_grpc.PyBulletAPI.SubmitCommand',
+    index=0,
+    containing_service=None,
+    input_type=_PYBULLETCOMMAND,
+    output_type=_PYBULLETSTATUS,
+    options=None,
+  ),
+])
+_sym_db.RegisterServiceDescriptor(_PYBULLETAPI)
+
+DESCRIPTOR.services_by_name['PyBulletAPI'] = _PYBULLETAPI
+
+# @@protoc_insertion_point(module_scope)

examples/SharedMemory/grpc/pybullet_pb2_grpc.py

@@ -0,0 +1,46 @@
+# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
+import grpc
+
+import pybullet_pb2 as pybullet__pb2
+
+
+class PyBulletAPIStub(object):
+  # missing associated documentation comment in .proto file
+  pass
+
+  def __init__(self, channel):
+    """Constructor.
+
+    Args:
+      channel: A grpc.Channel.
+    """
+    self.SubmitCommand = channel.unary_unary(
+        '/pybullet_grpc.PyBulletAPI/SubmitCommand',
+        request_serializer=pybullet__pb2.PyBulletCommand.SerializeToString,
+        response_deserializer=pybullet__pb2.PyBulletStatus.FromString,
+        )
+
+
+class PyBulletAPIServicer(object):
+  # missing associated documentation comment in .proto file
+  pass
+
+  def SubmitCommand(self, request, context):
+    """Sends a greeting
+    """
+    context.set_code(grpc.StatusCode.UNIMPLEMENTED)
+    context.set_details('Method not implemented!')
+    raise NotImplementedError('Method not implemented!')
+
+
+def add_PyBulletAPIServicer_to_server(servicer, server):
+  rpc_method_handlers = {
+      'SubmitCommand': grpc.unary_unary_rpc_method_handler(
+          servicer.SubmitCommand,
+          request_deserializer=pybullet__pb2.PyBulletCommand.FromString,
+          response_serializer=pybullet__pb2.PyBulletStatus.SerializeToString,
+      ),
+  }
+  generic_handler = grpc.method_handlers_generic_handler(
+      'pybullet_grpc.PyBulletAPI', rpc_method_handlers)
+  server.add_generic_rpc_handlers((generic_handler,))

examples/SharedMemory/premake4.lua

@@ -466,6 +466,7 @@ end
 
 include "udp"
 include "tcp"
+
 include "plugins/testPlugin"
 include "plugins/vrSyncPlugin"
 include "plugins/tinyRendererPlugin"
@@ -473,3 +474,6 @@ include "plugins/tinyRendererPlugin"
 include "plugins/pdControlPlugin"
 include "plugins/collisionFilterPlugin"
 
+if _OPTIONS["grpc"] then
+		include "grpc"
+end

src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp

@@ -33,8 +33,8 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simp
 	(void)simplexSolver;
 
 	btVector3 guessVectors[] = {
-		btVector3(transformB.getOrigin() - transformA.getOrigin()),
-		btVector3(transformA.getOrigin() - transformB.getOrigin()),
+		btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(),
+		btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
 		btVector3(0, 0, 1),
 		btVector3(0, 1, 0),
 		btVector3(1, 0, 0),

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -1,4 +1,4 @@
-/*
+/*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
@@ -83,6 +83,593 @@ void	btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result&
 	getClosestPointsNonVirtual(input,output,debugDraw);
 }
 
+static void btComputeSupport(const btConvexShape* convexA, const btTransform& localTransA, const btConvexShape* convexB, const btTransform& localTransB, const btVector3& dir, bool check2d, btVector3& supAworld, btVector3& supBworld, btVector3& aMinb)
+{
+	btVector3 seperatingAxisInA = (dir)* localTransA.getBasis();
+	btVector3 seperatingAxisInB = (-dir)* localTransB.getBasis();
+
+	btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+	btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+	btVector3 pInA = pInANoMargin;
+	btVector3 qInB = qInBNoMargin;
+
+	supAworld = localTransA(pInA);
+	supBworld = localTransB(qInB);
+
+	if (check2d)
+	{
+		supAworld[2] = 0.f;
+		supBworld[2] = 0.f;
+	}
+
+	aMinb = supAworld - supBworld;
+}
+
+struct btSupportVector 
+{
+	btVector3 v;  //!< Support point in minkowski sum
+	btVector3 v1; //!< Support point in obj1
+	btVector3 v2; //!< Support point in obj2
+};
+
+struct btSimplex 
+{
+	btSupportVector ps[4];
+	int last; //!< index of last added point
+};
+
+static btVector3 ccd_vec3_origin(0, 0, 0);
+
+
+inline void btSimplexInit(btSimplex *s)
+{
+	s->last = -1;
+}
+
+inline int btSimplexSize(const btSimplex *s)
+{
+	return s->last + 1;
+}
+
+inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx)
+{
+	// here is no check on boundaries
+	return &s->ps[idx];
+}
+inline void btSupportCopy(btSupportVector *d, const btSupportVector *s)
+{
+	*d = *s;
+}
+
+inline  void btVec3Copy(btVector3 *v, const btVector3* w)
+{
+	*v = *w;
+}
+
+inline void ccdVec3Add(btVector3*v, const btVector3*w)
+{
+	v->m_floats[0] += w->m_floats[0];
+	v->m_floats[1] += w->m_floats[1];
+	v->m_floats[2] += w->m_floats[2];
+}
+
+
+inline void ccdVec3Sub(btVector3 *v, const btVector3 *w)
+{
+	*v -= *w;
+}
+inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
+{
+	*d = (*v) - (*w);
+	
+}
+inline  btScalar btVec3Dot(const btVector3 *a, const btVector3 *b)
+{
+	btScalar dot;
+	dot = a->dot(*b);
+	
+	return dot;
+}
+
+inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3*b)
+{
+	btVector3 ab;
+	btVec3Sub2(&ab, a, b);
+	return btVec3Dot(&ab, &ab);
+}
+
+
+inline void btVec3Scale(btVector3 *d, btScalar k)
+{
+	d->m_floats[0] *= k;
+	d->m_floats[1] *= k;
+	d->m_floats[2] *= k;
+}
+
+inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
+{
+	d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]);
+	d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]);
+	d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]);
+}
+
+inline void btTripleCross(const btVector3 *a, const btVector3 *b,
+	const btVector3 *c, btVector3 *d)
+{
+	btVector3 e;
+	btVec3Cross(&e, a, b);
+	btVec3Cross(d, &e, c);
+}
+
+inline int ccdEq(btScalar _a, btScalar _b)
+{
+	btScalar ab;
+	btScalar a, b;
+
+	ab = btFabs(_a - _b);
+	if (btFabs(ab) < SIMD_EPSILON)
+		return 1;
+
+	a = btFabs(_a);
+	b = btFabs(_b);
+	if (b > a) {
+		return ab < SIMD_EPSILON * b;
+	}
+	else {
+		return ab < SIMD_EPSILON * a;
+	}
+}
+
+btScalar ccdVec3X(const btVector3* v)
+{
+	return v->x();
+}
+
+btScalar ccdVec3Y(const btVector3* v)
+{
+	return v->y();
+}
+
+btScalar ccdVec3Z(const btVector3* v)
+{
+	return v->z();
+}
+inline int btVec3Eq(const btVector3 *a, const btVector3 *b)
+{
+	return ccdEq(ccdVec3X(a), ccdVec3X(b))
+		&& ccdEq(ccdVec3Y(a), ccdVec3Y(b))
+		&& ccdEq(ccdVec3Z(a), ccdVec3Z(b));
+}
+
+
+inline void btSimplexAdd(btSimplex *s, const btSupportVector *v)
+{
+	// here is no check on boundaries in sake of speed
+	++s->last;
+	btSupportCopy(s->ps + s->last, v);
+}
+
+
+inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a)
+{
+	btSupportCopy(s->ps + pos, a);
+}
+
+inline void btSimplexSetSize(btSimplex *s, int size)
+{
+	s->last = size - 1;
+}
+
+inline const btSupportVector *ccdSimplexLast(const btSimplex *s)
+{
+	return btSimplexPoint(s, s->last);
+}
+
+inline int ccdSign(btScalar val)
+{
+	if (btFuzzyZero(val)) {
+		return 0;
+	}
+	else if (val < btScalar(0)) {
+		return -1;
+	}
+	return 1;
+}
+
+
+inline btScalar btVec3PointSegmentDist2(const btVector3 *P,
+	const btVector3 *x0,
+	const btVector3 *b,
+	btVector3 *witness)
+{
+	// The computation comes from solving equation of segment:
+	//      S(t) = x0 + t.d
+	//          where - x0 is initial point of segment
+	//                - d is direction of segment from x0 (|d| > 0)
+	//                - t belongs to <0, 1> interval
+	// 
+	// Than, distance from a segment to some point P can be expressed:
+	//      D(t) = |x0 + t.d - P|^2
+	//          which is distance from any point on segment. Minimization
+	//          of this function brings distance from P to segment.
+	// Minimization of D(t) leads to simple quadratic equation that's
+	// solving is straightforward.
+	//
+	// Bonus of this method is witness point for free.
+
+	btScalar dist, t;
+	btVector3 d, a;
+
+	// direction of segment
+	btVec3Sub2(&d, b, x0);
+
+	// precompute vector from P to x0
+	btVec3Sub2(&a, x0, P);
+
+	t = -btScalar(1.) * btVec3Dot(&a, &d);
+	t /= btVec3Dot(&d, &d);
+
+	if (t < btScalar(0) || btFuzzyZero(t)) {
+		dist = ccdVec3Dist2(x0, P);
+		if (witness)
+			btVec3Copy(witness, x0);
+	}
+	else if (t > btScalar(1) || ccdEq(t, btScalar(1))) {
+		dist = ccdVec3Dist2(b, P);
+		if (witness)
+			btVec3Copy(witness, b);
+	}
+	else {
+		if (witness) {
+			btVec3Copy(witness, &d);
+			btVec3Scale(witness, t);
+			ccdVec3Add(witness, x0);
+			dist = ccdVec3Dist2(witness, P);
+		}
+		else {
+			// recycling variables
+			btVec3Scale(&d, t);
+			ccdVec3Add(&d, &a);
+			dist = btVec3Dot(&d, &d);
+		}
+	}
+
+	return dist;
+}
+
+
+btScalar btVec3PointTriDist2(const btVector3 *P,
+	const btVector3 *x0, const btVector3 *B,
+	const btVector3 *C,
+	btVector3 *witness)
+{
+	// Computation comes from analytic expression for triangle (x0, B, C)
+	//      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
+	// Then equation for distance is:
+	//      D(s, t) = | T(s, t) - P |^2
+	// This leads to minimization of quadratic function of two variables.
+	// The solution from is taken only if s is between 0 and 1, t is
+	// between 0 and 1 and t + s < 1, otherwise distance from segment is
+	// computed.
+
+	btVector3 d1, d2, a;
+	double u, v, w, p, q, r;
+	double s, t, dist, dist2;
+	btVector3 witness2;
+
+	btVec3Sub2(&d1, B, x0);
+	btVec3Sub2(&d2, C, x0);
+	btVec3Sub2(&a, x0, P);
+
+	u = btVec3Dot(&a, &a);
+	v = btVec3Dot(&d1, &d1);
+	w = btVec3Dot(&d2, &d2);
+	p = btVec3Dot(&a, &d1);
+	q = btVec3Dot(&a, &d2);
+	r = btVec3Dot(&d1, &d2);
+
+	s = (q * r - w * p) / (w * v - r * r);
+	t = (-s * r - q) / w;
+
+	if ((btFuzzyZero(s) || s > btScalar(0))
+		&& (ccdEq(s, btScalar(1)) || s < btScalar(1))
+		&& (btFuzzyZero(t) || t > btScalar(0))
+		&& (ccdEq(t, btScalar(1)) || t < btScalar(1))
+		&& (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) {
+
+		if (witness) 
+		{
+			btVec3Scale(&d1, s);
+			btVec3Scale(&d2, t);
+			btVec3Copy(witness, x0);
+			ccdVec3Add(witness, &d1);
+			ccdVec3Add(witness, &d2);
+
+			dist = ccdVec3Dist2(witness, P);
+		}
+		else 
+		{
+			dist = s * s * v;
+			dist += t * t * w;
+			dist += btScalar(2.) * s * t * r;
+			dist += btScalar(2.) * s * p;
+			dist += btScalar(2.) * t * q;
+			dist += u;
+		}
+	}
+	else {
+		dist = btVec3PointSegmentDist2(P, x0, B, witness);
+
+		dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2);
+		if (dist2 < dist) {
+			dist = dist2;
+			if (witness)
+				btVec3Copy(witness, &witness2);
+		}
+
+		dist2 = btVec3PointSegmentDist2(P, B, C, &witness2);
+		if (dist2 < dist) {
+			dist = dist2;
+			if (witness)
+				btVec3Copy(witness, &witness2);
+		}
+	}
+
+	return dist;
+}
+
+
+static int btDoSimplex2(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B;
+	btVector3 AB, AO, tmp;
+	btScalar dot;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other point
+	B = btSimplexPoint(simplex, 0);
+	// compute AB oriented segment
+	btVec3Sub2(&AB, &B->v, &A->v);
+	// compute AO vector
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+
+	// dot product AB . AO
+	dot = btVec3Dot(&AB, &AO);
+
+	// check if origin doesn't lie on AB segment
+	btVec3Cross(&tmp, &AB, &AO);
+	if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) {
+		return 1;
+	}
+
+	// check if origin is in area where AB segment is
+	if (btFuzzyZero(dot) || dot < btScalar(0)) {
+		// origin is in outside are of A
+		btSimplexSet(simplex, 0, A);
+		btSimplexSetSize(simplex, 1);
+		btVec3Copy(dir, &AO);
+	}
+	else {
+		// origin is in area where AB segment is
+
+		// keep simplex untouched and set direction to
+		// AB x AO x AB
+		btTripleCross(&AB, &AO, &AB, dir);
+	}
+
+	return 0;
+}
+
+
+
+static int btDoSimplex3(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B, *C;
+	btVector3 AO, AB, AC, ABC, tmp;
+	btScalar dot, dist;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other points
+	B = btSimplexPoint(simplex, 1);
+	C = btSimplexPoint(simplex, 0);
+
+	// check touching contact
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
+	if (btFuzzyZero(dist)) {
+		return 1;
+	}
+
+	// check if triangle is really triangle (has area > 0)
+	// if not simplex can't be expanded and thus no itersection is found
+	if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) {
+		return -1;
+	}
+
+	// compute AO vector
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+
+	// compute AB and AC segments and ABC vector (perpendircular to triangle)
+	btVec3Sub2(&AB, &B->v, &A->v);
+	btVec3Sub2(&AC, &C->v, &A->v);
+	btVec3Cross(&ABC, &AB, &AC);
+
+	btVec3Cross(&tmp, &ABC, &AC);
+	dot = btVec3Dot(&tmp, &AO);
+	if (btFuzzyZero(dot) || dot > btScalar(0)) {
+		dot = btVec3Dot(&AC, &AO);
+		if (btFuzzyZero(dot) || dot > btScalar(0)) {
+			// C is already in place
+			btSimplexSet(simplex, 1, A);
+			btSimplexSetSize(simplex, 2);
+			btTripleCross(&AC, &AO, &AC, dir);
+		}
+		else {
+		
+			dot = btVec3Dot(&AB, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, A);
+				btSimplexSetSize(simplex, 2);
+				btTripleCross(&AB, &AO, &AB, dir);
+			}
+			else {
+				btSimplexSet(simplex, 0, A);
+				btSimplexSetSize(simplex, 1);
+				btVec3Copy(dir, &AO);
+			}
+		}
+	}
+	else {
+		btVec3Cross(&tmp, &AB, &ABC);
+		dot = btVec3Dot(&tmp, &AO);
+		if (btFuzzyZero(dot) || dot > btScalar(0)) 
+		{
+			dot = btVec3Dot(&AB, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, A);
+				btSimplexSetSize(simplex, 2);
+				btTripleCross(&AB, &AO, &AB, dir);
+			}
+			else {
+				btSimplexSet(simplex, 0, A);
+				btSimplexSetSize(simplex, 1);
+				btVec3Copy(dir, &AO);
+			}
+		}
+		else {
+			dot = btVec3Dot(&ABC, &AO);
+			if (btFuzzyZero(dot) || dot > btScalar(0)) {
+				btVec3Copy(dir, &ABC);
+			}
+			else {
+				btSupportVector tmp;
+				btSupportCopy(&tmp, C);
+				btSimplexSet(simplex, 0, B);
+				btSimplexSet(simplex, 1, &tmp);
+
+				btVec3Copy(dir, &ABC);
+				btVec3Scale(dir, -btScalar(1));
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int btDoSimplex4(btSimplex *simplex, btVector3 *dir)
+{
+	const btSupportVector *A, *B, *C, *D;
+	btVector3 AO, AB, AC, AD, ABC, ACD, ADB;
+	int B_on_ACD, C_on_ADB, D_on_ABC;
+	int AB_O, AC_O, AD_O;
+	btScalar dist;
+
+	// get last added as A
+	A = ccdSimplexLast(simplex);
+	// get the other points
+	B = btSimplexPoint(simplex, 2);
+	C = btSimplexPoint(simplex, 1);
+	D = btSimplexPoint(simplex, 0);
+
+	// check if tetrahedron is really tetrahedron (has volume > 0)
+	// if it is not simplex can't be expanded and thus no intersection is
+	// found
+	dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist)) {
+		return -1;
+	}
+
+	// check if origin lies on some of tetrahedron's face - if so objects
+	// intersect
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+	dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0);
+	if (btFuzzyZero(dist))
+		return 1;
+
+	// compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
+	btVec3Copy(&AO, &A->v);
+	btVec3Scale(&AO, -btScalar(1));
+	btVec3Sub2(&AB, &B->v, &A->v);
+	btVec3Sub2(&AC, &C->v, &A->v);
+	btVec3Sub2(&AD, &D->v, &A->v);
+	btVec3Cross(&ABC, &AB, &AC);
+	btVec3Cross(&ACD, &AC, &AD);
+	btVec3Cross(&ADB, &AD, &AB);
+
+	// side (positive or negative) of B, C, D relative to planes ACD, ADB
+	// and ABC respectively
+	B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB));
+	C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC));
+	D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD));
+
+	// whether origin is on same side of ACD, ADB, ABC as B, C, D
+	// respectively
+	AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD;
+	AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB;
+	AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC;
+
+	if (AB_O && AC_O && AD_O) {
+		// origin is in tetrahedron
+		return 1;
+		// rearrange simplex to triangle and call btDoSimplex3()
+	}
+	else if (!AB_O) {
+		// B is farthest from the origin among all of the tetrahedron's
+		// points, so remove it from the list and go on with the triangle
+		// case
+
+		// D and C are in place
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+	else if (!AC_O) {
+		// C is farthest
+		btSimplexSet(simplex, 1, D);
+		btSimplexSet(simplex, 0, B);
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+	else { // (!AD_O)
+		btSimplexSet(simplex, 0, C);
+		btSimplexSet(simplex, 1, B);
+		btSimplexSet(simplex, 2, A);
+		btSimplexSetSize(simplex, 3);
+	}
+
+	return btDoSimplex3(simplex, dir);
+}
+
+static int btDoSimplex(btSimplex *simplex, btVector3 *dir)
+{
+	if (btSimplexSize(simplex) == 2) {
+		// simplex contains segment only one segment
+		return btDoSimplex2(simplex, dir);
+	}
+	else if (btSimplexSize(simplex) == 3) {
+		// simplex contains triangle
+		return btDoSimplex3(simplex, dir);
+	}
+	else { // btSimplexSize(simplex) == 4
+			// tetrahedron - this is the only shape which can encapsule origin
+			// so btDoSimplex4() also contains test on it
+		return btDoSimplex4(simplex, dir);
+	}
+}
+
 #ifdef __SPU__
 void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
 #else
@@ -125,23 +712,132 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 	m_degenerateSimplex = 0;
 	
 	m_lastUsedMethod = -1;
+	int status = -2;
+	btVector3 orgNormalInB(0, 0, 0);
+	btScalar margin = marginA + marginB;
 	
+	//we add a separate implementation to check if the convex shapes intersect
+	//See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang
+	//Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver
+	//and remove this temporary code from libCCD
+	//this fixes issue https://github.com/bulletphysics/bullet3/issues/1703
+	//note, for large differences in shapes, use double precision build!
 	{
 		btScalar squaredDistance = BT_LARGE_FLOAT;
 		btScalar delta = btScalar(0.);
 
-		btScalar margin = marginA + marginB;
 
 
 		
+		btSimplex simplex1;
+		btSimplex* simplex = &simplex1;
+		btSimplexInit(simplex);
+
+		btVector3 dir(1, 0, 0);
+
+		{
+
+			btVector3 lastSupV;
+			btVector3 supAworld;
+			btVector3 supBworld;
+			btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
+			
+			btSupportVector last;
+			last.v = lastSupV;
+			last.v1 = supAworld;
+			last.v2 = supBworld;
+
+			btSimplexAdd(simplex, &last);
+
+			dir = -lastSupV;
+
+			
+			
+			// start iterations
+			for (int iterations = 0; iterations <gGjkMaxIter; iterations++)
+			{
+				// obtain support point
+				btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV);
+
+				// check if farthest point in Minkowski difference in direction dir
+				// isn't somewhere before origin (the test on negative dot product)
+				// - because if it is, objects are not intersecting at all.
+				btScalar delta = lastSupV.dot(dir);
+				if (delta < 0)
+				{
+					//no intersection, besides margin
+					status = -1;
+					break;
+				}
+				
+				// add last support vector to simplex
+				last.v = lastSupV;
+				last.v1 = supAworld;
+				last.v2 = supBworld;
+
+				btSimplexAdd(simplex, &last);
+
+				// if btDoSimplex returns 1 if objects intersect, -1 if objects don't
+				// intersect and 0 if algorithm should continue
+
+				btVector3 newDir;
+				int do_simplex_res = btDoSimplex(simplex, &dir); 
+
+				if (do_simplex_res == 1) 
+				{
+					status = 0; // intersection found
+					break;
+				}
+				else if (do_simplex_res == -1) 
+				{
+					// intersection not found
+					status = -1;
+					break;
+				}
+				
+				if (btFuzzyZero(btVec3Dot(&dir, &dir))) 
+				{
+					// intersection not found
+					status = -1;
+				}
+
+				if (dir.length2() < SIMD_EPSILON)
+				{
+					//no intersection, besides margin
+					status = -1;
+					break;
+				}
+
+				if (dir.fuzzyZero())
+				{
+					// intersection not found
+					status = -1;
+					break;
+				}
+			}
+
+		}
+
 		m_simplexSolver->reset();
+		if (status == 0)
+		{
+			//status = 0;
+			//printf("Intersect!\n");
+		}
 
+		if (status==-1)
+		{
+			//printf("not intersect\n");
+		}
+		//printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);
+		if (1)
+		{
 			for (; ; )
 				//while (true)
 			{
 
-			btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis();
-			btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis();
+				btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis();
+				btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis();
 
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
@@ -185,7 +881,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					if (f0 <= btScalar(0.))
 					{
 						m_degenerateSimplex = 2;
-				} else
+					}
+					else
 					{
 						m_degenerateSimplex = 11;
 					}
@@ -297,19 +994,24 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					pointOnB += m_cachedSeparatingAxis * (marginB / s);
 					distance = ((btScalar(1.) / rlen) - margin);
 					isValid = true;
+					orgNormalInB = normalInB;
 
 					m_lastUsedMethod = 1;
-			} else
+				}
+				else
 				{
 					m_lastUsedMethod = 2;
 				}
 			}
+		}
+
+
 		
 		bool catchDegeneratePenetrationCase = 
 			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
 
 		//if (checkPenetration && !isValid)
-		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))
+		if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase )) || (status == 0))
 		{
 			//penetration case
 
@@ -331,6 +1033,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 					);
 
 
+				if (m_cachedSeparatingAxis.length2())
+				{
 					if (isValid2)
 					{
 						btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
@@ -353,18 +1057,20 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 								pointOnA = tmpPointOnA;
 								pointOnB = tmpPointOnB;
 								normalInB = tmpNormalInB;
-							
 								isValid = true;
 
-						} else
+							}
+							else
 							{
 								m_lastUsedMethod = 8;
 							}
-					} else
+						}
+						else
 						{
 							m_lastUsedMethod = 9;
 						}
-				} else
+					}
+					else
 
 					{
 						///this is another degenerate case, where the initial GJK calculation reports a degenerate case
@@ -390,12 +1096,17 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 								isValid = true;
 								m_lastUsedMethod = 6;
-						} else
+							}
+							else
 							{
 								m_lastUsedMethod = 5;
 							}
 						}
 					}
+				} else
+				{
+					//printf("EPA didn't return a valid value\n");
+				}
 				
 			}
 
@@ -409,17 +1120,17 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 
 		m_cachedSeparatingAxis = normalInB;
 		m_cachedSeparatingDistance = distance;
-
+		if (1)
 		{
 		///todo: need to track down this EPA penetration solver degeneracy
 		///the penetration solver reports penetration but the contact normal
 		///connecting the contact points is pointing in the opposite direction
 		///until then, detect the issue and revert the normal
 
-			btScalar d1=0;
+			btScalar d2 = 0.f;
 			{
-				btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis();
-				btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis();
+				btVector3 seperatingAxisInA = (-orgNormalInB)* localTransA.getBasis();
+				btVector3 seperatingAxisInB = orgNormalInB* localTransB.getBasis();
 
 
 				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
@@ -428,7 +1139,24 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 				btVector3  pWorld = localTransA(pInA);
 				btVector3  qWorld = localTransB(qInB);
 				btVector3 w = pWorld - qWorld;
-				d1 = (-normalInB).dot(w);
+				d2 = orgNormalInB.dot(w)- margin;
+			}
+			
+			btScalar d1=0;
+			{
+				
+				btVector3 seperatingAxisInA = (normalInB)* localTransA.getBasis();
+				btVector3 seperatingAxisInB = -normalInB* localTransB.getBasis();
+			
+
+				btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA);
+				btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB);
+
+				btVector3  pWorld = localTransA(pInA);	
+				btVector3  qWorld = localTransB(qInB);
+				btVector3 w	= pWorld - qWorld;
+				d1 = (-normalInB).dot(w)- margin;
+
 			}
 			btScalar d0 = 0.f;
 			{
@@ -442,21 +1170,37 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 				btVector3  pWorld = localTransA(pInA);	
 				btVector3  qWorld = localTransB(qInB);
 				btVector3 w	= pWorld - qWorld;
-				d0 = normalInB.dot(w);
+				d0 = normalInB.dot(w)-margin;
 			}
+			
 			if (d1>d0)
 			{
 				m_lastUsedMethod = 10;
 				normalInB*=-1;
 			} 
 
+			if (orgNormalInB.length2())
+			{
+				if (d2 > d0 && d2 > d1 && d2 > distance)
+				{
+
+					normalInB = orgNormalInB;
+					distance = d2;
 				}
+			}
+		}
+
+
 		output.addContactPoint(
 			normalInB,
 			pointOnB+positionOffset,
 			distance);
 
 	}
+	else
+	{
+		//printf("invalid gjk query\n");
+	}
 
 
 }