From 7452df0964d84b78cadeb1246a639429ee5d28c3 Mon Sep 17 00:00:00 2001
From: = <=>
Date: Sun, 12 Jul 2015 14:48:43 -0700
Subject: [PATCH] Pass initial position and orientation for CMD_LOAD_URDF.
 (public github commit)

---
 examples/SharedMemory/PhysicsClient.cpp      |  7 +++++++
 examples/SharedMemory/PhysicsServer.cpp      | 21 ++++++++++++--------
 examples/SharedMemory/SharedMemoryCommands.h |  2 ++
 3 files changed, 22 insertions(+), 8 deletions(-)

diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp
index a2e5cad1c..90c64a986 100644
--- a/examples/SharedMemory/PhysicsClient.cpp
+++ b/examples/SharedMemory/PhysicsClient.cpp
@@ -341,6 +341,13 @@ void	PhysicsClient::createClientCommand()
 						{
 							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_initialPosition[0] = 0.0;
+							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0;
+							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0;
+							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0;
+							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0;
+							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0;
+							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0;
 							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
 							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
 
diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp
index 42d7bf561..3258deae1 100644
--- a/examples/SharedMemory/PhysicsServer.cpp
+++ b/examples/SharedMemory/PhysicsServer.cpp
@@ -12,7 +12,7 @@
 #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
 
 #include "SharedMemoryCommon.h"
-const char* blaatnaam = "basename";
+//const char* blaatnaam = "basename";
 struct UrdfLinkNameMapUtil
 {
 	btMultiBody* m_mb;
@@ -274,14 +274,19 @@ void	PhysicsServer::stepSimulation(float deltaTime)
 				}
                 case CMD_LOAD_URDF:
                 {
-                    b3Printf("Processed CMD_LOAD_URDF:%s",clientCmd.m_urdfArguments.m_urdfFileName);
-                    
-                    //load the actual URDF and send a report: completed or failed
+                    const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
+                    b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName);
 
-                    
-					
-                    bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName,
-                                               btVector3(0,0,0), btQuaternion(0,0,0,1),clientCmd.m_urdfArguments.m_useMultiBody,clientCmd.m_urdfArguments.m_useFixedBase);
+                    //load the actual URDF and send a report: completed or failed
+                    bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
+                                                btVector3(urdfArgs.m_initialPosition[0],
+                                                          urdfArgs.m_initialPosition[1],
+                                                          urdfArgs.m_initialPosition[2]),
+                                                btQuaternion(urdfArgs.m_initialOrientation[0],
+                                                             urdfArgs.m_initialOrientation[1],
+                                                             urdfArgs.m_initialOrientation[2],
+                                                             urdfArgs.m_initialOrientation[3]),
+                                                urdfArgs.m_useMultiBody, urdfArgs.m_useFixedBase);
                     SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
  
                     if (completedOk)
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index a0a950618..8e71cc3a3 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -44,6 +44,8 @@ enum SharedMemoryClientCommand
 struct UrdfArgs
 {
     char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
+    double m_initialPosition[3];
+    double m_initialOrientation[4];
 	bool m_useMultiBody;
 	bool m_useFixedBase;
 };