Merge pull request #408 from erwincoumans/master

more work on shared memory example (still preliminary), and fix of infinite recursion in 'getMargin'

examples/CommonInterfaces/CommonParameterInterface.h

@@ -32,12 +32,32 @@ struct SliderParams
 
 };
 
+typedef void (*ButtonParamChangedCallback) (int buttonId, bool buttonState, void* userPointer);
+
+struct ButtonParams
+{
+	const char* m_name;
+	int m_buttonId;
+	void* m_userPointer;
+
+	ButtonParamChangedCallback m_callback;
+	ButtonParams(const char* name, int buttonId, bool isTrigger)
+		:m_name(name),
+		m_buttonId(buttonId),
+		m_userPointer(0),
+	m_callback(0)
+	{
+	}
+};
+
 
 struct CommonParameterInterface
 {
 
 	virtual ~CommonParameterInterface() {}
 	virtual void registerSliderFloatParameter(SliderParams& params)=0;
+	virtual void registerButtonParameter(ButtonParams& params)=0;
+
 	virtual void syncParameters()=0;
 	virtual void removeAllParameters()=0;
     virtual void setSliderValue(int sliderIndex, double sliderValue)=0;

examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp

@@ -1,6 +1,27 @@
 #include "GwenParameterInterface.h"
 #include "gwenInternalData.h"
 
+struct MyButtonEventHandler : public Gwen::Event::Handler
+{
+	ButtonParamChangedCallback m_callback;
+	void* m_userPointer;
+	int m_buttonId;
+
+	MyButtonEventHandler(ButtonParamChangedCallback callback, int buttonId, void* userPointer)
+		:m_callback(callback),
+		m_userPointer(userPointer),
+		m_buttonId(buttonId)
+	{
+	}
+
+	void onButtonPress( Gwen::Controls::Base* pControl )
+	{
+		if (m_callback)
+		{
+			(*m_callback)(m_buttonId, true, m_userPointer);
+		}
+	}
+};
 
 
 template<typename T>
@@ -62,6 +83,9 @@ struct  GwenParameters
 {
 	b3AlignedObjectArray<MySliderEventHandler<btScalar>*> m_sliderEventHandlers;
 	b3AlignedObjectArray<Gwen::Controls::HorizontalSlider*> m_sliders;
+
+	b3AlignedObjectArray<Gwen::Controls::Button*> m_buttons;
+	b3AlignedObjectArray<MyButtonEventHandler*> m_buttonEventHandlers;
 	b3AlignedObjectArray<Gwen::Controls::TextBox*> m_textLabels;
 	int m_savedYposition;
 };
@@ -102,6 +126,24 @@ void GwenParameterInterface::setSliderValue(int sliderIndex, double sliderValue)
 }
 
 #include <stdio.h>
+
+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);
+	button->SetText(params.m_name);
+	button->onPress.Add( handler, &MyButtonEventHandler::onButtonPress );
+
+	m_paramInternalData->m_buttons.push_back(button);
+	m_paramInternalData->m_buttonEventHandlers.push_back(handler);
+
+	button->SetPos( 10, m_gwenInternalData->m_curYposition );
+	m_gwenInternalData->m_curYposition+=22;
+
+}
+
 void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
 {
 	Gwen::Controls::TextBox* label = new Gwen::Controls::TextBox(m_gwenInternalData->m_demoPage->GetPage());

examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h

@@ -12,6 +12,8 @@ struct GwenParameterInterface : public CommonParameterInterface
 	GwenParameterInterface(struct GwenInternalData* gwenInternalData);
 	virtual ~GwenParameterInterface();
 	virtual void registerSliderFloatParameter(SliderParams& params);
+	virtual void registerButtonParameter(ButtonParams& params);
+
     virtual void setSliderValue(int sliderIndex, double sliderValue);
 	virtual void syncParameters();
 	virtual void removeAllParameters();

examples/SharedMemory/PhysicsClient.cpp

@@ -5,6 +5,8 @@
 #include "PosixSharedMemory.h"
 #include "Win32SharedMemory.h"
 #include "SharedMemoryCommon.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
 
 class PhysicsClient : public SharedMemoryCommon
 {
@@ -12,6 +14,13 @@ class PhysicsClient : public SharedMemoryCommon
 	SharedMemoryExampleData*   m_testBlock1;
     int m_counter;
     bool m_wantsTermination;
+	btAlignedObjectArray<int> m_userCommandRequests;
+
+	bool m_serverLoadUrdfOK;
+	bool m_waitingForServer;
+	
+
+
     
 public:
     
@@ -35,13 +44,56 @@ public:
     {
         return m_wantsTermination;
     }
+	void submitCommand(int command);
+	
 };
 
+
+void MyCallback(int buttonId, bool buttonState, void* userPtr)
+{
+	PhysicsClient* cl = (PhysicsClient*) userPtr;
+	switch (buttonId)
+	{
+	case  CMD_LOAD_URDF:
+	{
+		cl->submitCommand(CMD_LOAD_URDF);
+		break;
+	}
+
+	case CMD_STEP_FORWARD_SIMULATION:
+		{
+			cl->submitCommand(CMD_STEP_FORWARD_SIMULATION);
+			break;
+		}
+	case CMD_SHUTDOWN:
+		{
+			cl->submitCommand(CMD_SHUTDOWN);
+			break;
+		}
+
+	default:
+		{
+			b3Error("Unknown buttonId");
+			btAssert(0);
+		}
+	};
+}
+
+
+void PhysicsClient::submitCommand(int command)
+{
+	m_userCommandRequests.push_back(command);
+	b3Printf("User submitted command request  %d (outstanding %d)\n",command, m_userCommandRequests.size());
+}
+
+
 PhysicsClient::PhysicsClient(GUIHelperInterface* helper)
 :SharedMemoryCommon(helper),
 m_testBlock1(0),
 m_counter(0),
-m_wantsTermination(false)
+m_wantsTermination(false),
+m_serverLoadUrdfOK(false),
+m_waitingForServer(false)
 {
 	b3Printf("Started PhysicsClient");
 #ifdef _WIN32
@@ -58,8 +110,34 @@ PhysicsClient::~PhysicsClient()
 	delete m_sharedMemory;
 }
 
+
 void	PhysicsClient::initPhysics()
 {
+	{
+		bool isTrigger = false;
+		ButtonParams button("Load URDF",CMD_LOAD_URDF,  isTrigger);
+		button.m_callback = MyCallback;
+		button.m_userPointer = this;
+		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+    }
+
+	{
+		bool isTrigger = false;
+		ButtonParams button("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
+		button.m_callback = MyCallback;
+		button.m_userPointer = this;
+		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+    }
+
+	{
+		bool isTrigger = false;
+		ButtonParams button("Shut Down",CMD_SHUTDOWN,  isTrigger);
+		button.m_callback = MyCallback;
+		button.m_userPointer = this;
+		m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+    }
+
+
     m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
     if (m_testBlock1)
     {
@@ -69,16 +147,10 @@ void	PhysicsClient::initPhysics()
             b3Error("Error: please start server before client");
             m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
             m_testBlock1 = 0;
-            
         } else
-        {
+		{
-            //submit a 'load urdf' command to get things started
+			b3Printf("Shared Memory status is OK");
-            
+		}
-            b3Printf("Client created CMD_LOAD_URDF");
-            m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
-            sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
-            m_testBlock1->m_numClientCommands++;
-        }
     }
     
 }
@@ -102,30 +174,20 @@ void	PhysicsClient::stepSimulation(float deltaTime)
             switch (serverCmd.m_type)
             {
 
+				case CMD_URDF_LOADING_COMPLETED:
+				{
+					m_serverLoadUrdfOK = true;
+					b3Printf("Server loading the URDF OK");
+					break;
+				}
                 case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
-                case CMD_URDF_LOADING_COMPLETED:
                 {
-                    //submit a 'step simulation' request
-                    
-                    if (m_counter<10)
-                    {
-                        m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
-                        m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
-                        m_testBlock1->m_numClientCommands++;
-                        m_counter++;
-                        
-                    } else
-                    {
-                        m_wantsTermination = true;
-                        m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
-                        m_testBlock1->m_numClientCommands++;
-                    }
-                    
                     break;
                 }
                 case CMD_URDF_LOADING_FAILED:
                 {
                     b3Printf("Server failed loading the URDF...");
+					m_serverLoadUrdfOK = false;
                     break;
                 }
                 default:
@@ -134,12 +196,83 @@ void	PhysicsClient::stepSimulation(float deltaTime)
                     btAssert(0);
                 }
             };
+			
+			
             m_testBlock1->m_numProcessedServerCommands++;
+
+			if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands)
+			{
+				m_waitingForServer = false;
+			} else
+			{
+				m_waitingForServer = true;
+			}
         }
-        
-    }
-    
     
+		if (!m_waitingForServer)
+		{
+			//process outstanding requests
+			if (m_userCommandRequests.size())
+			{
+				b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
+				int command = m_userCommandRequests[0];
+				m_userCommandRequests.remove(command);
+				switch (command)
+				{
+				case CMD_LOAD_URDF:
+					{
+						if (!m_serverLoadUrdfOK)
+						{
+							m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
+							sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
+							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
+							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
+
+							m_testBlock1->m_numClientCommands++;
+							b3Printf("Client created CMD_LOAD_URDF");
+							m_waitingForServer = true;
+						} else
+						{
+							b3Warning("Server already loaded URDF, no client command submitted");
+						}
+						break;
+					}
+				case CMD_STEP_FORWARD_SIMULATION:
+					{
+						if (m_serverLoadUrdfOK)
+						{
+						
+							 m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
+							m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
+							m_testBlock1->m_numClientCommands++;
+							b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
+							m_waitingForServer = true;
+						} else
+						{
+							b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted");
+						}
+						break;
+					}
+				case CMD_SHUTDOWN:
+					{
+						m_wantsTermination = true;
+						m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
+						m_testBlock1->m_numClientCommands++;
+						m_waitingForServer = true;
+						m_serverLoadUrdfOK = false;
+						b3Printf("client created CMD_SHUTDOWN\n");
+						break;
+					}
+				default:
+					{
+						b3Error("unknown command requested");
+					}
+				}
+			}
+		}
+
+	  }
+	
 
 }
 

examples/SharedMemory/PhysicsServer.cpp

@@ -15,6 +15,8 @@ class PhysicsServer : public SharedMemoryCommon
 {
 	SharedMemoryInterface* m_sharedMemory;
     SharedMemoryExampleData* m_testBlock1;
+
+	btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
 	bool m_wantsShutdown;
 
 public:
@@ -34,10 +36,10 @@ public:
     
 	virtual void resetCamera()
 	{
-		float dist = 1;
+		float dist = 5;
 		float pitch = 50;
 		float yaw = 35;
-		float targetPos[3]={-3,2.8,-2.5};
+		float targetPos[3]={0,0,0};//-3,2.8,-2.5};
 		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
 	}
     
@@ -83,7 +85,16 @@ PhysicsServer::~PhysicsServer()
 
 void	PhysicsServer::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] = -9.8;
+	this->m_dynamicsWorld->setGravity(grav);
     
     m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
     
@@ -135,8 +146,29 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
         
         ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
         btMultiBody* mb = creation.getBulletMultiBody();
+		if (useMultiBody)
+		{
+			if (mb)
+			{
+				return true;
+			} else
+			{
+				b3Warning("No multibody loaded from URDF");
+				return false;
+			}
+		} else
+		{
+			for (int i=0;i<m_dynamicsWorld->getNumConstraints();i++)
+			{
+				btTypedConstraint* c = m_dynamicsWorld->getConstraint(i);
+				btJointFeedback* fb = new btJointFeedback();
+				m_jointFeedbacks.push_back(fb);
+				c->setJointFeedback(fb);
 
-        return true;
+			}
+			return true;
+		}
+        
     }
     
     return false;
@@ -169,8 +201,9 @@ void	PhysicsServer::stepSimulation(float deltaTime)
                     //load the actual URDF and send a report: completed or failed
 
                     
+					
                     bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName,
-                                               btVector3(0,0,0), btQuaternion(0,0,0,1),true,true );
+                                               btVector3(0,0,0), btQuaternion(0,0,0,1),clientCmd.m_urdfArguments.m_useMultiBody,clientCmd.m_urdfArguments.m_useFixedBase);
                     SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
  
                     if (completedOk)
@@ -195,7 +228,24 @@ void	PhysicsServer::stepSimulation(float deltaTime)
                     
                     serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
                     m_testBlock1->m_numServerCommands++;
- 
+
+					//now we send back the actual q, q' and force/torque and IMU sensor values
+					for (int i=0;i<m_jointFeedbacks.size();i++)
+					{
+						printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n", 
+							m_jointFeedbacks[i]->m_appliedForceBodyA.x(),
+							m_jointFeedbacks[i]->m_appliedForceBodyA.y(),
+							m_jointFeedbacks[i]->m_appliedForceBodyA.z(),
+							m_jointFeedbacks[i]->m_appliedTorqueBodyA.x(),
+							m_jointFeedbacks[i]->m_appliedTorqueBodyA.y(),
+							m_jointFeedbacks[i]->m_appliedTorqueBodyA.z(),
+							m_jointFeedbacks[i]->m_appliedForceBodyB.x(),
+							m_jointFeedbacks[i]->m_appliedForceBodyB.y(),
+							m_jointFeedbacks[i]->m_appliedForceBodyB.z(),
+							m_jointFeedbacks[i]->m_appliedTorqueBodyB.x(),
+							m_jointFeedbacks[i]->m_appliedTorqueBodyB.y(),
+							m_jointFeedbacks[i]->m_appliedTorqueBodyB.z());
+					}
                     break;
                 }
                 case CMD_SHUTDOWN:

examples/SharedMemory/SharedMemoryInterface.h

@@ -31,6 +31,8 @@ enum SharedMemoryClientCommand{
 struct UrdfCommandArgument
 {
     char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
+	bool m_useMultiBody;
+	bool m_useFixedBase;
 };
 
 struct StepSimulationCommandArgument

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -13,8 +13,8 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod
 	m_jacSizeA(0),
 	m_jacSizeBoth(0),
 	m_isUnilateral(isUnilateral),
-	m_maxAppliedImpulse(100),
+	m_numDofsFinalized(-1),
-	m_numDofsFinalized(-1)
+	m_maxAppliedImpulse(100)
 {
 
 }

src/BulletSoftBody/btSoftBodyInternals.h

@@ -161,7 +161,7 @@ public:
 	}
 	virtual btScalar	getMargin() const
 	{
-		return getMargin();
+		return btConvexInternalShape::getMargin();
 	}
 };