Merge pull request #428 from erwincoumans/master

add very simple (rudimentary) time series graphing example

build3/premake4.lua

@@ -66,6 +66,13 @@
 		description = "Enable Lua scipting support in Example Browser"
 	}
 
+	 newoption {
+    trigger     = "targetdir",
+    value       = "path such as ../bin",
+    description = "Set the output location for the generated project files"
+  }
+	
+	
 	newoption
 	{
 		trigger = "without-gtest",
@@ -129,7 +136,9 @@
 -- comment-out for now, URDF reader needs exceptions
 --	flags { "NoRTTI", "NoExceptions"}
 --	defines { "_HAS_EXCEPTIONS=0" }
-	targetdir "../bin"
+--printf ( _OPTIONS["targetdir"] )
+
+	targetdir( _OPTIONS["targetdir"] or "../bin" )
 	location("./" .. act .. postfix)
 
 

build3/vs2010.bat

@@ -1,7 +1,18 @@
 
 rem premake4 --with-pe  vs2010
 rem premake4  --bullet2demos vs2010
-premake4   vs2010
+premake4   --targetdir="../bin" vs2010 
+rem premake4   --targetdir="../server2bin" vs2010 
+rem cd vs2010
+rem rename 0_Bullet3Solution.sln 0_server.sln
+rem cd ..
+rem rename vs2010 vs2010_server
+rem 
+rem premake4   --targetdir="../client2bin" vs2010 
+rem cd vs2010
+rem rename 0_Bullet3Solution.sln 0_client.sln
+rem cd ..
+rem rename vs2010 vs2010_client
+
 
-mkdir vs2010\cache
 pause

data/hinge.urdf

@@ -0,0 +1,49 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+  <link name="baseLink">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="0.0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+      <box size="0.1 .1 .1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 <box size="0.1 .1 .1"/>
+      </geometry>
+    </collision>
+  </link>
+   <link name="childA">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="10.0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.1 .1 .1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.1 .1 .1"/>
+      </geometry>
+    </collision>
+  </link>
+   <joint name="joint_baseLink_childA" type="continuous">
+    <parent link="baseLink"/>
+    <child link="childA"/>
+    <origin xyz="0 0 1.0"/>
+	<axis xyz="0 0 1"/>
+  </joint>
+ 
+</robot>
+

examples/CommonInterfaces/Common2dCanvasInterface.h

@@ -7,6 +7,8 @@ struct Common2dCanvasInterface
 	virtual int createCanvas(const char* canvasName, int width, int height)=0;
 	virtual void destroyCanvas(int canvasId)=0;
 	virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)=0;
+	virtual void getPixel(int canvasId, int x, int y, unsigned char& red, unsigned char& green,unsigned char& blue, unsigned char& alpha)=0;
+
 	virtual void refreshImageData(int canvasId)=0;
 
 };

examples/CommonInterfaces/CommonGraphicsAppInterface.h

@@ -187,7 +187,7 @@ struct CommonGraphicsApp
 				float cameraDistance = 	camera->getCameraDistance();
 				if (deltay<0 || cameraDistance>1)
 				{
-					cameraDistance -= deltay*0.1f;
+					cameraDistance -= deltay*0.01f;
 					if (cameraDistance<1)
 						cameraDistance=1;
 					camera->setCameraDistance(cameraDistance);

examples/ExampleBrowser/CMakeLists.txt

@@ -19,6 +19,7 @@ SET(App_ExampleBrowser_SRCS
 	../SharedMemory/PhysicsClient.cpp
 	../SharedMemory/PhysicsServerExample.cpp
 	../SharedMemory/PhysicsClientExample.cpp
+	../SharedMemory/RobotControlExample.cpp
 	../SharedMemory/PosixSharedMemory.cpp
 	../SharedMemory/Win32SharedMemory.cpp
 	../BasicDemo/BasicExample.cpp
@@ -45,6 +46,7 @@ SET(App_ExampleBrowser_SRCS
 	../RenderingExamples/RaytracerSetup.h
 	../RenderingExamples/RenderInstancingDemo.cpp
 	../RenderingExamples/RenderInstancingDemo.h
+	../RenderingExamples/TimeSeriesExample.cpp
 	../Benchmarks/BenchmarkDemo.cpp
 	../Benchmarks/BenchmarkDemo.h
 	../Benchmarks/landscapeData.h

examples/ExampleBrowser/ExampleEntries.cpp

@@ -32,8 +32,10 @@
 #include "../DynamicControlDemo/MotorDemo.h"
 #include "../RollingFrictionDemo/RollingFrictionDemo.h"
 #include "../SharedMemory/PhysicsServerExample.h"
+#include "../SharedMemory/RobotControlExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
 #include "../Constraints/TestHingeTorque.h"
+#include "../RenderingExamples/TimeSeriesExample.h"
 
 
 #ifdef ENABLE_LUA
@@ -185,6 +187,10 @@ static ExampleEntry gDefaultExamples[]=
 	
 
 	ExampleEntry(0,"Experiments"),
+	
+	ExampleEntry(1,"Robot Control", "Perform some robot control tasks, using physics server and client that communicate over shared memory",
+			RobotControlExampleCreateFunc),
+	
 	ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
 			PhysicsServerCreateFunc),
 	ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
@@ -204,6 +210,7 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(0,"Rendering"),
 	ExampleEntry(1,"Instanced Rendering", "Simple example of fast instanced rendering, only active when using OpenGL3+.",RenderInstancingCreateFunc),
 	ExampleEntry(1,"CoordinateSystemDemo","Show the axis and positive rotation direction around the axis.", CoordinateSystemCreateFunc),
+	ExampleEntry(1,"Time Series", "Render some value(s) in a 2D graph window, shifting to the left", TimeSeriesCreateFunc)
 	
 };
 

examples/ExampleBrowser/GwenGUISupport/GraphingTexture.h

@@ -24,6 +24,13 @@ struct GraphingTexture
 		m_imageData[x*4+y*4*m_width+3] = alpha;
 	}
 	
+	void getPixel(int x, int y, unsigned char& red, unsigned char& green, unsigned char& blue, unsigned char& alpha)
+	{
+		red = m_imageData[x*4+y*4*m_width+0];
+		green = m_imageData[x*4+y*4*m_width+1];
+		blue = m_imageData[x*4+y*4*m_width+2];
+		alpha = m_imageData[x*4+y*4*m_width+3];
+	}
 	void uploadImageData();
 	
 	int getTextureId()

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -578,6 +578,14 @@ struct QuickCanvas : public Common2dCanvasInterface
 		btAssert(m_curNumGraphWindows==1);
 		m_gt[canvasId]->setPixel(x,y,red,green,blue,alpha);
 	}
+	
+	virtual void getPixel(int canvasId, int x, int y, unsigned char& red, unsigned char& green,unsigned char& blue, unsigned char& alpha)
+	{
+		btAssert(canvasId==0);//hardcoded
+		btAssert(m_curNumGraphWindows==1);
+		m_gt[canvasId]->getPixel(x,y,red,green,blue,alpha);
+	}
+	
 	virtual void refreshImageData(int canvasId)
 	{
 		m_gt[canvasId]->uploadImageData();

examples/ExampleBrowser/premake4.lua

@@ -52,6 +52,7 @@
 		"**.h",
 		"../SharedMemory/PhysicsServerExample.cpp",
 		"../SharedMemory/PhysicsClientExample.cpp",
+		"../SharedMemory/RobotControlExample.cpp",
 		"../SharedMemory/PhysicsServer.cpp",
 		"../SharedMemory/PhysicsClient.cpp",
 		"../SharedMemory/PosixSharedMemory.cpp",

examples/OpenGLWindow/SimpleCamera.cpp

@@ -12,8 +12,8 @@ struct SimpleCameraInternalData
 		m_cameraUp(b3MakeVector3(0,1,0)),
 		m_cameraUpAxis(1),
 		m_cameraForward(b3MakeVector3(1,0,0)),
-		m_frustumZNear(1),
+		m_frustumZNear(0.01),
-		m_frustumZFar(10000),
+		m_frustumZFar(1000),
 		m_yaw(20),
 		m_pitch(0),
 		m_aspect(1)

examples/RenderingExamples/TimeSeriesExample.h

@@ -0,0 +1,8 @@
+#ifndef TIME_SERIES_EXAMPLE_H
+#define TIME_SERIES_EXAMPLE_H
+
+class CommonExampleInterface*    TimeSeriesCreateFunc(struct CommonExampleOptions& options);
+
+
+
+#endif//TIME_SERIES_EXAMPLE_H

examples/SharedMemory/PhysicsClient.cpp

@@ -205,9 +205,10 @@ void	PhysicsClientSharedMemory::processServerStatus()
 			case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
 				{
 					b3Printf("Received actual state\n");
+					SharedMemoryCommand& command = m_data->m_testBlock1->m_serverCommands[0];
 
-					int numQ = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
+					int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
-					int numU = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
+					int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU;
 					b3Printf("size Q = %d, size U = %d\n", numQ,numU);
 					char msg[1024];
 
@@ -217,10 +218,10 @@ void	PhysicsClientSharedMemory::processServerStatus()
 					{
 						if (i<numQ-1)
 						{
-							sprintf(msg,"%s%f,",msg,m_data->m_testBlock1->m_actualStateQ[i]);
+							sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
 						} else
 						{
-							sprintf(msg,"%s%f",msg,m_data->m_testBlock1->m_actualStateQ[i]);
+							sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
 						}
 					}
 					sprintf(msg,"%s]",msg);
@@ -271,17 +272,7 @@ bool	PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
 					{
 						if (!m_data->m_serverLoadUrdfOK)
 						{
-							m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
+							m_data->m_testBlock1->m_clientCommands[0] = command;
-							sprintf(m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[0] = 0.0;
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0;
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0;
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0;
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0;
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0;
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0;
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
-							m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
 							
 							m_data->m_testBlock1->m_numClientCommands++;
 							b3Printf("Client created CMD_LOAD_URDF\n");
@@ -371,37 +362,7 @@ bool	PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
                         if (m_data->m_serverLoadUrdfOK)
 						{
 							b3Printf("Sending desired state (pos, vel, torque)\n");
-							m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE;
+							m_data->m_testBlock1->m_clientCommands[0] = command;
-							//todo: expose a drop box in the GUI for this
-							int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
-
-							switch (controlMode)
-							{
-							case CONTROL_MODE_VELOCITY:
-								{
-									m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
-									for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
-									{
-										m_data->m_testBlock1->m_desiredStateQdot[i] = 1;
-										m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
-									}
-									break;
-								}
-							case CONTROL_MODE_TORQUE:
-								{
-									m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE;
-									for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
-									{
-										m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
-									}
-									break;
-								}
-							default:
-								{
-									b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
-									btAssert(0);
-								}
-							}
 							
 							m_data->m_testBlock1->m_numClientCommands++;
 

examples/SharedMemory/PhysicsClientExample.cpp

@@ -16,7 +16,7 @@ protected:
 
 	
 	bool m_wantsTermination;
-    btAlignedObjectArray<int> m_userCommandRequests;
+    btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
 
 	void	createButton(const char* name, int id, bool isTrigger );
 
@@ -42,7 +42,7 @@ public:
     {
         return m_wantsTermination;
     }
-	void enqueueCommand(int command);
+	void enqueueCommand(const SharedMemoryCommand& orgCommand);
 	
 };
 
@@ -50,17 +50,85 @@ public:
 void MyCallback(int buttonId, bool buttonState, void* userPtr)
 {
 	PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
+
+	SharedMemoryCommand command;
+
 	switch (buttonId)
 	{
 	case  CMD_LOAD_URDF:
+		{
+			command.m_type =CMD_LOAD_URDF;
+			sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf");
+			command.m_urdfArguments.m_initialPosition[0] = 0.0;
+			command.m_urdfArguments.m_initialPosition[1] = 0.0;
+			command.m_urdfArguments.m_initialPosition[2] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[0] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[1] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[2] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[3] = 1.0;
+			command.m_urdfArguments.m_useFixedBase = false;
+			command.m_urdfArguments.m_useMultiBody = true;
+			cl->enqueueCommand(command);
+			break;
+		}
 	case CMD_CREATE_BOX_COLLISION_SHAPE:
+		{
+			command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
+			cl->enqueueCommand(command);
+			break;
+		}
 	case CMD_REQUEST_ACTUAL_STATE:
+		{
+			command.m_type =CMD_REQUEST_ACTUAL_STATE;
+			cl->enqueueCommand(command);
+			break;
+		};
 	case CMD_STEP_FORWARD_SIMULATION:
-	case CMD_SHUTDOWN:
+		{
+			command.m_type =CMD_STEP_FORWARD_SIMULATION;
+			cl->enqueueCommand(command);
+			break;
+		}
+	
     case CMD_SEND_DESIRED_STATE:
+		{
+				command.m_type =CMD_SEND_DESIRED_STATE;
+				int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
+
+				command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
+				//todo: expose a drop box in the GUI for this
+				switch (controlMode)
+				{
+				case CONTROL_MODE_VELOCITY:
+					{
+						for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+						{
+							command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 1;
+							command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
+						}
+						break;
+					}
+				case CONTROL_MODE_TORQUE:
+					{
+						for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+						{
+							command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
+						}
+						break;
+					}
+				default:
+					{
+						b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
+						btAssert(0);
+					}
+				}
+			cl->enqueueCommand(command);		
+			break;
+		}
 	case CMD_SEND_BULLET_DATA_STREAM:
 		{
-			cl->enqueueCommand(buttonId);
+			command.m_type = buttonId;
+			cl->enqueueCommand(command);
 			break;
 		}
 
@@ -72,12 +140,13 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
 	};
 }
 
+ void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand)
+	{
+		m_userCommandRequests.push_back(orgCommand);
+		SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
 
-void PhysicsClientExample::enqueueCommand(int command)
+		b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
-{
+	}
-	m_userCommandRequests.push_back(command);
-	b3Printf("User put command request %d on queue (queue length = %d)\n",command, m_userCommandRequests.size());
-}
 
 
 PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
@@ -114,6 +183,7 @@ void	PhysicsClientExample::initPhysics()
 		
 	} else
 	{
+		/*
 		m_userCommandRequests.push_back(CMD_LOAD_URDF);
 		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
 		m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
@@ -124,6 +194,7 @@ void	PhysicsClientExample::initPhysics()
 		m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
 		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
 		m_userCommandRequests.push_back(CMD_SHUTDOWN);
+		*/
 
 	}
 
@@ -147,7 +218,7 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 			if (m_userCommandRequests.size())
 			{
 				b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
-				int command = m_userCommandRequests[0];
+				SharedMemoryCommand command = m_userCommandRequests[0];
 
 				//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
 				for (int i=1;i<m_userCommandRequests.size();i++)
@@ -156,9 +227,8 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 				}
 
 				m_userCommandRequests.pop_back();
-				SharedMemoryCommand tmp;
+				
-				tmp.m_type = command;
+				m_physicsClient.submitClientCommand(command);
-				m_physicsClient.submitClientCommand(tmp);
 			}
 		}
 	}

examples/SharedMemory/PhysicsServer.cpp

@@ -385,12 +385,12 @@ void PhysicsServerSharedMemory::processClientCommands()
                                         mb->clearForcesAndTorques();
                                         
                                         int torqueIndex = 0;
-                                        btVector3 f(m_data->m_testBlock1->m_desiredStateForceTorque[0],
+										btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
-                                                    m_data->m_testBlock1->m_desiredStateForceTorque[1],
+                                                    clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
-                                                    m_data->m_testBlock1->m_desiredStateForceTorque[2]);
+                                                    clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
-                                        btVector3 t(m_data->m_testBlock1->m_desiredStateForceTorque[3],
+                                        btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
-                                                    m_data->m_testBlock1->m_desiredStateForceTorque[4],
+                                                    clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
-                                                    m_data->m_testBlock1->m_desiredStateForceTorque[5]);
+                                                    clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
                                         torqueIndex+=6;
                                         mb->addBaseForce(f);
                                         mb->addBaseTorque(t);
@@ -399,7 +399,7 @@ void PhysicsServerSharedMemory::processClientCommands()
                                             
                                             for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
                                             {               
-                                                double torque = m_data->m_testBlock1->m_desiredStateForceTorque[torqueIndex];
+                                                double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
                                                 mb->addJointTorqueMultiDof(link,dof,torque);
                                                 torqueIndex++;
                                             }
@@ -423,12 +423,12 @@ void PhysicsServerSharedMemory::processClientCommands()
 													if (motorPtr)
 													{
 														btMultiBodyJointMotor* motor = *motorPtr;
-														btScalar desiredVelocity = m_data->m_testBlock1->m_desiredStateQdot[dofIndex];
+														btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
 														motor->setVelocityTarget(desiredVelocity);
 
 														btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation'
 
-														btScalar maxImp = m_data->m_testBlock1->m_desiredStateForceTorque[dofIndex]*physicsDeltaTime;
+														btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*physicsDeltaTime;
 														motor->setMaxAppliedImpulse(maxImp);
 														numMotors++;
 
@@ -478,37 +478,37 @@ void PhysicsServerSharedMemory::processClientCommands()
 								tr.setRotation(mb->getWorldToBaseRot().inverse());
 								
 								//base position in world space, carthesian
-								m_data->m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
+								serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
-								m_data->m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
+								serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
-								m_data->m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
+								serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
 
 								//base orientation, quaternion x,y,z,w, in world space, carthesian
-								m_data->m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0]; 
+								serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; 
-								m_data->m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1];
+								serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
-								m_data->m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2];
+								serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
-								m_data->m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3];
+								serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
 								totalDegreeOfFreedomQ +=7;//pos + quaternion
 
 								//base linear velocity (in world space, carthesian)
-								m_data->m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0];
+								serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0];
-								m_data->m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1];
+								serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1];
-								m_data->m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2];
+								serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2];
 	
 								//base angular velocity (in world space, carthesian)
-								m_data->m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0];
+								serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0];
-								m_data->m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1];
+								serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1];
-								m_data->m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2];
+								serverCmd.m_sendActualStateArgs.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_data->m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
+									serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
 								}
 								for (int d=0;d<mb->getLink(l).m_dofCount;d++)
 								{
-									m_data->m_testBlock1->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
+									serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
 								}
 
 							}

examples/SharedMemory/PhysicsServerExample.cpp

@@ -39,8 +39,6 @@ public:
 	virtual void	stepSimulation(float deltaTime);
     
     
-    bool loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
-                  bool useMultiBody, bool useFixedBase);
     
 	virtual void resetCamera()
 	{
@@ -59,7 +57,7 @@ PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
 m_wantsShutdown(false)
 {
 	b3Printf("Started PhysicsServer\n");
-	bool useServer = true;
+	
 }
 
 

examples/SharedMemory/RobotControlExample.cpp

@@ -0,0 +1,274 @@
+
+
+
+#include "RobotControlExample.h"
+
+
+
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "PhysicsServer.h"
+#include "PhysicsClient.h"
+#include "SharedMemoryCommon.h"
+#include "../Utils/b3Clock.h"
+
+//const char* blaatnaam = "basename";
+
+
+
+
+class RobotControlExample : public SharedMemoryCommon
+{
+	PhysicsServerSharedMemory	m_physicsServer;
+	PhysicsClientSharedMemory	m_physicsClient;
+	b3Clock	m_realtimeClock;
+
+	int	m_sequenceNumberGenerator;
+	bool m_wantsShutdown;
+	   
+	btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
+
+	void	createButton(const char* name, int id, bool isTrigger );
+	
+public:
+    
+	RobotControlExample(GUIHelperInterface* helper);
+    
+	virtual ~RobotControlExample();
+    
+	virtual void	initPhysics();
+    
+	virtual void	stepSimulation(float deltaTime);
+    
+    void enqueueCommand(const SharedMemoryCommand& orgCommand)
+	{
+		m_userCommandRequests.push_back(orgCommand);
+		SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
+		cmd.m_sequenceNumber = m_sequenceNumberGenerator++;
+		cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds();
+
+		b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
+	}
+  
+	virtual void resetCamera()
+	{
+		float dist = 5;
+		float pitch = 50;
+		float yaw = 35;
+		float targetPos[3]={0,0,0};//-3,2.8,-2.5};
+		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+	}
+    
+    virtual bool wantsTermination();
+};
+
+
+void MyCallback2(int buttonId, bool buttonState, void* userPtr)
+{
+	RobotControlExample* cl = (RobotControlExample*) userPtr;
+
+	SharedMemoryCommand command;
+
+	switch (buttonId)
+	{
+	case  CMD_LOAD_URDF:
+		{
+			command.m_type =CMD_LOAD_URDF;
+			sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf");
+			command.m_urdfArguments.m_initialPosition[0] = 0.0;
+			command.m_urdfArguments.m_initialPosition[1] = 0.0;
+			command.m_urdfArguments.m_initialPosition[2] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[0] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[1] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[2] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[3] = 1.0;
+			command.m_urdfArguments.m_useFixedBase = false;
+			command.m_urdfArguments.m_useMultiBody = true;
+			cl->enqueueCommand(command);
+			break;
+		}
+	case CMD_CREATE_BOX_COLLISION_SHAPE:
+		{
+			command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
+			cl->enqueueCommand(command);
+			break;
+		}
+	case CMD_REQUEST_ACTUAL_STATE:
+		{
+			command.m_type =CMD_REQUEST_ACTUAL_STATE;
+			cl->enqueueCommand(command);
+			break;
+		};
+	case CMD_STEP_FORWARD_SIMULATION:
+		{
+			command.m_type =CMD_STEP_FORWARD_SIMULATION;
+			cl->enqueueCommand(command);
+			break;
+		}
+	
+    case CMD_SEND_DESIRED_STATE:
+		{
+				command.m_type =CMD_SEND_DESIRED_STATE;
+				int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
+
+				command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
+				//todo: expose a drop box in the GUI for this
+				switch (controlMode)
+				{
+				case CONTROL_MODE_VELOCITY:
+					{
+						for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+						{
+							command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 1;
+							command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
+						}
+						break;
+					}
+				case CONTROL_MODE_TORQUE:
+					{
+						for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+						{
+							command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
+						}
+						break;
+					}
+				default:
+					{
+						b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
+						btAssert(0);
+					}
+				}
+			cl->enqueueCommand(command);		
+			break;
+		}
+	case CMD_SEND_BULLET_DATA_STREAM:
+		{
+			command.m_type = buttonId;
+			cl->enqueueCommand(command);
+			break;
+		}
+
+	default:
+		{
+			b3Error("Unknown buttonId");
+			btAssert(0);
+		}
+	};
+}
+void	RobotControlExample::createButton(const char* name, int buttonId, bool isTrigger )
+{
+	ButtonParams button(name,buttonId,  isTrigger);
+	button.m_callback = MyCallback2;
+	button.m_userPointer = this;
+	m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+}
+
+RobotControlExample::RobotControlExample(GUIHelperInterface* helper)
+:SharedMemoryCommon(helper),
+m_wantsShutdown(false),
+m_sequenceNumberGenerator(0)
+{
+	
+	bool useServer = true;
+}
+
+
+
+RobotControlExample::~RobotControlExample()
+{
+	bool deInitializeSharedMemory = true;
+	m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
+}
+
+void	RobotControlExample::initPhysics()
+{
+	///for this testing we use Z-axis up
+	int upAxis = 2;
+	m_guiHelper->setUpAxis(upAxis);
+
+    createEmptyDynamicsWorld();
+	//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	btVector3 grav(0,0,0);
+	grav[upAxis] = 0;//-9.8;
+	this->m_dynamicsWorld->setGravity(grav);
+    
+	bool allowSharedMemoryInitialization = true;
+	m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
+  
+	if (m_guiHelper && m_guiHelper->getParameterInterface())
+	{
+		bool isTrigger = false;
+		
+		createButton("Load URDF",CMD_LOAD_URDF,  isTrigger);
+		createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
+		createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM,  isTrigger);
+		createButton("Get State",CMD_REQUEST_ACTUAL_STATE,  isTrigger);
+		createButton("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
+		createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
+		
+	} else
+	{
+		/*
+		m_userCommandRequests.push_back(CMD_LOAD_URDF);
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
+		m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
+		//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
+		m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
+		//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
+		m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
+		m_userCommandRequests.push_back(CMD_SHUTDOWN);
+		*/
+	}
+
+	if (!m_physicsClient.connect())
+	{
+		b3Warning("Cannot eonnect to physics client");
+	}
+
+}
+
+
+bool RobotControlExample::wantsTermination()
+{
+    return m_wantsShutdown;
+}
+
+
+
+void	RobotControlExample::stepSimulation(float deltaTime)
+{
+    m_physicsServer.processClientCommands();
+
+	if (m_physicsClient.isConnected())
+    {
+		m_physicsClient.processServerStatus();
+    
+		if (m_physicsClient.canSubmitCommand())
+		{
+			if (m_userCommandRequests.size())
+			{
+				b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
+				SharedMemoryCommand& cmd = m_userCommandRequests[0];
+
+				//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
+				for (int i=1;i<m_userCommandRequests.size();i++)
+				{
+					m_userCommandRequests[i-1] = m_userCommandRequests[i];
+				}
+
+				m_userCommandRequests.pop_back();
+				m_physicsClient.submitClientCommand(cmd);
+			}
+		}
+	}
+}
+
+
+class CommonExampleInterface*    RobotControlExampleCreateFunc(struct CommonExampleOptions& options)
+{
+    return new RobotControlExample(options.m_guiHelper);
+}
+
+

examples/SharedMemory/RobotControlExample.h

@@ -0,0 +1,8 @@
+#ifndef ROBOT_CONTROL_EXAMPLE_H
+#define ROBOT_CONTROL_EXAMPLE_H
+
+class CommonExampleInterface*    RobotControlExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif //ROBOT_CONTROL_EXAMPLE_H
+
+

examples/SharedMemory/SharedMemoryCommands.h

@@ -4,6 +4,24 @@
 
 //this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc)
 
+#ifdef __GNUC__
+	#include <stdint.h>
+	typedef int32_t smInt32_t;
+	typedef int64_t smInt64_t;
+	typedef uint32_t smUint32_t;
+	typedef uint64_t smUint64_t;
+#elif defined(_MSC_VER)
+	typedef __int32 smInt32_t;
+	typedef __int64 smInt64_t;
+	typedef unsigned __int32 smUint32_t;
+	typedef unsigned __int64 smUint64_t;
+#else
+	typedef int smInt32_t;
+	typedef long long int smInt64_t;
+	typedef unsigned int smUint32_t;
+	typedef unsigned long long int smUint64_t;
+#endif
+
 enum SharedMemoryServerCommand
 {
     CMD_URDF_LOADING_COMPLETED,
@@ -74,6 +92,15 @@ struct SendDesiredStateArgs
 {
 	int m_bodyUniqueId;
 	int m_controlMode;
+
+	//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];
+	
+	//m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or
+	//or m_desiredStateForceTorque is the maximum applied force/torque for the motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY mode
+    double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
+ 
 };
 
 struct RequestActualStateArgs
@@ -99,6 +126,12 @@ struct SendActualStateArgs
 	int m_bodyUniqueId;
 	int m_numDegreeOfFreedomQ;
 	int m_numDegreeOfFreedomU;
+
+	  //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
+  
 };
 
 
@@ -112,6 +145,9 @@ struct SharedMemoryCommand
 {
     int m_type;
 	
+    smUint64_t	m_timeStamp;
+	int	m_sequenceNumber;
+
     union
     {
         UrdfArgs m_urdfArguments;

examples/SharedMemory/SharedMemoryInterface.h

@@ -3,7 +3,7 @@
 
 #define SHARED_MEMORY_KEY 12347
 #define SHARED_MEMORY_MAGIC_NUMBER 64738
-#define SHARED_MEMORY_MAX_COMMANDS 64
+#define SHARED_MEMORY_MAX_COMMANDS 32
 #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
 
 #include "SharedMemoryCommands.h"
@@ -20,22 +20,6 @@ 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];
-	
-	//m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or
-	//or m_desiredStateForceTorque is the maximum applied force/torque for the motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY mode
-    double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
-    
-    //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
-        
 	//m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints
 	//the Bullet data structures are more general purpose than the capabilities of a URDF file.
 	char	m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];

examples/SharedMemory/premake4.lua

@@ -33,6 +33,7 @@ files {
 	"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 	"../Importers/ImportURDFDemo/URDF2Bullet.h",
 	"../Utils/b3ResourcePath.cpp",
+	"../Utils/b3Clock.cpp",	
 	"../../Extras/Serialize/BulletWorldImporter/*",
 	"../../Extras/Serialize/BulletFileLoader/*",	
 	"../Importers/ImportURDFDemo/URDFImporterInterface.h",