diff --git a/examples/CommonInterfaces/CommonParameterInterface.h b/examples/CommonInterfaces/CommonParameterInterface.h
index 6243cc8c5..1d79b8329 100644
--- a/examples/CommonInterfaces/CommonParameterInterface.h
+++ b/examples/CommonInterfaces/CommonParameterInterface.h
@@ -33,6 +33,7 @@ struct SliderParams
 };
 
 typedef void (*ButtonParamChangedCallback) (int buttonId, bool buttonState, void* userPointer);
+typedef void (*ComboBoxCallback) (int combobox, const char* item, void* userPointer);
 
 struct ButtonParams
 {
@@ -50,6 +51,26 @@ struct ButtonParams
 	}
 };
 
+struct ComboBoxParams
+{
+	int m_comboboxId;
+	int m_numItems;
+	const char** m_items;
+	int m_startItem;
+	ComboBoxCallback m_callback;
+	void*	m_userPointer;
+
+	ComboBoxParams()
+		:m_comboboxId(-1),
+		m_numItems(0),
+		m_items(0),
+		m_startItem(0),
+		m_callback(0),
+		m_userPointer(0)
+	{
+	}
+};
+
 
 struct CommonParameterInterface
 {
@@ -57,6 +78,7 @@ struct CommonParameterInterface
 	virtual ~CommonParameterInterface() {}
 	virtual void registerSliderFloatParameter(SliderParams& params)=0;
 	virtual void registerButtonParameter(ButtonParams& params)=0;
+	virtual void registerComboBox(ComboBoxParams& params)=0;
 
 	virtual void syncParameters()=0;
 	virtual void removeAllParameters()=0;
diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
index 02c4cda11..a1d0ba177 100644
--- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
+++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp
@@ -83,7 +83,7 @@ struct  GwenParameters
 {
 	b3AlignedObjectArray<MySliderEventHandler<btScalar>*> m_sliderEventHandlers;
 	b3AlignedObjectArray<Gwen::Controls::HorizontalSlider*> m_sliders;
-
+	b3AlignedObjectArray<Gwen::Controls::ComboBox*> m_comboBoxes;
 	b3AlignedObjectArray<Gwen::Controls::Button*> m_buttons;
 	b3AlignedObjectArray<MyButtonEventHandler*> m_buttonEventHandlers;
 	b3AlignedObjectArray<Gwen::Controls::TextBox*> m_textLabels;
@@ -143,6 +143,58 @@ void GwenParameterInterface::registerButtonParameter(ButtonParams& params)
 	
 	m_gwenInternalData->m_curYposition+=22;
 
+}
+
+struct MyComboBoxHander2   :public Gwen::Event::Handler
+{
+	GwenInternalData*	m_data;
+	int					m_buttonId;
+	ComboBoxCallback	m_callback;
+	void*				m_userPointer;
+
+	MyComboBoxHander2  (GwenInternalData* data, int buttonId,ComboBoxCallback callback, void* userPointer)
+		:m_data(data),
+		m_buttonId(buttonId),
+		m_callback(callback),
+		m_userPointer(userPointer)
+	{
+	}
+
+	void onSelect( Gwen::Controls::Base* pControl )
+	{
+		Gwen::Controls::ComboBox* but = (Gwen::Controls::ComboBox*) pControl;
+
+		Gwen::String str = Gwen::Utility::UnicodeToString(	but->GetSelectedItem()->GetText());
+
+		if (m_callback)
+			(*m_callback)(m_buttonId,str.c_str(),m_userPointer);
+	}
+
+};
+
+
+void GwenParameterInterface::registerComboBox(ComboBoxParams& params)
+{
+	Gwen::Controls::ComboBox* combobox = new Gwen::Controls::ComboBox(m_gwenInternalData->m_demoPage->GetPage());
+	m_paramInternalData->m_comboBoxes.push_back(combobox);
+	MyComboBoxHander2* handler = new MyComboBoxHander2(m_gwenInternalData, params.m_comboboxId,params.m_callback, params.m_userPointer);
+	m_gwenInternalData->m_handlers.push_back(handler);
+
+	combobox->onSelection.Add(handler,&MyComboBoxHander2::onSelect);
+	int ypos = m_gwenInternalData->m_curYposition;
+	m_gwenInternalData->m_curYposition+=22;
+	combobox->SetPos(10, ypos );
+	combobox->SetWidth( 100 );
+	//box->SetPos(120,130);
+	for (int i=0;i<params.m_numItems;i++)
+	{
+		Gwen::Controls::MenuItem* item = combobox->AddItem(Gwen::Utility::StringToUnicode(params.m_items[i]));
+		if (i==params.m_startItem)
+			combobox->OnItemSelected(item);
+	}
+
+	
+
 }
 
 void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
@@ -188,6 +240,7 @@ void GwenParameterInterface::syncParameters()
 
 void GwenParameterInterface::removeAllParameters()
 {
+
 	for (int i=0;i<m_paramInternalData->m_buttons.size();i++)
 	{
 		delete m_paramInternalData->m_buttons[i];
@@ -222,5 +275,18 @@ void GwenParameterInterface::removeAllParameters()
 	}
 	m_paramInternalData->m_textLabels.clear();
 	
+	for (int i=0;i<m_paramInternalData->m_comboBoxes.size();i++)
+	{
+		delete m_paramInternalData->m_comboBoxes[i];
+	}
+	m_paramInternalData->m_comboBoxes.clear();
+
 	m_gwenInternalData->m_curYposition = this->m_paramInternalData->m_savedYposition;
-}
\ No newline at end of file
+	for (int i=0;i<m_gwenInternalData->m_handlers.size();i++)
+	{
+		delete m_gwenInternalData->m_handlers[i];
+	}
+	m_gwenInternalData->m_handlers.clear();
+
+	
+}
diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h
index 6ee4a54cd..262b6e9fa 100644
--- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h
+++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.h
@@ -13,10 +13,13 @@ struct GwenParameterInterface : public CommonParameterInterface
 	virtual ~GwenParameterInterface();
 	virtual void registerSliderFloatParameter(SliderParams& params);
 	virtual void registerButtonParameter(ButtonParams& params);
+	virtual void registerComboBox(ComboBoxParams& params);
 
     virtual void setSliderValue(int sliderIndex, double sliderValue);
 	virtual void syncParameters();
 	virtual void removeAllParameters();
+	
+
 
 };
 
diff --git a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
index 4f8ef5dd0..a70d9268a 100644
--- a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
+++ b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.cpp
@@ -428,7 +428,7 @@ void	GwenUserInterface::setToggleButtonCallback(b3ToggleButtonCallback callback)
 {
 	m_data->m_toggleButtonCallback = callback;
 }
-void	GwenUserInterface::registerToggleButton(int buttonId, const char* name)
+void	GwenUserInterface::registerToggleButton2(int buttonId, const char* name)
 {
 	assert(m_data);
 	assert(m_data->m_demoPage);
@@ -460,7 +460,7 @@ b3ComboBoxCallback GwenUserInterface::getComboBoxCallback()
 {
 	return m_data->m_comboBoxCallback;
 }
-void	GwenUserInterface::registerComboBox(int comboboxId, int numItems, const char** items, int startItem)
+void	GwenUserInterface::registerComboBox2(int comboboxId, int numItems, const char** items, int startItem)
 {
 	Gwen::Controls::ComboBox* combobox = new Gwen::Controls::ComboBox(m_data->m_demoPage->GetPage());
 	MyComboBoxHander* handler = new MyComboBoxHander(m_data, comboboxId);
diff --git a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h
index 15e4eb0d5..c448e9fff 100644
--- a/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h
+++ b/examples/ExampleBrowser/GwenGUISupport/gwenUserInterface.h
@@ -41,11 +41,11 @@ class GwenUserInterface
 		void	setToggleButtonCallback(b3ToggleButtonCallback callback);
 		b3ToggleButtonCallback getToggleButtonCallback();
 
-		void	registerToggleButton(int buttonId, const char* name);
+		void	registerToggleButton2(int buttonId, const char* name);
 
 		void	setComboBoxCallback(b3ComboBoxCallback callback);
 		b3ComboBoxCallback getComboBoxCallback();
-		void	registerComboBox(int buttonId, int numItems, const char** items, int startItem = 0);
+		void	registerComboBox2(int buttonId, int numItems, const char** items, int startItem = 0);
 		
 		void	setStatusBarMessage(const char* message, bool isLeft=true);
 
diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h
index 9090596b8..ac9e04038 100644
--- a/examples/SharedMemory/PhysicsClient.h
+++ b/examples/SharedMemory/PhysicsClient.h
@@ -24,9 +24,9 @@ public:
 
     virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
 
-    virtual int getNumJoints() const = 0;
+    virtual int getNumJoints(int bodyIndex) const = 0;
 
-    virtual void getJointInfo(int index, struct b3JointInfo& info) const = 0;
+    virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
 
     virtual void setSharedMemoryKey(int key) = 0;
 
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 62e75e042..bfa885d61 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -135,6 +135,7 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
     b3Assert(command);
 	command->m_type = CMD_SEND_DESIRED_STATE;
     command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
+	command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0;
 	command->m_updateFlags = 0;
     return (b3SharedMemoryCommandHandle) command;
 }
@@ -197,6 +198,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
     struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
     b3Assert(command);
     command->m_type =CMD_REQUEST_ACTUAL_STATE;
+	command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = 0;
     return (b3SharedMemoryCommandHandle) command;
 }
 
@@ -204,12 +206,17 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl
 {
   const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
   b3Assert(status);
-  b3JointInfo info;
-  b3GetJointInfo(physClient, jointIndex, &info);
-  state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
-  state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
-  for (int ii(0); ii < 6; ++ii) {
-    state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
+  int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
+  b3Assert(bodyIndex>=0);
+  if (bodyIndex>=0)
+  {
+	  b3JointInfo info;
+	  b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
+	  state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
+	  state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
+	  for (int ii(0); ii < 6; ++ii) {
+		state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
+	  }
   }
 }
 
@@ -279,6 +286,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
     command->m_type = CMD_CREATE_SENSOR;
     command->m_updateFlags = 0;
     command->m_createSensorArguments.m_numJointSensorChanges = 0;
+	command->m_createSensorArguments.m_bodyUniqueId = 0;
     return (b3SharedMemoryCommandHandle) command;
     
 }
@@ -349,7 +357,29 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
     return 0;
 }
 
+int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
+{
+	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+    b3Assert(status);
+	int bodyId = -1;
 
+	if (status)
+	{
+			switch (status->m_type)
+			{
+				case CMD_URDF_LOADING_COMPLETED:
+				{
+					bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
+					break;
+				}
+				default:
+				{
+					b3Assert(0);
+				}
+			};
+	}
+	return bodyId;
+}
 int	b3CanSubmitCommand(b3PhysicsClientHandle physClient)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -379,17 +409,17 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
 }
 
 
-int	b3GetNumJoints(b3PhysicsClientHandle physClient)
+int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
-	return cl->getNumJoints();
+	return cl->getNumJoints(bodyId);
 }
 
 
-void	b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info)
+void	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
-	cl->getJointInfo(linkIndex,*info);
+	cl->getJointInfo(bodyIndex, linkIndex,*info);
 }
 
 int b3PickBody(struct SharedMemoryCommand *command,
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 7e6c3c771..d8b1891db 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -34,10 +34,12 @@ b3SharedMemoryStatusHandle	b3ProcessServerStatus(b3PhysicsClientHandle physClien
 
 int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
 
-    
-int	b3GetNumJoints(b3PhysicsClientHandle physClient);
+int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
 
-void	b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info);
+    
+int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
+
+void	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
 
 b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
     
diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp
index adb8a6acb..4f0996b86 100644
--- a/examples/SharedMemory/PhysicsClientExample.cpp
+++ b/examples/SharedMemory/PhysicsClientExample.cpp
@@ -29,7 +29,8 @@ protected:
 	bool m_wantsTermination;
     btAlignedObjectArray<int> m_userCommandRequests;
     int m_sharedMemoryKey;
-    
+    int m_selectedBody;
+	int m_prevSelectedBody;
 	void	createButton(const char* name, int id, bool isTrigger );
 
 	void createButtons();
@@ -45,12 +46,22 @@ public:
 	virtual ~PhysicsClientExample();
     
 	virtual void	initPhysics();
-    
+    void selectComboBox(int comboIndex, const char* name)
+	{
+		if (m_guiHelper && m_guiHelper->getParameterInterface())
+		{
+			int bodyIndex = comboIndex;
+			if (m_selectedBody != bodyIndex)
+			{
+				m_selectedBody = bodyIndex;
+			}
+		}
+	}
 	virtual void	stepSimulation(float deltaTime);
     
 	virtual void resetCamera()
 	{
-				float dist = 5;
+		float dist = 5;
 		float pitch = 50;
 		float yaw = 35;
 		float targetPos[3]={0,0,0};//-3,2.8,-2.5};
@@ -144,13 +155,27 @@ public:
 };
 
 
+void MyComboBoxCallback (int combobox, const char* item, void* userPointer)
+{
+	b3Printf("Item selected %s", item);
+
+	PhysicsClientExample* cl = (PhysicsClientExample*) userPointer;
+	b3Assert(cl);
+	if (cl)
+	{
+		cl->selectComboBox(combobox,item);
+	}
+	
+}
 
 
 
 void MyCallback(int buttonId, bool buttonState, void* userPtr)
 {
 	PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
-    if (buttonState)
+	b3Assert(cl);
+
+    if (cl && buttonState)
     {
         cl->enqueueCommand(buttonId);
     }
@@ -173,9 +198,10 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
             
             //setting the initial position, orientation and other arguments are optional
             double startPosX = 0;
-            double startPosY = 0;
+            static double startPosY = 0;
             double startPosZ = 0;
             b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
+			startPosY += 2.f;
 //            ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
             b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
 
@@ -252,6 +278,11 @@ m_numMotors(0)
 
 PhysicsClientExample::~PhysicsClientExample()
 {
+	if (m_physicsClientHandle)
+	{
+		b3ProcessServerStatus(m_physicsClientHandle);
+		b3DisconnectSharedMemory(m_physicsClientHandle);
+	}
     b3Printf("~PhysicsClientExample\n");
 }
 
@@ -269,6 +300,8 @@ void	PhysicsClientExample::createButtons()
 	
     if (m_guiHelper && m_guiHelper->getParameterInterface())
     {
+		m_guiHelper->getParameterInterface()->removeAllParameters();
+
         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);
@@ -276,9 +309,44 @@ void	PhysicsClientExample::createButtons()
         createButton("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
         createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
         createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
+
+
+		if (m_selectedBody>=0)
+		{
+			int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
+			for (int i=0;i<numJoints;i++)
+			{
+				b3JointInfo info;
+				b3GetJointInfo(m_physicsClientHandle,m_selectedBody,i,&info);
+				b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+                
+				if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
+				{
+					if (m_numMotors<MAX_NUM_MOTORS)
+					{
+						char motorName[1024];
+						sprintf(motorName,"%s q'", info.m_jointName);
+						MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
+						motorInfo->m_velTarget = 0.f;
+						motorInfo->m_uIndex = info.m_uIndex;
+                        
+						SliderParams slider(motorName,&motorInfo->m_velTarget);
+						slider.m_minVal=-4;
+						slider.m_maxVal=4;
+						if (m_guiHelper && m_guiHelper->getParameterInterface())
+						{
+							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+						}
+						m_numMotors++;
+					}
+				}
+			}
+		}
     }
 }
 
+
+
 void	PhysicsClientExample::initPhysics()
 {
 	if (m_guiHelper && m_guiHelper->getParameterInterface())
@@ -296,6 +364,9 @@ void	PhysicsClientExample::initPhysics()
         MyCallback(CMD_RESET_SIMULATION,true,this);
 	}
 
+	m_selectedBody = -1;
+	m_prevSelectedBody = -1;
+
     m_physicsClientHandle  = b3ConnectSharedMemory(m_sharedMemoryKey);
     if (!b3CanSubmitCommand(m_physicsClientHandle))
     {
@@ -307,6 +378,11 @@ void	PhysicsClientExample::initPhysics()
 
 void	PhysicsClientExample::stepSimulation(float deltaTime)
 {
+	if (m_prevSelectedBody != m_selectedBody)
+	{
+		createButtons();
+		m_prevSelectedBody = m_selectedBody;
+	}
     b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
     bool hasStatus = (status != 0);
 
@@ -317,43 +393,34 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
         
       	if (statusType == CMD_URDF_LOADING_COMPLETED)
 		{
-            int numJoints = b3GetNumJoints(m_physicsClientHandle);
-            
-			for (int i=0;i<numJoints;i++)
+			int bodyIndex = b3GetStatusBodyIndex(status);
+			if (bodyIndex>=0)
 			{
-				b3JointInfo info;
-                b3GetJointInfo(m_physicsClientHandle,i,&info);
-                b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+				int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
+            
+				for (int i=0;i<numJoints;i++)
+				{
+					b3JointInfo info;
+					b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
+					b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
 				
-			}
+				}
+				ComboBoxParams comboParams;
+				comboParams.m_comboboxId = bodyIndex;
+				comboParams.m_numItems = 1;
+				comboParams.m_startItem = 0;
+				comboParams.m_callback = MyComboBoxCallback;
+				comboParams.m_userPointer = this;
+				const char* bla = "bla";
+				const char* blarray[1];
+				blarray[0] = bla;
+				
+				comboParams.m_items=blarray;//{&bla};
+				m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
 		
-            for (int i=0;i<numJoints;i++)
-            {
-                b3JointInfo info;
-                b3GetJointInfo(m_physicsClientHandle,i,&info);
-                b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
-                
-                if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
-                {
-                    if (m_numMotors<MAX_NUM_MOTORS)
-                    {
-                        char motorName[1024];
-                        sprintf(motorName,"%s q'", info.m_jointName);
-                        MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
-                        motorInfo->m_velTarget = 0.f;
-                        motorInfo->m_uIndex = info.m_uIndex;
-                        
-                        SliderParams slider(motorName,&motorInfo->m_velTarget);
-                        slider.m_minVal=-4;
-                        slider.m_maxVal=4;
-                        if (m_guiHelper && m_guiHelper->getParameterInterface())
-                        {
-                        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
-                        }
-                        m_numMotors++;
-                    }
-                }
-            }
+
+			}
+
 		}
     
     }
@@ -375,10 +442,7 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
             //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
             if (commandId ==CMD_RESET_SIMULATION)
             {
-                if (m_guiHelper->getParameterInterface())
-                {
-                    m_guiHelper->getParameterInterface()->removeAllParameters();
-                }
+				m_selectedBody = -1;
                 m_numMotors=0;
                 createButtons();
             }
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
index 71bf35d4a..20d91a8f9 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -30,12 +30,17 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z) {
     return tmp;
 }
 
+struct BodyJointInfoCache
+{
+	btAlignedObjectArray<b3JointInfo> m_jointInfo;
+};
+
 struct PhysicsClientSharedMemoryInternalData {
     SharedMemoryInterface* m_sharedMemory;
     SharedMemoryBlock* m_testBlock1;
+   
+	btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
 
-    btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
-    btAlignedObjectArray<b3JointInfo> m_jointInfo;
     btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
     btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
     btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
@@ -66,10 +71,29 @@ struct PhysicsClientSharedMemoryInternalData {
     bool canSubmitCommand() const;
 };
 
-int PhysicsClientSharedMemory::getNumJoints() const { return m_data->m_jointInfo.size(); }
+int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const 
+{
+	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
 
-void PhysicsClientSharedMemory::getJointInfo(int index, b3JointInfo& info) const {
-    info = m_data->m_jointInfo[index];
+		return bodyJoints->m_jointInfo.size(); 
+	}
+	btAssert(0);
+	return 0;
+	
+}
+
+void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const 
+{
+	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
+		info = bodyJoints->m_jointInfo[jointIndex];
+	}
+    
 }
 
 PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@@ -169,7 +193,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                         serverCmd.m_dataStreamArguments.m_streamChunkLength);
                     bf->setFileDNAisMemoryDNA();
                     bf->parse(false);
-                    m_data->m_robotMultiBodyData.push_back(bf);
+					int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+
+					BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
+                    m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
 
                     for (int i = 0; i < bf->m_multiBodies.size(); i++) {
                         int flag = bf->getFlags();
@@ -214,7 +241,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                                         (mb->m_links[link].m_jointType == ePrismaticType)) {
                                         info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
                                     }
-                                    m_data->m_jointInfo.push_back(info);
+                                    bodyJoints->m_jointInfo.push_back(info);
                                 }
                                 qOffset += mb->m_links[link].m_posVarCount;
                                 uOffset += mb->m_links[link].m_dofCount;
@@ -257,7 +284,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                                         (mb->m_links[link].m_jointType == ePrismaticType)) {
                                         info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
                                     }
-                                    m_data->m_jointInfo.push_back(info);
+                                    bodyJoints->m_jointInfo.push_back(info);
                                 }
                                 qOffset += mb->m_links[link].m_posVarCount;
                                 uOffset += mb->m_links[link].m_dofCount;
@@ -367,12 +394,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                 if (m_data->m_verboseOutput) {
                     b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
                 }
-                for (int i = 0; i < m_data->m_robotMultiBodyData.size(); i++) {
-                    delete m_data->m_robotMultiBodyData[i];
-                }
-                m_data->m_robotMultiBodyData.clear();
-
-                m_data->m_jointInfo.clear();
+                for (int i=0;i<m_data->m_bodyJointMap.size();i++)
+				{
+					BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
+					if (bodyJointsPtr && *bodyJointsPtr)
+					{
+						delete (*bodyJointsPtr);
+					}
+				}
+				m_data->m_bodyJointMap.clear();
+                
                 break;
             }
             case CMD_DEBUG_LINES_COMPLETED: {
diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h
index fb5e10181..05a4173db 100644
--- a/examples/SharedMemory/PhysicsClientSharedMemory.h
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.h
@@ -30,9 +30,9 @@ public:
 
     virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
 
-    virtual int getNumJoints() const;
+    virtual int getNumJoints(int bodyIndex) const;
 
-    virtual void getJointInfo(int index, struct b3JointInfo& info) const;
+    virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
 
     virtual void setSharedMemoryKey(int key);
 
diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp
index 1aabe081d..73304c3c5 100644
--- a/examples/SharedMemory/PhysicsServer.cpp
+++ b/examples/SharedMemory/PhysicsServer.cpp
@@ -74,14 +74,136 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
 		m_lines2.push_back(line);
 	}
 };
+
+
+struct InteralBodyData
+{
+	btMultiBody* m_multiBody;
+	int m_testData;
+
+	btTransform m_rootLocalInertialFrame;
+
+	InteralBodyData()
+		:m_multiBody(0),
+		m_testData(0)
+	{
+		m_rootLocalInertialFrame.setIdentity();
+	}
+};
+
+///todo: templatize this
+struct InternalBodyHandle : public InteralBodyData
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	int m_nextFreeHandle;
+	void SetNextFree(int next) 
+	{
+		m_nextFreeHandle = next;
+	}
+	int	GetNextFree() const 
+	{
+		return m_nextFreeHandle;
+	}
+};
+
 struct PhysicsServerInternalData
 {
+	///handle management
+	btAlignedObjectArray<InternalBodyHandle>	m_bodyHandles;
+	int m_numUsedHandles;						// number of active handles
+	
+	int	m_firstFreeHandle;		// free handles list
+	InternalBodyHandle* getHandle(int handle)
+	{
+		btAssert(handle>=0);
+		btAssert(handle<m_bodyHandles.size());
+		if ((handle<0) || (handle>=m_bodyHandles.size()))
+		{
+			return 0;
+		}
+		return &m_bodyHandles[handle];
+	}
+	const InternalBodyHandle* getHandle(int handle) const
+	{
+		return &m_bodyHandles[handle];
+	}
+
+	void increaseHandleCapacity(int extraCapacity)
+	{
+		int curCapacity = m_bodyHandles.size();
+		btAssert(curCapacity == m_numUsedHandles);
+		int newCapacity = curCapacity + extraCapacity;
+		m_bodyHandles.resize(newCapacity);
+
+		{
+			for (int i = curCapacity; i < newCapacity; i++)
+				m_bodyHandles[i].SetNextFree(i + 1);
+
+
+			m_bodyHandles[newCapacity - 1].SetNextFree(-1);
+		}
+		m_firstFreeHandle = curCapacity;
+	}
+	void initHandles()
+	{
+		m_numUsedHandles = 0;
+		m_firstFreeHandle = -1;
+		
+		increaseHandleCapacity(1);
+	}
+
+	void exitHandles()
+	{
+		m_bodyHandles.resize(0);
+		m_firstFreeHandle = -1;
+		m_numUsedHandles = 0;
+	}
+
+	int allocHandle()
+	{
+		btAssert(m_firstFreeHandle>=0);
+
+		int handle = m_firstFreeHandle;
+		m_firstFreeHandle = getHandle(handle)->GetNextFree();
+		m_numUsedHandles++;
+
+		if (m_firstFreeHandle<0)
+		{
+			int curCapacity = m_bodyHandles.size();
+			int additionalCapacity= m_bodyHandles.size();
+			increaseHandleCapacity(additionalCapacity);
+
+			
+			getHandle(handle)->SetNextFree(m_firstFreeHandle);
+		}
+		
+
+		return handle;
+	}
+
+
+	void freeHandle(int handle)
+	{
+		btAssert(handle >= 0);
+
+		getHandle(handle)->SetNextFree(m_firstFreeHandle);
+		m_firstFreeHandle = handle;
+
+		m_numUsedHandles--;
+	}
+
+	///end handle management
+	
+	
 	SharedMemoryInterface* m_sharedMemory;
     SharedMemoryBlock* m_testBlock1;
 	bool m_isConnected;
 	btScalar m_physicsDeltaTime;
 	btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
 
+	
+
 	btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
 	btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
 	btHashMap<btHashInt, btMultiBodyJointMotor*>	m_multiBodyJointMotorMap;
@@ -95,7 +217,7 @@ struct PhysicsServerInternalData
 	btMultiBodyDynamicsWorld* m_dynamicsWorld;
 	SharedMemoryDebugDrawer*		m_debugDrawer;
 
-    btTransform m_rootLocalInertialFrame;
+    
 
     
 	struct GUIHelperInterface* m_guiHelper;
@@ -127,7 +249,51 @@ struct PhysicsServerInternalData
 		m_pickedConstraint(0),
 		m_pickingMultiBodyPoint2Point(0)
 	{
-        m_rootLocalInertialFrame.setIdentity();
+        
+		initHandles();
+#if 0
+		btAlignedObjectArray<int> bla;
+
+		for (int i=0;i<1024;i++)
+		{
+			int handle = allocHandle();
+			bla.push_back(handle);
+			InternalBodyHandle* body = getHandle(handle);
+			InteralBodyData* body2 = body;
+		}
+		for (int i=0;i<bla.size();i++)
+		{
+			freeHandle(bla[i]);
+		}
+
+		bla.resize(0);
+
+		for (int i=0;i<1024;i++)
+		{
+			int handle = allocHandle();
+			bla.push_back(handle);
+			InternalBodyHandle* body = getHandle(handle);
+			InteralBodyData* body2 = body;
+		}
+		for (int i=0;i<bla.size();i++)
+		{
+			freeHandle(bla[i]);
+		}
+		bla.resize(0);
+
+		for (int i=0;i<1024;i++)
+		{
+			int handle = allocHandle();
+			bla.push_back(handle);
+			InternalBodyHandle* body = getHandle(handle);
+			InteralBodyData* body2 = body;
+		}
+		for (int i=0;i<bla.size();i++)
+		{
+			freeHandle(bla[i]);
+		}
+#endif
+
 	}
 
 	SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp)
@@ -422,7 +588,7 @@ void	PhysicsServerSharedMemory::createJointMotors(btMultiBody* mb)
 
 
 bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
-                             bool useMultiBody, bool useFixedBase)
+                             bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr)
 {
 	btAssert(m_data->m_dynamicsWorld);
 	if (!m_data->m_dynamicsWorld)
@@ -437,12 +603,19 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
     bool loadOk =  u2b.loadURDF(fileName, useFixedBase);
     if (loadOk)
     {
+		//get a body index
+		int bodyUniqueId = m_data->allocHandle();
+		if (bodyUniqueIdPtr)
+			*bodyUniqueIdPtr= bodyUniqueId;
+
+		InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
+
         {
             btScalar mass = 0;
-            m_data->m_rootLocalInertialFrame.setIdentity();
+            bodyHandle->m_rootLocalInertialFrame.setIdentity();
             btVector3 localInertiaDiagonal(0,0,0);
             int urdfLinkIndex = u2b.getRootLinkIndex();
-            u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,m_data->m_rootLocalInertialFrame);
+            u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame);
         }
 		if (m_data->m_verboseOutput)
 		{
@@ -463,6 +636,7 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3&
 		{
 			if (mb)
 			{
+				bodyHandle->m_multiBody = mb;
 				createJointMotors(mb);
 
 				//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
@@ -636,13 +810,7 @@ void PhysicsServerSharedMemory::processClientCommands()
 					}
                 case CMD_LOAD_URDF:
                 {
-					//at the moment, we only load 1 urdf / robot
-					if (m_data->m_urdfLinkNameMapper.size())
-					{
-						SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp);
-						m_data->submitServerStatus(status);
-						break;
-					}
+					
                     const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
 					if (m_data->m_verboseOutput)
 					{
@@ -667,21 +835,23 @@ void PhysicsServerSharedMemory::processClientCommands()
 					}
 					bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? urdfArgs.m_useMultiBody : true;
 					bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? urdfArgs.m_useFixedBase: false;
-
+					int bodyUniqueId;
                     //load the actual URDF and send a report: completed or failed
                     bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
                                                initialPos,initialOrn,
-                                               useMultiBody, useFixedBase);
-                    SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
+                                               useMultiBody, useFixedBase,&bodyUniqueId);
+                    
  
                     if (completedOk)
                     {
-						if (m_data->m_urdfLinkNameMapper.size())
-						{
-							serverCmd.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
-						}
+						
 						m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
 						SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
+						if (m_data->m_urdfLinkNameMapper.size())
+						{
+							status.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
+							status.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
+						}
 						m_data->submitServerStatus(status);
                         
                     } else
@@ -701,10 +871,11 @@ void PhysicsServerSharedMemory::processClientCommands()
 					{
 						b3Printf("Processed CMD_CREATE_SENSOR");
 					}
-                    
-                    if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
+					int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId;
+					InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+                    if (body && body->m_multiBody)
                     {
-                        btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
+                        btMultiBody* mb = body->m_multiBody;
                         btAssert(mb);
                         for (int i=0;i<clientCmd.m_createSensorArguments.m_numJointSensorChanges;i++)
                         {
@@ -767,9 +938,13 @@ void PhysicsServerSharedMemory::processClientCommands()
 						{
                             b3Printf("Processed CMD_SEND_DESIRED_STATE");
 						}
-                            if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
+						
+							int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
+							InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+
+                            if (body && body->m_multiBody)
                             {
-                                btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
+                                btMultiBody* mb = body->m_multiBody;
                                 btAssert(mb);
                                 
                                 switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
@@ -813,9 +988,7 @@ void PhysicsServerSharedMemory::processClientCommands()
                                         
 										int numMotors = 0;
 										//find the joint motors and apply the desired velocity and maximum force/torque
-										if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
 										{
-											btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
 											int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base
 											for (int link=0;link<mb->getNumLinks();link++)
 											{
@@ -851,9 +1024,7 @@ void PhysicsServerSharedMemory::processClientCommands()
 
 										int numMotors = 0;
 										//find the joint motors and apply the desired velocity and maximum force/torque
-										if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
 										{
-											btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
 											int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base
 											int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
 											for (int link=0;link<mb->getNumLinks();link++)
@@ -916,15 +1087,23 @@ void PhysicsServerSharedMemory::processClientCommands()
 						{
 							b3Printf("Sending the actual state (Q,U)");
 						}
-						if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
+						int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
+						InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+						btMultiBody* mb = 0;
+						if (body)
 						{
-							btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
+							mb = body->m_multiBody;
+						}
+						if (mb)
+						{
+							
 							SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
 
 							serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
 							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())"
@@ -934,20 +1113,20 @@ void PhysicsServerSharedMemory::processClientCommands()
 								tr.setRotation(mb->getWorldToBaseRot().inverse());
 								
                                 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] =
-                                    m_data->m_rootLocalInertialFrame.getOrigin()[0];
+                                    body->m_rootLocalInertialFrame.getOrigin()[0];
                                 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] =
-                                    m_data->m_rootLocalInertialFrame.getOrigin()[1];
+                                    body->m_rootLocalInertialFrame.getOrigin()[1];
                                 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] =
-                                    m_data->m_rootLocalInertialFrame.getOrigin()[2];
+                                    body->m_rootLocalInertialFrame.getOrigin()[2];
 
                                 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] =
-                                    m_data->m_rootLocalInertialFrame.getRotation()[0];
+                                    body->m_rootLocalInertialFrame.getRotation()[0];
                                 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] =
-                                    m_data->m_rootLocalInertialFrame.getRotation()[1];
+                                    body->m_rootLocalInertialFrame.getRotation()[1];
                                 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] =
-                                    m_data->m_rootLocalInertialFrame.getRotation()[2];
+                                    body->m_rootLocalInertialFrame.getRotation()[2];
                                 serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
-                                    m_data->m_rootLocalInertialFrame.getRotation()[3];
+                                    body->m_rootLocalInertialFrame.getRotation()[3];
 
 
                                 
@@ -1067,10 +1246,12 @@ void PhysicsServerSharedMemory::processClientCommands()
 					{
 						b3Printf("Server Init Pose not implemented yet");
 					}
-					int body_unique_id = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
-					if (m_data->m_dynamicsWorld->getNumMultibodies()>body_unique_id)
+					int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
+					InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+
+					if (body && body->m_multiBody)
 					{
-						btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(body_unique_id);
+						btMultiBody* mb = body->m_multiBody;
 						mb->setBasePos(btVector3(
 								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0],
 								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1],
diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h
index 80d8422ea..96011606a 100644
--- a/examples/SharedMemory/PhysicsServer.h
+++ b/examples/SharedMemory/PhysicsServer.h
@@ -25,7 +25,7 @@ protected:
 	void	releaseSharedMemory();
 	
 	bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
-                             bool useMultiBody, bool useFixedBase);
+                             bool useMultiBody, bool useFixedBase, int* bodyUniqueId);
 
 public:
 	PhysicsServerSharedMemory();
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index 68de98e67..02090d698 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -170,7 +170,9 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 {
 	btClock rtc;
 	btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
-	while (rtc.getTimeMilliseconds()<endTime)
+	int maxSteps = 10;
+
+	while (maxSteps-- && rtc.getTimeMilliseconds()<endTime)
 	{
 		m_physicsServer.processClientCommands();
 	}
diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c
index 68f1f884a..1c7fd4cfb 100644
--- a/test/SharedMemory/test.c
+++ b/test/SharedMemory/test.c
@@ -1,5 +1,6 @@
 //#include "SharedMemoryCommands.h"
 #include "PhysicsClientC_API.h"
+#include "SharedMemoryPublic.h"
 #include "Bullet3Common/b3Logging.h"
 #include <string.h>
 
@@ -24,7 +25,8 @@ int main(int argc, char* argv[])
 
 	
 	b3PhysicsClientHandle sm;
-	
+	int bodyIndex = -1;
+
 	printf("hello world\n");
 
 	sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
@@ -37,62 +39,68 @@ int main(int argc, char* argv[])
 		b3SubmitClientCommandAndWaitStatus(sm, command);
         }
 
+		
         {
-            b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
+            b3SharedMemoryStatusHandle statusHandle;
+			b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
+			
             //setting the initial position, orientation and other arguments are optional
             startPosX =2;
             startPosY =3;
             startPosZ = 1;
             ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
-            b3SubmitClientCommandAndWaitStatus(sm, command);
-        }
-        
-		numJoints = b3GetNumJoints(sm);
-        for (i=0;i<numJoints;i++)
-        {
-            struct b3JointInfo jointInfo;
-            b3GetJointInfo(sm,i,&jointInfo);
-            
-            printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
-            //pick the IMU link index based on torso name
-            if (strstr(jointInfo.m_linkName,"base_link"))
-            {
-                imuLinkIndex = i;
-            }
-            
-            //pick the joint index based on joint name
-            if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
-            {
-                sensorJointIndexLeft = i;
-            }
-            if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
-            {
-                sensorJointIndexRight = i;
-            }
-            
-        }
-        
-        if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
-        {
-            b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
-            b3SharedMemoryStatusHandle statusHandle;
-            if (imuLinkIndex>=0)
-            {
-				 ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
-            }
-            
-            if (sensorJointIndexLeft>=0)
-            {
-			  ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
-            }
-            if(sensorJointIndexRight>=0)
-            {
-                ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
-            }
             statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-            
+			bodyIndex = b3GetStatusBodyIndex(statusHandle);
         }
         
+		if (bodyIndex>=0)
+		{
+			numJoints = b3GetNumJoints(sm,bodyIndex);
+			for (i=0;i<numJoints;i++)
+			{
+				struct b3JointInfo jointInfo;
+				b3GetJointInfo(sm,bodyIndex, i,&jointInfo);
+            
+				printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
+				//pick the IMU link index based on torso name
+				if (strstr(jointInfo.m_linkName,"base_link"))
+				{
+					imuLinkIndex = i;
+				}
+            
+				//pick the joint index based on joint name
+				if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
+				{
+					sensorJointIndexLeft = i;
+				}
+				if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
+				{
+					sensorJointIndexRight = i;
+				}
+            
+			}
+        
+			if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
+			{
+				b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
+				b3SharedMemoryStatusHandle statusHandle;
+				if (imuLinkIndex>=0)
+				{
+					 ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
+				}
+            
+				if (sensorJointIndexLeft>=0)
+				{
+				  ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
+				}
+				if(sensorJointIndexRight>=0)
+				{
+					ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
+				}
+				statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+            
+			}
+		}
         
         {
             b3SharedMemoryStatusHandle statusHandle;
@@ -124,6 +132,7 @@ int main(int argc, char* argv[])
         }
         
         {
+#if 0
             b3SharedMemoryStatusHandle statusHandle;
              b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
             for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
@@ -132,7 +141,7 @@ int main(int argc, char* argv[])
                 b3JointControlSetMaximumForce(command,dofIndex,100);
             }
              statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-
+#endif
         }
         ///perform some simulation steps for testing
         for ( i=0;i<100;i++)
@@ -145,23 +154,32 @@ int main(int argc, char* argv[])
         }
         
         
-#if 0
+
         if (sensorJointIndexLeft>=0)
         {
+
+			struct  b3JointSensorState info;
+			
+			b3GetJointInfo(sm,bodyIndex,sensorJointIndexLeft,&info);
             b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
-                status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+0],
-                status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+1],
-                status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+2]);
+                info.m_jointForceTorque[0],
+                info.m_jointForceTorque[1],
+                info.m_jointForceTorque[2]);
+
         }
         
         if (sensorJointIndexRight>=0)
         {
+			struct  b3JointSensorState info;
+			
+			b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&info);
             b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
-                     status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+0],
-                     status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1],
-                     status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]);
+                     info.m_jointForceTorque[0],
+                     info.m_jointForceTorque[1],
+                     info.m_jointForceTorque[2]);
+
         }
-#endif
+
         {
             b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
         }