Repository: bulletphysics/bullet3

Issue:
Support GPU ray cast
GPU accelerated ray casting should be added. 

It can be used for picking, line-of-sight queries and graphics related queries (shadow generation, ray tracer etc). There is a basic demo as a starting point. It supports GPU ray cast against spheres. It needs to be extended to support all collision shapes.

When running out of GPU memory, don't crash
Currently, the GPU OpenCL rigid body pipeline ignores out-of-GPU-memory or other OpenCL errors. We need to deal with it properly.



Discussion:
Latest trunk has GPU raycast for sphere and convex shapes. It can be used for picking and for basic raytracing. The App_BasicGpuDemo uses it for picking, and the App_Bullet3\* demo includes a basic raytraced test.

I'm starting to make some effort on accelerating the GPU raycaster in this branch
(work in progress, not ready for review):
https://github.com/rtrius/bullet3/tree/plbvh_raycast

Presently, the branch implements a Linear BVH that runs on the GPU.
Only the PairBench demo, without large AABBs, is currently supported.

Using the LBVH for the broadphase is ~1-2 ms slower than the 1-axis GPU SAP broadphase. However, the BVH construction algorithm is of extremely low quality and it has yet to be optimized, so there should be a large amount of room for improvement.

That is very exciting! So you are building the tree on the GPU, and also the traversal?
Please keep me posted if you have something to merge, then j can review and pull/merge!

The plbvh_raycast branch is now ready for evaluation.

The latest commit includes slides in the /docs folder that explains the LBVH construction. Additionally, both construction and traversal should be functional and reasonably optimized.

The raycaster is also acclerated using the BVH. With the acceleration, most of the time in the raycasting demo is spent in ray generation and CPU-GPU data transfer. However, ray-triangle mesh and ray-compound tests are still not implemented.

The current method for raycasting is:
    -Update BVH using AABBs from any broadphase
    -BVH traversal generates {ray_index, rigid_index} pairs
    -Sort {ray_index, rigid_index} pairs by ray_index
    -Launch ray-rigid narrowphase/intersection kernel, 1 thread per ray

In my tests, the Linear BVH performs better than the 1-axis SAP for dense scenes, but worse for sparse scenes. Both broadphases are worse than the grid, due to the uniform size of objects. With 27000 AABBs, the break even point where the PLBVH starts to outperform the 1-axis SAP in the PairBench is about scale 76(~5700 overlapping pairs).

```
Some timings for the PairBench with 30x30x30 == 27000 objects, Radeon 5850HD:

At scale: 20 (~226000 overlapping pairs)
    GPU Grid: 9.9 ms
    GPU 1-SAP LDS: 13 ms
    PLBVH: 10.1 ms (3 ms construction, 7 ms traversal)

At scale: 300 (~100 overlapping pairs)
    GPU Grid: 1.9 ms
    GPU 1-SAP LDS: 2.5 ms
    PLBVH: 4 ms (2.8 ms construction, 1.2 ms traversal)
```

Please let me know if there are any issues or places for improvement.

Thanks a lot, that sounds good! 
You provided the timings for finding the overlapping pairs, right?
Do you have any timings for the actual ray test? There is a noticeable slow-down when doing a ray test for many objects, when using the 1-SAP and Grid, because ray test is brute force.
How are the timings of the ray test using the BVH compare?

Here are some timings for the RaycastShadowDemo with the simulation paused,
and the same GPU as above. (Edit: texture size is 512 x 512 == 262 144 rays.)
- ms/frame includes ray generation, CPU-GPU transfer, and writing the result to OpenGL.
- 'primary rays' and 'shadow rays' is the time spent executing BVH/raycast kernels.

## Raycast Scene 1

Timings for Scene 1 (5 x 50 x 5 = 1250 dynamic rigids):
- Brute force: 716ms/frame (294ms primary rays, 313ms shadow rays)
- BVH: 120 ms/frame (25ms primary rays, 26ms shadow rays)

Timings with same camera position as Scene 1, but fewer rigid bodies
(2 x 50 x 2 = 200 dynamic rigids):
- Brute force: 211 ms/frame (48ms primary rays, 50ms shadow rays )
- BVH: 106 ms/frame (14ms primary rays, 13ms shadow rays)
  ![raycastscene1](https://f.cloud.github.com/assets/1758207/2429301/e2e3b976-ac77-11e3-9103-5db090547493.png)

## Raycast Scene 2

Timings for Scene2 (20 x 20 x 20 = 8000 dynamic rigids)
- Brute force: 4100ms/frame (2100ms primary rays, 1900ms shadow rays)
- BVH: 236 ms/frame (85ms primary rays, 34ms shadow rays)
  ![raycastscene2](https://f.cloud.github.com/assets/1758207/2429302/ea24545c-ac77-11e3-8851-63e6830cee67.png)

## Actual profiling output for 'Raycast Scene 2'

(The time for 'Root' over multiple frames varies from 221ms to 244ms):
Profiling: Root (total running time: 235.173 ms) ---
0 -- glFinish (0.00 %) :: 0.000 ms / frame (1 calls)
1 -- window->endRendering (0.22 %) :: 0.521 ms / frame (1 calls)
2 -- gui->draw (1.37 %) :: 3.215 ms / frame (1 calls)
3 -- renderScene (98.30 %) :: 231.175 ms / frame (1 calls)
4 -- texture (0.00 %) :: 0.000 ms / frame (0 calls)
Unaccounted: (0.111 %) :: 0.262 ms
...----------------------------------
...Profiling: renderScene (total running time: 231.175 ms) ---
...0 -- raytrace (99.98 %) :: 231.140 ms / frame (1 calls)
...Unaccounted: (0.015 %) :: 0.035 ms
......----------------------------------
......Profiling: raytrace (total running time: 231.140 ms) ---
......0 -- drawTexturedRect (0.29 %) :: 0.672 ms / frame (1 calls)
......1 -- glGetError (0.00 %) :: 0.001 ms / frame (2 calls)
......2 -- glTexImage2D (0.32 %) :: 0.739 ms / frame (1 calls)
......3 -- get error (0.00 %) :: 0.006 ms / frame (1 calls)
......4 -- write texels (7.16 %) :: 16.544 ms / frame (1 calls)
......5 -- cast shadow rays (19.94 %) :: 46.086 ms / frame (1 calls)
......6 -- init shadow rays (6.98 %) :: 16.132 ms / frame (1 calls)
......7 -- shadowHits.resize (5.88 %) :: 13.596 ms / frame (1 calls)
......8 -- shadowRays.resize (3.99 %) :: 9.223 ms / frame (1 calls)
......9 -- cast primary rays (39.27 %) :: 90.778 ms / frame (1 calls)
......10 -- init hits (1.56 %) :: 3.612 ms / frame (1 calls)
......11 -- hits.resize (3.72 %) :: 8.602 ms / frame (1 calls)
......12 -- Generate primary rays (7.90 %) :: 18.258 ms / frame (1 calls)
......13 -- readbackAllBodiesToCpu (0.47 %) :: 1.079 ms / frame (1 calls)
......14 -- update camera (0.00 %) :: 0.005 ms / frame (1 calls)
......Unaccounted: (2.512 %) :: 5.807 ms
.........----------------------------------
.........Profiling: cast shadow rays (total running time: 46.086 ms) ---
.........0 -- castRaysGPU (99.99 %) :: 46.082 ms / frame (1 calls)
.........Unaccounted: (0.009 %) :: 0.004 ms
............----------------------------------
............Profiling: castRaysGPU (total running time: 46.082 ms) ---
............0 -- raycast copyToHost (11.08 %) :: 5.108 ms / frame (1 calls)
............1 -- ray-rigid intersection (6.17 %) :: 2.845 ms / frame (1 calls)
............2 -- detect ray-rigid pair index ranges (9.96 %) :: 4.588 ms / frame (1 calls)
............3 -- sort ray-rigid pairs (0.26 %) :: 0.121 ms / frame (1 calls)
............4 -- PLBVH testRaysAgainstBvhAabbs() (46.53 %) :: 21.443 ms / frame (1 calls)
............5 -- b3ParallelLinearBvh::build() (6.45 %) :: 2.972 ms / frame (1 calls)
............6 -- raycast copyFromHost (19.11 %) :: 8.807 ms / frame (1 calls)
............Unaccounted: (0.430 %) :: 0.198 ms
...............----------------------------------
...............Profiling: detect ray-rigid pair index ranges (total running time: 4.588 ms) ---
...............0 -- reset ray-rigid pair index ranges (68.55 %) :: 3.145 ms / frame (1 calls)
...............Unaccounted: (31.452 %) :: 1.443 ms
...............----------------------------------
...............Profiling: PLBVH testRaysAgainstBvhAabbs() (total running time: 21.443 ms) ---
...............0 -- PLBVH ray test large AABB (19.36 %) :: 4.151 ms / frame (1 calls)
...............1 -- PLBVH ray test small AABB (78.53 %) :: 16.839 ms / frame (1 calls)
...............Unaccounted: (2.113 %) :: 0.453 ms
...............----------------------------------
...............Profiling: b3ParallelLinearBvh::build() (total running time: 2.972 ms) ---
...............0 -- m_findLeafIndexRangesKernel (4.41 %) :: 0.131 ms / frame (1 calls)
...............1 -- b3GpuParallelLinearBvh::constructBinaryRadixTree() (37.18 %) :: 1.105 ms / frame (1 calls)
...............2 -- Sort leaves by morton codes (23.82 %) :: 0.708 ms / frame (1 calls)
...............3 -- Assign morton codes (4.07 %) :: 0.121 ms / frame (1 calls)
...............4 -- Find AABB of merged nodes (9.35 %) :: 0.278 ms / frame (1 calls)
...............5 -- Separate large and small AABBs (20.83 %) :: 0.619 ms / frame (1 calls)
...............Unaccounted: (0.336 %) :: 0.010 ms
..................----------------------------------
..................Profiling: b3GpuParallelLinearBvh::constructBinaryRadixTree() (total running time: 1.105 ms) ---
..................0 -- m_buildBinaryRadixTreeAabbsRecursiveKernel (46.15 %) :: 0.510 ms / frame (1 calls)
..................1 -- m_findDistanceFromRootKernel (12.94 %) :: 0.143 ms / frame (1 calls)
..................2 -- m_buildBinaryRadixTreeInternalNodesKernel (19.00 %) :: 0.210 ms / frame (1 calls)
..................3 -- m_buildBinaryRadixTreeLeafNodesKernel (10.68 %) :: 0.118 ms / frame (1 calls)
..................4 -- m_computeAdjacentPairCommonPrefixKernel (11.04 %) :: 0.122 ms / frame (1 calls)
..................Unaccounted: (0.181 %) :: 0.002 ms
.....................----------------------------------
.....................Profiling: m_buildBinaryRadixTreeAabbsRecursiveKernel (total running time: 0.510 ms) ---
.....................0 -- copy maxDistanceFromRoot to CPU (30.39 %) :: 0.155 ms / frame (1 calls)
.....................Unaccounted: (69.608 %) :: 0.355 ms
.........----------------------------------
.........Profiling: cast primary rays (total running time: 90.778 ms) ---
.........0 -- castRaysGPU (100.00 %) :: 90.775 ms / frame (1 calls)
.........Unaccounted: (0.003 %) :: 0.003 ms
............----------------------------------
............Profiling: castRaysGPU (total running time: 90.775 ms) ---
............0 -- raycast copyToHost (5.46 %) :: 4.957 ms / frame (1 calls)
............1 -- ray-rigid intersection (7.01 %) :: 6.367 ms / frame (1 calls)
............2 -- detect ray-rigid pair index ranges (17.13 %) :: 15.552 ms / frame (1 calls)
............3 -- sort ray-rigid pairs (0.13 %) :: 0.122 ms / frame (1 calls)
............4 -- PLBVH testRaysAgainstBvhAabbs() (57.84 %) :: 52.507 ms / frame (1 calls)
............5 -- b3ParallelLinearBvh::build() (3.37 %) :: 3.059 ms / frame (1 calls)
............6 -- raycast copyFromHost (8.83 %) :: 8.014 ms / frame (1 calls)
............Unaccounted: (0.217 %) :: 0.197 ms
...............----------------------------------
...............Profiling: detect ray-rigid pair index ranges (total running time: 15.552 ms) ---
...............0 -- reset ray-rigid pair index ranges (49.18 %) :: 7.649 ms / frame (1 calls)
...............Unaccounted: (50.817 %) :: 7.903 ms
...............----------------------------------
...............Profiling: PLBVH testRaysAgainstBvhAabbs() (total running time: 52.507 ms) ---
...............0 -- PLBVH ray test large AABB (9.62 %) :: 5.053 ms / frame (1 calls)
...............1 -- PLBVH ray test small AABB (89.46 %) :: 46.975 ms / frame (1 calls)
...............Unaccounted: (0.912 %) :: 0.479 ms
...............----------------------------------
...............Profiling: b3ParallelLinearBvh::build() (total running time: 3.059 ms) ---
...............0 -- m_findLeafIndexRangesKernel (4.09 %) :: 0.125 ms / frame (1 calls)
...............1 -- b3GpuParallelLinearBvh::constructBinaryRadixTree() (36.81 %) :: 1.126 ms / frame (1 calls)
...............2 -- Sort leaves by morton codes (24.75 %) :: 0.757 ms / frame (1 calls)
...............3 -- Assign morton codes (3.89 %) :: 0.119 ms / frame (1 calls)
...............4 -- Find AABB of merged nodes (9.02 %) :: 0.276 ms / frame (1 calls)
...............5 -- Separate large and small AABBs (21.22 %) :: 0.649 ms / frame (1 calls)
...............Unaccounted: (0.229 %) :: 0.007 ms
..................----------------------------------
..................Profiling: b3GpuParallelLinearBvh::constructBinaryRadixTree() (total running time: 1.126 ms) ---
..................0 -- m_buildBinaryRadixTreeAabbsRecursiveKernel (46.80 %) :: 0.527 ms / frame (1 calls)
..................1 -- m_findDistanceFromRootKernel (12.70 %) :: 0.143 ms / frame (1 calls)
..................2 -- m_buildBinaryRadixTreeInternalNodesKernel (18.56 %) :: 0.209 ms / frame (1 calls)
..................3 -- m_buildBinaryRadixTreeLeafNodesKernel (10.66 %) :: 0.120 ms / frame (1 calls)
..................4 -- m_computeAdjacentPairCommonPrefixKernel (10.83 %) :: 0.122 ms / frame (1 calls)
..................Unaccounted: (0.444 %) :: 0.005 ms
.....................----------------------------------
.....................Profiling: m_buildBinaryRadixTreeAabbsRecursiveKernel (total running time: 0.527 ms) ---
.....................0 -- copy maxDistanceFromRoot to CPU (31.88 %) :: 0.168 ms / frame (1 calls)
.....................Unaccounted: (68.121 %) :: 0.359 ms

Most crashes due to memory-overflow should be solved in latest trunk.


Patch:
diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt
index 8abac18f51..b2c4523b6d 100644
--- a/Extras/BulletRobotics/CMakeLists.txt
+++ b/Extras/BulletRobotics/CMakeLists.txt
@@ -120,6 +120,10 @@ ELSE(WIN32)
 	IF(BUILD_CLSOCKET)	
 		ADD_DEFINITIONS(${OSDEF})
 	ENDIF(BUILD_CLSOCKET)
+		
+	IF(NOT APPLE)
+		LINK_LIBRARIES(  pthread ${DL} )
+	ENDIF(APPLE)
 ENDIF(WIN32)
 
 IF(BUILD_ENET)
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 2554ac4fa3..aec69df379 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -175,6 +175,8 @@ SET(BulletExampleBrowser_SRCS
 	../SharedMemory/b3PluginManager.cpp
 	../RobotSimulator/b3RobotSimulatorClientAPI.cpp
 	../RobotSimulator/b3RobotSimulatorClientAPI.h
+	../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp
+	../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
 	../BasicDemo/BasicExample.cpp
 	../BasicDemo/BasicExample.h
 	../InverseDynamics/InverseDynamicsExample.cpp
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 56789385cd..65e38ece86 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -122,6 +122,8 @@ project "App_BulletExampleBrowser"
 		"../InverseDynamics/InverseDynamicsExample.h",
 		"../RobotSimulator/b3RobotSimulatorClientAPI.cpp",
 		"../RobotSimulator/b3RobotSimulatorClientAPI.h",		
+		"../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp",
+		"../RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h",		
 		"../BasicDemo/BasicExample.*",
 		"../Tutorial/*",
 		"../ExtendedTutorials/*",
diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt
index d0bb13eaf9..0647edf72e 100644
--- a/examples/RobotSimulator/CMakeLists.txt
+++ b/examples/RobotSimulator/CMakeLists.txt
@@ -1,4 +1,3 @@
-
 INCLUDE_DIRECTORIES( 
 		${BULLET_PHYSICS_SOURCE_DIR}/src
 		${BULLET_PHYSICS_SOURCE_DIR}/examples
@@ -9,77 +8,20 @@ INCLUDE_DIRECTORIES(
 
 
 SET(RobotSimulator_SRCS
-		RobotSimulatorMain.cpp
-		b3RobotSimulatorClientAPI.cpp
-		b3RobotSimulatorClientAPI.h	
-		MinitaurSetup.cpp
-		MinitaurSetup.h
-		../../examples/SharedMemory/IKTrajectoryHelper.cpp
-		../../examples/SharedMemory/IKTrajectoryHelper.h
-		../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
-		../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
-		../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
-		../../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
-		../../examples/SharedMemory/PhysicsServer.cpp
-		../../examples/SharedMemory/PhysicsServer.h
-		../../examples/SharedMemory/PhysicsServerExample.cpp
-		../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
-		../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
-		../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
-		../../examples/SharedMemory/PhysicsServerSharedMemory.h
-		../../examples/SharedMemory/PhysicsDirect.cpp
-		../../examples/SharedMemory/PhysicsDirect.h
-		../../examples/SharedMemory/PhysicsDirectC_API.cpp
-		../../examples/SharedMemory/PhysicsDirectC_API.h
-		../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
-		../../examples/SharedMemory/PhysicsServerCommandProcessor.h
-		../../examples/SharedMemory/b3PluginManager.cpp
-		
-		../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
-		../../examples/SharedMemory/PhysicsClientSharedMemory.h
-		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
-		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
-		../../examples/SharedMemory/PhysicsClientC_API.cpp
-		../../examples/SharedMemory/PhysicsClientC_API.h
-		../../examples/SharedMemory/Win32SharedMemory.cpp
-		../../examples/SharedMemory/Win32SharedMemory.h
-		../../examples/SharedMemory/PosixSharedMemory.cpp
-		../../examples/SharedMemory/PosixSharedMemory.h
-		../../examples/Utils/b3ResourcePath.cpp
-		../../examples/Utils/b3ResourcePath.h
-		../../examples/Utils/RobotLoggingUtil.cpp
-		../../examples/Utils/RobotLoggingUtil.h
-		../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
-		../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
-		../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
-		../../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
-		../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
-		../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
-		../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
-		../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
-		../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
-		../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
-		../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp 
-		../../examples/MultiThreading/b3PosixThreadSupport.cpp
-		../../examples/MultiThreading/b3Win32ThreadSupport.cpp
-		../../examples/MultiThreading/b3ThreadSupportInterface.cpp
-		
-)
+RobotSimulatorMain.cpp
+	b3RobotSimulatorClientAPI.cpp
+	b3RobotSimulatorClientAPI.h	
+	b3RobotSimulatorClientAPI_NoGUI.cpp
+	b3RobotSimulatorClientAPI_NoGUI.h	
+	MinitaurSetup.cpp
+	MinitaurSetup.h
+	../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
+	../../examples/SharedMemory/PhysicsServerExample.cpp
+	../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
+	../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp		
+)	
+
+
 
 IF(BUILD_CLSOCKET)
  ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
@@ -89,54 +31,9 @@ IF(WIN32)
 	LINK_LIBRARIES(
 		${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
 	)
-	IF(BUILD_ENET)
-		ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET)
-	ENDIF(BUILD_ENET)
-	IF(BUILD_CLSOCKET)
-		ADD_DEFINITIONS(-DWIN32)
-	ENDIF(BUILD_CLSOCKET)
-
-ELSE(WIN32)
-	IF(BUILD_ENET)
-		ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET)
-	ENDIF(BUILD_ENET)
-
-	IF(BUILD_CLSOCKET)	
-		ADD_DEFINITIONS(${OSDEF})
-	ENDIF(BUILD_CLSOCKET)
 ENDIF(WIN32)
 
 
-IF(BUILD_ENET)
-	set(RobotSimulator_SRCS ${RobotSimulator_SRCS} 
-		../../examples/SharedMemory/PhysicsClientUDP.cpp
-		../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp
-		../../examples/SharedMemory/PhysicsClientUDP.h		
-		../../examples/SharedMemory/PhysicsClientUDP_C_API.h
-		../../examples/ThirdPartyLibs/enet/win32.c
-		../../examples/ThirdPartyLibs/enet/unix.c
-		../../examples/ThirdPartyLibs/enet/callbacks.c
-		../../examples/ThirdPartyLibs/enet/compress.c
-		../../examples/ThirdPartyLibs/enet/host.c
-		../../examples/ThirdPartyLibs/enet/list.c
-		../../examples/ThirdPartyLibs/enet/packet.c
-		../../examples/ThirdPartyLibs/enet/peer.c
-		../../examples/ThirdPartyLibs/enet/protocol.c
-	)
-ENDIF(BUILD_ENET)
-
-IF(BUILD_CLSOCKET)
-	set(RobotSimulator_SRCS ${RobotSimulator_SRCS}
-		 ../../examples/SharedMemory/PhysicsClientTCP.cpp
-                 ../../examples/SharedMemory/PhysicsClientTCP.h
-                 ../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp
-                 ../../examples/SharedMemory/PhysicsClientTCP_C_API.h
-		 ../../examples/ThirdPartyLibs/clsocket/src/SimpleSocket.cpp
-		 ../../examples/ThirdPartyLibs/clsocket/src/ActiveSocket.cpp
-		 ../../examples/ThirdPartyLibs/clsocket/src/PassiveSocket.cpp
-	)
-ENDIF()
-
 
 #some code to support OpenGL and Glew cross platform
 IF (WIN32)
@@ -168,6 +65,8 @@ ADD_EXECUTABLE(App_RobotSimulator  ${RobotSimulator_SRCS})
 
 SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES VERSION ${BULLET_VERSION})
 SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d")
+SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES COMPILE_DEFINITIONS "B3_USE_ROBOTSIM_GUI")
+
 
 
 IF(WIN32)
@@ -176,8 +75,56 @@ IF(WIN32)
 	ENDIF(BUILD_ENET OR BUILD_CLSOCKET)
 ENDIF(WIN32)
 
+TARGET_LINK_LIBRARIES(App_RobotSimulator BulletRobotics BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen Bullet3Common)
+
+
+
+
+
+
+
+INCLUDE_DIRECTORIES( 
+		${BULLET_PHYSICS_SOURCE_DIR}/src
+		${BULLET_PHYSICS_SOURCE_DIR}/examples
+		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
+		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
+		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
+  )
+
+SET(RobotSimulator_NoGUI_SRCS
+	RobotSimulatorMain.cpp
+	b3RobotSimulatorClientAPI_NoGUI.cpp
+	b3RobotSimulatorClientAPI_NoGUI.h	
+	MinitaurSetup.cpp
+	MinitaurSetup.h
+)
+
+ADD_EXECUTABLE(App_RobotSimulator_NoGUI  ${RobotSimulator_NoGUI_SRCS})
+
+SET_TARGET_PROPERTIES(App_RobotSimulator_NoGUI PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(App_RobotSimulator_NoGUI PROPERTIES DEBUG_POSTFIX "_d")
+
+
+IF(WIN32)
+	IF(BUILD_ENET OR BUILD_CLSOCKET)
+		TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI ws2_32 )
+	ENDIF(BUILD_ENET OR BUILD_CLSOCKET)
+ELSE()
+	IF(APPLE)
+	ELSE(APPLE)
+		TARGET_LINK_LIBRARIES( App_RobotSimulator_NoGUI pthread ${DL} )
+	ENDIF(APPLE)
+ENDIF(WIN32)
+
+TARGET_LINK_LIBRARIES(App_RobotSimulator_NoGUI BulletRobotics BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath Bullet3Common)
+
+
+
+
+
+
+
 
 
-TARGET_LINK_LIBRARIES(App_RobotSimulator BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common)
 
 
diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp
index ec4a174e44..16d1020727 100644
--- a/examples/RobotSimulator/MinitaurSetup.cpp
+++ b/examples/RobotSimulator/MinitaurSetup.cpp
@@ -1,5 +1,5 @@
 #include "MinitaurSetup.h"
-#include "b3RobotSimulatorClientAPI.h"
+#include "b3RobotSimulatorClientAPI_NoGUI.h"
 
 #include "Bullet3Common/b3HashMap.h"
 
@@ -27,7 +27,7 @@ MinitaurSetup::~MinitaurSetup()
 	delete m_data;
 }
 
-void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd)
+void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd)
 {
 	b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
 	controlArgs.m_maxTorqueValue = maxTorque;
@@ -158,7 +158,7 @@ static const char* minitaurURDF="quadruped/minitaur_rainbow_dash_v1.urdf";
 #endif
 
 
-void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim)
+void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim)
 {
 	//release all motors
 	int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
@@ -255,7 +255,7 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim)
 
 }
 
-int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn)
+int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn)
 {
 	
 	b3RobotSimulatorLoadUrdfFileArgs args;
diff --git a/examples/RobotSimulator/MinitaurSetup.h b/examples/RobotSimulator/MinitaurSetup.h
index a054ade928..6d9f8c211f 100644
--- a/examples/RobotSimulator/MinitaurSetup.h
+++ b/examples/RobotSimulator/MinitaurSetup.h
@@ -6,15 +6,15 @@
 class MinitaurSetup
 {
 	struct MinitaurSetupInternalData* m_data;
-	void resetPose(class b3RobotSimulatorClientAPI* sim);
+	void resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim);
 
 public:
 	MinitaurSetup();
 	virtual ~MinitaurSetup();
 
-	int setupMinitaur(class b3RobotSimulatorClientAPI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1));
+	int setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1));
 
-	void setDesiredMotorAngle(class b3RobotSimulatorClientAPI* sim, const char* motorName, double desiredAngle, double maxTorque=3,double kp=0.1, double kd=0.9);
+	void setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque=3,double kp=0.1, double kd=0.9);
 
 };
 #endif //MINITAUR_SIMULATION_SETUP_H
diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp
index c3711f20f9..2a6b90584b 100644
--- a/examples/RobotSimulator/RobotSimulatorMain.cpp
+++ b/examples/RobotSimulator/RobotSimulatorMain.cpp
@@ -1,5 +1,11 @@
 
-#include "b3RobotSimulatorClientAPI.h"
+#ifdef B3_USE_ROBOTSIM_GUI
+	#include "b3RobotSimulatorClientAPI.h"
+#else
+	#include "b3RobotSimulatorClientAPI_NoGUI.h"
+#endif
+
+
 #include "../Utils/b3Clock.h"
 
 #include <string.h>
@@ -7,11 +13,23 @@
 #include <assert.h>
 #define ASSERT_EQ(a,b) assert((a)==(b));
 #include "MinitaurSetup.h"
+
+
+
 int main(int argc, char* argv[])
 {
+#ifdef B3_USE_ROBOTSIM_GUI
 	b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();
-
-	sim->connect(eCONNECT_GUI);
+	bool isConnected = sim->connect(eCONNECT_GUI);
+#else
+	b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI();
+	bool isConnected = sim->connect(eCONNECT_DIRECT);
+#endif
+	if (!isConnected)
+	{
+		printf("Cannot connect\n");
+		return -1;
+	}
 	//Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example:
 	//sim->connect(eCONNECT_UDP, "localhost", 1234);
 	sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
index 1a7bf5b773..f22e4c0aa1 100644
--- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
+++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
@@ -1,9 +1,7 @@
 #include "b3RobotSimulatorClientAPI.h"
 
-//#include "SharedMemoryCommands.h"
-
 #include "../SharedMemory/PhysicsClientC_API.h"
-
+#include "b3RobotSimulatorClientAPI_InternalData.h""
 #ifdef BT_ENABLE_ENET
 #include "../SharedMemory/PhysicsClientUDP_C_API.h"
 #endif  //PHYSICS_UDP
@@ -16,36 +14,20 @@
 
 #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
 
-
 #include "../SharedMemory/SharedMemoryPublic.h"
 #include "Bullet3Common/b3Logging.h"
 
-struct b3RobotSimulatorClientAPI_InternalData
-{
-	b3PhysicsClientHandle m_physicsClientHandle;
-	struct GUIHelperInterface* m_guiHelper;
-
-	b3RobotSimulatorClientAPI_InternalData()
-		: m_physicsClientHandle(0),
-		m_guiHelper(0)
-	{
-	}
-};
 
 b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
 {
-	m_data = new b3RobotSimulatorClientAPI_InternalData();
+	
 }
 
 b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
 {
-	delete m_data;
 }
 
-void b3RobotSimulatorClientAPI::setGuiHelper(struct GUIHelperInterface* guiHelper)
-{
-	m_data->m_guiHelper = guiHelper;
-}
+
 
 void b3RobotSimulatorClientAPI::renderScene()
 {
@@ -82,7 +64,7 @@ bool	b3RobotSimulatorClientAPI::mouseMoveCallback(float x,float y)
 	}
 	if (m_data->m_guiHelper)
 	{
-		return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y);
+		return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y)!=0;
 	}
 	return false;
 }
@@ -95,7 +77,7 @@ bool	b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float
 	}
 	if (m_data->m_guiHelper)
 	{
-		return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y);
+		return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y)!=0;
 	}
 	return false;
 }
@@ -118,13 +100,13 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 
 	switch (mode)
 	{
-		case eCONNECT_EXISTING_EXAMPLE_BROWSER:
+	case eCONNECT_EXISTING_EXAMPLE_BROWSER:
 		{
 			sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper);
 			break;
 		}
 
-		case eCONNECT_GUI:
+	case eCONNECT_GUI:
 		{
 			int argc = 0;
 			char* argv[1] = {0};
@@ -135,7 +117,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 #endif
 			break;
 		}
-		case eCONNECT_GUI_SERVER:
+	case eCONNECT_GUI_SERVER:
 		{
 			int argc = 0;
 			char* argv[1] = {0};
@@ -146,12 +128,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 #endif
 			break;
 		}
-		case eCONNECT_DIRECT:
+	case eCONNECT_DIRECT:
 		{
 			sm = b3ConnectPhysicsDirect();
 			break;
 		}
-		case eCONNECT_SHARED_MEMORY:
+	case eCONNECT_SHARED_MEMORY:
 		{
 			if (portOrKey >= 0)
 			{
@@ -160,7 +142,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 			sm = b3ConnectSharedMemory(key);
 			break;
 		}
-		case eCONNECT_UDP:
+	case eCONNECT_UDP:
 		{
 			if (portOrKey >= 0)
 			{
@@ -175,7 +157,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 
 			break;
 		}
-		case eCONNECT_TCP:
+	case eCONNECT_TCP:
 		{
 			if (portOrKey >= 0)
 			{
@@ -190,7 +172,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 			break;
 		}
 
-		default:
+	default:
 		{
 			b3Warning("connectPhysicsServer unexpected argument");
 		}
@@ -208,952 +190,3 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 	}
 	return false;
 }
-
-bool b3RobotSimulatorClientAPI::isConnected() const
-{
-	return (m_data->m_physicsClientHandle != 0);
-}
-
-void b3RobotSimulatorClientAPI::setTimeOut(double timeOutInSec)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec);
-
-}
-
-
-void b3RobotSimulatorClientAPI::disconnect()
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3DisconnectSharedMemory(m_data->m_physicsClientHandle);
-	m_data->m_physicsClientHandle = 0;
-}
-
-void b3RobotSimulatorClientAPI::syncBodies()
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryCommandHandle command;
-	b3SharedMemoryStatusHandle statusHandle;
-	int statusType;
-
-	command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	statusType = b3GetStatusType(statusHandle);
-}
-
-void b3RobotSimulatorClientAPI::resetSimulation()
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3SharedMemoryStatusHandle statusHandle;
-	statusHandle = b3SubmitClientCommandAndWaitStatus(
-		m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
-}
-
-bool b3RobotSimulatorClientAPI::canSubmitCommand() const
-{
-	if (!isConnected())
-	{
-		return false;
-	}
-	return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
-}
-
-void b3RobotSimulatorClientAPI::stepSimulation()
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryStatusHandle statusHandle;
-	int statusType;
-	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
-	{
-		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
-		statusType = b3GetStatusType(statusHandle);
-		//b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
-	}
-}
-
-void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
-
-	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-	b3SharedMemoryStatusHandle statusHandle;
-	b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-//	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
-}
-
-b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)
-{
-	b3Quaternion q;
-	q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
-	return q;
-}
-
-b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
-{
-	b3Scalar roll,pitch,yaw;
-	quat.getEulerZYX(yaw,pitch,roll);
-	b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
-	return rpy2;
-}
-
-int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
-{
-	int robotUniqueId = -1;
-
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return robotUniqueId;
-	}
-
-	b3SharedMemoryStatusHandle statusHandle;
-	int statusType;
-	b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-
-	//setting the initial position, orientation and other arguments are optional
-
-	b3LoadUrdfCommandSetFlags(command,args.m_flags);
-
-	b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
-									  args.m_startPosition[1],
-									  args.m_startPosition[2]);
-	b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
-	if (args.m_forceOverrideFixedBase)
-	{
-		b3LoadUrdfCommandSetUseFixedBase(command, true);
-	}
-	b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	statusType = b3GetStatusType(statusHandle);
-
-	b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
-	if (statusType == CMD_URDF_LOADING_COMPLETED)
-	{
-		robotUniqueId = b3GetStatusBodyIndex(statusHandle);
-	}
-	return robotUniqueId;
-}
-
-bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	b3SharedMemoryStatusHandle statusHandle;
-	int statusType;
-	b3SharedMemoryCommandHandle command;
-
-	command = b3LoadMJCFCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	statusType = b3GetStatusType(statusHandle);
-	if (statusType != CMD_MJCF_LOADING_COMPLETED)
-	{
-		b3Warning("Couldn't load .mjcf file.");
-		return false;
-	}
-	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
-	if (numBodies)
-	{
-		results.m_uniqueObjectIds.resize(numBodies);
-		int numBodies;
-		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
-	}
-
-	return true;
-}
-
-bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	b3SharedMemoryStatusHandle statusHandle;
-	int statusType;
-	b3SharedMemoryCommandHandle command;
-
-	command = b3LoadBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	statusType = b3GetStatusType(statusHandle);
-	if (statusType != CMD_BULLET_LOADING_COMPLETED)
-	{
-		return false;
-	}
-	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
-	if (numBodies)
-	{
-		results.m_uniqueObjectIds.resize(numBodies);
-		int numBodies;
-		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
-	}
-
-	return true;
-}
-
-bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	bool statusOk = false;
-
-	b3SharedMemoryStatusHandle statusHandle;
-	int statusType;
-	b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-	b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	statusType = b3GetStatusType(statusHandle);
-	b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
-	if (statusType == CMD_SDF_LOADING_COMPLETED)
-	{
-		int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
-		if (numBodies)
-		{
-			results.m_uniqueObjectIds.resize(numBodies);
-			int numBodies;
-			numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
-		}
-		statusOk = true;
-	}
-
-	return statusOk;
-}
-
-bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
-	return (result != 0);
-}
-
-bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	b3SharedMemoryCommandHandle cmd_handle =
-		b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
-	b3SharedMemoryStatusHandle status_handle =
-		b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
-
-	const int status_type = b3GetStatusType(status_handle);
-	const double* actualStateQ;
-
-	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
-	{
-		return false;
-	}
-
-	b3GetStatusActualState(
-		status_handle, 0 /* body_unique_id */,
-		0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
-		0 /*root_local_inertial_frame*/, &actualStateQ,
-		0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */);
-
-	basePosition[0] = actualStateQ[0];
-	basePosition[1] = actualStateQ[1];
-	basePosition[2] = actualStateQ[2];
-
-	baseOrientation[0] = actualStateQ[3];
-	baseOrientation[1] = actualStateQ[4];
-	baseOrientation[2] = actualStateQ[5];
-	baseOrientation[3] = actualStateQ[6];
-	return true;
-}
-
-bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	b3SharedMemoryCommandHandle commandHandle;
-
-	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
-
-	b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
-	b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
-										  baseOrientation[2], baseOrientation[3]);
-
-	b3SharedMemoryStatusHandle statusHandle;
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-
-	return true;
-}
-
-bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
-	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-
-	const int status_type = b3GetStatusType(statusHandle);
-	const double* actualStateQdot;
-
-	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
-	{
-		return false;
-	}
-
-	b3GetStatusActualState(statusHandle, 0 /* body_unique_id */,
-						   0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
-						   0 /*root_local_inertial_frame*/, 0,
-						   &actualStateQdot, 0 /* joint_reaction_forces */);
-
-	baseLinearVelocity[0] = actualStateQdot[0];
-	baseLinearVelocity[1] = actualStateQdot[1];
-	baseLinearVelocity[2] = actualStateQdot[2];
-
-	baseAngularVelocity[0] = actualStateQdot[3];
-	baseAngularVelocity[1] = actualStateQdot[4];
-	baseAngularVelocity[2] = actualStateQdot[5];
-	return true;
-}
-
-bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	b3SharedMemoryCommandHandle commandHandle;
-	b3SharedMemoryStatusHandle statusHandle;
-
-	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
-
-	b3Vector3DoubleData linVelDouble;
-	linearVelocity.serializeDouble(linVelDouble);
-	b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats);
-
-	b3Vector3DoubleData angVelDouble;
-	angularVelocity.serializeDouble(angVelDouble);
-	b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats);
-
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	return true;
-}
-
-void b3RobotSimulatorClientAPI::setInternalSimFlags(int flags)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	{
-		b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-		b3SharedMemoryStatusHandle statusHandle;
-		b3PhysicsParamSetInternalSimFlags(command, flags);
-		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	}
-}
-
-
-void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-	b3SharedMemoryStatusHandle statusHandle;
-
-	int ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
-
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-
-	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
-}
-
-int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-	return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
-}
-
-bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-	return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
-}
-
-int b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return -1;
-	}
-	b3SharedMemoryStatusHandle statusHandle;
-	b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
-	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
-	{
-		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
-		int statusType = b3GetStatusType(statusHandle);
-		if (statusType == CMD_USER_CONSTRAINT_COMPLETED)
-		{
-			int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
-			return userConstraintUid;
-		}
-	}
-	return -1;
-}
-
-int b3RobotSimulatorClientAPI::changeConstraint(int constraintId, b3JointInfo* jointInfo)
-{
-
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return -1;
-	}
-	b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
-
-	if (jointInfo->m_flags & eJointChangeMaxForce)
-	{
-		b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce);
-	}
-
-	if (jointInfo->m_flags & eJointChangeChildFramePosition)
-	{
-		b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]);
-	}
-	if (jointInfo->m_flags & eJointChangeChildFrameOrientation)
-	{
-		b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]);
-	}
-
-	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	int statusType = b3GetStatusType(statusHandle);
-	return statusType;
-}
-
-void b3RobotSimulatorClientAPI::removeConstraint(int constraintId)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
-	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	int statusType = b3GetStatusType(statusHandle);
-}
-
-
-bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
-	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	int statusType = b3GetStatusType(statusHandle);
-	if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
-	{
-		if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
-		{
-			return true;
-		}
-	}
-	return false;
-}
-
-bool b3RobotSimulatorClientAPI::getJointStates(int bodyUniqueId, b3JointStates2& state)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-
-	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
-    b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-    
-    if (statusHandle)
-    {
-     //   double rootInertialFrame[7];
-        const double* rootLocalInertialFrame;
-        const double* actualStateQ;
-        const double* actualStateQdot;
-        const double* jointReactionForces;
-        
-        int stat = b3GetStatusActualState(statusHandle,
-                        &state.m_bodyUniqueId,
-                        &state.m_numDegreeOfFreedomQ,
-                        &state.m_numDegreeOfFreedomU,
-                        &rootLocalInertialFrame,
-                        &actualStateQ,
-                        &actualStateQdot,
-                        &jointReactionForces);
-        if (stat)
-        {
-            state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
-            state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);
-
-            for (int i=0;i<state.m_numDegreeOfFreedomQ;i++)
-            {
-                state.m_actualStateQ[i] = actualStateQ[i];
-            }
-            for (int i=0;i<state.m_numDegreeOfFreedomU;i++)
-            {
-                state.m_actualStateQdot[i] = actualStateQdot[i];
-            }
-            int numJoints = getNumJoints(bodyUniqueId);
-            state.m_jointReactionForces.resize(6*numJoints);
-            for (int i=0;i<numJoints*6;i++)
-            {
-                state.m_jointReactionForces[i] = jointReactionForces[i];
-            }
-            
-            return true;
-        }
-        //rootInertialFrame,
-          //              &state.m_actualStateQ[0],
-            //            &state.m_actualStateQdot[0],
-              //          &state.m_jointReactionForces[0]);
-        
-     
-    }
-    return false;
-}
-
-bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-	b3SharedMemoryCommandHandle commandHandle;
-	b3SharedMemoryStatusHandle statusHandle;
-	int numJoints;
-
-	numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
-	if ((jointIndex >= numJoints) || (jointIndex < 0))
-	{
-		return false;
-	}
-
-	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
-
-	b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
-										targetValue);
-
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	return false;
-}
-
-void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3SharedMemoryStatusHandle statusHandle;
-	switch (args.m_controlMode)
-	{
-		case CONTROL_MODE_VELOCITY:
-		{
-			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
-			b3JointInfo jointInfo;
-			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
-			int uIndex = jointInfo.m_uIndex;
-			b3JointControlSetKd(command, uIndex, args.m_kd);
-			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
-			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
-			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-			break;
-		}
-		case CONTROL_MODE_POSITION_VELOCITY_PD:
-		{
-			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
-			b3JointInfo jointInfo;
-			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
-			int uIndex = jointInfo.m_uIndex;
-			int qIndex = jointInfo.m_qIndex;
-
-			b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
-			b3JointControlSetKp(command, uIndex, args.m_kp);
-			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
-			b3JointControlSetKd(command, uIndex, args.m_kd);
-			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
-			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-			break;
-		}
-		case CONTROL_MODE_TORQUE:
-		{
-			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
-			b3JointInfo jointInfo;
-			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
-			int uIndex = jointInfo.m_uIndex;
-			b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
-			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-			break;
-		}
-		default:
-		{
-			b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl");
-		}
-	}
-}
-
-void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-	b3SharedMemoryStatusHandle statusHandle;
-	b3PhysicsParamSetNumSolverIterations(command, numIterations);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
-}
-
-void b3RobotSimulatorClientAPI::setContactBreakingThreshold(double threshold)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-	b3SharedMemoryStatusHandle statusHandle;
-	b3PhysicsParamSetContactBreakingThreshold(command,threshold);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
-}
-
-
-void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-	b3SharedMemoryStatusHandle statusHandle;
-	int ret;
-	ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-}
-
-void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-	b3SharedMemoryStatusHandle statusHandle;
-	b3PhysicsParamSetNumSubSteps(command, numSubSteps);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
-}
-
-bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-	b3Assert(args.m_endEffectorLinkIndex >= 0);
-	b3Assert(args.m_bodyUniqueId >= 0);
-
-	b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId);
-	if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
-	{
-		b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
-	}
-	else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
-	{
-		b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
-	}
-	else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
-	{
-		b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
-	}
-	else
-	{
-		b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
-	}
-
-	if (args.m_flags & B3_HAS_JOINT_DAMPING)
-	{
-		b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
-	}
-
-	b3SharedMemoryStatusHandle statusHandle;
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-
-	int numPos = 0;
-
-	bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
-															 &results.m_bodyUniqueId,
-															 &numPos,
-															 0) != 0;
-	if (result && numPos)
-	{
-		results.m_calculatedJointPositions.resize(numPos);
-		result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
-															&results.m_bodyUniqueId,
-															&numPos,
-															&results.m_calculatedJointPositions[0]) != 0;
-	}
-	return result;
-}
-
-bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-	b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
-	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-
-	if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
-	{
-		int dofCount;
-		b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian);
-		return true;
-	}
-	return false;
-}
-
-bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return false;
-	}
-	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
-	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-
-	if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
-	{
-		b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
-		return true;
-	}
-	return false;
-}
-
-void b3RobotSimulatorClientAPI::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-	b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
-	b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable);
-	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-}
-
-void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter)
-{
-	vrEventsData->m_numControllerEvents = 0;
-	vrEventsData->m_controllerEvents = 0;
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
-	b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
-	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
-}
-
-void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
-{
-	keyboardEventsData->m_numKeyboardEvents = 0;
-	keyboardEventsData->m_keyboardEvents = 0;
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
-	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
-}
-
-int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return -1;
-	}
-	int loggingUniqueId = -1;
-	b3SharedMemoryCommandHandle commandHandle;
-	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
-
-	b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
-
-	for (int i = 0; i < objectUniqueIds.size(); i++)
-	{
-		int objectUid = objectUniqueIds[i];
-		b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
-	}
-
-	if (maxLogDof > 0)
-	{
-		b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
-	}
-
-	b3SharedMemoryStatusHandle statusHandle;
-	int statusType;
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	statusType = b3GetStatusType(statusHandle);
-	if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
-	{
-		loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
-	}
-	return loggingUniqueId;
-}
-
-void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryCommandHandle commandHandle;
-	b3SharedMemoryStatusHandle statusHandle;
-	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
-	b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
-	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-}
-
-void b3RobotSimulatorClientAPI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
-	if (commandHandle)
-	{
-	if ((cameraDistance >= 0))
-	{
-		b3Vector3FloatData camTargetPos;
-		targetPos.serializeFloat(camTargetPos);
-		b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats);			
-	}
-	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	}
-}
-
-void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string&  profileName, int durationInMicroSeconds)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-	b3SharedMemoryCommandHandle	commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
-	if (durationInMicroSeconds>=0)
-	{
-		b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
-	}
-	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-}
-
-void b3RobotSimulatorClientAPI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
-{
-	if (!isConnected())
-	{
-		b3Warning("Not connected");
-		return;
-	}
-
-    b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-    b3LoadSoftBodySetScale(command, scale);
-    b3LoadSoftBodySetMass(command, mass);
-    b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
-    b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-}
diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h
index d2bee53384..a5f1a0cbd1 100644
--- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h
+++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h
@@ -1,246 +1,32 @@
-#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H
-#define B3_ROBOT_SIMULATOR_CLIENT_API_H
+#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H
+#define B3_ROBOT_SIMULATOR_CLIENT_API_GUI_H
 
-#include "../SharedMemory/SharedMemoryPublic.h"
-#include "Bullet3Common/b3Vector3.h"
-#include "Bullet3Common/b3Quaternion.h"
-#include "Bullet3Common/b3Transform.h"
-#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "b3RobotSimulatorClientAPI_NoGUI.h"
 
-#include <string>
 
-struct b3RobotSimulatorLoadUrdfFileArgs
-{
-	b3Vector3 m_startPosition;
-	b3Quaternion m_startOrientation;
-	bool m_forceOverrideFixedBase;
-	bool m_useMultiBody;
-	int m_flags;
-
-	b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn)
-		: m_startPosition(startPos),
-		  m_startOrientation(startOrn),
-		  m_forceOverrideFixedBase(false),
-		  m_useMultiBody(true),
-		  m_flags(0)
-	{
-	}
-
-	b3RobotSimulatorLoadUrdfFileArgs()
-		: m_startPosition(b3MakeVector3(0, 0, 0)),
-		  m_startOrientation(b3Quaternion(0, 0, 0, 1)),
-		  m_forceOverrideFixedBase(false),
-		  m_useMultiBody(true),
-		  m_flags(0)
-	{
-	}
-};
-
-struct b3RobotSimulatorLoadSdfFileArgs
-{
-	bool m_forceOverrideFixedBase;
-	bool m_useMultiBody;
-
-	b3RobotSimulatorLoadSdfFileArgs()
-		: m_forceOverrideFixedBase(false),
-		  m_useMultiBody(true)
-	{
-	}
-};
-
-struct b3RobotSimulatorLoadFileResults
-{
-	b3AlignedObjectArray<int> m_uniqueObjectIds;
-	b3RobotSimulatorLoadFileResults()
-	{
-	}
-};
-
-struct b3RobotSimulatorJointMotorArgs
-{
-	int m_controlMode;
-
-	double m_targetPosition;
-	double m_kp;
-
-	double m_targetVelocity;
-	double m_kd;
-
-	double m_maxTorqueValue;
-
-	b3RobotSimulatorJointMotorArgs(int controlMode)
-		: m_controlMode(controlMode),
-		  m_targetPosition(0),
-		  m_kp(0.1),
-		  m_targetVelocity(0),
-		  m_kd(0.9),
-		  m_maxTorqueValue(1000)
-	{
-	}
-};
-
-enum b3RobotSimulatorInverseKinematicsFlags
-{
-	B3_HAS_IK_TARGET_ORIENTATION = 1,
-	B3_HAS_NULL_SPACE_VELOCITY = 2,
-	B3_HAS_JOINT_DAMPING = 4,
-};
-
-struct b3RobotSimulatorInverseKinematicArgs
-{
-	int m_bodyUniqueId;
-	//	double* m_currentJointPositions;
-	//	int m_numPositions;
-	double m_endEffectorTargetPosition[3];
-	double m_endEffectorTargetOrientation[4];
-	int m_endEffectorLinkIndex;
-	int m_flags;
-	int m_numDegreeOfFreedom;
-	b3AlignedObjectArray<double> m_lowerLimits;
-	b3AlignedObjectArray<double> m_upperLimits;
-	b3AlignedObjectArray<double> m_jointRanges;
-	b3AlignedObjectArray<double> m_restPoses;
-	b3AlignedObjectArray<double> m_jointDamping;
-
-	b3RobotSimulatorInverseKinematicArgs()
-		: m_bodyUniqueId(-1),
-		  m_endEffectorLinkIndex(-1),
-		  m_flags(0)
-	{
-		m_endEffectorTargetPosition[0] = 0;
-		m_endEffectorTargetPosition[1] = 0;
-		m_endEffectorTargetPosition[2] = 0;
-
-		m_endEffectorTargetOrientation[0] = 0;
-		m_endEffectorTargetOrientation[1] = 0;
-		m_endEffectorTargetOrientation[2] = 0;
-		m_endEffectorTargetOrientation[3] = 1;
-	}
-};
-
-struct b3RobotSimulatorInverseKinematicsResults
-{
-	int m_bodyUniqueId;
-	b3AlignedObjectArray<double> m_calculatedJointPositions;
-};
-
-struct b3JointStates2
-{
-    int m_bodyUniqueId;
-    int m_numDegreeOfFreedomQ;
-    int m_numDegreeOfFreedomU;
-    b3Transform m_rootLocalInertialFrame;
-    b3AlignedObjectArray<double> m_actualStateQ;
-    b3AlignedObjectArray<double> m_actualStateQdot;
-    b3AlignedObjectArray<double> m_jointReactionForces;
-};
-
-///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
+///The b3RobotSimulatorClientAPI_GUI is pretty much the C++ version of pybullet
 ///as documented in the pybullet Quickstart Guide
 ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
-class b3RobotSimulatorClientAPI
+class b3RobotSimulatorClientAPI : public b3RobotSimulatorClientAPI_NoGUI
 {
-	struct b3RobotSimulatorClientAPI_InternalData* m_data;
 
 public:
 
 
 	b3RobotSimulatorClientAPI();
+	
 	virtual ~b3RobotSimulatorClientAPI();
 
-	bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
-
-	void disconnect();
-
-	bool isConnected() const;
-
-	void setTimeOut(double timeOutInSec);
-
-	void syncBodies();
-
-	void resetSimulation();
-
-	b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
-	b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
-
-	int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs());
-	bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
-	bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
-	bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
-
-	bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo);
-
-	bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const;
-	bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation);
-
-	bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const;
-	bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const;
-
-	int getNumJoints(int bodyUniqueId) const;
-
-	bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
-
-	int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
-
-	int changeConstraint(int constraintId, b3JointInfo* jointInfo);
-
-	void removeConstraint(int constraintId);
-
-	bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
-
-	bool getJointStates(int bodyUniqueId, b3JointStates2& state);
-
-	bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
-
-	void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);
-
-	void stepSimulation();
-
-	bool canSubmitCommand() const;
-
-	void setRealTimeSimulation(bool enableRealTimeSimulation);
-
-	void setInternalSimFlags(int flags);
-
-	void setGravity(const b3Vector3& gravityAcceleration);
-
-	void setTimeStep(double timeStepInSeconds);
-	void setNumSimulationSubSteps(int numSubSteps);
-	void setNumSolverIterations(int numIterations);
-	void setContactBreakingThreshold(double threshold);
-
-	bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
-
-	bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
-
-	bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState);
-
-	void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
-	void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos);
-
-	int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds=b3AlignedObjectArray<int>(), int maxLogDof = -1);
-	void stopStateLogging(int stateLoggerUniqueId);
-
-	void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter);
-	void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
-
-	void submitProfileTiming(const std::string&  profileName, int durationInMicroSeconds=1);				
-
-
-	//////////////// INTERNAL
-
-	void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin);
-
-	//setGuiHelper is only used when embedded in existing example browser
-	void setGuiHelper(struct GUIHelperInterface* guiHelper);
-	//renderScene is only used when embedded in existing example browser
+	virtual bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
+	
 	virtual void renderScene();
-	//debugDraw is only used when embedded in existing example browser
+
 	virtual void debugDraw(int debugDrawMode);
+
 	virtual bool	mouseMoveCallback(float x,float y);
+
 	virtual bool	mouseButtonCallback(int button, int state, float x, float y);
 
-	////////////////INTERNAL
 };
 
 #endif  //B3_ROBOT_SIMULATOR_CLIENT_API_H
diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h
new file mode 100644
index 0000000000..cac680fda3
--- /dev/null
+++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_InternalData.h
@@ -0,0 +1,18 @@
+#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H
+#define B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H
+
+#include "../SharedMemory/PhysicsClientC_API.h"
+
+struct b3RobotSimulatorClientAPI_InternalData
+{
+	b3PhysicsClientHandle m_physicsClientHandle;
+	struct GUIHelperInterface* m_guiHelper;
+
+	b3RobotSimulatorClientAPI_InternalData()
+		: m_physicsClientHandle(0),
+		m_guiHelper(0)
+	{
+	}
+};
+
+#endif //B3_ROBOT_SIMULATOR_CLIENT_API_INTERNAL_DATA_H
\ No newline at end of file
diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp
new file mode 100644
index 0000000000..8beeed506a
--- /dev/null
+++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp
@@ -0,0 +1,1997 @@
+#include "b3RobotSimulatorClientAPI_NoGUI.h"
+
+
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "b3RobotSimulatorClientAPI_InternalData.h"
+
+#ifdef BT_ENABLE_ENET
+#include "../SharedMemory/PhysicsClientUDP_C_API.h"
+#endif  //PHYSICS_UDP
+
+#ifdef BT_ENABLE_CLSOCKET
+#include "../SharedMemory/PhysicsClientTCP_C_API.h"
+#endif  //PHYSICS_TCP
+
+#include "../SharedMemory/PhysicsDirectC_API.h"
+
+#include "../SharedMemory/SharedMemoryPublic.h"
+#include "Bullet3Common/b3Logging.h"
+
+static void scalarToDouble3(b3Scalar a[3], double b[3]) 
+{
+	for (int i = 0; i < 3; i++) 
+	{
+		b[i] = a[i];
+	}
+}
+
+static void scalarToDouble4(b3Scalar a[4], double b[4]) 
+{
+	for (int i = 0; i < 4; i++) 
+	{
+		b[i] = a[i];
+	}
+}
+
+
+b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI()
+{
+	m_data = new b3RobotSimulatorClientAPI_InternalData();
+}
+
+b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI()
+{
+	delete m_data;
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostName, int portOrKey)
+{
+	if (m_data->m_physicsClientHandle)
+	{
+		b3Warning("Already connected, disconnect first.");
+		return false;
+	}
+	b3PhysicsClientHandle sm = 0;
+
+	int udpPort = 1234;
+	int tcpPort = 6667;
+	int key = SHARED_MEMORY_KEY;
+	bool connected = false;
+
+	switch (mode)
+	{
+	
+	case eCONNECT_DIRECT:
+		{
+			sm = b3ConnectPhysicsDirect();
+			break;
+		}
+	case eCONNECT_SHARED_MEMORY:
+		{
+			if (portOrKey >= 0)
+			{
+				key = portOrKey;
+			}
+			sm = b3ConnectSharedMemory(key);
+			break;
+		}
+	case eCONNECT_UDP:
+		{
+			if (portOrKey >= 0)
+			{
+				udpPort = portOrKey;
+			}
+#ifdef BT_ENABLE_ENET
+
+			sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
+#else
+			b3Warning("UDP is not enabled in this build");
+#endif  //BT_ENABLE_ENET
+
+			break;
+		}
+	case eCONNECT_TCP:
+		{
+			if (portOrKey >= 0)
+			{
+				tcpPort = portOrKey;
+			}
+#ifdef BT_ENABLE_CLSOCKET
+
+			sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
+#else
+			b3Warning("TCP is not enabled in this pybullet build");
+#endif  //BT_ENABLE_CLSOCKET
+			break;
+		}
+
+	default:
+		{
+			b3Warning("connectPhysicsServer unexpected argument");
+		}
+	};
+
+	if (sm)
+	{
+		m_data->m_physicsClientHandle = sm;
+		if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
+		{
+			disconnect();
+			return false;
+		}
+		return true;
+	}
+	return false;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::isConnected() const
+{
+	return (m_data->m_physicsClientHandle != 0);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::setTimeOut(double timeOutInSec)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec);
+
+}
+
+
+void b3RobotSimulatorClientAPI_NoGUI::disconnect()
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3DisconnectSharedMemory(m_data->m_physicsClientHandle);
+	m_data->m_physicsClientHandle = 0;
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::syncBodies()
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	statusType = b3GetStatusType(statusHandle);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::resetSimulation()
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryStatusHandle statusHandle;
+	statusHandle = b3SubmitClientCommandAndWaitStatus(
+		m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::canSubmitCommand() const
+{
+	if (!isConnected())
+	{
+		return false;
+	}
+	return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::stepSimulation()
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
+	{
+		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
+		statusType = b3GetStatusType(statusHandle);
+		//b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
+	}
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::setGravity(const b3Vector3& gravityAcceleration)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
+
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+	b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	//	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
+}
+
+b3Quaternion b3RobotSimulatorClientAPI_NoGUI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)
+{
+	b3Quaternion q;
+	q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
+	return q;
+}
+
+b3Vector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const b3Quaternion& quat)
+{
+	b3Scalar roll,pitch,yaw;
+	quat.getEulerZYX(yaw,pitch,roll);
+	b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
+	return rpy2;
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
+{
+	int robotUniqueId = -1;
+
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return robotUniqueId;
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+
+	//setting the initial position, orientation and other arguments are optional
+
+	b3LoadUrdfCommandSetFlags(command,args.m_flags);
+
+	b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
+		args.m_startPosition[1],
+		args.m_startPosition[2]);
+	b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
+	if (args.m_forceOverrideFixedBase)
+	{
+		b3LoadUrdfCommandSetUseFixedBase(command, true);
+	}
+	b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	statusType = b3GetStatusType(statusHandle);
+
+	b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
+	if (statusType == CMD_URDF_LOADING_COMPLETED)
+	{
+		robotUniqueId = b3GetStatusBodyIndex(statusHandle);
+	}
+	return robotUniqueId;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+
+	command = b3LoadMJCFCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_MJCF_LOADING_COMPLETED)
+	{
+		b3Warning("Couldn't load .mjcf file.");
+		return false;
+	}
+	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
+	if (numBodies)
+	{
+		results.m_uniqueObjectIds.resize(numBodies);
+		int numBodies;
+		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
+	}
+
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+
+	command = b3LoadBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_BULLET_LOADING_COMPLETED)
+	{
+		return false;
+	}
+	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
+	if (numBodies)
+	{
+		results.m_uniqueObjectIds.resize(numBodies);
+		int numBodies;
+		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
+	}
+
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	bool statusOk = false;
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+	b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	statusType = b3GetStatusType(statusHandle);
+	b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
+	if (statusType == CMD_SDF_LOADING_COMPLETED)
+	{
+		int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
+		if (numBodies)
+		{
+			results.m_uniqueObjectIds.resize(numBodies);
+			int numBodies;
+			numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
+		}
+		statusOk = true;
+	}
+
+	return statusOk;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
+	return (result != 0);
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryCommandHandle cmd_handle =
+		b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+	b3SharedMemoryStatusHandle status_handle =
+		b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
+
+	const int status_type = b3GetStatusType(status_handle);
+	const double* actualStateQ;
+
+	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+	{
+		return false;
+	}
+
+	b3GetStatusActualState(
+		status_handle, 0 /* body_unique_id */,
+		0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
+		0 /*root_local_inertial_frame*/, &actualStateQ,
+		0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */);
+
+	basePosition[0] = actualStateQ[0];
+	basePosition[1] = actualStateQ[1];
+	basePosition[2] = actualStateQ[2];
+
+	baseOrientation[0] = actualStateQ[3];
+	baseOrientation[1] = actualStateQ[4];
+	baseOrientation[2] = actualStateQ[5];
+	baseOrientation[3] = actualStateQ[6];
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryCommandHandle commandHandle;
+
+	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+
+	b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
+	b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
+		baseOrientation[2], baseOrientation[3]);
+
+	b3SharedMemoryStatusHandle statusHandle;
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+	const int status_type = b3GetStatusType(statusHandle);
+	const double* actualStateQdot;
+
+	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+	{
+		return false;
+	}
+
+	b3GetStatusActualState(statusHandle, 0 /* body_unique_id */,
+		0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
+		0 /*root_local_inertial_frame*/, 0,
+		&actualStateQdot, 0 /* joint_reaction_forces */);
+
+	baseLinearVelocity[0] = actualStateQdot[0];
+	baseLinearVelocity[1] = actualStateQdot[1];
+	baseLinearVelocity[2] = actualStateQdot[2];
+
+	baseAngularVelocity[0] = actualStateQdot[3];
+	baseAngularVelocity[1] = actualStateQdot[4];
+	baseAngularVelocity[2] = actualStateQdot[5];
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+
+	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+
+	b3Vector3DoubleData linVelDouble;
+	linearVelocity.serializeDouble(linVelDouble);
+	b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats);
+
+	b3Vector3DoubleData angVelDouble;
+	angularVelocity.serializeDouble(angVelDouble);
+	b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	return true;
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::setInternalSimFlags(int flags)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	{
+		b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+		b3SharedMemoryStatusHandle statusHandle;
+		b3PhysicsParamSetInternalSimFlags(command, flags);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	}
+}
+
+
+void b3RobotSimulatorClientAPI_NoGUI::setRealTimeSimulation(bool enableRealTimeSimulation)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+
+	int ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::getNumJoints(int bodyUniqueId) const
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+	return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+	return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return -1;
+	}
+	b3SharedMemoryStatusHandle statusHandle;
+	b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
+	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
+	{
+		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
+		int statusType = b3GetStatusType(statusHandle);
+		if (statusType == CMD_USER_CONSTRAINT_COMPLETED)
+		{
+			int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
+			return userConstraintUid;
+		}
+	}
+	return -1;
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::changeConstraint(int constraintId, b3JointInfo* jointInfo)
+{
+
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return -1;
+	}
+	b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
+
+	if (jointInfo->m_flags & eJointChangeMaxForce)
+	{
+		b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce);
+	}
+
+	if (jointInfo->m_flags & eJointChangeChildFramePosition)
+	{
+		b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]);
+	}
+	if (jointInfo->m_flags & eJointChangeChildFrameOrientation)
+	{
+		b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]);
+	}
+
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	int statusType = b3GetStatusType(statusHandle);
+	return statusType;
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::removeConstraint(int constraintId)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	int statusType = b3GetStatusType(statusHandle);
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	int statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+	{
+		if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
+		{
+			return true;
+		}
+	}
+	return false;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getJointStates(int bodyUniqueId, b3JointStates2& state)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+	if (statusHandle)
+	{
+		//   double rootInertialFrame[7];
+		const double* rootLocalInertialFrame;
+		const double* actualStateQ;
+		const double* actualStateQdot;
+		const double* jointReactionForces;
+
+		int stat = b3GetStatusActualState(statusHandle,
+			&state.m_bodyUniqueId,
+			&state.m_numDegreeOfFreedomQ,
+			&state.m_numDegreeOfFreedomU,
+			&rootLocalInertialFrame,
+			&actualStateQ,
+			&actualStateQdot,
+			&jointReactionForces);
+		if (stat)
+		{
+			state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
+			state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);
+
+			for (int i=0;i<state.m_numDegreeOfFreedomQ;i++)
+			{
+				state.m_actualStateQ[i] = actualStateQ[i];
+			}
+			for (int i=0;i<state.m_numDegreeOfFreedomU;i++)
+			{
+				state.m_actualStateQdot[i] = actualStateQdot[i];
+			}
+			int numJoints = getNumJoints(bodyUniqueId);
+			state.m_jointReactionForces.resize(6*numJoints);
+			for (int i=0;i<numJoints*6;i++)
+			{
+				state.m_jointReactionForces[i] = jointReactionForces[i];
+			}
+
+			return true;
+		}
+		//rootInertialFrame,
+		//              &state.m_actualStateQ[0],
+		//            &state.m_actualStateQdot[0],
+		//          &state.m_jointReactionForces[0]);
+
+
+	}
+	return false;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int numJoints;
+
+	numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
+	if ((jointIndex >= numJoints) || (jointIndex < 0))
+	{
+		return false;
+	}
+
+	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+
+	b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
+		targetValue);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	return false;
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryStatusHandle statusHandle;
+	switch (args.m_controlMode)
+	{
+	case CONTROL_MODE_VELOCITY:
+		{
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
+			b3JointInfo jointInfo;
+			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
+			int uIndex = jointInfo.m_uIndex;
+			b3JointControlSetKd(command, uIndex, args.m_kd);
+			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
+			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
+			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+			break;
+		}
+	case CONTROL_MODE_POSITION_VELOCITY_PD:
+		{
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
+			b3JointInfo jointInfo;
+			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
+			int uIndex = jointInfo.m_uIndex;
+			int qIndex = jointInfo.m_qIndex;
+
+			b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
+			b3JointControlSetKp(command, uIndex, args.m_kp);
+			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
+			b3JointControlSetKd(command, uIndex, args.m_kd);
+			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
+			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+			break;
+		}
+	case CONTROL_MODE_TORQUE:
+		{
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
+			b3JointInfo jointInfo;
+			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
+			int uIndex = jointInfo.m_uIndex;
+			b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
+			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+			break;
+		}
+	default:
+		{
+			b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl");
+		}
+	}
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::setNumSolverIterations(int numIterations)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+	b3PhysicsParamSetNumSolverIterations(command, numIterations);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double threshold)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+	b3PhysicsParamSetContactBreakingThreshold(command,threshold);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
+}
+
+
+void b3RobotSimulatorClientAPI_NoGUI::setTimeStep(double timeStepInSeconds)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+	int ret;
+	ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::setNumSimulationSubSteps(int numSubSteps)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+	b3PhysicsParamSetNumSubSteps(command, numSubSteps);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+	b3Assert(args.m_endEffectorLinkIndex >= 0);
+	b3Assert(args.m_bodyUniqueId >= 0);
+
+	b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId);
+	if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
+	{
+		b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
+	}
+	else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
+	{
+		b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
+	}
+	else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
+	{
+		b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
+	}
+	else
+	{
+		b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
+	}
+
+	if (args.m_flags & B3_HAS_JOINT_DAMPING)
+	{
+		b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+	int numPos = 0;
+
+	bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+		&results.m_bodyUniqueId,
+		&numPos,
+		0) != 0;
+	if (result && numPos)
+	{
+		results.m_calculatedJointPositions.resize(numPos);
+		result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+			&results.m_bodyUniqueId,
+			&numPos,
+			&results.m_calculatedJointPositions[0]) != 0;
+	}
+	return result;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+	if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
+	{
+		int dofCount;
+		b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian);
+		return true;
+	}
+	return false;
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState)
+{
+	bool computeLinkVelocity = true;
+	bool computeForwardKinematics = true;
+
+	return getLinkState(bodyUniqueId, linkIndex, computeLinkVelocity, computeForwardKinematics, linkState);
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState)
+{
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+
+	if (computeLinkVelocity) {
+		b3RequestActualStateCommandComputeLinkVelocity(command, computeLinkVelocity);
+	}
+
+	if (computeForwardKinematics) {
+		b3RequestActualStateCommandComputeForwardKinematics(command, computeForwardKinematics);
+	}
+
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+
+	if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
+		b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
+		return true;
+	}
+	return false;
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
+	b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable);
+	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter)
+{
+	vrEventsData->m_numControllerEvents = 0;
+	vrEventsData->m_controllerEvents = 0;
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
+	b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
+	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
+{
+	keyboardEventsData->m_numKeyboardEvents = 0;
+	keyboardEventsData->m_keyboardEvents = 0;
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
+	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return -1;
+	}
+	int loggingUniqueId = -1;
+	b3SharedMemoryCommandHandle commandHandle;
+	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
+
+	b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
+
+	for (int i = 0; i < objectUniqueIds.size(); i++)
+	{
+		int objectUid = objectUniqueIds[i];
+		b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
+	}
+
+	if (maxLogDof > 0)
+	{
+		b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
+	{
+		loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
+	}
+	return loggingUniqueId;
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
+	b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
+	if (commandHandle)
+	{
+		if ((cameraDistance >= 0))
+		{
+			b3Vector3FloatData camTargetPos;
+			targetPos.serializeFloat(camTargetPos);
+			b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats);			
+		}
+		b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	}
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::submitProfileTiming(const std::string&  profileName, int durationInMicroSeconds)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle	commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
+	if (durationInMicroSeconds>=0)
+	{
+		b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
+	}
+	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
+	b3LoadSoftBodySetScale(command, scale);
+	b3LoadSoftBodySetMass(command, mass);
+	b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
+	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+}
+
+void b3RobotSimulatorClientAPI_NoGUI::getMouseEvents(b3MouseEventsData* mouseEventsData)
+{
+	mouseEventsData->m_numMouseEvents = 0;
+	mouseEventsData->m_mouseEvents = 0;
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return;
+	}
+
+	b3SharedMemoryCommandHandle command = b3RequestMouseEventsCommandInit(m_data->m_physicsClientHandle);
+	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	b3GetMouseEventsData(m_data->m_physicsClientHandle, mouseEventsData);
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities,
+	double *jointAccelerations, double *jointForcesOutput) 
+{
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return false;
+	}
+
+	int numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions,
+		jointVelocities, jointAccelerations);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) {
+		int bodyUniqueId;
+		int dofCount;
+
+		b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0);
+
+		if (dofCount) {
+			b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,	jointForcesOutput);
+			return true;
+		}
+	}
+	return false;
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::getNumBodies() const
+{
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return false;
+	}
+	return b3GetNumBodies(m_data->m_physicsClientHandle);
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::getBodyUniqueId(int bodyId) const
+{
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return false;
+	}
+	return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId);
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::removeBody(int bodyUniqueId) 
+{
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) {
+		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitRemoveBodyCommand(m_data->m_physicsClientHandle, bodyUniqueId));
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType == CMD_REMOVE_BODY_COMPLETED) {
+			return true;
+		} else {
+			b3Warning("getDynamicsInfo did not complete");
+			return false;
+		}
+	}
+	b3Warning("removeBody could not submit command");
+	return false;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) {
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return false;
+	}
+	int status_type = 0;
+	b3SharedMemoryCommandHandle cmd_handle;
+	b3SharedMemoryStatusHandle status_handle;
+	//  struct b3DynamicsInfo info;
+
+	if (bodyUniqueId < 0) {
+		b3Warning("getDynamicsInfo failed; invalid bodyUniqueId");
+		return false;
+	}
+	if (linkIndex < -1) {
+		b3Warning("getDynamicsInfo failed; invalid linkIndex");
+		return false;
+	}
+
+	if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) {
+		cmd_handle = b3GetDynamicsInfoCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex);
+		status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
+		status_type = b3GetStatusType(status_handle);
+		if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) {
+			return true;
+		} else {
+			b3Warning("getDynamicsInfo did not complete");
+			return false;
+		}
+	} 
+	b3Warning("getDynamicsInfo could not submit command");
+	return false;
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected to physics server.");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
+	b3SharedMemoryStatusHandle statusHandle;
+
+	if (args.m_mass >= 0) {
+		b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass);
+	}
+
+	if (args.m_lateralFriction >= 0) {
+		b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, args.m_lateralFriction);
+	}
+
+	if (args.m_spinningFriction>=0) {
+		b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex, args.m_spinningFriction);
+	}
+
+	if (args.m_rollingFriction>=0) {
+		b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex, args.m_rollingFriction);
+	}
+
+	if (args.m_linearDamping>=0)	{
+		b3ChangeDynamicsInfoSetLinearDamping(command, bodyUniqueId, args.m_linearDamping);
+	}
+
+	if (args.m_angularDamping>=0) {
+		b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, args.m_angularDamping);
+	}
+
+	if (args.m_restitution>=0) {
+		b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, args.m_restitution);
+	}
+
+	if (args.m_contactStiffness>=0 && args.m_contactDamping >=0) {
+		b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command, bodyUniqueId, linkIndex, args.m_contactStiffness, args.m_contactDamping);
+	}
+
+	if (args.m_frictionAnchor>=0) {
+		b3ChangeDynamicsInfoSetFrictionAnchor(command, bodyUniqueId,linkIndex, args.m_frictionAnchor);
+	}
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	return true;
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) {
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected to physics server.");
+		return -1;
+	}
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	commandHandle = b3InitUserDebugAddParameter(sm, paramName, rangeMin, rangeMax, startValue);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
+		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
+		return  debugItemUniqueId;
+	}
+	b3Warning("addUserDebugParameter failed.");
+	return -1;
+}
+
+
+double b3RobotSimulatorClientAPI_NoGUI::readUserDebugParameter(int itemUniqueId) {
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected to physics server.");
+		return 0;
+	}
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) {
+		double paramValue = 0.f;
+		int ok = b3GetStatusDebugParameterValue(statusHandle, &paramValue);
+		if (ok) {
+			return paramValue;
+		} 
+	}
+	b3Warning("readUserDebugParameter failed.");
+	return 0;
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) {
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected to physics server.");
+		return false;
+	}
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	return true;
+}
+
+
+int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected to physics server.");
+		return -1;
+	}
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime);
+
+	if (args.m_parentObjectUniqueId>=0) {
+		b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
+	}
+
+	if (args.m_flags & DEBUG_TEXT_HAS_ORIENTATION) {
+		b3UserDebugTextSetOrientation(commandHandle, &args.m_textOrientation[0]);
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
+		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
+		return  debugItemUniqueId;
+	}
+	b3Warning("addUserDebugText3D failed.");
+	return -1;
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, b3Vector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
+{
+	double dposXYZ[3];
+	dposXYZ[0] = posXYZ.x;
+	dposXYZ[1] = posXYZ.y;
+	dposXYZ[2] = posXYZ.z;
+
+	return addUserDebugText(text, &dposXYZ[0], args);
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected to physics server.");
+		return -1;
+	}
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, &args.m_colorRGB[0], args.m_lineWidth, args.m_lifeTime);
+
+	if (args.m_parentObjectUniqueId>=0) {
+		b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
+		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
+		return  debugItemUniqueId;
+	}
+	b3Warning("addUserDebugLine failed.");
+	return -1;
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
+{
+	double dfromXYZ[3];
+	double dtoXYZ[3];
+	dfromXYZ[0] = fromXYZ.x;
+	dfromXYZ[1] = fromXYZ.y;
+	dfromXYZ[2] = fromXYZ.z;
+
+	dtoXYZ[0] = toXYZ.x;
+	dtoXYZ[1] = toXYZ.y;
+	dtoXYZ[2] = toXYZ.z;
+	return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args);
+}
+
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected to physics server.");
+		return false;
+	}
+	int numJoints = b3GetNumJoints(sm, bodyUniqueId);
+
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	//  int statusType;
+	struct b3JointInfo info;
+
+	commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, args.m_controlMode);
+
+	for (int i=0;i<args.m_numControlledDofs;i++) {
+		double targetVelocity = 0.0;
+		double targetPosition = 0.0;
+		double force = 100000.0;
+		double kp = 0.1;
+		double kd = 1.0;
+		int jointIndex;
+
+		if (args.m_jointIndices) {
+			jointIndex = args.m_jointIndices[i];
+		} else {
+			jointIndex = i;
+		}
+
+		if (args.m_targetVelocities) {
+			targetVelocity = args.m_targetVelocities[i];
+		}
+
+		if (args.m_targetPositions) {
+			targetPosition = args.m_targetPositions[i];
+		}
+
+		if (args.m_forces) {
+			force = args.m_forces[i];
+		}
+
+		if (args.m_kps) {
+			kp = args.m_kps[i];
+		}
+
+		if (args.m_kds) {
+			kd = args.m_kds[i];
+		}
+
+		b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
+
+		switch (args.m_controlMode) {
+		case CONTROL_MODE_VELOCITY: {
+			b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
+			b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+			b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
+			break;
+									}
+
+		case CONTROL_MODE_TORQUE: {
+			b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, force);
+			break;
+								  }
+
+		case CONTROL_MODE_POSITION_VELOCITY_PD: {
+			b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
+			b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
+			b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
+			b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
+			b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
+			break;
+												}
+
+		default: {}
+		};
+	}
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	return true;
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+	b3SharedMemoryStatusHandle statusHandle;
+
+	if (args.m_numSolverIterations >= 0) {
+		b3PhysicsParamSetNumSolverIterations(command, args.m_numSolverIterations);
+	}
+
+	if (args.m_collisionFilterMode >= 0) {
+		b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode);
+	}
+
+	if (args.m_numSubSteps >= 0)	{
+		b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps);
+	}
+
+	if (args.m_fixedTimeStep >= 0) {
+		b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep);
+	}
+
+	if (args.m_useSplitImpulse >= 0) {
+		b3PhysicsParamSetUseSplitImpulse(command, args.m_useSplitImpulse);
+	}
+
+	if (args.m_splitImpulsePenetrationThreshold >= 0) {
+		b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, args.m_splitImpulsePenetrationThreshold);
+	}
+
+	if (args.m_contactBreakingThreshold >= 0) {
+		b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold);
+	}
+
+	if (args.m_maxNumCmdPer1ms >= -1) {
+		b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms);
+	}
+
+	if (args.m_restitutionVelocityThreshold>=0) {
+		b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold);
+	}
+
+	if (args.m_enableFileCaching>=0) {
+		b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching);
+	}
+
+	if (args.m_erp>=0) {
+		b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp);
+	}
+
+	if (args.m_contactERP>=0) {
+		b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP);
+	}
+
+	if (args.m_frictionERP >=0) {
+		b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP);
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	return true;
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags) 
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+
+	command = b3ApplyExternalForceCommandInit(sm);
+	b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, flags);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags)
+{
+	double dforce[3];
+	double dposition[3];
+
+	dforce[0] = force.x;
+	dforce[1] = force.y;
+	dforce[2] = force.z;
+
+	dposition[0] = position.x;
+	dposition[1] = position.y;
+	dposition[2] = position.z;
+
+	return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags);
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags) 
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+
+	command = b3ApplyExternalForceCommandInit(sm);
+	b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags)
+{
+	double dtorque[3];
+
+	dtorque[0] = torque.x;
+	dtorque[1] = torque.y;
+	dtorque[2] = torque.z;
+
+	return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags);
+}
+
+
+bool  b3RobotSimulatorClientAPI_NoGUI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	int numJoints = b3GetNumJoints(sm, bodyUniqueId);
+	if ((jointIndex < 0) || (jointIndex >= numJoints)) {
+		b3Warning("Error: invalid jointIndex.");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	command = b3CreateSensorCommandInit(sm, bodyUniqueId);
+	b3CreateSensorEnable6DofJointForceTorqueSensor(command, jointIndex, enable);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_CLIENT_COMMAND_COMPLETED) {
+		return true;
+	}
+	return false;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	command = b3InitRequestOpenGLVisualizerCameraCommand(sm);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusOpenGLVisualizerCamera(statusHandle, cameraInfo);
+
+	if (statusType) {
+		return true;
+	}
+	return false;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	command = b3InitRequestContactPointInformation(sm);
+
+	if (args.m_bodyUniqueIdA>=0) {
+		b3SetContactFilterBodyA(command, args.m_bodyUniqueIdA);
+	}
+	if (args.m_bodyUniqueIdB>=0) {
+		b3SetContactFilterBodyB(command, args.m_bodyUniqueIdB);
+	}
+	if (args.m_linkIndexA>=-1) {
+		b3SetContactFilterLinkA(command, args.m_linkIndexA);
+	}
+	if (args.m_linkIndexB >=-1) {
+		b3SetContactFilterLinkB(command, args.m_linkIndexB);
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
+		b3GetContactPointInformation(sm, contactInfo);
+		return true;
+	}
+	return false;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	command = b3InitClosestDistanceQuery(sm);
+
+	b3SetClosestDistanceFilterBodyA(command, args.m_bodyUniqueIdA);
+	b3SetClosestDistanceFilterBodyB(command, args.m_bodyUniqueIdB);
+	b3SetClosestDistanceThreshold(command, distance);
+
+	if (args.m_linkIndexA>=-1) {
+		b3SetClosestDistanceFilterLinkA(command, args.m_linkIndexA);
+	}
+	if (args.m_linkIndexB >=-1) {
+		b3SetClosestDistanceFilterLinkB(command, args.m_linkIndexB);
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
+		b3GetContactPointInformation(sm, contactInfo);
+		return true;
+	}
+	return false;
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	//  int statusType;
+
+	command = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	b3GetAABBOverlapResults(sm, overlapData);
+
+	return true;
+}
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData)
+{
+	double daabbMin[3];
+	double daabbMax[3];
+
+	daabbMin[0] = aabbMin.x;
+	daabbMin[1] = aabbMin.y;
+	daabbMin[2] = aabbMin.z;
+
+	daabbMax[0] = aabbMax.x;
+	daabbMax[1] = aabbMax.y;
+	daabbMax[2] = aabbMax.z;
+
+	return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData);
+}
+
+
+
+bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	if (bodyUniqueId < 0) {
+		b3Warning("Invalid bodyUniqueId");
+		return false;
+	}
+
+	if (linkIndex < -1) {
+		b3Warning("Invalid linkIndex");
+		return false;
+	}
+
+	if (aabbMin == NULL || aabbMax == NULL) {
+		b3Warning("Output AABB matrix is NULL");
+		return false;
+	}    
+
+	command = b3RequestCollisionInfoCommandInit(sm, bodyUniqueId);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_REQUEST_COLLISION_INFO_COMPLETED) {
+		return false;
+	}
+	if (b3GetStatusAABB(statusHandle, linkIndex, aabbMin, aabbMax)) {
+		return true;
+	}
+	return false;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax)
+{
+	double daabbMin[3];
+	double daabbMax[3];
+
+	bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]);
+
+	aabbMin.x = (float)daabbMin[0];
+	aabbMin.y = (float)daabbMin[1];
+	aabbMin.z = (float)daabbMin[2];
+
+	aabbMax.x = (float)daabbMax[0];
+	aabbMax.y = (float)daabbMax[1];
+	aabbMax.z = (float)daabbMax[2];
+
+	return status;
+}
+
+
+int b3RobotSimulatorClientAPI_NoGUI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	int shapeIndex = -1;
+
+	command = b3CreateCollisionShapeCommandInit(sm);
+
+	if (shapeType==GEOM_SPHERE && args.m_radius>0) {
+		shapeIndex = b3CreateCollisionShapeAddSphere(command, args.m_radius);
+	}
+	if (shapeType==GEOM_BOX)  {
+		double halfExtents[3];
+		scalarToDouble3(args.m_halfExtents, halfExtents);
+		shapeIndex = b3CreateCollisionShapeAddBox(command, halfExtents);
+	}
+	if (shapeType==GEOM_CAPSULE && args.m_radius>0 && args.m_height>=0) {
+		shapeIndex = b3CreateCollisionShapeAddCapsule(command, args.m_radius, args.m_height);
+	}
+	if (shapeType==GEOM_CYLINDER && args.m_radius>0 && args.m_height>=0) {
+		shapeIndex = b3CreateCollisionShapeAddCylinder(command, args.m_radius, args.m_height);
+	}
+	if (shapeType==GEOM_MESH && args.m_fileName) {
+		double meshScale[3];
+		scalarToDouble3(args.m_meshScale, meshScale);
+		shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale);
+	}
+	if (shapeType==GEOM_PLANE) {
+		double planeConstant=0;
+		double planeNormal[3];
+		scalarToDouble3(args.m_planeNormal, planeNormal);
+		shapeIndex = b3CreateCollisionShapeAddPlane(command, planeNormal, planeConstant);
+	}
+	if (shapeIndex>=0 && args.m_flags) {
+		b3CreateCollisionSetFlag(command, shapeIndex, args.m_flags);
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) {
+		int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
+		return uid;
+	}
+	return -1;
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType, baseIndex;
+
+	double doubleBasePosition[3];
+	double doubleBaseInertialFramePosition[3];
+	scalarToDouble3(args.m_basePosition.m_floats, doubleBasePosition);
+	scalarToDouble3(args.m_baseInertialFramePosition.m_floats, doubleBaseInertialFramePosition);
+
+	double doubleBaseOrientation[4];
+	double doubleBaseInertialFrameOrientation[4];
+	scalarToDouble4(args.m_baseOrientation.m_floats, doubleBaseOrientation);
+	scalarToDouble4(args.m_baseInertialFrameOrientation.m_floats, doubleBaseInertialFrameOrientation);
+
+	command = b3CreateMultiBodyCommandInit(sm);
+
+	baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex,
+		doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation);
+
+	for (int i = 0; i < args.m_numLinks; i++) {
+		double linkMass = args.m_linkMasses[i];
+		int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i];
+		int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i];
+		b3Vector3 linkPosition = args.m_linkPositions[i];
+		b3Quaternion linkOrientation = args.m_linkOrientations[i];
+		b3Vector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i];
+		b3Quaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i];
+		int linkParentIndex = args.m_linkParentIndices[i];
+		int linkJointType = args.m_linkJointTypes[i];
+		b3Vector3 linkJointAxis = args.m_linkJointAxes[i];
+
+		double doubleLinkPosition[3];
+		double doubleLinkInertialFramePosition[3];
+		double doubleLinkJointAxis[3];
+		scalarToDouble3(linkPosition.m_floats, doubleLinkPosition);
+		scalarToDouble3(linkInertialFramePosition.m_floats, doubleLinkInertialFramePosition);
+		scalarToDouble3(linkJointAxis.m_floats, doubleLinkJointAxis);
+
+		double doubleLinkOrientation[4];
+		double doubleLinkInertialFrameOrientation[4];
+		scalarToDouble4(linkOrientation.m_floats, doubleLinkOrientation);
+		scalarToDouble4(linkInertialFrameOrientation.m_floats, doubleLinkInertialFrameOrientation);
+
+		b3CreateMultiBodyLink(command, 
+			linkMass, 
+			linkCollisionShapeIndex, 
+			linkVisualShapeIndex, 
+			doubleLinkPosition, 
+			doubleLinkOrientation,
+			doubleLinkInertialFramePosition,
+			doubleLinkInertialFrameOrientation,
+			linkParentIndex,
+			linkJointType,
+			doubleLinkJointAxis
+			);    
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) {
+		int uid = b3GetStatusBodyIndex(statusHandle);
+		return uid;
+	}
+	return -1;
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::getNumConstraints() const
+{
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return -1;
+	}
+	return b3GetNumUserConstraints(m_data->m_physicsClientHandle);
+}
+
+int b3RobotSimulatorClientAPI_NoGUI::getConstraintUniqueId(int serialIndex)
+{
+	if (!isConnected()) {
+		b3Warning("Not connected");
+		return -1;
+	}
+	int userConstraintId = -1;
+	userConstraintId = b3GetUserConstraintId(m_data->m_physicsClientHandle, serialIndex);
+	return userConstraintId;
+}
+
+
+
+void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* guiHelper)
+{
+	m_data->m_guiHelper = guiHelper;
+}
+
+struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper()
+{
+	return m_data->m_guiHelper;
+}
\ No newline at end of file
diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
new file mode 100644
index 0000000000..229d058ec3
--- /dev/null
+++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
@@ -0,0 +1,572 @@
+#ifndef B3_ROBOT_SIMULATOR_CLIENT_API_H
+#define B3_ROBOT_SIMULATOR_CLIENT_API_H
+
+#include "../SharedMemory/SharedMemoryPublic.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Transform.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+#include <string>
+
+struct b3RobotSimulatorLoadUrdfFileArgs
+{
+	b3Vector3 m_startPosition;
+	b3Quaternion m_startOrientation;
+	bool m_forceOverrideFixedBase;
+	bool m_useMultiBody;
+	int m_flags;
+
+	b3RobotSimulatorLoadUrdfFileArgs(const b3Vector3& startPos, const b3Quaternion& startOrn)
+		: m_startPosition(startPos),
+		m_startOrientation(startOrn),
+		m_forceOverrideFixedBase(false),
+		m_useMultiBody(true),
+		m_flags(0)
+	{
+	}
+
+	b3RobotSimulatorLoadUrdfFileArgs()
+		: m_startPosition(b3MakeVector3(0, 0, 0)),
+		m_startOrientation(b3Quaternion(0, 0, 0, 1)),
+		m_forceOverrideFixedBase(false),
+		m_useMultiBody(true),
+		m_flags(0)
+	{
+	}
+};
+
+struct b3RobotSimulatorLoadSdfFileArgs
+{
+	bool m_forceOverrideFixedBase;
+	bool m_useMultiBody;
+
+	b3RobotSimulatorLoadSdfFileArgs()
+		: m_forceOverrideFixedBase(false),
+		m_useMultiBody(true)
+	{
+	}
+};
+
+struct b3RobotSimulatorLoadFileResults
+{
+	b3AlignedObjectArray<int> m_uniqueObjectIds;
+	b3RobotSimulatorLoadFileResults()
+	{
+	}
+};
+
+struct b3RobotSimulatorJointMotorArgs
+{
+	int m_controlMode;
+
+	double m_targetPosition;
+	double m_kp;
+
+	double m_targetVelocity;
+	double m_kd;
+
+	double m_maxTorqueValue;
+
+	b3RobotSimulatorJointMotorArgs(int controlMode)
+		: m_controlMode(controlMode),
+		m_targetPosition(0),
+		m_kp(0.1),
+		m_targetVelocity(0),
+		m_kd(0.9),
+		m_maxTorqueValue(1000)
+	{
+	}
+};
+
+enum b3RobotSimulatorInverseKinematicsFlags
+{
+	B3_HAS_IK_TARGET_ORIENTATION = 1,
+	B3_HAS_NULL_SPACE_VELOCITY = 2,
+	B3_HAS_JOINT_DAMPING = 4,
+};
+
+struct b3RobotSimulatorInverseKinematicArgs
+{
+	int m_bodyUniqueId;
+	//	double* m_currentJointPositions;
+	//	int m_numPositions;
+	double m_endEffectorTargetPosition[3];
+	double m_endEffectorTargetOrientation[4];
+	int m_endEffectorLinkIndex;
+	int m_flags;
+	int m_numDegreeOfFreedom;
+	b3AlignedObjectArray<double> m_lowerLimits;
+	b3AlignedObjectArray<double> m_upperLimits;
+	b3AlignedObjectArray<double> m_jointRanges;
+	b3AlignedObjectArray<double> m_restPoses;
+	b3AlignedObjectArray<double> m_jointDamping;
+
+	b3RobotSimulatorInverseKinematicArgs()
+		: m_bodyUniqueId(-1),
+		m_endEffectorLinkIndex(-1),
+		m_flags(0)
+	{
+		m_endEffectorTargetPosition[0] = 0;
+		m_endEffectorTargetPosition[1] = 0;
+		m_endEffectorTargetPosition[2] = 0;
+
+		m_endEffectorTargetOrientation[0] = 0;
+		m_endEffectorTargetOrientation[1] = 0;
+		m_endEffectorTargetOrientation[2] = 0;
+		m_endEffectorTargetOrientation[3] = 1;
+	}
+};
+
+struct b3RobotSimulatorInverseKinematicsResults
+{
+	int m_bodyUniqueId;
+	b3AlignedObjectArray<double> m_calculatedJointPositions;
+};
+
+struct b3JointStates2
+{
+	int m_bodyUniqueId;
+	int m_numDegreeOfFreedomQ;
+	int m_numDegreeOfFreedomU;
+	b3Transform m_rootLocalInertialFrame;
+	b3AlignedObjectArray<double> m_actualStateQ;
+	b3AlignedObjectArray<double> m_actualStateQdot;
+	b3AlignedObjectArray<double> m_jointReactionForces;
+};
+
+
+struct b3RobotSimulatorJointMotorArrayArgs
+{
+	int m_controlMode;
+	int m_numControlledDofs;
+
+	int *m_jointIndices;
+
+	double *m_targetPositions;
+	double *m_kps;
+
+	double *m_targetVelocities;
+	double *m_kds;
+
+	double *m_forces;
+
+	b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs)
+		: m_controlMode(controlMode), m_numControlledDofs(numControlledDofs)
+	{
+	}
+};
+
+struct b3RobotSimulatorGetCameraImageArgs
+{
+	int m_width;
+	int m_height;
+	float *m_viewMatrix;
+	float *m_projectionMatrix;
+	float *m_lightDirection;
+	float *m_lightColor;
+	float m_lightDistance;
+	int m_hasShadow;
+	float m_lightAmbientCoeff;
+	float m_lightDiffuseCoeff;
+	float m_lightSpecularCoeff;
+	int m_renderer;
+
+	b3RobotSimulatorGetCameraImageArgs(int width, int height)
+		: m_width(width),
+		m_height(height),
+		m_viewMatrix(NULL),
+		m_projectionMatrix(NULL),
+		m_lightDirection(NULL),
+		m_lightColor(NULL),
+		m_lightDistance(-1),
+		m_hasShadow(-1),
+		m_lightAmbientCoeff(-1),
+		m_lightDiffuseCoeff(-1),
+		m_lightSpecularCoeff(-1),
+		m_renderer(-1)
+	{
+	}
+};
+
+struct b3RobotSimulatorSetPhysicsEngineParameters 
+{
+	double m_fixedTimeStep;
+	int m_numSolverIterations;
+	int m_useSplitImpulse;
+	double m_splitImpulsePenetrationThreshold;
+	int m_numSubSteps;
+	int m_collisionFilterMode;
+	double m_contactBreakingThreshold;
+	int m_maxNumCmdPer1ms;
+	int m_enableFileCaching;
+	double m_restitutionVelocityThreshold;
+	double m_erp;
+	double m_contactERP;
+	double m_frictionERP;
+
+	b3RobotSimulatorSetPhysicsEngineParameters()
+		: m_fixedTimeStep(-1),
+		m_numSolverIterations(-1),
+		m_useSplitImpulse(-1),
+		m_splitImpulsePenetrationThreshold(-1),
+		m_numSubSteps(-1),
+		m_collisionFilterMode(-1),
+		m_contactBreakingThreshold(-1),
+		m_maxNumCmdPer1ms(-1),
+		m_enableFileCaching(-1),
+		m_restitutionVelocityThreshold(-1),
+		m_erp(-1),
+		m_contactERP(-1),
+		m_frictionERP(-1)
+	{}
+};
+
+struct b3RobotSimulatorChangeDynamicsArgs
+{
+	double m_mass;
+	double m_lateralFriction;
+	double m_spinningFriction;
+	double m_rollingFriction;
+	double m_restitution;
+	double m_linearDamping;
+	double m_angularDamping;
+	double m_contactStiffness;
+	double m_contactDamping;
+	int m_frictionAnchor;
+
+	b3RobotSimulatorChangeDynamicsArgs()
+		: m_mass(-1),
+		m_lateralFriction(-1),
+		m_spinningFriction(-1),
+		m_rollingFriction(-1),
+		m_restitution(-1),
+		m_linearDamping(-1),
+		m_angularDamping(-1),
+		m_contactStiffness(-1),
+		m_contactDamping(-1),
+		m_frictionAnchor(-1)
+	{}
+};
+
+struct b3RobotSimulatorAddUserDebugLineArgs
+{
+	double m_colorRGB[3];
+	double m_lineWidth;
+	double m_lifeTime;
+	int m_parentObjectUniqueId;
+	int m_parentLinkIndex;
+
+	b3RobotSimulatorAddUserDebugLineArgs()
+		: 
+	m_lineWidth(1),
+		m_lifeTime(0),
+		m_parentObjectUniqueId(-1),
+		m_parentLinkIndex(-1)
+	{
+		m_colorRGB[0] = 1;
+		m_colorRGB[1] = 1;
+		m_colorRGB[2] = 1;
+	}
+};
+
+enum b3AddUserDebugTextFlags {
+	DEBUG_TEXT_HAS_ORIENTATION = 1
+};
+
+struct b3RobotSimulatorAddUserDebugTextArgs
+{
+	double m_colorRGB[3];
+	double m_size;
+	double m_lifeTime;
+	double m_textOrientation[4];
+	int m_parentObjectUniqueId;
+	int m_parentLinkIndex;
+	int m_flags;
+
+	b3RobotSimulatorAddUserDebugTextArgs()
+		: m_size(1),
+		m_lifeTime(0),
+		m_parentObjectUniqueId(-1),
+		m_parentLinkIndex(-1),
+		m_flags(0)
+	{
+		m_colorRGB[0] = 1;
+		m_colorRGB[1] = 1;
+		m_colorRGB[2] = 1;
+
+		m_textOrientation[0] = 0;
+		m_textOrientation[1] = 0;
+		m_textOrientation[2] = 0;
+		m_textOrientation[3] = 1;
+
+	}
+};
+
+struct b3RobotSimulatorGetContactPointsArgs
+{
+	int m_bodyUniqueIdA;
+	int m_bodyUniqueIdB;
+	int m_linkIndexA;
+	int m_linkIndexB;
+
+	b3RobotSimulatorGetContactPointsArgs()
+		: m_bodyUniqueIdA(-1),
+		m_bodyUniqueIdB(-1),
+		m_linkIndexA(-2),
+		m_linkIndexB(-2)
+	{}
+};
+
+struct b3RobotSimulatorCreateCollisionShapeArgs
+{
+	int m_shapeType;
+	double m_radius;
+	b3Vector3 m_halfExtents;
+	double m_height;
+	char* m_fileName;
+	b3Vector3 m_meshScale;
+	b3Vector3 m_planeNormal;
+	int m_flags;
+	b3RobotSimulatorCreateCollisionShapeArgs()
+		: m_shapeType(-1),
+		m_radius(0.5),
+		m_height(1),
+		m_fileName(NULL),
+		m_flags(0)
+	{
+		m_halfExtents.m_floats[0] = 1;
+		m_halfExtents.m_floats[1] = 1;
+		m_halfExtents.m_floats[2] = 1;
+
+		m_meshScale.m_floats[0] = 1;
+		m_meshScale.m_floats[1] = 1;
+		m_meshScale.m_floats[2] = 1;
+
+		m_planeNormal.m_floats[0] = 0;
+		m_planeNormal.m_floats[1] = 0;
+		m_planeNormal.m_floats[2] = 1;
+	}
+
+};
+
+struct b3RobotSimulatorCreateMultiBodyArgs
+{
+	double m_baseMass;
+	int m_baseCollisionShapeIndex;
+	int m_baseVisualShapeIndex;
+	b3Vector3 m_basePosition;
+	b3Quaternion m_baseOrientation;
+	b3Vector3 m_baseInertialFramePosition;
+	b3Quaternion m_baseInertialFrameOrientation;
+
+	int m_numLinks;
+	double *m_linkMasses;
+	int *m_linkCollisionShapeIndices;
+	int *m_linkVisualShapeIndices;
+	b3Vector3 *m_linkPositions;
+	b3Quaternion *m_linkOrientations;
+	b3Vector3 *m_linkInertialFramePositions;
+	b3Quaternion *m_linkInertialFrameOrientations;
+	int *m_linkParentIndices;
+	int *m_linkJointTypes;
+	b3Vector3 *m_linkJointAxes;
+
+	int m_useMaximalCoordinates;
+
+	b3RobotSimulatorCreateMultiBodyArgs()
+		: m_numLinks(0), m_baseMass(0), m_baseCollisionShapeIndex(-1), m_baseVisualShapeIndex(-1), m_useMaximalCoordinates(0), 
+		m_linkMasses(NULL), 
+		m_linkCollisionShapeIndices(NULL), 
+		m_linkVisualShapeIndices(NULL), 
+		m_linkPositions(NULL), 
+		m_linkOrientations(NULL), 
+		m_linkInertialFramePositions(NULL), 
+		m_linkInertialFrameOrientations(NULL), 
+		m_linkParentIndices(NULL), 
+		m_linkJointTypes(NULL), 
+		m_linkJointAxes(NULL)      
+	{
+		m_basePosition.setValue(0,0,0);
+		m_baseOrientation.setValue(0,0,0,1);
+		m_baseInertialFramePosition.setValue(0,0,0);
+		m_baseInertialFrameOrientation.setValue(0,0,0,1);
+	}
+};
+
+
+
+///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
+///as documented in the pybullet Quickstart Guide
+///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
+class b3RobotSimulatorClientAPI_NoGUI
+{
+protected:
+
+	struct b3RobotSimulatorClientAPI_InternalData* m_data;
+
+public:
+	
+	b3RobotSimulatorClientAPI_NoGUI();
+	virtual ~b3RobotSimulatorClientAPI_NoGUI();
+
+	bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
+
+	void disconnect();
+
+	bool isConnected() const;
+
+	void setTimeOut(double timeOutInSec);
+
+	void syncBodies();
+
+	void resetSimulation();
+
+	b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw);
+	b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat);
+
+	int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs());
+	bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
+	bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
+	bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
+
+	bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo);
+
+	bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const;
+	bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation);
+
+	bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const;
+	bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const;
+
+	int getNumJoints(int bodyUniqueId) const;
+
+	bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
+
+	int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
+
+	int changeConstraint(int constraintId, b3JointInfo* jointInfo);
+
+	void removeConstraint(int constraintId);
+
+	bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
+
+	bool getJointStates(int bodyUniqueId, b3JointStates2& state);
+
+	bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
+
+	void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);
+
+	bool setJointMotorControlArray(int bodyUniqueId, int controlMode, int numControlledDofs,
+		int *jointIndices, double *targetVelocities, double *targetPositions,
+		double *forces, double *kps, double *kds);
+
+	void stepSimulation();
+
+	bool canSubmitCommand() const;
+
+	void setRealTimeSimulation(bool enableRealTimeSimulation);
+
+	void setInternalSimFlags(int flags);
+
+	void setGravity(const b3Vector3& gravityAcceleration);
+
+	void setTimeStep(double timeStepInSeconds);
+	void setNumSimulationSubSteps(int numSubSteps);
+	void setNumSolverIterations(int numIterations);
+	void setContactBreakingThreshold(double threshold);
+
+	bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results);
+
+	bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
+
+	bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState);
+
+	void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);
+	void resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const b3Vector3& targetPos);
+
+	int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds=b3AlignedObjectArray<int>(), int maxLogDof = -1);
+	void stopStateLogging(int stateLoggerUniqueId);
+
+	void getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter);
+	void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData);
+
+	void submitProfileTiming(const std::string&  profileName, int durationInMicroSeconds=1);
+
+	// JFC: added these 24 methods
+
+	void getMouseEvents(b3MouseEventsData* mouseEventsData);
+
+	bool getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeInverseKinematics, b3LinkState* linkState);
+
+	bool getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, b3CameraImageData &imageData);
+
+	bool calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities, double *jointAccelerations, double *jointForcesOutput);
+
+	int getNumBodies() const;
+
+	int getBodyUniqueId(int bodyId) const;
+
+	bool removeBody(int bodyUniqueId);
+
+	bool getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo);
+
+	bool changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args);
+
+	int addUserDebugParameter(char *paramName, double rangeMin, double rangeMax, double startValue);
+
+	double readUserDebugParameter(int itemUniqueId);
+
+	bool removeUserDebugItem(int itemUniqueId);
+
+	int addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args);
+
+	int addUserDebugText(char *text, b3Vector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args);
+
+	int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args);
+
+	int addUserDebugLine(b3Vector3 &fromXYZ, b3Vector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args);
+
+	bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args);
+
+	bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args);
+
+	bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags);
+
+	bool applyExternalForce(int objectUniqueId, int linkIndex, b3Vector3 &force, b3Vector3 &position, int flags);
+
+	bool applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags);
+
+	bool applyExternalTorque(int objectUniqueId, int linkIndex, b3Vector3 &torque, int flags);
+
+	bool enableJointForceTorqueSensor(int bodyUniqueId, int linkIndex, bool enable);
+
+	bool getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo);
+
+	bool getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo);
+
+	bool getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo);
+
+	bool getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData);
+
+	bool getOverlappingObjects(b3Vector3 &aabbMin, b3Vector3 &aabbMax, struct b3AABBOverlapData *overlapData);
+
+	bool getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax);
+
+	bool getAABB(int bodyUniqueId, int linkIndex, b3Vector3 &aabbMin, b3Vector3 &aabbMax);
+
+	int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args);
+
+	int createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args);
+
+	int getNumConstraints() const;
+
+	int getConstraintUniqueId(int serialIndex);
+
+	void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin);
+	
+	virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
+	virtual struct GUIHelperInterface* getGuiHelper();
+	
+};
+
+#endif  //B3_ROBOT_SIMULATOR_CLIENT_API_H
diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua
index 020ef2c644..f24ba6b903 100644
--- a/examples/RobotSimulator/premake4.lua
+++ b/examples/RobotSimulator/premake4.lua
@@ -193,6 +193,8 @@ if not _OPTIONS["no-enet"] then
 			"RobotSimulatorMain.cpp",
 			"b3RobotSimulatorClientAPI.cpp",
 			"b3RobotSimulatorClientAPI.h",
+			"b3RobotSimulatorClientAPI_NoGUI.cpp",
+			"b3RobotSimulatorClientAPI_NoGUI.h",
 			"MinitaurSetup.cpp",
 			"MinitaurSetup.h",
 			myfiles
@@ -283,6 +285,8 @@ project ("App_VRGloveHandSimulator")
 			"VRGloveSimulatorMain.cpp",
 			"b3RobotSimulatorClientAPI.cpp",
 			"b3RobotSimulatorClientAPI.h",
+			"b3RobotSimulatorClientAPI_NoGUI.cpp",
+			"b3RobotSimulatorClientAPI_NoGUI.h",
 			myfiles
 		}
 
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 34f5d9f311..6dea731295 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -32,7 +32,6 @@ B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
 #include "PhysicsClientTCP_C_API.h"
 #endif
 
-#include "SharedMemoryInProcessPhysicsC_API.h"
 
 #ifdef __cplusplus
 extern "C" { 
diff --git a/examples/TwoJoint/TwoJointMain.cpp b/examples/TwoJoint/TwoJointMain.cpp
index 6c25fe5bee..64e325bf55 100644
--- a/examples/TwoJoint/TwoJointMain.cpp
+++ b/examples/TwoJoint/TwoJointMain.cpp
@@ -9,6 +9,7 @@
 #include "SharedMemory/PhysicsClientC_API.h"
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Common/b3Quaternion.h"
+#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
 
 extern const int CONTROL_RATE;
 const int CONTROL_RATE = 500;