Merge remote-tracking branch 'upstream/master'

2016-06-27 15:14:47 +02:00
parent e16082a7cd ed9be25570
commit 50b6003470
65 changed files with 2993 additions and 616 deletions
--- a/.travis.yml
+++ b/.travis.yml
@@ -5,10 +5,14 @@ os:
 compiler:
  - gcc
  - clang
+addons:
+  apt:
+    packages:
+      - python3
 script:
  - echo "CXX="$CXX
  - echo "CC="$CC
-  - cmake . -G "Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
+  - cmake . -DBUILD_PYBULLET=ON -G"Unix Makefiles" #-DCMAKE_CXX_FLAGS=-Werror
  - make -j8
  - ctest -j8 --output-on-failure
  # Build again with double precision
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -191,14 +191,15 @@ IF (APPLE)
 ENDIF()

 OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON)
+
+FIND_PACKAGE(PythonLibs)
 OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF)

+
 IF(BUILD_PYBULLET)
 	IF(WIN32)
-		FIND_PACKAGE(PythonLibs 3.4 REQUIRED)
 		SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE)
 	ELSE(WIN32)
-		FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
 		SET(BUILD_SHARED_LIBS ON CACHE BOOL "Shared Libs" FORCE)
 	ENDIF(WIN32)
 ENDIF(BUILD_PYBULLET)
--- a/README.md
+++ b/README.md
@@ -34,7 +34,7 @@ The entire collision detection and rigid body dynamics is executed on the GPU.
 A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
 We succesfully tested the software under Windows, Linux and Mac OSX.
 The software currently doesn't work on OpenCL CPU devices. It might run
-on a laptop GPU but performance is likely not very good. Note that
+on a laptop GPU but performance will not likely be very good. Note that
 often an OpenCL drivers fails to compile a kernel. Some unit tests exist to
 track down the issue, but more work is required to cover all OpenCL kernels.

@@ -88,7 +88,7 @@ You can just run it though a terminal/command prompt, or by clicking it.
 [--fixed_timestep= 0.0]             Use either a real-time delta time (0.0) or a fixed step size (0.016666)
 ```

-You can use mouse picking to grab objects. When holding the ALT of CONTROL key, you have Maya style camera mouse controls.
-Press F1 to create series of screenshot. Hit ESCAPE to exit the demo app.
+You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
+Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.

 See docs folder for further information.
--- a/build_and_run_cmake.sh
+++ b/build_and_run_cmake.sh
@@ -2,6 +2,6 @@
 rm CMakeCache.txt
 mkdir build_cmake
 cd build_cmake
-cmake ..
+cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release ..
 make -j12
 examples/ExampleBrowser/App_ExampleBrowser
--- a/data/cube.mtl
+++ b/data/cube.mtl
@@ -10,5 +10,5 @@ newmtl cube
  Ks 0.0000 0.0000 0.0000
  Ke 0.0000 0.0000 0.0000
  map_Ka cube.tga
-  map_Kd floor_diffuse.tga
+  map_Kd cube.png

--- a/data/kuka_iiwa/model.sdf
+++ b/data/kuka_iiwa/model.sdf
@@ -32,6 +32,12 @@
            <uri>meshes/link_0.stl</uri>
          </mesh>
        </geometry>
+        <material>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>0 0 1 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+          <emissive>0 0 0 0</emissive>
+        </material>
      </visual>
    </link>
    <link name='lbr_iiwa_link_1'>
@@ -407,4 +413,4 @@
    </joint>
  </model>
 </world>
-</sdf>
+</sdf>
--- a/data/sphere2.urdf
+++ b/data/sphere2.urdf
@@ -9,7 +9,7 @@
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
-      <sphere radius="0.5"/>
+<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>      
      </geometry>
    </visual>
    <collision>
--- a/data/two_cubes.sdf
+++ b/data/two_cubes.sdf
@@ -86,6 +86,7 @@
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
    <model name='unit_box_0'>
+ <static>1</static>
      <pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
      <link name='unit_box_0::link'>
        <inertial>
@@ -121,11 +122,13 @@
        </collision>
        <visual name='visual'>
          <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
-          </geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>cube.obj</uri>
+          </mesh>
+        </geometry>
          <material>
+            <diffuse>1 1 1 1</diffuse>
            <script>
              <name>Gazebo/Grey</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
@@ -171,17 +174,15 @@
            </friction>
          </surface>
        </collision>
-        <visual name='visual'>
+         <visual name='visual'>
          <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
-          </geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <uri>cube.obj</uri>
+          </mesh>
+        </geometry>
          <material>
-            <script>
-              <name>Gazebo/Grey</name>
-              <uri>file://media/materials/scripts/gazebo.material</uri>
-            </script>
+            <diffuse>1 1 1 1</diffuse>
          </material>
        </visual>
        <self_collide>0</self_collide>
--- a/examples/BasicDemo/BasicExample.cpp
+++ b/examples/BasicDemo/BasicExample.cpp
@@ -42,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
 		float dist = 41;
 		float pitch = 52;
 		float yaw = 35;
-		float targetPos[3]={0,0.46,0};
+		float targetPos[3]={0,0,0};
 		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
 	}
 };
--- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
+++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp
@@ -167,6 +167,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
 							}
 						}
 					}
+					delete hull;
 				}
 			} else
 			{
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -245,7 +245,10 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
 			PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),

-	ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
+	ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
+	ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
+
+		


 #ifdef ENABLE_LUA
--- a/examples/ExampleBrowser/main.cpp
+++ b/examples/ExampleBrowser/main.cpp
@@ -13,6 +13,8 @@
 #include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
 #include "../Importers/ImportURDFDemo/ImportURDFSetup.h"
 #include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
+#include "../Importers/ImportSTLDemo/ImportSTLSetup.h"
+


 int main(int argc, char* argv[])
@@ -29,6 +31,7 @@ int main(int argc, char* argv[])
 	exampleBrowser->registerFileImporter(".urdf",ImportURDFCreateFunc);
 	exampleBrowser->registerFileImporter(".sdf",ImportSDFCreateFunc);
 	exampleBrowser->registerFileImporter(".obj",ImportObjCreateFunc);
+	exampleBrowser->registerFileImporter(".stl",ImportSTLCreateFunc);
 	
 	clock.reset();
 	if (init)
--- a/examples/ExtendedTutorials/premake4.lua
+++ b/examples/ExtendedTutorials/premake4.lua
@@ -38,7 +38,8 @@ files {
 			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-			
+			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",
 }


@@ -89,7 +90,8 @@ files {
 			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
+			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",
 }

 if os.is("Linux") then initX11() end
@@ -153,7 +155,8 @@ files {
 			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
+			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",     
 }

 if os.is("Linux") then initX11() end
@@ -213,6 +216,7 @@ files {
 			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
+			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",     
 }

--- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
@@ -1,15 +1,12 @@
 #include "b3ImportMeshUtility.h"

 #include <vector>
-#include "../../OpenGLWindow/GLInstancingRenderer.h"
-#include"Wavefront/tiny_obj_loader.h"
-#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
-#include "btBulletDynamicsCommon.h"
-#include "../../OpenGLWindow/SimpleOpenGL3App.h"
+#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+#include "LinearMath/btVector3.h"
 #include "../ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h"
 #include "../../Utils/b3ResourcePath.h"
 #include "Bullet3Common/b3FileUtils.h"
-#include "stb_image/stb_image.h"
+#include "../../ThirdPartyLibs/stb_image/stb_image.h"


 bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
@@ -88,28 +85,4 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
 	return false;
 }

-int b3ImportMeshUtility::loadAndRegisterMeshFromFile(const std::string& fileName, CommonRenderInterface* renderer)
-{
-	int shapeId = -1;

-	b3ImportMeshData meshData;
-	if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
-	{
-		int textureIndex = -1;
-
-		if (meshData.m_textureImage)
-		{
-			textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
-		}
-
-		shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], 
-									meshData.m_gfxShape->m_numvertices, 
-									&meshData.m_gfxShape->m_indices->at(0), 
-									meshData.m_gfxShape->m_numIndices,
-									B3_GL_TRIANGLES,
-									textureIndex);
-		delete meshData.m_gfxShape;
-		delete meshData.m_textureImage;
-	}
-	return shapeId;
-}
--- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h
+++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.h
@@ -15,7 +15,6 @@ struct b3ImportMeshData
 class b3ImportMeshUtility
 {
 public:
-static int loadAndRegisterMeshFromFile(const std::string& fileName, class CommonRenderInterface* renderer);

 static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);

--- a/examples/Importers/ImportObjDemo/ImportObjExample.cpp
+++ b/examples/Importers/ImportObjDemo/ImportObjExample.cpp
@@ -56,7 +56,31 @@ ImportObjSetup::~ImportObjSetup()



-
+int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterface* renderer)
+{
+	int shapeId = -1;
+	
+	b3ImportMeshData meshData;
+	if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
+	{
+		int textureIndex = -1;
+		
+		if (meshData.m_textureImage)
+		{
+			textureIndex = renderer->registerTexture(meshData.m_textureImage,meshData.m_textureWidth,meshData.m_textureHeight);
+		}
+		
+		shapeId = renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], 
+										  meshData.m_gfxShape->m_numvertices, 
+										  &meshData.m_gfxShape->m_indices->at(0), 
+										  meshData.m_gfxShape->m_numIndices,
+										  B3_GL_TRIANGLES,
+										  textureIndex);
+		delete meshData.m_gfxShape;
+		delete meshData.m_textureImage;
+	}
+	return shapeId;
+}



@@ -77,7 +101,7 @@ void ImportObjSetup::initPhysics()
    btVector3 scaling(1,1,1);
 	btVector3 color(1,1,1);
 		
-   int shapeId = b3ImportMeshUtility::loadAndRegisterMeshFromFile(m_fileName, m_guiHelper->getRenderInterface());    
+   int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());    
   if (shapeId>=0)
   {
        //int id = 
--- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
+++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
@@ -235,6 +235,60 @@ void ImportSDFSetup::initPhysics()
 			ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true);
 			mb = creation.getBulletMultiBody();
 			
+			
+			if (m_useMultiBody && mb )
+			{
+				std::string*   name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
+				m_nameMemory.push_back(name);
+#ifdef TEST_MULTIBODY_SERIALIZATION
+				s->registerNameForPointer(name->c_str(),name->c_str());
+#endif//TEST_MULTIBODY_SERIALIZATION
+				mb->setBaseName(name->c_str());
+				//create motors for each btMultiBody joint
+				int numLinks = mb->getNumLinks();
+				for (int i=0;i<numLinks;i++)
+				{
+					int mbLinkIndex = i;
+					int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
+
+					std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
+					std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
+#ifdef TEST_MULTIBODY_SERIALIZATION					
+					s->registerNameForPointer(jointName->c_str(),jointName->c_str());
+					s->registerNameForPointer(linkName->c_str(),linkName->c_str());
+#endif//TEST_MULTIBODY_SERIALIZATION
+					m_nameMemory.push_back(jointName);
+					m_nameMemory.push_back(linkName);
+
+					mb->getLink(i).m_linkName = linkName->c_str();
+					mb->getLink(i).m_jointName = jointName->c_str();
+							
+					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
+					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
+					)
+					{
+						if (m_data->m_numMotors<MAX_NUM_MOTORS)
+						{
+							
+							char motorName[1024];
+							sprintf(motorName,"%s q'", jointName->c_str());
+							btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
+							*motorVel = 0.f;
+							SliderParams slider(motorName,motorVel);
+							slider.m_minVal=-4;
+							slider.m_maxVal=4;
+							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+							float maxMotorImpulse = 10.1f;
+							btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
+							//motor->setMaxAppliedImpulse(0);
+							m_data->m_jointMotors[m_data->m_numMotors]=motor;
+							m_dynamicsWorld->addMultiBodyConstraint(motor);
+							m_data->m_numMotors++;
+						}
+					}
+
+				}
+			} 
 		}

 	
--- a/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
+++ b/examples/Importers/ImportSTLDemo/ImportSTLSetup.cpp
@@ -13,9 +13,12 @@

 class ImportSTLSetup : public CommonRigidBodyBase
 {
+
+	const char* m_fileName;
+	btVector3 m_scaling;
 	
 public:
-    ImportSTLSetup(struct GUIHelperInterface* helper);
+    ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName);
    virtual ~ImportSTLSetup();
    
 	virtual void initPhysics();
@@ -31,10 +34,19 @@ public:
 };


-ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper)
-:CommonRigidBodyBase(helper)
+ImportSTLSetup::ImportSTLSetup(struct GUIHelperInterface* helper, const char* fileName)
+:CommonRigidBodyBase(helper),
+m_scaling(btVector3(10,10,10))
 {
-    
+	if (fileName)
+	{
+		m_fileName = fileName;
+		m_scaling = btVector3(0.01,0.01,0.01);
+	} else
+	{
+		m_fileName = "l_finger_tip.stl";
+
+	}
 }

 ImportSTLSetup::~ImportSTLSetup()
@@ -51,17 +63,16 @@ void ImportSTLSetup::initPhysics()
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);

-	const char* fileName = "l_finger_tip.stl";
+	
    char relativeFileName[1024];
-  if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
+  if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
        {
-                b3Warning("Cannot find file %s\n", fileName);
+                b3Warning("Cannot find file %s\n", m_fileName);
                return;
        }

 	
 	btVector3 shift(0,0,0);
-	btVector3 scaling(10,10,10);
 //	int index=10;
 	
 	{
@@ -81,12 +92,12 @@ void ImportSTLSetup::initPhysics()
 		
 		
 	
-		m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+		m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId,position,orn,color,m_scaling);

 	}
 }

 class CommonExampleInterface*    ImportSTLCreateFunc(struct CommonExampleOptions& options)
 {
-	return new ImportSTLSetup(options.m_guiHelper);
+	return new ImportSTLSetup(options.m_guiHelper, options.m_fileName);
 }
--- a/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
+++ b/examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
@@ -66,17 +66,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
 						for (int i=0;i<numTriangles;i++)
 						{
 							char* curPtr = &memoryBuffer[84+i*50];
-							MySTLTriangle* tri = (MySTLTriangle*) curPtr;
+							MySTLTriangle tmp;
+							memcpy(&tmp,curPtr,sizeof(MySTLTriangle));
 							
 							GLInstanceVertex v0,v1,v2;
 							v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
 							v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
 							for (int v=0;v<3;v++)
 							{
-								v0.xyzw[v] = tri->vertex0[v];
-								v1.xyzw[v] = tri->vertex1[v];
-								v2.xyzw[v] = tri->vertex2[v];
-								v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
+								v0.xyzw[v] = tmp.vertex0[v];
+								v1.xyzw[v] = tmp.vertex1[v];
+								v2.xyzw[v] = tmp.vertex2[v];
+								v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v];
 							}
 							v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
 							
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -13,7 +13,7 @@ subject to the following restrictions:


 #include "BulletUrdfImporter.h"
-
+#include "../../CommonInterfaces/CommonRenderInterface.h"

 #include "URDFImporterInterface.h"
 #include "btBulletCollisionCommon.h"
@@ -26,12 +26,20 @@ subject to the following restrictions:
 #include <string>
 #include "../../Utils/b3ResourcePath.h"

+#include "../ImportMeshUtility/b3ImportMeshUtility.h"


 #include <iostream>
 #include <fstream>
 #include "UrdfParser.h"

+struct MyTexture
+{
+	int m_width;
+	int m_height;
+	unsigned char* textureData;
+};
+
 struct BulletURDFInternalData
 {
 	UrdfParser m_urdfParser;
@@ -267,14 +275,25 @@ void  BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
 	//the link->m_inertia is NOT necessarily aligned with the inertial frame
 	//so an additional transform might need to be computed
 	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
+	
+	
 	btAssert(linkPtr);
 	if (linkPtr)
 	{
 		UrdfLink* link = *linkPtr;
-		mass = link->m_inertia.m_mass;
+		if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
+		{
+			mass = 0.f;
+			localInertiaDiagonal.setValue(0,0,0);
+		}
+		else
+		{
+			mass = link->m_inertia.m_mass;
+			localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
+										  link->m_inertia.m_izz);
+		}
 		inertialFrame = link->m_inertia.m_linkLocalFrame;
-		localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
-									  link->m_inertia.m_izz);
+		
 	}
 	else
    {
@@ -578,7 +597,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 }


-static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
+static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture>& texturesOut)
 {

 	
@@ -682,7 +701,23 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 						{
                            case FILE_OBJ:
                            {
-                                glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+//                                glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+						
+								b3ImportMeshData meshData;
+								if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
+								{
+									
+									if (meshData.m_textureImage)
+									{
+										MyTexture texData;
+										texData.m_width = meshData.m_textureWidth;
+										texData.m_height = meshData.m_textureHeight;
+										texData.textureData = meshData.m_textureImage;
+										texturesOut.push_back(texData);
+									}
+									glmesh = meshData.m_gfxShape;
+								}
+								
                                break;
                            }
                           
@@ -903,7 +938,8 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
    btAlignedObjectArray<GLInstanceVertex> vertices;
 	btAlignedObjectArray<int> indices;
 	btTransform startTrans; startTrans.setIdentity();
-
+	btAlignedObjectArray<MyTexture> textures;
+	
    const UrdfModel& model = m_data->m_urdfParser.getModel();
 	UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
 	if (linkPtr)
@@ -923,14 +959,34 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
 				//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
 				m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
 			}
-			convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices);
+			convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
 		
 		
 		}
 	}
 	if (vertices.size() && indices.size())
 	{
-		graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+//		graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+		//graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
+		
+		CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
+		
+		if (renderer)
+		{
+			int textureIndex = -1;
+			if (textures.size())
+			{
+				textureIndex = renderer->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
+			}
+			graphicsIndex = renderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex);
+			
+		}
+	}
+	
+	//delete textures
+	for (int i=0;i<textures.size();i++)
+	{
+		delete textures[i].textureData;
 	}
 	return graphicsIndex;
 }
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
@@ -128,7 +128,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gFileNameArray.size()==0)
 		{
-			gFileNameArray.push_back("r2d2.urdf");
+			gFileNameArray.push_back("sphere2.urdf");

 		}

--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -393,13 +393,22 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
          }
          
          geom.m_meshFileName = shape->Attribute("filename");
-          
-          if (shape->Attribute("scale"))
+		  geom.m_meshScale.setValue(1,1,1);
+
+		  if (shape->Attribute("scale"))
          {
-              parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger);
+              if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger))
+			  {
+				  logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n");
+				  std::string scalar_str = shape->Attribute("scale");
+				  double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
+				  if (scaleFactor)
+				  {
+					  geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor);
+				  }
+			  }
          } else
          {
-              geom.m_meshScale.setValue(1,1,1);
          }
      }
  }
@@ -490,29 +499,47 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
  // Material
  TiXmlElement *mat = config->FirstChildElement("material");
 //todo(erwincoumans) skip materials in SDF for now (due to complexity)
-  if (mat && !m_parseSDF)
+  if (mat)
  {
-	  // get material name
-	  if (!mat->Attribute("name")) 
-	  {
-		  logger->reportError("Visual material must contain a name attribute");
-		  return false;
-	  }
-	  visual.m_materialName = mat->Attribute("name");
-	  
-	  // try to parse material element in place
-	  
-	  TiXmlElement *t = mat->FirstChildElement("texture");
-	  TiXmlElement *c = mat->FirstChildElement("color");
-	  if (t||c)
-	  {
-		  if (parseMaterial(visual.m_localMaterial, mat,logger))
-		  {
-			  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
-			  model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
-			  visual.m_hasLocalMaterial = true;
-		  }
-	  }
+    if (m_parseSDF)
+    {
+        UrdfMaterial* matPtr = new UrdfMaterial;
+        matPtr->m_name = "mat";
+        TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
+        if (diffuse) {
+            std::string diffuseText = diffuse->GetText();
+            btVector4 rgba(1,0,0,1);
+            parseVector4(rgba,diffuseText);
+            matPtr->m_rgbaColor = rgba;
+            model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
+            visual.m_materialName = "mat";
+            visual.m_hasLocalMaterial = true;
+        }
+    } 
+    else
+      {
+          // get material name
+          if (!mat->Attribute("name")) 
+          {
+              logger->reportError("Visual material must contain a name attribute");
+              return false;
+          }
+          visual.m_materialName = mat->Attribute("name");
+          
+          // try to parse material element in place
+          
+          TiXmlElement *t = mat->FirstChildElement("texture");
+          TiXmlElement *c = mat->FirstChildElement("color");
+          if (t||c)
+          {
+              if (parseMaterial(visual.m_localMaterial, mat,logger))
+              {
+                  UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
+                  model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
+                  visual.m_hasLocalMaterial = true;
+              }
+          }
+      }
  }
  
  return true;
@@ -529,6 +556,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
 	link.m_name = linkName;
    
    if (m_parseSDF) {
+
+
        TiXmlElement* pose = config->FirstChildElement("pose");
        if (0==pose)
        {
@@ -572,7 +601,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
          logger->reportWarning(link.m_name.c_str());
      }
  }
-		
+
  // Multiple Visuals (optional)
  for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
  {
@@ -1240,6 +1269,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
        UrdfModel* localModel = new UrdfModel;
        m_tmpModels.push_back(localModel);
        
+		TiXmlElement* stat = robot_xml->FirstChildElement("static");
+        if (0!=stat)
+        {
+			int val = int(atof(stat->GetText()));
+			if (val==1)
+			{
+				localModel->m_overrideFixedBase = true;
+			}
+		}
+
        
        // Get robot name
        const char *name = robot_xml->Attribute("name");
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -134,6 +134,13 @@ struct UrdfModel
 	btHashMap<btHashString, UrdfJoint*> m_joints;
 	
 	btArray<UrdfLink*> m_rootLinks;
+	bool m_overrideFixedBase;
+
+	UrdfModel()
+		:m_overrideFixedBase(false)
+	{
+		m_rootTransformInWorld.setIdentity();
+	}
 	
 };

--- a/examples/InverseDynamics/premake4.lua
+++ b/examples/InverseDynamics/premake4.lua
@@ -38,7 +38,8 @@ files {
 			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-			
+			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",	
 }


@@ -89,7 +90,8 @@ files {
 			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
+			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",     
 }

 if os.is("Linux") then initX11() end
@@ -153,7 +155,8 @@ files {
 			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
+			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",     
 }

 if os.is("Linux") then initX11() end
@@ -211,6 +214,7 @@ files {
 			"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
-
+			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",     
 }
               
--- a/examples/OpenGLWindow/MacOpenGLWindow.mm
+++ b/examples/OpenGLWindow/MacOpenGLWindow.mm
@@ -102,11 +102,11 @@ float loop;
 }
 -(void)drawRect:(NSRect)rect
 {
+	
 	if (([self frame].size.width != m_lastWidth) || ([self frame].size.height != m_lastHeight))
 	{
 		m_lastWidth = [self frame].size.width;
 		m_lastHeight = [self frame].size.height;
-		
 		// Only needed on resize:
 		[m_context clearDrawable];
 		
@@ -114,7 +114,6 @@ float loop;
        float width = [self frame].size.width;
        float height = [self frame].size.height;
        
-        
        // Get view dimensions in pixels
     //   glViewport(0,0,10,10);
        
@@ -209,16 +208,12 @@ struct MacOpenGLWindowInternalData
        m_myview = 0;
        m_pool = 0;
        m_window = 0;
-        m_width = -1;
-        m_height = -1;
        m_exitRequested = false;
    }
    NSApplication*      m_myApp;
    TestView*             m_myview;
    NSAutoreleasePool*  m_pool;
    NSWindow*           m_window;
-    int m_width;
-    int m_height;
    bool m_exitRequested;
    
 };
@@ -294,8 +289,6 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
    if (m_internalData)
        closeWindow();

-    int width = ci.m_width;
-	int height = ci.m_height;
 	const char* windowTitle = ci.m_title;
 	

@@ -303,9 +296,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
    NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
    
    m_internalData = new MacOpenGLWindowInternalData;
-    m_internalData->m_width = width;
-    m_internalData->m_height = height;
-    
+  
    m_internalData->m_pool = [NSAutoreleasePool new];
 	m_internalData->m_myApp = [NSApplication sharedApplication];
 	//myApp = [MyApp sharedApplication];
@@ -373,7 +364,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
    [newItem release];
    */
    
-	NSRect frame = NSMakeRect(0., 0., width, height);
+	NSRect frame = NSMakeRect(0., 0., ci.m_width, ci.m_height);
 	
 	m_internalData->m_window = [NSWindow alloc];
 	[m_internalData->m_window initWithContentRect:frame
@@ -423,9 +414,7 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
    [m_internalData->m_window makeKeyAndOrderFront: nil];
    
    [m_internalData->m_myview MakeCurrent];
-    m_internalData->m_width = m_internalData->m_myview.GetWindowWidth;
-    m_internalData->m_height = m_internalData->m_myview.GetWindowHeight;
-    
+	
    
    [NSApp activateIgnoringOtherApps:YES];
    
@@ -1035,13 +1024,13 @@ void MacOpenGLWindow::startRendering()
    float aspect;
    //b3Vector3 extents;
    
-    if (m_internalData->m_width > m_internalData->m_height)
+    if (getWidth()  > getHeight())
    {
-        aspect = (float)m_internalData->m_width / (float)m_internalData->m_height;
+        aspect = (float)getWidth() / (float)getHeight();
        //extents.setValue(aspect * 1.0f, 1.0f,0);
    } else
    {
-        aspect = (float)m_internalData->m_height / (float)m_internalData->m_width;
+        aspect = (float)getHeight() / (float)getWidth();
        //extents.setValue(1.0f, aspect*1.f,0);
    }
    
@@ -1082,7 +1071,7 @@ int MacOpenGLWindow::fileOpenDialog(char* filename, int maxNameLength)
    NSOpenGLContext *foo = [NSOpenGLContext currentContext];
    // get the url of a .txt file
    NSOpenPanel * zOpenPanel = [NSOpenPanel openPanel];
-	NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",nil];
+	NSArray * zAryOfExtensions = [NSArray arrayWithObjects:@"urdf",@"bullet",@"obj",@"sdf",@"stl",nil];
    [zOpenPanel setAllowedFileTypes:zAryOfExtensions];
    NSInteger zIntResult = [zOpenPanel runModal];
    
@@ -1136,6 +1125,7 @@ int   MacOpenGLWindow::getWidth() const
 {
    if (m_internalData && m_internalData->m_myview && m_internalData->m_myview.GetWindowWidth)
        return m_internalData->m_myview.GetWindowWidth;
+
    return 0;
 }

@@ -1152,7 +1142,7 @@ void MacOpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback)
    [m_internalData->m_myview setResizeCallback:resizeCallback];
    if (resizeCallback)
    {
-        (resizeCallback)(m_internalData->m_width,m_internalData->m_height);
+		(resizeCallback)(getWidth(), getHeight());
    }
 }

--- a/examples/RenderingExamples/TinyRendererSetup.cpp
+++ b/examples/RenderingExamples/TinyRendererSetup.cpp
@@ -147,7 +147,9 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
 	m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight());
 	
 	m_app->m_renderer->enableBlend(true);
-	const char* fileName = "teddy.obj";//cube.obj";//textured_sphere_smooth.obj";//cube.obj";
+    
+	const char* fileName = "textured_sphere_smooth.obj";
+    fileName = "cube.obj";
 	

 	{
@@ -181,7 +183,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
 				m_guiHelper->getRenderInterface()->writeTransforms();

 				m_internalData->m_shapePtr.push_back(0);
-				TinyRenderObjectData* ob = new TinyRenderObjectData(m_internalData->m_width,m_internalData->m_height,
+				TinyRenderObjectData* ob = new TinyRenderObjectData(
 					m_internalData->m_rgbColorBuffer,
 					m_internalData->m_depthBuffer);
 					//ob->loadModel("cube.obj");
--- a/examples/SharedMemory/PhysicsClient.h
+++ b/examples/SharedMemory/PhysicsClient.h
@@ -26,7 +26,7 @@ public:

    virtual int getNumJoints(int bodyIndex) const = 0;

-    virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
+    virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;

    virtual void setSharedMemoryKey(int key) = 0;

--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -1,10 +1,34 @@
 #include "PhysicsClientC_API.h"
 #include "PhysicsClientSharedMemory.h"
 #include "Bullet3Common/b3Scalar.h"
+#include "Bullet3Common/b3Vector3.h"
 #include <string.h>
 #include "SharedMemoryCommands.h"


+b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+	command->m_type = CMD_LOAD_SDF;
+	int len = strlen(sdfFileName);
+	if (len<MAX_SDF_FILENAME_LENGTH)
+	{
+		strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
+	} else
+	{
+		command->m_sdfArguments.m_sdfFileName[0] = 0;
+	}
+	command->m_updateFlags = SDF_ARGS_FILE_NAME;
+	
+	return (b3SharedMemoryCommandHandle) command;
+}
+
+

 b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
 {
@@ -127,7 +151,12 @@ b3SharedMemoryCommandHandle     b3InitResetSimulationCommand(b3PhysicsClientHand
 }


-b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode)
+b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
+{
+    return b3JointControlCommandInit2(physClient,0,controlMode);
+}
+
+b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
 {
    PhysicsClient* cl = (PhysicsClient* ) physClient;
    b3Assert(cl);
@@ -136,8 +165,12 @@ 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_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
 	command->m_updateFlags = 0;
+    for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+    {
+        command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
+    }
    return (b3SharedMemoryCommandHandle) command;
 }

@@ -146,6 +179,9 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
    b3Assert(command);
    command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
+
    return 0;
 }

@@ -154,6 +190,9 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
    b3Assert(command);
    command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
+
    return 0;
 }

@@ -162,6 +201,9 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
    b3Assert(command);
    command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
+
    return 0;
 }

@@ -170,6 +212,9 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
    b3Assert(command);
    command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
+
    return 0;
 }

@@ -179,6 +224,9 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
    b3Assert(command);
    command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
+	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+
    return 0;
 }

@@ -187,6 +235,9 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
    b3Assert(command);
    command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
+    command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+
    return 0;
 }

@@ -335,6 +386,7 @@ int	b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan

 b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
 {
+    
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
    b3Assert(cl);
    b3Assert(cl->canSubmitCommand());
@@ -343,6 +395,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl
    command->m_type = CMD_INIT_POSE;
    command->m_updateFlags =0;
 	command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
+	//a bit slow, initialing the full range to zero...
+	for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+    {
+        command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
+    }
    return (b3SharedMemoryCommandHandle) command;
 }

@@ -355,6 +412,11 @@ int	b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle
 	command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
 	command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
 	command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
+	
+	command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
+	
 	return 0;
 }

@@ -368,6 +430,12 @@ int	b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
 	command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
 	command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
 	command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
+	
+	command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
+	
 	return 0;
 }

@@ -380,6 +448,7 @@ int	b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
 	for (int i=0;i<numJointPositions;i++)
 	{
 		command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
+		command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
 	}
 	return 0;
 }
@@ -396,6 +465,7 @@ int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
 	if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
 	{  
 		command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
+		command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
 	}
 	return 0;
 }
@@ -403,7 +473,7 @@ int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar



-b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient)
+b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {
    PhysicsClient* cl = (PhysicsClient* ) physClient;
    b3Assert(cl);
@@ -414,7 +484,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;
+	command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId;
    return (b3SharedMemoryCommandHandle) command;
    
 }
@@ -489,6 +559,33 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
    return CMD_INVALID_STATUS;
 }

+int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
+{
+    int numBodies = 0;
+    const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+    b3Assert(status);
+	
+	if (status)
+	{
+			switch (status->m_type)
+			{
+				case CMD_SDF_LOADING_COMPLETED:
+				{
+				    int i,maxBodies;
+				    numBodies = status->m_sdfLoadedArgs.m_numBodies;
+				    maxBodies = btMin(bodyIndicesCapacity, numBodies);
+				    for (i=0;i<maxBodies;i++)
+				    {
+                            bodyIndicesOut[i] = status->m_sdfLoadedArgs.m_bodyUniqueIds[i];
+				    }
+					break;
+				}
+			}
+	}
+	
+	return numBodies;
+}
+
 int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -528,6 +625,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
                           const double* jointReactionForces[]) {
    const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
    const SendActualStateArgs &args = status->m_sendActualStateArgs;
+    btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
+    if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+        return false;
+    
    if (bodyUniqueId) {
        *bodyUniqueId = args.m_bodyUniqueId;
    }
@@ -592,10 +693,10 @@ int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
 }


-void	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
+int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
-	cl->getJointInfo(bodyIndex, linkIndex,*info);
+	return cl->getJointInfo(bodyIndex, linkIndex,*info);
 }

 b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
@@ -705,6 +806,94 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa
 	command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
 }

+void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
+{
+    b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
+    b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
+    b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
+    b3Vector3 f = (center - eye).normalized();
+    b3Vector3 u = up.normalized();
+    b3Vector3 s = (f.cross(u)).normalized();
+    u = s.cross(f);
+    
+    float viewMatrix[16];
+    
+    viewMatrix[0*4+0] = s.x;
+    viewMatrix[1*4+0] = s.y;
+    viewMatrix[2*4+0] = s.z;
+    
+    viewMatrix[0*4+1] = u.x;
+    viewMatrix[1*4+1] = u.y;
+    viewMatrix[2*4+1] = u.z;
+    
+    viewMatrix[0*4+2] =-f.x;
+    viewMatrix[1*4+2] =-f.y;
+    viewMatrix[2*4+2] =-f.z;
+    
+    viewMatrix[0*4+3] = 0.f;
+    viewMatrix[1*4+3] = 0.f;
+    viewMatrix[2*4+3] = 0.f;
+    
+    viewMatrix[3*4+0] = -s.dot(eye);
+    viewMatrix[3*4+1] = -u.dot(eye);
+    viewMatrix[3*4+2] = f.dot(eye);
+    viewMatrix[3*4+3] = 1.f;
+    
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+    for (int i=0;i<16;i++)
+    {
+        command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
+    }
+    command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
+
+}
+
+void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float  right, float bottom, float top, float nearVal, float farVal)
+{
+    float frustum[16];
+    
+    frustum[0*4+0] = (float(2) * nearVal) / (right - left);
+    frustum[0*4+1] = float(0);
+    frustum[0*4+2] = float(0);
+    frustum[0*4+3] = float(0);
+    
+    frustum[1*4+0] = float(0);
+    frustum[1*4+1] = (float(2) * nearVal) / (top - bottom);
+    frustum[1*4+2] = float(0);
+    frustum[1*4+3] = float(0);
+    
+    frustum[2*4+0] = (right + left) / (right - left);
+    frustum[2*4+1] = (top + bottom) / (top - bottom);
+    frustum[2*4+2] = -(farVal + nearVal) / (farVal - nearVal);
+    frustum[2*4+3] = float(-1);
+    
+    frustum[3*4+0] = float(0);
+    frustum[3*4+1] = float(0);
+    frustum[3*4+2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
+    frustum[3*4+3] = float(0);
+    
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+    for (int i=0;i<16;i++)
+    {
+        command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i];
+    }
+    command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
+}
+
+void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height )
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+	command->m_requestPixelDataArguments.m_pixelWidth = width;
+	command->m_requestPixelDataArguments.m_pixelHeight = height;
+	command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT;	
+}
+
 void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -714,3 +903,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
 	}
 }

+b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
+{
+    PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    
+    command->m_type = CMD_APPLY_EXTERNAL_FORCE;
+    command->m_updateFlags = 0;
+    command->m_externalForceArguments.m_numForcesAndTorques = 0;
+    return (b3SharedMemoryCommandHandle) command;
+}
+
+void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
+    int index = command->m_externalForceArguments.m_numForcesAndTorques;
+    command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
+    command->m_externalForceArguments.m_linkIds[index] = linkId;
+    command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag;
+    for (int i = 0; i < 3; ++i) {
+        command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i];
+        command->m_externalForceArguments.m_positions[index+i] = position[i];
+    }
+    
+    command->m_externalForceArguments.m_numForcesAndTorques++;
+}
+
+void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
+    int index = command->m_externalForceArguments.m_numForcesAndTorques;
+    command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
+    command->m_externalForceArguments.m_linkIds[index] = linkId;
+    command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag;
+
+    for (int i = 0; i < 3; ++i) {
+        command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i];
+    }
+    command->m_externalForceArguments.m_numForcesAndTorques++;
+}
+
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -40,6 +40,8 @@ b3SharedMemoryStatusHandle	b3ProcessServerStatus(b3PhysicsClientHandle physClien
 /// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
 int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);

+int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
+
 int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);

 int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
@@ -55,7 +57,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
 int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);

 ///given a body and link index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
-void	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);
+int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info);

 ///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
 ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
@@ -68,6 +70,9 @@ void    b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
 ///request an image from a simulated camera, using a software renderer.
 b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
 void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
+void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
+void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
+void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
 void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);


@@ -88,14 +93,22 @@ int	b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
 int	b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
 int	b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);

-///Set joint control variables such as desired position/angle, desired velocity,
-///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
+b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+
+///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
 b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
+
+    
+///Set joint motor control variables such as desired position/angle, desired velocity,
+///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
+b3SharedMemoryCommandHandle  b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
+
 ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
 int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
 int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
 int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
-//Only use when controlMode is CONTROL_MODE_VELOCITY
+
+///Only use when controlMode is CONTROL_MODE_VELOCITY
 int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
 int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex,  double value);
 ///Only use if when controlMode is CONTROL_MODE_TORQUE,
@@ -127,7 +140,7 @@ int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar

 ///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
 ///This is rather inconsistent, to mix programmatical creation with loading from file.
-b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
+b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
 int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
 ///b3CreateSensorEnableIMUForLink is not implemented yet.
 ///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
@@ -146,6 +159,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
                                             double rayToWorldZ);
 b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);

+/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
+b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
+void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
+void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
+
 #ifdef __cplusplus
 }
 #endif
--- a/examples/SharedMemory/PhysicsClientExample.cpp
+++ b/examples/SharedMemory/PhysicsClientExample.cpp
@@ -21,8 +21,8 @@ struct MyMotorInfo2
    int     m_qIndex;
 };

-int camVisualizerWidth = 640;//1024/3;
-int camVisualizerHeight = 480;//768/3;
+int camVisualizerWidth = 320;//1024/3;
+int camVisualizerHeight = 240;//768/3;


 #define MAX_NUM_MOTORS 128
@@ -37,6 +37,9 @@ protected:
 	
 	bool m_wantsTermination;
    btAlignedObjectArray<int> m_userCommandRequests;
+    btAlignedObjectArray<int> m_bodyUniqueIds;
+    
+    
    int m_sharedMemoryKey;
    int m_selectedBody;
 	int m_prevSelectedBody;
@@ -66,7 +69,8 @@ protected:
 	{
 		if (m_guiHelper && m_guiHelper->getParameterInterface())
 		{
-			int bodyIndex = comboIndex;
+            int itemIndex = int(atoi(name));
+			int bodyIndex = m_bodyUniqueIds[itemIndex];
 			if (m_selectedBody != bodyIndex)
 			{
 				m_selectedBody = bodyIndex;
@@ -77,10 +81,10 @@ protected:
    
 	virtual void resetCamera()
 	{
-		float dist = 5;
-		float pitch = 50;
-		float yaw = 35;
-		float targetPos[3]={0,0,0};//-3,2.8,-2.5};
+		float dist = 4;
+		float pitch = 193;
+		float yaw = 25;
+		float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
 		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);

 	}
@@ -154,18 +158,20 @@ protected:

 	void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle)
 	{
+        
        for (int i=0;i<m_numMotors;i++)
        {
-            // btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
-            // int uIndex = m_motorTargetVelocities[i].m_uIndex;
-            // b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
            
            btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
            int qIndex = m_motorTargetPositions[i].m_qIndex;
            int uIndex = m_motorTargetPositions[i].m_uIndex;
+            static int serial=0;
+            serial++;
+          //  b3Printf("# motors = %d, cmd[%d] qIndex = %d, uIndex = %d, targetPos = %f", m_numMotors, serial, qIndex,uIndex,targetPos);
+            
            b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
-            b3JointControlSetKp(commandHandle, uIndex, 0.1);
-            b3JointControlSetKd(commandHandle, uIndex, 0.0);
+            b3JointControlSetKp(commandHandle, qIndex, 0.1);
+            b3JointControlSetKd(commandHandle, uIndex, 0);
            
            b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
        }
@@ -191,7 +197,7 @@ protected:

 void MyComboBoxCallback (int combobox, const char* item, void* userPointer)
 {
-	b3Printf("Item selected %s", item);
+	//b3Printf("Item selected %s", item);

 	PhysicsClientExample* cl = (PhysicsClientExample*) userPointer;
 	b3Assert(cl);
@@ -227,9 +233,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
    {
        case  CMD_LOAD_URDF:
        {
-            
            b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
-            
            //setting the initial position, orientation and other arguments are optional
            double startPosX = 0;
            static double startPosY = 0;
@@ -238,7 +242,13 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
 			startPosY += 2.f;
 //            ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
-
+            break;
+        }
+        
+        case  CMD_LOAD_SDF:
+        {
+            b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.sdf");
+            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        case CMD_REQUEST_CAMERA_IMAGE_DATA:
@@ -252,7 +262,8 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
 			this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
 			this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
            b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
-            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+			b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight);
+			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        case CMD_CREATE_BOX_COLLISION_SHAPE:
@@ -363,11 +374,18 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
        }
        case CMD_SEND_DESIRED_STATE:
        {
-            // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
-            b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
+	   if (m_selectedBody>=0)
+           {
+            // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
+            b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
+          //  b3Printf("prepare control command for body %d", m_selectedBody);
+               
            prepareControlCommand(command);
+            
+               
            b3SubmitClientCommand(m_physicsClientHandle, command);
-            break;
+        }   
+	 break;
        }
        case CMD_RESET_SIMULATION:
        {
@@ -430,6 +448,11 @@ PhysicsClientExample::~PhysicsClientExample()
 		bool deInitializeSharedMemory = true;
 		m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
 	}
+	
+	if (m_canvas && (m_canvasIndex>=0))
+	{
+		m_canvas->destroyCanvas(m_canvasIndex);
+	}
    b3Printf("~PhysicsClientExample\n");
 }

@@ -450,6 +473,7 @@ void	PhysicsClientExample::createButtons()
 		m_guiHelper->getParameterInterface()->removeAllParameters();

        createButton("Load URDF",CMD_LOAD_URDF,  isTrigger);
+        createButton("Load SDF",CMD_LOAD_SDF,  isTrigger);
        createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
        createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
        createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM,  isTrigger);
@@ -464,9 +488,36 @@ void	PhysicsClientExample::createButtons()
 		createButton("Initialize Pose",CMD_INIT_POSE,  isTrigger);
        createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);

+        if (m_bodyUniqueIds.size())
+        {
+            if (m_selectedBody<0)
+                m_selectedBody = 0;
+            
+            ComboBoxParams comboParams;
+            comboParams.m_comboboxId = 0;
+            comboParams.m_numItems = m_bodyUniqueIds.size();
+            comboParams.m_startItem = m_selectedBody;
+            comboParams.m_callback = MyComboBoxCallback;
+            comboParams.m_userPointer = this;
+            //todo: get the real object name
+
+            const char** blarray = new const char*[m_bodyUniqueIds.size()];
+            
+            for (int i=0;i<m_bodyUniqueIds.size();i++)
+            {
+                char* bla = new char[16];
+                sprintf(bla,"%d", i);
+                blarray[i] = bla;
+                comboParams.m_items=blarray;//{&bla};
+            }
+            m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+        }
+        

 		if (m_physicsClientHandle && m_selectedBody>=0)
 		{
+            m_numMotors = 0;
+            
 			int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
 			for (int i=0;i<numJoints;i++)
 			{
@@ -527,7 +578,6 @@ void	PhysicsClientExample::initPhysics()
 	m_selectedBody = -1;
 	m_prevSelectedBody = -1;

-	if (m_options == eCLIENTEXAMPLE_SERVER)
 	{
 		m_canvas = m_guiHelper->get2dCanvasInterface();
 		if (m_canvas)
@@ -557,13 +607,22 @@ void	PhysicsClientExample::initPhysics()
 			
 		}

-		m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
 	}

-    m_physicsClientHandle  = b3ConnectSharedMemory(m_sharedMemoryKey);
-	//m_physicsClientHandle  = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
-	//m_physicsClientHandle = b3ConnectPhysicsDirect();
-
+    if (m_options == eCLIENTEXAMPLE_SERVER)
+    {
+        m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
+    }
+    
+	if (m_options == eCLIENTEXAMPLE_DIRECT)
+	{
+		m_physicsClientHandle = b3ConnectPhysicsDirect();
+	} else
+	{
+	    m_physicsClientHandle  = b3ConnectSharedMemory(m_sharedMemoryKey);
+		//m_physicsClientHandle  = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
+	}
+	
    if (!b3CanSubmitCommand(m_physicsClientHandle))
    {
 		b3Warning("Cannot connect to physics client");
@@ -603,48 +662,78 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 			}
 			if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
            {
-				static int counter=0;
-				char msg[1024];
-				sprintf(msg,"Camera image %d OK\n",counter++);
+			//	static int counter=0;
+			//	char msg[1024];
+			//	sprintf(msg,"Camera image %d OK\n",counter++);
 				b3CameraImageData imageData;
 				b3GetCameraImageData(m_physicsClientHandle,&imageData);
 				if (m_canvas && m_canvasIndex >=0)
 				{
-					for (int i=0;i<imageData.m_pixelWidth;i++)
+					for (int i=0;i<camVisualizerWidth;i++)
 					{
-						for (int j=0;j<imageData.m_pixelHeight;j++)
+						for (int j=0;j<camVisualizerHeight;j++)
 						{
-                            int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
-                            int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
+                            int xIndex = int(float(i)*(float(imageData.m_pixelWidth)/float(camVisualizerWidth)));
+                            int yIndex = int(float(j)*(float(imageData.m_pixelHeight)/float(camVisualizerHeight)));
 							btClamp(yIndex,0,imageData.m_pixelHeight);
 							btClamp(xIndex,0,imageData.m_pixelWidth);
-							int bytesPerPixel = 4;
+							int bytesPerPixel = 4; //RGBA
 							
-							int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
-							m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
+							int pixelIndex = (xIndex+yIndex*imageData.m_pixelWidth)*bytesPerPixel;
+							m_canvas->setPixel(m_canvasIndex,i,j,
                                               
 									imageData.m_rgbColorData[pixelIndex],
 									imageData.m_rgbColorData[pixelIndex+1],
 									imageData.m_rgbColorData[pixelIndex+2],
-                                               255);
-//									imageData.m_rgbColorData[pixelIndex+3]);
+                                               255); //alpha set to 255
 						}
 					}
 					m_canvas->refreshImageData(m_canvasIndex);
 				}

-                b3Printf(msg);
+               // b3Printf(msg);
            } 
            if (statusType == CMD_CAMERA_IMAGE_FAILED)
            {
-                b3Printf("Camera image FAILED\n");
+                b3Warning("Camera image FAILED\n");
            }
       
-        
-      		if (statusType == CMD_URDF_LOADING_COMPLETED)
+            if (statusType == CMD_SDF_LOADING_COMPLETED)
 			{
-				int bodyIndex = b3GetStatusBodyIndex(status);
-				if (bodyIndex>=0)
+                int bodyIndicesOut[1024];
+                int bodyCapacity = 1024;
+			    int numBodies = b3GetStatusBodyIndices(status, bodyIndicesOut, bodyCapacity);
+			    if (numBodies > bodyCapacity)
+                {
+                    b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity);
+                } else
+                {
+                    for (int i=0;i<numBodies;i++)
+                    {
+                        int bodyUniqueId = bodyIndicesOut[i];
+                        m_bodyUniqueIds.push_back(bodyUniqueId);
+                        int numJoints =  b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
+                        if (numJoints>0)
+                        {
+                            m_selectedBody = bodyUniqueId;
+                        }
+/*                        int numJoints =  b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
+                        b3Printf("numJoints = %d", numJoints);
+                        for (int i=0;i<numJoints;i++)
+                        {
+                            b3JointInfo info;
+                            b3GetJointInfo(m_physicsClientHandle,bodyUniqueId,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);
+			    
+				//int bodyIndex = b3GetStatusBodyIndex(status);
+				/*if (bodyIndex>=0)
 				{
 					int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
            
@@ -653,7 +742,6 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 						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;
@@ -667,12 +755,29 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 				
 					comboParams.m_items=blarray;//{&bla};
 					m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
-		
-
 				}
-
+				*/
+			}
+			
+        
+      		if (statusType == CMD_URDF_LOADING_COMPLETED)
+			{
+				int bodyIndex = b3GetStatusBodyIndex(status);
+				if (bodyIndex>=0)
+				{
+                    m_bodyUniqueIds.push_back(bodyIndex);
+                    m_selectedBody = bodyIndex;
+					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);
+					}
+					
+				}
 			}
-    
 		}
 	}
    if (b3CanSubmitCommand(m_physicsClientHandle))
@@ -695,6 +800,7 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
            {
 				m_selectedBody = -1;
                m_numMotors=0;
+                m_bodyUniqueIds.clear();
                createButtons();
 				b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
 				if (m_options == eCLIENTEXAMPLE_SERVER)
@@ -707,8 +813,8 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 						bool hasStatus = (status != 0);
 						if (hasStatus)
 						{
-							int statusType = b3GetStatusType(status);
-							b3Printf("Status after reset: %d",statusType);
+							//int statusType = b3GetStatusType(status);
+							//b3Printf("Status after reset: %d",statusType);
 						}
 					}
 				} else
@@ -725,13 +831,12 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
            if (m_numMotors)
            {
                enqueueCommand(CMD_SEND_DESIRED_STATE);
-                enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
-				if (m_options != eCLIENTEXAMPLE_SERVER)
-				{
-					enqueueCommand(CMD_REQUEST_DEBUG_LINES);
-				}
-                //enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
            }
+            enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
+			if (m_options != eCLIENTEXAMPLE_SERVER)
+			{
+				enqueueCommand(CMD_REQUEST_DEBUG_LINES);
+			}
        }
    }

--- a/examples/SharedMemory/PhysicsClientExample.h
+++ b/examples/SharedMemory/PhysicsClientExample.h
@@ -4,7 +4,7 @@
 enum ClientExampleOptions
 {
 	eCLIENTEXAMPLE_LOOPBACK=1,
-	eCLIENTEAXMPLE_DIRECT=2,
+	eCLIENTEXAMPLE_DIRECT=2,
 	eCLIENTEXAMPLE_SERVER=3,
 };

--- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp
@@ -37,10 +37,13 @@ struct PhysicsClientSharedMemoryInternalData {
 	btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
 	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;

+    btAlignedObjectArray<int> m_bodyIdsRequestInfo;
+    SharedMemoryStatus m_tempBackupServerStatus;
+    
    SharedMemoryStatus m_lastServerStatus;

    int m_counter;
-    bool m_serverLoadUrdfOK;
+    
    bool m_isConnected;
    bool m_waitingForServer;
    bool m_hasLastServerStatus;
@@ -54,7 +57,6 @@ struct PhysicsClientSharedMemoryInternalData {
 		  m_counter(0),
 		  m_cachedCameraPixelsWidth(0),
 		  m_cachedCameraPixelsHeight(0),
-	      m_serverLoadUrdfOK(false),
          m_isConnected(false),
          m_waitingForServer(false),
          m_hasLastServerStatus(false),
@@ -83,15 +85,16 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
 	
 }

-void PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const 
+bool 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];
+        return true;
 	}
-    
+    return false;
 }

 PhysicsClientSharedMemory::PhysicsClientSharedMemory()
@@ -168,6 +171,48 @@ bool PhysicsClientSharedMemory::connect() {
    return true;
 }

+
+///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
+void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
+{
+    bParse::btBulletFile bf(
+                            &this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
+                            serverCmd.m_dataStreamArguments.m_streamChunkLength);
+    bf.setFileDNAisMemoryDNA();
+    bf.parse(false);
+    
+    
+    BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
+    m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+    
+    for (int i = 0; i < bf.m_multiBodies.size(); i++)
+    {
+        int flag = bf.getFlags();
+        if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
+        {
+            Bullet::btMultiBodyDoubleData* mb =
+            (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
+            
+            addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+        } else
+        {
+            Bullet::btMultiBodyFloatData* mb =
+            (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
+            
+            addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+        }
+    }
+    if (bf.ok()) {
+        if (m_data->m_verboseOutput)
+        {
+            b3Printf("Received robot description ok!\n");
+        }
+    } else
+    {
+        b3Warning("Robot description not received");
+    }
+}
+
 const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
    SharedMemoryStatus* stat = 0;

@@ -204,8 +249,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                }
                break;
            }
+            case CMD_SDF_LOADING_COMPLETED: {
+                
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server loading the SDF OK\n");
+                }
+
+                break;
+            }
            case CMD_URDF_LOADING_COMPLETED: {
-                m_data->m_serverLoadUrdfOK = true;
+                
                if (m_data->m_verboseOutput) {
                    b3Printf("Server loading the URDF OK\n");
                }
@@ -265,7 +318,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                if (m_data->m_verboseOutput) {
                    b3Printf("Server failed loading the URDF...\n");
                }
-                m_data->m_serverLoadUrdfOK = false;
+                
+                break;
+            }
+            
+            case CMD_BODY_INFO_COMPLETED:
+            {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Received body info\n");
+                }
+                int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+                processBodyJointInfo(bodyUniqueId, serverCmd);
+
+                break;
+            }
+             case CMD_SDF_LOADING_FAILED: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server failed loading the SDF...\n");
+                }
+                
                break;
            }

@@ -448,8 +519,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                
 				unsigned char* rgbaPixelsReceived =
                    (unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
-                printf("pixel = %d\n", rgbaPixelsReceived[0]);
+              //  printf("pixel = %d\n", rgbaPixelsReceived[0]);
                
+				float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
+			
+				for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
+				{
+					m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
+				}
+				
 				for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
 				{
 					m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] 
@@ -461,7 +539,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
            
            case CMD_CAMERA_IMAGE_FAILED:
            {
-                b3Printf("Camera image FAILED\n");
+                b3Warning("Camera image FAILED\n");
                break;
            }

@@ -483,6 +561,50 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
            m_data->m_waitingForServer = true;
        }

+        if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
+        {
+            int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
+            if (numBodies>0)
+            {
+                m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;
+                
+                for (int i=0;i<numBodies;i++)
+                {
+                    m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
+                }
+                
+                int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
+                m_data->m_bodyIdsRequestInfo.pop_back();
+                
+                SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+                //now transfer the information of the individual objects etc.
+                command.m_type = CMD_REQUEST_BODY_INFO;
+                command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
+                submitClientCommand(command);
+                return 0;    
+            }
+        }
+        
+        if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
+        {
+            //are there any bodies left to be processed?
+            if (m_data->m_bodyIdsRequestInfo.size())
+            {
+                int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size()-1];
+                m_data->m_bodyIdsRequestInfo.pop_back();
+                
+                SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
+                //now transfer the information of the individual objects etc.
+                command.m_type = CMD_REQUEST_BODY_INFO;
+                command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
+                submitClientCommand(command);
+                return 0;
+            } else
+            {
+                m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
+            }
+        }
+        
 		if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
 		{
 			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
--- a/examples/SharedMemory/PhysicsClientSharedMemory.h
+++ b/examples/SharedMemory/PhysicsClientSharedMemory.h
@@ -11,7 +11,9 @@ class PhysicsClientSharedMemory : public PhysicsClient {

 protected:
 	virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
+    void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);

+        
 public:
    PhysicsClientSharedMemory();
    virtual ~PhysicsClientSharedMemory();
@@ -34,7 +36,7 @@ public:

    virtual int getNumJoints(int bodyIndex) const;

-    virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+    virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

    virtual void setSharedMemoryKey(int key);

--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/examples/SharedMemory/PhysicsDirect.cpp
@@ -33,6 +33,12 @@ struct PhysicsDirectInternalData
 	
 	char    m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];

+	int m_cachedCameraPixelsWidth;
+	int m_cachedCameraPixelsHeight;
+	btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
+	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
+
+
 	PhysicsServerCommandProcessor* m_commandProcessor;

 	PhysicsDirectInternalData()
@@ -167,6 +173,122 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
 	return m_data->m_hasStatus;
 }

+bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
+{
+	SharedMemoryCommand command = orgCommand;
+
+	const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+	do
+	{
+
+		bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+		m_data->m_hasStatus = hasStatus;
+		if (hasStatus)
+		{
+			btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED);
+
+			if (m_data->m_verboseOutput) 
+			{
+				b3Printf("Camera image OK\n");
+			}
+
+			int numBytesPerPixel = 4;//RGBA
+			int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
+				serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
+				serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
+
+			m_data->m_cachedCameraPixelsWidth = 0;
+			m_data->m_cachedCameraPixelsHeight = 0;
+
+            int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
+
+            m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
+			m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
+			m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
+                
+                
+			unsigned char* rgbaPixelsReceived =
+                (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
+			
+			float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
+			
+          //  printf("pixel = %d\n", rgbaPixelsReceived[0]);
+                
+			for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;i++)
+			{
+				m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
+			}
+			for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
+			{
+				m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] 
+					= rgbaPixelsReceived[i];
+			}
+
+			if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
+			{
+				
+
+				// continue requesting remaining pixels
+				command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
+				command.m_requestPixelDataArguments.m_startPixelIndex = 
+					serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + 
+					serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
+				
+			} else
+			{
+				m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
+				m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
+			}	
+		}
+	}  while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0);
+	
+	return m_data->m_hasStatus;
+
+
+}
+
+
+void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
+{
+    bParse::btBulletFile bf(
+        &m_data->m_bulletStreamDataServerToClient[0],
+        serverCmd.m_dataStreamArguments.m_streamChunkLength);
+    bf.setFileDNAisMemoryDNA();
+    bf.parse(false);
+
+
+    BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
+    m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+
+    for (int i = 0; i < bf.m_multiBodies.size(); i++) 
+    {
+        int flag = bf.getFlags();
+        if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) 
+        {
+            Bullet::btMultiBodyDoubleData* mb =
+                (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
+
+            addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+        } else 
+        {
+            Bullet::btMultiBodyFloatData* mb =
+                (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
+
+            addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+        }
+    }
+    if (bf.ok()) {
+        if (m_data->m_verboseOutput) 
+        {
+            b3Printf("Received robot description ok!\n");
+        }
+    } else 
+    {
+        b3Warning("Robot description not received");
+    }
+}
+
 bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
 {
 	if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@@ -174,6 +296,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
 		return processDebugLines(command);
 	}

+	if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA)
+	{
+		return processCamera(command);
+	}
+
 	bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
 	m_data->m_hasStatus = hasStatus;
 	if (hasStatus)
@@ -210,46 +337,35 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
                
 				break;
 			}
+			case CMD_SDF_LOADING_COMPLETED:
+            {
+                //we'll stream further info from the physics server
+                //so serverCmd will be invalid, make a copy
+                
+                
+                int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
+                for (int i=0;i<numBodies;i++)
+                {
+                    int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
+                    SharedMemoryCommand infoRequestCommand;
+                    infoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
+                    infoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
+                    SharedMemoryStatus infoStatus;
+                    bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+                    if (hasStatus)
+                    {
+                        processBodyJointInfo(bodyUniqueId, infoStatus);
+                    }
+                }
+                break;
+            }
 			case CMD_URDF_LOADING_COMPLETED:
 			{
+				
 				if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) 
 				{
-					bParse::btBulletFile bf(
-						&m_data->m_bulletStreamDataServerToClient[0],
-						serverCmd.m_dataStreamArguments.m_streamChunkLength);
-					bf.setFileDNAisMemoryDNA();
-					bf.parse(false);
-					int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
-
-					BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
-					m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
-
-					for (int i = 0; i < bf.m_multiBodies.size(); i++) 
-					{
-						int flag = bf.getFlags();
-						if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) 
-						{
-							Bullet::btMultiBodyDoubleData* mb =
-								(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
-
-							addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
-						} else 
-						{
-							Bullet::btMultiBodyFloatData* mb =
-								(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
-
-							addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
-						}
-					}
-					if (bf.ok()) {
-						if (m_data->m_verboseOutput) 
-						{
-							b3Printf("Received robot description ok!\n");
-						}
-					} else 
-					{
-						b3Warning("Robot description not received");
-					}
+				    int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+                    processBodyJointInfo(bodyIndex,serverCmd);
 				}
                break;
            }
@@ -277,14 +393,19 @@ int PhysicsDirect::getNumJoints(int bodyIndex) const
 	return 0;
 }

-void PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
+bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
 {
 	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
 	if (bodyJointsPtr && *bodyJointsPtr)
 	{
 		BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
-		info = bodyJoints->m_jointInfo[jointIndex];
+        if (jointIndex < bodyJoints->m_jointInfo.size())
+        {
+            info = bodyJoints->m_jointInfo[jointIndex];
+            return true;
+        }
 	}
+    return false;
 }

 ///todo: move this out of the
@@ -333,9 +454,9 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
 {
 	if (cameraData)
 	{
-		cameraData->m_pixelHeight = 0;
-		cameraData->m_pixelWidth = 0;
-		cameraData->m_depthValues = 0;
-		cameraData->m_rgbColorData = 0;
+		cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
+		cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
+		cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
+		cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
 	}
 }
--- a/examples/SharedMemory/PhysicsDirect.h
+++ b/examples/SharedMemory/PhysicsDirect.h
@@ -19,6 +19,10 @@ protected:

 	bool processDebugLines(const struct SharedMemoryCommand& orgCommand);

+	bool processCamera(const struct SharedMemoryCommand& orgCommand);
+
+    void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
+    
 public:

    PhysicsDirect();
@@ -44,7 +48,7 @@ public:

    virtual int getNumJoints(int bodyIndex) const;

-    virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+    virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

 	///todo: move this out of the
    virtual void setSharedMemoryKey(int key);
--- a/examples/SharedMemory/PhysicsLoopBack.cpp
+++ b/examples/SharedMemory/PhysicsLoopBack.cpp
@@ -79,9 +79,9 @@ int PhysicsLoopBack::getNumJoints(int bodyIndex) const
 	return m_data->m_physicsClient->getNumJoints(bodyIndex);
 }

-void PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
+bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
 {
-	m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
+	return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
 }

 ///todo: move this out of the
--- a/examples/SharedMemory/PhysicsLoopBack.h
+++ b/examples/SharedMemory/PhysicsLoopBack.h
@@ -40,7 +40,7 @@ public:

    virtual int getNumJoints(int bodyIndex) const;

-    virtual void getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+    virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

 	///todo: move this out of the
    virtual void setSharedMemoryKey(int key);
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.h
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h
@@ -19,10 +19,16 @@ class PhysicsServerCommandProcessor
 protected:


+    
+
+    bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes);
+
 	bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
                             bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);

 	bool	supportsJointMotor(class btMultiBody* body, int linkIndex);
+	
+	int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);

 public:
 	PhysicsServerCommandProcessor();
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -30,8 +30,10 @@
 #define MAX_DEGREE_OF_FREEDOM 64
 #define MAX_NUM_SENSORS 256
 #define MAX_URDF_FILENAME_LENGTH 1024
+#define MAX_SDF_FILENAME_LENGTH 1024
 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
 #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
+#define MAX_SDF_BODIES 1024

 struct TmpFloat3 
 {
@@ -54,6 +56,16 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z)
    return tmp;
 }

+enum EnumSdfArgsUpdateFlags
+{
+	SDF_ARGS_FILE_NAME=1,
+};
+
+struct SdfArgs
+{
+	char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
+};
+
 enum EnumUrdfArgsUpdateFlags
 {
 	URDF_ARGS_FILE_NAME=1,
@@ -103,6 +115,7 @@ enum EnumInitPoseFlags
 struct InitPoseArgs
 {
 	int m_bodyUniqueId;
+	int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
 	double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
 };

@@ -118,12 +131,16 @@ struct RequestPixelDataArgs
 	float m_viewMatrix[16];
 	float m_projectionMatrix[16];
 	int m_startPixelIndex;
+	int m_pixelWidth;
+	int m_pixelHeight;
 };

 enum EnumRequestPixelDataUpdateFlags
 {
 	REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1,
 	REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL=2,
+	REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4,
+	
 };


@@ -163,11 +180,14 @@ struct SendDesiredStateArgs
 	double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
 	double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link

+    int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM];
+    
 	//desired state is only written by the client, read-only access by server is expected

 	//m_desiredStateQ is indexed by position variables, 
 	//starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables
    double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
+    

 	//m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables
    double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
@@ -179,6 +199,15 @@ struct SendDesiredStateArgs
    
 };

+enum EnumSimDesiredStateUpdateFlags
+{
+	SIM_DESIRED_STATE_HAS_Q=1,
+	SIM_DESIRED_STATE_HAS_QDOT=2,
+	SIM_DESIRED_STATE_HAS_KD=4,
+	SIM_DESIRED_STATE_HAS_KP=8,
+	SIM_DESIRED_STATE_HAS_MAX_FORCE=16,
+};
+

 enum EnumSimParamUpdateFlags
 {
@@ -274,6 +303,49 @@ struct CreateBoxShapeArgs
 	double m_colorRGBA[4];
 };

+struct SdfLoadedArgs
+{
+    int m_numBodies;
+    int m_bodyUniqueIds[MAX_SDF_BODIES];
+    
+    ///@todo(erwincoumans) load cameras, lights etc
+    //int m_numCameras; 
+    //int m_numLights; 
+};
+
+
+struct SdfRequestInfoArgs
+{
+    int m_bodyUniqueId;
+};
+
+///flags for b3ApplyExternalTorque and b3ApplyExternalForce
+enum EnumExternalForcePrivateFlags
+{
+//    EF_LINK_FRAME=1,
+//    EF_WORLD_FRAME=2,
+    EF_TORQUE=4,
+    EF_FORCE=8,
+};
+
+
+
+struct ExternalForceArgs
+{
+    int m_numForcesAndTorques;
+    int m_bodyUniqueIds[MAX_SDF_BODIES];
+    int m_linkIds[MAX_SDF_BODIES];
+    double m_forcesAndTorques[3*MAX_SDF_BODIES];
+    double m_positions[3*MAX_SDF_BODIES];
+    int m_forceFlags[MAX_SDF_BODIES];
+};
+
+enum EnumSdfRequestInfoFlags
+{
+    SDF_REQUEST_INFO_BODY=1,
+    //SDF_REQUEST_INFO_CAMERA=2,
+};
+
 struct SharedMemoryCommand
 {
 	int m_type;
@@ -287,6 +359,8 @@ struct SharedMemoryCommand
    union
    {
        struct UrdfArgs m_urdfArguments;
+		struct SdfArgs m_sdfArguments;
+		struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
 		struct InitPoseArgs m_initPoseArgs;
 		struct SendPhysicsSimulationParameters m_physSimParamArgs;
 		struct BulletDataStreamArgs	m_dataStreamArguments;
@@ -297,6 +371,7 @@ struct SharedMemoryCommand
 		struct RequestDebugLinesArgs m_requestDebugLinesArguments;
 		struct RequestPixelDataArgs m_requestPixelDataArguments;
 		struct PickBodyArgs m_pickBodyArguments;
+        struct ExternalForceArgs m_externalForceArguments;
    };
 };

@@ -315,6 +390,7 @@ struct SharedMemoryStatus
 	union
 	{
 		struct BulletDataStreamArgs	m_dataStreamArguments;
+		struct SdfLoadedArgs m_sdfLoadedArgs;
 		struct SendActualStateArgs m_sendActualStateArgs;
 		struct SendDebugLinesArgs m_sendDebugLinesArgs;
 		struct SendPixelDataArgs m_sendPixelDataArguments;
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -5,7 +5,8 @@

 enum EnumSharedMemoryClientCommand
 {
-    CMD_LOAD_URDF,
+    CMD_LOAD_SDF,
+	CMD_LOAD_URDF,
        CMD_SEND_BULLET_DATA_STREAM,
        CMD_CREATE_BOX_COLLISION_SHAPE,
 //      CMD_DELETE_BOX_COLLISION_SHAPE,
@@ -18,12 +19,14 @@ enum EnumSharedMemoryClientCommand
        CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
        CMD_REQUEST_ACTUAL_STATE,
        CMD_REQUEST_DEBUG_LINES,
+    CMD_REQUEST_BODY_INFO,
    CMD_STEP_FORWARD_SIMULATION,
    CMD_RESET_SIMULATION,
    CMD_PICK_BODY,
    CMD_MOVE_PICKED_BODY,
    CMD_REMOVE_PICKING_CONSTRAINT_BODY,
    CMD_REQUEST_CAMERA_IMAGE_DATA,
+    CMD_APPLY_EXTERNAL_FORCE,
    CMD_MAX_CLIENT_COMMANDS
 };

@@ -35,7 +38,8 @@ enum EnumSharedMemoryServerStatus
        CMD_CLIENT_COMMAND_COMPLETED,
        //the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
        CMD_UNKNOWN_COMMAND_FLUSHED,
-
+		CMD_SDF_LOADING_COMPLETED,
+        CMD_SDF_LOADING_FAILED,
        CMD_URDF_LOADING_COMPLETED,
        CMD_URDF_LOADING_FAILED,
        CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
@@ -52,6 +56,8 @@ enum EnumSharedMemoryServerStatus
        CMD_RESET_SIMULATION_COMPLETED,
        CMD_CAMERA_IMAGE_COMPLETED,
        CMD_CAMERA_IMAGE_FAILED,
+        CMD_BODY_INFO_COMPLETED,
+        CMD_BODY_INFO_FAILED,
 		CMD_INVALID_STATUS,
        CMD_MAX_SERVER_COMMANDS
 };
@@ -138,5 +144,11 @@ enum {
    CONTROL_MODE_POSITION_VELOCITY_PD,
 };

+///flags for b3ApplyExternalTorque and b3ApplyExternalForce
+enum EnumExternalForceFlags
+{
+    EF_LINK_FRAME=1,
+    EF_WORLD_FRAME=2,
+};

 #endif//SHARED_MEMORY_PUBLIC_H
--- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
@@ -28,7 +28,7 @@ subject to the following restrictions:
 #include "../Utils/b3ResourcePath.h"
 #include "../TinyRenderer/TinyRenderer.h"
 #include "../OpenGLWindow/SimpleCamera.h"
-
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
 #include <iostream>
 #include <fstream>
 #include "../Importers/ImportURDFDemo/UrdfParser.h"
@@ -41,6 +41,13 @@ enum MyFileType
    MY_FILE_OBJ=3,
 };

+struct MyTexture2
+{
+	unsigned char* textureData;
+	int m_width;
+	int m_height;
+};
+
 struct TinyRendererObjectArray
 {
  btAlignedObjectArray<  TinyRenderObjectData*> m_renderObjects;
@@ -95,7 +102,7 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter()



-void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
+void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
 {

 	
@@ -201,7 +208,23 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 						{
                            case MY_FILE_OBJ:
                            {
-                                glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+                                //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+								b3ImportMeshData meshData;
+								if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
+								{
+									
+									if (meshData.m_textureImage)
+									{
+										MyTexture2 texData;
+										texData.m_width = meshData.m_textureWidth;
+										texData.m_height = meshData.m_textureHeight;
+										texData.textureData = meshData.m_textureImage;
+										texturesOut.push_back(texData);
+									}
+									glmesh = meshData.m_gfxShape;
+								}
+
+								
                                break;
                            }
                           
@@ -420,11 +443,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj)
 {
    
-	btAlignedObjectArray<GLInstanceVertex> vertices;
-	btAlignedObjectArray<int> indices;
-	btTransform startTrans; startTrans.setIdentity();
-	int graphicsIndex = -1;
-
+	
 	UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
 	if (linkPtr)
 	{
@@ -433,6 +452,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
 	
 		for (int v = 0; v < link->m_visualArray.size();v++)
 		{
+			btAlignedObjectArray<MyTexture2> textures;
+			btAlignedObjectArray<GLInstanceVertex> vertices;
+			btAlignedObjectArray<int> indices;
+			btTransform startTrans; startTrans.setIdentity();
+			int graphicsIndex = -1;
+
 			const UrdfVisual& vis = link->m_visualArray[v];
 			btTransform childTrans = vis.m_linkLocalFrame;
 			btHashString matName(vis.m_materialName.c_str());
@@ -458,14 +483,29 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
            btAssert(visualsPtr);
            TinyRendererObjectArray* visuals = *visualsPtr;
            
-			convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices);
+			convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);

            if (vertices.size() && indices.size())
            {
-                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_swWidth,m_data->m_swHeight,m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
-                tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor);
+                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer);
+				unsigned char* textureImage=0;
+				int textureWidth=0;
+				int textureHeight=0;
+				if (textures.size())
+				{
+					textureImage = textures[0].textureData;
+					textureWidth = textures[0].m_width;
+					textureHeight = textures[0].m_height;
+				}
+				
+                tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor,
+										   textureImage,textureWidth,textureHeight);
                visuals->m_renderObjects.push_back(tinyObj);
            }
+			for (int i=0;i<textures.size();i++)
+			{
+				delete textures[i].textureData;
+			}
 		}
 	}
 }
@@ -541,7 +581,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
    
    lightDirWorld.normalize();
    
-    printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
+  //  printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
    for (int i=0;i<m_data->m_swRenderInstances.size();i++)
    {
        TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);
@@ -580,7 +620,9 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
        }
    }
 	//printf("write tga \n");
-	m_data->m_rgbColorBuffer.write_tga_file("camera.tga");
+	//m_data->m_rgbColorBuffer.write_tga_file("camera.tga");
+//	printf("flipped!\n");
+	m_data->m_rgbColorBuffer.flip_vertically();

 }

@@ -590,6 +632,17 @@ void TinyRendererVisualShapeConverter::getWidthAndHeight(int& width, int& height
    height = m_data->m_swHeight;
 }

+
+void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
+{
+	m_data->m_swWidth = width;
+	m_data->m_swHeight = height;
+
+	m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
+	m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
+		
+}
+
 void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
 {
    int w = m_data->m_rgbColorBuffer.get_width();
@@ -612,6 +665,10 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
    {
        for (int i=0;i<numRequestedPixels;i++)
        {
+			if (depthBuffer)
+			{
+				depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
+			}
            if (pixelsRGBA)
            {
                pixelsRGBA[i*numBytesPerPixel] =   m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0];
--- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h
@@ -24,6 +24,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
 	void resetAll();

    void getWidthAndHeight(int& width, int& height);
+	void setWidthAndHeight(int width, int height);
    
    void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
    
--- a/examples/SharedMemory/premake4.lua
+++ b/examples/SharedMemory/premake4.lua
@@ -26,6 +26,8 @@ files {
 	"PhysicsServer.h",
 	"main.cpp",
 	"PhysicsClientC_API.cpp",
+	"SharedMemoryCommands.h",
+	"SharedMemoryPublic.h",
 	"PhysicsServer.cpp",
 	"PosixSharedMemory.cpp",
 	"Win32SharedMemory.cpp",
@@ -78,6 +80,7 @@ files {
    "../ThirdPartyLibs/tinyxml/tinyxml.cpp",
    "../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
    "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
-
+"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../ThirdPartyLibs/stb_image/stb_image.cpp",     
 }

--- a/examples/SimpleOpenGL3/main.cpp
+++ b/examples/SimpleOpenGL3/main.cpp
@@ -14,8 +14,8 @@ static b3MouseButtonCallback sOldMouseButtonCB = 0;
 static b3KeyboardCallback sOldKeyboardCB = 0;
 //static b3RenderCallback sOldRenderCB = 0;

-float gWidth = 0 ;
-float gHeight = 0;
+float gWidth = 1024;
+float gHeight = 768;

 void MyWheelCallback(float deltax, float deltay)
 {
@@ -61,7 +61,7 @@ int main(int argc, char* argv[])
    b3CommandLineArgs myArgs(argc,argv);


-	SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768,true);
+	SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",gWidth,gHeight,true);
 	
 	app->m_instancingRenderer->getActiveCamera()->setCameraDistance(13);
 	app->m_instancingRenderer->getActiveCamera()->setCameraPitch(0);
--- a/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
+++ b/examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp
@@ -110,8 +110,9 @@ public:
   	    int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices);
   	    if (shapeIndex>=0)
        {
-            TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer);
-            swObj->registerMeshShape(vertices,numvertices,indices,numIndices);
+            TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
+        float rgbaColor[4] = {1,1,1,1};    
+	swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
 			//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
            m_swRenderObjects.insert(shapeIndex,swObj);
        }
--- a/examples/StandaloneMain/main_tinyrenderer_single_example.cpp
+++ b/examples/StandaloneMain/main_tinyrenderer_single_example.cpp
@@ -253,8 +253,9 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
 	{
 		int shapeIndex = m_swRenderObjects.size();

-		TinyRenderObjectData* swObj = new TinyRenderObjectData(m_swWidth,m_swHeight,m_rgbColorBuffer,m_depthBuffer);
-        swObj->registerMeshShape(vertices,numvertices,indices,numIndices);
+		TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
+        float rgbaColor[4] = {1,1,1,1};
+	swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
 		//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
        m_swRenderObjects.insert(shapeIndex,swObj);
 		return shapeIndex;
--- a/examples/TinyRenderer/TinyRenderer.cpp
+++ b/examples/TinyRenderer/TinyRenderer.cpp
@@ -76,21 +76,20 @@ struct Shader : public IShader {
 		float diff = ambient+b3Min(b3Max(0.f, bn*m_light_dir_local),(1-ambient));
 		//float diff = b3Max(0.f, n*m_light_dir_local);
        color = m_model->diffuse(uv)*diff;
-        //colors are store in BGRA?
-        color = TGAColor(color[0]*m_colorRGBA[2],
-                            color[1]*m_colorRGBA[1],
-                            color[2]*m_colorRGBA[0],
-                            color[3]*m_colorRGBA[3]);
-
+        
+        //warning: bgra color is swapped to rgba to upload texture
+        color.bgra[0] *= m_colorRGBA[0];
+        color.bgra[1] *= m_colorRGBA[1];
+        color.bgra[2] *= m_colorRGBA[2];
+        color.bgra[3] *= m_colorRGBA[3];
+        
        return false;
    }
 };


-TinyRenderObjectData::TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
-:m_width(width),
-m_height(height),
-m_rgbColorBuffer(rgbColorBuffer),
+TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer)
+:m_rgbColorBuffer(rgbColorBuffer),
 m_depthBuffer(depthBuffer),
 m_model(0),
 m_userData(0),
@@ -102,11 +101,6 @@ m_userIndex(-1)
    m_lightDirWorld.setValue(0,0,0);
 	m_localScaling.setValue(1,1,1);
    m_modelMatrix = Matrix::identity();
-    m_viewMatrix = lookat(eye, center, up);
-    //m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
-	//m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
-	m_viewportMatrix = viewport(0,0,width,height);
-    m_projectionMatrix = projection(-1.f/(eye-center).norm());
 
 }

@@ -239,6 +233,9 @@ TinyRenderObjectData::~TinyRenderObjectData()

 void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
 {
+	int width = renderData.m_rgbColorBuffer.get_width();
+	int height = renderData.m_rgbColorBuffer.get_height();
+
 	Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]);
    Model* model = renderData.m_model;
    if (0==model)
@@ -246,13 +243,8 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
    
 	

-	//renderData.m_viewMatrix = lookat(eye, center, up);
-	int width = renderData.m_width;
-	int height = renderData.m_height;
-	//renderData.m_viewportMatrix = viewport(width/8, height/8, width*3/4, height*3/4);
-	renderData.m_viewportMatrix = viewport(0,0,renderData.m_width,renderData.m_height);
-    //renderData.m_projectionMatrix = projection(-1.f/(eye-center).norm());
-
+	renderData.m_viewportMatrix = viewport(0,0,width, height);
+	
    b3AlignedObjectArray<float>& zbuffer = renderData.m_depthBuffer;
    
    TGAImage& frame = renderData.m_rgbColorBuffer;
@@ -264,7 +256,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
        Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
        Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA());
        		
-		printf("Render %d triangles.\n",model->nfaces());
+		//printf("Render %d triangles.\n",model->nfaces());
 		for (int i=0; i<model->nfaces(); i++) 
 		{
 			 
--- a/examples/TinyRenderer/TinyRenderer.h
+++ b/examples/TinyRenderer/TinyRenderer.h
@@ -25,12 +25,11 @@ struct TinyRenderObjectData
    //class IShader* m_shader; todo(erwincoumans) expose the shader, for now we use a default shader
            
    //Output
-    int m_width;
-    int m_height;
+    
    TGAImage& m_rgbColorBuffer;
    b3AlignedObjectArray<float>& m_depthBuffer;
    
-    TinyRenderObjectData(int width, int height,TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
+    TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>& depthBuffer);
    virtual ~TinyRenderObjectData();
    
    void loadModel(const char* fileName);
--- a/examples/TinyRenderer/main.cpp
+++ b/examples/TinyRenderer/main.cpp
@@ -86,7 +86,7 @@ int main(int argc, char* argv[])
        b3AlignedObjectArray<float> depthBuffer;
 	depthBuffer.resize(gWidth*gHeight);
 
-	TinyRenderObjectData renderData(textureWidth, textureHeight,rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
+	TinyRenderObjectData renderData(rgbColorBuffer,depthBuffer);//, "african_head/african_head.obj");//floor.obj");
 	
 	//renderData.loadModel("african_head/african_head.obj");
 	renderData.loadModel("floor.obj");
--- a/examples/TinyRenderer/model.cpp
+++ b/examples/TinyRenderer/model.cpp
@@ -57,9 +57,9 @@ void Model::setDiffuseTextureFromData(unsigned char* textureImage,int textureWid
 		for (int j=0;j<textureHeight;j++)
 		{
 			TGAColor color;
-			color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
-			color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
-			color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
+            color.bgra[0] = textureImage[(i+j*textureWidth)*3+0];
+            color.bgra[1] = textureImage[(i+j*textureWidth)*3+1];
+            color.bgra[2] = textureImage[(i+j*textureWidth)*3+2];
 			color.bgra[3] = 255;

 			color.bytespp = 3;
--- a/examples/Utils/b3ResourcePath.cpp
+++ b/examples/Utils/b3ResourcePath.cpp
@@ -77,6 +77,12 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
 				{
 					return strlen(resourcePath);
 				}
+         sprintf(resourcePath,"%s.runfiles/google3/third_party/bullet/data/%s",exePath,resourceName);
+				//printf("try resource at %s\n", resourcePath);	
+				if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
+				{
+					return strlen(resourcePath);
+				}  
        	}
 	}

--- a/examples/pybullet/CMakeLists.txt
+++ b/examples/pybullet/CMakeLists.txt
@@ -9,6 +9,15 @@ INCLUDE_DIRECTORIES(
 SET(pybullet_SRCS
 	pybullet.c
 	../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
+      ../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+                                                                                                ../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
+                                                                                                ../../examples/OpenGLWindow/SimpleCamera.cpp
+                                                                                                ../../examples/OpenGLWindow/SimpleCamera.h
+                                                                                                ../../examples/TinyRenderer/geometry.cpp
+                                                                                                ../../examples/TinyRenderer/model.cpp
+                                                                                                ../../examples/TinyRenderer/tgaimage.cpp
+                                                                                                ../../examples/TinyRenderer/our_gl.cpp
+                                                                                                ../../examples/TinyRenderer/TinyRenderer.cpp
                        ../../examples/SharedMemory/InProcessMemory.cpp
                        ../../examples/SharedMemory/PhysicsClient.cpp
                        ../../examples/SharedMemory/PhysicsClient.h
@@ -40,6 +49,7 @@ SET(pybullet_SRCS
                        ../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
                        ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
                        ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
+			../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
                        ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
                        ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
                        ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
@@ -48,8 +58,8 @@ SET(pybullet_SRCS
                        ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
                        ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
                        ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
-			../../examples/Importers/ImportURDFDemo/DefaultVisualShapeConverter.cpp
-                        ../../examples/MultiThreading/b3PosixThreadSupport.cpp
+			../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp 
+			../../examples/MultiThreading/b3PosixThreadSupport.cpp
                        ../../examples/MultiThreading/b3Win32ThreadSupport.cpp
 			../../examples/MultiThreading/b3ThreadSupportInterface.cpp

--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
--- a/src/BulletCollision/CollisionShapes/btConeShape.h
+++ b/src/BulletCollision/CollisionShapes/btConeShape.h
@@ -43,6 +43,15 @@ public:
 	btScalar getRadius() const { return m_radius;}
 	btScalar getHeight() const { return m_height;}

+	void setRadius(const btScalar radius)
+	{
+		m_radius = radius;
+	}
+	void setHeight(const btScalar height)
+	{
+		m_height = height;
+	}
+

 	virtual void	calculateLocalInertia(btScalar mass,btVector3& inertia) const
 	{
--- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
+++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
@@ -145,11 +145,6 @@ public:
 		// lo and hi limits for variables (set to -/+ infinity on entry).
 		btScalar *m_lowerLimit,*m_upperLimit;

-		// findex vector for variables. see the LCP solver interface for a
-		// description of what this does. this is set to -1 on entry.
-		// note that the returned indexes are relative to the first index of
-		// the constraint.
-		int *findex;
 		// number of solver iterations
 		int m_numIterations;

--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
@@ -23,7 +23,10 @@ subject to the following restrictions:

 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
-	m_desiredVelocity(desiredVelocity)
+	m_desiredVelocity(desiredVelocity),
+	m_desiredPosition(0),
+	m_kd(0.1),
+	m_kp(0)
 {

 	m_maxAppliedImpulse = maxMotorImpulse;
@@ -51,7 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof()
 btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
 	//:btMultiBodyConstraint(body,0,link,-1,1,true),
 	:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
-	m_desiredVelocity(desiredVelocity)
+	m_desiredVelocity(desiredVelocity),
+	m_desiredPosition(0),
+	m_kd(0.1),
+	m_kp(0)
 {
 	btAssert(linkDoF < body->getLink(link).m_dofCount);

@@ -113,9 +119,22 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 	for (int row=0;row<getNumRows();row++)
 	{
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-
-
-		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
+
+        int dof = 0;
+        btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
+        btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+        btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
+        btScalar velocityError = (m_desiredVelocity - currentVelocity);
+        btScalar rhs =   m_kp * positionStabiliationTerm + m_kd * velocityError;
+        printf("m_kd = %f\n", m_kd);
+        printf("m_kp = %f\n", m_kp);
+        printf("m_desiredVelocity = %f\n", m_desiredVelocity);
+        printf("m_desiredPosition = %f\n", m_desiredPosition);
+        printf("m_maxAppliedImpulse = %f\n", m_maxAppliedImpulse);
+        printf("rhs = %f\n", rhs);
+        
+        
+		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs);
 		constraintRow.m_orgConstraint = this;
 		constraintRow.m_orgDofIndex = row;
 		{
--- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
@@ -25,8 +25,11 @@ class btMultiBodyJointMotor : public btMultiBodyConstraint
 {
 protected:

-
 	btScalar	m_desiredVelocity;
+	btScalar	m_desiredPosition;
+	btScalar    m_kd;
+	btScalar    m_kp;
+	

 public:

@@ -42,11 +45,19 @@ public:
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal);

-    virtual void setVelocityTarget(btScalar velTarget)
+    virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 0.1f)
    {
        m_desiredVelocity = velTarget;
+        m_kd = kd;
    }

+    virtual void setPositionTarget(btScalar posTarget, btScalar kp = 0.1f)
+    {
+        m_desiredPosition = posTarget;
+        m_kp = kp;
+    }
+    
+
 	virtual void debugDraw(class btIDebugDraw* drawer)
 	{
 		//todo(erwincoumans)
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -132,6 +132,8 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame

 	const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
 	const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
+    const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user!
+    
 	btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
 	btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.

@@ -149,6 +151,7 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 			m_jointFeedback(0),
 			m_linkName(0),
 			m_jointName(0),
+            m_userPtr(0),
 			m_jointDamping(0),
 			m_jointFriction(0)
 	{
--- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
+++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp
@@ -48,9 +48,9 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }

 class vecx : public btVectorX<idScalar> {
 public:
-	vecx(int size) : btVectorX(size) {}
+	vecx(int size) : btVectorX<idScalar>(size) {}
 	const vecx& operator=(const btVectorX<idScalar>& rhs) {
-		*static_cast<btVectorX*>(this) = rhs;
+		*static_cast<btVectorX<idScalar>*>(this) = rhs;
 		return *this;
 	}

--- a/src/LinearMath/btMatrix3x3.h
+++ b/src/LinearMath/btMatrix3x3.h
@@ -1329,7 +1329,9 @@ SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
    c0 = _mm_and_ps(c0, c1);
    c0 = _mm_and_ps(c0, c2);

-    return (0x7 == _mm_movemask_ps((__m128)c0));
+	int m = _mm_movemask_ps((__m128)c0);
+	return (0x7 == (m & 0x7));
+	
 #else 
 	return 
    (   m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
--- a/test/InverseDynamics/premake4.lua
+++ b/test/InverseDynamics/premake4.lua
@@ -78,6 +78,8 @@
        "../../examples/Importers/ImportURDFDemo/UrdfParser.h",
        "../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
        "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h",
+	"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+        "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
        "../../examples/Utils/b3Clock.cpp",
        "../../Extras/Serialize/BulletWorldImporter/*",
        "../../Extras/Serialize/BulletFileLoader/*",
--- a/test/SharedMemory/CMakeLists.txt
+++ b/test/SharedMemory/CMakeLists.txt
@@ -71,7 +71,8 @@ ENDIF()
                        ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
                        ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
                        ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
-
+			../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+			../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
                        
 	)

--- a/test/SharedMemory/premake4.lua
+++ b/test/SharedMemory/premake4.lua
@@ -91,7 +91,9 @@ project ("Test_PhysicsServerLoopBack")
 			"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
-		}
+			"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
+	}
 		
 		project ("Test_PhysicsServerDirect")

@@ -157,6 +159,8 @@ project ("Test_PhysicsServerLoopBack")
 			"../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp",
 			"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
+			"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",     	
 		}
 		

@@ -255,8 +259,10 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
                        "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
 			"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
 			"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
-                "../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
-			}
+                	"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
+			"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+                        "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
+	}
 	if os.is("Linux") then
       		initX11()
 	end
--- a/test/SharedMemory/test.c
+++ b/test/SharedMemory/test.c
@@ -35,6 +35,7 @@ void testSharedMemory(b3PhysicsClientHandle sm)
    int sensorJointIndexLeft=-1;
    int sensorJointIndexRight=-1;
 	const char* urdfFileName = "r2d2.urdf";
+	const char* sdfFileName = "kuka_iiwa/model.sdf";
 	double gravx=0, gravy=0, gravz=-9.8;
 	double timeStep = 1./60.;
 	double startPosX, startPosY,startPosZ;
@@ -52,7 +53,53 @@ void testSharedMemory(b3PhysicsClientHandle sm)
        ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
        }

-		
+        {
+            b3SharedMemoryStatusHandle statusHandle;
+            int statusType;
+            int bodyIndicesOut[10];//MAX_SDF_BODIES = 10
+            int numJoints, numBodies;
+            b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
+            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+			statusType = b3GetStatusType(statusHandle);
+			ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
+			
+			numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
+            ASSERT_EQ(numBodies,1);
+            int bodyUniqueId = bodyIndicesOut[0];
+            
+            numJoints = b3GetNumJoints(sm,bodyUniqueId);
+            ASSERT_EQ(numJoints,7);
+
+#if 0
+            b3Printf("numJoints: %d\n", numJoints);
+            for (i=0;i<numJoints;i++)
+            {
+                struct b3JointInfo jointInfo;
+                if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
+                {
+                    b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
+                }
+            }
+#endif
+            {
+                b3SharedMemoryStatusHandle statusHandle;
+                b3SharedMemoryCommandHandle commandHandle;
+                double jointAngle = 0.f;
+               	int jointIndex; 
+                commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
+                for (jointIndex=0;jointIndex<numJoints;jointIndex++)
+                {
+                    b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
+                }
+                
+                statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+
+                ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+            }
+            
+        }
+        
+        
        {
            b3SharedMemoryStatusHandle statusHandle;
 			int statusType;
@@ -98,7 +145,7 @@ void testSharedMemory(b3PhysicsClientHandle sm)
        
 			if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
 			{
-				b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
+				b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
 				b3SharedMemoryStatusHandle statusHandle;
 				if (imuLinkIndex>=0)
 				{