minor update for shared memory interface

examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp

@@ -129,7 +129,6 @@ void GwenParameterInterface::setSliderValue(int sliderIndex, double sliderValue)
 
 void GwenParameterInterface::registerButtonParameter(ButtonParams& params)
 {
-	Gwen::Controls::TextBox* label = new Gwen::Controls::TextBox(m_gwenInternalData->m_demoPage->GetPage());
 	
 	Gwen::Controls::Button* button = new Gwen::Controls::Button(m_gwenInternalData->m_demoPage->GetPage());
 	MyButtonEventHandler* handler = new MyButtonEventHandler(params.m_callback,params.m_buttonId,params.m_userPointer);
@@ -139,7 +138,9 @@ void GwenParameterInterface::registerButtonParameter(ButtonParams& params)
 	m_paramInternalData->m_buttons.push_back(button);
 	m_paramInternalData->m_buttonEventHandlers.push_back(handler);
 
-	button->SetPos( 10, m_gwenInternalData->m_curYposition );
+	button->SetPos( 5, m_gwenInternalData->m_curYposition );
+	button->SetWidth(120);
+	
 	m_gwenInternalData->m_curYposition+=22;
 
 }

examples/SharedMemory/PhysicsClient.cpp

@@ -59,7 +59,11 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
 		cl->submitCommand(CMD_LOAD_URDF);
 		break;
 	}
-
+	case CMD_REQUEST_ACTUAL_STATE:
+		{
+			cl->submitCommand(CMD_REQUEST_ACTUAL_STATE);
+			break;
+		}
 	case CMD_STEP_FORWARD_SIMULATION:
 		{
 			cl->submitCommand(CMD_STEP_FORWARD_SIMULATION);
@@ -129,6 +133,22 @@ void	PhysicsClient::initPhysics()
 		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
     }
 
+	{
+		bool isTrigger = false;
+		ButtonParams button("Get State",CMD_REQUEST_ACTUAL_STATE,  isTrigger);
+		button.m_callback = MyCallback;
+		button.m_userPointer = this;
+		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+    }
+	/*
+	{
+		bool isTrigger = false;
+		ButtonParams button("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
+		button.m_callback = MyCallback;
+		button.m_userPointer = this;
+		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+    }
+	*/
 	{
 		bool isTrigger = false;
 		ButtonParams button("Shut Down",CMD_SHUTDOWN,  isTrigger);
@@ -190,6 +210,33 @@ void	PhysicsClient::stepSimulation(float deltaTime)
 					m_serverLoadUrdfOK = false;
                     break;
                 }
+				case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
+					{
+						b3Printf("Received actual state");
+						
+						int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
+						int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
+						b3Printf("size Q = %d, size U = %d\n", numQ,numU);
+						char msg[1024];
+
+						sprintf(msg,"Q=[");
+						
+						for (int i=0;i<numQ;i++)
+						{
+							if (i<numQ-1)
+							{
+								sprintf(msg,"%s%f,",msg,m_testBlock1->m_actualStateQ[i]);
+							} else
+							{
+								sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]);
+							}
+						}
+						sprintf(msg,"%s]",msg);
+						
+						b3Printf(msg);
+						
+						break;
+					}
                 default:
                 {
                     b3Error("Unknown server command");
@@ -237,6 +284,20 @@ void	PhysicsClient::stepSimulation(float deltaTime)
 						}
 						break;
 					}
+				case CMD_REQUEST_ACTUAL_STATE:
+					{
+						if (m_serverLoadUrdfOK)
+						{
+							b3Printf("Requesting actual state");
+							m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
+							m_testBlock1->m_numClientCommands++;
+
+						} else
+						{
+							b3Warning("No URDF loaded");
+						}
+						break;
+					}
 				case CMD_STEP_FORWARD_SIMULATION:
 					{
 						if (m_serverLoadUrdfOK)

examples/SharedMemory/PhysicsServer.cpp

@@ -217,17 +217,73 @@ void	PhysicsServer::stepSimulation(float deltaTime)
                     m_testBlock1->m_numServerCommands++;
                     break;
                 }
-                case CMD_STEP_FORWARD_SIMULATION:
+				case CMD_REQUEST_ACTUAL_STATE:
 					{
-                   
+	                    b3Printf("Sending the actual state (Q,U)");
-                    b3Printf("Step simulation request");
+						if (m_dynamicsWorld->getNumMultibodies()>0)
-                    double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds;
+						{
-                    m_dynamicsWorld->stepSimulation(timeStep);
+							btMultiBody* mb = m_dynamicsWorld->getMultiBody(0);
-                    
 							SharedMemoryCommand& serverCmd = m_testBlock1->m_serverCommands[0];
+							serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
 
-                    serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
+							serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
-                    m_testBlock1->m_numServerCommands++;
+							int totalDegreeOfFreedomQ = 0;
+							int totalDegreeOfFreedomU = 0; 
+							
+							//always add the base, even for static (non-moving objects)
+							//so that we can easily move the 'fixed' base when needed
+							//do we don't use this conditional "if (!mb->hasFixedBase())"
+							{
+								btTransform tr;
+								tr.setOrigin(mb->getBasePos());
+								tr.setRotation(mb->getWorldToBaseRot().inverse());
+								
+								//base position in world space, carthesian
+								m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
+								m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
+								m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
+
+								//base orientation, quaternion x,y,z,w, in world space, carthesian
+								m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0]; 
+								m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1];
+								m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2];
+								m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3];
+								totalDegreeOfFreedomQ +=7;//pos + quaternion
+
+								//base linear velocity (in world space, carthesian)
+								m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0];
+								m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1];
+								m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2];
+	
+								//base angular velocity (in world space, carthesian)
+								m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0];
+								m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1];
+								m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2];
+								totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
+							}
+							for (int l=0;l<mb->getNumLinks();l++)
+							{
+								for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
+								{
+									m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
+								}
+								for (int d=0;d<mb->getLink(l).m_dofCount;d++)
+								{
+									m_testBlock1->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
+								}
+
+							}
+
+							serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
+							serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
+							
+							
+						} else
+						{
+							b3Warning("Request state but no multibody available");
+							//rigid bodies?
+						}
+/*
 
 											//now we send back the actual q, q' and force/torque and IMU sensor values
 					for (int i=0;i<m_jointFeedbacks.size();i++)
@@ -246,6 +302,25 @@ void	PhysicsServer::stepSimulation(float deltaTime)
 							m_jointFeedbacks[i]->m_appliedTorqueBodyB.y(),
 							m_jointFeedbacks[i]->m_appliedTorqueBodyB.z());
 					}
+					*/
+
+						m_testBlock1->m_numServerCommands++;
+
+						
+						break;
+					}
+                case CMD_STEP_FORWARD_SIMULATION:
+                {
+                   
+                    b3Printf("Step simulation request");
+                    double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds;
+                    m_dynamicsWorld->stepSimulation(timeStep);
+                    
+                    SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
+                    
+                    serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
+                    m_testBlock1->m_numServerCommands++;
+
                     break;
                 }
                 case CMD_SHUTDOWN:

examples/SharedMemory/SharedMemoryCommands.h

@@ -0,0 +1,76 @@
+
+#ifndef SHARED_MEMORY_COMMANDS_H
+#define SHARED_MEMORY_COMMANDS_H
+
+//this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc)
+
+enum SharedMemoryServerCommand{
+    CMD_URDF_LOADING_COMPLETED,
+    CMD_URDF_LOADING_FAILED,
+	CMD_ACTUAL_STATE_UPDATE_COMPLETED,
+//	CMD_DESIRED_STATE_RECEIVED_COMPLETED,
+    CMD_STEP_FORWARD_SIMULATION_COMPLETED,
+    CMD_MAX_SERVER_COMMANDS
+};
+
+enum SharedMemoryClientCommand{
+    CMD_LOAD_URDF,
+	CMD_REQUEST_ACTUAL_STATE,
+//	CMD_SEND_DESIRED_STATE,
+    CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE
+    CMD_SHUTDOWN,
+    CMD_MAX_CLIENT_COMMANDS
+};
+
+#define SHARED_MEMORY_SERVER_TEST_C
+#define MAX_DEGREE_OF_FREEDOM 1024
+#define MAX_NUM_SENSORS 1024
+#define MAX_URDF_FILENAME_LENGTH 1024
+
+
+struct UrdfArgs
+{
+    char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
+	bool m_useMultiBody;
+	bool m_useFixedBase;
+};
+
+struct SendDesiredStateArgs
+{
+	int m_bodyUniqueId;
+};
+
+struct RequestActualStateArgs
+{
+	int m_bodyUniqueId;
+};
+
+struct SendActualStateArgs
+{
+	int m_bodyUniqueId;
+	int m_numDegreeOfFreedomQ;
+	int m_numDegreeOfFreedomU;
+};
+
+
+struct StepSimulationArgs
+{
+    double m_deltaTimeInSeconds;
+};
+
+struct SharedMemoryCommand
+{
+    int m_type;
+    
+    union
+    {
+        UrdfArgs m_urdfArguments;
+        StepSimulationArgs m_stepSimulationArguments;
+		SendDesiredStateArgs m_sendDesiredQandUStateCommandArgument;
+		RequestActualStateArgs m_requestActualStateInformationCommandArgument;
+		SendActualStateArgs m_sendActualStateArgs;
+    };
+};
+
+
+#endif //SHARED_MEMORY_COMMANDS_H

examples/SharedMemory/SharedMemoryInterface.h

@@ -5,51 +5,7 @@
 #define SHARED_MEMORY_MAGIC_NUMBER 64738
 #define SHARED_MEMORY_MAX_COMMANDS 64
 
-enum SharedMemoryServerCommand{
+#include "SharedMemoryCommands.h"
-    CMD_URDF_LOADING_COMPLETED,
-    CMD_URDF_LOADING_FAILED,
-    
-    CMD_SERVER_STATE_UPDATE_COMPLETED,
-    CMD_STEP_FORWARD_SIMULATION_COMPLETED,
-    CMD_MAX_SERVER_COMMANDS
-};
-
-enum SharedMemoryClientCommand{
-    CMD_LOAD_URDF,
-    CMD_STATE_UPDATED,
-    CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE
-    CMD_SHUTDOWN,
-    CMD_MAX_CLIENT_COMMANDS
-};
-
-#define SHARED_MEMORY_SERVER_TEST_C
-#define MAX_DEGREE_OF_FREEDOM 1024
-#define MAX_NUM_SENSORS 1024
-#define MAX_URDF_FILENAME_LENGTH 1024
-
-
-struct UrdfCommandArgument
-{
-    char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
-	bool m_useMultiBody;
-	bool m_useFixedBase;
-};
-
-struct StepSimulationCommandArgument
-{
-    double m_deltaTimeInSeconds;
-};
-
-struct SharedMemoryCommand
-{
-    int m_type;
-    
-    union
-    {
-        UrdfCommandArgument m_urdfArguments;
-        StepSimulationCommandArgument m_stepSimulationArguments;
-    };
-};
 
 struct SharedMemoryExampleData
 {
@@ -63,18 +19,18 @@ struct SharedMemoryExampleData
     int m_numServerCommands;
     int m_numProcessedServerCommands;
   
+	//TODO: it is better to move this single desired/actual state (m_desiredStateQ, m_actualStateQ etc) into the command,
+	//so we can deal with multiple bodies/robots
 	
     //desired state is only written by the client, read-only access by server is expected
     double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
     double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
-    double m_desiredStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
     
     //actual state is only written by the server, read-only access by client is expected
     double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
     double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
     double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
         
-    
 };
 
 #define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData)