build3/premake4.lua

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

examples/CommonInterfaces/CommonMultiBodyBase.h

@@ -478,6 +478,12 @@ struct CommonMultiBodyBase : public CommonExampleInterface
 		if (m_pickedConstraint)
 		{
 			m_dynamicsWorld->removeConstraint(m_pickedConstraint);
+
+			if (m_pickedBody)
+			{
+				m_pickedBody->forceActivationState(ACTIVE_TAG);
+				m_pickedBody->activate(true);
+			}
 			delete m_pickedConstraint;
 			m_pickedConstraint = 0;
 			m_pickedBody = 0;

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -564,9 +564,13 @@ bool findExistingMeshFile(
 		return false;
 	}
 
-	std::string drop_it = "package://";
-	if (fn.substr(0, drop_it.length())==drop_it)
-		fn = fn.substr(drop_it.length());
+	std::string drop_it_pack = "package://";
+	std::string drop_it_model = "model://";
+	if (fn.substr(0, drop_it_pack.length())==drop_it_pack)
+		fn = fn.substr(drop_it_pack.length());
+        else if (fn.substr(0, drop_it_model.length())==drop_it_model)
+		fn = fn.substr(drop_it_model.length());
+
 
 	std::list<std::string> shorter;
 	shorter.push_back("../..");

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -610,9 +610,10 @@ void ConvertURDF2BulletInternal(
 					}
                 } else
                 {
-					if (canSleep)
+//					if (canSleep)
 					{
-						if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumDofs()==0)
+						if (cache.m_bulletMultiBody->getBaseMass()==0)
+							//&& cache.m_bulletMultiBody->getNumDofs()==0)
 						{
 							//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
 							col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);

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;
@@ -629,6 +653,15 @@ B3_SHARED_API    int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCom
     return 0;
 }
 
+B3_SHARED_API    int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+	command->m_physSimParamArgs.m_minimumSolverIslandSize = minimumSolverIslandSize;
+	command->m_updateFlags |= SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE;
+	return 0;
+}
+
 
 B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
 {
@@ -711,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;
+	return (b3SharedMemoryCommandHandle)command;
 }
 
+
 B3_SHARED_API b3SharedMemoryCommandHandle     b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -320,7 +320,8 @@ B3_SHARED_API	int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommand
 B3_SHARED_API	int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop);
 B3_SHARED_API	int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT);
 B3_SHARED_API    int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType);
-    
+B3_SHARED_API    int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize);
+
     
 
 
@@ -337,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

@@ -2367,7 +2367,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 
 	//int maxProxies = 32768;
 	//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
-    m_data->m_broadphase = new btDbvtBroadphase(m_data->m_pairCache);
+	btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache);
+	bv->setVelocityPrediction(0);
+	m_data->m_broadphase = bv;
     
 
     m_data->m_solver = new btMultiBodyConstraintSolver;
@@ -2390,6 +2392,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
 	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
+	gDbvtMargin = btScalar(0);
 	m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
 //	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
 	//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
@@ -3004,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/SharedMemoryCommands.h

@@ -468,6 +468,8 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
 	SIM_PARAM_ENABLE_SAT = 8388608,
     SIM_PARAM_CONSTRAINT_SOLVER_TYPE =16777216,
+	SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 33554432,
+
 };
 
 enum EnumLoadSoftBodyUpdateFlags

examples/SharedMemory/SharedMemoryPublic.h

@@ -898,6 +898,7 @@ struct b3PhysicsSimulationParameters
 	double m_contactSlop;
 	int m_enableSAT;
     int m_constraintSolverType;
+	int m_minimumSolverIslandSize;
 };
 
 enum eConstraintSolverTypes

examples/SharedMemory/b3PluginManager.cpp

@@ -21,7 +21,11 @@
     
     typedef void*                   B3_DYNLIB_HANDLE;
 
+#ifdef __APPLE__
     #define B3_DYNLIB_OPEN(path)  dlopen(path, RTLD_NOW | RTLD_GLOBAL)
+#else
+    #define B3_DYNLIB_OPEN(path)  dlmopen(LM_ID_NEWLM, path, RTLD_LAZY)
+#endif
     #define B3_DYNLIB_CLOSE       dlclose
     #define B3_DYNLIB_IMPORT      dlsym
 #endif

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -1353,6 +1353,10 @@ bool b3RobotSimulatorClientAPI_NoDirect::changeDynamics(int bodyUniqueId, int li
 	b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
 	b3SharedMemoryStatusHandle statusHandle;
 
+	if (args.m_activationState >= 0)
+	{
+		b3ChangeDynamicsInfoSetActivationState(command, bodyUniqueId,args.m_activationState);
+	}
 	if (args.m_mass >= 0) {
 		b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass);
 	}
@@ -1714,6 +1718,15 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(const struct
 		b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold);
 	}
 
+	if (args.m_constraintSolverType >= 0)
+	{
+		b3PhysicsParameterSetConstraintSolverType(command, args.m_constraintSolverType);
+	}
+	if (args.m_minimumSolverIslandSize >= 0)
+	{
+		b3PhysicsParameterSetMinimumSolverIslandSize(command, args.m_minimumSolverIslandSize);
+	}
+
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	return true;
 }

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -268,7 +268,8 @@ struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameter
 		
 		m_frictionERP=-1;
 	  	m_solverResidualThreshold=-1;
-
+		m_constraintSolverType = -1;
+		m_minimumSolverIslandSize = -1;
 		
 	}
 };
@@ -285,6 +286,7 @@ struct b3RobotSimulatorChangeDynamicsArgs
 	double m_contactStiffness;
 	double m_contactDamping;
 	int m_frictionAnchor;
+	int m_activationState;
 
 	b3RobotSimulatorChangeDynamicsArgs()
 		: m_mass(-1),
@@ -296,7 +298,8 @@ struct b3RobotSimulatorChangeDynamicsArgs
 		m_angularDamping(-1),
 		m_contactStiffness(-1),
 		m_contactDamping(-1),
-		m_frictionAnchor(-1)
+		m_frictionAnchor(-1),
+		m_activationState(-1)
 	{}
 };
 

examples/SharedMemory/grpc/ConvertGRPCBullet.cpp

+#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

+
+#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
\ No newline at end of file

examples/SharedMemory/grpc/main.cpp

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

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

+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

+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

+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

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

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

+"""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_grpc.py

+# 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
\ No newline at end of file

examples/pybullet/examples/saveRestoreState.py

@@ -12,7 +12,7 @@ logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings
 
 def setupWorld():
 	p.resetSimulation()
-        p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
+	p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
 	p.loadURDF("planeMesh.urdf")
 	kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
 	for i in range (p.getNumJoints(kukaId)):

examples/pybullet/pybullet.c

@@ -1469,12 +1469,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
     int constraintSolverType=-1;
     double globalCFM = -1;
     
+	int minimumSolverIslandSize = -1;
+	
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "physicsClientId", NULL};
+	static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "minimumSolverIslandSize", "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
-									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
+									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1495,6 +1497,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 			b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
 		}
 
+		if (minimumSolverIslandSize >= 0)
+		{
+			b3PhysicsParameterSetMinimumSolverIslandSize(command, minimumSolverIslandSize);
+		}
+
+
 		if (solverResidualThreshold>=0)
 		{
 			b3PhysicsParamSetSolverResidualThreshold(command, solverResidualThreshold);

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -17,7 +17,7 @@ subject to the following restrictions:
 
 #include "btDbvtBroadphase.h"
 #include "LinearMath/btThreads.h"
-
+btScalar gDbvtMargin = btScalar(0.05);
 //
 // Profiling
 //
@@ -332,12 +332,9 @@ void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
 				if(delta[0]<0) velocity[0]=-velocity[0];
 				if(delta[1]<0) velocity[1]=-velocity[1];
 				if(delta[2]<0) velocity[2]=-velocity[2];
-				if	(
-#ifdef DBVT_BP_MARGIN				
-					m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN)
-#else
-					m_sets[0].update(proxy->leaf,aabb,velocity)
-#endif
+				if (
+					m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
+
 					)
 				{
 					++m_updates_done;

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -29,7 +29,8 @@ subject to the following restrictions:
 #define DBVT_BP_PREVENTFALSEUPDATE		0
 #define DBVT_BP_ACCURATESLEEPING		0
 #define DBVT_BP_ENABLE_BENCHMARK		0
-#define DBVT_BP_MARGIN					(btScalar)0.05
+//#define DBVT_BP_MARGIN					(btScalar)0.05
+extern btScalar gDbvtMargin;
 
 #if DBVT_BP_PROFILE
 #define	DBVT_BP_PROFILING_RATE	256

src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp

@@ -455,11 +455,20 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr
 	btBvhTriangleMeshShape* trimesh = 0;
 	
 	if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
-	   trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
-   else	   
-	   trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
-	   
-   	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
+	{
+		trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
+	}
+	else
+	{
+		if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
+		{
+			trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
+		}
+	}
+	if (trimesh==0)
+		return;
+
+	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
 	if (!triangleInfoMapPtr)
 		return;
 

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/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -1406,6 +1406,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 
 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {
+	//printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
 	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
 }
 
@@ -1656,6 +1657,8 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 
 void  btMultiBodyC