more GRPC work

examples/SharedMemory/grpc/ConvertGRPCBullet.h

@@ -10,7 +10,11 @@ namespace pybullet_grpc
 	class PyBulletStatus;
 };
 
-struct SharedMemoryCommand* convertGRPCAndSubmitCommand(pybullet_grpc::PyBulletCommand& grpcCommand, struct SharedMemoryCommand& cmd);
+struct SharedMemoryCommand* convertGRPCToBulletCommand(const pybullet_grpc::PyBulletCommand& grpcCommand, struct SharedMemoryCommand& cmd);
+
+pybullet_grpc::PyBulletCommand* convertBulletToGRPCCommand(const struct SharedMemoryCommand& clientCmd, pybullet_grpc::PyBulletCommand& grpcCommand);
+
+bool convertGRPCToStatus(const pybullet_grpc::PyBulletStatus& grpcReply, struct SharedMemoryStatus& serverStatus, char* bufferServerToClient, int bufferSizeInBytes);
 
 bool convertStatusToGRPC(const struct SharedMemoryStatus& serverStatus, char* bufferServerToClient, int bufferSizeInBytes, pybullet_grpc::PyBulletStatus& grpcReply);
 

examples/SharedMemory/grpc/main.cpp

@@ -53,12 +53,12 @@ public:
 	}
 
 	
-	void Run(MyCommandProcessor* comProc) {
-		std::string server_address("0.0.0.0:50051");
+	void Run(MyCommandProcessor* comProc, const std::string& hostNamePort) {
+		
 
 		ServerBuilder builder;
 		// Listen on the given address without any authentication mechanism.
-		builder.AddListeningPort(server_address, grpc::InsecureServerCredentials());
+		builder.AddListeningPort(hostNamePort, 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_);
@@ -67,7 +67,7 @@ public:
 		cq_ = builder.AddCompletionQueue();
 		// Finally assemble the server.
 		server_ = builder.BuildAndStart();
-		std::cout << "Server listening on " << server_address << std::endl;
+		std::cout << "Server listening on " << hostNamePort << std::endl;
 
 		// Proceed to the server's main loop.
 		HandleRpcs(comProc);
@@ -121,41 +121,49 @@ private:
 				
 				m_status.set_statustype(CMD_UNKNOWN_COMMAND_FLUSHED);
 				
-				cmdPtr = convertGRPCAndSubmitCommand(m_command, cmd);
-								
-
-				if (cmdPtr)
+				
+				if (m_command.has_checkversioncommand())
 				{
-					bool hasStatus = m_comProc->processCommand(*cmdPtr, serverStatus, &buffer[0], buffer.size());
-					double timeOutInSeconds = 10;
-					b3Clock clock;
-					double startTimeSeconds = clock.getTimeInSeconds();
-					double curTimeSeconds = clock.getTimeInSeconds();
+					m_status.set_statustype(CMD_CLIENT_COMMAND_COMPLETED);
+					m_status.mutable_checkversionstatus()->set_serverversion(SHARED_MEMORY_MAGIC_NUMBER);
+				}
+				else
+				{
+					cmdPtr = convertGRPCToBulletCommand(m_command, cmd);
 
-					while ((!hasStatus) && ((curTimeSeconds - startTimeSeconds) <timeOutInSeconds))
+
+					if (cmdPtr)
 					{
-						hasStatus = m_comProc->receiveStatus(serverStatus, &buffer[0], buffer.size());
-						curTimeSeconds = clock.getTimeInSeconds();
+						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 (gVerboseNetworkMessagesServer)
+
+					if (m_command.has_terminateservercommand())
 					{
-						//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);
+						status_ = TERMINATE;
 					}
 				}
-
-				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
@@ -245,6 +253,8 @@ int main(int argc, char** argv)
 
 	int port = 6667;
 	parseArgs.GetCmdLineArgument("port", port);
+	std::string hostName = "localhost";
+	std::string hostNamePort = hostName + ":" + std::to_string(port);
 
 	gVerboseNetworkMessagesServer = parseArgs.CheckCmdLineFlag("verbose");
 
@@ -263,7 +273,7 @@ int main(int argc, char** argv)
 	{
 		ServerImpl server;
 
-		server.Run(sm);
+		server.Run(sm, hostNamePort);
 	}
 	else
 	{

examples/SharedMemory/grpc/proto/pybullet.proto

@@ -24,6 +24,7 @@ message vec3
 	double z=3;
 };
 
+
 message quat4
 {
 	double x=1;
@@ -32,6 +33,38 @@ message quat4
 	double w=4;
 };
 
+message vec4
+{
+	double x=1;
+	double y=2;
+	double z=3;
+	double w=4;
+};
+
+
+message transform
+{
+	vec3 	origin=1;
+	quat4 orientation=2;
+};
+
+message matrix4x4
+{
+	//assume 16 elements, with translation in 12,13,14,
+	//'right' vector is elements 0,1,3 and 4
+	repeated double elems=1;
+};
+
+message CheckVersionCommand
+{
+	int32 clientVersion=1;	
+};
+
+message CheckVersionStatus
+{
+	int32 serverVersion=1;
+};
+
 
 message TerminateServerCommand
 {
@@ -42,6 +75,31 @@ message StepSimulationCommand
 {
 };
 
+message SyncBodiesCommand
+{
+
+};
+
+message SyncBodiesStatus
+{
+	repeated int32 bodyUniqueIds=1;
+	repeated int32 userConstraintUniqueIds=2;
+};
+
+
+message RequestBodyInfoCommand
+{
+	int32 bodyUniqueId=1;
+};
+
+message RequestBodyInfoStatus
+{
+	int32 bodyUniqueId=1;
+	string bodyName=2;
+};
+
+
+
 message LoadUrdfCommand {
 	string fileName=1;
 	vec3 initialPosition=2;
@@ -54,8 +112,11 @@ message LoadUrdfCommand {
 	oneof hasGlobalScaling {	double globalScaling=7;
 	}
 };
+
 message LoadUrdfStatus {
 	int32 bodyUniqueId=1;
+	string bodyName=2;
+	string fileName=3;
 }
 
 
@@ -133,10 +194,11 @@ message GetDynamicsStatus
 message InitPoseCommand
 {
 	int32 bodyUniqueId=1;
-	repeated int32 hasInitialStateQ=2;
-	repeated double initialStateQ=3;
-	repeated int32 hasInitialStateQdot=4;
-	repeated double initialStateQdot=5;
+	int32 updateflags=2;
+	repeated int32 hasInitialStateQ=3;
+	repeated double initialStateQ=4;
+	repeated int32 hasInitialStateQdot=5;
+	repeated double initialStateQdot=6;
 };
 
 
@@ -147,6 +209,7 @@ message RequestActualStateCommand
 	bool computeLinkVelocities=3;
 };
 
+
 message SendActualStateStatus
 {
 	int32 bodyUniqueId=1;
@@ -171,37 +234,230 @@ message SendActualStateStatus
 };
 
 
+message ConfigureOpenGLVisualizerCommand
+{
+		int32 updateFlags=1;
+		
+    double cameraDistance=2;
+    double cameraPitch=3;
+    double cameraYaw=4;
+    vec3 cameraTargetPosition=5;
+  
+    int32 setFlag=6;
+    int32 setEnabled=7;
+};
+
+
+message PhysicsSimulationParameters
+{
+	double deltaTime=1;
+	vec3 gravityAcceleration=2;
+	int32 numSimulationSubSteps=3;
+	int32 numSolverIterations=4;
+	int32 useRealTimeSimulation=5;
+	int32 useSplitImpulse=6;
+	double splitImpulsePenetrationThreshold=7;
+	double contactBreakingThreshold=8;
+	int32 internalSimFlags=9;
+	double defaultContactERP=10;
+	int32 collisionFilterMode=11;
+	int32 enableFileCaching=12;
+	double restitutionVelocityThreshold=13;
+	double defaultNonContactERP=14;
+	double frictionERP=15;
+	double defaultGlobalCFM=16;
+	double frictionCFM=17;
+	int32 enableConeFriction=18;
+	int32 deterministicOverlappingPairs=19;
+	double allowedCcdPenetration=20;
+	int32 jointFeedbackMode=21;
+	double solverResidualThreshold=22;
+	double contactSlop=23;
+	int32 enableSAT=24;
+  int32 constraintSolverType=25;
+	int32 minimumSolverIslandSize=26;
+};
+
+
+message PhysicsSimulationParametersCommand
+{
+	int32 updateFlags=1;
+	PhysicsSimulationParameters params=2;
+};
+
+
+
+message JointMotorControlCommand
+{
+	int32 bodyUniqueId=1;
+	int32 controlMode=2;
+	int32 updateFlags=3;
+	
+	//PD parameters in case controlMode == CONTROL_MODE_POSITION_VELOCITY_PD
+	repeated double Kp=4;//indexed by degree of freedom, 6 for base, and then the dofs for each link
+	repeated double Kd=5;//indexed by degree of freedom, 6 for base, and then the dofs for each link
+	repeated double maxVelocity=6;
+
+  repeated int32 hasDesiredStateFlags=7;
+    
+	//desired state is only written by the client, read-only access by server is expected
+
+	//desiredStateQ is indexed by position variables, 
+	//starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables
+  repeated double desiredStateQ=8;
+    
+	//desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables
+  repeated double desiredStateQdot=9;
+	
+	//desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or
+	//or the maximum applied force/torque for the PD/motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY and CONTROL_MODE_POSITION_VELOCITY_PD mode
+	//indexed by degree of freedom, 6 dof base, and then dofs for each link
+	repeated double desiredStateForceTorque=10;
+};
+
+
+message UserConstraintCommand
+{
+	int32 parentBodyIndex=1;
+	int32 parentJointIndex=2;
+	int32 childBodyIndex=3;
+	int32 childJointIndex=4;
+	transform parentFrame=5;
+	transform childFrame=6;
+	vec3 jointAxis=7;
+	int32 jointType=8;
+	double maxAppliedForce=9;
+	int32 userConstraintUniqueId=10;
+	double gearRatio=11;
+	int32 gearAuxLink=12;
+	double relativePositionTarget=13;
+	double erp=14;
+	int32 updateFlags=15;
+};
+
+message UserConstraintStatus
+{
+	double maxAppliedForce=9;
+	int32 userConstraintUniqueId=10;
+};
+
+message UserConstraintStateStatus
+{
+	vec3 appliedConstraintForcesLinear=1;
+	vec3 appliedConstraintForcesAngular=2;
+	int32 numDofs=3;
+};
+
+
+message RequestKeyboardEventsCommand
+{
+
+};
+
+message KeyboardEvent
+{
+	int32 keyCode=1;//ascii
+	int32 keyState=2;// see b3VRButtonInfo
+};
+
+
+message KeyboardEventsStatus
+{
+	repeated KeyboardEvent keyboardEvents=1;
+};
+
+
+message RequestCameraImageCommand
+{
+	int32 updateFlags=1;
+	int32 cameraFlags=2;
+	matrix4x4 viewMatrix=3;
+	matrix4x4 projectionMatrix=4;
+	int32 startPixelIndex=5;
+	int32 pixelWidth=6;
+	int32 pixelHeight=7;
+	vec3 lightDirection=8;
+	vec3 lightColor=9;
+	double lightDistance=10;
+	double lightAmbientCoeff=11;
+	double lightDiffuseCoeff=12;
+	double lightSpecularCoeff=13;
+	int32 hasShadow=14;
+	matrix4x4 projectiveTextureViewMatrix=15;
+	matrix4x4 projectiveTextureProjectionMatrix=16;
+};
+
+message RequestCameraImageStatus
+{
+	int32 imageWidth=1;
+	int32 imageHeight=2;
+
+	int32 startingPixelIndex=3;
+	int32 numPixelsCopied=4;
+	int32 numRemainingPixels=5;
+};
+
 
 // The request message containing the command
 message PyBulletCommand {
   int32 commandType=1;
   
+  repeated bytes binaryBlob=2;
+  
+  repeated bytes unknownCommandBinaryBlob=3;
+    
   oneof commands {
-    LoadUrdfCommand loadUrdfCommand = 3;
-    TerminateServerCommand terminateServerCommand=4;
-		StepSimulationCommand stepSimulationCommand= 5;
-		LoadSdfCommand loadSdfCommand=6;
-		LoadMjcfCommand loadMjcfCommand=7;
-		ChangeDynamicsCommand changeDynamicsCommand=8;
-		GetDynamicsCommand getDynamicsCommand=9;
-		InitPoseCommand initPoseCommand=10;
-		RequestActualStateCommand requestActualStateCommand=11;
+  	
+    LoadUrdfCommand loadUrdfCommand = 4;
+    TerminateServerCommand terminateServerCommand=5;
+		StepSimulationCommand stepSimulationCommand= 6;
+		LoadSdfCommand loadSdfCommand=7;
+		LoadMjcfCommand loadMjcfCommand=8;
+		ChangeDynamicsCommand changeDynamicsCommand=9;
+		GetDynamicsCommand getDynamicsCommand=10;
+		InitPoseCommand initPoseCommand=11;
+		RequestActualStateCommand requestActualStateCommand=12;
+		ConfigureOpenGLVisualizerCommand configureOpenGLVisualizerCommand =13;
+		SyncBodiesCommand syncBodiesCommand=14;
+		RequestBodyInfoCommand requestBodyInfoCommand=15;
+		PhysicsSimulationParametersCommand setPhysicsSimulationParametersCommand=16;
+		JointMotorControlCommand jointMotorControlCommand=17;
+		UserConstraintCommand userConstraintCommand=18;
+		CheckVersionCommand checkVersionCommand=19;
+		RequestKeyboardEventsCommand requestKeyboardEventsCommand=20;
+		RequestCameraImageCommand requestCameraImageCommand=21;
   }
   
+  
 }
 
 // The response message containing the status
 message PyBulletStatus {
   int32 statusType=1;
   
+  repeated bytes binaryBlob=2;
+  
+  repeated bytes unknownStatusBinaryBlob=3;
+  
   oneof status
   {
-  	LoadUrdfStatus urdfStatus = 2;
-  	SdfLoadedStatus sdfStatus = 3;
-  	MjcfLoadedStatus mjcfStatus = 4;
-  	GetDynamicsStatus getDynamicsStatus = 5;
-  	SendActualStateStatus actualStateStatus = 6;
+  	
+  	LoadUrdfStatus urdfStatus = 4;
+  	SdfLoadedStatus sdfStatus = 5;
+  	MjcfLoadedStatus mjcfStatus = 6;
+  	GetDynamicsStatus getDynamicsStatus = 7;
+  	SendActualStateStatus actualStateStatus = 8;
+  	SyncBodiesStatus syncBodiesStatus=9;
+  	RequestBodyInfoStatus requestBodyInfoStatus = 10;
+  	PhysicsSimulationParameters requestPhysicsSimulationParametersStatus=11;
+  	CheckVersionStatus checkVersionStatus=12;
+  	UserConstraintStatus userConstraintStatus=13;
+		UserConstraintStateStatus userConstraintStateStatus=14;
+		KeyboardEventsStatus keyboardEventsStatus=15;
+		RequestCameraImageStatus requestCameraImageStatus=16;
   }
+  
+  
 }
 
 

examples/SharedMemory/grpc/pybullet_client.py

@@ -12,11 +12,23 @@ MJCF_COLORS_FROM_FILE = 512
 
 def run():
 	print("grpc.insecure_channel")
-	channel = grpc.insecure_channel('localhost:50051')
+	channel = grpc.insecure_channel('localhost:6667')
 	print("pybullet_pb2_grpc.PyBulletAPIStub")
 	stub = pybullet_pb2_grpc.PyBulletAPIStub(channel)
 	response=0
 
+	print("submit CheckVersionCommand")
+	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(checkVersionCommand=pybullet_pb2.CheckVersionCommand(clientVersion=123)))
+	print("PyBullet client received: " , response)
+
+
+	print("submit LoadUrdfCommand ")
+	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(fileName="door.urdf", initialPosition=pybullet_pb2.vec3(x=0,y=0,z=0), useMultiBody=True, useFixedBase=True, globalScaling=2, flags = 1)))
+	print("PyBullet client received: " , response)
+	bodyUniqueId = response.urdfStatus.bodyUniqueId
+	
+	
+
 	print("submit LoadSdfCommand")
 	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadSdfCommand=pybullet_pb2.LoadSdfCommand(fileName="two_cubes.sdf", useMultiBody=True, globalScaling=2)))
 	print("PyBullet client received: " , response)
@@ -27,10 +39,6 @@ def run():
 	print("PyBullet client received: " , response)
 
 	
-	print("submit LoadUrdfCommand ")
-	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(loadUrdfCommand=pybullet_pb2.LoadUrdfCommand(fileName="door.urdf", initialPosition=pybullet_pb2.vec3(x=0,y=0,z=0), useMultiBody=True, useFixedBase=True, globalScaling=2, flags = 1)))
-	print("PyBullet client received: " , response)
-	bodyUniqueId = response.urdfStatus.bodyUniqueId
 	
 	print("submit ChangeDynamicsCommand ")
 	response = stub.SubmitCommand(pybullet_pb2.PyBulletCommand(changeDynamicsCommand=pybullet_pb2.ChangeDynamicsCommand(bodyUniqueId=bodyUniqueId, linkIndex=-1, mass=10)))